[Servo] Fix collision checking with attached objects (#2747)

(cherry picked from commit 68e8680)
moveit · Mar 19, 2024 · 80e173a · 80e173a
1 parent 0a27609
commit 80e173a
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 6 deletions.
diff --git a/moveit_ros/moveit_servo/include/moveit_servo/collision_monitor.hpp b/moveit_ros/moveit_servo/include/moveit_servo/collision_monitor.hpp
@@ -67,8 +67,9 @@ class CollisionMonitor
   // Variables
 
   const servo::Params& servo_params_;
-  moveit::core::RobotStatePtr robot_state_;
+
   const planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
+  moveit::core::RobotState robot_state_;
 
   // The collision monitor thread.
   std::thread monitor_thread_;

diff --git a/moveit_ros/moveit_servo/src/collision_monitor.cpp b/moveit_ros/moveit_servo/src/collision_monitor.cpp
@@ -52,6 +52,7 @@ CollisionMonitor::CollisionMonitor(const planning_scene_monitor::PlanningSceneMo
                                    const servo::Params& servo_params, std::atomic<double>& collision_velocity_scale)
   : servo_params_(servo_params)
   , planning_scene_monitor_(planning_scene_monitor)
+  , robot_state_(planning_scene_monitor->getPlanningScene()->getCurrentState())
   , collision_velocity_scale_(collision_velocity_scale)
 {
 }
@@ -97,23 +98,24 @@ void CollisionMonitor::checkCollisions()
 
     if (servo_params_.check_collisions)
     {
-      // Fetch latest robot state.
-      robot_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState();
+      // Fetch latest robot state using planning scene instead of state monitor due to
+      // https://github.com/ros-planning/moveit2/issues/2748
+      robot_state_ = planning_scene_monitor_->getPlanningScene()->getCurrentState();
       // This must be called before doing collision checking.
-      robot_state_->updateCollisionBodyTransforms();
+      robot_state_.updateCollisionBodyTransforms();
 
       // Get a read-only copy of planning scene.
       planning_scene_monitor::LockedPlanningSceneRO locked_scene(planning_scene_monitor_);
 
       // Check collision with environment.
       scene_collision_result_.clear();
       locked_scene->getCollisionEnv()->checkRobotCollision(scene_collision_request_, scene_collision_result_,
-                                                           *robot_state_, locked_scene->getAllowedCollisionMatrix());
+                                                           robot_state_, locked_scene->getAllowedCollisionMatrix());
 
       // Check robot self collision.
       self_collision_result_.clear();
       locked_scene->getCollisionEnvUnpadded()->checkSelfCollision(
-          self_collision_request_, self_collision_result_, *robot_state_, locked_scene->getAllowedCollisionMatrix());
+          self_collision_request_, self_collision_result_, robot_state_, locked_scene->getAllowedCollisionMatrix());
 
       // If collision detected scale velocity to 0, else start decelerating exponentially.
       // velocity_scale = e ^ k * (collision_distance - threshold)