From d48034df46d3a871900e556e837f6a7df9b14bf9 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Thu, 29 Sep 2016 17:06:48 +0200 Subject: [PATCH] Simplify OccupancyGrid publishing. (#73) The publish_occupancy_grid option is removed, and the publisher always created in 2D. Only when subscribed to, we start computing OccupancyGrids. Adds documentation for published topics where this is now documented. --- .../configuration_files/backpack_2d.lua | 1 - .../configuration_files/backpack_3d.lua | 1 - .../src/cartographer_node_main.cc | 7 ++-- cartographer_ros/src/node_options.cc | 6 ---- cartographer_ros/src/node_options.h | 1 - cartographer_ros/src/occupancy_grid.cc | 2 ++ docs/source/index.rst | 1 + docs/source/options.rst | 5 --- docs/source/rosapi.rst | 36 +++++++++++++++++++ 9 files changed, 44 insertions(+), 16 deletions(-) create mode 100644 docs/source/rosapi.rst diff --git a/cartographer_ros/configuration_files/backpack_2d.lua b/cartographer_ros/configuration_files/backpack_2d.lua index 105bdb4..2ac2dc9 100644 --- a/cartographer_ros/configuration_files/backpack_2d.lua +++ b/cartographer_ros/configuration_files/backpack_2d.lua @@ -25,7 +25,6 @@ options = { use_constant_odometry_variance = false, constant_odometry_translational_variance = 0., constant_odometry_rotational_variance = 0., - publish_occupancy_grid = false, use_horizontal_laser = false, use_horizontal_multi_echo_laser = true, horizontal_laser_min_range = 0., diff --git a/cartographer_ros/configuration_files/backpack_3d.lua b/cartographer_ros/configuration_files/backpack_3d.lua index a3e9acb..3f2a0e4 100644 --- a/cartographer_ros/configuration_files/backpack_3d.lua +++ b/cartographer_ros/configuration_files/backpack_3d.lua @@ -25,7 +25,6 @@ options = { use_constant_odometry_variance = false, constant_odometry_translational_variance = 0., constant_odometry_rotational_variance = 0., - publish_occupancy_grid = false, use_horizontal_laser = false, use_horizontal_multi_echo_laser = false, horizontal_laser_min_range = 0., diff --git a/cartographer_ros/src/cartographer_node_main.cc b/cartographer_ros/src/cartographer_node_main.cc index 364e125..4ef3cc2 100644 --- a/cartographer_ros/src/cartographer_node_main.cc +++ b/cartographer_ros/src/cartographer_node_main.cc @@ -384,7 +384,7 @@ void Node::Initialize() { submap_query_server_ = node_handle_.advertiseService( kSubmapQueryServiceName, &Node::HandleSubmapQuery, this); - if (options_.publish_occupancy_grid) { + if (options_.map_builder_options.use_trajectory_builder_2d()) { occupancy_grid_publisher_ = node_handle_.advertise<::nav_msgs::OccupancyGrid>( kOccupancyGridTopic, kLatestOnlyPublisherQueueSize, @@ -535,16 +535,19 @@ void Node::PublishPoseAndScanMatchedPointCloud( void Node::SpinOccupancyGridThreadForever() { for (;;) { + std::this_thread::sleep_for(carto::common::FromMilliseconds(1000)); { carto::common::MutexLocker lock(&mutex_); if (terminating_) { return; } } + if (occupancy_grid_publisher_.getNumSubscribers() == 0) { + continue; + } const auto trajectory_nodes = map_builder_.sparse_pose_graph()->GetTrajectoryNodes(); if (trajectory_nodes.empty()) { - std::this_thread::sleep_for(carto::common::FromMilliseconds(1000)); continue; } ::nav_msgs::OccupancyGrid occupancy_grid; diff --git a/cartographer_ros/src/node_options.cc b/cartographer_ros/src/node_options.cc index c562c07..fc2a3ac 100644 --- a/cartographer_ros/src/node_options.cc +++ b/cartographer_ros/src/node_options.cc @@ -33,8 +33,6 @@ NodeOptions CreateNodeOptions( options.published_frame = lua_parameter_dictionary->GetString("published_frame"); options.odom_frame = lua_parameter_dictionary->GetString("odom_frame"); - options.publish_occupancy_grid = - lua_parameter_dictionary->GetBool("publish_occupancy_grid"); options.provide_odom_frame = lua_parameter_dictionary->GetBool("provide_odom_frame"); options.use_odometry_data = @@ -79,10 +77,6 @@ NodeOptions CreateNodeOptions( 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.publish_occupancy_grid) { - CHECK(options.map_builder_options.use_trajectory_builder_2d()) - << "Publishing OccupancyGrids for 3D data is not yet supported"; - } return options; } diff --git a/cartographer_ros/src/node_options.h b/cartographer_ros/src/node_options.h index 189d023..382ce31 100644 --- a/cartographer_ros/src/node_options.h +++ b/cartographer_ros/src/node_options.h @@ -32,7 +32,6 @@ struct NodeOptions { string tracking_frame; string published_frame; string odom_frame; - bool publish_occupancy_grid; bool provide_odom_frame; bool use_odometry_data; bool use_constant_odometry_variance; diff --git a/cartographer_ros/src/occupancy_grid.cc b/cartographer_ros/src/occupancy_grid.cc index 0fd445c..667a360 100644 --- a/cartographer_ros/src/occupancy_grid.cc +++ b/cartographer_ros/src/occupancy_grid.cc @@ -28,6 +28,8 @@ void BuildOccupancyGrid( const NodeOptions& options, ::nav_msgs::OccupancyGrid* const occupancy_grid) { namespace carto = ::cartographer; + CHECK(options.map_builder_options.use_trajectory_builder_2d()) + << "Publishing OccupancyGrids for 3D data is not yet supported"; const auto& submaps_options = options.map_builder_options.trajectory_builder_2d_options() .submaps_options(); diff --git a/docs/source/index.rst b/docs/source/index.rst index 060e318..c409b6a 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -21,6 +21,7 @@ Cartographer ROS Integration :hidden: options + rosapi `Cartographer`_ is a system that provides real-time simultaneous localization and mapping `SLAM`_ across multiple platforms and sensor configurations. This diff --git a/docs/source/options.rst b/docs/source/options.rst index 3e4f1ec..b4b0ba1 100644 --- a/docs/source/options.rst +++ b/docs/source/options.rst @@ -67,11 +67,6 @@ constant_odometry_rotational_variance The variance to use for the rotational component of odometry to use if *use_constant_odometry_variance* is enabled. -publish_occupancy_grid - If enabled, a background thread will continuously compute and publish - `nav_msgs::OccupancyGrid`_ messages on the "map" topic. Depending on the size - of the map, it can take a few seconds between updates. - use_horizontal_laser 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* diff --git a/docs/source/rosapi.rst b/docs/source/rosapi.rst new file mode 100644 index 0000000..1ccb8ee --- /dev/null +++ b/docs/source/rosapi.rst @@ -0,0 +1,36 @@ +.. 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. + +======= +ROS API +======= + +Published Topics +================ + +map (`nav_msgs/OccupancyGrid`_) + Currently only supported in 2D. If subscribed to, a background thread will + continuously compute and publish the map. Depending on the size of the map, it + can take a few seconds between updates. + +scan_matched_points2 (`sensor_msgs/PointCloud2`_) + Point cloud as it was used for the purpose of scan-to-submap matching. + +submap_list (`cartographer_ros_msgs/SubmapList`_) + List of all submaps of all trajectories, containing the pose and latest + version number of each submap. + +.. _cartographer_ros_msgs/SubmapList: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/msg/SubmapList.msg +.. _nav_msgs/OccupancyGrid: http://docs.ros.org/api/nav_msgs/html/msg/OccupancyGrid.html +.. _sensor_msgs/PointCloud2: http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud2.html