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

Pulls out sensor data wiring into main. (#198)

master
Damon Kohler 2016-11-29 13:28:11 +01:00 committed by GitHub
parent 102fb4ef4e
commit 141bf26dbb
4 changed files with 128 additions and 110 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
cartographer_ros/cartographer_ros/map_builder_bridge.h
View File

@ -25,7 +25,6 @@
#include "cartographer_ros/node_options.h"
#include "cartographer_ros/sensor_bridge.h"
#include "cartographer_ros/tf_bridge.h"
#include "cartographer_ros_msgs/FinishTrajectory.h"
#include "cartographer_ros_msgs/SubmapEntry.h"
#include "cartographer_ros_msgs/SubmapList.h"
#include "cartographer_ros_msgs/SubmapQuery.h"

105
cartographer_ros/cartographer_ros/node.cc
View File

@ -45,21 +45,14 @@ namespace carto = ::cartographer;
using carto::transform::Rigid3d;
constexpr int kInfiniteSubscriberQueueSize = 0;
constexpr int kLatestOnlyPublisherQueueSize = 1;
constexpr double kTfBufferCacheTimeInSeconds = 1e6;
// Unique default topic names. Expected to be remapped as needed.
constexpr char kLaserScanTopic[] = "scan";
constexpr char kMultiEchoLaserScanTopic[] = "echoes";
constexpr char kPointCloud2Topic[] = "points2";
constexpr char kImuTopic[] = "imu";
constexpr char kOdometryTopic[] = "odom";
// Default topic names; expected to be remapped as needed.
constexpr char kOccupancyGridTopic[] = "map";
constexpr char kScanMatchedPointCloudTopic[] = "scan_matched_points2";
constexpr char kSubmapListTopic[] = "submap_list";
constexpr char kSubmapQueryServiceName[] = "submap_query";
constexpr char kFinishTrajectoryServiceName[] = "finish_trajectory";
Node::Node(const NodeOptions& options)
: options_(options),
@ -70,8 +63,6 @@ Node::Node(const NodeOptions& options)
Node::~Node() {
{
carto::common::MutexLocker lock(&mutex_);
CHECK_GE(trajectory_id_, 0);
map_builder_bridge_.FinishTrajectory(trajectory_id_);
terminating_ = true;
}
if (occupancy_grid_thread_.joinable()) {
@ -81,77 +72,6 @@ Node::~Node() {
void Node::Initialize() {
carto::common::MutexLocker lock(&mutex_);
// For 2D SLAM, subscribe to exactly one horizontal laser.
if (options_.use_laser_scan) {
horizontal_laser_scan_subscriber_ = node_handle_.subscribe(
kLaserScanTopic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::LaserScan::ConstPtr&)>(
[this](const sensor_msgs::LaserScan::ConstPtr& msg) {
map_builder_bridge_.sensor_bridge(trajectory_id_)->HandleLaserScanMessage(
kLaserScanTopic, msg);
}));
expected_sensor_ids_.insert(kLaserScanTopic);
}
if (options_.use_multi_echo_laser_scan) {
horizontal_laser_scan_subscriber_ = node_handle_.subscribe(
kMultiEchoLaserScanTopic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::MultiEchoLaserScan::ConstPtr&)>(
[this](const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
map_builder_bridge_.sensor_bridge(trajectory_id_)
->HandleMultiEchoLaserScanMessage(kMultiEchoLaserScanTopic,
msg);
}));
expected_sensor_ids_.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) {
string topic = kPointCloud2Topic;
if (options_.num_point_clouds > 1) {
topic += "_" + std::to_string(i + 1);
}
point_cloud_subscribers_.push_back(node_handle_.subscribe(
topic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::PointCloud2::ConstPtr&)>(
[this, topic](const sensor_msgs::PointCloud2::ConstPtr& msg) {
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.
if (options_.map_builder_options.use_trajectory_builder_3d() ||
(options_.map_builder_options.use_trajectory_builder_2d() &&
options_.map_builder_options.trajectory_builder_2d_options()
.use_imu_data())) {
imu_subscriber_ = node_handle_.subscribe(
kImuTopic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::Imu::ConstPtr& msg)>(
[this](const sensor_msgs::Imu::ConstPtr& msg) {
map_builder_bridge_.sensor_bridge(trajectory_id_)->HandleImuMessage(kImuTopic,
msg);
}));
expected_sensor_ids_.insert(kImuTopic);
}
if (options_.use_odometry) {
odometry_subscriber_ = node_handle_.subscribe(
kOdometryTopic, kInfiniteSubscriberQueueSize,
boost::function<void(const nav_msgs::Odometry::ConstPtr&)>(
[this](const nav_msgs::Odometry::ConstPtr& msg) {
map_builder_bridge_.sensor_bridge(trajectory_id_)->HandleOdometryMessage(
kOdometryTopic, msg);
}));
expected_sensor_ids_.insert(kOdometryTopic);
}
trajectory_id_ = map_builder_bridge_.AddTrajectory(expected_sensor_ids_,
options_.tracking_frame);
submap_list_publisher_ =
node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(
kSubmapListTopic, kLatestOnlyPublisherQueueSize);
@ -171,9 +91,6 @@ void Node::Initialize() {
node_handle_.advertise<sensor_msgs::PointCloud2>(
kScanMatchedPointCloudTopic, kLatestOnlyPublisherQueueSize);
finish_trajectory_server_ = node_handle_.advertiseService(
kFinishTrajectoryServiceName, &Node::HandleFinishTrajectory, this);
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(options_.submap_publish_period_sec),
&Node::PublishSubmapList, this));
@ -182,6 +99,12 @@ void Node::Initialize() {
&Node::PublishTrajectoryStates, this));
}
void Node::Spin() { ::ros::spin(); }
::ros::NodeHandle* Node::node_handle() { return &node_handle_; }
MapBuilderBridge* Node::map_builder_bridge() { return &map_builder_bridge_; }
bool Node::HandleSubmapQuery(
::cartographer_ros_msgs::SubmapQuery::Request& request,
::cartographer_ros_msgs::SubmapQuery::Response& response) {
@ -189,18 +112,6 @@ bool Node::HandleSubmapQuery(
return map_builder_bridge_.HandleSubmapQuery(request, response);
}
bool Node::HandleFinishTrajectory(
::cartographer_ros_msgs::FinishTrajectory::Request& request,
::cartographer_ros_msgs::FinishTrajectory::Response&) {
carto::common::MutexLocker lock(&mutex_);
const int previous_trajectory_id = trajectory_id_;
trajectory_id_ = map_builder_bridge_.AddTrajectory(expected_sensor_ids_,
options_.tracking_frame);
map_builder_bridge_.FinishTrajectory(previous_trajectory_id);
map_builder_bridge_.WriteAssets(request.stem);
return true;
}
void Node::PublishSubmapList(const ::ros::WallTimerEvent& unused_timer_event) {
carto::common::MutexLocker lock(&mutex_);
submap_list_publisher_.publish(map_builder_bridge_.GetSubmapList());
@ -282,6 +193,4 @@ void Node::SpinOccupancyGridThreadForever() {
}
}
void Node::SpinForever() { ::ros::spin(); }
} // namespace cartographer_ros

13
cartographer_ros/cartographer_ros/node.h
View File

@ -43,16 +43,16 @@ class Node {
Node(const Node&) = delete;
Node& operator=(const Node&) = delete;
void SpinForever();
void Initialize();
void Spin();
::ros::NodeHandle* node_handle();
MapBuilderBridge* map_builder_bridge();
private:
bool HandleSubmapQuery(
cartographer_ros_msgs::SubmapQuery::Request& request,
cartographer_ros_msgs::SubmapQuery::Response& response);
bool HandleFinishTrajectory(
cartographer_ros_msgs::FinishTrajectory::Request& request,
cartographer_ros_msgs::FinishTrajectory::Response& response);
void PublishSubmapList(const ::ros::WallTimerEvent& timer_event);
void PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event);
@ -70,16 +70,11 @@ class Node {
std::unordered_set<string> expected_sensor_ids_;
::ros::NodeHandle node_handle_;
::ros::Subscriber imu_subscriber_;
::ros::Subscriber horizontal_laser_scan_subscriber_;
std::vector<::ros::Subscriber> point_cloud_subscribers_;
::ros::Subscriber odometry_subscriber_;
::ros::Publisher submap_list_publisher_;
::ros::ServiceServer submap_query_server_;
::ros::Publisher scan_matched_point_cloud_publisher_;
cartographer::common::Time last_scan_matched_point_cloud_time_ =
cartographer::common::Time::min();
::ros::ServiceServer finish_trajectory_server_;
::ros::Publisher occupancy_grid_publisher_;
std::thread occupancy_grid_thread_;

119
cartographer_ros/cartographer_ros/node_main.cc
View File

@ -35,6 +35,16 @@ DEFINE_string(configuration_basename, "",
namespace cartographer_ros {
namespace {
constexpr int kInfiniteSubscriberQueueSize = 0;
// Default topic names; expected to be remapped as needed.
constexpr char kLaserScanTopic[] = "scan";
constexpr char kMultiEchoLaserScanTopic[] = "echoes";
constexpr char kPointCloud2Topic[] = "points2";
constexpr char kImuTopic[] = "imu";
constexpr char kOdometryTopic[] = "odom";
constexpr char kFinishTrajectoryServiceName[] = "finish_trajectory";
void Run() {
auto file_resolver = cartographer::common::make_unique<
cartographer::common::ConfigurationFileResolver>(
@ -44,9 +54,114 @@ void Run() {
cartographer::common::LuaParameterDictionary lua_parameter_dictionary(
code, std::move(file_resolver));
Node node(CreateNodeOptions(&lua_parameter_dictionary));
const auto options = CreateNodeOptions(&lua_parameter_dictionary);
Node node(options);
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.map_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.Initialize();
node.SpinForever();
node.Spin();
node.map_builder_bridge()->FinishTrajectory(trajectory_id);
}
} // namespace