3732 costmap raw update msgs rebased (fixes github issues with PR ros…

…-navigation#3948) (ros-navigation#3965)

* Adding CostmapUpdate msg

* CostmapRawUpdate Publisher

* remove test logs and typos

* change data type to uint8

* pass unique ptr to raw costmap update publisher

* adding subsriber for updates

* -Werror=type-limits for update lower bounds checking

* copy update msg

* change code formatting

* adding lock guards on costmap in footprint_collision_checker.cpp

* adding lock guards for costmap subscriber clients

* review suggestions implementation

* fixing gtests errors

* changes after second round of review

* Update nav2_msgs/msg/CostmapUpdate.msg

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Integration test for costmap subscriber draft

* Change the current grid parameters in separate method

* WIP int test

* adding method in PublisherCostmap2D for OccupancyGridUpdate population

* test full costmap and updates while generating results

* Integration tests for subscribers

* Expected number of msgs related to number of mapchanges in tests

* next round of reviews

* refactor names of Costmap2DPublisher

* remove unnecessary costmap_received_ flag form CostmapSubscriber

---------

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

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,7 +82,12 @@ class DummyCostmapSubscriber : public nav2_costmap_2d::CostmapSubscriber
   void setCostmap(nav2_msgs::msg::Costmap::SharedPtr msg)
   {
     costmap_msg_ = msg;
-    costmap_received_ = true;
+    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();
   }
 };
 

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,9 +139,16 @@ class Costmap2DPublisher
   void prepareGrid();
   void prepareCostmap();
 
+  /** @brief Prepare OccupancyGridUpdate msg for publication. */
+  std::unique_ptr<map_msgs::msg::OccupancyGridUpdate> createGridUpdateMsg();
+  /** @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);
 
+  void updateGridParams();
+
   /** @brief GetCostmap callback service */
   void costmap_service_callback(
     const std::shared_ptr<rmw_request_id_t> request_header,
@@ -164,12 +174,14 @@ 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_;
 
-  float grid_resolution;
-  unsigned int grid_width, grid_height;
+  float grid_resolution_;
+  unsigned int grid_width_, grid_height_;
   std::unique_ptr<nav_msgs::msg::OccupancyGrid> grid_;
   std::unique_ptr<nav2_msgs::msg::Costmap> costmap_raw_;
   // Translate from 0-255 values in costmap to -1 to 100 values in message.

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:
+  bool isCostmapReceived() {return costmap_ != nullptr;}
+  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>(
@@ -115,32 +117,37 @@ void Costmap2DPublisher::onNewSubscription(const ros::SingleSubscriberPublisher&
   pub.publish(grid_);
 } */
 
+
+void Costmap2DPublisher::updateGridParams()
+{
+  saved_origin_x_ = costmap_->getOriginX();
+  saved_origin_y_ = costmap_->getOriginY();
+  grid_resolution_ = costmap_->getResolution();
+  grid_width_ = costmap_->getSizeInCellsX();
+  grid_height_ = costmap_->getSizeInCellsY();
+}
+
 // prepare grid_ message for publication.
 void Costmap2DPublisher::prepareGrid()
 {
   std::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
-  grid_resolution = costmap_->getResolution();
-  grid_width = costmap_->getSizeInCellsX();
-  grid_height = costmap_->getSizeInCellsY();
 
   grid_ = std::make_unique<nav_msgs::msg::OccupancyGrid>();
 
   grid_->header.frame_id = global_frame_;
   grid_->header.stamp = clock_->now();
 
-  grid_->info.resolution = grid_resolution;
+  grid_->info.resolution = grid_resolution_;
 
-  grid_->info.width = grid_width;
-  grid_->info.height = grid_height;
+  grid_->info.width = grid_width_;
+  grid_->info.height = grid_height_;
 
   double wx, wy;
   costmap_->mapToWorld(0, 0, wx, wy);
-  grid_->info.origin.position.x = wx - grid_resolution / 2;
-  grid_->info.origin.position.y = wy - grid_resolution / 2;
+  grid_->info.origin.position.x = wx - grid_resolution_ / 2;
+  grid_->info.origin.position.y = wy - grid_resolution_ / 2;
   grid_->info.origin.position.z = 0.0;
   grid_->info.origin.orientation.w = 1.0;
-  saved_origin_x_ = costmap_->getOriginX();
-  saved_origin_y_ = costmap_->getOriginY();
 
   grid_->data.resize(grid_->info.width * grid_->info.height);
 
@@ -181,44 +188,74 @@ void Costmap2DPublisher::prepareCostmap()
   }
 }
 
-void Costmap2DPublisher::publishCostmap()
+std::unique_ptr<map_msgs::msg::OccupancyGridUpdate> Costmap2DPublisher::createGridUpdateMsg()
+{
+  auto update = std::make_unique<map_msgs::msg::OccupancyGridUpdate>();
+
+  update->header.stamp = clock_->now();
+  update->header.frame_id = global_frame_;
+  update->x = x0_;
+  update->y = y0_;
+  update->width = xn_ - x0_;
+  update->height = yn_ - y0_;
+  update->data.resize(update->width * update->height);
+
+  std::uint32_t i = 0;
+  for (std::uint32_t y = y0_; y < yn_; y++) {
+    for (std::uint32_t x = x0_; x < xn_; x++) {
+      update->data[i++] = cost_translation_table_[costmap_->getCost(x, y)];
+    }
+  }
+  return update;
+}
+
+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++) {
+      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() ||
-    grid_height != costmap_->getSizeInCellsY() ||
+  if (always_send_full_costmap_ || grid_resolution_ != resolution ||
+    grid_width_ != costmap_->getSizeInCellsX() ||
+    grid_height_ != costmap_->getSizeInCellsY() ||
     saved_origin_x_ != costmap_->getOriginX() ||
     saved_origin_y_ != costmap_->getOriginY())
   {
+    updateGridParams();
     if (costmap_pub_->get_subscription_count() > 0) {
       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 update msgs
+    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_;
-      update->x = x0_;
-      update->y = y0_;
-      update->width = xn_ - x0_;
-      update->height = yn_ - y0_;
-      update->data.resize(update->width * update->height);
-      unsigned int i = 0;
-      for (unsigned int y = y0_; y < yn_; y++) {
-        for (unsigned int x = x0_; x < xn_; x++) {
-          unsigned char cost = costmap_->getCost(x, y);
-          update->data[i++] = cost_translation_table_[cost];
-        }
-      }
-      costmap_update_pub_->publish(std::move(update));
+      costmap_update_pub_->publish(createGridUpdateMsg());
+    }
+    if (costmap_raw_update_pub_->get_subscription_count() > 0) {
+      costmap_raw_update_pub_->publish(createCostmapUpdateMsg());
     }
   }