After redoing the mocap calibration, we should verify that the following (in https://github.com/UM-ARM-Lab/lightweight_vicon_bridge/blob/master/launch/mocap_to_kinect_static_transforms.launch) is still correct:

        <node pkg="tf2_ros" type="static_transform_publisher" name="kinect2_roof_tf_broadcaster" required="true"
              args="2.219   -0.327   1.993
                   -0.718206154753868   0.693198976663552  -0.047013494809794   0.038010910697280
                    mocap_world kinect2_roof_link">
            <param name="use_sim_time" value="$(arg use_sim_time)"/>
        </node>

Tasks:

• create new vicon_interfaces package for messages

  • Discuss: why do we want to separate out the interfaces of vicon? Is there a use case where we want to use the interface but not the actual mocap system

• update the code (zixuan/ros2 branch)

  • lightweight_vicon_bridge_node.cpp
  • tf_publisher_node.cpp
  • raw_marker_bridge_node.cpp

• Existing issues

  • Can't link vicon_sdk properly during compilation. Need to configure LD_LIBRARY_PATH mannually. We should double check the CMakeLists.txt
  • Can't build with colcon build --symlink-install. Need to rebuild if launch script is modified
  • attribute type of param is no longer supported, causing type mismatch problem
  • Substitution doesn't work in launch script
  • Double declaration error for use_sim_time
  • Logging format is a bit messy
  • "If you want a counterpart of tf::StampedTransform in tf2, you need to go with geometry_msgs::TransformStamped". Check whether they have the same structures

• clean up raw_marker_publisher

In the past calibration, the transforms that I had setup enabled the world floor and victor to line up well. They no longer do.

Here are the results we got from calibrating Val's torso with the Vicon markers. I'm not sold on the accuracy, due to all the backlash in the system, but it may be better than what we had.

parent_frame_id: mocap_ValOnHusky_ValOnHusky
frame_id: val_root

pose:
  position:
    x: -0.0318574064543
    y: -0.0313317285606
    z: -0.0304785243467
  orientation:
    x: -0.0179397003967
    y: 0.0105275909015
    z: -0.708665575372
    w: 0.70523786006

roslaunch lightweight_vicon_bridge vicon_bridge.launch will report success and "Streaming data..." even when the vicon is not running properly and not streaming data.

There should be a red error message if the ros-portion launches, but no data is observed, indicating that the vicon system should be restarted

In the position that the Kinect is at on the roof, the orientation of the transform is not stable, particularly at the distance of the point cloud.

One potential solution would be to add a filter to collect and average the positions over the last some number of seconds, and broadcast a new transform. Alternatively this could be done as a calibration step before each experiment, then use a static transform publisher.