Apply global obstacle handling code into nav2_costmap_2d::global_voxe…

…l_layer also

nav2_bringup/params/acloi00_nav2_params.yaml

@@ -196,34 +196,14 @@ local_costmap:
       height: 4
       resolution: 0.1
       robot_radius: 0.34
-      plugins: ["global_obstacle_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"
-      #   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"
+      global_voxel_layer:
+        plugin: "nav2_costmap_2d::GlobalVoxelLayer"
         enabled: True
         publish_voxel_map: True
         origin_z: 0.0
@@ -233,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

nav2_costmap_2d/include/nav2_costmap_2d/global_obstacle_layer.hpp

@@ -61,6 +61,7 @@ class GlobalObstacleLayer : public 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
@@ -72,10 +73,10 @@ class GlobalObstacleLayer : public ObstacleLayer
    * @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);
+    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
@@ -107,7 +108,6 @@ class GlobalObstacleLayer : public ObstacleLayer
     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_;
 

nav2_costmap_2d/include/nav2_costmap_2d/global_voxel_layer.hpp

@@ -55,6 +55,10 @@ class GlobalVoxelLayer : public VoxelLayer
   GlobalVoxelLayer()
   : VoxelLayer()
   {};
+  /**
+   * @brief Initialization process of layer on startup
+   */
+  virtual void onInitialize();
 
   /**
    * @brief Update the bounds of the master costmap by this layer's update dimensions
@@ -71,8 +75,56 @@ class GlobalVoxelLayer : public VoxelLayer
     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

nav2_costmap_2d/plugins/global_obstacle_layer.cpp

@@ -54,7 +54,7 @@ void GlobalObstacleLayer::onInitialize()
 {
   ObstacleLayer::onInitialize();
 
-  RCLCPP_INFO(logger_, "[GlobalObstacleLayer] onInitialize with Obstacle Index Publisher");
+  RCLCPP_INFO(logger_, "[GlobalObstacleLayer] onInitialize with Obstacle Publisher");
 
   auto node = node_.lock();
   if (!node) {