![]() |
NASA Astrobee Robot Software
Astrobee Version:
Flight software for the Astrobee robots operating inside the International Space Station.
|
This is the high-level tool for post-processing raw HTC vive data captured by the low-level driver and recorded to a rosbag. It fuses light capture measurements to track the pose of a robot over time.
In order to calibrate the system, you must first collect a rosbag containing the raw Vive data hw/vive/* and output from the Astrobee localization system loc/pose. Let's assume this bag is located at /path/to/test.bag.
First, examine the bag or use the vive_tool to determine your lighthouse and tracker serial numbers.
Second, declare the locations of the trackers with respect to the body-frame of the robot in your robot config file. If you are using the standard Vive brackets, you can just copy and paste the transforms below:
robot_vive_extrinsics = { { -- port serial = "LHR-08DE963B", pose = transform( vec3(0, -0.1397, -0.1397), quat4(1.0, 0.0, 0.0, 0.0)) },{ -- starboard serial = "LHR-1FC0DEF4", pose = transform( vec3(0, 0.1397, -0.1397), quat4(0.0, 1.0, 0.0, 0.0)) } }
Third, declare the lighthouses in your system in the world config file. It doesn't really matter what transforms you choose, because you will be solving for them as part of the calibration phase.
world_vive_lighthouses = { { -- back serial = "2935772913", pose = transform( vec3(-1.3027, -0.364447, 1.72141), quat4(-0.580631, 0.495535, -0.265315, 0.589)) },{ -- front serial = "668207140", pose = transform( vec3(0, 0, 0), quat4(0, 0, 0, 1)) } }
Fourth, declare the registration transform in your world config file. It doesn't really matter what transform you choose, because you will be solving for them as part of the registration phase.
world_vive_registration = transform( vec3(0.634971, 1.17579, -1.63761), quat4(0.321928, -0.355711, 0.436771, 0.7609629))
Fifth, turn on all optional steps in the solver stage. make sure the following variables are set in astrobee/localization/vive_localization.config.
calibrate = true refine = true register = true
Finally, post-process the bag using the following command:
roslaunch vive_localization vive_localization.launch bag:=/path/to/test.bag rviz:=true
Once the command has run check in the GUI to make sure that there is a good match between the EKF (red) and Vive (green) trajectories, and that the master and slave lighthouses look correct. If this is the case, then follow the instructions written to the console and update the lighthouse and registration information in your world config file.
To do this you will need a calibrated system and a rosbag containing the raw Vive data /hw/vive/*. This bag may also optional have output from the Astrobee localization system at /loc/pose.
First, turn off the calibration and registration steps of the solver. By doing this you use the default values in your world config, which maximizes accuracy. In other words, make sure the following variables are set in astrobee/localization/vive_localization.config.
calibrate = false refine = true register = false
Next, post-process the bag using the following command:
roslaunch vive_localization vive_localization.launch bag:=/path/to/test.bag rviz:=true
Once the command has run check in the GUI to make sure that the Vive (green) trajectory looks sensible. If the bag file included EKF data, you will also see a red trajectory. By default, a CSV file comparing the Vive to EKF results for pose only will be written to ~/.ros/performance.csv. You can change this location with the argument out:=/path/to/file.csv.
roslaunch vive_localization vive_localization.launch bag:=/path/to/test.bag out:=/path/to/performance.csv rviz:=true
There are a number of additional options in the astrobee/localization/vive_localization.config file.