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.

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.,

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.,

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;

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;
 }
 

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;

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();

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

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*

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