Enable multi-trajectory on ROS node (#346)

master
Yutaka Takaoka 2017-05-31 02:21:24 -07:00 committed by Holger Rapp
parent cd1276a99c
commit 9d5b221ed4
18 changed files with 561 additions and 222 deletions

View File

@ -37,6 +37,7 @@ TEST_P(ConfigurationFilesTest, ValidateNodeOptions) {
::cartographer::common::LuaParameterDictionary lua_parameter_dictionary(
code, std::move(file_resolver));
::cartographer_ros::CreateNodeOptions(&lua_parameter_dictionary);
::cartographer_ros::CreateTrajectoryOptions(&lua_parameter_dictionary);
});
}

View File

@ -23,24 +23,28 @@
namespace cartographer_ros {
MapBuilderBridge::MapBuilderBridge(const NodeOptions& options,
MapBuilderBridge::MapBuilderBridge(const NodeOptions& node_options,
tf2_ros::Buffer* const tf_buffer)
: options_(options),
map_builder_(options.map_builder_options),
: node_options_(node_options),
map_builder_(node_options.map_builder_options),
tf_buffer_(tf_buffer) {}
int MapBuilderBridge::AddTrajectory(
const std::unordered_set<string>& expected_sensor_ids,
const string& tracking_frame) {
const TrajectoryOptions& trajectory_options) {
const int trajectory_id = map_builder_.AddTrajectoryBuilder(
expected_sensor_ids, options_.trajectory_builder_options);
expected_sensor_ids, trajectory_options.trajectory_builder_options);
LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'.";
CHECK_EQ(sensor_bridges_.count(trajectory_id), 0);
sensor_bridges_[trajectory_id] =
cartographer::common::make_unique<SensorBridge>(
tracking_frame, options_.lookup_transform_timeout_sec, tf_buffer_,
trajectory_options.tracking_frame,
node_options_.lookup_transform_timeout_sec, tf_buffer_,
map_builder_.GetTrajectoryBuilder(trajectory_id));
auto emplace_result =
trajectory_options_.emplace(trajectory_id, trajectory_options);
CHECK(emplace_result.second == true);
return trajectory_id;
}
@ -64,18 +68,22 @@ void MapBuilderBridge::WriteAssets(const string& stem) {
LOG(WARNING) << "No data was collected and no assets will be written.";
} else {
LOG(INFO) << "Writing assets with stem '" << stem << "'...";
if (options_.map_builder_options.use_trajectory_builder_2d()) {
// TODO(yutakaoka): Add multi-trajectory support.
CHECK_EQ(trajectory_options_.count(0), 1);
if (node_options_.map_builder_options.use_trajectory_builder_2d()) {
Write2DAssets(
trajectory_nodes, options_.map_frame,
options_.trajectory_builder_options.trajectory_builder_2d_options()
trajectory_nodes, node_options_.map_frame,
trajectory_options_[0]
.trajectory_builder_options.trajectory_builder_2d_options()
.submaps_options(),
stem);
}
if (options_.map_builder_options.use_trajectory_builder_3d()) {
if (node_options_.map_builder_options.use_trajectory_builder_3d()) {
Write3DAssets(
trajectory_nodes,
options_.trajectory_builder_options.trajectory_builder_3d_options()
trajectory_options_[0]
.trajectory_builder_options.trajectory_builder_3d_options()
.submaps_options()
.high_resolution(),
stem);
@ -108,7 +116,7 @@ bool MapBuilderBridge::HandleSubmapQuery(
cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {
cartographer_ros_msgs::SubmapList submap_list;
submap_list.header.stamp = ::ros::Time::now();
submap_list.header.frame_id = options_.map_frame;
submap_list.header.frame_id = node_options_.map_frame;
for (int trajectory_id = 0;
trajectory_id < map_builder_.num_trajectory_builders();
++trajectory_id) {
@ -133,7 +141,7 @@ cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {
std::unique_ptr<nav_msgs::OccupancyGrid>
MapBuilderBridge::BuildOccupancyGrid() {
CHECK(options_.map_builder_options.use_trajectory_builder_2d())
CHECK(node_options_.map_builder_options.use_trajectory_builder_2d())
<< "Publishing OccupancyGrids for 3D data is not yet supported";
std::vector<::cartographer::mapping::TrajectoryNode> trajectory_nodes;
for (const auto& single_trajectory :
@ -145,9 +153,11 @@ MapBuilderBridge::BuildOccupancyGrid() {
if (!trajectory_nodes.empty()) {
occupancy_grid =
cartographer::common::make_unique<nav_msgs::OccupancyGrid>();
CHECK_EQ(trajectory_options_.count(0), 1);
BuildOccupancyGrid2D(
trajectory_nodes, options_.map_frame,
options_.trajectory_builder_options.trajectory_builder_2d_options()
trajectory_nodes, node_options_.map_frame,
trajectory_options_[0]
.trajectory_builder_options.trajectory_builder_2d_options()
.submaps_options(),
occupancy_grid.get());
}
@ -169,12 +179,15 @@ MapBuilderBridge::GetTrajectoryStates() {
continue;
}
CHECK_EQ(trajectory_options_.count(trajectory_id), 1);
trajectory_states[trajectory_id] = {
pose_estimate,
map_builder_.sparse_pose_graph()->GetLocalToGlobalTransform(
trajectory_id),
sensor_bridge.tf_bridge().LookupToTracking(pose_estimate.time,
options_.published_frame)};
sensor_bridge.tf_bridge().LookupToTracking(
pose_estimate.time,
trajectory_options_[trajectory_id].published_frame),
trajectory_options_[trajectory_id]};
}
return trajectory_states;
}

View File

@ -26,6 +26,7 @@
#include "cartographer_ros/node_options.h"
#include "cartographer_ros/sensor_bridge.h"
#include "cartographer_ros/tf_bridge.h"
#include "cartographer_ros/trajectory_options.h"
#include "cartographer_ros_msgs/SubmapEntry.h"
#include "cartographer_ros_msgs/SubmapList.h"
#include "cartographer_ros_msgs/SubmapQuery.h"
@ -39,15 +40,16 @@ class MapBuilderBridge {
cartographer::mapping::TrajectoryBuilder::PoseEstimate pose_estimate;
cartographer::transform::Rigid3d local_to_map;
std::unique_ptr<cartographer::transform::Rigid3d> published_to_tracking;
TrajectoryOptions trajectory_options;
};
MapBuilderBridge(const NodeOptions& options, tf2_ros::Buffer* tf_buffer);
MapBuilderBridge(const NodeOptions& node_options, tf2_ros::Buffer* tf_buffer);
MapBuilderBridge(const MapBuilderBridge&) = delete;
MapBuilderBridge& operator=(const MapBuilderBridge&) = delete;
int AddTrajectory(const std::unordered_set<string>& expected_sensor_ids,
const string& tracking_frame);
const TrajectoryOptions& trajectory_options);
void FinishTrajectory(int trajectory_id);
void WriteAssets(const string& stem);
@ -62,10 +64,12 @@ class MapBuilderBridge {
SensorBridge* sensor_bridge(int trajectory_id);
private:
const NodeOptions options_;
const NodeOptions node_options_;
cartographer::mapping::MapBuilder map_builder_;
tf2_ros::Buffer* const tf_buffer_;
// These are keyed with 'trajectory_id'.
std::unordered_map<int, TrajectoryOptions> trajectory_options_;
std::unordered_map<int, std::unique_ptr<SensorBridge>> sensor_bridges_;
};

View File

@ -21,6 +21,8 @@
#include <vector>
#include "Eigen/Core"
#include "cartographer/common/configuration_file_resolver.h"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/make_unique.h"
#include "cartographer/common/port.h"
#include "cartographer/common/time.h"
@ -45,30 +47,59 @@ namespace carto = ::cartographer;
using carto::transform::Rigid3d;
namespace {
constexpr int kInfiniteSubscriberQueueSize = 0;
constexpr int kLatestOnlyPublisherQueueSize = 1;
Node::Node(const NodeOptions& options, tf2_ros::Buffer* const tf_buffer)
: options_(options), map_builder_bridge_(options_, tf_buffer) {}
Node::~Node() {
{
carto::common::MutexLocker lock(&mutex_);
terminating_ = true;
}
if (occupancy_grid_thread_.joinable()) {
occupancy_grid_thread_.join();
TrajectoryOptions ToTrajectoryOptions(
cartographer_ros_msgs::TrajectoryOptions ros_options) {
TrajectoryOptions options;
options.tracking_frame = ros_options.tracking_frame;
options.published_frame = ros_options.published_frame;
options.odom_frame = ros_options.odom_frame;
options.provide_odom_frame = ros_options.provide_odom_frame;
options.use_odometry = ros_options.use_odometry;
options.use_laser_scan = ros_options.use_laser_scan;
options.use_multi_echo_laser_scan = ros_options.use_multi_echo_laser_scan;
options.num_point_clouds = ros_options.num_point_clouds;
if (!options.trajectory_builder_options.ParseFromString(
ros_options.trajectory_builder_options_proto)) {
ROS_ERROR("Failed to parse protobuf");
}
return options;
}
void Node::Initialize() {
void ShutdownSubscriber(std::unordered_map<int, ::ros::Subscriber>& subscribers,
int trajectory_id) {
if (subscribers.count(trajectory_id) == 0) {
return;
}
subscribers[trajectory_id].shutdown();
ROS_INFO_STREAM("Shutdown the subscriber of ["
<< subscribers[trajectory_id].getTopic() << "]");
CHECK_EQ(subscribers.erase(trajectory_id), 1);
}
} // namespace
Node::Node(const NodeOptions& node_options, tf2_ros::Buffer* const tf_buffer)
: node_options_(node_options),
map_builder_bridge_(node_options_, tf_buffer) {
carto::common::MutexLocker lock(&mutex_);
submap_list_publisher_ =
node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(
kSubmapListTopic, kLatestOnlyPublisherQueueSize);
submap_query_server_ = node_handle_.advertiseService(
kSubmapQueryServiceName, &Node::HandleSubmapQuery, this);
service_servers_.push_back(node_handle_.advertiseService(
kSubmapQueryServiceName, &Node::HandleSubmapQuery, this));
service_servers_.push_back(node_handle_.advertiseService(
kStartTrajectoryServiceName, &Node::HandleStartTrajectory, this));
service_servers_.push_back(node_handle_.advertiseService(
kFinishTrajectoryServiceName, &Node::HandleFinishTrajectory, this));
service_servers_.push_back(node_handle_.advertiseService(
kWriteAssetsServiceName, &Node::HandleWriteAssets, this));
if (options_.map_builder_options.use_trajectory_builder_2d()) {
if (node_options_.map_builder_options.use_trajectory_builder_2d()) {
occupancy_grid_publisher_ =
node_handle_.advertise<::nav_msgs::OccupancyGrid>(
kOccupancyGridTopic, kLatestOnlyPublisherQueueSize,
@ -82,13 +113,23 @@ void Node::Initialize() {
kScanMatchedPointCloudTopic, kLatestOnlyPublisherQueueSize);
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(options_.submap_publish_period_sec),
::ros::WallDuration(node_options_.submap_publish_period_sec),
&Node::PublishSubmapList, this));
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(options_.pose_publish_period_sec),
::ros::WallDuration(node_options_.pose_publish_period_sec),
&Node::PublishTrajectoryStates, this));
}
Node::~Node() {
{
carto::common::MutexLocker lock(&mutex_);
terminating_ = true;
}
if (occupancy_grid_thread_.joinable()) {
occupancy_grid_thread_.join();
}
}
::ros::NodeHandle* Node::node_handle() { return &node_handle_; }
MapBuilderBridge* Node::map_builder_bridge() { return &map_builder_bridge_; }
@ -123,7 +164,7 @@ void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) {
last_scan_matched_point_cloud_time_) {
scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message(
carto::common::ToUniversal(trajectory_state.pose_estimate.time),
options_.tracking_frame,
trajectory_state.trajectory_options.tracking_frame,
carto::sensor::TransformPointCloud(
trajectory_state.pose_estimate.point_cloud,
tracking_to_local.inverse().cast<float>())));
@ -135,27 +176,29 @@ void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) {
}
if (trajectory_state.published_to_tracking != nullptr) {
if (options_.provide_odom_frame) {
if (trajectory_state.trajectory_options.provide_odom_frame) {
std::vector<geometry_msgs::TransformStamped> stamped_transforms;
stamped_transform.header.frame_id = options_.map_frame;
// TODO(damonkohler): 'odom_frame' and 'published_frame' must be
// per-trajectory to fully support the multi-robot use case.
stamped_transform.child_frame_id = options_.odom_frame;
stamped_transform.header.frame_id = node_options_.map_frame;
stamped_transform.child_frame_id =
trajectory_state.trajectory_options.odom_frame;
stamped_transform.transform =
ToGeometryMsgTransform(trajectory_state.local_to_map);
stamped_transforms.push_back(stamped_transform);
stamped_transform.header.frame_id = options_.odom_frame;
stamped_transform.child_frame_id = options_.published_frame;
stamped_transform.header.frame_id =
trajectory_state.trajectory_options.odom_frame;
stamped_transform.child_frame_id =
trajectory_state.trajectory_options.published_frame;
stamped_transform.transform = ToGeometryMsgTransform(
tracking_to_local * (*trajectory_state.published_to_tracking));
stamped_transforms.push_back(stamped_transform);
tf_broadcaster_.sendTransform(stamped_transforms);
} else {
stamped_transform.header.frame_id = options_.map_frame;
stamped_transform.child_frame_id = options_.published_frame;
stamped_transform.header.frame_id = node_options_.map_frame;
stamped_transform.child_frame_id =
trajectory_state.trajectory_options.published_frame;
stamped_transform.transform = ToGeometryMsgTransform(
tracking_to_map * (*trajectory_state.published_to_tracking));
tf_broadcaster_.sendTransform(stamped_transform);
@ -183,4 +226,192 @@ void Node::SpinOccupancyGridThreadForever() {
}
}
int Node::AddTrajectory(const TrajectoryOptions& options,
const cartographer_ros_msgs::SensorTopics& topics) {
std::unordered_set<string> expected_sensor_ids;
if (node_options_.map_builder_options.use_trajectory_builder_2d()) {
// Using point clouds is only supported in 3D.
CHECK_EQ(options.num_point_clouds, 0);
}
if (options.use_laser_scan) {
expected_sensor_ids.insert(topics.laser_scan_topic);
}
if (options.use_multi_echo_laser_scan) {
expected_sensor_ids.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);
}
}
if (options.trajectory_builder_options.trajectory_builder_2d_options()
.use_imu_data()) {
expected_sensor_ids.insert(topics.imu_topic);
}
if (options.use_odometry) {
expected_sensor_ids.insert(topics.odometry_topic);
}
return map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
}
void Node::LaunchSubscribers(const TrajectoryOptions& options,
const cartographer_ros_msgs::SensorTopics& topics,
const int trajectory_id) {
if (options.use_laser_scan) {
const string topic = topics.laser_scan_topic;
laser_scan_subscribers_[trajectory_id] =
node_handle_.subscribe<sensor_msgs::LaserScan>(
topic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::LaserScan::ConstPtr&)>(
[this, trajectory_id,
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);
}));
}
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;
}
if (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);
}));
}
if (options.use_odometry) {
string topic = topics.odometry_topic;
odom_subscribers_[trajectory_id] =
node_handle_.subscribe<nav_msgs::Odometry>(
topic, kInfiniteSubscriberQueueSize,
boost::function<void(const nav_msgs::Odometry::ConstPtr&)>(
[this, trajectory_id,
topic](const nav_msgs::Odometry::ConstPtr& msg) {
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleOdometryMessage(topic, msg);
}));
}
is_active_trajectory_[trajectory_id] = true;
}
bool Node::HandleStartTrajectory(
::cartographer_ros_msgs::StartTrajectory::Request& request,
::cartographer_ros_msgs::StartTrajectory::Response& response) {
carto::common::MutexLocker lock(&mutex_);
std::unordered_set<string> expected_sensor_ids;
TrajectoryOptions options = ToTrajectoryOptions(request.options);
const int trajectory_id = AddTrajectory(options, request.topics);
LaunchSubscribers(options, request.topics, trajectory_id);
is_active_trajectory_[trajectory_id] = true;
return true;
}
void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
carto::common::MutexLocker lock(&mutex_);
cartographer_ros_msgs::SensorTopics topics;
topics.laser_scan_topic = kLaserScanTopic;
topics.multi_echo_laser_scan_topic = kMultiEchoLaserScanTopic;
topics.point_cloud2_topic = kPointCloud2Topic;
topics.imu_topic = kImuTopic;
topics.odometry_topic = kOdometryTopic;
const int trajectory_id = AddTrajectory(options, topics);
LaunchSubscribers(options, topics, trajectory_id);
}
bool Node::HandleFinishTrajectory(
::cartographer_ros_msgs::FinishTrajectory::Request& request,
::cartographer_ros_msgs::FinishTrajectory::Response& response) {
carto::common::MutexLocker lock(&mutex_);
const int trajectory_id = request.trajectory_id;
if (is_active_trajectory_.count(trajectory_id) == 0) {
ROS_INFO_STREAM("Trajectory_id " << trajectory_id
<< " is not created yet.");
return false;
}
if (!is_active_trajectory_[trajectory_id]) {
ROS_INFO_STREAM("Trajectory_id " << trajectory_id
<< " has already been finished.");
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]) {
ROS_INFO_STREAM("Shutdown the subscriber of [" << entry.getTopic()
<< "]");
entry.shutdown();
}
CHECK_EQ(point_cloud_subscribers_.erase(trajectory_id), 1);
}
map_builder_bridge_.FinishTrajectory(trajectory_id);
is_active_trajectory_[trajectory_id] = false;
return true;
}
bool Node::HandleWriteAssets(
::cartographer_ros_msgs::WriteAssets::Request& request,
::cartographer_ros_msgs::WriteAssets::Response& response) {
carto::common::MutexLocker lock(&mutex_);
map_builder_bridge_.WriteAssets(request.stem);
return true;
}
void Node::FinishAllTrajectories() {
carto::common::MutexLocker lock(&mutex_);
for (const auto& entry : is_active_trajectory_) {
const int trajectory_id = entry.first;
if (entry.second) {
map_builder_bridge_.FinishTrajectory(trajectory_id);
}
}
}
} // namespace cartographer_ros

View File

@ -23,11 +23,16 @@
#include "cartographer/common/mutex.h"
#include "cartographer_ros/map_builder_bridge.h"
#include "cartographer_ros/node_options.h"
#include "cartographer_ros/trajectory_options.h"
#include "cartographer_ros_msgs/FinishTrajectory.h"
#include "cartographer_ros_msgs/SensorTopics.h"
#include "cartographer_ros_msgs/StartTrajectory.h"
#include "cartographer_ros_msgs/SubmapEntry.h"
#include "cartographer_ros_msgs/SubmapList.h"
#include "cartographer_ros_msgs/SubmapQuery.h"
#include "cartographer_ros_msgs/TrajectoryOptions.h"
#include "cartographer_ros_msgs/TrajectorySubmapList.h"
#include "cartographer_ros_msgs/WriteAssets.h"
#include "ros/ros.h"
#include "tf2_ros/transform_broadcaster.h"
@ -44,17 +49,23 @@ constexpr char kOccupancyGridTopic[] = "map";
constexpr char kScanMatchedPointCloudTopic[] = "scan_matched_points2";
constexpr char kSubmapListTopic[] = "submap_list";
constexpr char kSubmapQueryServiceName[] = "submap_query";
constexpr char kStartTrajectoryServiceName[] = "start_trajectory";
constexpr char kWriteAssetsServiceName[] = "write_assets";
// Wires up ROS topics to SLAM.
class Node {
public:
Node(const NodeOptions& options, tf2_ros::Buffer* tf_buffer);
Node(const NodeOptions& node_options, tf2_ros::Buffer* tf_buffer);
~Node();
Node(const Node&) = delete;
Node& operator=(const Node&) = delete;
void Initialize();
// Finishes all yet active trajectories.
void FinishAllTrajectories();
// Starts the first trajectory with the default topics.
void StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options);
::ros::NodeHandle* node_handle();
MapBuilderBridge* map_builder_bridge();
@ -63,27 +74,47 @@ class Node {
bool HandleSubmapQuery(
cartographer_ros_msgs::SubmapQuery::Request& request,
cartographer_ros_msgs::SubmapQuery::Response& response);
bool HandleStartTrajectory(
cartographer_ros_msgs::StartTrajectory::Request& request,
cartographer_ros_msgs::StartTrajectory::Response& response);
bool HandleFinishTrajectory(
cartographer_ros_msgs::FinishTrajectory::Request& request,
cartographer_ros_msgs::FinishTrajectory::Response& response);
bool HandleWriteAssets(
cartographer_ros_msgs::WriteAssets::Request& request,
cartographer_ros_msgs::WriteAssets::Response& response);
int AddTrajectory(const TrajectoryOptions& options,
const cartographer_ros_msgs::SensorTopics& topics);
void LaunchSubscribers(const TrajectoryOptions& options,
const cartographer_ros_msgs::SensorTopics& topics,
int trajectory_id);
void PublishSubmapList(const ::ros::WallTimerEvent& timer_event);
void PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event);
void SpinOccupancyGridThreadForever();
const NodeOptions options_;
const NodeOptions node_options_;
tf2_ros::TransformBroadcaster tf_broadcaster_;
cartographer::common::Mutex mutex_;
MapBuilderBridge map_builder_bridge_ GUARDED_BY(mutex_);
int trajectory_id_ = -1;
std::unordered_set<string> expected_sensor_ids_;
::ros::NodeHandle node_handle_;
::ros::Publisher submap_list_publisher_;
::ros::ServiceServer submap_query_server_;
// These ros::ServiceServers need to live for the lifetime of the node.
std::vector<::ros::ServiceServer> service_servers_;
::ros::Publisher scan_matched_point_cloud_publisher_;
cartographer::common::Time last_scan_matched_point_cloud_time_ =
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, bool> is_active_trajectory_ GUARDED_BY(mutex_);
::ros::Publisher occupancy_grid_publisher_;
std::thread occupancy_grid_thread_;
bool terminating_ = false GUARDED_BY(mutex_);

View File

@ -22,6 +22,7 @@
#include "cartographer/common/make_unique.h"
#include "cartographer/common/port.h"
#include "cartographer_ros/node.h"
#include "cartographer_ros/node_options.h"
#include "cartographer_ros/ros_log_sink.h"
#include "gflags/gflags.h"
#include "tf2_ros/transform_listener.h"
@ -37,9 +38,7 @@ DEFINE_string(configuration_basename, "",
namespace cartographer_ros {
namespace {
constexpr int kInfiniteSubscriberQueueSize = 0;
NodeOptions LoadOptions() {
std::tuple<NodeOptions, TrajectoryOptions> LoadOptions() {
auto file_resolver = cartographer::common::make_unique<
cartographer::common::ConfigurationFileResolver>(
std::vector<string>{FLAGS_configuration_directory});
@ -48,121 +47,24 @@ NodeOptions LoadOptions() {
cartographer::common::LuaParameterDictionary lua_parameter_dictionary(
code, std::move(file_resolver));
return CreateNodeOptions(&lua_parameter_dictionary);
return std::make_tuple(CreateNodeOptions(&lua_parameter_dictionary),
CreateTrajectoryOptions(&lua_parameter_dictionary));
}
void Run() {
const auto options = LoadOptions();
constexpr double kTfBufferCacheTimeInSeconds = 1e6;
tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)};
tf2_ros::TransformListener tf(tf_buffer);
Node node(options, &tf_buffer);
node.Initialize();
NodeOptions node_options;
TrajectoryOptions trajectory_options;
std::tie(node_options, trajectory_options) = LoadOptions();
int trajectory_id = -1;
std::unordered_set<string> expected_sensor_ids;
// For 2D SLAM, subscribe to exactly one horizontal laser.
::ros::Subscriber laser_scan_subscriber;
if (options.use_laser_scan) {
laser_scan_subscriber = node.node_handle()->subscribe(
kLaserScanTopic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::LaserScan::ConstPtr&)>(
[&](const sensor_msgs::LaserScan::ConstPtr& msg) {
node.map_builder_bridge()
->sensor_bridge(trajectory_id)
->HandleLaserScanMessage(kLaserScanTopic, msg);
}));
expected_sensor_ids.insert(kLaserScanTopic);
}
if (options.use_multi_echo_laser_scan) {
laser_scan_subscriber = node.node_handle()->subscribe(
kMultiEchoLaserScanTopic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::MultiEchoLaserScan::ConstPtr&)>(
[&](const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
node.map_builder_bridge()
->sensor_bridge(trajectory_id)
->HandleMultiEchoLaserScanMessage(kMultiEchoLaserScanTopic,
msg);
}));
expected_sensor_ids.insert(kMultiEchoLaserScanTopic);
}
// For 3D SLAM, subscribe to all point clouds topics.
std::vector<::ros::Subscriber> point_cloud_subscribers;
if (options.num_point_clouds > 0) {
for (int i = 0; i < options.num_point_clouds; ++i) {
string topic = kPointCloud2Topic;
if (options.num_point_clouds > 1) {
topic += "_" + std::to_string(i + 1);
}
point_cloud_subscribers.push_back(node.node_handle()->subscribe(
topic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::PointCloud2::ConstPtr&)>(
[&, topic](const sensor_msgs::PointCloud2::ConstPtr& msg) {
node.map_builder_bridge()
->sensor_bridge(trajectory_id)
->HandlePointCloud2Message(topic, msg);
})));
expected_sensor_ids.insert(topic);
}
}
// For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
// required.
::ros::Subscriber imu_subscriber;
if (options.map_builder_options.use_trajectory_builder_3d() ||
(options.map_builder_options.use_trajectory_builder_2d() &&
options.trajectory_builder_options.trajectory_builder_2d_options()
.use_imu_data())) {
imu_subscriber = node.node_handle()->subscribe(
kImuTopic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::Imu::ConstPtr& msg)>(
[&](const sensor_msgs::Imu::ConstPtr& msg) {
node.map_builder_bridge()
->sensor_bridge(trajectory_id)
->HandleImuMessage(kImuTopic, msg);
}));
expected_sensor_ids.insert(kImuTopic);
}
// For both 2D and 3D SLAM, odometry is optional.
::ros::Subscriber odometry_subscriber;
if (options.use_odometry) {
odometry_subscriber = node.node_handle()->subscribe(
kOdometryTopic, kInfiniteSubscriberQueueSize,
boost::function<void(const nav_msgs::Odometry::ConstPtr&)>(
[&](const nav_msgs::Odometry::ConstPtr& msg) {
node.map_builder_bridge()
->sensor_bridge(trajectory_id)
->HandleOdometryMessage(kOdometryTopic, msg);
}));
expected_sensor_ids.insert(kOdometryTopic);
}
trajectory_id = node.map_builder_bridge()->AddTrajectory(
expected_sensor_ids, options.tracking_frame);
::ros::ServiceServer finish_trajectory_server =
node.node_handle()->advertiseService(
kFinishTrajectoryServiceName,
boost::function<bool(
::cartographer_ros_msgs::FinishTrajectory::Request&,
::cartographer_ros_msgs::FinishTrajectory::Response&)>(
[&](::cartographer_ros_msgs::FinishTrajectory::Request& request,
::cartographer_ros_msgs::FinishTrajectory::Response&) {
const int previous_trajectory_id = trajectory_id;
trajectory_id = node.map_builder_bridge()->AddTrajectory(
expected_sensor_ids, options.tracking_frame);
node.map_builder_bridge()->FinishTrajectory(
previous_trajectory_id);
node.map_builder_bridge()->WriteAssets(request.stem);
return true;
}));
Node node(node_options, &tf_buffer);
node.StartTrajectoryWithDefaultTopics(trajectory_options);
::ros::spin();
node.map_builder_bridge()->FinishTrajectory(trajectory_id);
node.FinishAllTrajectories();
}
} // namespace

View File

@ -27,23 +27,7 @@ NodeOptions CreateNodeOptions(
options.map_builder_options =
::cartographer::mapping::CreateMapBuilderOptions(
lua_parameter_dictionary->GetDictionary("map_builder").get());
options.trajectory_builder_options =
::cartographer::mapping::CreateTrajectoryBuilderOptions(
lua_parameter_dictionary->GetDictionary("trajectory_builder").get());
options.map_frame = lua_parameter_dictionary->GetString("map_frame");
options.tracking_frame =
lua_parameter_dictionary->GetString("tracking_frame");
options.published_frame =
lua_parameter_dictionary->GetString("published_frame");
options.odom_frame = lua_parameter_dictionary->GetString("odom_frame");
options.provide_odom_frame =
lua_parameter_dictionary->GetBool("provide_odom_frame");
options.use_odometry = lua_parameter_dictionary->GetBool("use_odometry");
options.use_laser_scan = lua_parameter_dictionary->GetBool("use_laser_scan");
options.use_multi_echo_laser_scan =
lua_parameter_dictionary->GetBool("use_multi_echo_laser_scan");
options.num_point_clouds =
lua_parameter_dictionary->GetNonNegativeInt("num_point_clouds");
options.lookup_transform_timeout_sec =
lua_parameter_dictionary->GetDouble("lookup_transform_timeout_sec");
options.submap_publish_period_sec =
@ -51,17 +35,6 @@ NodeOptions CreateNodeOptions(
options.pose_publish_period_sec =
lua_parameter_dictionary->GetDouble("pose_publish_period_sec");
CHECK_EQ(options.use_laser_scan + options.use_multi_echo_laser_scan +
(options.num_point_clouds > 0),
1)
<< "Configuration error: 'use_laser_scan', "
"'use_multi_echo_laser_scan' and 'num_point_clouds' are "
"mutually exclusive, but one is required.";
if (options.map_builder_options.use_trajectory_builder_2d()) {
// Using point clouds is only supported in 3D.
CHECK_EQ(options.num_point_clouds, 0);
}
return options;
}

View File

@ -20,26 +20,15 @@
#include <string>
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/port.h"
#include "cartographer/mapping/map_builder.h"
#include "cartographer_ros/sensor_bridge.h"
#include "cartographer_ros/trajectory_options.h"
namespace cartographer_ros {
// Top-level options of Cartographer's ROS integration.
struct NodeOptions {
::cartographer::mapping::proto::MapBuilderOptions map_builder_options;
::cartographer::mapping::proto::TrajectoryBuilderOptions
trajectory_builder_options;
string map_frame;
string tracking_frame;
string published_frame;
string odom_frame;
bool provide_odom_frame;
bool use_odometry;
bool use_laser_scan;
bool use_multi_echo_laser_scan;
int num_point_clouds;
double lookup_transform_timeout_sec;
double submap_publish_period_sec;
double pose_publish_period_sec;

View File

@ -24,6 +24,7 @@
#include "cartographer/common/make_unique.h"
#include "cartographer/common/port.h"
#include "cartographer_ros/node.h"
#include "cartographer_ros/node_options.h"
#include "cartographer_ros/ros_log_sink.h"
#include "cartographer_ros/urdf_reader.h"
#include "gflags/gflags.h"
@ -71,7 +72,9 @@ std::vector<string> SplitString(const string& input, const char delimiter) {
return tokens;
}
NodeOptions LoadOptions() {
// TODO(hrapp): This is duplicated in node_main.cc. Pull out into a config
// unit.
std::tuple<NodeOptions, TrajectoryOptions> LoadOptions() {
auto file_resolver = cartographer::common::make_unique<
cartographer::common::ConfigurationFileResolver>(
std::vector<string>{FLAGS_configuration_directory});
@ -80,11 +83,14 @@ NodeOptions LoadOptions() {
cartographer::common::LuaParameterDictionary lua_parameter_dictionary(
code, std::move(file_resolver));
return CreateNodeOptions(&lua_parameter_dictionary);
return std::make_tuple(CreateNodeOptions(&lua_parameter_dictionary),
CreateTrajectoryOptions(&lua_parameter_dictionary));
}
void Run(const std::vector<string>& bag_filenames) {
auto options = LoadOptions();
NodeOptions node_options;
TrajectoryOptions trajectory_options;
std::tie(node_options, trajectory_options) = LoadOptions();
tf2_ros::Buffer tf_buffer;
@ -99,9 +105,8 @@ void Run(const std::vector<string>& bag_filenames) {
// Since we preload the transform buffer, we should never have to wait for a
// transform. When we finish processing the bag, we will simply drop any
// remaining sensor data that cannot be transformed due to missing transforms.
options.lookup_transform_timeout_sec = 0.;
Node node(options, &tf_buffer);
node.Initialize();
node_options.lookup_transform_timeout_sec = 0.;
Node node(node_options, &tf_buffer);
std::unordered_set<string> expected_sensor_ids;
const auto check_insert = [&expected_sensor_ids, &node](const string& topic) {
@ -110,18 +115,19 @@ void Run(const std::vector<string>& bag_filenames) {
};
// For 2D SLAM, subscribe to exactly one horizontal laser.
if (options.use_laser_scan) {
if (trajectory_options.use_laser_scan) {
check_insert(kLaserScanTopic);
}
if (options.use_multi_echo_laser_scan) {
if (trajectory_options.use_multi_echo_laser_scan) {
check_insert(kMultiEchoLaserScanTopic);
}
// For 3D SLAM, subscribe to all point clouds topics.
if (options.num_point_clouds > 0) {
for (int i = 0; i < options.num_point_clouds; ++i) {
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 (options.num_point_clouds > 1) {
if (trajectory_options.num_point_clouds > 1) {
topic += "_" + std::to_string(i + 1);
}
check_insert(topic);
@ -130,15 +136,16 @@ void Run(const std::vector<string>& bag_filenames) {
// For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
// required.
if (options.map_builder_options.use_trajectory_builder_3d() ||
(options.map_builder_options.use_trajectory_builder_2d() &&
options.trajectory_builder_options.trajectory_builder_2d_options()
if (node_options.map_builder_options.use_trajectory_builder_3d() ||
(node_options.map_builder_options.use_trajectory_builder_2d() &&
trajectory_options.trajectory_builder_options
.trajectory_builder_2d_options()
.use_imu_data())) {
check_insert(kImuTopic);
}
// For both 2D and 3D SLAM, odometry is optional.
if (options.use_odometry) {
if (trajectory_options.use_odometry) {
check_insert(kOdometryTopic);
}
@ -162,7 +169,7 @@ void Run(const std::vector<string>& bag_filenames) {
}
const int trajectory_id = node.map_builder_bridge()->AddTrajectory(
expected_sensor_ids, options.tracking_frame);
expected_sensor_ids, trajectory_options);
rosbag::Bag bag;
bag.open(bag_filename, rosbag::bagmode::Read);

View File

@ -0,0 +1,53 @@
/*
* 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_options.h"
#include "glog/logging.h"
namespace cartographer_ros {
TrajectoryOptions CreateTrajectoryOptions(
::cartographer::common::LuaParameterDictionary* const
lua_parameter_dictionary) {
TrajectoryOptions options;
options.trajectory_builder_options =
::cartographer::mapping::CreateTrajectoryBuilderOptions(
lua_parameter_dictionary->GetDictionary("trajectory_builder").get());
options.tracking_frame =
lua_parameter_dictionary->GetString("tracking_frame");
options.published_frame =
lua_parameter_dictionary->GetString("published_frame");
options.odom_frame = lua_parameter_dictionary->GetString("odom_frame");
options.provide_odom_frame =
lua_parameter_dictionary->GetBool("provide_odom_frame");
options.use_odometry = lua_parameter_dictionary->GetBool("use_odometry");
options.use_laser_scan = lua_parameter_dictionary->GetBool("use_laser_scan");
options.use_multi_echo_laser_scan =
lua_parameter_dictionary->GetBool("use_multi_echo_laser_scan");
options.num_point_clouds =
lua_parameter_dictionary->GetNonNegativeInt("num_point_clouds");
CHECK_EQ(options.use_laser_scan + options.use_multi_echo_laser_scan +
(options.num_point_clouds > 0),
1)
<< "Configuration error: 'use_laser_scan', "
"'use_multi_echo_laser_scan' and 'num_point_clouds' are "
"mutually exclusive, but one is required.";
return options;
}
} // namespace cartographer_ros

View File

@ -0,0 +1,46 @@
/*
* 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.
*/
#ifndef CARTOGRAPHER_ROS_TRAJECTORY_OPTIONS_H_
#define CARTOGRAPHER_ROS_TRAJECTORY_OPTIONS_H_
#include <string>
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/port.h"
#include "cartographer_ros/sensor_bridge.h"
namespace cartographer_ros {
struct TrajectoryOptions {
::cartographer::mapping::proto::TrajectoryBuilderOptions
trajectory_builder_options;
string tracking_frame;
string published_frame;
string odom_frame;
bool provide_odom_frame;
bool use_odometry;
bool use_laser_scan;
bool use_multi_echo_laser_scan;
int num_point_clouds;
};
TrajectoryOptions CreateTrajectoryOptions(
::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary);
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_TRAJECTORY_OPTIONS_H_

View File

@ -27,12 +27,16 @@ add_message_files(
SubmapList.msg
TrajectorySubmapList.msg
SubmapEntry.msg
SensorTopics.msg
TrajectoryOptions.msg
)
add_service_files(
FILES
SubmapQuery.srv
FinishTrajectory.srv
StartTrajectory.srv
WriteAssets.srv
)
generate_messages(

View File

@ -0,0 +1,19 @@
# 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.
string laser_scan_topic
string multi_echo_laser_scan_topic
string point_cloud2_topic
string imu_topic
string odometry_topic

View File

@ -0,0 +1,26 @@
# 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.
string tracking_frame
string published_frame
string odom_frame
bool provide_odom_frame
bool use_odometry
bool use_laser_scan
bool use_multi_echo_laser_scan
int32 num_point_clouds
# This is a binary-encoded
# 'cartographer.mapping.proto.TrajectoryBuilderOptions' proto.
string trajectory_builder_options_proto

View File

@ -12,5 +12,5 @@
# See the License for the specific language governing permissions and
# limitations under the License.
string stem
int32 trajectory_id
---

View File

@ -0,0 +1,18 @@
# 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.
cartographer_ros_msgs/TrajectoryOptions options
cartographer_ros_msgs/SensorTopics topics
---
int32 trajectory_id

View File

@ -0,0 +1,16 @@
# 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.
string stem
---

View File

@ -88,11 +88,17 @@ Services
submap_query (`cartographer_ros_msgs/SubmapQuery`_)
Fetches the requested submap.
start_trajectory (`cartographer_ros_msgs/StartTrajectory`_)
Starts another trajectory by specifying its sensor topics and trajectory
options as an binary-encoded proto. Returns an assigned trajectory ID.
finish_trajectory (`cartographer_ros_msgs/FinishTrajectory`_)
Finishes the current trajectory by flushing all queued sensor data, running a
final optimization, and writing artifacts (e.g. the map) to disk. The `stem`
argument is used as a prefix for the various files which are written. Files
will usually end up in `~/.ros` or `ROS_HOME` if it is set.
Finishes the given `trajectory_id`'s trajectory by running a final optimization.
write_assets (`cartographer_ros_msgs/WriteAssets`_)
Writes artifacts (e.g. the map) to disk. The `stem` argument is used as a prefix
for the various files which are written. Files will usually end up in `~/.ros` or
`ROS_HOME` if it is set.
Required tf Transforms
======================