Pose topic using Crazyswarm

All discussions related to the Loco Positioning system
Post Reply
aramos
Beginner
Posts: 6
Joined: Tue Feb 12, 2019 10:27 am

Pose topic using Crazyswarm

Post by aramos »

Hi everyone,

Since Whoenig recommended us to use the Crazyswarm package instead of the crazyflie_ros package to fly more than one cf2 (viewtopic.php?f=16&t=3362) we have been flying properly with it. However, we have a doubt about the information it offers over ROS. In contrast with crazyflie_ros, now there is no pose topic and we would like to know how to get this information because we need it for our algorithm.

Best regards,
Agustín.
whoenig
Expert
Posts: 395
Joined: Mon Oct 27, 2014 2:55 am

Re: Pose topic using Crazyswarm

Post by whoenig »

Which topic are you referring to? You can still get the pose of each robot (they are part of ROS tf). There are currently some differences in the feature set between the two solutions, so if something else is missing, you might need to port some code from crazyflie_server to crazyswarm_server. If you tell me which topic you are missing, I can help with the details.
aramos
Beginner
Posts: 6
Joined: Tue Feb 12, 2019 10:27 am

Re: Pose topic using Crazyswarm

Post by aramos »

I am referring to the topic crazyflie/pose. When I used lps-ros with crazyflie_ros I could see the pose of the cf with it. Now, when I launch hover_swarm.launch I can see the following topics and services:

ROSTOPIC LIST:

/cf4/cmd_full_state
/cf4/cmd_position
/cf4/cmd_stop
/clicked_point
/diagnostics
/initialpose
/joy
/move_base_simple/goal
/pointCloud
/rosout
/rosout_agg
/tf
/tf_static
/virtual_interactive_object

ROSSERVICE LIST:

/cf4/go_to
/cf4/land
/cf4/set_group_mask
/cf4/start_trajectory
/cf4/takeoff
/cf4/update_params
/cf4/upload_trajectory
/crazyswarm_server/get_loggers
/crazyswarm_server/set_logger_level
/crazyswarm_teleop/get_loggers
/crazyswarm_teleop/set_logger_level
/emergency
/go_to
/joy/get_loggers
/joy/set_logger_level
/land
/next_phase
/rosout/get_loggers
/rosout/set_logger_level
/rviz/get_loggers
/rviz/reload_shaders
/rviz/set_logger_level
/start_trajectory
/takeoff

I can command take_off, go_to and land services correctly, however, there is no topic which publish the pose of the cf. Furthermore, the topics /tf and /tf_static does not publish anything and rviz is empty.
whoenig
Expert
Posts: 395
Joined: Mon Oct 27, 2014 2:55 am

Re: Pose topic using Crazyswarm

Post by whoenig »

What you describe is actually not part of crazyflie_ros, but lps-ros, namely this file: https://github.com/bitcraze/lps-ros/blo ... _bridge.py. This current solution is not perfect, because position and orientation are not necessarily logged at the same time. To implement this properly we need:
1. firmware change to allow logging of the "compressed" state.
2. Implement the logic of lps_ekf_bridge in crazyflie_ros.
3. Implement the logic in the crazyswarm.

Note that at least the first 2 are of high interest anyways, because of the new roadrunner product. I added https://github.com/whoenig/crazyflie_ros/issues/130 to track that. For the crazyswarm I added https://github.com/USC-ACTLab/crazyswarm/issues/145.

Edit: Note that the tf in the crazyswarm is only populated if you use a motion capture system.
jamador
Beginner
Posts: 7
Joined: Mon Apr 29, 2019 2:22 pm

Re: Pose topic using Crazyswarm

Post by jamador »

Hi Whoenig,

I am a researcher of GRVC, working with aramos. We got to publish pose topic in the crazyswarm API using your last post as guide. Thank you so much for your help.

We have some doubts about it. First, in your proposed code you log x,y,z and the compressed quaternion:

{"stateEstimate", "x"},
{"stateEstimate", "y"},
{"stateEstimate", "z"},
{"stateEstimateZ", "quat"}

However, the stateEstimateZ.quat can not be founded by crazyswarm. Furthermore, when we look for in the firmware variable list (this list appears in the log configurations in cfclient) , to obtain the pose, we can find the stateEstimate but not stateEstimateZ to get compressed orientation. Currently, we are using roll, pitch and yaw that stateEstimate provides. Is this a good way to do that? How could we use the compressed estimation? When we will be sure how to do this, we will commit.

On the other hand, Do you know if is there any document which describe the firmware variable list that we mentioned above?

Thank you in advanced,
best regards.

Juan Manuel Amador Olivares.
whoenig
Expert
Posts: 395
Joined: Mon Oct 27, 2014 2:55 am

Re: Pose topic using Crazyswarm

Post by whoenig »

Hi,

Which firmware are you using? The compressed state estimate is only available in very recent firmwares (official and crazyswarm). The logic has been added to crazyflie_ros as well, but not yet to the crazyswarm.

For the list of variables, you can use the cfclient, or `rosrun crazyflie_tools listLogVariables`. Unfortunately, there is no documentation on the meaning of the variables, other than the firmware source code.
jamador
Beginner
Posts: 7
Joined: Mon Apr 29, 2019 2:22 pm

Re: Pose topic using Crazyswarm

Post by jamador »

Hi,

I finally got it (with the latest crazyflie-firmware). I do the same you do in crazyflie_ros, creating the new topic and publishing using the custom logBlock:

Code: Select all

struct logPose {
    float x;
    float y;
    float z;
    float roll;
    float pitch;
    float yaw;
} __attribute__((packed));

.
.
.

std::function<void(uint32_t, logPose*)> cb = std::bind(&CrazyflieROS::onPoseData, this, std::placeholders::_1, std::placeholders::_2);

    logBlockPose.reset(new LogBlock<logPose>(
      &m_cf,{
        {"stateEstimate", "x"},
        {"stateEstimate", "y"},
        {"stateEstimate", "z"},
        {"stateEstimate", "roll"},
        {"stateEstimate", "pitch"},
        {"stateEstimate", "yaw"}
      }, cb));
    logBlockPose->start(2.5);
Do you know how the final line "logBlockPose->start" works? the argument is a coefficient of the period of the logging?

Now, I am testing the lighthouse code, and I want to publish in ros the angles computated by "lighthouse.c". Log varuables are alredy coded and I can see the values in cfclient. To do that easily i want to use the same topics I used to publish the pose, only changing the logBlock and the name of the variables (and the publish function):

Code: Select all

std::function<void(uint32_t, logPose*)> cb = std::bind(&CrazyflieROS::onPoseData, this, std::placeholders::_1, std::placeholders::_2);

    logBlockPose.reset(new LogBlock<logPose>(
      &m_cf,{
        {"lighthouse", "angle0x"},
        {"lighthouse", "angle0y"},
        {"lighthouse", "angle1x"}
      }, cb));
    logBlockPose->start(2.5);
but It does not work. "crazyswarm_server" finds the log variables (because If I am wrong writting the variable name, the node exits), but It only reads 0, not the value I can see in "cfclient", and happens with every log variable of "lighthouse.c". Do you know what is happening?

Thank you for your great work in Crazyswarm and your support!
Post Reply