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

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.
master
gaschler 2018-01-26 16:04:42 +01:00 committed by Wally B. Feed
parent 7fc931688c
commit 9a63a0479c
5 changed files with 99 additions and 88 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -79,7 +79,8 @@ void MapBuilderBridge::LoadMap(const std::string& map_filename) {
}
int MapBuilderBridge::AddTrajectory(
const std::unordered_set<std::string>& expected_sensor_ids,
const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
expected_sensor_ids,
const TrajectoryOptions& trajectory_options) {
const int trajectory_id = map_builder_->AddTrajectoryBuilder(
expected_sensor_ids, trajectory_options.trajectory_builder_options,

8
cartographer_ros/cartographer_ros/map_builder_bridge.h
View File

@ -18,13 +18,14 @@
#define CARTOGRAPHER_ROS_MAP_BUILDER_BRIDGE_H_
#include <memory>
#include <set>
#include <string>
#include <unordered_map>
#include <unordered_set>
#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,7 +64,10 @@ class MapBuilderBridge {
MapBuilderBridge& operator=(const MapBuilderBridge&) = delete;
void LoadMap(const std::string& map_filename);
int AddTrajectory(const std::unordered_set<std::string>& expected_sensor_ids,
int AddTrajectory(
const std::set<
::cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
expected_sensor_ids,
const TrajectoryOptions& trajectory_options);
void FinishTrajectory(int trajectory_id);
void RunFinalOptimization();

61
cartographer_ros/cartographer_ros/node.cc
View File

@ -255,23 +255,26 @@ void Node::PublishConstraintList(
}
}
std::unordered_set<std::string> Node::ComputeExpectedTopics(
std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
Node::ComputeExpectedSensorIds(
const TrajectoryOptions& options,
const cartographer_ros_msgs::SensorTopics& topics) {
std::unordered_set<std::string> expected_topics;
const cartographer_ros_msgs::SensorTopics& topics) const {
using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId;
using SensorType = SensorId::SensorType;
std::set<SensorId> 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<std::string> 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<std::string> Node::ComputeExpectedTopics(
int Node::AddTrajectory(const TrajectoryOptions& options,
const cartographer_ros_msgs::SensorTopics& topics) {
const std::unordered_set<std::string> expected_sensor_ids =
ComputeExpectedTopics(options, topics);
const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
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<std::string> Node::ComputeDefaultTopics(
const TrajectoryOptions& options) {
carto::common::MutexLocker lock(&mutex_);
return ComputeExpectedTopics(options, DefaultSensorTopics());
std::vector<
std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>>
Node::ComputeDefaultSensorIdsForMultipleBags(
const std::vector<TrajectoryOptions>& bags_options) const {
using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId;
std::vector<std::set<SensorId>> 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<SensorId> 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<std::string>& expected_sensor_ids,
const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
expected_sensor_ids,
const TrajectoryOptions& options) {
carto::common::MutexLocker lock(&mutex_);
const int trajectory_id =

23
cartographer_ros/cartographer_ros/node.h
View File

@ -19,6 +19,7 @@
#include <map>
#include <memory>
#include <set>
#include <unordered_map>
#include <unordered_set>
#include <vector>
@ -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<std::string> 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<TrajectoryOptions>& bags_options) const;
// Adds a trajectory for offline processing, i.e. not listening to topics.
int AddOfflineTrajectory(
const std::unordered_set<std::string>& 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<std::string> 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,

90
cartographer_ros/cartographer_ros/offline_node.cc
View File

@ -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<std::string> 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<std::string>(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<std::unordered_set<std::string>> bag_sensor_topics;
std::unordered_map<int, int> bag_index_to_trajectory_id;
auto bag_expected_sensor_ids =
node.ComputeDefaultSensorIdsForMultipleBags(bag_trajectory_options);
std::map<std::string,
cartographer::mapping::TrajectoryBuilderInterface::SensorId>
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<std::string> 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<std::string>(
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<std::string> 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<int, int> 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<sensor_msgs::LaserScan>()) {
node.HandleLaserScanMessage(trajectory_id, topic,
node.HandleLaserScanMessage(trajectory_id, sensor_id,
msg.instantiate<sensor_msgs::LaserScan>());
}
if (msg.isType<sensor_msgs::MultiEchoLaserScan>()) {
node.HandleMultiEchoLaserScanMessage(
trajectory_id, topic,
trajectory_id, sensor_id,
msg.instantiate<sensor_msgs::MultiEchoLaserScan>());
}
if (msg.isType<sensor_msgs::PointCloud2>()) {
node.HandlePointCloud2Message(
trajectory_id, topic, msg.instantiate<sensor_msgs::PointCloud2>());
trajectory_id, sensor_id,
msg.instantiate<sensor_msgs::PointCloud2>());
}
if (msg.isType<sensor_msgs::Imu>()) {
node.HandleImuMessage(trajectory_id, topic,
node.HandleImuMessage(trajectory_id, sensor_id,
msg.instantiate<sensor_msgs::Imu>());
}
if (msg.isType<nav_msgs::Odometry>()) {
node.HandleOdometryMessage(trajectory_id, topic,
node.HandleOdometryMessage(trajectory_id, sensor_id,
msg.instantiate<nav_msgs::Odometry>());
}
if (msg.isType<sensor_msgs::NavSatFix>()) {
node.HandleNavSatFixMessage(trajectory_id, topic,
node.HandleNavSatFixMessage(trajectory_id, sensor_id,
msg.instantiate<sensor_msgs::NavSatFix>());
}
}