From 9a63a0479c19d9c267ec9bb62acc58f66d7975dc Mon Sep 17 00:00:00 2001 From: gaschler Date: Fri, 26 Jan 2018 16:04:42 +0100 Subject: [PATCH] Follow googlecartographer/cartographer#839 (#686) Follow change googlecartographer/cartographer#839 from string to struct SensorId. Compute expected sensor ids for multiple trajectories. Remove command argument input for sensor ids. Make some methods const. Clean up. --- .../cartographer_ros/map_builder_bridge.cc | 3 +- .../cartographer_ros/map_builder_bridge.h | 10 ++- cartographer_ros/cartographer_ros/node.cc | 61 +++++++++---- cartographer_ros/cartographer_ros/node.h | 23 +++-- .../cartographer_ros/offline_node.cc | 90 +++++++------------ 5 files changed, 99 insertions(+), 88 deletions(-) diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index 3611c8f..a021ad9 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -79,7 +79,8 @@ void MapBuilderBridge::LoadMap(const std::string& map_filename) { } int MapBuilderBridge::AddTrajectory( - const std::unordered_set& expected_sensor_ids, + const std::set& + expected_sensor_ids, const TrajectoryOptions& trajectory_options) { const int trajectory_id = map_builder_->AddTrajectoryBuilder( expected_sensor_ids, trajectory_options.trajectory_builder_options, diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.h b/cartographer_ros/cartographer_ros/map_builder_bridge.h index 73314a8..605559a 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.h +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.h @@ -18,13 +18,14 @@ #define CARTOGRAPHER_ROS_MAP_BUILDER_BRIDGE_H_ #include +#include #include #include -#include #include "cartographer/common/mutex.h" #include "cartographer/mapping/map_builder_interface.h" #include "cartographer/mapping/proto/trajectory_builder_options.pb.h" +#include "cartographer/mapping/trajectory_builder_interface.h" #include "cartographer_ros/node_options.h" #include "cartographer_ros/sensor_bridge.h" #include "cartographer_ros/tf_bridge.h" @@ -63,8 +64,11 @@ class MapBuilderBridge { MapBuilderBridge& operator=(const MapBuilderBridge&) = delete; void LoadMap(const std::string& map_filename); - int AddTrajectory(const std::unordered_set& expected_sensor_ids, - const TrajectoryOptions& trajectory_options); + int AddTrajectory( + const std::set< + ::cartographer::mapping::TrajectoryBuilderInterface::SensorId>& + expected_sensor_ids, + const TrajectoryOptions& trajectory_options); void FinishTrajectory(int trajectory_id); void RunFinalOptimization(); void SerializeState(const std::string& filename); diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index e1e580d..9363abc 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -255,23 +255,26 @@ void Node::PublishConstraintList( } } -std::unordered_set Node::ComputeExpectedTopics( +std::set +Node::ComputeExpectedSensorIds( const TrajectoryOptions& options, - const cartographer_ros_msgs::SensorTopics& topics) { - std::unordered_set expected_topics; + const cartographer_ros_msgs::SensorTopics& topics) const { + using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId; + using SensorType = SensorId::SensorType; + std::set expected_topics; // Subscribe to all laser scan, multi echo laser scan, and point cloud topics. for (const std::string& topic : ComputeRepeatedTopicNames( topics.laser_scan_topic, options.num_laser_scans)) { - expected_topics.insert(topic); + expected_topics.insert(SensorId{SensorType::RANGE, topic}); } for (const std::string& topic : ComputeRepeatedTopicNames(topics.multi_echo_laser_scan_topic, options.num_multi_echo_laser_scans)) { - expected_topics.insert(topic); + expected_topics.insert(SensorId{SensorType::RANGE, topic}); } for (const std::string& topic : ComputeRepeatedTopicNames( topics.point_cloud2_topic, options.num_point_clouds)) { - expected_topics.insert(topic); + expected_topics.insert(SensorId{SensorType::RANGE, topic}); } // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is // required. @@ -279,15 +282,17 @@ std::unordered_set Node::ComputeExpectedTopics( (node_options_.map_builder_options.use_trajectory_builder_2d() && options.trajectory_builder_options.trajectory_builder_2d_options() .use_imu_data())) { - expected_topics.insert(topics.imu_topic); + expected_topics.insert(SensorId{SensorType::IMU, topics.imu_topic}); } // Odometry is optional. if (options.use_odometry) { - expected_topics.insert(topics.odometry_topic); + expected_topics.insert( + SensorId{SensorType::ODOMETRY, topics.odometry_topic}); } // NavSatFix is optional. if (options.use_nav_sat) { - expected_topics.insert(topics.nav_sat_fix_topic); + expected_topics.insert( + SensorId{SensorType::FIXED_FRAME_POSE, topics.nav_sat_fix_topic}); } return expected_topics; @@ -295,16 +300,17 @@ std::unordered_set Node::ComputeExpectedTopics( int Node::AddTrajectory(const TrajectoryOptions& options, const cartographer_ros_msgs::SensorTopics& topics) { - const std::unordered_set expected_sensor_ids = - ComputeExpectedTopics(options, topics); + const std::set + expected_sensor_ids = ComputeExpectedSensorIds(options, topics); const int trajectory_id = map_builder_bridge_.AddTrajectory(expected_sensor_ids, options); AddExtrapolator(trajectory_id, options); AddSensorSamplers(trajectory_id, options); LaunchSubscribers(options, topics, trajectory_id); is_active_trajectory_[trajectory_id] = true; - subscribed_topics_.insert(expected_sensor_ids.begin(), - expected_sensor_ids.end()); + for (const auto& sensor_id : expected_sensor_ids) { + subscribed_topics_.insert(sensor_id.id); + } return trajectory_id; } @@ -384,7 +390,8 @@ bool Node::ValidateTrajectoryOptions(const TrajectoryOptions& options) { bool Node::ValidateTopicNames( const ::cartographer_ros_msgs::SensorTopics& topics, const TrajectoryOptions& options) { - for (const std::string& topic : ComputeExpectedTopics(options, topics)) { + for (const auto& sensor_id : ComputeExpectedSensorIds(options, topics)) { + const std::string& topic = sensor_id.id; if (subscribed_topics_.count(topic) > 0) { LOG(ERROR) << "Topic name [" << topic << "] is already used."; return false; @@ -441,14 +448,30 @@ void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) { AddTrajectory(options, DefaultSensorTopics()); } -std::unordered_set Node::ComputeDefaultTopics( - const TrajectoryOptions& options) { - carto::common::MutexLocker lock(&mutex_); - return ComputeExpectedTopics(options, DefaultSensorTopics()); +std::vector< + std::set> +Node::ComputeDefaultSensorIdsForMultipleBags( + const std::vector& bags_options) const { + using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId; + std::vector> bags_sensor_ids; + for (size_t i = 0; i < bags_options.size(); ++i) { + std::string prefix; + if (bags_options.size() > 1) { + prefix = "bag_" + std::to_string(i + 1) + "_"; + } + std::set unique_sensor_ids; + for (const auto& sensor_id : + ComputeExpectedSensorIds(bags_options.at(i), DefaultSensorTopics())) { + unique_sensor_ids.insert(SensorId{sensor_id.type, prefix + sensor_id.id}); + } + bags_sensor_ids.push_back(unique_sensor_ids); + } + return bags_sensor_ids; } int Node::AddOfflineTrajectory( - const std::unordered_set& expected_sensor_ids, + const std::set& + expected_sensor_ids, const TrajectoryOptions& options) { carto::common::MutexLocker lock(&mutex_); const int trajectory_id = diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index 4db9a28..1ec6623 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -73,13 +74,19 @@ class Node { // Starts the first trajectory with the default topics. void StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options); - // Compute the default topics for the given 'options'. - std::unordered_set ComputeDefaultTopics( - const TrajectoryOptions& options); + // Returns unique SensorIds for multiple input bag files based on + // their TrajectoryOptions. + // 'SensorId::id' is the expected ROS topic name. + std::vector< + std::set<::cartographer::mapping::TrajectoryBuilderInterface::SensorId>> + ComputeDefaultSensorIdsForMultipleBags( + const std::vector& bags_options) const; // Adds a trajectory for offline processing, i.e. not listening to topics. int AddOfflineTrajectory( - const std::unordered_set& expected_sensor_ids, + const std::set< + cartographer::mapping::TrajectoryBuilderInterface::SensorId>& + expected_sensor_ids, const TrajectoryOptions& options); // The following functions handle adding sensor data to a trajectory. @@ -127,10 +134,12 @@ class Node { cartographer_ros_msgs::FinishTrajectory::Response& response); bool HandleWriteState(cartographer_ros_msgs::WriteState::Request& request, cartographer_ros_msgs::WriteState::Response& response); - // Returns the set of topic names we want to subscribe to. - std::unordered_set ComputeExpectedTopics( + // Returns the set of SensorIds expected for a trajectory. + // 'SensorId::id' is the expected ROS topic name. + std::set<::cartographer::mapping::TrajectoryBuilderInterface::SensorId> + ComputeExpectedSensorIds( const TrajectoryOptions& options, - const cartographer_ros_msgs::SensorTopics& topics); + const cartographer_ros_msgs::SensorTopics& topics) const; int AddTrajectory(const TrajectoryOptions& options, const cartographer_ros_msgs::SensorTopics& topics); void LaunchSubscribers(const TrajectoryOptions& options, diff --git a/cartographer_ros/cartographer_ros/offline_node.cc b/cartographer_ros/cartographer_ros/offline_node.cc index 1456564..31439bb 100644 --- a/cartographer_ros/cartographer_ros/offline_node.cc +++ b/cartographer_ros/cartographer_ros/offline_node.cc @@ -56,15 +56,6 @@ DEFINE_string(pbstream_filename, "", DEFINE_bool(keep_running, false, "Keep running the offline node after all messages from the bag " "have been processed."); -DEFINE_string(sensor_topics, "", - "Optional comma-separated list of colon-separated lists of " - "sensor topics for each trajectory. A single trajectory's topics " - "are delimited with colons, while different trajectories are " - "delimited with commas. If a blank list is given for a " - "trajectory, the default topics are used. If a single " - "colon-separated list is given, it will be used for all " - "trajectories. If omitted, default topics will be used " - "for all trajectories."); namespace cartographer_ros { @@ -160,28 +151,11 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) { }, false /* oneshot */, false /* autostart */); - // Colon-delimited string lists of sensor topics for each bag. - std::vector bag_sensor_topics_strings; - - if (FLAGS_sensor_topics.empty()) { - // Use default topic names for all bags, denoted by an empty sensor - // topic list string for each bag. - bag_sensor_topics_strings = - std::vector(bag_filenames.size(), std::string()); - } else { - bag_sensor_topics_strings = - cartographer_ros::SplitString(FLAGS_sensor_topics, ','); - if (bag_sensor_topics_strings.size() == 1) { - // Use the single specified topic list string for all bags. - bag_sensor_topics_strings.insert(bag_sensor_topics_strings.end(), - bag_filenames.size() - 1, - bag_sensor_topics_strings.at(0)); - } - } - CHECK_EQ(bag_sensor_topics_strings.size(), bag_filenames.size()); - - std::vector> bag_sensor_topics; - std::unordered_map bag_index_to_trajectory_id; + auto bag_expected_sensor_ids = + node.ComputeDefaultSensorIdsForMultipleBags(bag_trajectory_options); + std::map + bag_topic_to_sensor_id; PlayableBagMultiplexer playable_bag_multiplexer; for (size_t current_bag_index = 0; current_bag_index < bag_filenames.size(); ++current_bag_index) { @@ -189,25 +163,21 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) { if (!::ros::ok()) { return; } - std::vector unresolved_current_bag_sensor_topics; - if (bag_sensor_topics_strings.at(current_bag_index).empty()) { - // Empty topic list string provided for this trajectory, - // use default topics. - const auto default_topics = node.ComputeDefaultTopics( - bag_trajectory_options.at(current_bag_index)); - unresolved_current_bag_sensor_topics = std::vector( - default_topics.begin(), default_topics.end()); - } else { - unresolved_current_bag_sensor_topics = cartographer_ros::SplitString( - bag_sensor_topics_strings.at(current_bag_index), ':'); - } std::unordered_set current_bag_sensor_topics; - for (const auto& topic : unresolved_current_bag_sensor_topics) { - CHECK(current_bag_sensor_topics - .insert(node.node_handle()->resolveName(topic)) - .second); + for (const auto& expected_sensor_id : + bag_expected_sensor_ids.at(current_bag_index)) { + std::string resolved_topic = + node.node_handle()->resolveName(expected_sensor_id.id); + if (bag_topic_to_sensor_id.count(resolved_topic) != 0) { + LOG(ERROR) << "Sensor " << expected_sensor_id.id + << " resolves to topic " << resolved_topic + << " which is already used by " + << " sensor " + << bag_topic_to_sensor_id.at(resolved_topic).id; + } + bag_topic_to_sensor_id[resolved_topic] = expected_sensor_id; + current_bag_sensor_topics.insert(resolved_topic); } - bag_sensor_topics.push_back(current_bag_sensor_topics); playable_bag_multiplexer.AddPlayableBag(PlayableBag( bag_filename, current_bag_index, ros::TIME_MIN, ros::TIME_MAX, kDelay, @@ -243,8 +213,9 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) { } })); } - CHECK_EQ(bag_sensor_topics.size(), bag_filenames.size()); + // TODO(gaschler): Warn if resolved topics are not in bags. + std::unordered_map bag_index_to_trajectory_id; while (playable_bag_multiplexer.IsMessageAvailable()) { const auto next_msg_tuple = playable_bag_multiplexer.GetNextMessage(); const rosbag::MessageInstance& msg = std::get<0>(next_msg_tuple); @@ -256,7 +227,7 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) { // to avoid blocking the sensor queue. if (bag_index_to_trajectory_id.count(bag_index) == 0) { trajectory_id = - node.AddOfflineTrajectory(bag_sensor_topics.at(bag_index), + node.AddOfflineTrajectory(bag_expected_sensor_ids.at(bag_index), bag_trajectory_options.at(bag_index)); CHECK(bag_index_to_trajectory_id .emplace(std::piecewise_construct, @@ -272,32 +243,35 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) { if (!::ros::ok()) { return; } - const std::string topic = + const std::string bag_topic = node.node_handle()->resolveName(msg.getTopic(), false /* resolve */); - if (bag_sensor_topics.at(bag_index).count(topic) > 0) { + auto it = bag_topic_to_sensor_id.find(bag_topic); + if (it != bag_topic_to_sensor_id.end()) { + const std::string& sensor_id = it->second.id; if (msg.isType()) { - node.HandleLaserScanMessage(trajectory_id, topic, + node.HandleLaserScanMessage(trajectory_id, sensor_id, msg.instantiate()); } if (msg.isType()) { node.HandleMultiEchoLaserScanMessage( - trajectory_id, topic, + trajectory_id, sensor_id, msg.instantiate()); } if (msg.isType()) { node.HandlePointCloud2Message( - trajectory_id, topic, msg.instantiate()); + trajectory_id, sensor_id, + msg.instantiate()); } if (msg.isType()) { - node.HandleImuMessage(trajectory_id, topic, + node.HandleImuMessage(trajectory_id, sensor_id, msg.instantiate()); } if (msg.isType()) { - node.HandleOdometryMessage(trajectory_id, topic, + node.HandleOdometryMessage(trajectory_id, sensor_id, msg.instantiate()); } if (msg.isType()) { - node.HandleNavSatFixMessage(trajectory_id, topic, + node.HandleNavSatFixMessage(trajectory_id, sensor_id, msg.instantiate()); } }