From 51dfbcc73751947341de11c16ef643df9ef33171 Mon Sep 17 00:00:00 2001 From: Erik Holum Date: Thu, 9 Nov 2023 09:29:09 -0500 Subject: [PATCH] Fix threading issue for collision velocity scaling in MoveIt Servo (#2517) --- moveit_ros/moveit_servo/src/collision_monitor.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/moveit_ros/moveit_servo/src/collision_monitor.cpp b/moveit_ros/moveit_servo/src/collision_monitor.cpp index e892629c063..5a84a98a37e 100644 --- a/moveit_ros/moveit_servo/src/collision_monitor.cpp +++ b/moveit_ros/moveit_servo/src/collision_monitor.cpp @@ -95,9 +95,6 @@ void CollisionMonitor::checkCollisions() const double self_velocity_scale_coefficient{ log_val / servo_params_.self_collision_proximity_threshold }; const double scene_velocity_scale_coefficient{ log_val / servo_params_.scene_collision_proximity_threshold }; - // Reset the scale on every iteration. - collision_velocity_scale_ = 1.0; - if (servo_params_.check_collisions) { // Fetch latest robot state. @@ -123,6 +120,11 @@ void CollisionMonitor::checkCollisions() // k = - ln(0.001) / collision_proximity_threshold // velocity_scale should equal one when collision_distance is at collision_proximity_threshold. // velocity_scale should equal 0.001 when collision_distance is at zero. + // + // NOTE: + // collision_velocity_scale_ is shared by the primary servo thread. Be sure to not set any + // intermediate values in this loop or they can be picked up and throw off scaling while processing + // joint updates. if (self_collision_result_.collision || scene_collision_result_.collision) { @@ -154,6 +156,12 @@ void CollisionMonitor::checkCollisions() collision_velocity_scale_ = std::min(scene_collision_scale, self_collision_scale); } } + else + { + // If collision checking is disabled we do not scale + collision_velocity_scale_ = 1.0; + } + rate.sleep(); } }