diff --git a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h index 742f399fe3a..c6d6ab82440 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h +++ b/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, + std::shared_ptr servo_param_listener); ~CollisionCheck() { @@ -83,6 +84,8 @@ class CollisionCheck // Pointer to the ROS node const std::shared_ptr node_; + // Servo parameters + std::shared_ptr 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_; diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo.h b/moveit_ros/moveit_servo/include/moveit_servo/servo.h index 244a441080e..1938b4fda4a 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo.h +++ b/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 servo_param_listener); + std::shared_ptr servo_param_listener); /** \brief Start servo node */ void start(); @@ -88,7 +88,10 @@ class Servo friend class ServoFixture; private: + // Servo parameters + std::shared_ptr servo_param_listener_; servo::Params servo_params_; + // Pointer to the collision environment planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h index 60a7c477f73..fc0bc0a8dd0 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h +++ b/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 servo_param_listener); + std::shared_ptr servo_param_listener); ~ServoCalcs(); @@ -200,7 +200,7 @@ class ServoCalcs std::shared_ptr node_; // Servo parameters - std::unique_ptr servo_param_listener_; + std::shared_ptr servo_param_listener_; servo::Params servo_params_; // Pointer to the collision environment diff --git a/moveit_ros/moveit_servo/src/collision_check.cpp b/moveit_ros/moveit_servo/src/collision_check.cpp index 7c8dc5f84c1..d5975c617de 100644 --- a/moveit_ros/moveit_servo/src/collision_check.cpp +++ b/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, + std::shared_ptr servo_param_listener) : node_(node) - , servo_params_(servo_params) + , servo_param_listener_(std::move(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(); msg->data = velocity_scale_; diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index 2b9bdd091ca..9a7f7b04aa5 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/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 servo_param_listener) - : servo_params_{ servo_param_listener->get_params() } + std::shared_ptr servo_param_listener) + : servo_param_listener_(std::move(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(); diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp index be284ea9070..3f450bb746f 100644 --- a/moveit_ros/moveit_servo/src/servo_calcs.cpp +++ b/moveit_ros/moveit_servo/src/servo_calcs.cpp @@ -74,7 +74,7 @@ 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 servo_param_listener) + std::shared_ptr servo_param_listener) : node_(node) , servo_param_listener_(std::move(servo_param_listener)) , servo_params_(servo_param_listener_->get_params())