diff --git a/cartographer_ros/cartographer_ros/offline_node_main.cc b/cartographer_ros/cartographer_ros/offline_node_main.cc index 0a710a1..335f665 100644 --- a/cartographer_ros/cartographer_ros/offline_node_main.cc +++ b/cartographer_ros/cartographer_ros/offline_node_main.cc @@ -51,7 +51,6 @@ namespace { constexpr int kLatestOnlyPublisherQueueSize = 1; constexpr char kClockTopic[] = "clock"; -constexpr char kTfStaticTopic[] = "/tf_static"; std::vector<string> SplitString(const string& input, const char delimiter) { std::stringstream stream(input); @@ -92,15 +91,17 @@ void Run(std::vector<string> bag_filenames) { node.Initialize(); std::unordered_set<string> expected_sensor_ids; + const auto check_insert = [&expected_sensor_ids, &node](const string& topic) { + CHECK(expected_sensor_ids.insert(node.node_handle()->resolveName(topic)) + .second); + }; // For 2D SLAM, subscribe to exactly one horizontal laser. if (options.use_laser_scan) { - expected_sensor_ids.insert( - node.node_handle()->resolveName(kLaserScanTopic, false /* remap */)); + check_insert(kLaserScanTopic); } if (options.use_multi_echo_laser_scan) { - expected_sensor_ids.insert(node.node_handle()->resolveName( - kMultiEchoLaserScanTopic, false /* remap */)); + check_insert(kMultiEchoLaserScanTopic); } // For 3D SLAM, subscribe to all point clouds topics. @@ -110,8 +111,7 @@ void Run(std::vector<string> bag_filenames) { if (options.num_point_clouds > 1) { topic += "_" + std::to_string(i + 1); } - expected_sensor_ids.insert( - node.node_handle()->resolveName(topic, false /* remap */)); + check_insert(topic); } } @@ -121,14 +121,12 @@ void Run(std::vector<string> bag_filenames) { (options.map_builder_options.use_trajectory_builder_2d() && options.map_builder_options.trajectory_builder_2d_options() .use_imu_data())) { - expected_sensor_ids.insert( - node.node_handle()->resolveName(kImuTopic, false /* remap */)); + check_insert(kImuTopic); } // For both 2D and 3D SLAM, odometry is optional. if (options.use_odometry) { - expected_sensor_ids.insert( - node.node_handle()->resolveName(kOdometryTopic, false /* remap */)); + check_insert(kOdometryTopic); } ::ros::Publisher clock_publisher = @@ -155,7 +153,8 @@ void Run(std::vector<string> bag_filenames) { } // TODO(damonkohler): Republish non-conflicting tf messages. - const string topic = node.node_handle()->resolveName(msg.getTopic()); + const string topic = + node.node_handle()->resolveName(msg.getTopic(), false /* resolve */); if (expected_sensor_ids.count(topic) == 0) { continue; } diff --git a/cartographer_ros/launch/offline_backpack_2d.launch b/cartographer_ros/launch/offline_backpack_2d.launch index 5388b61..3a738b2 100644 --- a/cartographer_ros/launch/offline_backpack_2d.launch +++ b/cartographer_ros/launch/offline_backpack_2d.launch @@ -23,10 +23,10 @@ <node name="cartographer_offline_node" pkg="cartographer_ros" type="cartographer_offline_node" args=" -configuration_directory $(find cartographer_ros)/configuration_files - -configuration_basename backpack_2d.lua" + -configuration_basename backpack_2d.lua -urdf_filename $(find cartographer_ros)/urdf/backpack_2d.urdf -bag_filenames $(arg bag_filenames)" output="screen"> - <remap from="horizontal_laser_2d" to="echoes" /> + <remap from="echoes" to="horizontal_laser_2d" /> </node> </launch> diff --git a/cartographer_ros/launch/offline_backpack_3d.launch b/cartographer_ros/launch/offline_backpack_3d.launch index bbd10cf..2979ec7 100644 --- a/cartographer_ros/launch/offline_backpack_3d.launch +++ b/cartographer_ros/launch/offline_backpack_3d.launch @@ -23,11 +23,11 @@ <node name="cartographer_offline_node" pkg="cartographer_ros" type="cartographer_offline_node" args=" -configuration_directory $(find cartographer_ros)/configuration_files - -configuration_basename backpack_3d.lua" + -configuration_basename backpack_3d.lua -urdf_filename $(find cartographer_ros)/urdf/backpack_3d.urdf -bag_filenames $(arg bag_filenames)" output="screen"> - <remap from="horizontal_laser_3d" to="points2_1" /> - <remap from="vertical_laser_3d" to="points2_2" /> + <remap from="points2_1" to="horizontal_laser_3d" /> + <remap from="points2_2" to="vertical_laser_3d" /> </node> </launch>