diff --git a/cartographer_ros/cartographer_ros/configuration_files_test.cc b/cartographer_ros/cartographer_ros/configuration_files_test.cc index 1237d00..2fff3cc 100644 --- a/cartographer_ros/cartographer_ros/configuration_files_test.cc +++ b/cartographer_ros/cartographer_ros/configuration_files_test.cc @@ -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 diff --git a/cartographer_ros/cartographer_ros/node_main.cc b/cartographer_ros/cartographer_ros/node_main.cc index 3e61baf..6959fb0 100644 --- a/cartographer_ros/cartographer_ros/node_main.cc +++ b/cartographer_ros/cartographer_ros/node_main.cc @@ -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( @@ -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( @@ -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( [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( diff --git a/cartographer_ros/cartographer_ros/node_options.cc b/cartographer_ros/cartographer_ros/node_options.cc index 03d64d7..2c7234a 100644 --- a/cartographer_ros/cartographer_ros/node_options.cc +++ b/cartographer_ros/cartographer_ros/node_options.cc @@ -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; } diff --git a/cartographer_ros/cartographer_ros/node_options.h b/cartographer_ros/cartographer_ros/node_options.h index 9461f2f..bb6f2ff 100644 --- a/cartographer_ros/cartographer_ros/node_options.h +++ b/cartographer_ros/cartographer_ros/node_options.h @@ -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; diff --git a/cartographer_ros/configuration_files/backpack_2d.lua b/cartographer_ros/configuration_files/backpack_2d.lua index 73904a5..13f5ce0 100644 --- a/cartographer_ros/configuration_files/backpack_2d.lua +++ b/cartographer_ros/configuration_files/backpack_2d.lua @@ -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, diff --git a/cartographer_ros/configuration_files/backpack_3d.lua b/cartographer_ros/configuration_files/backpack_3d.lua index e209e18..c85c712 100644 --- a/cartographer_ros/configuration_files/backpack_3d.lua +++ b/cartographer_ros/configuration_files/backpack_3d.lua @@ -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, diff --git a/cartographer_ros/configuration_files/pr2.lua b/cartographer_ros/configuration_files/pr2.lua index 42d3c31..d8ca9d4 100644 --- a/cartographer_ros/configuration_files/pr2.lua +++ b/cartographer_ros/configuration_files/pr2.lua @@ -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, diff --git a/cartographer_ros/configuration_files/revo_lds.lua b/cartographer_ros/configuration_files/revo_lds.lua index a2200df..6d687b5 100644 --- a/cartographer_ros/configuration_files/revo_lds.lua +++ b/cartographer_ros/configuration_files/revo_lds.lua @@ -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, diff --git a/cartographer_ros/configuration_files/taurob_tracker.lua b/cartographer_ros/configuration_files/taurob_tracker.lua new file mode 100644 index 0000000..0ce6826 --- /dev/null +++ b/cartographer_ros/configuration_files/taurob_tracker.lua @@ -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 diff --git a/cartographer_ros/launch/demo_taurob_tracker.launch b/cartographer_ros/launch/demo_taurob_tracker.launch new file mode 100644 index 0000000..1af7f65 --- /dev/null +++ b/cartographer_ros/launch/demo_taurob_tracker.launch @@ -0,0 +1,26 @@ + + + + + + + + + + diff --git a/cartographer_ros/launch/taurob_tracker.launch b/cartographer_ros/launch/taurob_tracker.launch new file mode 100644 index 0000000..bc5b51a --- /dev/null +++ b/cartographer_ros/launch/taurob_tracker.launch @@ -0,0 +1,26 @@ + + + + + + + + diff --git a/docs/source/configuration.rst b/docs/source/configuration.rst index c8b86f3..35492a5 100644 --- a/docs/source/configuration.rst +++ b/docs/source/configuration.rst @@ -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 diff --git a/docs/source/index.rst b/docs/source/index.rst index 54acb01..ae66163 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -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. diff --git a/docs/source/ros_api.rst b/docs/source/ros_api.rst index 20f4e51..387074d 100644 --- a/docs/source/ros_api.rst +++ b/docs/source/ros_api.rst @@ -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.