Change servo collision checking parameters to dynamically update (#2183)

moveit_ros/moveit_servo/include/moveit_servo/collision_check.h

@@ -59,8 +59,9 @@ class CollisionCheck
    *  \param planning_scene_monitor: PSM should have scene monitor and state monitor
    *                                 already started when passed into this class
    */
-  CollisionCheck(const rclcpp::Node::SharedPtr& node, const servo::Params& servo_params,
-                 const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor);
+  CollisionCheck(const rclcpp::Node::SharedPtr& node,
+                 const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
+                 const std::shared_ptr<const servo::ParamListener>& servo_param_listener);
 
   ~CollisionCheck()
   {
@@ -83,6 +84,8 @@ class CollisionCheck
   // Pointer to the ROS node
   const std::shared_ptr<rclcpp::Node> node_;
 
+  // Servo parameters
+  const std::shared_ptr<const servo::ParamListener> servo_param_listener_;
   servo::Params servo_params_;
 
   // Pointer to the collision environment
@@ -94,9 +97,9 @@ class CollisionCheck
   // Scale robot velocity according to collision proximity and user-defined thresholds.
   // I scaled exponentially (cubic power) so velocity drops off quickly after the threshold.
   // Proximity decreasing --> decelerate
-  double velocity_scale_ = 1;
-  double self_collision_distance_ = 0;
-  double scene_collision_distance_ = 0;
+  double velocity_scale_ = 1.0;
+  double self_collision_distance_ = 0.0;
+  double scene_collision_distance_ = 0.0;
   bool collision_detected_ = false;
 
   const double self_velocity_scale_coefficient_;

moveit_ros/moveit_servo/include/moveit_servo/servo.h

@@ -56,7 +56,7 @@ class Servo
 public:
   Servo(const rclcpp::Node::SharedPtr& node,
         const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
-        std::unique_ptr<const servo::ParamListener> servo_param_listener);
+        const std::shared_ptr<const servo::ParamListener>& servo_param_listener);
 
   /** \brief Start servo node */
   void start();
@@ -88,7 +88,10 @@ class Servo
   friend class ServoFixture;
 
 private:
+  // Servo parameters
+  const std::shared_ptr<const servo::ParamListener> servo_param_listener_;
   servo::Params servo_params_;
+
   // Pointer to the collision environment
   planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
 

moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h

@@ -79,7 +79,7 @@ class ServoCalcs
 public:
   ServoCalcs(const rclcpp::Node::SharedPtr& node,
              const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
-             std::unique_ptr<const servo::ParamListener> servo_param_listener);
+             const std::shared_ptr<const servo::ParamListener>& servo_param_listener);
 
   ~ServoCalcs();
 
@@ -200,7 +200,7 @@ class ServoCalcs
   std::shared_ptr<rclcpp::Node> node_;
 
   // Servo parameters
-  std::unique_ptr<const servo::ParamListener> servo_param_listener_;
+  const std::shared_ptr<const servo::ParamListener> servo_param_listener_;
   servo::Params servo_params_;
 
   // Pointer to the collision environment

moveit_ros/moveit_servo/src/collision_check.cpp

@@ -49,14 +49,16 @@ constexpr size_t ROS_LOG_THROTTLE_PERIOD = 30 * 1000;  // Milliseconds to thrott
 namespace moveit_servo
 {
 // Constructor for the class that handles collision checking
-CollisionCheck::CollisionCheck(const rclcpp::Node::SharedPtr& node, const servo::Params& servo_params,
-                               const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor)
+CollisionCheck::CollisionCheck(const rclcpp::Node::SharedPtr& node,
+                               const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
+                               const std::shared_ptr<const servo::ParamListener>& servo_param_listener)
   : node_(node)
-  , servo_params_(servo_params)
+  , servo_param_listener_(servo_param_listener)
+  , servo_params_(servo_param_listener_->get_params())
   , planning_scene_monitor_(planning_scene_monitor)
   , self_velocity_scale_coefficient_(-log(0.001) / servo_params_.self_collision_proximity_threshold)
   , scene_velocity_scale_coefficient_(-log(0.001) / servo_params_.scene_collision_proximity_threshold)
-  , period_(1. / servo_params_.collision_check_rate)
+  , period_(1.0 / servo_params_.collision_check_rate)
 {
   // Init collision request
   collision_request_.group_name = servo_params_.move_group_name;
@@ -100,58 +102,68 @@ void CollisionCheck::stop()
 
 void CollisionCheck::run()
 {
-  // Update to the latest current state
-  current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState();
-  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_);
-  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());
-  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_)
+  // Update the latest parameters
+  if (servo_params_.enable_parameter_update)
   {
-    velocity_scale_ = 0;
+    servo_params_ = servo_param_listener_->get_params();
   }
-  else
+
+  // Only do collision checking if the check_collisions parameter is currently true.
+  velocity_scale_ = 1;
+  if (servo_params_.check_collisions)
   {
-    // If we are far from a collision, velocity_scale should be 1.
-    // If we are very close to a collision, velocity_scale should be ~zero.
-    // When scene_collision_proximity_threshold is breached, start decelerating exponentially.
-    if (scene_collision_distance_ < servo_params_.scene_collision_proximity_threshold)
+    // Update to the latest current state
+    current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState();
+    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_);
+    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());
+    self_collision_distance_ = collision_result_.distance;
+    collision_detected_ |= collision_result_.collision;
+    collision_result_.print();
+
+    // If we're definitely in collision, stop immediately
+    if (collision_detected_)
     {
-      // velocity_scale = e ^ k * (collision_distance - threshold)
-      // 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.
-      velocity_scale_ = std::min(velocity_scale_,
-                                 exp(scene_velocity_scale_coefficient_ *
-                                     (scene_collision_distance_ - servo_params_.scene_collision_proximity_threshold)));
+      velocity_scale_ = 0;
     }
-
-    if (self_collision_distance_ < servo_params_.self_collision_proximity_threshold)
+    else
     {
-      velocity_scale_ =
-          std::min(velocity_scale_, exp(self_velocity_scale_coefficient_ *
-                                        (self_collision_distance_ - servo_params_.self_collision_proximity_threshold)));
+      // If we are far from a collision, velocity_scale should be 1.
+      // If we are very close to a collision, velocity_scale should be ~zero.
+      // When scene_collision_proximity_threshold is breached, start decelerating exponentially.
+      if (scene_collision_distance_ < servo_params_.scene_collision_proximity_threshold)
+      {
+        // velocity_scale = e ^ k * (collision_distance - threshold)
+        // 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.
+        velocity_scale_ = std::min(
+            velocity_scale_, exp(scene_velocity_scale_coefficient_ *
+                                 (scene_collision_distance_ - servo_params_.scene_collision_proximity_threshold)));
+      }
+
+      if (self_collision_distance_ < servo_params_.self_collision_proximity_threshold)
+      {
+        velocity_scale_ = std::min(velocity_scale_,
+                                   exp(self_velocity_scale_coefficient_ *
+                                       (self_collision_distance_ - servo_params_.self_collision_proximity_threshold)));
+      }
     }
   }
 
-  // publish message
+  // Publish collision velocity scaling message.
   {
     auto msg = std::make_unique<std_msgs::msg::Float64>();
     msg->data = velocity_scale_;

moveit_ros/moveit_servo/src/servo.cpp

@@ -50,12 +50,12 @@ constexpr double ROBOT_STATE_WAIT_TIME = 10.0;  // seconds
 
 Servo::Servo(const rclcpp::Node::SharedPtr& node,
              const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
-             std::unique_ptr<const servo::ParamListener> servo_param_listener)
-  : servo_params_{ servo_param_listener->get_params() }
+             const std::shared_ptr<const servo::ParamListener>& servo_param_listener)
+  : servo_param_listener_(servo_param_listener)
+  , servo_params_{ servo_param_listener_->get_params() }
   , planning_scene_monitor_{ planning_scene_monitor }
-  , servo_calcs_{ node, planning_scene_monitor_, std::move(servo_param_listener) }
-  , collision_checker_{ node, servo_params_, planning_scene_monitor_ }
-
+  , servo_calcs_{ node, planning_scene_monitor_, servo_param_listener_ }
+  , collision_checker_{ node, planning_scene_monitor_, servo_param_listener_ }
 {
 }
 
@@ -68,6 +68,8 @@ void Servo::start()
     return;
   }
 
+  servo_params_ = servo_param_listener_->get_params();
+
   // Crunch the numbers in this timer
   servo_calcs_.start();
 

moveit_ros/moveit_servo/src/servo_calcs.cpp

@@ -74,9 +74,9 @@ int const THREAD_PRIORITY = 40;
 // Constructor for the class that handles servoing calculations
 ServoCalcs::ServoCalcs(const rclcpp::Node::SharedPtr& node,
                        const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
-                       std::unique_ptr<const servo::ParamListener> servo_param_listener)
+                       const std::shared_ptr<const servo::ParamListener>& servo_param_listener)
   : node_(node)
-  , servo_param_listener_(std::move(servo_param_listener))
+  , servo_param_listener_(servo_param_listener)
   , servo_params_(servo_param_listener_->get_params())
   , planning_scene_monitor_(planning_scene_monitor)
   , stop_requested_(true)