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

Improves parameter names and fixes #40. (#181)

master
Damon Kohler 2016-11-22 15:39:35 +01:00 committed by GitHub
parent ca5b7d1bff
commit 479694963c
14 changed files with 169 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
cartographer_ros/cartographer_ros/configuration_files_test.cc
View File

@ -42,7 +42,8 @@ TEST_P(ConfigurationFilesTest, ValidateNodeOptions) {
INSTANTIATE_TEST_CASE_P(ValidateAllNodeOptions, ConfigurationFilesTest,
::testing::Values("backpack_2d.lua", "backpack_3d.lua",
"pr2.lua", "revo_lds.lua"));
"pr2.lua", "revo_lds.lua",
"taurob_tracker.lua"));
} // namespace
} // namespace cartographer_ros

18
cartographer_ros/cartographer_ros/node_main.cc
View File

@ -135,7 +135,7 @@ class Node {
::ros::NodeHandle node_handle_;
::ros::Subscriber imu_subscriber_;
::ros::Subscriber horizontal_laser_scan_subscriber_;
std::vector<::ros::Subscriber> laser_subscribers_3d_;
std::vector<::ros::Subscriber> point_cloud_subscribers_;
::ros::Subscriber odometry_subscriber_;
::ros::Publisher submap_list_publisher_;
::ros::ServiceServer submap_query_server_;
@ -176,7 +176,7 @@ void Node::Initialize() {
carto::common::MutexLocker lock(&mutex_);
// For 2D SLAM, subscribe to exactly one horizontal laser.
if (options_.use_horizontal_laser) {
if (options_.use_laser_scan) {
horizontal_laser_scan_subscriber_ = node_handle_.subscribe(
kLaserScanTopic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::LaserScan::ConstPtr&)>(
@ -185,7 +185,7 @@ void Node::Initialize() {
}));
expected_sensor_ids_.insert(kLaserScanTopic);
}
if (options_.use_horizontal_multi_echo_laser) {
if (options_.use_multi_echo_laser_scan) {
horizontal_laser_scan_subscriber_ = node_handle_.subscribe(
kMultiEchoLaserScanTopic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::MultiEchoLaserScan::ConstPtr&)>(
@ -196,14 +196,14 @@ void Node::Initialize() {
expected_sensor_ids_.insert(kMultiEchoLaserScanTopic);
}
// For 3D SLAM, subscribe to all 3D lasers.
if (options_.num_lasers_3d > 0) {
for (int i = 0; i < options_.num_lasers_3d; ++i) {
// 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_lasers_3d > 1) {
if (options_.num_point_clouds > 1) {
topic += "_" + std::to_string(i + 1);
}
laser_subscribers_3d_.push_back(node_handle_.subscribe(
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) {
@ -228,7 +228,7 @@ void Node::Initialize() {
expected_sensor_ids_.insert(kImuTopic);
}
if (options_.use_odometry_data) {
if (options_.use_odometry) {
odometry_subscriber_ = node_handle_.subscribe(
kOdometryTopic, kInfiniteSubscriberQueueSize,
boost::function<void(const nav_msgs::Odometry::ConstPtr&)>(

33
cartographer_ros/cartographer_ros/node_options.cc
View File

@ -35,14 +35,12 @@ NodeOptions CreateNodeOptions(
options.odom_frame = lua_parameter_dictionary->GetString("odom_frame");
options.provide_odom_frame =
lua_parameter_dictionary->GetBool("provide_odom_frame");
options.use_odometry_data =
lua_parameter_dictionary->GetBool("use_odometry_data");
options.use_horizontal_laser =
lua_parameter_dictionary->GetBool("use_horizontal_laser");
options.use_horizontal_multi_echo_laser =
lua_parameter_dictionary->GetBool("use_horizontal_multi_echo_laser");
options.num_lasers_3d =
lua_parameter_dictionary->GetNonNegativeInt("num_lasers_3d");
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 =
@ -50,18 +48,17 @@ NodeOptions CreateNodeOptions(
options.pose_publish_period_sec =
lua_parameter_dictionary->GetDouble("pose_publish_period_sec");
CHECK_EQ(options.use_horizontal_laser +
options.use_horizontal_multi_echo_laser +
(options.num_lasers_3d > 0),
CHECK_EQ(options.use_laser_scan + options.use_multi_echo_laser_scan +
(options.num_point_clouds > 0),
1)
<< "Configuration error: 'use_horizontal_laser', "
"'use_horizontal_multi_echo_laser' and 'num_lasers_3d' are "
<< "Configuration error: 'use_laser_scan', "
"'use_multi_echo_laser_scan' and 'num_point_clouds' are "
"mutually exclusive, but one is required.";
CHECK_EQ(
options.map_builder_options.use_trajectory_builder_2d(),
options.use_horizontal_laser || options.use_horizontal_multi_echo_laser);
CHECK_EQ(options.map_builder_options.use_trajectory_builder_3d(),
options.num_lasers_3d > 0);
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;
}

8
cartographer_ros/cartographer_ros/node_options.h
View File

@ -34,10 +34,10 @@ struct NodeOptions {
string published_frame;
string odom_frame;
bool provide_odom_frame;
bool use_odometry_data;
bool use_horizontal_laser;
bool use_horizontal_multi_echo_laser;
int num_lasers_3d;
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;

8
cartographer_ros/configuration_files/backpack_2d.lua
View File

@ -21,10 +21,10 @@ options = {
published_frame = "base_link",
odom_frame = "odom",
provide_odom_frame = true,
use_odometry_data = false,
use_horizontal_laser = false,
use_horizontal_multi_echo_laser = true,
num_lasers_3d = 0,
use_odometry = false,
use_laser_scan = false,
use_multi_echo_laser_scan = true,
num_point_clouds = 0,
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,
pose_publish_period_sec = 5e-3,

8
cartographer_ros/configuration_files/backpack_3d.lua
View File

@ -21,10 +21,10 @@ options = {
published_frame = "base_link",
odom_frame = "odom",
provide_odom_frame = true,
use_odometry_data = false,
use_horizontal_laser = false,
use_horizontal_multi_echo_laser = false,
num_lasers_3d = 2,
use_odometry = false,
use_laser_scan = false,
use_multi_echo_laser_scan = false,
num_point_clouds = 2,
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,
pose_publish_period_sec = 5e-3,

8
cartographer_ros/configuration_files/pr2.lua
View File

@ -21,10 +21,10 @@ options = {
published_frame = "base_footprint",
odom_frame = "odom",
provide_odom_frame = true,
use_odometry_data = false,
use_horizontal_laser = true,
use_horizontal_multi_echo_laser = false,
num_lasers_3d = 0,
use_odometry = false,
use_laser_scan = true,
use_multi_echo_laser_scan = false,
num_point_clouds = 0,
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,
pose_publish_period_sec = 5e-3,

8
cartographer_ros/configuration_files/revo_lds.lua
View File

@ -21,10 +21,10 @@ options = {
published_frame = "horizontal_laser_link",
odom_frame = "odom",
provide_odom_frame = true,
use_odometry_data = false,
use_horizontal_laser = true,
use_horizontal_multi_echo_laser = false,
num_lasers_3d = 0,
use_odometry = false,
use_laser_scan = true,
use_multi_echo_laser_scan = false,
num_point_clouds = 0,
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,
pose_publish_period_sec = 5e-3,

49
cartographer_ros/configuration_files/taurob_tracker.lua Normal file
View File

@ -0,0 +1,49 @@
-- 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 "map_builder.lua"
options = {
map_builder = MAP_BUILDER,
map_frame = "map",
tracking_frame = "base_link",
published_frame = "odom",
odom_frame = "odom",
provide_odom_frame = false,
use_odometry = true,
use_laser_scan = true,
use_multi_echo_laser_scan = false,
num_point_clouds = 0,
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,
pose_publish_period_sec = 5e-3,
}
TRAJECTORY_BUILDER_3D.scans_per_accumulation = 90
TRAJECTORY_BUILDER_3D.laser_min_range = 0.5
TRAJECTORY_BUILDER_3D.laser_max_range = 20.
MAP_BUILDER.use_trajectory_builder_3d = true
MAP_BUILDER.num_background_threads = 7
MAP_BUILDER.sparse_pose_graph.optimization_problem.huber_scale = 5e2
MAP_BUILDER.sparse_pose_graph.optimize_every_n_scans = 320
MAP_BUILDER.sparse_pose_graph.constraint_builder.sampling_ratio = 0.03
MAP_BUILDER.sparse_pose_graph.optimization_problem.ceres_solver_options.max_num_iterations = 10
-- Reuse the coarser 3D voxel filter to speed up the computation of loop closure
-- constraints.
MAP_BUILDER.sparse_pose_graph.constraint_builder.adaptive_voxel_filter = TRAJECTORY_BUILDER_3D.high_resolution_adaptive_voxel_filter
MAP_BUILDER.sparse_pose_graph.constraint_builder.min_score = 0.62
MAP_BUILDER.sparse_pose_graph.constraint_builder.log_matches = true
return options

26
cartographer_ros/launch/demo_taurob_tracker.launch Normal file
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.
-->
<launch>
<param name="/use_sim_time" value="true" />
<include file="$(find cartographer_ros)/launch/taurob_tracker.launch" />
<node name="rviz" pkg="rviz" type="rviz" required="true"
args="-d $(find cartographer_ros)/configuration_files/demo_3d.rviz" />
<node name="playbag" pkg="rosbag" type="play"
args="--clock $(arg bag_filename)" />
</launch>

26
cartographer_ros/launch/taurob_tracker.launch Normal file
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.
-->
<launch>
<node name="cartographer_node" pkg="cartographer_ros"
type="cartographer_node" args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename taurob_tracker.lua"
output="screen">
<remap from="scan" to="/spin_laser/scan" />
<remap from="imu" to="/imu/data" />
</node>
</launch>

12
docs/source/configuration.rst
View File

@ -51,21 +51,21 @@ provide_odom_frame
If enabled, the local, non-loop-closed, continuous pose will be published as
the *odom_frame* in the *map_frame*.
use_odometry_data
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_horizontal_laser
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_horizontal_multi_echo_laser*
topic. If 2D SLAM is used, either this or *use_multi_echo_laser_scan*
must be enabled.
use_horizontal_multi_echo_laser
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_horizontal_laser*
"echoes" topic. If 2D SLAM is used, either this or *use_laser_scan*
must be enabled.
num_lasers_3d
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

6
docs/source/index.rst
View File

@ -108,4 +108,10 @@ the demo:
# Launch the PR2 demo.
roslaunch cartographer_ros demo_pr2.launch bag_filename:=${HOME}/Downloads/2011-09-15-08-32-46.bag
# Download the Taurob Tracker example bag.
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/taurob_tracker/taurob_tracker_simulation.bag
# Launch the Taurob Tracker demo.
roslaunch cartographer_ros demo_taurob_tracker.launch bag_filename:=${HOME}/Downloads/taurob_tracker_simulation.bag
The launch files will bring up ``roscore`` and ``rviz`` automatically.

19
docs/source/ros_api.rst
View File

@ -38,19 +38,20 @@ The following range data topics are mutually exclusive. At least one source of
range data is required.
scan (`sensor_msgs/LaserScan`_)
Only supported in 2D. If *use_horizontal_laser* is enabled in the
:doc:`configuration`, this topic will be used as input for SLAM.
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.
echoes (`sensor_msgs/MultiEchoLaserScan`_)
Only supported in 2D. If *use_horizontal_multi_echo_laser* is enabled in the
:doc:`configuration`, this topic will be used as input for SLAM. Only the
first echo is used.
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.
points2 (`sensor_msgs/PointCloud2`_)
Only supported in 3D. If *num_lasers_3d* is set to 1 in the
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_lasers_3d* is greater than 1, multiple numbered points2 topics (i.e.
points2_1, points2_2, points2_3, ... up to and including *num_lasers_3d*)
*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.
@ -60,7 +61,7 @@ imu (`sensor_msgs/Imu`_)
input for SLAM.
odom (`nav_msgs/Odometry`_)
Supported in 2D (optional) and 3D (optional). If *use_odometry_data* is
Supported in 2D (optional) and 3D (optional). If *use_odometry* is
enabled in the :doc:`configuration`, this topic will be used as input for
SLAM.