21 #if ROS_VERSION_MINIMUM(1, 14, 0)
27 #include <visualization_msgs/Marker.h>
36 m_mutex = PTHREAD_MUTEX_INITIALIZER;
41 ROS_WARN(
"Used default parameter for 'costmap_obstacle_treshold' [250].");
53 double footprint_update_frequency;
55 ROS_WARN(
"Used default parameter for 'footprint_update_frequency' [1.0 Hz].");
56 pnh_.
param(
"footprint_update_frequency", footprint_update_frequency, 1.0);
62 ROS_WARN(
"Used default parameter for 'global_frame' [/base_link]");
66 ROS_WARN(
"Used default parameter for 'robot_base_frame' [/base_link]");
70 ROS_WARN(
"Used default parameter for 'influence_radius' [1.5 m]");
77 ROS_WARN(
"Used default parameter for 'stop_threshold' [0.1 m]");
81 ROS_WARN(
"Used default parameter for 'obstacle_damping_dist' [5.0 m]");
86 ROS_WARN(
"obstacle_damping_dist <= stop_threshold -> robot will stop without deceleration!");
90 ROS_WARN(
"Used default parameter for 'use_circumscribed_threshold' [0.2 rad/s]");
94 ROS_WARN(
"Used default parameter for 'pot_ctrl_vmax' [0.6]");
98 ROS_WARN(
"Used default parameter for 'pot_ctrl_vtheta_max' [0.8]");
102 ROS_WARN(
"Used default parameter for 'pot_ctrl_kv' [1.0]");
106 ROS_WARN(
"Used default parameter for 'pot_ctrl_kp' [2.0]");
110 ROS_WARN(
"Used default parameter for 'pot_ctrl_virt_mass' [0.8]");
117 "You have set more than 4 points as robot_footprint, cob_collision_velocity_filter can deal only with rectangular footprints so far!");
121 ROS_WARN(
"Used default parameter for 'max_acceleration' [0.5, 0.5, 0.7]");
145 ROS_DEBUG(
"[cob_collision_velocity_filter] Initialized");
159 ROS_DEBUG_NAMED(
"joystickVelocityCB",
"[cob_collision_velocity_filter] Received command");
165 pthread_mutex_unlock(&
m_mutex);
177 ROS_DEBUG(
"[cob_collision_velocity_filter] Update footprint");
179 std::vector<geometry_msgs::Point> footprint;
190 for (
unsigned int i = 0; i < footprint.size(); i++)
202 pthread_mutex_unlock(&
m_mutex);
207 const cob_collision_velocity_filter::CollisionVelocityFilterConfig &config,
const uint32_t level)
216 ROS_WARN(
"obstacle_damping_dist <= stop_threshold -> robot will stop without deceleration!");
221 ROS_WARN(
"Not changing influence_radius since obstacle_damping_dist and/or stop_threshold is bigger!");
229 ROS_WARN(
"Turned off obstacle avoidance!");
230 pthread_mutex_unlock(&
m_mutex);
238 double vx_max, vy_max;
239 geometry_msgs::Twist cmd_vel, cmd_vel_in;
249 double vel_angle = atan2(cmd_vel.linear.y, cmd_vel.linear.x);
250 vx_max =
v_max_ * fabs(cos(vel_angle));
251 if (vx_max > fabs(cmd_vel.linear.x))
252 vx_max = fabs(cmd_vel.linear.x);
253 vy_max =
v_max_ * fabs(sin(vel_angle));
254 if (vy_max > fabs(cmd_vel.linear.y))
255 vy_max = fabs(cmd_vel.linear.y);
261 double vx_d, vy_d, vx_factor, vy_factor;
262 double kv_obst =
kv_, vx_max_obst = vx_max, vy_max_obst = vy_max;
267 if (vx_max_obst > vx_max)
268 vx_max_obst = vx_max;
269 else if (vx_max_obst < 0.0
f)
274 if (vy_max_obst > vy_max)
275 vy_max_obst = vy_max;
276 else if (vy_max_obst < 0.0
f)
283 vx_d =
kp_ / kv_obst * closest_obstacle_dist_x;
284 vy_d =
kp_ / kv_obst * closest_obstacle_dist_y;
285 vx_factor = vx_max_obst / sqrt(vy_d * vy_d + vx_d * vx_d);
286 vy_factor = vy_max_obst / sqrt(vy_d * vy_d + vx_d * vx_d);
292 F_x = -kv_obst *
vx_last_ + vx_factor *
kp_ * closest_obstacle_dist_x;
293 F_y = -kv_obst *
vy_last_ + vy_factor *
kp_ * closest_obstacle_dist_y;
301 if (fabs(cmd_vel.linear.x) > vx_max)
302 cmd_vel.linear.x =
sign(cmd_vel.linear.x) * vx_max;
303 if (fabs(cmd_vel.linear.y) > vy_max)
304 cmd_vel.linear.y =
sign(cmd_vel.linear.y) * vy_max;
312 if (fabs(cmd_vel.linear.x) > fabs(
vx_last_))
319 if (fabs(cmd_vel.linear.y) > fabs(
vy_last_))
338 pthread_mutex_unlock(&
m_mutex);
341 cmd_vel_in.angular.z, cmd_vel.angular.z);
360 pthread_mutex_unlock(&
m_mutex);
362 double cur_distance_to_center, cur_distance_to_border;
363 double obstacle_theta_robot, obstacle_delta_theta_robot, obstacle_dist_vel_dir;
364 bool cur_obstacle_relevant;
365 geometry_msgs::Point cur_obstacle_robot;
366 geometry_msgs::Point zero_position;
367 zero_position.x = 0.0f;
368 zero_position.y = 0.0f;
369 zero_position.z = 0.0f;
370 bool use_circumscribed =
true, use_tube =
true;
373 double corner_front_left, corner_rear_left, corner_rear_right, corner_front_right;
389 use_circumscribed =
false;
396 use_circumscribed =
false;
401 double velocity_angle = 0.0f, velocity_ortho_angle;
402 double corner_angle, delta_corner_angle;
403 double ortho_corner_dist;
404 double tube_left_border = 0.0f, tube_right_border = 0.0f;
405 double tube_left_origin = 0.0f, tube_right_origin = 0.0f;
406 double corner_dist, circumscribed_radius = 0.0f;
411 if (corner_dist > circumscribed_radius)
412 circumscribed_radius = corner_dist;
419 velocity_ortho_angle = velocity_angle + M_PI / 2.0f;
424 delta_corner_angle = velocity_ortho_angle - corner_angle;
426 ortho_corner_dist = cos(delta_corner_angle) * corner_dist;
428 if (ortho_corner_dist < tube_right_border)
430 tube_right_border = ortho_corner_dist;
431 tube_right_origin = sin(delta_corner_angle) * corner_dist;
433 else if (ortho_corner_dist > tube_left_border)
435 tube_left_border = ortho_corner_dist;
436 tube_left_origin = sin(delta_corner_angle) * corner_dist;
452 for (
unsigned int i = 0;
469 geometry_msgs::Point cell;
478 cur_obstacle_relevant =
false;
481 if (use_circumscribed && cur_distance_to_center <= circumscribed_radius)
483 cur_obstacle_robot = cell;
485 if (
obstacleValid(cur_obstacle_robot.x, cur_obstacle_robot.y))
487 cur_obstacle_relevant =
true;
488 obstacle_theta_robot = atan2(cur_obstacle_robot.y, cur_obstacle_robot.x);
495 cur_obstacle_robot = cell;
497 if (
obstacleValid(cur_obstacle_robot.x, cur_obstacle_robot.y))
499 obstacle_theta_robot = atan2(cur_obstacle_robot.y, cur_obstacle_robot.x);
500 obstacle_delta_theta_robot = obstacle_theta_robot - velocity_angle;
501 obstacle_dist_vel_dir = sin(obstacle_delta_theta_robot) * cur_distance_to_center;
503 if (obstacle_dist_vel_dir <= tube_left_border && obstacle_dist_vel_dir >= tube_right_border)
507 if (
sign(obstacle_dist_vel_dir) >= 0)
509 if (cos(obstacle_delta_theta_robot) * cur_distance_to_center >= tube_left_origin)
512 cur_obstacle_relevant =
true;
517 if (cos(obstacle_delta_theta_robot) * cur_distance_to_center >= tube_right_origin)
520 cur_obstacle_relevant =
true;
527 if (cur_obstacle_relevant)
534 if (obstacle_theta_robot >= corner_front_right && obstacle_theta_robot < corner_front_left)
537 cur_distance_to_border = cur_distance_to_center - fabs(
footprint_front_) / fabs(cos(obstacle_theta_robot));
539 else if (obstacle_theta_robot >= corner_front_left && obstacle_theta_robot < corner_rear_left)
542 cur_distance_to_border = cur_distance_to_center - fabs(
footprint_left_) / fabs(sin(obstacle_theta_robot));
544 else if (obstacle_theta_robot >= corner_rear_left || obstacle_theta_robot < corner_rear_right)
547 cur_distance_to_border = cur_distance_to_center - fabs(
footprint_rear_) / fabs(cos(obstacle_theta_robot));
552 cur_distance_to_border = cur_distance_to_center - fabs(
footprint_right_) / fabs(sin(obstacle_theta_robot));
567 pthread_mutex_unlock(&
m_mutex);
576 return sqrt(pow(a.x - b.x, 2) + pow(a.y - b.y, 2));
592 ROS_WARN(
"Found an obstacle inside robot_footprint: Skip!");
601 geometry_msgs::Twist stop_twist;
602 stop_twist.linear.x = 0.0f;
603 stop_twist.linear.y = 0.0f;
604 stop_twist.linear.z = 0.0f;
605 stop_twist.angular.x = 0.0f;
606 stop_twist.angular.y = 0.0f;
607 stop_twist.linear.z = 0.0f;
616 int main(
int argc,
char** argv)
619 ros::init(argc, argv,
"cob_collision_velocity_filter");
623 #if ROS_VERSION_MINIMUM(1, 14, 0)