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

Support multiple LaserScan and MultiEchoLaserScan topics. (#435) 

This fixes #212.
master
Wolfgang Hess 2017-07-21 14:07:15 +02:00 committed by GitHub
parent bd28ec1a58
commit e5c2881b90
13 changed files with 84 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
cartographer_ros/cartographer_ros/node.cc
View File

@ -225,11 +225,14 @@ std::unordered_set<string> Node::ComputeExpectedTopics(
const cartographer_ros_msgs::SensorTopics& topics) {
std::unordered_set<string> expected_topics;
if (options.use_laser_scan) {
expected_topics.insert(topics.laser_scan_topic);
for (const string& topic : ComputeRepeatedTopicNames(
topics.laser_scan_topic, options.num_laser_scans)) {
expected_topics.insert(topic);
}
if (options.use_multi_echo_laser_scan) {
expected_topics.insert(topics.multi_echo_laser_scan_topic);
for (const string& topic :
ComputeRepeatedTopicNames(topics.multi_echo_laser_scan_topic,
options.num_multi_echo_laser_scans)) {
expected_topics.insert(topic);
}
for (const string& topic : ComputeRepeatedTopicNames(
topics.point_cloud2_topic, options.num_point_clouds)) {
@ -264,8 +267,8 @@ int Node::AddTrajectory(const TrajectoryOptions& 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;
for (const string& topic : ComputeRepeatedTopicNames(
topics.laser_scan_topic, options.num_laser_scans)) {
subscribers_[trajectory_id].push_back(
node_handle_.subscribe<sensor_msgs::LaserScan>(
topic, kInfiniteSubscriberQueueSize,
@ -276,9 +279,9 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options,
->HandleLaserScanMessage(topic, msg);
})));
}
if (options.use_multi_echo_laser_scan) {
const string topic = topics.multi_echo_laser_scan_topic;
for (const string& topic :
ComputeRepeatedTopicNames(topics.multi_echo_laser_scan_topic,
options.num_multi_echo_laser_scans)) {
subscribers_[trajectory_id].push_back(node_handle_.subscribe<
sensor_msgs::MultiEchoLaserScan>(
topic, kInfiniteSubscriberQueueSize,
@ -289,7 +292,6 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options,
->HandleMultiEchoLaserScanMessage(topic, msg);
})));
}
for (const string& topic : ComputeRepeatedTopicNames(
topics.point_cloud2_topic, options.num_point_clouds)) {
subscribers_[trajectory_id].push_back(node_handle_.subscribe(

17
cartographer_ros/cartographer_ros/offline_node_main.cc
View File

@ -115,15 +115,16 @@ void Run(const std::vector<string>& bag_filenames) {
.second);
};
// For 2D SLAM, subscribe to exactly one horizontal laser.
if (trajectory_options.use_laser_scan) {
check_insert(kLaserScanTopic);
// Subscribe to all laser scan, multi echo laser scan, and point cloud topics.
for (const string& topic : ComputeRepeatedTopicNames(
kLaserScanTopic, trajectory_options.num_laser_scans)) {
check_insert(topic);
}
if (trajectory_options.use_multi_echo_laser_scan) {
check_insert(kMultiEchoLaserScanTopic);
for (const string& topic : ComputeRepeatedTopicNames(
kMultiEchoLaserScanTopic,
trajectory_options.num_multi_echo_laser_scans)) {
check_insert(topic);
}
// Subscribe to all point cloud topics.
for (const string& topic : ComputeRepeatedTopicNames(
kPointCloud2Topic, trajectory_options.num_point_clouds)) {
check_insert(topic);
@ -139,7 +140,7 @@ void Run(const std::vector<string>& bag_filenames) {
check_insert(kImuTopic);
}
// For both 2D and 3D SLAM, odometry is optional.
// Odometry is optional.
if (trajectory_options.use_odometry) {
check_insert(kOdometryTopic);
}

25
cartographer_ros/cartographer_ros/trajectory_options.cc
View File

@ -24,12 +24,12 @@ namespace {
void CheckTrajectoryOptions(const TrajectoryOptions& options) {
CHECK_GE(options.num_subdivisions_per_laser_scan, 1);
CHECK_EQ(options.use_laser_scan + options.use_multi_echo_laser_scan +
(options.num_point_clouds > 0),
CHECK_GE(options.num_laser_scans + options.num_multi_echo_laser_scans +
options.num_point_clouds,
1)
<< "Configuration error: 'use_laser_scan', "
"'use_multi_echo_laser_scan' and 'num_point_clouds' are "
"mutually exclusive, but one is required.";
<< "Configuration error: 'num_laser_scans', "
"'num_multi_echo_laser_scans' and 'num_point_clouds' are "
"all zero, but at least one is required.";
}
} // namespace
@ -49,9 +49,10 @@ TrajectoryOptions CreateTrajectoryOptions(
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_laser_scans =
lua_parameter_dictionary->GetNonNegativeInt("num_laser_scans");
options.num_multi_echo_laser_scans =
lua_parameter_dictionary->GetNonNegativeInt("num_multi_echo_laser_scans");
options.num_subdivisions_per_laser_scan =
lua_parameter_dictionary->GetNonNegativeInt(
"num_subdivisions_per_laser_scan");
@ -68,8 +69,8 @@ bool FromRosMessage(const cartographer_ros_msgs::TrajectoryOptions& msg,
options->odom_frame = msg.odom_frame;
options->provide_odom_frame = msg.provide_odom_frame;
options->use_odometry = msg.use_odometry;
options->use_laser_scan = msg.use_laser_scan;
options->use_multi_echo_laser_scan = msg.use_multi_echo_laser_scan;
options->num_laser_scans = msg.num_laser_scans;
options->num_multi_echo_laser_scans = msg.num_multi_echo_laser_scans;
options->num_subdivisions_per_laser_scan =
msg.num_subdivisions_per_laser_scan;
options->num_point_clouds = msg.num_point_clouds;
@ -90,8 +91,8 @@ cartographer_ros_msgs::TrajectoryOptions ToRosMessage(
msg.odom_frame = options.odom_frame;
msg.provide_odom_frame = options.provide_odom_frame;
msg.use_odometry = options.use_odometry;
msg.use_laser_scan = options.use_laser_scan;
msg.use_multi_echo_laser_scan = options.use_multi_echo_laser_scan;
msg.num_laser_scans = options.num_laser_scans;
msg.num_multi_echo_laser_scans = options.num_multi_echo_laser_scans;
msg.num_subdivisions_per_laser_scan = options.num_subdivisions_per_laser_scan;
msg.num_point_clouds = options.num_point_clouds;
options.trajectory_builder_options.SerializeToString(

4
cartographer_ros/cartographer_ros/trajectory_options.h
View File

@ -34,8 +34,8 @@ struct TrajectoryOptions {
string odom_frame;
bool provide_odom_frame;
bool use_odometry;
bool use_laser_scan;
bool use_multi_echo_laser_scan;
int num_laser_scans;
int num_multi_echo_laser_scans;
int num_subdivisions_per_laser_scan;
int num_point_clouds;
};

4
cartographer_ros/configuration_files/backpack_2d.lua
View File

@ -24,8 +24,8 @@ options = {
odom_frame = "odom",
provide_odom_frame = true,
use_odometry = false,
use_laser_scan = false,
use_multi_echo_laser_scan = true,
num_laser_scans = 0,
num_multi_echo_laser_scans = 1,
num_subdivisions_per_laser_scan = 10,
num_point_clouds = 0,
lookup_transform_timeout_sec = 0.2,

4
cartographer_ros/configuration_files/backpack_3d.lua
View File

@ -24,8 +24,8 @@ options = {
odom_frame = "odom",
provide_odom_frame = true,
use_odometry = false,
use_laser_scan = false,
use_multi_echo_laser_scan = false,
num_laser_scans = 0,
num_multi_echo_laser_scans = 0,
num_subdivisions_per_laser_scan = 1,
num_point_clouds = 2,
lookup_transform_timeout_sec = 0.2,

4
cartographer_ros/configuration_files/pr2.lua
View File

@ -24,8 +24,8 @@ options = {
odom_frame = "odom",
provide_odom_frame = true,
use_odometry = false,
use_laser_scan = true,
use_multi_echo_laser_scan = false,
num_laser_scans = 1,
num_multi_echo_laser_scans = 0,
num_subdivisions_per_laser_scan = 1,
num_point_clouds = 0,
lookup_transform_timeout_sec = 0.2,

4
cartographer_ros/configuration_files/revo_lds.lua
View File

@ -24,8 +24,8 @@ options = {
odom_frame = "odom",
provide_odom_frame = true,
use_odometry = false,
use_laser_scan = true,
use_multi_echo_laser_scan = false,
num_laser_scans = 1,
num_multi_echo_laser_scans = 0,
num_subdivisions_per_laser_scan = 1,
num_point_clouds = 0,
lookup_transform_timeout_sec = 0.2,

4
cartographer_ros/configuration_files/taurob_tracker.lua
View File

@ -24,8 +24,8 @@ options = {
odom_frame = "odom",
provide_odom_frame = false,
use_odometry = true,
use_laser_scan = true,
use_multi_echo_laser_scan = false,
num_laser_scans = 1,
num_multi_echo_laser_scans = 0,
num_subdivisions_per_laser_scan = 1,
num_point_clouds = 0,
lookup_transform_timeout_sec = 0.2,

4
cartographer_ros_msgs/msg/TrajectoryOptions.msg
View File

@ -17,8 +17,8 @@ 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_laser_scans
int32 num_multi_echo_laser_scans
int32 num_subdivisions_per_laser_scan
int32 num_point_clouds

10
cartographer_rviz/cartographer_rviz/drawable_submap.cc
View File

@ -64,11 +64,11 @@ Ogre::Quaternion ToOgre(const Eigen::Quaterniond& q) {
} // namespace
DrawableSubmap::DrawableSubmap(
const ::cartographer::mapping::SubmapId& id,
::rviz::DisplayContext* const display_context,
::rviz::Property* const submap_category, const bool visible,
const float pose_axes_length, const float pose_axes_radius)
DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id,
::rviz::DisplayContext* const display_context,
::rviz::Property* const submap_category,
const bool visible, const float pose_axes_length,
const float pose_axes_radius)
: id_(id),
display_context_(display_context),
scene_node_(display_context->getSceneManager()

30
docs/source/configuration.rst
View File

@ -55,21 +55,27 @@ use_odometry
If enabled, subscribes to `nav_msgs/Odometry`_ on the topic "odom". Odometry
must be provided in this case, and the information will be included in SLAM.
use_laser_scan
If enabled, the node subscribes to `sensor_msgs/LaserScan`_ on the "scan"
topic. If 2D SLAM is used, either this or *use_multi_echo_laser_scan*
must be enabled.
num_laser_scans
Number of laser scan topics to subscribe to. Subscribes to
`sensor_msgs/LaserScan`_ on the "scan" topic for one laser scanner, or topics
"scan_1", "scan_2", etc. for multiple laser scanners.
use_multi_echo_laser_scan
If enabled, the node subscribes to `sensor_msgs/MultiEchoLaserScan`_ on the
"echoes" topic. If 2D SLAM is used, either this or *use_laser_scan*
must be enabled.
num_multi_echo_laser_scans
Number of multi-echo laser scan topics to subscribe to. Subscribes to
`sensor_msgs/MultiEchoLaserScan`_ on the "echoes" topic for one laser scanner,
or topics "echoes_1", "echoes_2", etc. for multiple laser scanners.
num_subdivisions_per_laser_scan
Number of point clouds to split each received (multi-echo) laser scan into.
Subdividing a scan makes it possible to unwarp scans acquired while the
scanners are moving. There is a corresponding trajectory builder option to
accumulate the subdivided scans into a point cloud that will be used for scan
matching.
num_point_clouds
Number of 3D lasers to subscribe to. Must be a positive value if and only if
using 3D SLAM. Subscribes to `sensor_msgs/PointCloud2`_ on the "points2"
topic for one laser, or topics "points2_1", "points2_2", etc for multiple
lasers.
Number of point cloud topics to subscribe to. Subscribes to
`sensor_msgs/PointCloud2`_ on the "points2" topic for one rangefinder, or
topics "points2_1", "points2_2", etc. for multiple rangefinders.
lookup_transform_timeout_sec
Timeout in seconds to use for looking up transforms using `tf2`_.

22
docs/source/ros_api.rst
View File

@ -39,20 +39,24 @@ range data is required.
scan (`sensor_msgs/LaserScan`_)
Supported in 2D and 3D (e.g. using an axially rotating planar laser scanner).
If *use_laser_scan* is enabled in the :doc:`configuration`, this topic will
be used as input for SLAM.
If *num_laser_scans* is set to 1 in the :doc:`configuration`, this topic will
be used as input for SLAM. If *num_laser_scans* is greater than 1, multiple
numbered scan topics (i.e. scan_1, scan_2, scan_3, ... up to and including
*num_laser_scans*) will be used as inputs for SLAM.
echoes (`sensor_msgs/MultiEchoLaserScan`_)
Supported in 2D and 3D (e.g. using an axially rotating planar laser scanner).
If *use_multi_echo_laser_scan* is enabled in the :doc:`configuration`, this
topic will be used as input for SLAM. Only the first echo is used.
If *num_multi_echo_laser_scans* is set to 1 in the :doc:`configuration`, this
topic will be used as input for SLAM. Only the first echo is used. If
*num_multi_echo_laser_scans* is greater than 1, multiple numbered echoes
topics (i.e. echoes_1, echoes_2, echoes_3, ... up to and including
*num_multi_echo_laser_scans*) will be used as inputs for SLAM.
points2 (`sensor_msgs/PointCloud2`_)
Only supported in 3D. If *num_point_clouds* is set to 1 in the
:doc:`configuration`, this topic will be used as input for SLAM. If
*num_point_clouds* is greater than 1, multiple numbered points2 topics (i.e.
points2_1, points2_2, points2_3, ... up to and including *num_point_clouds*)
will be used as inputs for SLAM.
If *num_point_clouds* is set to 1 in the :doc:`configuration`, this topic will
be used as input for SLAM. If *num_point_clouds* is greater than 1, multiple
numbered points2 topics (i.e. points2_1, points2_2, points2_3, ... up to and
including *num_point_clouds*) will be used as inputs for SLAM.
The following additional sensor data topics may also be provided.