VRPN + Optitrack

[kaushikbalasundar]

Hello,

I am using VRPN with optitrack and trying to do the hover example. I have a few questions that I'll be grateful to your answer for.

1. I am launching a separate VRPN node that streams the pose of the marker. In MOTIV on my windows system, I created a rigid body using three markers for a single CF and called it 'cf1'.

2. Why does hover_swarm need the optitrack host pc's IP address in this configuration since VRPN is doing the streaming? What information is it getting additionally that VRPN isn't giving it already? Is this for other configurations in case the streaming is done by say mocap instead?

3. The hovering isn't consistent. There seems to be some inherent drift and probably needs trimming of the roll/pitch. How do you suggest I go about doing that?

[whoenig]

1./2.: Crazyswarm has built-in VRPN support for lowest possible latency. That means, you should not launch a VRPN node yourself.

3.: Which flight mode/script are you using for your tests?

[kaushikbalasundar]

1/2. How do I run streaming with VRPN on crazyswarm? Is it with mocap_helper.launch, by changing the "motion_capture_type" to "vrpn", and using the corresponding motion_capture_host_name? If this is the case, is this to be run before launching hover_swarm.launch? I did not find anything regarding VRPN in the documentation and would appreciate if you could clarify this. So the order of launch would be:

roslaunch crazyswarm mocap_helper.launch
roslaunch crazyswarm hover_swarm.launch
python3 hello_world.py

Is this correct?

EDIT 1: On running roslaunch crazyswarm mocap_helper.launch, I am able to see the rigid body pose but not the corresponding cf1 TF on rviz. I can only see it if I run the vrpn_client_ros node launch file separately.

EDIT 2: I got the CFs to show up on Rviz., by changing the motion_capture_type to vrpn (it was optitrack initially), but I am getting the following warning: No updated pose for motion capture object cf1. In this configuration, running hello_world.py causes the drone to flip over. I have however verified the coordinate representation of frames as detailed in 3.

In hover_swarm.launch I have:

<?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>
      # Logging configuration (Use enable_logging to actually enable logging)
      genericLogTopics: ["log1"]
      genericLogTopicFrequencies: [10]
      genericLogTopic_log1_Variables: ["stateEstimate.x", "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: 1 # 1: complementary, 2: kalman
          controller: 1 # 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: "vrpn" # one of none,vicon,optitrack,optitrack_closed_source,qualisys,vrpn
      object_tracking_type: "motionCapture" # one of motionCapture,libobjecttracker
      send_position_only: False # set to  S False to send position+orientation; set to True to send position only
      motion_capture_host_name: "172.16.51.140"
      # motion_capture_interface_ip: "" # optional for optitrack with multiple interfaces
      save_point_clouds: "/dev/null" # set to a valid path to log mocap point cloud binary file.
      print_latency: False
      write_csvs: False
      force_no_cache: False
      enable_parameters: True
      enable_logging: False
      enable_logging_pose: 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>

This is my mocap_helper.launch

<?xml version="1.0"?>
<launch>

  <node pkg="crazyswarm" type="mocap_helper" name="mocap_helper" output="screen" >
    <rosparam>
      # tracking
      motion_capture_type: "vrpn" # one of vicon,optitrack,optitrack_closed_source,qualisys,vrpn
      motion_capture_host_name: "172.16.51.140" #optitrack pc
      # motion_capture_interface_ip: "" # optional for optitrack with multiple interfaces
    </rosparam>
  </node>
</launch>

3. I am testing the hover functionality by running hello_world.py and using only one CF. I am ensuring that the positive direction of the x axis in my world frame is aligned with the positive direction of the x axis of my CF frame, and that front of the CF is aligned with the positive direction of the X axis in my CF frame. In other words, moving the CF in the direction of the antenna, makes the X value increase in the positive direction. The Y and Z axes also align with that of the world frame's coordinates and is right handed. Is there anything else I might be doing incorrectly? Sometimes the drone flips over or flies along the x or y axes without gaining altitude. However, a few times I have seen the drone take off (although with some drift along the roll and pitch axes) and then land as expected. The hello_world.py simulation seems to work fine. I am also not able to read the battery voltage from the CF when running chooser.py. It constantly says 0.0V. I have however updated my radio firmware to the latest version (99.55). Some details of my system setup:

Motiv Version: 2.3
Platform: Ubuntu 20 with Python 3.8.10, ROS Noetic

[whoenig]

• VRPN is only for users with "unsupported" motion capture systems. Since you use OptiTrack with a supported Motive version, you should use "optitrack" as "motion_capture_type".
• mocap_helper.launch is just for debugging connectivity problems. It does not publish anything and therefore you won't see results in rviz (only printed on the terminal). You only need to run hover_swarm.launch and your python script of choice.
• In general I recommend using libobjecttracker for OptiTrack, because Motive doesn't use any dynamics filtering and can cause axis-flips (which lead to crashes). If you have trouble tracking, the most robust approach is a single marker per Crazyflie using the libobjecttracker tracking mode.
• For 3., Make sure you use the ROS coordinate system, not the Motive coordinate system.
• The battery voltage reading should work. Are you using the latest official firmware (master or 2021.06 release)?

[kaushikbalasundar]

I got hover and waypoint tracking to work using the following steps. Documenting it here so that users in the future can use as a debugging guide:

1. Make sure the firmware on the drone and the radio is up to date.
2. Make sure that the x-axis of the drone is aligned with the front of the drone. If you move the drone forward along the direction of the antenna, the x axis value needs to increase.
3. In motiv, makes sure your rigid bodies are named as 'cf1' , 'cf2' etc, and the streaming ID is set to 1, 2, etc., respectively. Make sure VRPN streaming is turned on. Use the default multicast address.
4. Use mocap_helper.launch to interface with VRPN. Use: motion_capture_type: "vrpn". Prior to doing this make sure you can stream data with VRPN using the vrpn-client-ros.
5. In hover_swarm.launch make the following changes:

motion_capture_type: "vrpn" # one of none,vicon,optitrack,optitrack_closed_source,qualisys,vrpn
object_tracking_type: "motionCapture" # one of motionCapture,libobjecttracker 
motion_capture_host_name: "172.16.xx.xxx" #Replace with your hostname that you verified is streaming with VRPN

6. The sequence used to run:
   (a) roslaunch crazyswarm mocap_helper.launch
   (b) roslaunch crayzyswarm hover_swarm.launch
   (c) python3 hello_world.py

When the RViz window opens up in (b) you should be able to see the cfs.

hello_world.py works for me even with the "No updated pose for motion capture object 'cf1' " warning on the terminal. Hope this helps future users.