add dynamic costmap layer (#13)

* add dynamic costmap layer

* remove backward ros

navyu_costmap_2d/config/navyu_costmap_2d_params.yaml

@@ -3,9 +3,12 @@ global_costmap_node:
     update_frequency: 1.0
     base_frame_id: base_link
     map_frame_id: map
-    plugins: [static_layer, inflation_layer]
+    plugins: [static_layer, dynamic_layer, inflation_layer]
     inflation_layer:
       inflation_radius: 0.55
       robot_radius: 0.22
     static_layer:
       map_topic: map
+    dynamic_layer:
+      scan_topic: scan
+      global_frame: map

navyu_costmap_2d/include/navyu_costmap_2d/navyu_costmap_2d.hpp

@@ -22,9 +22,7 @@
 #include <nav_msgs/msg/occupancy_grid.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 
-#include <tf2/transform_datatypes.h>
 #include <tf2_ros/buffer.h>
-#include <tf2_ros/transform_broadcaster.h>
 #include <tf2_ros/transform_listener.h>
 
 class NavyuCostmap2D : public rclcpp::Node
@@ -35,21 +33,16 @@ class NavyuCostmap2D : public rclcpp::Node
 
   void update();
 
-private:
-  geometry_msgs::msg::TransformStamped get_transform(
-    const std::string target_frame, const std::string source_frame);
-
 private:
   rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr costmap_publisher_;
   rclcpp::TimerBase::SharedPtr timer_;
 
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> listener_;
+
   std::vector<std::string> plugins_;
   std::map<std::string, std::shared_ptr<Layer>> layer_function_;
 
-  tf2_ros::Buffer tf_buffer_{get_clock()};
-  tf2_ros::TransformListener tf_listener_{tf_buffer_};
-  std::shared_ptr<tf2_ros::TransformBroadcaster> broadcaster_;
-
   double update_frequency_;
   std::string base_frame_id_;
   std::string map_frame_id_;

navyu_costmap_2d/include/navyu_costmap_2d/plugins/dynamic_layer.hpp

@@ -0,0 +1,131 @@
+// Copyright 2024 RyuYamamoto.
+//
+// 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
+
+#ifndef NAVYU_COSTMAP_2D__PLUGINS__DYNAMIC_LAYER_HPP_
+#define NAVYU_COSTMAP_2D__PLUGINS__DYNAMIC_LAYER_HPP_
+
+#include "navyu_costmap_2d/plugins/layer.hpp"
+
+#include <laser_geometry/laser_geometry.hpp>
+#include <pcl/impl/point_types.hpp>
+#include <pcl_ros/transforms.hpp>
+#include <tf2_eigen/tf2_eigen.hpp>
+
+#include <geometry_msgs/msg/detail/pose_with_covariance_stamped__struct.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <sensor_msgs/msg/detail/point_cloud2__struct.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+#include <pcl/common/transforms.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <tf2/transform_datatypes.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+class DynamicLayer : public Layer
+{
+public:
+  explicit DynamicLayer(rclcpp::Node * node) : Layer(node) {}
+
+  void configure() override
+  {
+    global_frame_ = node_->declare_parameter<std::string>("dynamic_layer.global_frame");
+    const std::string scan_topic =
+      node_->declare_parameter<std::string>("dynamic_layer.scan_topic");
+    scan_subscriber_ = node_->create_subscription<sensor_msgs::msg::LaserScan>(
+      scan_topic, 10, std::bind(&DynamicLayer::callback, this, std::placeholders::_1));
+  }
+
+  void pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
+  {
+    pose_ = msg;
+  }
+
+  void callback(const sensor_msgs::msg::LaserScan::SharedPtr msg) { scan_ = msg; }
+
+  void update(nav_msgs::msg::OccupancyGrid & master_costmap) override
+  {
+    if (scan_ == nullptr) {
+      RCLCPP_ERROR_STREAM(node_->get_logger(), "scan is empty.");
+      return;
+    }
+    if (master_costmap.data.empty()) {
+      RCLCPP_ERROR_STREAM(node_->get_logger(), "master costmap is empty.");
+      return;
+    }
+
+    sensor_msgs::msg::PointCloud2 cloud_msg, transform_cloud_msg;
+    projection_.projectLaser(*scan_, cloud_msg);
+
+    geometry_msgs::msg::TransformStamped map_to_sensor_frame;
+    if (!get_transform(global_frame_, scan_->header.frame_id, map_to_sensor_frame)) {
+      RCLCPP_ERROR_STREAM(node_->get_logger(), "Can not get frame.");
+      return;
+    }
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr laser_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr transform_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::fromROSMsg(cloud_msg, *laser_cloud);
+
+    const Eigen::Affine3d affine = tf2::transformToEigen(map_to_sensor_frame);
+    const Eigen::Matrix4f matrix = affine.matrix().cast<float>();
+
+    pcl::transformPointCloud(*laser_cloud, *transform_cloud, matrix);
+
+    const double resolution = master_costmap.info.resolution;
+    const int width = master_costmap.info.width;
+    const int height = master_costmap.info.height;
+    const double origin_x = master_costmap.info.origin.position.x;
+    const double origin_y = master_costmap.info.origin.position.y;
+
+    for (auto cloud : transform_cloud->points) {
+      const int ix = static_cast<int>((cloud.x - origin_x) / resolution);
+      const int iy = static_cast<int>((cloud.y - origin_y) / resolution);
+
+      int index = ix + width * iy;
+      master_costmap.data[index] = LETHAL_COST;
+    }
+  }
+
+  bool get_transform(
+    const std::string target_frame, const std::string source_frame,
+    geometry_msgs::msg::TransformStamped & frame)
+  {
+    try {
+      frame = tf_buffer_->lookupTransform(
+        target_frame, source_frame, tf2::TimePointZero, tf2::durationFromSec(0.5));
+    } catch (tf2::TransformException & ex) {
+      RCLCPP_ERROR(node_->get_logger(), "%s", ex.what());
+      return false;
+    }
+    return true;
+  }
+
+private:
+  rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_subscriber_;
+
+  sensor_msgs::msg::LaserScan::SharedPtr scan_;
+  geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr pose_;
+
+  laser_geometry::LaserProjection projection_;
+
+  std::string global_frame_;
+};
+
+#endif

navyu_costmap_2d/include/navyu_costmap_2d/plugins/inflation_layer.hpp

@@ -20,41 +20,41 @@
 class InflationLayer : public Layer
 {
 public:
-  explicit InflationLayer(rclcpp::Node * node) : Layer(node) { node_ = node; }
+  explicit InflationLayer(rclcpp::Node * node) : Layer(node) {}
 
-  void initialize() override
+  void configure() override
   {
     node_->declare_parameter<double>("inflation_layer.inflation_radius");
     node_->get_parameter("inflation_layer.inflation_radius", inflation_radius_);
     node_->declare_parameter<double>("inflation_layer.robot_radius");
     node_->get_parameter("inflation_layer.robot_radius", robot_radius_);
   }
 
-  void update(nav_msgs::msg::OccupancyGrid::SharedPtr & master_costmap) override
+  void update(nav_msgs::msg::OccupancyGrid & master_costmap) override
   {
-    if (master_costmap == nullptr) {
-      RCLCPP_ERROR_STREAM(node_->get_logger(), "costmap is nullptr.");
+    if (master_costmap.data.empty()) {
+      RCLCPP_ERROR_STREAM(node_->get_logger(), "costmap is empty.");
       return;
     }
 
-    const int size_x = master_costmap->info.width;
-    const int size_y = master_costmap->info.height;
+    const int size_x = master_costmap.info.width;
+    const int size_y = master_costmap.info.height;
 
     for (int map_y = 0; map_y < size_y; map_y++) {
       for (int map_x = 0; map_x < size_x; map_x++) {
         int map_index = map_x + size_x * map_y;
 
-        if (master_costmap->data[map_index] == LETHAL_COST) {
+        if (master_costmap.data[map_index] == LETHAL_COST) {
           expand_cost(master_costmap, map_x, map_y);
         }
       }
     }
   }
 
-  void expand_cost(nav_msgs::msg::OccupancyGrid::SharedPtr & costmap, const int mx, const int my)
+  void expand_cost(nav_msgs::msg::OccupancyGrid & costmap, const int mx, const int my)
   {
-    const int size_x = costmap->info.width;
-    const double resolution = costmap->info.resolution;
+    const int size_x = costmap.info.width;
+    const double resolution = costmap.info.resolution;
     const int cell_inflation_radius = std::ceil(inflation_radius_ / resolution);
 
     const int min_x = mx - cell_inflation_radius;
@@ -66,24 +66,22 @@ class InflationLayer : public Layer
       for (int x = min_x; x < max_x; x++) {
         const double distance = std::hypot(x - mx, y - my);
         const int index = x + size_x * y;
-        const auto old_cost = costmap->data[index];
+        const auto old_cost = costmap.data[index];
 
         if (distance * resolution < robot_radius_) {
-          costmap->data[index] = std::max(INSCRIBED_COST, old_cost);
+          costmap.data[index] = std::max(INSCRIBED_COST, old_cost);
         } else {
           if (cell_inflation_radius < distance) continue;
-          if (costmap->data[index] == -1) continue;
+          if (costmap.data[index] == -1) continue;
           const int8_t new_cost =
             std::exp(-1 * 3.0 * (distance * resolution - robot_radius_)) * INSCRIBED_COST - 1;
-          costmap->data[index] = std::max(new_cost, old_cost);
+          costmap.data[index] = std::max(new_cost, old_cost);
         }
       }
     }
   }
 
 private:
-  rclcpp::Node * node_;
-
   double inflation_radius_;
   double robot_radius_;
 };

navyu_costmap_2d/include/navyu_costmap_2d/plugins/layer.hpp

@@ -19,6 +19,9 @@
 
 #include <nav_msgs/msg/occupancy_grid.hpp>
 
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
 static constexpr int8_t LETHAL_COST = 100;
 static constexpr int8_t INSCRIBED_COST = 99;
 
@@ -27,11 +30,18 @@ class Layer
 public:
   explicit Layer(rclcpp::Node * node) { node_ = node; }
   virtual ~Layer() {}
-  virtual void initialize() = 0;
-  virtual void update(nav_msgs::msg::OccupancyGrid::SharedPtr & master_costmap) = 0;
+  void initialize(tf2_ros::Buffer * tf_buffer, tf2_ros::TransformListener * listener)
+  {
+    tf_buffer_ = tf_buffer;
+    listener_ = listener;
+  };
+  virtual void configure() = 0;
+  virtual void update(nav_msgs::msg::OccupancyGrid & master_costmap) = 0;
 
-private:
+protected:
   rclcpp::Node * node_;
+  tf2_ros::Buffer * tf_buffer_;
+  tf2_ros::TransformListener * listener_;
 };
 
 #endif  // NAVYU_COSTMAP_2D__PLUGINS__LAYER_HPP_

navyu_costmap_2d/include/navyu_costmap_2d/plugins/static_layer.hpp

@@ -22,9 +22,9 @@
 class StaticLayer : public Layer
 {
 public:
-  explicit StaticLayer(rclcpp::Node * node) : Layer(node) { node_ = node; }
+  explicit StaticLayer(rclcpp::Node * node) : Layer(node) {}
 
-  void initialize() override
+  void configure() override
   {
     rclcpp::QoS qos(10);
     qos.transient_local();
@@ -36,19 +36,14 @@ class StaticLayer : public Layer
       map_topic, qos, std::bind(&StaticLayer::callback_map, this, std::placeholders::_1));
   }
 
-  void callback_map(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { costmap_ = msg; }
+  void callback_map(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { map_ = *msg; }
 
-  void update(nav_msgs::msg::OccupancyGrid::SharedPtr & master_costmap) override
-  {
-    master_costmap = costmap_;
-  }
+  void update(nav_msgs::msg::OccupancyGrid & master_costmap) override { master_costmap = map_; }
 
 private:
-  rclcpp::Node * node_;
-
   rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr map_subscriber_;
 
-  nav_msgs::msg::OccupancyGrid::SharedPtr costmap_;
+  nav_msgs::msg::OccupancyGrid map_;
 };
 
 #endif  // NAVYU_COSTMAP_2D__PLUGINS__STATIC_LAYER_HPP_

navyu_costmap_2d/package.xml

@@ -13,10 +13,12 @@
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>sensor_msgs</depend>
+  <depend>laser_geometry</depend>
   <depend>tf2</depend>
   <depend>tf2_eigen</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
+  <depend>pcl_ros</depend>
 
   <exec_depend>launch_ros</exec_depend>
   <test_depend>ament_lint_auto</test_depend>