From 06356444fc7cd6238908409450de96ccb943957a Mon Sep 17 00:00:00 2001
From: Maria Beneyto <mbeneyto@robotnik.es>
Date: Thu, 19 Dec 2024 16:03:32 +0100
Subject: [PATCH] fix acm compilation error due to error in merge

---
 .../moveit_servo/src/collision_check.cpp      | 38 ++++++++-----------
 1 file changed, 16 insertions(+), 22 deletions(-)

diff --git a/moveit_ros/moveit_servo/src/collision_check.cpp b/moveit_ros/moveit_servo/src/collision_check.cpp
index b233c484bc..13eee0b6f1 100644
--- a/moveit_ros/moveit_servo/src/collision_check.cpp
+++ b/moveit_ros/moveit_servo/src/collision_check.cpp
@@ -38,7 +38,6 @@
  */
 
 #include <std_msgs/Float64.h>
-#include <std_msgs/Float32.h>
 
 #include <moveit_servo/collision_check.h>
 #include <moveit_servo/make_shared_from_pool.h>
@@ -76,7 +75,6 @@ CollisionCheck::CollisionCheck(ros::NodeHandle& nh, const moveit_servo::ServoPar
   // Internal namespace
   ros::NodeHandle internal_nh(nh_, "internal");
   collision_velocity_scale_pub_ = internal_nh.advertise<std_msgs::Float64>("collision_velocity_scale", ROS_QUEUE_SIZE);
-  collision_piechart_pub_ = internal_nh.advertise<std_msgs::Float32>("/collision_piechart", ROS_QUEUE_SIZE);
   worst_case_stop_time_sub_ =
       internal_nh.subscribe("worst_case_stop_time", ROS_QUEUE_SIZE, &CollisionCheck::worstCaseStopTimeCB, this);
 
@@ -114,29 +112,25 @@ void CollisionCheck::run(const ros::TimerEvent& timer_event)
   current_state_->updateCollisionBodyTransforms();
   collision_detected_ = false;
 
-  // Do a timer-safe distance-based collision detection
-  collision_result_.clear();
-  getLockedPlanningSceneRO()->getCollisionEnv()->checkRobotCollision(collision_request_, collision_result_,
-                                                                     *current_state_,getLockedPlanningSceneRO()->getAllowedCollisionMatrix());
-  scene_collision_distance_ = collision_result_.distance;
-
   {
-    auto piechart_msg = moveit::util::make_shared_from_pool<std_msgs::Float32>();
-    piechart_msg->data = scene_collision_distance_;
-    collision_piechart_pub_.publish(piechart_msg);
+    auto scene = getLockedPlanningSceneRO();
+    auto& acm = scene->getAllowedCollisionMatrix();
+
+    // Do a timer-safe distance-based collision detection
+    collision_result_.clear();
+    scene->getCollisionEnv()->checkRobotCollision(collision_request_, collision_result_, *current_state_, getLockedPlanningSceneRO()->getAllowedCollisionMatrix());
+    scene_collision_distance_ = collision_result_.distance;
+    collision_detected_ |= collision_result_.collision;
+    collision_result_.print();
+
+    // Self-collisions and scene collisions are checked separately so different thresholds can be used
+    collision_result_.clear();
+    scene->getCollisionEnvUnpadded()->checkSelfCollision(collision_request_, collision_result_, *current_state_, getLockedPlanningSceneRO()->getAllowedCollisionMatrix());
+    self_collision_distance_ = collision_result_.distance;
+    collision_detected_ |= collision_result_.collision;
+    collision_result_.print();
   }
 
-  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_, acm_);
-  self_collision_distance_ = collision_result_.distance;
-  collision_detected_ |= collision_result_.collision;
-  collision_result_.print();
-
   velocity_scale_ = 1;
   // If we're definitely in collision, stop immediately
   if (collision_detected_)