Adds PR2 demo and script for dropping TF frames. (#77)

cartographer_ros/CMakeLists.txt

@@ -129,3 +129,7 @@ install(TARGETS cartographer_node
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
+
+install(PROGRAMS scripts/tf_remove_frames.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)

cartographer_ros/configuration_files/pr2.lua

@@ -0,0 +1,46 @@
+-- Copyright 2016 The Cartographer Authors
+--
+-- Licensed under the Apache License, Version 2.0 (the "License");
+-- you may not use this file except in compliance with the License.
+-- You may obtain a copy of the License at
+--
+--      http://www.apache.org/licenses/LICENSE-2.0
+--
+-- Unless required by applicable law or agreed to in writing, software
+-- distributed under the License is distributed on an "AS IS" BASIS,
+-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+-- See the License for the specific language governing permissions and
+-- limitations under the License.
+
+include "map_builder.lua"
+
+options = {
+  map_builder = MAP_BUILDER,
+  map_frame = "map",
+  tracking_frame = "base_footprint",
+  published_frame = "base_footprint",
+  odom_frame = "odom",
+  provide_odom_frame = true,
+  use_odometry_data = false,
+  use_constant_odometry_variance = false,
+  constant_odometry_translational_variance = 0.,
+  constant_odometry_rotational_variance = 0.,
+  use_horizontal_laser = true,
+  use_horizontal_multi_echo_laser = false,
+  horizontal_laser_min_range = 0.,
+  horizontal_laser_max_range = 30.,
+  horizontal_laser_missing_echo_ray_length = 5.,
+  num_lasers_3d = 0,
+  lookup_transform_timeout_sec = 0.2,
+  submap_publish_period_sec = 0.3,
+  pose_publish_period_sec = 5e-3,
+}
+
+MAP_BUILDER.use_trajectory_builder_2d = true
+TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
+TRAJECTORY_BUILDER_2D.use_imu_data = false
+TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.15
+TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.angular_search_window = math.rad(35.)
+SPARSE_POSE_GRAPH.optimization_problem.huber_scale = 1e2
+
+return options

cartographer_ros/launch/demo_pr2.launch

@@ -0,0 +1,44 @@
+<!--
+  Copyright 2016 The Cartographer Authors
+
+  Licensed under the Apache License, Version 2.0 (the "License");
+  you may not use this file except in compliance with the License.
+  You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+  Unless required by applicable law or agreed to in writing, software
+  distributed under the License is distributed on an "AS IS" BASIS,
+  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  See the License for the specific language governing permissions and
+  limitations under the License.
+-->
+
+<launch>
+  <param name="/use_sim_time" value="true" />
+
+  <node name="cartographer_node" pkg="cartographer_ros"
+      type="cartographer_node" args="
+          -configuration_directory
+              $(find cartographer_ros)/configuration_files
+          -configuration_basename pr2.lua"
+      output="screen">
+    <remap from="scan" to="/base_scan" />
+  </node>
+
+  <node name="tf_remove_frames" pkg="cartographer_ros"
+      type="tf_remove_frames.py">
+    <remap from="tf_out" to="/tf" />
+    <rosparam param="remove_frames">
+      - map
+      - odom_combined
+    </rosparam>
+  </node>
+
+  <node name="rviz" pkg="rviz" type="rviz" required="true"
+      args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
+  <node name="playbag" pkg="rosbag" type="play"
+      args="--clock $(arg bag_filename)">
+    <remap from="tf" to="tf_in" />
+  </node>
+</launch>

cartographer_ros/scripts/tf_remove_frames.py

@@ -0,0 +1,38 @@
+#!/usr/bin/python
+# -*- coding: utf-8 -*-
+
+# Copyright 2016 The Cartographer Authors
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#    http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import rospy
+from tf.msg import tfMessage
+
+
+def main():
+  rospy.init_node('tf_remove_frames')
+  publisher = rospy.Publisher('/tf_out', tfMessage, queue_size=1)
+  remove_frames = rospy.get_param('~remove_frames', [])
+
+  def callback(msg):
+    msg.transforms = [t for t in msg.transforms if
+                      t.header.frame_id.lstrip('/') not in remove_frames and
+                      t.child_frame_id.lstrip('/') not in remove_frames]
+    publisher.publish(msg)
+
+  rospy.Subscriber('/tf_in', tfMessage, callback)
+  rospy.spin()
+
+
+if __name__ == '__main__':
+  main()

docs/source/index.rst

@@ -87,9 +87,15 @@ in this case ``~/Downloads``, and use ``roslaunch`` to bring up the demo:
     roslaunch cartographer_ros demo_backpack_3d.launch bag_filename:=${HOME}/Downloads/cartographer_3d_deutsches_museum.bag
 
     # Download the Revo LDS example bag.
-    wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_3d/cartographer_paper_revo_lds.bag
+    wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/revo_lds/cartographer_paper_revo_lds.bag
 
     # Launch the Revo LDS demo.
     roslaunch cartographer_ros demo_revo_lds.launch bag_filename:=${HOME}/Downloads/cartographer_paper_revo_lds.bag
 
+    # Download the PR2 example bag.
+    wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/pr2/2011-09-15-08-32-46.bag
+
+    # Launch the PR2 demo.
+    roslaunch cartographer_ros demo_pr2.launch bag_filename:=${HOME}/Downloads/2011-09-15-08-32-46.bag
+
 The launch files will bring up ``roscore`` and ``rviz`` automatically.