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
parent
a09a2a3ee3
commit
794ca96b92
|
@ -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;
|
||||||
}
|
}
|
||||||
|
|
|
@ -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>
|
||||||
|
|
|
@ -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>
|
||||||
|
|
Loading…
Reference in New Issue