12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Offline node: better support for sequential bags. (#694) 

Allow same topics to be used in different bags (a previously supported use case).
Remove unused variable `current_bag_sensor_topics`.
Touch up flag descriptions.

Fixes #693.

pair=@gaschler
master
Juraj Oršulić 2018-01-29 15:43:25 +01:00 committed by Wally B. Feed
parent 9a63a0479c
commit 2538ac3e45
1 changed files with 16 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
cartographer_ros/cartographer_ros/offline_node.cc
View File

@ -40,12 +40,13 @@ DEFINE_string(
configuration_basenames, "", configuration_basenames, "",
"Comma-separated list of basenames, i.e. not containing any " "Comma-separated list of basenames, i.e. not containing any "
"directory prefix, of the configuration files for each trajectory. " "directory prefix, of the configuration files for each trajectory. "
"The first configuration file will be used for node options." "The first configuration file will be used for node options. "
"If less configuration files are specified than trajectories, the " "If less configuration files are specified than trajectories, the "
"first file will be used the remaining trajectories."); "first file will be used the remaining trajectories.");
DEFINE_string( DEFINE_string(
bag_filenames, "", bag_filenames, "",
"Comma-separated list of bags to process. One bag per trajectory."); "Comma-separated list of bags to process. One bag per trajectory. "
"Any combination of simultaneous and sequential bags is supported.");
DEFINE_string(urdf_filenames, "", DEFINE_string(urdf_filenames, "",
"Comma-separated list of one or more URDF files that contain " "Comma-separated list of one or more URDF files that contain "
"static links for the sensor configuration(s)."); "static links for the sensor configuration(s).");
@ -153,7 +154,7 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
auto bag_expected_sensor_ids = auto bag_expected_sensor_ids =
node.ComputeDefaultSensorIdsForMultipleBags(bag_trajectory_options); node.ComputeDefaultSensorIdsForMultipleBags(bag_trajectory_options);
std::map<std::string, std::map<std::pair<int /* bag_index */, std::string>,
cartographer::mapping::TrajectoryBuilderInterface::SensorId> cartographer::mapping::TrajectoryBuilderInterface::SensorId>
bag_topic_to_sensor_id; bag_topic_to_sensor_id;
PlayableBagMultiplexer playable_bag_multiplexer; PlayableBagMultiplexer playable_bag_multiplexer;
@ -163,20 +164,19 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
if (!::ros::ok()) { if (!::ros::ok()) {
return; return;
} }
std::unordered_set<std::string> current_bag_sensor_topics;
for (const auto& expected_sensor_id : for (const auto& expected_sensor_id :
bag_expected_sensor_ids.at(current_bag_index)) { bag_expected_sensor_ids.at(current_bag_index)) {
std::string resolved_topic = const auto bag_resolved_topic = std::make_pair(
node.node_handle()->resolveName(expected_sensor_id.id); static_cast<int>(current_bag_index),
if (bag_topic_to_sensor_id.count(resolved_topic) != 0) { node.node_handle()->resolveName(expected_sensor_id.id));
LOG(ERROR) << "Sensor " << expected_sensor_id.id if (bag_topic_to_sensor_id.count(bag_resolved_topic) != 0) {
<< " resolves to topic " << resolved_topic LOG(ERROR) << "Sensor " << expected_sensor_id.id << " of bag "
<< " which is already used by " << current_bag_index << " resolves to topic "
<< bag_resolved_topic.second << " which is already used by "
<< " sensor " << " sensor "
<< bag_topic_to_sensor_id.at(resolved_topic).id; << bag_topic_to_sensor_id.at(bag_resolved_topic).id;
} }
bag_topic_to_sensor_id[resolved_topic] = expected_sensor_id; bag_topic_to_sensor_id[bag_resolved_topic] = expected_sensor_id;
current_bag_sensor_topics.insert(resolved_topic);
} }
playable_bag_multiplexer.AddPlayableBag(PlayableBag( playable_bag_multiplexer.AddPlayableBag(PlayableBag(
@ -243,8 +243,9 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
if (!::ros::ok()) { if (!::ros::ok()) {
return; return;
} }
const std::string bag_topic = const auto bag_topic = std::make_pair(
node.node_handle()->resolveName(msg.getTopic(), false /* resolve */); bag_index,
node.node_handle()->resolveName(msg.getTopic(), false /* resolve */));
auto it = bag_topic_to_sensor_id.find(bag_topic); auto it = bag_topic_to_sensor_id.find(bag_topic);
if (it != bag_topic_to_sensor_id.end()) { if (it != bag_topic_to_sensor_id.end()) {
const std::string& sensor_id = it->second.id; const std::string& sensor_id = it->second.id;