41#include <rclcpp/rclcpp.hpp>
55 const servo::Params& servo_params, std::atomic<double>& collision_velocity_scale)
56 : servo_params_(servo_params)
59 , collision_velocity_scale_(collision_velocity_scale)
61 scene_collision_request_.
distance =
true;
62 scene_collision_request_.
group_name = servo_params.move_group_name;
64 self_collision_request_.
distance =
true;
65 self_collision_request_.
group_name = servo_params.move_group_name;
70 stop_requested_ =
false;
71 if (!monitor_thread_.joinable())
73 monitor_thread_ = std::thread(&CollisionMonitor::checkCollisions,
this);
74 RCLCPP_INFO_STREAM(getLogger(),
"Collision monitor started");
78 RCLCPP_ERROR_STREAM(getLogger(),
"Collision monitor could not be started");
84 stop_requested_ =
true;
85 if (monitor_thread_.joinable())
87 monitor_thread_.join();
89 RCLCPP_INFO_STREAM(getLogger(),
"Collision monitor stopped");
92void CollisionMonitor::checkCollisions()
94 rclcpp::WallRate rate(servo_params_.collision_check_rate);
96 bool approaching_self_collision, approaching_scene_collision;
97 double self_collision_threshold_delta, scene_collision_threshold_delta;
98 double self_collision_scale, scene_collision_scale;
99 const double log_val = -log(0.001);
101 while (rclcpp::ok() && !stop_requested_)
103 const double self_velocity_scale_coefficient{ log_val / servo_params_.self_collision_proximity_threshold };
104 const double scene_velocity_scale_coefficient{ log_val / servo_params_.scene_collision_proximity_threshold };
106 if (servo_params_.check_collisions)
110 robot_state_ = planning_scene_monitor_->getPlanningScene()->getCurrentState();
118 scene_collision_result_.
clear();
119 locked_scene->getCollisionEnv()->checkRobotCollision(scene_collision_request_, scene_collision_result_,
120 robot_state_, locked_scene->getAllowedCollisionMatrix());
123 self_collision_result_.
clear();
124 locked_scene->getCollisionEnvUnpadded()->checkSelfCollision(
125 self_collision_request_, self_collision_result_, robot_state_, locked_scene->getAllowedCollisionMatrix());
140 collision_velocity_scale_ = 0.0;
144 self_collision_scale = scene_collision_scale = 1.0;
146 approaching_scene_collision =
147 scene_collision_result_.
distance < servo_params_.scene_collision_proximity_threshold;
148 approaching_self_collision = self_collision_result_.
distance < servo_params_.self_collision_proximity_threshold;
150 if (approaching_scene_collision)
152 scene_collision_threshold_delta =
153 scene_collision_result_.
distance - servo_params_.scene_collision_proximity_threshold;
154 scene_collision_scale = std::exp(scene_velocity_scale_coefficient * scene_collision_threshold_delta);
157 if (approaching_self_collision)
159 self_collision_threshold_delta =
160 self_collision_result_.
distance - servo_params_.self_collision_proximity_threshold;
161 self_collision_scale = std::exp(self_velocity_scale_coefficient * self_collision_threshold_delta);
165 collision_velocity_scale_ = std::min(scene_collision_scale, self_collision_scale);
171 collision_velocity_scale_ = 1.0;
void updateCollisionBodyTransforms()
Update the transforms for the collision bodies. This call is needed before calling collision checking...
CollisionMonitor(const planning_scene_monitor::PlanningSceneMonitorPtr &planning_scene_monitor, const servo::Params &servo_params, std::atomic< double > &collision_velocity_scale)
This is a convenience class for obtaining access to an instance of a locked PlanningScene.
rclcpp::Logger getLogger()
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.
std::string group_name
The group name to check collisions for (optional; if empty, assume the complete robot)....
bool distance
If true, compute proximity distance.
double distance
Closest distance between two bodies.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW void clear()
Clear a previously stored result.
bool collision
True if collision was found, false otherwise.