Fix offline node. (#219)

Flips topic remapping to match online node and checks that topics are
wired to unique sensor inputs.
Fixes broken launch files.
master
Damon Kohler 2016-12-08 11:15:02 +01:00 committed by GitHub
parent a09a2a3ee3
commit 794ca96b92
3 changed files with 16 additions and 17 deletions

View File

@ -51,7 +51,6 @@ namespace {
constexpr int kLatestOnlyPublisherQueueSize = 1; constexpr int kLatestOnlyPublisherQueueSize = 1;
constexpr char kClockTopic[] = "clock"; constexpr char kClockTopic[] = "clock";
constexpr char kTfStaticTopic[] = "/tf_static";
std::vector<string> SplitString(const string& input, const char delimiter) { std::vector<string> SplitString(const string& input, const char delimiter) {
std::stringstream stream(input); std::stringstream stream(input);
@ -92,15 +91,17 @@ void Run(std::vector<string> bag_filenames) {
node.Initialize(); node.Initialize();
std::unordered_set<string> expected_sensor_ids; 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. // For 2D SLAM, subscribe to exactly one horizontal laser.
if (options.use_laser_scan) { if (options.use_laser_scan) {
expected_sensor_ids.insert( check_insert(kLaserScanTopic);
node.node_handle()->resolveName(kLaserScanTopic, false /* remap */));
} }
if (options.use_multi_echo_laser_scan) { if (options.use_multi_echo_laser_scan) {
expected_sensor_ids.insert(node.node_handle()->resolveName( check_insert(kMultiEchoLaserScanTopic);
kMultiEchoLaserScanTopic, false /* remap */));
} }
// For 3D SLAM, subscribe to all point clouds topics. // 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) { if (options.num_point_clouds > 1) {
topic += "_" + std::to_string(i + 1); topic += "_" + std::to_string(i + 1);
} }
expected_sensor_ids.insert( check_insert(topic);
node.node_handle()->resolveName(topic, false /* remap */));
} }
} }
@ -121,14 +121,12 @@ void Run(std::vector<string> bag_filenames) {
(options.map_builder_options.use_trajectory_builder_2d() && (options.map_builder_options.use_trajectory_builder_2d() &&
options.map_builder_options.trajectory_builder_2d_options() options.map_builder_options.trajectory_builder_2d_options()
.use_imu_data())) { .use_imu_data())) {
expected_sensor_ids.insert( check_insert(kImuTopic);
node.node_handle()->resolveName(kImuTopic, false /* remap */));
} }
// For both 2D and 3D SLAM, odometry is optional. // For both 2D and 3D SLAM, odometry is optional.
if (options.use_odometry) { if (options.use_odometry) {
expected_sensor_ids.insert( check_insert(kOdometryTopic);
node.node_handle()->resolveName(kOdometryTopic, false /* remap */));
} }
::ros::Publisher clock_publisher = ::ros::Publisher clock_publisher =
@ -155,7 +153,8 @@ void Run(std::vector<string> bag_filenames) {
} }
// TODO(damonkohler): Republish non-conflicting tf messages. // 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) { if (expected_sensor_ids.count(topic) == 0) {
continue; continue;
} }

View File

@ -23,10 +23,10 @@
<node name="cartographer_offline_node" pkg="cartographer_ros" <node name="cartographer_offline_node" pkg="cartographer_ros"
type="cartographer_offline_node" args=" type="cartographer_offline_node" args="
-configuration_directory $(find cartographer_ros)/configuration_files -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 -urdf_filename $(find cartographer_ros)/urdf/backpack_2d.urdf
-bag_filenames $(arg bag_filenames)" -bag_filenames $(arg bag_filenames)"
output="screen"> output="screen">
<remap from="horizontal_laser_2d" to="echoes" /> <remap from="echoes" to="horizontal_laser_2d" />
</node> </node>
</launch> </launch>

View File

@ -23,11 +23,11 @@
<node name="cartographer_offline_node" pkg="cartographer_ros" <node name="cartographer_offline_node" pkg="cartographer_ros"
type="cartographer_offline_node" args=" type="cartographer_offline_node" args="
-configuration_directory $(find cartographer_ros)/configuration_files -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 -urdf_filename $(find cartographer_ros)/urdf/backpack_3d.urdf
-bag_filenames $(arg bag_filenames)" -bag_filenames $(arg bag_filenames)"
output="screen"> output="screen">
<remap from="horizontal_laser_3d" to="points2_1" /> <remap from="points2_1" to="horizontal_laser_3d" />
<remap from="vertical_laser_3d" to="points2_2" /> <remap from="points2_2" to="vertical_laser_3d" />
</node> </node>
</launch> </launch>