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

Follow googlecartographer/cartographer#839 () 

Follow change  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

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,

10
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,8 +64,11 @@ 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,
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);

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>());
}
}