From e1aaf65ac399dece7cbf3c5bacefaafe4854348c Mon Sep 17 00:00:00 2001 From: Damon Kohler Date: Fri, 30 Sep 2016 15:09:31 +0200 Subject: [PATCH] Adds PR2 demo and script for dropping TF frames. (#77) --- cartographer_ros/CMakeLists.txt | 4 ++ cartographer_ros/configuration_files/pr2.lua | 46 ++++++++++++++++++++ cartographer_ros/launch/demo_pr2.launch | 44 +++++++++++++++++++ cartographer_ros/scripts/tf_remove_frames.py | 38 ++++++++++++++++ docs/source/index.rst | 8 +++- 5 files changed, 139 insertions(+), 1 deletion(-) create mode 100644 cartographer_ros/configuration_files/pr2.lua create mode 100644 cartographer_ros/launch/demo_pr2.launch create mode 100755 cartographer_ros/scripts/tf_remove_frames.py diff --git a/cartographer_ros/CMakeLists.txt b/cartographer_ros/CMakeLists.txt index 2eeb30b..7dd43c4 100644 --- a/cartographer_ros/CMakeLists.txt +++ b/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} +) diff --git a/cartographer_ros/configuration_files/pr2.lua b/cartographer_ros/configuration_files/pr2.lua new file mode 100644 index 0000000..341de00 --- /dev/null +++ b/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 diff --git a/cartographer_ros/launch/demo_pr2.launch b/cartographer_ros/launch/demo_pr2.launch new file mode 100644 index 0000000..c7e197a --- /dev/null +++ b/cartographer_ros/launch/demo_pr2.launch @@ -0,0 +1,44 @@ + + + + + + + + + + + + + - map + - odom_combined + + + + + + + + diff --git a/cartographer_ros/scripts/tf_remove_frames.py b/cartographer_ros/scripts/tf_remove_frames.py new file mode 100755 index 0000000..9788a3a --- /dev/null +++ b/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() diff --git a/docs/source/index.rst b/docs/source/index.rst index 46e8190..616e3d8 100644 --- a/docs/source/index.rst +++ b/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.