Only released in EOL distros:  

ethzasl_drivers: canon_vbc50i | panasonic_blc111 | vicon_bridge

Package Summary

This is a driver providing data from VICON motion capture systems. It is based on the vicon_mocap package from the starmac stacks. Additionally, it can handle multiple subjects / segments and allows to calibrate an origin of the vehicle(s) as this is somehow tedious with the VICON Tracker.

ethzasl_drivers: canon_vbc50i | panasonic_blc111 | vicon_bridge

Package Summary

This is a driver providing data from VICON motion capture systems. It is based on the vicon_mocap package from the starmac stacks. Additionally, it can handle multiple subjects / segments and allows to calibrate an origin of the vehicle(s) as this is somehow tedious with the VICON Tracker.

Package Summary

This is a driver providing data from VICON motion capture systems. It is based on the vicon_mocap package from the starmac stacks. Additionally, it can handle multiple subjects / segments and allows to calibrate an origin of the vehicle(s) as this is somehow tedious with the VICON Tracker.

Package Summary

This is a driver providing data from VICON motion capture systems. It is based on the vicon_mocap package from the starmac stacks. Additionally, it can handle multiple subjects / segments and allows to calibrate an origin of the vehicle(s) as this is somehow tedious with the VICON Tracker.

Package Summary

This is a driver providing data from VICON motion capture systems. It is based on the vicon_mocap package from the starmac stacks. Additionally, it can handle multiple subjects / segments and allows to calibrate an origin of the vehicle(s) as this is somehow tedious with the VICON Tracker.

NOTE: The modern, Catkinized source is now here: https://github.com/ethz-asl/vicon_bridge

Quick Start

roslaunch vicon_bridge vicon.launch

You may need to set the "datastream_hostport" parameter to your vicon computer's ip/hostname

Operation

The vicon_bridge node initiates a connection with the Vicon data source (e.g. Nexus or Tracker) via the DataStream API. The parameter ~datastream_hostport should be set to the IP address or hostname and port (joined with a colon) of the DataStream server machine, e.g. 192.168.0.254:801 (801 is the default port). The parameter ~stream_mode is used for the call to the DataStream SetStreamMode() method; valid values are ServerPush and ClientPull. Note: We experienced buffering problems and framedrops when using ServerPush when more than one object is successfully tracked. We therefore recommend ClientPull, unfortunately at the cost of some additional delay.

All available subjects and segments are recognized automatically and are published as tf transform and geometry_msgs/TransformStamped as vicon/<subject_name>/<segment_name>.

Segment Origin Calibration

when an object is created in vicon tracker, it places the origin more or less arbitrary in the object. To calibrate the origin of a segment, place the vehicle at the vicon origin that you set during vicon calibration, and measure/estimate the height (z offset) of your desired origin from the floor. Then run

rosrun vicon_bridge calibrate <subject name> <segment name> <z offset>

Poses are grabbed, averaged and the vehicle is placed in the vicon's origin. The calibration parameters are stored on the parameter server as ~/<subject_name>/<segment_name>/zero_pose (see Origin Calibration Parameters). On startup, the node checks if calibration parameters are available. Do this the first time while running rviz and observe what happens (and if everything went correct ;-) ).

Simulating bad Data

Sometimes vicon data is too perfect ;-) This motivated tf_distort, which adds different types of noise, adds delay or reduces the framerate to test how robots work with bad data.

rosrun vicon_bridge tf_distort

Then run reconfigure_gui to set the parameters. This is work in progress, suggestions are welcome.

Nodes

vicon_bridge

interface to the Vicon DataStream SDK

Published Topics

vicon/<subject_name>/<segment_name> (geometry_msgs/TransformStamped)
  • pose of the <subject>/<segment>
vicon/markers (vicon_bridge/Markers)
  • publishes all labeled and unlabeled markers. Labeled markers are not affected by origin calibration.

Services

~/calibrate_segment (vicon_bridge/viconCalibrateSegment) ~/grab_vicon_pose (vicon_bridge/viconGrabPose)
  • grabs n poses for a given <subject>/<segment> and returns the averaged pose

Parameters

Static Parameters
parameters that are statically set
~stream_mode (string, default: "ClientPull)
  • mode to connect to the DataStream server. Values: "ClientPull", "ServerPush".
~datastream_hostport (string, default: vicon:801)
  • host and port of the DataStream server (<ip/hostname>:<port>).
~tf_ref_frame_id (string, default: world)
  • tf reference frame id
Origin Calibration Parameters
Origin pose for each <subject/segment>, i.e. the pose of the segment seen from the vicon frame.
~/<subject_name>/<segment_name>/zero_pose/orientation/w (double, default: 1)
~/<subject_name>/<segment_name>/zero_pose/orientation/x (double, default: 0)
~/<subject_name>/<segment_name>/zero_pose/orientation/y (double, default: 0)
~/<subject_name>/<segment_name>/zero_pose/orientation/z (double, default: 0)
~/<subject_name>/<segment_name>/zero_pose/position/x (double, default: 0)
~/<subject_name>/<segment_name>/zero_pose/position/y (double, default: 0)
~/<subject_name>/<segment_name>/zero_pose/position/z (double, default: 0)

Parameters Set

Origin Calibration Parameters
Origin pose for each <subject/segment>, i.e. the pose of the segment seen from the vicon frame.
~/<subject_name>/<segment_name>/zero_pose/orientation/w (double, default: 1)
~/<subject_name>/<segment_name>/zero_pose/orientation/x (double, default: 0)
~/<subject_name>/<segment_name>/zero_pose/orientation/y (double, default: 0)
~/<subject_name>/<segment_name>/zero_pose/orientation/z (double, default: 0)
~/<subject_name>/<segment_name>/zero_pose/position/x (double, default: 0)
~/<subject_name>/<segment_name>/zero_pose/position/y (double, default: 0)
~/<subject_name>/<segment_name>/zero_pose/position/z (double, default: 0)

Provided tf Transforms

worldvicon/<subject_name>/<segment_name>
  • tf transform from world to vicon/<subject_name>/<segment_name>.from can be changed with ~tf_ref_frame_id.

calibrate

Node to call the origin pose calibration service for a give subject and segment name plus additional height of the origin.

Services Called

~/calibrate_segment (vicon_bridge/viconCalibrateSegment)

Wiki: vicon_bridge (last edited 2015-04-29 18:46:18 by IliaBaranov)