WIP support clear_map

Author: makeecat

nvblox_ros/include/nvblox_ros/nvblox_node.hpp

@@ -46,6 +46,7 @@
 #include <sensor_msgs/msg/image.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <std_msgs/msg/string.hpp>
+#include <std_srvs/srv/empty.hpp>
 #include <visualization_msgs/msg/marker.hpp>
 
 #include <nvblox_msgs/srv/file_path.hpp>
@@ -173,6 +174,9 @@ class NvbloxNode : public rclcpp::Node
   void getEsdfAndGradientService(
     const std::shared_ptr<nvblox_msgs::srv::EsdfAndGradients::Request> request,
     std::shared_ptr<nvblox_msgs::srv::EsdfAndGradients::Response> response);
+  void clearMapService(
+      const std::shared_ptr<std_srvs::srv::Empty::Request> request,
+      std::shared_ptr<std_srvs::srv::Empty::Response> response);
 
   // Main tick function that process all input data queues in order
   virtual void tick();
@@ -449,6 +453,7 @@ class NvbloxNode : public rclcpp::Node
   rclcpp::Service<nvblox_msgs::srv::FilePath>::SharedPtr save_rates_service_;
   rclcpp::Service<nvblox_msgs::srv::FilePath>::SharedPtr save_timings_service_;
   rclcpp::Service<nvblox_msgs::srv::EsdfAndGradients>::SharedPtr send_esdf_and_gradient_service_;
+  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr clear_map_service_;
 
   // Callback groups.
   rclcpp::CallbackGroup::SharedPtr group_processing_;
@@ -529,11 +534,14 @@ class NvbloxNode : public rclcpp::Node
       nvblox_msgs::srv::EsdfAndGradients>>;
   using FilePathServiceQueuedType = std::shared_ptr<ServiceRequestTask<NvbloxNode,
       nvblox_msgs::srv::FilePath>>;
+  using ClearMapServiceQueuedType = std::shared_ptr<ServiceRequestTask<NvbloxNode,
+      std_srvs::srv::Empty>>;
 
   // Input queues. Unique pointers are used to enable more flexibility when deallocating.
   std::unique_ptr<std::list<sensor_msgs::msg::PointCloud2::ConstSharedPtr>> pointcloud_queue_;
   std::unique_ptr<std::list<EsdfServiceQueuedType>> esdf_service_queue_;
   std::unique_ptr<std::list<FilePathServiceQueuedType>> file_path_service_queue_;
+  std::unique_ptr<std::list<ClearMapServiceQueuedType>> clear_map_service_queue_;
   std::unique_ptr<std::list<ImageTypeVariant>> depth_image_queue_;
   std::unique_ptr<std::list<ImageTypeVariant>>
   color_image_queue_;
@@ -544,6 +552,7 @@ class NvbloxNode : public rclcpp::Node
   static constexpr char kPointcloudQueueName[] = "pointcloud_queue";
   static constexpr char kFilePathServiceQueueName[] = "file_path_service_queue";
   static constexpr char kEsdfServiceQueueName[] = "esdf_service_queue";
+  static constexpr char kClearMapServiceQueueName[] = "clear_map_service_queue";
 
   // Input queue mutexes.
   std::mutex depth_queue_mutex_;
@@ -552,6 +561,7 @@ class NvbloxNode : public rclcpp::Node
   std::mutex color_mask_queue_mutex_;
   std::mutex pointcloud_queue_mutex_;
   std::mutex esdf_service_queue_mutex_;
+  std::mutex clear_map_service_queue_mutex_;
   std::mutex file_path_service_queue_mutex_;
 
   // Counts the number of messages dropped from the input queues.

nvblox_ros/src/lib/nvblox_node.cpp

@@ -223,6 +223,7 @@ void NvbloxNode::subscribeToTopics()
   pointcloud_queue_ = std::make_unique<std::list<sensor_msgs::msg::PointCloud2::ConstSharedPtr>>();
   esdf_service_queue_ = std::make_unique<std::list<EsdfServiceQueuedType>>();
   file_path_service_queue_ = std::make_unique<std::list<FilePathServiceQueuedType>>();
+  clear_map_service_queue_ = std::make_unique<std::list<ClearMapServiceQueuedType>>();
 
   constexpr int kQueueSize = 10;
 
@@ -457,6 +458,10 @@ void NvbloxNode::advertiseServices()
       &NvbloxNode::getEsdfAndGradientService, this, std::placeholders::_1,
       std::placeholders::_2),
     rmw_qos_profile_services_default);
+  clear_map_service_ = create_service<std_srvs::srv::Empty>(
+    "~/clear_map",
+    std::bind(&NvbloxNode::clearMapService, this, std::placeholders::_1, std::placeholders::_2),
+    rmw_qos_profile_services_default);
 }
 
 void NvbloxNode::setupTimers()
@@ -791,6 +796,11 @@ void NvbloxNode::processServiceRequestTaskQueue()
     &file_path_service_queue_mutex_,
     service_ready,
     task);
+  processQueue<ClearMapServiceQueuedType>(
+    clear_map_service_queue_,
+    &clear_map_service_queue_mutex_,
+    service_ready,
+    task);
   // If a service requested visualization, publish right now.
   if (publish_layers_requested_) {
     publishLayers();
@@ -1828,6 +1838,54 @@ void NvbloxNode::getEsdfAndGradientService(
   pushOntoQueue(kEsdfServiceQueueName, task, esdf_service_queue_, &esdf_service_queue_mutex_);
   task->waitForTaskCompletion();
 }
+void NvbloxNode::clearMapService(
+  const std::shared_ptr<std_srvs::srv::Empty::Request> request,
+  std::shared_ptr<std_srvs::srv::Empty::Response> response)
+{
+  // Define the task function
+  TaskFunctionType<NvbloxNode, std_srvs::srv::Empty> request_task =
+    [](auto node,
+      auto service_request,
+      auto service_response) {
+      RCLCPP_INFO(node->get_logger(), "Clearing the entire map");
+
+      // Get the current position
+      Transform T_L_MC;
+      if (node->transformer_.lookupTransformToGlobalFrame(
+          node->params_.map_clearing_frame_id, rclcpp::Time(0),
+          &T_L_MC))
+      {
+        // Set radius to 0 to clear everything
+        float original_radius = node->params_.map_clearing_radius_m;
+        node->params_.map_clearing_radius_m = 0.0f;
+
+        // Call the existing function to clear everything
+        node->clearMapOutsideOfRadiusOfLastKnownPose();
+
+        // Restore the original radius
+        node->params_.map_clearing_radius_m = original_radius;
+
+        // Request visualization update
+        node->publish_layers_requested_ = true;
+
+        RCLCPP_INFO(node->get_logger(), "Map cleared successfully");
+        return true;
+      } else {
+        RCLCPP_ERROR(
+          node->get_logger(), "Failed to lookup transform for frame: %s",
+          node->params_.map_clearing_frame_id.get().c_str());
+        return false;
+      }
+    };
+
+  // Create the ServiceRequestTask
+  auto task =
+    std::make_shared<ServiceRequestTask<NvbloxNode, std_srvs::srv::Empty>>(
+    request_task, this, request, response);
+  // Push the task onto the queue and wait for completion.
+  pushOntoQueue(kClearMapServiceQueueName, task, clear_map_service_queue_, &clear_map_service_queue_mutex_);
+  task->waitForTaskCompletion();
+}
 
 }  // namespace nvblox