i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added example of using global_pose topic (edge prior, https://github.com/introlab/rtabmap/pull/196)

Browse Source
This commit is contained in:
matlabbe
2018-02-09 16:42:48 -05:00
parent 457adfa3c1
commit a735069ba9
3 changed files with 147 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
launch/tests/test_prior.launch
+82
View File
@@ -0,0 +1,82 @@
<launch>
<!-- Example with rgbd datasets:
$ wget http://vision.in.tum.de/rgbd/dataset/freiburg3/rgbd_dataset_freiburg3_long_office_household.bag
$ rosbag decompress rgbd_dataset_freiburg3_long_office_household.bag
$ chmod +x test_prior_rename_kinect_bag_tf.py
$ ./test_prior_rename_kinect_bag_tf.py
We simulate an external "global_pose" by republishing ground truth TF (VICON) with some
covariance, normally you should not have to use test_prior_tf_to_pose.py as the node
publishing the global pose would give it directly as a pose with correct covariance.
Rename all child_frame_id "/kinect" to "/kinect_gt" Tf in the bag
using test_prior_rename_kinect_bag_tf.py in this directory!
$ roslaunch rtabmap_ros test_prior.launch
$ chmod +x test_prior_tf_to_pose.py
$ ./test_prior_tf_to_pose.py
$ rosbag play -.-clock rgbd_dataset_freiburg3_long_office_household_tf_renamed.bag
-->
<param name="use_sim_time" type="bool" value="True"/>
<!-- Choose visualization -->
<arg name="rviz" default="true" />
<arg name="rtabmapviz" default="false" />
<!-- TF FRAMES -->
<node pkg="tf" type="static_transform_publisher" name="world_to_map"
args="0.0 0.0 0.0 0.0 0.0 0.0 /world /map 100" />
<group ns="rtabmap">
<!-- Odometry -->
<node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
<remap from="rgb/image" to="/camera/rgb/image_color"/>
<remap from="depth/image" to="/camera/depth/image"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
<remap from="odom" to="vis_odom"/>
<param name="odom_frame_id" type="string" value="vis_odom"/>
<param name="frame_id" type="string" value="kinect"/>
</node>
<!-- Visual SLAM -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="frame_id" type="string" value="kinect"/>
<param name="RGBD/CreateOccupancyGrid" type="string" value="false"/>
<param name="Optimizer/PriorsIgnored" type="string" value="false"/>
<remap from="rgb/image" to="/camera/rgb/image_color"/>
<remap from="depth/image" to="/camera/depth/image"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
<remap from="odom" to="vis_odom"/>
<remap from="global_pose" to="/global_pose"/>
<param name="ground_truth_frame_id" type="string" value="world"/>
<param name="ground_truth_base_frame_id" type="string" value="kinect_gt"/>
</node>
<!-- Visualisation -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="false"/>
<param name="subscribe_odom_info" type="bool" value="true"/>
<param name="queue_size" type="int" value="30"/>
<param name="frame_id" type="string" value="kinect"/>
<remap from="rgb/image" to="/camera/rgb/image_color"/>
<remap from="depth/image" to="/camera/depth/image"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
<remap from="odom" to="vis_odom"/>
</node>
</group>
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbdslam_datasets.rviz"/>
</launch>
launch/tests/test_prior_rename_kinect_bag_tf.py
+19
View File
@@ -0,0 +1,19 @@
#!/usr/bin/env python
import rosbag
from tf.msg import tfMessage
with rosbag.Bag('rgbd_dataset_freiburg3_long_office_household_tf_renamed.bag', 'w') as outbag:
for topic, msg, t in rosbag.Bag('rgbd_dataset_freiburg3_long_office_household.bag').read_messages():
if topic == "/tf" and msg.transforms:
newList = [];
for m in msg.transforms:
if m.child_frame_id != "/kinect":
newList.append(m)
else:
m.child_frame_id = "/kinect_gt"
newList.append(m)
print 'kinect frame renamed!'
if len(newList)>0:
msg.transforms = newList
outbag.write(topic, msg, t)
else:
outbag.write(topic, msg, t)
launch/tests/test_prior_tf_to_pose.py
Executable
+46
View File
@@ -0,0 +1,46 @@
#!/usr/bin/env python
import rospy
import tf
import numpy
import tf2_ros
from geometry_msgs.msg import PoseWithCovarianceStamped
if __name__ == '__main__':
rospy.init_node('tf_to_pose', anonymous=True)
listener = tf.TransformListener()
frame = rospy.get_param('~frame', 'world')
childFrame = rospy.get_param('~child_frame', 'kinect_gt')
outputFrame = rospy.get_param('~output_frame', 'kinect')
cov = rospy.get_param('~cov', 1)
rateParam = rospy.get_param('~rate', 30) # 10hz
pub = rospy.Publisher('global_pose', PoseWithCovarianceStamped, queue_size=1)
print 'start loop!'
rate = rospy.Rate(rateParam)
while not rospy.is_shutdown():
poseOut = PoseWithCovarianceStamped()
try:
now = rospy.get_rostime()
listener.waitForTransform(frame, childFrame, now, rospy.Duration(0.033))
(trans,rot) = listener.lookupTransform(frame, childFrame, now)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, tf2_ros.TransformException), e:
print str(e)
rate.sleep()
continue
poseOut.header.stamp.nsecs = now.nsecs
poseOut.header.stamp.secs = now.secs
poseOut.header.frame_id = outputFrame
poseOut.pose.pose.position.x = trans[0]
poseOut.pose.pose.position.y = trans[1]
poseOut.pose.pose.position.z = trans[2]
poseOut.pose.pose.orientation.x = rot[0]
poseOut.pose.pose.orientation.y = rot[1]
poseOut.pose.pose.orientation.z = rot[2]
poseOut.pose.pose.orientation.w = rot[3]
poseOut.pose.covariance = (cov * numpy.eye(6, dtype=numpy.float64)).tolist()
poseOut.pose.covariance = [item for sublist in poseOut.pose.covariance for item in sublist]
print str(poseOut)
pub.publish(poseOut)
rate.sleep()