Use ACM in all MoveIt Servo collision checks (#2643)

Co-authored-by: Sebastian Castro <[email protected]>
moveit · Mar 14, 2024 · 360299c · 360299c
1 parent 4c8e353
commit 360299c
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 12 deletions.
diff --git a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h
@@ -80,9 +80,6 @@ class CollisionCheck
   /** \brief Run one iteration of collision checking */
   void run();
 
-  /** \brief Get a read-only copy of the planning scene */
-  planning_scene_monitor::LockedPlanningSceneRO getLockedPlanningSceneRO() const;
-
   // Pointer to the ROS node
   const std::shared_ptr<rclcpp::Node> node_;
 

diff --git a/moveit_ros/moveit_servo/src/collision_check.cpp b/moveit_ros/moveit_servo/src/collision_check.cpp
@@ -77,11 +77,6 @@ CollisionCheck::CollisionCheck(const rclcpp::Node::SharedPtr& node, const ServoP
   current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState();
 }
 
-planning_scene_monitor::LockedPlanningSceneRO CollisionCheck::getLockedPlanningSceneRO() const
-{
-  return planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor_);
-}
-
 void CollisionCheck::start()
 {
   timer_ = node_->create_wall_timer(std::chrono::duration<double>(period_), [this]() { return run(); });
@@ -101,16 +96,17 @@ void CollisionCheck::run()
 
   // Do a timer-safe distance-based collision detection
   collision_result_.clear();
-  getLockedPlanningSceneRO()->getCollisionEnv()->checkRobotCollision(collision_request_, collision_result_,
-                                                                     *current_state_);
+  auto locked_scene = planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor_);
+  locked_scene->getCollisionEnv()->checkRobotCollision(collision_request_, collision_result_, *current_state_,
+                                                       locked_scene->getAllowedCollisionMatrix());
   scene_collision_distance_ = collision_result_.distance;
   collision_detected_ |= collision_result_.collision;
   collision_result_.print();
 
   collision_result_.clear();
   // Self-collisions and scene collisions are checked separately so different thresholds can be used
-  getLockedPlanningSceneRO()->getCollisionEnvUnpadded()->checkSelfCollision(
-      collision_request_, collision_result_, *current_state_, getLockedPlanningSceneRO()->getAllowedCollisionMatrix());
+  locked_scene->getCollisionEnvUnpadded()->checkSelfCollision(collision_request_, collision_result_, *current_state_,
+                                                              locked_scene->getAllowedCollisionMatrix());
   self_collision_distance_ = collision_result_.distance;
   collision_detected_ |= collision_result_.collision;
   collision_result_.print();