Add costmap downsampler - fixes SteveMacenski/navigation2#4

nav2_bringup/bringup/params/nav2_params.yaml

@@ -254,6 +254,8 @@ planner_server:
 
     GridBased:
       tolerance: 0.5                    # tolerance for planning if unable to reach exact pose, in meters
+      downsample_costmap: false         # whether or not to downsample the map to the specified resolution
+      downsampling_factor: 3            # multiplier for the resolution of the costmap layer
       allow_unknown: false              # allow traveling in unknown space
       travel_cost_scale: 0.8            # [0,1] metric for how much you value staying out of higher-cost spaces (higher more adverse)
       max_iterations: -1                # maximum total iterations to search for before returning
@@ -270,8 +272,8 @@ planner_server:
           w_curve: 30.0 #3.0 # range 0 - 50
           w_dist: 0.0 #300 w/0 cost
           w_smooth: 15000.0 #15000
-          w_cost: 0.025
-          max_curve: 2.0         # translates to |delta angle| / |delta distance|, where distance is ~2x costmap resolution
+          w_cost: 0.025         # Use higher value when using the costmap downsampler (e.g. use 2.5 for downsampling_factor = 3)
+          max_curve: 2.0        # translates to |delta angle| / |delta distance|, where distance is ~2x costmap resolution
           cost_scaling_factor: 1.0 # this should match the inflation layer's parameter
           # 7.8 in 5cm resolution (45 deg / 10cm)
           # 4.0 in 10cm resolution (45 deg / 20 cm)

nav2_bringup/bringup/rviz/nav2_default_view.rviz

@@ -249,6 +249,20 @@ Visualization Manager:
             Value: /global_costmap/costmap
           Use Timestamp: false
           Value: true
+        - Alpha: 0.3
+          Class: rviz_default_plugins/Map
+          Color Scheme: costmap
+          Draw Behind: false
+          Enabled: true
+          Name: Downsampled Costmap
+          Topic:
+            Depth: 1
+            Durability Policy: Transient Local
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /downsampled_costmap
+          Use Timestamp: false
+          Value: true
         - Alpha: 1
           Buffer Length: 1
           Class: rviz_default_plugins/Path

smac_planner/include/smac_planner/costmap_downsampler.hpp

@@ -0,0 +1,194 @@
+// Copyright (c) 2020, Carlos Luis
+//
+// 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. Reserved.
+
+#ifndef SMAC_PLANNER__DOWNSAMPLER_HPP_
+#define SMAC_PLANNER__DOWNSAMPLER_HPP_
+
+#include <algorithm>
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "smac_planner/constants.hpp"
+
+namespace smac_planner
+{
+
+/**
+ * @class smac_planner::CostmapDownsampler
+ * @brief A costmap downsampler for more efficient path planning
+ */
+class CostmapDownsampler
+{
+public:
+  /**
+   * @brief A constructor for CostmapDownsampler
+   * @param node Lifecycle node pointer
+   */
+  explicit CostmapDownsampler(const nav2_util::LifecycleNode::SharedPtr & node)
+  : _costmap(nullptr),
+    _downsampled_costmap(nullptr),
+    _downsampled_costmap_pub(nullptr),
+    _node(node)
+  {
+  }
+
+  /**
+   * @brief A destructor for CostmapDownsampler
+   */
+  ~CostmapDownsampler()
+  {
+  }
+
+  /**
+   * @brief Initialize the downsampled costmap object and the ROS publisher
+   * @param global_frame The ID of the global frame used by the costmap
+   * @param topic_name The name of the topic to publish the downsampled costmap
+   * @param costmap The costmap we want to downsample
+   * @param downsampling_factor Multiplier for the costmap resolution
+   */
+   void initialize(
+    const std::string & global_frame,
+    const std::string & topic_name,
+    nav2_costmap_2d::Costmap2D * const costmap,
+    const unsigned int & downsampling_factor)
+  {
+    _topic_name = topic_name;
+    _costmap = costmap;
+    _downsampling_factor = downsampling_factor;
+    updateCostmapSize();
+
+    _downsampled_costmap = std::make_unique<nav2_costmap_2d::Costmap2D>
+      (_downsampled_size_x, _downsampled_size_y, _downsampled_resolution,
+       _costmap->getOriginX(), _costmap->getOriginY(), UNKNOWN);
+
+    _downsampled_costmap_pub = std::make_unique<nav2_costmap_2d::Costmap2DPublisher>(
+      _node, _downsampled_costmap.get(), global_frame, _topic_name, false);
+  }
+
+  /**
+   * @brief Activate the publisher of the downsampled costmap
+   */
+  void activatePublisher() {
+    _downsampled_costmap_pub->on_activate();
+  }
+
+  /**
+   * @brief Deactivate the publisher of the downsampled costmap
+   */
+  void deactivatePublisher() {
+    _downsampled_costmap_pub->on_deactivate();
+  }
+
+  /**
+   * @brief Downsample the given costmap by the downsampling factor, and publish the downsampled costmap
+   * @param downsampling_factor Multiplier for the costmap resolution
+   * @return A ptr to the downsampled costmap
+   */
+  nav2_costmap_2d::Costmap2D * downsample(const unsigned int & downsampling_factor)
+  {
+    _downsampling_factor = downsampling_factor;
+    updateCostmapSize();
+
+    // Adjust costmap size if needed
+    if (_downsampled_costmap->getSizeInCellsX() != _downsampled_size_x ||
+      _downsampled_costmap->getSizeInCellsY() != _downsampled_size_y ||
+      _downsampled_costmap->getResolution() != _downsampled_resolution) {
+      resizeCostmap();
+    }
+
+    // Assign costs
+    for (int i = 0; i < _downsampled_size_x; ++i) {
+      for (int j = 0; j < _downsampled_size_y; ++j) {
+        setCostOfCell(i, j);
+      }
+    }
+
+    if (_node->count_subscribers(_topic_name) > 0) {
+      _downsampled_costmap_pub->publishCostmap();
+    }
+
+    return _downsampled_costmap.get();
+  }
+
+private:
+  /**
+   * Update the sizes X-Y of the costmap and its downsampled version
+   */
+  void updateCostmapSize()
+  {
+    _size_x = _costmap->getSizeInCellsX();
+    _size_y = _costmap->getSizeInCellsY();
+    _downsampled_size_x = ceil(static_cast<float>(_size_x) / _downsampling_factor);
+    _downsampled_size_y = ceil(static_cast<float>(_size_y) / _downsampling_factor);
+    _downsampled_resolution = _downsampling_factor * _costmap->getResolution();
+  }
+
+  /**
+   * Resize the downsampled costmap. Used in case the costmap changes and we need to update the downsampled version
+   */
+  void resizeCostmap()
+  {
+    _downsampled_costmap->resizeMap(
+      _downsampled_size_x,
+      _downsampled_size_y,
+      _downsampled_resolution,
+      _costmap->getOriginX(),
+      _costmap->getOriginY());
+  }
+
+  /**
+   * @brief Explore all subcells of the original costmap and assign the max cost to the new (downsampled) cell
+   * @param new_mx The X-coordinate of the cell in the new costmap
+   * @param new_my The Y-coordinate of the cell in the new costmap
+   */
+  void setCostOfCell(
+    const unsigned int & new_mx,
+    const unsigned int & new_my)
+  {
+    unsigned int mx, my;
+    unsigned char cost = 0;
+    unsigned int x_offset = new_mx * _downsampling_factor;
+    unsigned int y_offset = new_my * _downsampling_factor;
+
+    for (int i = 0; i < _downsampling_factor; ++i) {
+      mx = x_offset + i;
+      if (mx >= _size_x) {
+        continue;
+      }
+      for (int j = 0; j < _downsampling_factor; ++j) {
+        my = y_offset + j;
+        if (my >= _size_y) {
+          continue;
+        }
+        cost = std::max(cost, _costmap->getCost(mx, my));
+      }
+    }
+
+    _downsampled_costmap->setCost(new_mx, new_my, cost);
+  }
+
+  unsigned int _size_x;
+  unsigned int _size_y;
+  unsigned int _downsampled_size_x;
+  unsigned int _downsampled_size_y;
+  unsigned int _downsampling_factor;
+  float _downsampled_resolution;
+  std::string _topic_name;
+  nav2_util::LifecycleNode::SharedPtr _node;
+  nav2_costmap_2d::Costmap2D * _costmap;
+  std::unique_ptr<nav2_costmap_2d::Costmap2D> _downsampled_costmap;
+  std::unique_ptr<nav2_costmap_2d::Costmap2DPublisher> _downsampled_costmap_pub;
+};
+
+}  // namespace smac_planner
+
+#endif // SMAC_PLANNER__DOWNSAMPLER_HPP_

smac_planner/include/smac_planner/smac_planner.hpp

@@ -22,6 +22,8 @@
 #include "smac_planner/a_star.hpp"
 #include "smac_planner/smoother.hpp"
 #include "smac_planner/upsampler.hpp"
+#include "smac_planner/costmap_downsampler.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
 #include "nav2_core/global_planner.hpp"
 #include "nav_msgs/msg/path.hpp"
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
@@ -92,14 +94,17 @@ class SmacPlanner : public nav2_core::GlobalPlanner
   std::unique_ptr<AStarAlgorithm<Node>> _a_star;
   std::unique_ptr<Smoother> _smoother;
   std::unique_ptr<Upsampler> _upsampler;
+  std::unique_ptr<CostmapDownsampler> _costmap_downsampler;
   nav2_util::LifecycleNode::SharedPtr _node;
   nav2_costmap_2d::Costmap2D * _costmap;
   std::string _global_frame, _name;
   float _tolerance;
+  int _downsampling_factor;
+  bool _downsample_costmap;
   rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr _raw_plan_publisher;
-  rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr smoother_debug1_pub_;
-  rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr smoother_debug2_pub_;
-  rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr smoother_debug3_pub_;
+  rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr _smoother_debug1_pub;
+  rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr _smoother_debug2_pub;
+  rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr _smoother_debug3_pub;
   SmootherParams _smoother_params;
   OptimizerParams _optimizer_params;
   int _upsampling_ratio;