Enable multi-trajectory on ROS node (#346)
parent
cd1276a99c
commit
9d5b221ed4
|
@ -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);
|
||||
});
|
||||
}
|
||||
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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_;
|
||||
};
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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_);
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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
|
|
@ -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_
|
|
@ -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(
|
||||
|
|
|
@ -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
|
|
@ -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
|
|
@ -12,5 +12,5 @@
|
|||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
string stem
|
||||
int32 trajectory_id
|
||||
---
|
||||
|
|
|
@ -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
|
|
@ -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
|
||||
---
|
|
@ -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
|
||||
======================
|
||||
|
|
Loading…
Reference in New Issue