search

USC-ACTLab / crazyswarm

A Large Quadcopter Swarm
MIT License
319 stars 317 forks source link

CF2 does not fly (+ crash) with optitrack (coordinate frame issue) #49

Closed gitsr-sys closed 6 years ago

gitsr-sys commented 6 years ago

Hi, I am integrating optitrack and cf2 with crazyflie_ros and vrpn_client_ros. It worked correctly.

Now I am using this crazyswarm package. I think my setting is almost done. However, CF2 does not take off.

<The setting of hover_swarm.launch>
     # tracking
      motion_capture_type: "optitrack" # one of vicon, optitrack
      object_tracking_type: "libobjecttracker" # one of motionCapture, libobjecttracker
      # vicon_host_name: "vicon"
      optitrack_local_ip: "130.xx.xxx.xxx" # ubuntu computer
      optitrack_server_ip: "130.xx.xxx.xxx" # optitrack computer
 I updated the values using mocap_helper.launch
<crazyflieTypes.yaml>
markerConfigurations:
  "0":  # for standard Crazyflie
    numPoints: 4
    offset: [0.0, -0.01, -0.04]
    points:
      "0": [-0.00285348,-0.0684936,0.0211864]
      "1": [-0.0704766,-0.00605787,0.0223118]
      "2": [0.060272,3.81677e-05,0.0231011]
      "3": [-0.00905949,-0.000530424,0.0350827]

I executed the following commands.

$ python chooser.py
In chooser window, click "Clear" and "Fill", then click "reboot"
$ roslaunch crazyswarm hover_swarm.launch

After that, CF2 appeared in rvis window. (There were two frames; "cf1" and "world") Part of terminal window's line is shown in the following.

[ INFO] [1520450020.300528125]: [cf1] Update parameters
[ERROR] [1520450020.301070905]: Could not find param kalmanUSC/rstWithExtPos
[ERROR] [1520450020.303812255]: Could not find param traj/auto_yaw
[ INFO] [1520450020.349110276]: Update params: 0.065297 s
[ INFO] [1520450020.349302876]: Started 1 threads

(What is the param errors?)

Then, I push the start button of joystick, and terminal window showed [ INFO] [1520450065.633917239]: Takeoff!. But, CF2 did not move at all.

I attach the LED patterns.

  1. When I turn on crazyflie: https://youtu.be/4_la075u4G4
  2. When I reboot through chooser.py: https://youtu.be/PyFIcwxN7Fc
  3. When I run "$roslaunch crazyswarm hover_swarm.launch": https://youtu.be/KYxmz74xods

Despite of that I executed the following command $ python figure8_canned.py. Then, CF2 flew to the wrong side and crashed to the wall. The terminal window outputted the following error message several times.

[pcl::IterativeClosestPoint::computeTransformation] Not enough correspondences found. Relax your threshold parameters.
[ WARN] [1520450711.869466057]: ICP did not converge!
[ WARN] [1520450711.869526159]: No updated pose for CF cf1 for 0.049901 s.
[ WARN] [1520450711.877692015]: Dynamic check failed
fitness: 0.00160511 >= 0.001

Then, it automatically went to emergency mode after crashing.

terminate called after throwing an instance of 'boost::exception_detail::clone_impl<boost::exception_detail::error_info_injector<boost::lock_error> >'
  what():  boost: mutex lock failed in pthread_mutex_lock: Invalid argument
[ INFO] [1520450713.327218320]: emergency requested...
[ INFO] [1520450713.328431185]: Done.
[crazyflie_server-2] process has died [pid 7083, exit code -6, cmd /home/ubuntu/crazyswarm/ros_ws/devel/lib/crazyflie_driver/crazyflie_server __name:=crazyflie_server __log:=/home/ubuntu/.ros/log/3df1d818-223d-11e8-a760-74d435137038/crazyflie_server-2.log].
log file: /home/_ubuntu/.ros/log/3df1d818-223d-11e8-a760-74d435137038/crazyflie_server-2*.log

Please help me to resolve this issue. Thank you.

whoenig commented 6 years ago
  1. When you choose reboot in chooser.py the crazyflie should spin up the motors again, just like in your first video. Note, that you can only use chooser.py while the server is not running (since only one process can use the Crazyradio at a time.) If it doesn't work, make sure you use the provided (or latest) NRF51 firmware.
  2. The parameter errors you are seeing are caused by a wrong firmware version (STM32). Please flash the ones that are in the prebuilt folder (or build the provided firmware yourself).
  3. In your last video the CF is already in some sort of error state (fast blinking of the red LED). This could again be related to the wrong firmware version or some other event that happened between the reboot and the roslaunch command.
  4. I am not sure why the joystick would not work, but the script does - they simple send the very same command to the crazyflie_server. If you have an LED-ring, it will show you the communication status between the CF and the server.
  5. For the crashed flight, did you check that the coordinate system is following the ROS standard? There are some settings in Motive about the coordinate system that you might need to adjust. A good test is to manually move the CF around while the server is running and verify in rviz that the position feedback is correct.
gitsr-sys commented 6 years ago

frames

also executed $ python figure8_canned.py Doesn't cf2 hover at the first time? I just executed commands and did not press any buttons. But cf2 flew and crashed on the ground......it did not fly correctly... FYI, I achieved hovering with crazyflie ros package with optitrack...

See the video first Flight testVideo. I show you whole log messages.

... logging to /home/ubuntu/.ros/log/0a6b3d68-230f-11e8-a760-74d435137038/roslaunch-ubuntu-14702.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://ubuntu:33263/

SUMMARY
========

PARAMETERS
 * /crazyflieTypes/default/batteryVoltageWarning: 3.8
 * /crazyflieTypes/default/batteryVoltateCritical: 3.7
 * /crazyflieTypes/default/bigQuad: False
 * /crazyflieTypes/default/dynamicsConfiguration: 0
 * /crazyflieTypes/default/firmwareParams/ctrlMel/i_range_m_z: 1500
 * /crazyflieTypes/default/firmwareParams/ctrlMel/i_range_xy: 2.0
 * /crazyflieTypes/default/firmwareParams/ctrlMel/i_range_z: 0.4
 * /crazyflieTypes/default/firmwareParams/ctrlMel/kR_xy: 70000
 * /crazyflieTypes/default/firmwareParams/ctrlMel/kR_z: 60000
 * /crazyflieTypes/default/firmwareParams/ctrlMel/kd_omega_rp: 200
 * /crazyflieTypes/default/firmwareParams/ctrlMel/kd_xy: 0.2
 * /crazyflieTypes/default/firmwareParams/ctrlMel/kd_z: 0.4
 * /crazyflieTypes/default/firmwareParams/ctrlMel/ki_m_z: 500
 * /crazyflieTypes/default/firmwareParams/ctrlMel/ki_xy: 0.05
 * /crazyflieTypes/default/firmwareParams/ctrlMel/ki_z: 0.05
 * /crazyflieTypes/default/firmwareParams/ctrlMel/kp_xy: 0.4
 * /crazyflieTypes/default/firmwareParams/ctrlMel/kp_z: 1.25
 * /crazyflieTypes/default/firmwareParams/ctrlMel/kw_xy: 20000
 * /crazyflieTypes/default/firmwareParams/ctrlMel/kw_z: 12000
 * /crazyflieTypes/default/firmwareParams/ctrlMel/mass: 0.032
 * /crazyflieTypes/default/firmwareParams/ctrlMel/massThrust: 132000
 * /crazyflieTypes/default/markerConfiguration: 0
 * /crazyflieTypes/large/batteryVoltageWarning: 11.4
 * /crazyflieTypes/large/batteryVoltateCritical: 11.1
 * /crazyflieTypes/large/bigQuad: True
 * /crazyflieTypes/large/dynamicsConfiguration: 0
 * /crazyflieTypes/large/firmwareParams/ctrlMel/i_range_m_z: 1500
 * /crazyflieTypes/large/firmwareParams/ctrlMel/i_range_xy: 2.0
 * /crazyflieTypes/large/firmwareParams/ctrlMel/i_range_z: 4.0
 * /crazyflieTypes/large/firmwareParams/ctrlMel/kR_xy: 30000
 * /crazyflieTypes/large/firmwareParams/ctrlMel/kR_z: 40000
 * /crazyflieTypes/large/firmwareParams/ctrlMel/kd_omega_rp: 100
 * /crazyflieTypes/large/firmwareParams/ctrlMel/kd_xy: 3.0
 * /crazyflieTypes/large/firmwareParams/ctrlMel/kd_z: 3.0
 * /crazyflieTypes/large/firmwareParams/ctrlMel/ki_m_z: 500
 * /crazyflieTypes/large/firmwareParams/ctrlMel/ki_xy: 1.0
 * /crazyflieTypes/large/firmwareParams/ctrlMel/ki_z: 2.0
 * /crazyflieTypes/large/firmwareParams/ctrlMel/kp_xy: 8.0
 * /crazyflieTypes/large/firmwareParams/ctrlMel/kp_z: 8.0
 * /crazyflieTypes/large/firmwareParams/ctrlMel/kw_xy: 9000
 * /crazyflieTypes/large/firmwareParams/ctrlMel/kw_z: 10000
 * /crazyflieTypes/large/firmwareParams/ctrlMel/mass: 0.38
 * /crazyflieTypes/large/firmwareParams/ctrlMel/massThrust: 5400
 * /crazyflieTypes/large/markerConfiguration: 2
 * /crazyflieTypes/medium/batteryVoltageWarning: 7.6
 * /crazyflieTypes/medium/batteryVoltateCritical: 7.4
 * /crazyflieTypes/medium/bigQuad: True
 * /crazyflieTypes/medium/dynamicsConfiguration: 0
 * /crazyflieTypes/medium/firmwareParams/ctrlMel/i_range_m_z: 1500
 * /crazyflieTypes/medium/firmwareParams/ctrlMel/i_range_xy: 2.0
 * /crazyflieTypes/medium/firmwareParams/ctrlMel/i_range_z: 2.0
 * /crazyflieTypes/medium/firmwareParams/ctrlMel/kR_xy: 50000
 * /crazyflieTypes/medium/firmwareParams/ctrlMel/kR_z: 60000
 * /crazyflieTypes/medium/firmwareParams/ctrlMel/kd_omega_rp: 200
 * /crazyflieTypes/medium/firmwareParams/ctrlMel/kd_xy: 1.0
 * /crazyflieTypes/medium/firmwareParams/ctrlMel/kd_z: 1.0
 * /crazyflieTypes/medium/firmwareParams/ctrlMel/ki_m_z: 500
 * /crazyflieTypes/medium/firmwareParams/ctrlMel/ki_xy: 0.1
 * /crazyflieTypes/medium/firmwareParams/ctrlMel/ki_z: 0.5
 * /crazyflieTypes/medium/firmwareParams/ctrlMel/kp_xy: 2.0
 * /crazyflieTypes/medium/firmwareParams/ctrlMel/kp_z: 3
 * /crazyflieTypes/medium/firmwareParams/ctrlMel/kw_xy: 16000
 * /crazyflieTypes/medium/firmwareParams/ctrlMel/kw_z: 12000
 * /crazyflieTypes/medium/firmwareParams/ctrlMel/mass: 0.11
 * /crazyflieTypes/medium/firmwareParams/ctrlMel/massThrust: 23000
 * /crazyflieTypes/medium/markerConfiguration: 1
 * /crazyflie_manager/csv_file: /home/ubuntu...
 * /crazyflie_manager/timescale: 0.8
 * /crazyflie_server/broadcast_address: FFE7E7E7E7
 * /crazyflie_server/enable_logging: True
 * /crazyflie_server/enable_parameters: True
 * /crazyflie_server/firmwareParams/flightmode/posCtrl: 1
 * /crazyflie_server/firmwareParams/kalmanUSC/rstWithExtPos: 1
 * /crazyflie_server/firmwareParams/ring/effect: 14
 * /crazyflie_server/firmwareParams/ring/headlightEnable: 0
 * /crazyflie_server/firmwareParams/ring/solidBlue: 255
 * /crazyflie_server/firmwareParams/ring/solidGreen: 0
 * /crazyflie_server/firmwareParams/ring/solidRed: 0
 * /crazyflie_server/firmwareParams/traj/auto_yaw: 0
 * /crazyflie_server/force_no_cache: False
 * /crazyflie_server/genericLogTopicFrequencies: [10]
 * /crazyflie_server/genericLogTopic_log1_Variables: ['ctrlStat.edist']
 * /crazyflie_server/genericLogTopics: ['log1']
 * /crazyflie_server/motion_capture_type: optitrack
 * /crazyflie_server/object_tracking_type: libobjecttracker
 * /crazyflie_server/optitrack_local_ip: 130.85.170.237
 * /crazyflie_server/optitrack_server_ip: 130.85.170.251
 * /crazyflie_server/print_latency: False
 * /crazyflie_server/save_point_clouds: ~/pointCloud.ot
 * /crazyflie_server/world_frame: /world
 * /crazyflie_server/write_csvs: False
 * /crazyflies: [{'initialPositio...
 * /dynamicsConfigurations/0/maxFitnessScore: 0.001
 * /dynamicsConfigurations/0/maxPitch: 1.4
 * /dynamicsConfigurations/0/maxPitchRate: 20.0
 * /dynamicsConfigurations/0/maxRoll: 1.4
 * /dynamicsConfigurations/0/maxRollRate: 20.0
 * /dynamicsConfigurations/0/maxXVelocity: 2.0
 * /dynamicsConfigurations/0/maxYVelocity: 2.0
 * /dynamicsConfigurations/0/maxYawRate: 10.0
 * /dynamicsConfigurations/0/maxZVelocity: 3.0
 * /joy/dev: /dev/input/js0
 * /markerConfigurations/0/numPoints: 4
 * /markerConfigurations/0/offset: [0.0, -0.01, -0.04]
 * /markerConfigurations/0/points/0: [-0.00353211, -0....
 * /markerConfigurations/0/points/1: [-0.00353563, 0.0...
 * /markerConfigurations/0/points/2: [-0.0062451, -0.0...
 * /markerConfigurations/0/points/3: [-0.0691729, -0.0...
 * /markerConfigurations/1/numPoints: 4
 * /markerConfigurations/1/offset: [0.0, 0.0, -0.03]
 * /markerConfigurations/1/points/0: [-0.00896228, -0....
 * /markerConfigurations/1/points/1: [-0.0156318, 0.09...
 * /markerConfigurations/1/points/2: [0.0461693, -0.08...
 * /markerConfigurations/1/points/3: [-0.0789959, -0.0...
 * /markerConfigurations/2/numPoints: 4
 * /markerConfigurations/2/offset: [0.0, 0.0, -0.06]
 * /markerConfigurations/2/points/0: [0.0558163, -0.00...
 * /markerConfigurations/2/points/1: [-0.0113941, 0.00...
 * /markerConfigurations/2/points/2: [-0.0306277, 0.05...
 * /markerConfigurations/2/points/3: [0.0535816, -0.04...
 * /numDynamicsConfigurations: 1
 * /numMarkerConfigurations: 3
 * /rosdistro: kinetic
 * /rosversion: 1.12.12

NODES
  /
    crazyflie_manager (crazyflie_manager/crazyflie_manager)
    crazyflie_server (crazyflie_driver/crazyflie_server)
    joy (joy/joy_node)
    rqt_plot_x (rqt_plot/rqt_plot)
    rviz (rviz/rviz)

auto-starting new master
process[master]: started with pid [14713]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 0a6b3d68-230f-11e8-a760-74d435137038
process[rosout-1]: started with pid [14726]
started core service [/rosout]
process[crazyflie_server-2]: started with pid [14743]
process[joy-3]: started with pid [14744]
process[crazyflie_manager-4]: started with pid [14745]
process[rviz-5]: started with pid [14769]
[ INFO] [1520540814.663149024]: Wait for services...
[ INFO] [1520540814.663645913]: waitForService: Service [/emergency] has not been advertised, waiting...
process[rqt_plot_x-6]: started with pid [14781]
[ INFO] [1520540814.684711588]: waitForService: Service [/emergency] is now available.
[ INFO] [1520540814.688921174]: Manager ready.
NatNetClient connecting: ***.**.170.237 to ***.**.170.251...
WARNING: Could not set receive buffer size. Asked for 1048576B got 0B
[Client] Server Software: Motive
[Client] NatNetVersion: 2.10
[Client] ServerVersion: 1.10
NatNet server version: 4222244226
ch: 1
[ INFO] [1520540814.718530634]: Opened joystick: /dev/input/js0. deadzone_: 0.050000.
[ INFO] [1520540814.758955538]: Adding CF: cf1 (radio://0/100/2M/E7E7E7E701, cf1)...
[ INFO] [1520540814.763645320]: CF ctor: 0.004600 s
[ INFO] [1520540814.772013739]: [cf1] Requesting parameters...
[ INFO] [1520540814.780566853]: Found variables in cache.
[ INFO] [1520540815.054032195]: [cf1] reqParamTOC: 0.290336 s
[ INFO] [1520540815.054064096]: [cf1] Requesting logging variables...
[ INFO] [1520540815.063050284]: Found variables in cache.
[ INFO] [1520540815.064698101]: [cf1] reqLogTOC: 0.010667 s
[ INFO] [1520540815.083291512]: [cf1] logBlocks: 0.290336 s
[ INFO] [1520540815.083349890]: [cf1] Ready. Elapsed: 0.319658 s
[ INFO] [1520540815.083372153]: CF run: 0.319761 s
update flightmode/posCtrl to 1
update kalmanUSC/rstWithExtPos to 1
update ring/effect to 14
update ring/headlightEnable to 0
update ring/solidBlue to 255
update ring/solidGreen to 0
update ring/solidRed to 0
update traj/auto_yaw to 0
update ctrlMel/i_range_m_z to 1500
update ctrlMel/i_range_xy to 2
update ctrlMel/i_range_z to 0.4
update ctrlMel/kR_xy to 70000
update ctrlMel/kR_z to 60000
update ctrlMel/kd_omega_rp to 200
update ctrlMel/kd_xy to 0.2
update ctrlMel/kd_z to 0.4
update ctrlMel/ki_m_z to 500
update ctrlMel/ki_xy to 0.05
update ctrlMel/ki_z to 0.05
update ctrlMel/kp_xy to 0.4
update ctrlMel/kp_z to 1.25
update ctrlMel/kw_xy to 20000
update ctrlMel/kw_z to 12000
update ctrlMel/mass to 0.032
update ctrlMel/massThrust to 132000
[ INFO] [1520540815.098070261]: [cf1] Update parameters
[ INFO] [1520540815.144409935]: Update params: 0.060987 s
[ INFO] [1520540815.144533033]: Started 1 threads
[ INFO] [1520540849.522668382]: Takeoff!
[pcl::IterativeClosestPoint::computeTransformation] Not enough correspondences found. Relax your threshold parameters.
[ WARN] [1520540850.791102806]: ICP did not converge!
[ WARN] [1520540850.791173874]: No updated pose for CF cf1 for 0.008265 s.
[pcl::IterativeClosestPoint::computeTransformation] Not enough correspondences found. Relax your threshold parameters.
[ WARN] [1520540850.799216255]: ICP did not converge!
[ WARN] [1520540850.799289471]: No updated pose for CF cf1 for 0.016349 s.
[pcl::IterativeClosestPoint::computeTransformation] Not enough correspondences found. Relax your threshold parameters.
[ WARN] [1520540850.807802531]: ICP did not converge!
[ WARN] [1520540850.807874287]: No updated pose for CF cf1 for 0.024980 s.
[ WARN] [1520540850.816152181]: Dynamic check failed
wroll: 48.6757 >= 20
wyaw: 17.8638 >= 10
roll: 2.20153 >= 1.4
fitness: 0.0018263 >= 0.001

[ WARN] [1520540850.816246969]: No updated pose for CF cf1 for 0.033159 s.
[ WARN] [1520540850.824820547]: Dynamic check failed
fitness: 0.00141485 >= 0.001

[ WARN] [1520540850.824885926]: No updated pose for CF cf1 for 0.041854 s.
[ WARN] [1520540850.832873176]: Dynamic check failed
wyaw: -59.2716 >= 10

[ WARN] [1520540850.832951291]: No updated pose for CF cf1 for 0.049917 s.
[pcl::IterativeClosestPoint::computeTransformation] Not enough correspondences found. Relax your threshold parameters.
[ WARN] [1520540850.841206477]: ICP did not converge!
.
. (Same messages outputted many times)
.
[ WARN] [1520540851.732717174]: No updated pose for CF cf1 for 0.949819 s.
[FATAL] [1520540851.738713991]: Emergency requested!
[ INFO] [1520540851.739007038]: Done.
[pcl::IterativeClosestPoint::computeTransformation] Not enough correspondences found. Relax your threshold parameters.
[ WARN] [1520540851.741323459]: ICP did not converge!
[ WARN] [1520540851.741385691]: No updated pose for CF cf1 for 0.958504 s.
terminate called after throwing an instance of 'boost::exception_detail::clone_impl<boost::exception_detail::error_info_injector<boost::lock_error> >'
  what():  boost: mutex lock failed in pthread_mutex_lock: Invalid argument
[crazyflie_server-2] process has died [pid 14743, exit code -6, cmd /home/ubuntu/crazyswarm/ros_ws/devel/lib/crazyflie_driver/crazyflie_server __name:=crazyflie_server __log:=/home/ubuntu/.ros/log/0a6b3d68-230f-11e8-a760-74d435137038/crazyflie_server-2.log].
log file: /home/ubuntu/.ros/log/0a6b3d68-230f-11e8-a760-74d435137038/crazyflie_server-2*.log
^C[rviz-5] killing on exit
[rqt_plot_x-6] killing on exit
[crazyflie_manager-4] killing on exit
[joy-3] killing on exit
[rosout-1] killing on exit
[master] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done
whoenig commented 6 years ago

The LED pattern is fine. crazyflie_ros and crazyswarm use different ways on how they communicate with Motive. Your problem looks like your coordinate system is flipped. Make sure that forward is positive x (forward is where the CF's antenna is), left is positive y, and up is positive z. The current optitrack backend transforms the points from motive, so you need to make sure that rviz shows them correctly accordingly. There are settings in Motive to change the coordinate system (since I never used it myself I can't be more specific.)

gitsr-sys commented 6 years ago

Hi, thank you for your reply.

After you mentioned about the frame, I think it is the issue in my system. I put cf2 making the forward positive x in "world frame" It is shown in the figure in the following figure.

frames

In addition, I moved cf2 to its front left side as shown in the following figure.

frame_test01

As you can see, its left is not positive y!! That is the problem.............I am figuring out how to change this coordinate frame... If you have any idea, then please let me know. (I think some people are working with optitrack... if you have any idea could you help me @itzzartem @naus3a ?) Also, if I found the way how to change y direction, then I will post it.

For your information, the optitrack program has its own setting for z-axis. In "Streaming" menu, there is "Up Axis" option which can be chosen between "Z Up" and "Y Up". I made successful flight using crazyflie ros package with vrpn_ros_client and "Z Up" option. One more thing is that the ground 2D plane is defined with x-axis and z-axis after calibration in Motive program..

Thank you.

gitsr-sys commented 6 years ago

Hi, I just found what was wrong...the coordinate frame direction. I recognized that the world frame is fixed, so I needed to rotate cf2 to the right direction. I just confused it with when I used vrpn_ros_client package.... So I found right direction rotating cf2, then finally got the successful result with "python figure8_canned.py"

Thank you for all your support and development, @whoenig !!! I really appreciate it.