Commands ignored by crazyflie

medioman22 commented 5 years ago

Hello,

I have followed the instructions for using crazyswarm with OptiTrack but I am running through some issues.

Let me first describe my setup:

I have 1 crazyflie 2.1
1 crazyradio PA
OptiTrack mocap system with Motive 2.0.2
Ubuntu 16.04 ROS kinetic

After some efforts I managed to make the drone take off with hover_swarm.launch, and I am quite satisfied by the stabilization achieved. However, there seems to be a communication problem between the cf and the crazyradio, because the drone does not execute all the instructions sent.

When I pressed start on the controller to take off, I always get the output on the console, but the drone actually takes off sometimes and not always.

Here is the output of the terminal:

~/Documents/github/crazyswarm/ros_ws$ roslaunch crazyswarm hover_swarm.launch 
... logging to /home/lis-laptop-15-20/.ros/log/754d29e6-7fcc-11e9-a40b-507b9d6b467f/roslaunch-lislaptop1520-ThinkPad-T450s-9110.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://lislaptop1520-ThinkPad-T450s:43974/

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
 * /crazyflies: [{'initialPositio...
 * /crazyswarm_server/broadcast_address: E7E7E7E701
 * /crazyswarm_server/enable_logging: False
 * /crazyswarm_server/enable_parameters: True
 * /crazyswarm_server/firmware: crazyswarm
 * /crazyswarm_server/firmwareParams/commander/enHighLevel: 1
 * /crazyswarm_server/firmwareParams/stabilizer/controller: 2
 * /crazyswarm_server/firmwareParams/stabilizer/estimator: 3
 * /crazyswarm_server/force_no_cache: False
 * /crazyswarm_server/genericLogTopicFrequencies: [10]
 * /crazyswarm_server/genericLogTopic_log1_Variables: ['ctrlStat.edist']
 * /crazyswarm_server/genericLogTopics: ['log1']
 * /crazyswarm_server/motion_capture_type: optitrack
 * /crazyswarm_server/object_tracking_type: motionCapture
 * /crazyswarm_server/optitrack_host_name: 192.168.1.100
 * /crazyswarm_server/print_latency: False
 * /crazyswarm_server/save_point_clouds: ~/pointCloud.ot
 * /crazyswarm_server/vrpn_host_name: 192.168.1.100
 * /crazyswarm_server/world_frame: /world
 * /crazyswarm_server/write_csvs: False
 * /crazyswarm_teleop/csv_file: /home/lis-laptop-...
 * /crazyswarm_teleop/timescale: 0.8
 * /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.0177184, 0.013...
 * /markerConfigurations/0/points/1: [-0.0262914, 0.05...
 * /markerConfigurations/0/points/2: [-0.0328889, -0.0...
 * /markerConfigurations/0/points/3: [0.0431307, -0.03...
 * /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...
 * /markerConfigurations/3/numPoints: 1
 * /markerConfigurations/3/offset: [0.0, -0.01, -0.04]
 * /markerConfigurations/3/points/0: [0.0177184, 0.013...
 * /numDynamicsConfigurations: 1
 * /numMarkerConfigurations: 4
 * /rosdistro: kinetic
 * /rosversion: 1.12.14
 * /vrpn_client_node/broadcast_tf: True
 * /vrpn_client_node/frame_id: world
 * /vrpn_client_node/port: 3883
 * /vrpn_client_node/refresh_tracker_frequency: 1.0
 * /vrpn_client_node/server: 192.168.1.100
 * /vrpn_client_node/update_frequency: 100.0
 * /vrpn_client_node/use_server_time: False

NODES
  /
    crazyswarm_server (crazyswarm/crazyswarm_server)
    crazyswarm_teleop (crazyswarm/crazyswarm_teleop)
    joy (joy/joy_node)
    rviz (rviz/rviz)
    vrpn_client_node (vrpn_client_ros/vrpn_client_node)

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

setting /run_id to 754d29e6-7fcc-11e9-a40b-507b9d6b467f
WARNING: Package "cfclient" does not follow the version conventions. It should not contain leading zeros (unless the number is 0).
process[rosout-1]: started with pid [9133]
started core service [/rosout]
process[crazyswarm_server-2]: started with pid [9150]
process[joy-3]: started with pid [9151]
process[crazyswarm_teleop-4]: started with pid [9152]
process[rviz-5]: started with pid [9166]
process[vrpn_client_node-6]: started with pid [9180]
[ INFO] [1558885174.091926175]: Wait for services...
[ INFO] [1558885174.095806902]: waitForService: Service [/takeoff] has not been advertised, waiting...
[ INFO] [1558885174.110603024]: Connecting to VRPN server at 192.168.1.100:3883
[ INFO] [1558885174.113144360]: Opened joystick: /dev/input/js0. deadzone_: 0.050000.
[ INFO] [1558885174.118346430]: Connection established
[ INFO] [1558885174.118793313]: waitForService: Service [/takeoff] is now available.
[ INFO] [1558885174.121347314]: Manager ready.
check_vrpn_cookie(): VRPN Note: minor version number doesn't match: (prefer 'vrpn: ver. 07.34', got 'vrpn: ver. 07.20  0').  This is not normally a problem.
ch: 1
[ INFO] [1558885174.221816264]: Adding CF: cf1 (radio://0/80/2M/E7E7E7E701, cf1)...
[ INFO] [1558885174.240218830]: CF ctor: 0.018318 s
[ INFO] [1558885174.242287537]: [cf1] Requesting parameters...
[ WARN] [1558885174.246279847]: Don't know ack: Port: 13 Channel: 1 Len: 6
[ WARN] [1558885174.247739394]: Don't know ack: Port: 13 Channel: 1 Len: 6
[ INFO] [1558885174.248646258]: Found variables in cache.
[ WARN] [1558885174.277101682]: [cf1] Link Quality low (0.680000)
[ INFO] [1558885174.676431660]: [cf1] reqParamTOC: 0.436152 s
[ INFO] [1558885174.676475114]: Requesting memories...
[ INFO] [1558885174.679614669]: Memories: 5
[ INFO] [1558885174.689540055]: [cf1] Ready. Elapsed: 0.449265 s
[ INFO] [1558885174.689578046]: CF run: 0.449383 s
update commander/enHighLevel to 1
update stabilizer/controller to 2
update stabilizer/estimator to 3
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] [1558885174.715723334]: [cf1] Update parameters
[ INFO] [1558885174.738034155]: CF Console: CONTROLLER: Using controller 2
[ INFO] [1558885174.740564263]: CF Console: ESTIMATOR: Using estimator 3
[ INFO] [1558885174.766430720]: Update params: 0.076805 s
[ INFO] [1558885174.766561945]: Started 1 threads
[ INFO] [1558885175.123379520]: Found new sender: cf1
[ INFO] [1558885175.123545192]: Creating new tracker cf1
[ WARN] [1558885176.893531232]: [cf1] Link Quality low (0.690000)
[ INFO] [1558885178.029765811]: Takeoff!
[ INFO] [1558885179.709442929]: Takeoff!
[ INFO] [1558885180.171974267]: Takeoff!
[ INFO] [1558885180.481736260]: Takeoff!

I sometimes get the message "[cf1] Link Quality low" even though I have only one cf, about 3 meters away (and I also get this message when I have the cf next to the crazyradio). What is strange is that using the python client app, I always have a perfect link quality.

I also noticed that when I set logging_enabled to false in the launch file, the drone does not respond to any command at all. Checking in the source of crazy_server I noticed that when logging is disabled, no ping is sent to the cf in the runSlow() function of CrazyflieGroup.

I have spent a lot of time trying to figure out how to use crazyswarm but I am not sure what to do now.

I have tried to use the python API in vain. I have tried using the ROS services for high-level commands as well as the topics for low-level commands also in vain. I experience the same issues, the ROS part works fine and I always get the output about the commands I send, but the cf does not always execute the commands.

What could be the problem ?

communication with the cf
firmware (cf and crazyradio) ? I have updated them several time and explained in the documentation of crazyswarm
I saw that there could be problems with the USB3 ports, is that still an issue ? my laptop only has USB3 ports.
Some sort of command buffer being filled in the drone or in the crazyradio ?

If someone has similar issues, did you find any workaround. There should be no such issue for controlling only one drone right ?

Best,

whoenig commented 5 years ago

For the radio, I suggest running rosrun crazyflie_tools comCheck --uri <yourURI>, which measures a lot of statistics for link quality. I don't think there is often an issue with USB3, and if there is it is HW-related and you would have trouble with the cfclient as well.
For the other issues: How recently did you clone (or update) the crazyswarm package? 3 or 4 weeks ago a switched the communication to use the "safemode", but that implementation had a major bug which just got fixed about a week ago. This bug caused behavior similar to what you describe. There were no major bug fixes in the firmware recently (although, the latest crazyswarm uses new messages to broadcast the pose information, so if your crazyswarm clone was older, make sure that you do update the firmware as well.)
Looks like you are also starting vrpn_client node. This is not needed for the crazyswarm (although running it shouldn't cause the issues you described.)

medioman22 commented 5 years ago

Thank you for such a quick reply.

I have ran the command rosrun crazyflie_tools comCheck --uri <yourURI> which gives me very good results.

rosrun crazyflie_tools comCheck --uri radio://0/80/2M/E7E7E7E701
numPacketsReceived: 95.6 %
Avg. roundtrip time: 20.7856 ms
751.88 packets/s sent
link quality: 1

I had already noticed the update on the repo and indeed the results were much better after it.
Indeed I am running the vrpn_client node, to track other objects as part of my application.

After investigating, I managed to get satisfying results by sending several time the same high-level command. Basically, I update the command I send at a rate of 2 Hz, but i send the same command at a rate of 25 Hz to make sure the drone executes it. However, for my specific application I would need to update the command at a frequency of about 10-15 Hz. I am using the Melinger controller as it is very stable, but I am aware that it means that the crazyflie has to recompute a trajectory onboard for each new command. For this reason, I have observed that the crazyflie can't remain stable if I send a new command at a higher rate.

For my application, I need to send continuously position setpoints to the drone, and the setpoints are not changing drastically but they are not known in advance (i.e. I cannot just update a trajectory and ask the drone to follow it). Basically instead of asking the drone to move 0.5m in 1second (which is suited for the high-level ROS services), I would rather like to ask it to move 5cm in 0.1second. However, I have issues with the two options that seem suited for this task:

Using the PID controller instead of the Mellinger controller. The PID controller is much less stable and the drone is drifting. Is there a parameter to adjust the roll and pitch inputs to correct this effect ?
Using the low-level ROS topics (/cf1/cmd_position). I am not entirely sure how they work. When I send some commands the drone starts flying but then the motors stop. Do I need to continuously send position commands ? I have seen on some other issues that the high-level and low-level commands cannot be mixed, is that correct ?

Finally, I still have the same question about the enable_logging parameter. When it is set to false, the drone does not respond at all to any command. Is that the intended behavior ?

Thanks

whoenig commented 5 years ago

PID controller: Not sure, but it should actually behave similarly to the mellinger controller. The performance might depend on which state estimator you use. The PID controller is optimized for Kalman, while mellinger has been optimized for KalmanUSC.
Commands: Ideally you should send cmd_position or cmd_full_state, and not goTo commands for your application. Both of those do not do any onboard planning and use the provided setpoint directly instead. You do need to send the commands fairly frequently - there is a timeout on-board that is triggered after 500ms of no command received (at which point it will try to hover; after 2s the motors shut off). You are correct that mixing low- and high-level commands does not fully work yet. You can take-off using high-level, then moving using low-level, and land using low-level. You cannot switch from low-level to high-level, but you can switch from high-level to low-level.
Enable_logging should only have an effect on streaming data from the CF to the basestation. What you describe sounds like a bug to me. Do you have any more details on that?

medioman22 commented 5 years ago

Hello !

Thank you very much for your answer. I have looked into it, and manage to get something satisfying for my application:

PID controller: indeed after switching the estimator to Kalman (not KalmanUSC) the result is quite good. I have been able to send high-level commands at my desired rate and am quite happy with the results, as the drone trajectory is quite smooth even when I change the setpoints.
Thank you for the explanation, it did not understand that the low-level command had to be sent continuously. I will stick with the high-level commands for now as they provide a neat interface for take-off and landing, and I obtained satisfying results.
Basically, whenever l set Enable_logging to False, hover_swarm.launch works as usual and I can see all the commands being sent, however the drone nerver takes off. In fact, the LED "M4" which usually is green or flickering green does not light up, except at startup. Looking at the source code of the server, I noticed that at startup, a ping message is sent 50 times to each drone listed in the config file. This corresponds to the quick flickering of LED "M4" that I notice when I launch hover_swarm.launch. Here are the corresponding lines in crazyswarm_server.cpp, in CrazyflieGroup::readObjects():
```
// Turn all CFs on
for (const auto& config : cfConfigs) {
Crazyflie cf(config.uri);
cf.syson();
for (size_t i = 0; i < 50; ++i) {
     cf.sendPing();
}
}
```

However, I noticed that when logging is disabled, no ping is sent at all. I read somewhere (forgot where) that a ping should be sent regularly to the cf to keep the connection working. However, in CrazyflieGroup::runSlow():

while(ros::ok() && !m_isEmergency) {
      if (enableLogging) {
        for (const auto& cf : m_cfs) {
          cf->sendPing();
        }
      }
      m_slowQueue.callAvailable(ros::WallDuration(0));
      std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }

the ping is not sent at all if Enable_logging is false.

whoenig commented 5 years ago

About 3: Since the crazyswarm is mostly using broadcast, it is actually intended to not send a ping if enable_logging is set to false. What is not intended is that your CF is not doing anything in that particular case - this does sound like a bug. Note that for a larger swarm, you will need to set enable_logging to false, otherwise you can only control a few (~4) CFs/radio.

USC-ACTLab / crazyswarm

Commands ignored by crazyflie #154