no cfs in rviz when using lps

[Supredan]

Hi,
I am using LPS to run crazyswarm, everything works fine, except that rviz cannot display the cf's trace.
but here says :

Furthermore, rviz will show the estimated pose of all CFs.

i also see here #108 :

Yes - there is no data streaming. One possibility would be to enable logging, log the current state estimate, and sent this to rviz. This hasn't been implemented so far (and you would need a good number of radios for a high update rate.)

But I still can’t subscribe any useful topic in rviz after Enable the log. Is there any more detailed instructions or tutorials? Thanks very much!!!

[Supredan]

<?xml version="1.0"?>
<launch>
  <arg name="joy_dev" default="/dev/input/js0" />

  <rosparam command="load" file="$(find crazyswarm)/launch/crazyflieTypes.yaml" />
  <rosparam command="load" file="$(find crazyswarm)/launch/crazyflies.yaml" />

  <node pkg="crazyswarm" type="crazyswarm_server" name="crazyswarm_server" output="screen" >
    <rosparam>
      world_frame: "/world"
      # Logging configuration (Use enable_logging to actually enable logging)
      genericLogTopics: ["log1"]
      genericLogTopicFrequencies: [10]
      genericLogTopic_log1_Variables: ["stateEstimate.z", "ctrltarget.x"]
      # firmware parameters for all drones (use crazyflieTypes.yaml to set per type, or
      # allCrazyflies.yaml to set per drone)
      firmwareParams:
        commander:
          enHighLevel: 1
        stabilizer:
          estimator: 2 # 1: complementary, 2: kalman
          controller: 2 # 1: PID, 2: mellinger
        ring:
          effect: 16 # 6: double spinner, 7: solid color, 16: packetRate
          solidBlue: 255 # if set to solid color
          solidGreen: 0 # if set to solid color
          solidRed: 0 # if set to solid color
          headlightEnable: 0
        locSrv:
          extPosStdDev: 1e-3
          extQuatStdDev: 0.5e-1
        kalman:
          resetEstimation: 1
      # tracking
      motion_capture_type: "none" # one of none,vicon,optitrack,qualisys,vrpn
      object_tracking_type: "libobjecttracker" # one of motionCapture,libobjecttracker
      send_position_only: False # set to False to send position+orientation; set to True to send position only
      vicon_host_name: "vicon"
      # optitrack_host_name: "optitrack"
      # qualisys_host_name: "10.0.5.219"
      # qualisys_base_port: 22222
      # vrpn_host_name: "vicon"
      save_point_clouds: ~/pointCloud.ot
      print_latency: False
      write_csvs: False
      force_no_cache: False
      enable_parameters: True
      enable_logging: True
    </rosparam>
  </node>

  <node name="joy" pkg="joy" type="joy_node" output="screen">
    <param name="dev" value="$(arg joy_dev)" />
  </node>

  <node pkg="crazyswarm" type="crazyswarm_teleop" name="crazyswarm_teleop" output="screen">
    <param name="csv_file" value="$(find crazyswarm)/launch/figure8_smooth.csv" />
    <param name="timescale" value="0.8" />
  </node>

  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find crazyswarm)/launch/test.rviz"/>

  <!-- <node pkg="rqt_plot" type="rqt_plot" name="rqt_plot_x" args="/cf2/log1/values[0]"/> -->
  <!-- <node pkg="rqt_plot" type="rqt_plot" name="rqt_plot_roll" args="/cf1/log1/values[2] /cf1/log1/values[3]"/> -->

</launch>

BTW here is my hover_swarm.launch, should i make some change in Logging configuration?

[jpreiss]

RViz only shows the poses when you are using a motion capture system. With LPS, the PC doesn't know the positions.

There is no built-in functionality in Crazyswarm to convert the logging variables into ROS transforms that RViz will show. You would need to write a ROS node that converts the logging topics (like cf2/log1/values[0]) into ROS transforms, and add it to your launch file.

If you write such a utility, we would appreciate it if you contribute the code back to Crazyswarm in a PR for the benefit of other users!

Like Wolfgang mentioned in #108, this will use a lot of radio bandwidth.

For info on configuring the logging system, check pp31-32 of Flying Multiple UAVs Using ROS (pdf). It would be good to add this to Crazyswarm's docs in the future.

[sabdi1]

@whoenig

You can just set enable_logging_pose to true, and you should get a /cf/pose topic that you can visualize in rviz. See https://github.com/USC-ACTLab/crazyswarm/blob/master/ros_ws/src/crazyswarm/src/crazyswarm_server.cpp#L201. This updates every 10 Hz by default.

As mentioned here, I set the enable_logging_pose to True and enable_logging to False, but I could not find any cfx/pose/ topic being published. If I set both enable_logging_pose and enable_logging to True, I get an error saying

what(): Could not find stateEstimate.x in log toc!

Do I set both of them to true? Am I missing something here?

[whoenig]

Both need to be enabled.

[WaterHyacinthInNANHU]

0. Yes, you are correct that enable_logging needs to be set to True.
1. For custom log topics here. For the pose, you need to change the source code here. You might also be able to support more CFs per radio by increasing the value here.
2. Yes, but each Crazyflie is bound to a channel and each radio is serving exactly one channel. So if you use more channels, you will need more radios and each radio will server fewer Crazyflies.

Sorry for getting back to you late.Thanks for your reply!
I tested with 3 PAs with 2 crazyflies logging to each PA and it works just fine!

[sabdi1]

Both need to be enabled.

I'm able to get the pose information with LPS from /cf1/pose/ topic and pose is correct. Now I have replaced the LPS deck with flow deck. The position information in /cf1/pose/ topic doesn't seem to be right. Should I be doing something different for flow deck?

[whoenig]

No, it's the same approach. Since the flow deck can only directly measure velocity, the position will drift over time.