Feat/local costmap (#23)

* add local costmap

* fix pre-commit

* remove backward ros

navyu_costmap_2d/config/navyu_costmap_2d_params.yaml

@@ -1,8 +1,7 @@
 global_costmap_node:
   ros__parameters:
     update_frequency: 1.0
-    base_frame_id: base_link
-    map_frame_id: map
+    global_frame_id: map
     plugins: [static_layer, dynamic_layer, inflation_layer]
     inflation_layer:
       inflation_radius: 0.55
@@ -14,3 +13,20 @@ global_costmap_node:
       global_frame: map
       min_laser_range: 0.0
       max_laser_range: 5.0
+
+local_costmap_node:
+  ros__parameters:
+    update_frequency: 5.0
+    global_frame_id: base_link
+    width: 3
+    height: 3
+    resolution: 0.05
+    plugins: [dynamic_layer, inflation_layer]
+    inflation_layer:
+      inflation_radius: 0.55
+      robot_radius: 0.22
+    dynamic_layer:
+      scan_topic: scan
+      global_frame: base_link
+      min_laser_range: 0.0
+      max_laser_range: 5.0

navyu_costmap_2d/include/navyu_costmap_2d/navyu_costmap_2d.hpp

@@ -40,9 +40,16 @@ class NavyuCostmap2D : public rclcpp::Node
   std::vector<std::string> plugins_;
   std::map<std::string, std::shared_ptr<Layer>> layer_function_;
 
+  nav_msgs::msg::OccupancyGrid master_costmap_;
+
   double update_frequency_;
-  std::string base_frame_id_;
-  std::string map_frame_id_;
+  std::string global_frame_id_;
+
+  double resolution_;
+  double origin_x_;
+  double origin_y_;
+  int size_x_;
+  int size_y_;
 };
 
 #endif  // NAVYU_COSTMAP_2D__NAVYU_COSTMAP_2D_HPP_

navyu_costmap_2d/include/navyu_costmap_2d/plugins/dynamic_layer.hpp

@@ -56,15 +56,11 @@ class DynamicLayer : public Layer
 
   void callback(const sensor_msgs::msg::LaserScan::SharedPtr msg) { scan_ = msg; }
 
-  void update(nav_msgs::msg::OccupancyGrid & master_costmap) override
+  bool 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;
+      return false;
     }
 
     // scan range filter
@@ -81,33 +77,42 @@ class DynamicLayer : public Layer
     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;
+      return false;
     }
 
     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);
 
     // transform 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);
+    if (scan_->header.frame_id != global_frame_) {
+      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);
+    } else {
+      transform_cloud = laser_cloud;
+    }
 
     // get costmap info
     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;
+    master_costmap.data.resize(width * height);
 
     // calculation grid index
     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;
+      if (0 <= ix and ix < width and 0 <= iy and iy < height) {
+        int index = ix + width * iy;
+        master_costmap.data[index] = LETHAL_COST;
+      }
     }
+
+    return true;
   }
 
   bool get_transform(

navyu_costmap_2d/include/navyu_costmap_2d/plugins/inflation_layer.hpp

@@ -30,11 +30,11 @@ class InflationLayer : public Layer
     node_->get_parameter("inflation_layer.robot_radius", robot_radius_);
   }
 
-  void update(nav_msgs::msg::OccupancyGrid & master_costmap) override
+  bool update(nav_msgs::msg::OccupancyGrid & master_costmap) override
   {
     if (master_costmap.data.empty()) {
       RCLCPP_ERROR_STREAM(node_->get_logger(), "costmap is empty.");
-      return;
+      return false;
     }
 
     const int size_x = master_costmap.info.width;
@@ -49,11 +49,14 @@ class InflationLayer : public Layer
         }
       }
     }
+
+    return true;
   }
 
   void expand_cost(nav_msgs::msg::OccupancyGrid & costmap, const int mx, const int my)
   {
     const int size_x = costmap.info.width;
+    const int size_y = costmap.info.height;
     const double resolution = costmap.info.resolution;
     const int cell_inflation_radius = std::ceil(inflation_radius_ / resolution);
 
@@ -64,18 +67,20 @@ class InflationLayer : public Layer
 
     for (int y = min_y; y < max_y; y++) {
       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];
+        if (0 <= x and x < size_x and 0 <= y and y < size_y) {
+          const double distance = std::hypot(x - mx, y - my);
+          const int index = x + size_x * y;
+          const auto old_cost = costmap.data[index];
 
-        if (distance * resolution < robot_radius_) {
-          costmap.data[index] = std::max(INSCRIBED_COST, old_cost);
-        } else {
-          if (cell_inflation_radius < distance) 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);
+          if (distance * resolution < robot_radius_) {
+            costmap.data[index] = std::max(INSCRIBED_COST, old_cost);
+          } else {
+            if (cell_inflation_radius < distance) 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);
+          }
         }
       }
     }

navyu_costmap_2d/include/navyu_costmap_2d/plugins/layer.hpp

@@ -36,7 +36,7 @@ class Layer
     listener_ = listener;
   };
   virtual void configure() = 0;
-  virtual void update(nav_msgs::msg::OccupancyGrid & master_costmap) = 0;
+  virtual bool update(nav_msgs::msg::OccupancyGrid & master_costmap) = 0;
 
 protected:
   rclcpp::Node * node_;

navyu_costmap_2d/include/navyu_costmap_2d/plugins/static_layer.hpp

@@ -36,7 +36,15 @@ class StaticLayer : public Layer
 
   void callback_map(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { map_ = *msg; }
 
-  void update(nav_msgs::msg::OccupancyGrid & master_costmap) override { master_costmap = map_; }
+  bool update(nav_msgs::msg::OccupancyGrid & master_costmap) override
+  {
+    if (map_.data.empty()) {
+      RCLCPP_ERROR_STREAM(node_->get_logger(), "map is empty.");
+      return false;
+    }
+    master_costmap = map_;
+    return true;
+  }
 
 private:
   rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr map_subscriber_;

navyu_costmap_2d/launch/navyu_costmap_2d.launch.py

@@ -38,25 +38,16 @@ def generate_launch_description():
                 executable="navyu_costmap_2d_node",
                 name="global_costmap_node",
                 output="screen",
+                remappings=[("/costmap", "/global_costmap")],
                 parameters=[costmap_map_config_path, {"use_sim_time": use_sim_time}],
             ),
             Node(
-                package="nav2_map_server",
-                executable="map_server",
-                name="map_server",
-                parameters=[{"yaml_filename": map_path}, {"frame_id": "map"}],
-            ),
-            Node(
-                package="nav2_lifecycle_manager",
-                executable="lifecycle_manager",
-                name="map_server_lifecycle_manager",
+                package="navyu_costmap_2d",
+                executable="navyu_costmap_2d_node",
+                name="local_costmap_node",
                 output="screen",
-                emulate_tty=True,
-                parameters=[
-                    {"use_sim_time": True},
-                    {"autostart": True},
-                    {"node_names": ["map_server"]},
-                ],
+                remappings=[("/costmap", "/local_costmap")],
+                parameters=[costmap_map_config_path, {"use_sim_time": use_sim_time}],
             ),
         ]
     )

navyu_costmap_2d/src/navyu_costmap_2d.cpp

@@ -25,8 +25,21 @@ NavyuCostmap2D::NavyuCostmap2D(const rclcpp::NodeOptions & node_options)
 : Node("navyu_costmap_2d", node_options)
 {
   update_frequency_ = declare_parameter<double>("update_frequency");
-  base_frame_id_ = declare_parameter<std::string>("base_frame_id");
-  map_frame_id_ = declare_parameter<std::string>("map_frame_id");
+  global_frame_id_ = declare_parameter<std::string>("global_frame_id");
+
+  resolution_ = declare_parameter<double>("resolution", 0.05);
+  origin_x_ = declare_parameter<double>("origin_x", 0.0);
+  origin_y_ = declare_parameter<double>("origin_y", 0.0);
+  size_x_ = declare_parameter<int>("width", 5);
+  size_y_ = declare_parameter<int>("height", 5);
+
+  master_costmap_.header.frame_id = global_frame_id_;
+  master_costmap_.info.resolution = resolution_;
+  master_costmap_.info.width = static_cast<int32_t>(size_x_ / resolution_);
+  master_costmap_.info.height = static_cast<int32_t>(size_y_ / resolution_);
+  master_costmap_.info.origin.position.x = origin_x_ - static_cast<double>(size_x_) / 2.0;
+  master_costmap_.info.origin.position.y = origin_y_ - static_cast<double>(size_y_) / 2.0;
+  master_costmap_.data.resize(master_costmap_.info.width * master_costmap_.info.height);
 
   plugins_ = declare_parameter<std::vector<std::string>>("plugins");
 
@@ -61,22 +74,24 @@ NavyuCostmap2D::~NavyuCostmap2D()
 
 void NavyuCostmap2D::update()
 {
-  nav_msgs::msg::OccupancyGrid master_costmap;
+  master_costmap_.data.clear();
 
   // update costmap
   for (auto & plugin : plugins_) {
-    layer_function_[plugin]->update(master_costmap);
+    if (!layer_function_[plugin]->update(master_costmap_)) {
+      RCLCPP_ERROR_STREAM(get_logger(), "Can not update costmap. " << plugin.c_str());
+      return;
+    }
   }
 
-  if (master_costmap.data.empty()) {
+  if (master_costmap_.data.empty()) {
     RCLCPP_ERROR_STREAM(get_logger(), "master costmap is empty");
     return;
   }
 
-  master_costmap.header.stamp = now();
-  master_costmap.header.frame_id = map_frame_id_;
+  master_costmap_.header.stamp = now();
 
-  costmap_publisher_->publish(master_costmap);
+  costmap_publisher_->publish(master_costmap_);
 }
 
 #include "rclcpp_components/register_node_macro.hpp"

navyu_navigation/config/navyu_params.yaml

@@ -1,8 +1,7 @@
 navyu_global_costmap_node:
   ros__parameters:
     update_frequency: 1.0
-    base_frame_id: base_link
-    map_frame_id: map
+    global_frame_id: map
     plugins: [static_layer, dynamic_layer, inflation_layer]
     inflation_layer:
       inflation_radius: 0.55
@@ -15,6 +14,23 @@ navyu_global_costmap_node:
       min_laser_range: 0.0
       max_laser_range: 5.0
 
+navyu_local_costmap_node:
+  ros__parameters:
+    update_frequency: 1.0
+    global_frame_id: base_link
+    width: 5
+    height: 5
+    resolution: 0.05
+    plugins: [dynamic_layer, inflation_layer]
+    inflation_layer:
+      inflation_radius: 0.55
+      robot_radius: 0.22
+    dynamic_layer:
+      scan_topic: scan
+      global_frame: base_link
+      min_laser_range: 0.0
+      max_laser_range: 5.0
+
 navyu_global_planner_node:
   ros__parameters:
     map_frame: map

navyu_navigation/launch/navyu_bringup.launch.py

@@ -52,12 +52,21 @@ def generate_launch_description():
                 package="navyu_costmap_2d",
                 executable="navyu_costmap_2d_node",
                 name="navyu_global_costmap_node",
+                remappings=[("/costmap", "/global_costmap")],
+                parameters=[navyu_config_path, {"use_sim_time": use_sim_time}],
+            ),
+            Node(
+                package="navyu_costmap_2d",
+                executable="navyu_costmap_2d_node",
+                name="navyu_local_costmap_node",
+                remappings=[("/costmap", "/local_costmap")],
                 parameters=[navyu_config_path, {"use_sim_time": use_sim_time}],
             ),
             Node(
                 package="navyu_path_planner",
                 executable="navyu_global_planner_node",
                 name="navyu_global_planner_node",
+                remappings=[("/costmap", "/global_costmap")],
                 parameters=[navyu_config_path, {"use_sim_time": use_sim_time}],
             ),
             Node(