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

Simplify sensor subscribers. (#434)

master
Wolfgang Hess 2017-07-21 10:38:22 +02:00 committed by GitHub
parent 603439ac05
commit 1358f719a5
5 changed files with 126 additions and 140 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

190
cartographer_ros/cartographer_ros/node.cc
View File

@ -48,33 +48,6 @@ namespace carto = ::cartographer;
using carto::transform::Rigid3d;
namespace {
void ShutdownSubscriber(std::unordered_map<int, ::ros::Subscriber>& subscribers,
int trajectory_id) {
if (subscribers.count(trajectory_id) == 0) {
return;
}
subscribers[trajectory_id].shutdown();
LOG(INFO) << "Shutdown the subscriber of ["
<< subscribers[trajectory_id].getTopic() << "]";
CHECK_EQ(subscribers.erase(trajectory_id), 1);
}
bool IsTopicNameUnique(
const string& topic,
const std::unordered_map<int, ::ros::Subscriber>& subscribers) {
for (auto& entry : subscribers) {
if (entry.second.getTopic() == topic) {
LOG(ERROR) << "Topic name [" << topic << "] is already used.";
return false;
}
}
return true;
}
} // namespace
Node::Node(const NodeOptions& node_options, tf2_ros::Buffer* const tf_buffer)
: node_options_(node_options),
map_builder_bridge_(node_options_, tf_buffer) {
@ -247,24 +220,20 @@ void Node::SpinOccupancyGridThreadForever() {
}
}
int Node::AddTrajectory(const TrajectoryOptions& options,
const cartographer_ros_msgs::SensorTopics& topics) {
std::unordered_set<string> expected_sensor_ids;
std::unordered_set<string> Node::ComputeExpectedTopics(
const TrajectoryOptions& options,
const cartographer_ros_msgs::SensorTopics& topics) {
std::unordered_set<string> expected_topics;
if (options.use_laser_scan) {
expected_sensor_ids.insert(topics.laser_scan_topic);
expected_topics.insert(topics.laser_scan_topic);
}
if (options.use_multi_echo_laser_scan) {
expected_sensor_ids.insert(topics.multi_echo_laser_scan_topic);
expected_topics.insert(topics.multi_echo_laser_scan_topic);
}
if (options.num_point_clouds > 0) {
for (int i = 0; i < options.num_point_clouds; ++i) {
string topic = topics.point_cloud2_topic;
if (options.num_point_clouds > 1) {
topic += "_" + std::to_string(i + 1);
}
expected_sensor_ids.insert(topic);
}
for (const string& topic : ComputeRepeatedTopicNames(
topics.point_cloud2_topic, options.num_point_clouds)) {
expected_topics.insert(topic);
}
// For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
// required.
@ -272,12 +241,24 @@ int Node::AddTrajectory(const TrajectoryOptions& options,
(node_options_.map_builder_options.use_trajectory_builder_2d() &&
options.trajectory_builder_options.trajectory_builder_2d_options()
.use_imu_data())) {
expected_sensor_ids.insert(topics.imu_topic);
expected_topics.insert(topics.imu_topic);
}
if (options.use_odometry) {
expected_sensor_ids.insert(topics.odometry_topic);
expected_topics.insert(topics.odometry_topic);
}
return map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
return expected_topics;
}
int Node::AddTrajectory(const TrajectoryOptions& options,
const cartographer_ros_msgs::SensorTopics& topics) {
const std::unordered_set<string> expected_sensor_ids =
ComputeExpectedTopics(options, topics);
const int trajectory_id =
map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
LaunchSubscribers(options, topics, trajectory_id);
subscribed_topics_.insert(expected_sensor_ids.begin(),
expected_sensor_ids.end());
return trajectory_id;
}
void Node::LaunchSubscribers(const TrajectoryOptions& options,
@ -285,7 +266,7 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options,
const int trajectory_id) {
if (options.use_laser_scan) {
const string topic = topics.laser_scan_topic;
laser_scan_subscribers_[trajectory_id] =
subscribers_[trajectory_id].push_back(
node_handle_.subscribe<sensor_msgs::LaserScan>(
topic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::LaserScan::ConstPtr&)>(
@ -293,40 +274,32 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options,
topic](const sensor_msgs::LaserScan::ConstPtr& msg) {
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleLaserScanMessage(topic, msg);
}));
})));
}
if (options.use_multi_echo_laser_scan) {
const string topic = topics.multi_echo_laser_scan_topic;
multi_echo_laser_scan_subscribers_[trajectory_id] =
node_handle_.subscribe<sensor_msgs::MultiEchoLaserScan>(
topic, kInfiniteSubscriberQueueSize,
boost::function<void(
const sensor_msgs::MultiEchoLaserScan::ConstPtr&)>(
[this, trajectory_id,
topic](const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleMultiEchoLaserScanMessage(topic, msg);
}));
subscribers_[trajectory_id].push_back(node_handle_.subscribe<
sensor_msgs::MultiEchoLaserScan>(
topic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::MultiEchoLaserScan::ConstPtr&)>(
[this, trajectory_id,
topic](const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleMultiEchoLaserScanMessage(topic, msg);
})));
}
std::vector<::ros::Subscriber> grouped_point_cloud_subscribers;
if (options.num_point_clouds > 0) {
for (int i = 0; i < options.num_point_clouds; ++i) {
string topic = topics.point_cloud2_topic;
if (options.num_point_clouds > 1) {
topic += "_" + std::to_string(i + 1);
}
grouped_point_cloud_subscribers.push_back(node_handle_.subscribe(
topic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::PointCloud2::ConstPtr&)>(
[this, trajectory_id,
topic](const sensor_msgs::PointCloud2::ConstPtr& msg) {
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandlePointCloud2Message(topic, msg);
})));
}
point_cloud_subscribers_[trajectory_id] = grouped_point_cloud_subscribers;
for (const string& topic : ComputeRepeatedTopicNames(
topics.point_cloud2_topic, options.num_point_clouds)) {
subscribers_[trajectory_id].push_back(node_handle_.subscribe(
topic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::PointCloud2::ConstPtr&)>(
[this, trajectory_id,
topic](const sensor_msgs::PointCloud2::ConstPtr& msg) {
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandlePointCloud2Message(topic, msg);
})));
}
// For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
@ -336,19 +309,20 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options,
options.trajectory_builder_options.trajectory_builder_2d_options()
.use_imu_data())) {
string topic = topics.imu_topic;
imu_subscribers_[trajectory_id] = node_handle_.subscribe<sensor_msgs::Imu>(
topic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::Imu::ConstPtr&)>(
[this, trajectory_id,
topic](const sensor_msgs::Imu::ConstPtr& msg) {
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleImuMessage(topic, msg);
}));
subscribers_[trajectory_id].push_back(
node_handle_.subscribe<sensor_msgs::Imu>(
topic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::Imu::ConstPtr&)>(
[this, trajectory_id,
topic](const sensor_msgs::Imu::ConstPtr& msg) {
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleImuMessage(topic, msg);
})));
}
if (options.use_odometry) {
string topic = topics.odometry_topic;
odom_subscribers_[trajectory_id] =
subscribers_[trajectory_id].push_back(
node_handle_.subscribe<nav_msgs::Odometry>(
topic, kInfiniteSubscriberQueueSize,
boost::function<void(const nav_msgs::Odometry::ConstPtr&)>(
@ -356,7 +330,7 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options,
topic](const nav_msgs::Odometry::ConstPtr& msg) {
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleOdometryMessage(topic, msg);
}));
})));
}
is_active_trajectory_[trajectory_id] = true;
@ -379,34 +353,13 @@ bool Node::ValidateTrajectoryOptions(const TrajectoryOptions& options) {
return false;
}
bool Node::ValidateTopicName(
bool Node::ValidateTopicNames(
const ::cartographer_ros_msgs::SensorTopics& topics,
const TrajectoryOptions& options) {
if (!IsTopicNameUnique(topics.laser_scan_topic, laser_scan_subscribers_)) {
return false;
}
if (!IsTopicNameUnique(topics.multi_echo_laser_scan_topic,
multi_echo_laser_scan_subscribers_)) {
return false;
}
if (!IsTopicNameUnique(topics.imu_topic, imu_subscribers_)) {
return false;
}
if (!IsTopicNameUnique(topics.odometry_topic, odom_subscribers_)) {
return false;
}
for (auto& subscribers : point_cloud_subscribers_) {
string topic = topics.point_cloud2_topic;
int count = 0;
for (auto& subscriber : subscribers.second) {
if (options.num_point_clouds > 1) {
topic += "_" + std::to_string(count + 1);
++count;
}
if (subscriber.getTopic() == topic) {
LOG(ERROR) << "Topic name [" << topic << "] is already used";
return false;
}
for (const std::string& topic : ComputeExpectedTopics(options, topics)) {
if (subscribed_topics_.count(topic) > 0) {
LOG(ERROR) << "Topic name [" << topic << "] is already used.";
return false;
}
}
return true;
@ -422,13 +375,12 @@ bool Node::HandleStartTrajectory(
LOG(ERROR) << "Invalid trajectory options.";
return false;
}
if (!Node::ValidateTopicName(request.topics, options)) {
if (!Node::ValidateTopicNames(request.topics, options)) {
LOG(ERROR) << "Invalid topics.";
return false;
}
const int trajectory_id = AddTrajectory(options, request.topics);
LaunchSubscribers(options, request.topics, trajectory_id);
response.trajectory_id = trajectory_id;
is_active_trajectory_[trajectory_id] = true;
@ -445,7 +397,6 @@ void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
topics.odometry_topic = kOdometryTopic;
const int trajectory_id = AddTrajectory(options, topics);
LaunchSubscribers(options, topics, trajectory_id);
is_active_trajectory_[trajectory_id] = true;
}
@ -464,18 +415,13 @@ bool Node::HandleFinishTrajectory(
return false;
}
ShutdownSubscriber(laser_scan_subscribers_, trajectory_id);
ShutdownSubscriber(multi_echo_laser_scan_subscribers_, trajectory_id);
ShutdownSubscriber(odom_subscribers_, trajectory_id);
ShutdownSubscriber(imu_subscribers_, trajectory_id);
if (point_cloud_subscribers_.count(trajectory_id) != 0) {
for (auto& entry : point_cloud_subscribers_[trajectory_id]) {
LOG(INFO) << "Shutdown the subscriber of [" << entry.getTopic() << "]";
entry.shutdown();
}
CHECK_EQ(point_cloud_subscribers_.erase(trajectory_id), 1);
// Shutdown the subscribers of this trajectory.
for (auto& entry : subscribers_[trajectory_id]) {
entry.shutdown();
subscribed_topics_.erase(entry.getTopic());
LOG(INFO) << "Shutdown the subscriber of [" << entry.getTopic() << "]";
}
CHECK_EQ(subscribers_.erase(trajectory_id), 1);
map_builder_bridge_.FinishTrajectory(trajectory_id);
is_active_trajectory_[trajectory_id] = false;
return true;

18
cartographer_ros/cartographer_ros/node.h
View File

@ -18,6 +18,8 @@
#define CARTOGRAPHER_ROS_NODE_H_
#include <memory>
#include <unordered_map>
#include <unordered_set>
#include <vector>
#include "cartographer/common/mutex.h"
@ -72,6 +74,10 @@ class Node {
bool HandleWriteAssets(
cartographer_ros_msgs::WriteAssets::Request& request,
cartographer_ros_msgs::WriteAssets::Response& response);
// Returns the set of topic names we want to subscribe to.
std::unordered_set<string> ComputeExpectedTopics(
const TrajectoryOptions& options,
const cartographer_ros_msgs::SensorTopics& topics);
int AddTrajectory(const TrajectoryOptions& options,
const cartographer_ros_msgs::SensorTopics& topics);
void LaunchSubscribers(const TrajectoryOptions& options,
@ -83,8 +89,8 @@ class Node {
void PublishConstraintList(const ::ros::WallTimerEvent& timer_event);
void SpinOccupancyGridThreadForever();
bool ValidateTrajectoryOptions(const TrajectoryOptions& options);
bool ValidateTopicName(const ::cartographer_ros_msgs::SensorTopics& topics,
const TrajectoryOptions& options);
bool ValidateTopicNames(const ::cartographer_ros_msgs::SensorTopics& topics,
const TrajectoryOptions& options);
const NodeOptions node_options_;
@ -104,12 +110,8 @@ class Node {
cartographer::common::Time::min();
// These are keyed with 'trajectory_id'.
std::unordered_map<int, ::ros::Subscriber> laser_scan_subscribers_;
std::unordered_map<int, ::ros::Subscriber> multi_echo_laser_scan_subscribers_;
std::unordered_map<int, ::ros::Subscriber> odom_subscribers_;
std::unordered_map<int, ::ros::Subscriber> imu_subscribers_;
std::unordered_map<int, std::vector<::ros::Subscriber>>
point_cloud_subscribers_;
std::unordered_map<int, std::vector<::ros::Subscriber>> subscribers_;
std::unordered_set<std::string> subscribed_topics_;
std::unordered_map<int, bool> is_active_trajectory_ GUARDED_BY(mutex_);
::ros::Publisher occupancy_grid_publisher_;
std::thread occupancy_grid_thread_;

37
cartographer_ros/cartographer_ros/node_constants.cc Normal file
View File

@ -0,0 +1,37 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "cartographer_ros/node_constants.h"
#include "glog/logging.h"
namespace cartographer_ros {
std::vector<std::string> ComputeRepeatedTopicNames(const std::string& topic,
const int num_topics) {
CHECK_GE(num_topics, 0);
if (num_topics == 1) {
return {topic};
}
std::vector<std::string> topics;
topics.reserve(num_topics);
for (int i = 0; i < num_topics; ++i) {
topics.emplace_back(topic + "_" + std::to_string(i + 1));
}
return topics;
}
} // namespace cartographer_ros

7
cartographer_ros/cartographer_ros/node_constants.h
View File

@ -17,6 +17,9 @@
#ifndef CARTOGRAPHER_ROS_NODE_CONSTANTS_H_
#define CARTOGRAPHER_ROS_NODE_CONSTANTS_H_
#include <string>
#include <vector>
namespace cartographer_ros {
// Default topic names; expected to be remapped as needed.
@ -39,6 +42,10 @@ constexpr double kConstraintPublishPeriodSec = 0.5;
constexpr int kInfiniteSubscriberQueueSize = 0;
constexpr int kLatestOnlyPublisherQueueSize = 1;
// For multiple topics adds numbers to the topic name and returns the list.
std::vector<std::string> ComputeRepeatedTopicNames(const std::string& topic,
int num_topics);
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_NODE_CONSTANTS_H_

14
cartographer_ros/cartographer_ros/offline_node_main.cc
View File

@ -123,16 +123,10 @@ void Run(const std::vector<string>& bag_filenames) {
check_insert(kMultiEchoLaserScanTopic);
}
// For 3D SLAM, subscribe to all point clouds topics.
if (trajectory_options.num_point_clouds > 0) {
for (int i = 0; i < trajectory_options.num_point_clouds; ++i) {
// TODO(hrapp): This code is duplicated in places. Pull out a method.
string topic = kPointCloud2Topic;
if (trajectory_options.num_point_clouds > 1) {
topic += "_" + std::to_string(i + 1);
}
check_insert(topic);
}
// Subscribe to all point cloud topics.
for (const string& topic : ComputeRepeatedTopicNames(
kPointCloud2Topic, trajectory_options.num_point_clouds)) {
check_insert(topic);
}
// For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is