Online cmdPosition & cmdFullState

[hanzheteng]

Hi @whoenig ,

Nice to see your latest update on cmdFullState!

From the most recent commit, I noticed that you added trajectories for takeoff and landing processes. Does it mean we can only do offline planning for now?

I tried my script with goTo and cmdFullState API. The crazyflie responses to goTo command but ignores cmdFullState command. Does it have some relation with the high-level/low-level command privilege?

Usually it should response to cmdFullState (low level cmd) first, even though some high level command like goTo is running. (In my case they are not running at the same time, I run goTo first and then cmdFullState.)

My goal is to make the crazyflie track a wand in my hand. I also added similar structures in crazyflie.py so that I can use cmdPosition API as well, because I saw this part was added to crazyswarm_server node but not in crazyflie.py wrapper yet. But anyway, it's not working just like cmdFullState.

By the way, on the ros node/topic graph, I can only see crazyflieAPI node publish /cf1/cmdFullState topic, but no other node subscribe it. The crazyswarm_server node only publish to /tf and /pointCloud topic.

Thank you again for your great contribution.

[whoenig]

The offline trajectories are just examples. Of course, you can generate setpoints dynamically instead (as long as you can generate it realtime, it should be fine).

You are right that there is a priority, and that cmdFullState is given higher priority. Did you enable the high level mode in the firmware by setting the enHighLevel parameter to 1?

cmdPosition should work in a similar fashion, although I did not test it. Is your code somewhere online in a fork?

crazyswarm_server should subscribe to that topic, see https://github.com/USC-ACTLab/crazyswarm/blob/master/ros_ws/src/crazyswarm/src/crazyswarm_server.cpp#L207.

[hanzheteng]

Thank you for your quick reply.

The main issue is that the crazyflie didn't response to real-time cmdPosition or cmdFullState commands during my tests.

I have to check firmware later on. Thanks for this info.

I just implemented it the same way as you did for cmdFullState in crazyflie.py file. We do have a fork repo but I haven't pushed the latest code yet. I will have access to the code a couple of days later.

Yes, I do agree the crazyswarm_server should subscribe to those topics, but I didn't see them on ROS graph. Probably something goes wrong with the initialization part.

Will get back to this later. Thanks!

[whoenig]

Does executing cmdFullState.py work for you? That's the script I tested the cmdFullState topic with in our lab. I never tested cmdPosition.

[hanzheteng]

cmdFullState.py works for me. In this script, all the control input is from cmdFullState topic right? Did you try using high level commands takeoff first and then low level commands cmdFullState?

I am not sure if there are some restrictions in the firmware preventing us from mixing these commands. As you mentioned, enHighLevel parameter needs to be 1. But I didn't find a way to check this parameter. The firmware on my robot is from prebuilt directory. The initialization function of Crazyflie class doesn't offer this option as well.

A similar issue appears on my test of a single crazyflie. I am not sure whether it relates to the firmware on the robot. During the experiment, I used only crazyflie_ros stack. After launching the crazyflie_server and adding my robot by crazyflie_add, I tried a simple takeoff service call on command line. No response. But if I used crazyflie_controller to send cmd_vel, it worked. Seems like it only responses to low level commands at that moment.

By the way, what's the difference between the crazyswarm firmware and Bitcraze firmware? Did you modify the high level commands? From the commits I saw you mentioned this idea, but not sure what exactly the influence is.

[whoenig]

The example script cmdFullState.py only uses the cmdFullState topic. It should be possible to mix low- and high-level commands: low-level commands have priority, high-level commands are only executed after there has been no low-level command for about 500ms, see https://github.com/bitcraze/crazyflie-firmware/blob/master/src/modules/src/commander.c#L79-L105.

The crazyswarm and Bitcraze firmwares are very similar already. In fact, it is possible to use the official firmware with the Crazyswarm. There is no difference in terms of the high/low-level command behavior. The Crazyswarm firmware has a different extended Kalman filter that produces (slightly) smoother results in our testing.

Crazyflie_ros supports low- and high-levels the same way the crazyswarm does, but is not well documented. You can find an example in https://github.com/whoenig/crazyflie_ros/blob/master/crazyflie_demo/scripts/test_high_level.py.

[hanzheteng]

Thank you for the explanation to firmware.

You're right. The commander.c file shows that enHighLevel parameter is set to False by default. That's the reason why the crazyflie didn't response to my takeoff service call. After executing test_high_level.py script for once, it can response to the service call. Thanks!

However, during my experiments, it seems like test_high_level.py can only be used when Kalman filter is enabled (estimator 2). The robot will go to random places if I set the estimator to be 1 or 3.

Also, if using VICON (by publishing to external position) and flow deck at the same time, it seems like the robot will consider flow desk first as the localization method, because I saw the robot swing a bit before reaching some certain height.