Hi, I am integrating optitrack and cf2 with <a href="https://github.com/whoenig/cr

When you choose reboot in chooser.py the crazyflie should spin up the motors aga

After re-flashing the firmware, the parameter errors is resolved.

CF2 does not fly (+ crash) with optitrack (coordinate frame issue) about crazyswarm HOT 5 CLOSED

usc-actlab commented on June 30, 2024

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

from crazyswarm.

Comments (5)

whoenig commented on June 30, 2024

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.
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).
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.
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.
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.

from crazyswarm.

gitsr-sys commented on June 30, 2024

After re-flashing the firmware, the parameter errors is resolved. Thank you.
I think the coordinate system is correct as shown in the figure.

I executed
$ roslaunch crazyswarm hover_swarm.launch
At this moment the LED pattern is shown in the following video.
LED Pattern
Is the led pattern okay?

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

from crazyswarm.

whoenig commented on June 30, 2024

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.)

from crazyswarm.

gitsr-sys commented on June 30, 2024

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.

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

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.

from crazyswarm.

gitsr-sys commented on June 30, 2024

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.

from crazyswarm.

CF2 does not fly (+ crash) with optitrack (coordinate frame issue) about crazyswarm HOT 5 CLOSED

Comments (5)

Related Issues (20)

Recommend Projects

React

Vue.js

Typescript

TensorFlow

Django

Laravel

D3

Recommend Topics

javascript

web

server

Machine learning

Visualization

Game

Recommend Org

Facebook

Microsoft

Google

Alibaba

D3

Tencent