3732 costmap raw update msgs

nav2_constrained_smoother/src/constrained_smoother.cpp

@@ -132,6 +132,7 @@ bool ConstrainedSmoother::smooth(nav_msgs::msg::Path & path, const rclcpp::Durat
 
   // Smooth plan
   auto costmap = costmap_sub_->getCostmap();
+  std::lock_guard<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getMutex()));
   if (!smoother_->smooth(path_world, start_dir, end_dir, costmap.get(), smoother_params_)) {
     RCLCPP_WARN(
       logger_,

nav2_constrained_smoother/test/test_constrained_smoother.cpp

@@ -82,6 +82,12 @@ class DummyCostmapSubscriber : public nav2_costmap_2d::CostmapSubscriber
   void setCostmap(nav2_msgs::msg::Costmap::SharedPtr msg)
   {
     costmap_msg_ = msg;
+    costmap_ = std::make_shared<nav2_costmap_2d::Costmap2D>(
+      msg->metadata.size_x, msg->metadata.size_y,
+      msg->metadata.resolution, msg->metadata.origin.position.x,
+      msg->metadata.origin.position.y);
+
+    processCurrentCostmapMsg();
     costmap_received_ = true;
   }
 };

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp

@@ -48,6 +48,7 @@
 #include "nav_msgs/msg/occupancy_grid.hpp"
 #include "map_msgs/msg/occupancy_grid_update.hpp"
 #include "nav2_msgs/msg/costmap.hpp"
+#include "nav2_msgs/msg/costmap_update.hpp"
 #include "nav2_msgs/srv/get_costmap.hpp"
 #include "tf2/transform_datatypes.h"
 #include "nav2_util/lifecycle_node.hpp"
@@ -91,6 +92,7 @@ class Costmap2DPublisher
     costmap_pub_->on_activate();
     costmap_update_pub_->on_activate();
     costmap_raw_pub_->on_activate();
+    costmap_raw_update_pub_->on_activate();
   }
 
   /**
@@ -101,6 +103,7 @@ class Costmap2DPublisher
     costmap_pub_->on_deactivate();
     costmap_update_pub_->on_deactivate();
     costmap_raw_pub_->on_deactivate();
+    costmap_raw_update_pub_->on_deactivate();
   }
 
   /**
@@ -136,6 +139,9 @@ class Costmap2DPublisher
   void prepareGrid();
   void prepareCostmap();
 
+  /** @brief Prepare CostmapUpdate msg for publication. */
+  std::unique_ptr<nav2_msgs::msg::CostmapUpdate> createCostmapUpdateMsg();
+
   /** @brief Publish the latest full costmap to the new subscriber. */
   // void onNewSubscription(const ros::SingleSubscriberPublisher& pub);
 
@@ -164,6 +170,8 @@ class Costmap2DPublisher
 
   // Publisher for raw costmap values as msg::Costmap from layered costmap
   rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::Costmap>::SharedPtr costmap_raw_pub_;
+  rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::CostmapUpdate>::SharedPtr
+    costmap_raw_update_pub_;
 
   // Service for getting the costmaps
   rclcpp::Service<nav2_msgs::srv::GetCostmap>::SharedPtr costmap_service_;

nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp

@@ -21,6 +21,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_costmap_2d/costmap_2d.hpp"
 #include "nav2_msgs/msg/costmap.hpp"
+#include "nav2_msgs/msg/costmap_update.hpp"
 #include "nav2_util/lifecycle_node.hpp"
 
 namespace nav2_costmap_2d
@@ -52,25 +53,36 @@ class CostmapSubscriber
   ~CostmapSubscriber() {}
 
   /**
-   * @brief A Get the costmap from topic
+   * @brief Get current costmap
    */
   std::shared_ptr<Costmap2D> getCostmap();
-
-  /**
-   * @brief Convert an occ grid message into a costmap object
-   */
-  void toCostmap2D();
   /**
    * @brief Callback for the costmap topic
    */
   void costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr msg);
+  /**
+   * @brief Callback for the costmap's update topic
+   */
+  void costmapUpdateCallback(const nav2_msgs::msg::CostmapUpdate::SharedPtr update_msg);
 
 protected:
+  void processCurrentCostmapMsg();
+
+  bool haveCostmapParametersChanged();
+  bool hasCostmapSizeChanged();
+  bool hasCostmapResolutionChanged();
+  bool hasCostmapOriginPositionChanged();
+
+  rclcpp::Subscription<nav2_msgs::msg::Costmap>::SharedPtr costmap_sub_;
+  rclcpp::Subscription<nav2_msgs::msg::CostmapUpdate>::SharedPtr costmap_update_sub_;
+
   std::shared_ptr<Costmap2D> costmap_;
   nav2_msgs::msg::Costmap::SharedPtr costmap_msg_;
+
   std::string topic_name_;
   bool costmap_received_{false};
-  rclcpp::Subscription<nav2_msgs::msg::Costmap>::SharedPtr costmap_sub_;
+  std::mutex costmap_msg_mutex_;
+  rclcpp::Logger logger_{rclcpp::get_logger("nav2_costmap_2d")};
 };
 
 }  // namespace nav2_costmap_2d

nav2_costmap_2d/src/costmap_2d_publisher.cpp

@@ -76,6 +76,8 @@ Costmap2DPublisher::Costmap2DPublisher(
     custom_qos);
   costmap_update_pub_ = node->create_publisher<map_msgs::msg::OccupancyGridUpdate>(
     topic_name + "_updates", custom_qos);
+  costmap_raw_update_pub_ = node->create_publisher<nav2_msgs::msg::CostmapUpdate>(
+    topic_name + "_raw_updates", custom_qos);
 
   // Create a service that will use the callback function to handle requests.
   costmap_service_ = node->create_service<nav2_msgs::srv::GetCostmap>(
@@ -181,13 +183,30 @@ void Costmap2DPublisher::prepareCostmap()
   }
 }
 
-void Costmap2DPublisher::publishCostmap()
+std::unique_ptr<nav2_msgs::msg::CostmapUpdate> Costmap2DPublisher::createCostmapUpdateMsg()
 {
-  if (costmap_raw_pub_->get_subscription_count() > 0) {
-    prepareCostmap();
-    costmap_raw_pub_->publish(std::move(costmap_raw_));
+  auto msg = std::make_unique<nav2_msgs::msg::CostmapUpdate>();
+
+  msg->header.stamp = clock_->now();
+  msg->header.frame_id = global_frame_;
+  msg->x = x0_;
+  msg->y = y0_;
+  msg->size_x = xn_ - x0_;
+  msg->size_y = yn_ - y0_;
+  msg->data.resize(msg->size_x * msg->size_y);
+
+  std::uint32_t i = 0;
+  for (std::uint32_t y = y0_; y < yn_; y++) {
+    for (std::uint32_t x = x0_; x < xn_; x++) {
+      //todo copy from array instead of calling method
+      msg->data[i++] = costmap_->getCost(x, y);
+    }
   }
+  return msg;
+}
 
+void Costmap2DPublisher::publishCostmap()
+{
   float resolution = costmap_->getResolution();
   if (always_send_full_costmap_ || grid_resolution != resolution ||
     grid_width != costmap_->getSizeInCellsX() ||
@@ -199,10 +218,14 @@ void Costmap2DPublisher::publishCostmap()
       prepareGrid();
       costmap_pub_->publish(std::move(grid_));
     }
+    if (costmap_raw_pub_->get_subscription_count() > 0) {
+      prepareCostmap();
+      costmap_raw_pub_->publish(std::move(costmap_raw_));
+    }
   } else if (x0_ < xn_) {
+    // Publish Just Updates
+    std::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
     if (costmap_update_pub_->get_subscription_count() > 0) {
-      std::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
-      // Publish Just an Update
       auto update = std::make_unique<map_msgs::msg::OccupancyGridUpdate>();
       update->header.stamp = rclcpp::Time();
       update->header.frame_id = global_frame_;
@@ -220,6 +243,9 @@ void Costmap2DPublisher::publishCostmap()
       }
       costmap_update_pub_->publish(std::move(update));
     }
+    if (costmap_raw_update_pub_->get_subscription_count() > 0) {
+      costmap_raw_update_pub_->publish(createCostmapUpdateMsg());
+    }
   }
 
   xn_ = yn_ = 0;

nav2_costmap_2d/src/costmap_subscriber.cpp

@@ -14,84 +14,155 @@
 
 #include <string>
 #include <memory>
+#include <mutex>
 
 #include "nav2_costmap_2d/costmap_subscriber.hpp"
 
 namespace nav2_costmap_2d
 {
 
+constexpr int costmapUpdateQueueDepth = 10;
+
 CostmapSubscriber::CostmapSubscriber(
   const nav2_util::LifecycleNode::WeakPtr & parent,
   const std::string & topic_name)
 : topic_name_(topic_name)
 {
   auto node = parent.lock();
+  logger_ = node->get_logger();
   costmap_sub_ = node->create_subscription<nav2_msgs::msg::Costmap>(
     topic_name_,
     rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
     std::bind(&CostmapSubscriber::costmapCallback, this, std::placeholders::_1));
+  costmap_update_sub_ = node->create_subscription<nav2_msgs::msg::CostmapUpdate>(
+    topic_name_ + "_updates",
+    rclcpp::QoS(rclcpp::KeepLast(costmapUpdateQueueDepth)).transient_local().reliable(),
+    std::bind(&CostmapSubscriber::costmapUpdateCallback, this, std::placeholders::_1));
 }
 
+// todo: is it still necessary
 CostmapSubscriber::CostmapSubscriber(
   const rclcpp::Node::WeakPtr & parent,
   const std::string & topic_name)
 : topic_name_(topic_name)
 {
   auto node = parent.lock();
+  logger_ = node->get_logger();
   costmap_sub_ = node->create_subscription<nav2_msgs::msg::Costmap>(
     topic_name_,
     rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
     std::bind(&CostmapSubscriber::costmapCallback, this, std::placeholders::_1));
+  costmap_update_sub_ = node->create_subscription<nav2_msgs::msg::CostmapUpdate>(
+    topic_name_ + "_updates",
+    rclcpp::QoS(rclcpp::KeepLast(costmapUpdateQueueDepth)).transient_local().reliable(),
+    std::bind(&CostmapSubscriber::costmapUpdateCallback, this, std::placeholders::_1));
 }
 
+// todo: change to const?
 std::shared_ptr<Costmap2D> CostmapSubscriber::getCostmap()
 {
   if (!costmap_received_) {
     throw std::runtime_error("Costmap is not available");
   }
-  toCostmap2D();
+  if (costmap_msg_ != nullptr) {
+    processCurrentCostmapMsg();
+  }
   return costmap_;
 }
 
-void CostmapSubscriber::toCostmap2D()
+void CostmapSubscriber::costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr msg)
 {
-  auto current_costmap_msg = std::atomic_load(&costmap_msg_);
-
-  if (costmap_ == nullptr) {
-    costmap_ = std::make_shared<Costmap2D>(
-      current_costmap_msg->metadata.size_x, current_costmap_msg->metadata.size_y,
-      current_costmap_msg->metadata.resolution, current_costmap_msg->metadata.origin.position.x,
-      current_costmap_msg->metadata.origin.position.y);
-  } else if (costmap_->getSizeInCellsX() != current_costmap_msg->metadata.size_x ||  // NOLINT
-    costmap_->getSizeInCellsY() != current_costmap_msg->metadata.size_y ||
-    costmap_->getResolution() != current_costmap_msg->metadata.resolution ||
-    costmap_->getOriginX() != current_costmap_msg->metadata.origin.position.x ||
-    costmap_->getOriginY() != current_costmap_msg->metadata.origin.position.y)
   {
-    // Update the size of the costmap
-    costmap_->resizeMap(
-      current_costmap_msg->metadata.size_x, current_costmap_msg->metadata.size_y,
-      current_costmap_msg->metadata.resolution,
-      current_costmap_msg->metadata.origin.position.x,
-      current_costmap_msg->metadata.origin.position.y);
+    std::lock_guard<std::mutex> lock(costmap_msg_mutex_);
+    costmap_msg_ = msg;
   }
+  if (!costmap_received_) {
+    costmap_ = std::make_shared<Costmap2D>(
+      msg->metadata.size_x, msg->metadata.size_y,
+      msg->metadata.resolution, msg->metadata.origin.position.x,
+      msg->metadata.origin.position.y);
 
-  unsigned char * master_array = costmap_->getCharMap();
-  unsigned int index = 0;
-  for (unsigned int i = 0; i < current_costmap_msg->metadata.size_x; ++i) {
-    for (unsigned int j = 0; j < current_costmap_msg->metadata.size_y; ++j) {
-      master_array[index] = current_costmap_msg->data[index];
-      ++index;
+    processCurrentCostmapMsg();
+
+    costmap_received_ = true;
+  }
+}
+
+void CostmapSubscriber::costmapUpdateCallback(
+  const nav2_msgs::msg::CostmapUpdate::SharedPtr update_msg)
+{
+  if (costmap_received_) {
+    if (costmap_msg_) {
+      processCurrentCostmapMsg();
     }
+
+    std::lock_guard<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
+
+    auto map_cell_size_x = costmap_->getSizeInCellsX();
+    auto map_call_size_y = costmap_->getSizeInCellsY();
+
+    if (map_cell_size_x < update_msg->x + update_msg->size_x ||
+      map_call_size_y < update_msg->y + update_msg->size_y)
+    {
+      RCLCPP_WARN(
+        logger_, "Update area outside of original map area. Costmap bounds: %d X %d, "
+        "Update origin: %d, %d  bounds: %d X %d", map_cell_size_x, map_call_size_y,
+        update_msg->x, update_msg->y, update_msg->size_x, update_msg->size_y);
+      return;
+    }
+    unsigned char * master_array = costmap_->getCharMap();
+    // copy update msg row-wise
+    for (size_t y = 0; y < update_msg->size_y; ++y) {
+      auto starting_index_of_row_update_in_costmap = (y + update_msg->y) * map_cell_size_x +
+        update_msg->x;
+
+      std::copy_n(
+        update_msg->data.begin() + (y * update_msg->size_x),
+        update_msg->size_x, &master_array[starting_index_of_row_update_in_costmap]);
+    }
+  } else {
+    RCLCPP_WARN(logger_, "No costmap received.");
   }
 }
 
-void CostmapSubscriber::costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr msg)
+void CostmapSubscriber::processCurrentCostmapMsg()
 {
-  std::atomic_store(&costmap_msg_, msg);
-  if (!costmap_received_) {
-    costmap_received_ = true;
+  std::scoped_lock lock(*(costmap_->getMutex()), costmap_msg_mutex_);
+  if (haveCostmapParametersChanged()) {
+    costmap_->resizeMap(
+      costmap_msg_->metadata.size_x, costmap_msg_->metadata.size_y,
+      costmap_msg_->metadata.resolution,
+      costmap_msg_->metadata.origin.position.x,
+      costmap_msg_->metadata.origin.position.y);
   }
+
+  unsigned char * master_array = costmap_->getCharMap();
+  std::copy(costmap_msg_->data.begin(), costmap_msg_->data.end(), master_array);
+  costmap_msg_ = nullptr;
+}
+
+bool CostmapSubscriber::haveCostmapParametersChanged()
+{
+  return hasCostmapSizeChanged() ||
+         hasCostmapResolutionChanged() ||
+         hasCostmapOriginPositionChanged();
+}
+
+bool CostmapSubscriber::hasCostmapSizeChanged()
+{
+  return costmap_->getSizeInCellsX() != costmap_msg_->metadata.size_x ||
+         costmap_->getSizeInCellsY() != costmap_msg_->metadata.size_y;
+}
+
+bool CostmapSubscriber::hasCostmapResolutionChanged()
+{
+  return costmap_->getResolution() != costmap_msg_->metadata.resolution;
+}
+
+bool CostmapSubscriber::hasCostmapOriginPositionChanged()
+{
+  return costmap_->getOriginX() != costmap_msg_->metadata.origin.position.x ||
+         costmap_->getOriginY() != costmap_msg_->metadata.origin.position.y;
 }
 
 }  // namespace nav2_costmap_2d