Humble cloi costmap

nav2_bringup/params/acloi00_nav2_params.yaml

@@ -196,15 +196,14 @@ local_costmap:
       height: 4
       resolution: 0.1
       robot_radius: 0.34
-      # plugins: ["voxel_layer", "inflation_layer"]
-      plugins: ["voxel_layer", "inflation_layer"]
+      plugins: ["global_voxel_layer", "inflation_layer"]
         #filters: ["keepout_filter"]
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 6.5
         inflation_radius: 1.75
-      voxel_layer:
-        plugin: "nav2_costmap_2d::VoxelLayer"
+      global_voxel_layer:
+        plugin: "nav2_costmap_2d::GlobalVoxelLayer"
         enabled: True
         publish_voxel_map: True
         origin_z: 0.0
@@ -214,7 +213,7 @@ local_costmap:
         mark_threshold: 0
         observation_sources: scan
         scan:
-          topic: /acloi00/scan
+          topic: /acloi01/scan
           max_obstacle_height: 5.0
           clearing: True
           marking: True
@@ -228,10 +227,6 @@ local_costmap:
           # init_scan_angle: False
       static_layer:
         map_subscribe_transient_local: True
-      global_obstacle_layer:
-        plugin: "nav2_global_obstacle_costmap_plugin::GlobalObstacleLayer"
-        enabled: True
-        topic: "/global_obstacle/obstacle_list"
       keepout_filter:
         plugin: "nav2_costmap_2d::KeepoutFilter"
         enabled: True
@@ -249,7 +244,6 @@ global_costmap:
       robot_radius: 0.34
       resolution: 0.1
       track_unknown_space: true
-        #plugins: ["static_layer", "obstacle_layer", "global_obstacle_layer", "inflation_layer"]
       plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
         #filters: ["keepout_filter"]
       obstacle_layer:

nav2_bringup/params/acloi01_nav2_params.yaml

@@ -196,13 +196,33 @@ local_costmap:
       height: 4
       resolution: 0.1
       robot_radius: 0.34
-      plugins: ["voxel_layer", "inflation_layer"]
+      plugins: ["global_obstacle_layer", "inflation_layer"]
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 6.5
         inflation_radius: 1.75
-      voxel_layer:
-        plugin: "nav2_costmap_2d::VoxelLayer"
+      # voxel_layer:
+      #   plugin: "nav2_costmap_2d::VoxelLayer"
+      #   enabled: True
+      #   publish_voxel_map: True
+      #   origin_z: 0.0
+      #   z_resolution: 0.05
+      #   z_voxels: 16
+      #   max_obstacle_height: 2.0
+      #   mark_threshold: 0
+      #   observation_sources: scan
+      #   scan:
+      #     topic: /acloi01/scan
+      #     max_obstacle_height: 5.0
+      #     clearing: True
+      #     marking: True
+      #     data_type: "LaserScan"
+      #     raytrace_max_range: 15.0
+      #     raytrace_min_range: 0.0
+      #     obstacle_max_range: 5.0
+      #     obstacle_min_range: 0.0
+      global_obstacle_layer:
+        plugin: "nav2_costmap_2d::GlobalObstacleLayer"
         enabled: True
         publish_voxel_map: True
         origin_z: 0.0
@@ -221,6 +241,9 @@ local_costmap:
           raytrace_min_range: 0.0
           obstacle_max_range: 5.0
           obstacle_min_range: 0.0
+          scan_link_offset: 2.0
+          publish_cycle: 1.0
+          # init_scan_angle: False
       static_layer:
         map_subscribe_transient_local: True
       always_send_full_costmap: True

nav2_bringup/params/acloi02_nav2_params.yaml

@@ -196,13 +196,13 @@ local_costmap:
       height: 4
       resolution: 0.1
       robot_radius: 0.34
-      plugins: ["voxel_layer", "inflation_layer"]
+      plugins: ["global_voxel_layer", "inflation_layer"]
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 6.5
         inflation_radius: 1.75
-      voxel_layer:
-        plugin: "nav2_costmap_2d::VoxelLayer"
+      global_voxel_layer:
+        plugin: "nav2_costmap_2d::GlobalVoxelLayer"
         enabled: True
         publish_voxel_map: True
         origin_z: 0.0
@@ -212,7 +212,7 @@ local_costmap:
         mark_threshold: 0
         observation_sources: scan
         scan:
-          topic: /acloi02/scan
+          topic: /acloi01/scan
           max_obstacle_height: 5.0
           clearing: True
           marking: True
@@ -221,6 +221,9 @@ local_costmap:
           raytrace_min_range: 0.0
           obstacle_max_range: 5.0
           obstacle_min_range: 0.0
+          scan_link_offset: 2.0
+          publish_cycle: 1.0
+          # init_scan_angle: False
       static_layer:
         map_subscribe_transient_local: True
       always_send_full_costmap: True

nav2_costmap_2d/CMakeLists.txt

@@ -84,8 +84,10 @@ add_library(layers SHARED
   plugins/inflation_layer.cpp
   plugins/static_layer.cpp
   plugins/obstacle_layer.cpp
+  plugins/global_obstacle_layer.cpp
   src/observation_buffer.cpp
   plugins/voxel_layer.cpp
+  plugins/global_voxel_layer.cpp
   plugins/range_sensor_layer.cpp
 )
 ament_target_dependencies(layers

nav2_costmap_2d/costmap_plugins.xml

@@ -6,12 +6,18 @@
     <class type="nav2_costmap_2d::ObstacleLayer"   base_class_type="nav2_costmap_2d::Layer">
       <description>Listens to laser scan and point cloud messages and marks and clears grid cells.</description>
     </class>
+    <class type="nav2_costmap_2d::GlobalObstacleLayer"   base_class_type="nav2_costmap_2d::Layer">
+      <description>Listens to laser scan and point cloud messages and marks and clears grid cells.</description>
+    </class>
     <class type="nav2_costmap_2d::StaticLayer"     base_class_type="nav2_costmap_2d::Layer">
       <description>Listens to OccupancyGrid messages and copies them in, like from map_server.</description>
     </class>
     <class type="nav2_costmap_2d::VoxelLayer"     base_class_type="nav2_costmap_2d::Layer">
       <description>Similar to obstacle costmap, but uses 3D voxel grid to store data.</description>
     </class>
+    <class type="nav2_costmap_2d::GlobalVoxelLayer" base_class_type="nav2_costmap_2d::Layer">
+      <description>Similar to obstacle costmap, but uses 3D voxel grid to store data.</description>
+    </class>
     <class type="nav2_costmap_2d::RangeSensorLayer" base_class_type="nav2_costmap_2d::Layer">
       <description>A range-sensor (sonar, IR) based obstacle layer for costmap_2d</description>
     </class>

nav2_costmap_2d/include/nav2_costmap_2d/global_obstacle_layer.hpp

@@ -0,0 +1,134 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, 2013, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Eitan Marder-Eppstein
+ *         David V. Lu!!
+ *********************************************************************/
+#ifndef NAV2_COSTMAP_2D__GLOBAL_OBSTACLE_LAYER_HPP_
+#define NAV2_COSTMAP_2D__GLOBAL_OBSTACLE_LAYER_HPP_
+
+#include "nav2_costmap_2d/obstacle_layer.hpp"
+
+namespace nav2_costmap_2d
+{
+
+/**
+ * @class ObstacleLayer
+ * @brief Takes in laser and pointcloud data to populate into 2D costmap
+ */
+class GlobalObstacleLayer : public ObstacleLayer
+{
+public:
+  /**
+   * @brief A constructor
+   */
+  GlobalObstacleLayer()
+    : ObstacleLayer()
+  {}
+
+  /**
+   * @brief Initialization process of layer on startup
+   */
+  virtual void onInitialize();
+
+  /**
+   * @brief Update the bounds of the master costmap by this layer's update dimensions
+   * @param robot_x X pose of robot
+   * @param robot_y Y pose of robot
+   * @param robot_yaw Robot orientation
+   * @param min_x X min map coord of the window to update
+   * @param min_y Y min map coord of the window to update
+   * @param max_x X max map coord of the window to update
+   * @param max_y Y max map coord of the window to update
+   */
+  virtual void updateBounds(
+    double robot_x, double robot_y, double robot_yaw,
+    double * min_x, double * min_y,
+    double * max_x, double * max_y);
+
+  /**
+   * @brief Update the costs in the master costmap in the window
+   * @param master_grid The master costmap grid to update
+   * @param min_x X min map coord of the window to update
+   * @param min_y Y min map coord of the window to update
+   * @param max_x X max map coord of the window to update
+   * @param max_y Y max map coord of the window to update
+   */
+  virtual void updateCosts(
+    nav2_costmap_2d::Costmap2D & master_grid,
+    int min_i, int min_j, int max_i, int max_j);
+
+  /**
+   * @brief  A callback to handle buffering LaserScan messages
+   * @param message The message returned from a message notifier
+   * @param buffer A pointer to the observation buffer to update
+   */
+  void laserScanCallback(
+    sensor_msgs::msg::LaserScan::ConstSharedPtr message,
+    const std::shared_ptr<nav2_costmap_2d::ObservationBuffer> & buffer);
+
+  /**
+   * @brief A callback to handle buffering LaserScan messages which need filtering to turn Inf values into range_max.
+   * @param message The message returned from a message notifier
+   * @param buffer A pointer to the observation buffer to update
+   */
+  void laserScanValidInfCallback(
+    sensor_msgs::msg::LaserScan::ConstSharedPtr message,
+    const std::shared_ptr<nav2_costmap_2d::ObservationBuffer> & buffer);
+
+protected:
+  rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr obstacle_grid_pub_;
+  std::unique_ptr<nav_msgs::msg::OccupancyGrid> grid_;
+
+  bool is_init_scan_angle_;
+  bool use_init_scan_angle_;
+  double scan_start_angle_;
+  double scan_end_angle_;
+  float scan_link_offset_;
+  double current_robot_yaw_;
+  static char * cost_translation_table_;
+  rclcpp::Time last_publish_{0, 0, RCL_ROS_TIME};
+  rclcpp::Duration publish_cycle_{1, 0};
+
+protected:
+  void updateRobotYaw(const double robot_yaw);
+
+private:
+  void initializeScanAngle(sensor_msgs::msg::LaserScan::ConstSharedPtr message);
+  void prepareGrid(nav2_costmap_2d::Costmap2D costmap);
+};
+
+}  // namespace nav2_costmap_2d
+
+#endif  // NAV2_COSTMAP_2D__OBSTACLE_LAYER_HPP_