Instantaneously clear of Octomap voxels overlapping with new CollisionObjects

moveit_core/collision_detection/include/moveit/collision_detection/occupancy_map.hpp

@@ -37,6 +37,7 @@
 #pragma once
 
 #include <octomap/octomap.h>
+#include <Eigen/Geometry>
 
 #include <memory>
 #include <string>
@@ -110,6 +111,21 @@ class OccMapTree : public octomap::OcTree
     update_callback_ = update_callback;
   }
 
+  /** @brief Clear all voxels in the specified bounding region */
+  void clearRegion(const Eigen::Vector3d& min_bound, const Eigen::Vector3d& max_bound)
+  {
+    octomap::point3d min_pt(min_bound.x(), min_bound.y(), min_bound.z());
+    octomap::point3d max_pt(max_bound.x(), max_bound.y(), max_bound.z());
+
+    this->lockWrite();
+    for (auto it = this->begin_leafs_bbx(min_pt, max_pt); it != this->end_leafs_bbx(); ++it)
+    {
+      it->setLogOdds(octomap::logodds(0.01));
+    }
+    this->updateInnerOccupancy();
+    this->unlockWrite();
+  }
+
 private:
   std::shared_mutex tree_mutex_;
   std::function<void()> update_callback_;

moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor.hpp

@@ -239,6 +239,15 @@ class OccupancyMapMonitor
    */
   ShapeHandle excludeShape(const shapes::ShapeConstPtr& shape);
 
+  /**
+   * \brief Clear all voxels in the octomap that collide with the given shape at the given pose.
+   *        Useful for clearing out voxels that are occupied by added collision object in the planning scene
+   *
+   * @param[in]  shape  The shape to be cleared from the octomap
+   * @param[in]  pose   The pose of the shape in the world frame
+   */
+  void clearShape(const shapes::ShapeConstPtr& shape, const Eigen::Isometry3d& pose);
+
   /**
    * \brief Forget about this shape handle and the shapes it corresponds to
    *

moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp

@@ -37,17 +37,18 @@
 #include <moveit/collision_detection/occupancy_map.hpp>
 #include <moveit/occupancy_map_monitor/occupancy_map_monitor.hpp>
 #include <moveit/occupancy_map_monitor/occupancy_map_monitor_middleware_handle.hpp>
+#include <moveit/utils/logger.hpp>
 #include <moveit_msgs/srv/load_map.hpp>
 #include <moveit_msgs/srv/save_map.hpp>
 #include <rclcpp/clock.hpp>
 #include <rclcpp/logger.hpp>
 #include <rclcpp/logging.hpp>
 #include <rclcpp/node.hpp>
+#include <geometric_shapes/shape_operations.h>
 #include <memory>
 #include <string>
 #include <utility>
 #include <vector>
-#include <moveit/utils/logger.hpp>
 
 namespace occupancy_map_monitor
 {
@@ -213,6 +214,35 @@ ShapeHandle OccupancyMapMonitor::excludeShape(const shapes::ShapeConstPtr& shape
   return h;
 }
 
+void OccupancyMapMonitor::clearShape(const shapes::ShapeConstPtr& shape, const Eigen::Isometry3d& pose)
+{
+  if (!tree_ || !shape)
+    return;
+
+  // get the local shape extents
+  Eigen::Vector3d extents = shapes::computeShapeExtents(shape.get());
+  Eigen::Vector3d local_min = -extents * 0.5;
+  Eigen::Vector3d local_max = extents * 0.5;
+
+  // AABB by transforming the 8 corners of the local bounding box
+  Eigen::Vector3d global_min(std::numeric_limits<double>::max(), std::numeric_limits<double>::max(),
+                             std::numeric_limits<double>::max());
+  Eigen::Vector3d global_max(-std::numeric_limits<double>::max(), -std::numeric_limits<double>::max(),
+                             -std::numeric_limits<double>::max());
+  for (int i = 0; i < 8; ++i)
+  {
+    Eigen::Vector3d corner;
+    corner << (i & 1 ? local_max.x() : local_min.x()), (i & 2 ? local_max.y() : local_min.y()),
+        (i & 4 ? local_max.z() : local_min.z());
+
+    Eigen::Vector3d transformed_corner = pose * corner;
+    global_min = global_min.cwiseMin(transformed_corner);
+    global_max = global_max.cwiseMax(transformed_corner);
+  }
+
+  tree_->clearRegion(global_min, global_max);
+}
+
 void OccupancyMapMonitor::forgetShape(ShapeHandle handle)
 {
   // if we have just one updater, remove the additional level of indirection

moveit_ros/occupancy_map_monitor/test/occupancy_map_monitor_tests.cpp

@@ -35,6 +35,9 @@
 /* Author: Tyler Weaver */
 
 #include <moveit/occupancy_map_monitor/occupancy_map_monitor.hpp>
+#include <moveit/collision_detection/occupancy_map.hpp>
+#include <octomap/octomap.h>
+#include <Eigen/Geometry>
 
 #include <gmock/gmock.h>
 #include <gtest/gtest.h>
@@ -84,8 +87,108 @@ TEST(OccupancyMapMonitorTests, ConstructorTest)
   };
 }
 
+/**
+ * @class OctomapVoxelClearingTests
+ * @brief Test fixtures to validate the instantaneous clearing of Octomap voxels
+ *        when a collision object is added to the environment.
+ */
+class OctomapVoxelClearingTests : public ::testing::Test
+{
+protected:
+  void SetUp() override
+  {
+    // dummy ROS 2 node for the monitor
+    auto node = std::make_shared<rclcpp::Node>("occupancy_map_monitor_test_node");
+    monitor_ = std::make_unique<occupancy_map_monitor::OccupancyMapMonitor>(node, 0.05);
+  }
+
+  void populateRegion(const octomap::point3d& min, const octomap::point3d& max)
+  {
+    auto tree = monitor_->getOcTreePtr();
+    double res = tree->getResolution();
+    for (double x = min.x(); x <= max.x(); x += res)
+      for (double y = min.y(); y <= max.y(); y += res)
+        for (double z = min.z(); z <= max.z(); z += res)
+          tree->updateNode(octomap::point3d(x, y, z), true);
+
+    tree->updateInnerOccupancy();
+  }
+
+  void verifyVolumeIsClear(const collision_detection::OccMapTreePtr& tree, const Eigen::Vector3d& center, double range)
+  {
+    tree->lockRead();
+    std::vector<Eigen::Vector3d> points = { center,
+                                            center + Eigen::Vector3d(range, 0, 0),
+                                            center - Eigen::Vector3d(range, 0, 0),
+                                            center + Eigen::Vector3d(0, range, 0),
+                                            center - Eigen::Vector3d(0, range, 0),
+                                            center + Eigen::Vector3d(0, 0, range),
+                                            center - Eigen::Vector3d(0, 0, range) };
+    for (const auto& pt : points)
+    {
+      octomap::OcTreeNode* node = tree->search(pt.x(), pt.y(), pt.z());
+      EXPECT_FALSE(node && tree->isNodeOccupied(node))
+          << "Voxel at (" << pt.x() << ", " << pt.y() << ", " << pt.z() << ") was not cleared!";
+    }
+    tree->unlockRead();
+  }
+
+  std::unique_ptr<occupancy_map_monitor::OccupancyMapMonitor> monitor_;
+};
+
+TEST_F(OctomapVoxelClearingTests, VoxelManualClearingTriggered)
+{
+  auto tree = monitor_->getOcTreePtr();
+  populateRegion(octomap::point3d(0, 0, 0), octomap::point3d(1, 1, 1));
+
+  // Define a sub-region to clear
+  Eigen::Vector3d clear_min(0.2, 0.2, 0.2);
+  Eigen::Vector3d clear_max(0.8, 0.8, 0.8);
+
+  // EXECUTE: Directly wipe the specified volume in the tree
+  tree->clearRegion(clear_min, clear_max);
+
+  // VERIFY: Check center of the box
+  verifyVolumeIsClear(tree, Eigen::Vector3d(0.5, 0.5, 0.5), 0.3);
+}
+
+TEST_F(OctomapVoxelClearingTests, VoxelClearingOnObjectAddition)
+{
+  auto tree = monitor_->getOcTreePtr();
+
+  // --- Test Case 1: Cylinder ---
+  populateRegion(octomap::point3d(0, 0, 0), octomap::point3d(1, 1, 1));
+
+  auto cylinder = std::make_shared<shapes::Cylinder>(0.2, 0.6);
+  Eigen::Isometry3d cyl_pose = Eigen::Isometry3d::Identity();
+  cyl_pose.translation() = Eigen::Vector3d(0.3, 0.3, 0.3);
+
+  // EXECUTE
+  monitor_->clearShape(cylinder, cyl_pose);
+
+  // VERIFY: Check center and points around the cylinder center
+  verifyVolumeIsClear(tree, Eigen::Vector3d(0.3, 0.3, 0.3), 0.1);
+
+  // --- Test Case 2: Rotated Box ---
+  populateRegion(octomap::point3d(0, 0, 0), octomap::point3d(1, 1, 1));
+
+  auto box = std::make_shared<shapes::Box>(0.2, 0.2, 0.8);
+  Eigen::Isometry3d box_pose = Eigen::Isometry3d::Identity();
+  box_pose = Eigen::AngleAxisd(M_PI / 4.0, Eigen::Vector3d::UnitY());  // Rotate 45 deg
+  box_pose.translation() = Eigen::Vector3d(0.6, 0.6, 0.6);
+
+  // EXECUTE
+  monitor_->clearShape(box, box_pose);
+
+  // VERIFY: Check center of the rotated box
+  verifyVolumeIsClear(tree, Eigen::Vector3d(0.6, 0.6, 0.6), 0.1);
+}
+
 int main(int argc, char** argv)
 {
+  rclcpp::init(argc, argv);
   ::testing::InitGoogleTest(&argc, argv);
-  return RUN_ALL_TESTS();
+  int result = RUN_ALL_TESTS();
+  rclcpp::shutdown();
+  return result;
 }

moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp

@@ -828,8 +828,22 @@ bool PlanningSceneMonitor::processCollisionObjectMsg(const moveit_msgs::msg::Col
   {
     std::unique_lock<std::shared_mutex> ulock(scene_update_mutex_);
     last_update_time_ = rclcpp::Clock().now();
-    if (!scene_->processCollisionObjectMsg(*object))
+    if (scene_->processCollisionObjectMsg(*object))
+    {
+      if (octomap_monitor_)
+      {
+        auto obj = scene_->getWorld()->getObject(object->id);
+        if (obj)
+        {
+          // clear the voxels for each shape coincide with the collision object
+          for (std::size_t i = 0; i < obj->shapes_.size(); ++i)
+            octomap_monitor_->clearShape(obj->shapes_[i], obj->global_shape_poses_[i]);
+        }
+      }
+    }
+    else
       return false;
+
     if (color_msg.has_value())
       scene_->setObjectColor(color_msg.value().id, color_msg.value().color);
   }
@@ -850,7 +864,20 @@ bool PlanningSceneMonitor::processAttachedCollisionObjectMsg(
   {
     std::unique_lock<std::shared_mutex> ulock(scene_update_mutex_);
     last_update_time_ = rclcpp::Clock().now();
-    if (!scene_->processAttachedCollisionObjectMsg(*object))
+    if (scene_->processAttachedCollisionObjectMsg(*object))
+    {
+      if (octomap_monitor_)
+      {
+        const moveit::core::AttachedBody* ab = scene_->getCurrentState().getAttachedBody(object->link_name);
+        if (ab)
+        {
+          // clear the voxels for each shape coincide with the object attached to body
+          for (std::size_t i = 0; i < ab->getShapes().size(); ++i)
+            octomap_monitor_->clearShape(ab->getShapes()[i], ab->getGlobalCollisionBodyTransforms()[i]);
+        }
+      }
+    }
+    else
       return false;
   }
   triggerSceneUpdateEvent(UPDATE_GEOMETRY);