cob_collision_velocity_filter.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9 
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 
19 
20 // Backwards compatibility of tf2 (tf for pre-kinetic) for costmap
21 #if ROS_VERSION_MINIMUM(1, 14, 0)
23 #else
24 #include <tf/transform_listener.h>
25 #endif
26 
27 #include <visualization_msgs/Marker.h>
28 
29 // Constructor
31 {
32  // create node handle
33  nh_ = ros::NodeHandle("");
34  pnh_ = ros::NodeHandle("~");
35 
36  m_mutex = PTHREAD_MUTEX_INITIALIZER;
37 
38  anti_collision_costmap_ = costmap;
39 
40  if (!pnh_.hasParam("costmap_obstacle_treshold"))
41  ROS_WARN("Used default parameter for 'costmap_obstacle_treshold' [250].");
42  pnh_.param("costmap_obstacle_treshold", costmap_obstacle_treshold_, 250);
43 
44  // implementation of topics to publish (command for base and list of relevant obstacles)
45  topic_pub_command_ = nh_.advertise<geometry_msgs::Twist>("command", 1);
46  topic_pub_relevant_obstacles_ = pnh_.advertise<nav_msgs::OccupancyGrid>("relevant_obstacles_grid", 1);
47 
48  // subscribe to twist-movement of teleop
49  joystick_velocity_sub_ = nh_.subscribe<geometry_msgs::Twist>("command_in", 10,
51 
52  // create Timer and call getFootprint Service periodically
53  double footprint_update_frequency;
54  if (!pnh_.hasParam("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);
57  get_footprint_timer_ = pnh_.createTimer(ros::Duration(1 / footprint_update_frequency),
59  // read parameters from parameter server
60  // parameters from costmap
61  if (!pnh_.hasParam("global_frame"))
62  ROS_WARN("Used default parameter for 'global_frame' [/base_link]");
63  pnh_.param("global_frame", global_frame_, std::string("/base_link"));
64 
65  if (!pnh_.hasParam("robot_base_frame"))
66  ROS_WARN("Used default parameter for 'robot_base_frame' [/base_link]");
67  pnh_.param("robot_base_frame", robot_frame_, std::string("/base_link"));
68 
69  if (!pnh_.hasParam("influence_radius"))
70  ROS_WARN("Used default parameter for 'influence_radius' [1.5 m]");
71  pnh_.param("influence_radius", influence_radius_, 1.5);
74 
75  // parameters for obstacle avoidance and velocity adjustment
76  if (!pnh_.hasParam("stop_threshold"))
77  ROS_WARN("Used default parameter for 'stop_threshold' [0.1 m]");
78  pnh_.param("stop_threshold", stop_threshold_, 0.10);
79 
80  if (!pnh_.hasParam("obstacle_damping_dist"))
81  ROS_WARN("Used default parameter for 'obstacle_damping_dist' [5.0 m]");
82  pnh_.param("obstacle_damping_dist", obstacle_damping_dist_, 5.0);
84  {
85  obstacle_damping_dist_ = stop_threshold_ + 0.01; // set to stop_threshold_+0.01 to avoid divide by zero error
86  ROS_WARN("obstacle_damping_dist <= stop_threshold -> robot will stop without deceleration!");
87  }
88 
89  if (!pnh_.hasParam("use_circumscribed_threshold"))
90  ROS_WARN("Used default parameter for 'use_circumscribed_threshold' [0.2 rad/s]");
91  pnh_.param("use_circumscribed_threshold", use_circumscribed_threshold_, 0.20);
92 
93  if (!pnh_.hasParam("pot_ctrl_vmax"))
94  ROS_WARN("Used default parameter for 'pot_ctrl_vmax' [0.6]");
95  pnh_.param("pot_ctrl_vmax", v_max_, 0.6);
96 
97  if (!pnh_.hasParam("pot_ctrl_vtheta_max"))
98  ROS_WARN("Used default parameter for 'pot_ctrl_vtheta_max' [0.8]");
99  pnh_.param("pot_ctrl_vtheta_max", vtheta_max_, 0.8);
100 
101  if (!pnh_.hasParam("pot_ctrl_kv"))
102  ROS_WARN("Used default parameter for 'pot_ctrl_kv' [1.0]");
103  pnh_.param("pot_ctrl_kv", kv_, 1.0);
104 
105  if (!pnh_.hasParam("pot_ctrl_kp"))
106  ROS_WARN("Used default parameter for 'pot_ctrl_kp' [2.0]");
107  pnh_.param("pot_ctrl_kp", kp_, 2.0);
108 
109  if (!pnh_.hasParam("pot_ctrl_virt_mass"))
110  ROS_WARN("Used default parameter for 'pot_ctrl_virt_mass' [0.8]");
111  pnh_.param("pot_ctrl_virt_mass", virt_mass_, 0.8);
112 
114 
115  if (robot_footprint_.size() > 4)
116  ROS_WARN(
117  "You have set more than 4 points as robot_footprint, cob_collision_velocity_filter can deal only with rectangular footprints so far!");
118 
119  // try to get the max_acceleration values from the parameter server
120  if (!pnh_.hasParam("max_acceleration"))
121  ROS_WARN("Used default parameter for 'max_acceleration' [0.5, 0.5, 0.7]");
122  XmlRpc::XmlRpcValue max_acc;
123  if (pnh_.getParam("max_acceleration", max_acc))
124  {
126  ax_max_ = (double)max_acc[0];
127  ay_max_ = (double)max_acc[1];
128  atheta_max_ = (double)max_acc[2];
129  }
130  else
131  {
132  ax_max_ = 0.5;
133  ay_max_ = 0.5;
134  atheta_max_ = 0.7;
135  }
136 
138  vx_last_ = 0.0;
139  vy_last_ = 0.0;
140  vtheta_last_ = 0.0;
141 
142  // dynamic reconfigure
143  dynCB_ = boost::bind(&CollisionVelocityFilter::dynamicReconfigureCB, this, _1, _2);
144  dyn_server_.setCallback(dynCB_);
145  ROS_DEBUG("[cob_collision_velocity_filter] Initialized");
146 }
147 
148 // Destructor
150 {
151 // costmap_thread_->interrupt();
152 // costmap_thread_->join();
153 }
154 
155 // joystick_velocityCB reads twist command from joystick
156 void CollisionVelocityFilter::joystickVelocityCB(const geometry_msgs::Twist::ConstPtr &twist)
157 {
158  //std::cout << "received command" << std::endl;
159  ROS_DEBUG_NAMED("joystickVelocityCB", "[cob_collision_velocity_filter] Received command");
160  pthread_mutex_lock(&m_mutex);
161 
162  robot_twist_linear_ = twist->linear;
163  robot_twist_angular_ = twist->angular;
164 
165  pthread_mutex_unlock(&m_mutex);
166 
167  // check for relevant obstacles
168  obstacleHandler();
169  // stop if we are about to run in an obstacle
171 
172 }
173 
174 // timer callback for periodically checking footprint
176 {
177  ROS_DEBUG("[cob_collision_velocity_filter] Update footprint");
178  // adjust footprint
179  std::vector<geometry_msgs::Point> footprint;
181 
182  pthread_mutex_lock(&m_mutex);
183 
188 
189  robot_footprint_ = footprint;
190  for (unsigned int i = 0; i < footprint.size(); i++)
191  {
192  if (footprint[i].x > footprint_front_)
193  footprint_front_ = footprint[i].x;
194  if (footprint[i].x < footprint_rear_)
195  footprint_rear_ = footprint[i].x;
196  if (footprint[i].y > footprint_left_)
197  footprint_left_ = footprint[i].y;
198  if (footprint[i].y < footprint_right_)
199  footprint_right_ = footprint[i].y;
200  }
201 
202  pthread_mutex_unlock(&m_mutex);
203 
204 }
205 
207  const cob_collision_velocity_filter::CollisionVelocityFilterConfig &config, const uint32_t level)
208 {
209  pthread_mutex_lock(&m_mutex);
210 
211  stop_threshold_ = config.stop_threshold;
212  obstacle_damping_dist_ = config.obstacle_damping_dist;
214  {
215  obstacle_damping_dist_ = stop_threshold_ + 0.01; // set to stop_threshold_+0.01 to avoid divide by zero error
216  ROS_WARN("obstacle_damping_dist <= stop_threshold -> robot will stop without deceleration!");
217  }
218 
219  if (obstacle_damping_dist_ > config.influence_radius || stop_threshold_ > config.influence_radius)
220  {
221  ROS_WARN("Not changing influence_radius since obstacle_damping_dist and/or stop_threshold is bigger!");
222  }
223  else
224  {
225  influence_radius_ = config.influence_radius;
226  }
227 
228  if (stop_threshold_ <= 0.0 || influence_radius_ <= 0.0)
229  ROS_WARN("Turned off obstacle avoidance!");
230  pthread_mutex_unlock(&m_mutex);
231 }
232 
233 // sets corrected velocity of joystick command
235 {
236 
237  double dt;
238  double vx_max, vy_max;
239  geometry_msgs::Twist cmd_vel, cmd_vel_in;
240 
241  cmd_vel_in.linear = robot_twist_linear_;
242  cmd_vel_in.angular = robot_twist_angular_;
243 
244  cmd_vel.linear = robot_twist_linear_;
245  cmd_vel.angular = robot_twist_angular_;
246  dt = ros::Time::now().toSec() - last_time_;
248 
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);
256 
257  //Slow down in any way while approximating an obstacle:
259  {
260  double F_x, F_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;
263 
264  //implementation for linear decrease of v_max:
265  double obstacle_linear_slope_x = vx_max / (obstacle_damping_dist_ - stop_threshold_);
266  vx_max_obst = (closest_obstacle_dist_ - stop_threshold_ + stop_threshold_ / 10.0f) * obstacle_linear_slope_x;
267  if (vx_max_obst > vx_max)
268  vx_max_obst = vx_max;
269  else if (vx_max_obst < 0.0f)
270  vx_max_obst = 0.0f;
271 
272  double obstacle_linear_slope_y = vy_max / (obstacle_damping_dist_ - stop_threshold_);
273  vy_max_obst = (closest_obstacle_dist_ - stop_threshold_ + stop_threshold_ / 10.0f) * obstacle_linear_slope_y;
274  if (vy_max_obst > vy_max)
275  vy_max_obst = vy_max;
276  else if (vy_max_obst < 0.0f)
277  vy_max_obst = 0.0f;
278 
279  //Translational movement
280  //calculation of v factor to limit maxspeed
281  double closest_obstacle_dist_x = closest_obstacle_dist_ * cos(closest_obstacle_angle_);
282  double closest_obstacle_dist_y = closest_obstacle_dist_ * sin(closest_obstacle_angle_);
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);
287  if (vx_factor > 1.0)
288  vx_factor = 1.0;
289  if (vy_factor > 1.0)
290  vy_factor = 1.0;
291 
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;
294 
295  cmd_vel.linear.x = vx_last_ + F_x / virt_mass_ * dt;
296  cmd_vel.linear.y = vy_last_ + F_y / virt_mass_ * dt;
297 
298  }
299 
300  // make sure, the computed and commanded velocities are not greater than the specified max velocities
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;
305  if (fabs(cmd_vel.angular.z) > vtheta_max_)
306  cmd_vel.angular.z = sign(cmd_vel.angular.z) * vtheta_max_;
307 
308  // limit acceleration:
309  // only acceleration (in terms of speeding up in any direction) is limited,
310  // deceleration (in terms of slowing down) is handeled either by cob_teleop or the potential field
311  // like slow-down behaviour above
312  if (fabs(cmd_vel.linear.x) > fabs(vx_last_))
313  {
314  if ((cmd_vel.linear.x - vx_last_) / dt > ax_max_)
315  cmd_vel.linear.x = vx_last_ + ax_max_ * dt;
316  else if ((cmd_vel.linear.x - vx_last_) / dt < -ax_max_)
317  cmd_vel.linear.x = vx_last_ - ax_max_ * dt;
318  }
319  if (fabs(cmd_vel.linear.y) > fabs(vy_last_))
320  {
321  if ((cmd_vel.linear.y - vy_last_) / dt > ay_max_)
322  cmd_vel.linear.y = vy_last_ + ay_max_ * dt;
323  else if ((cmd_vel.linear.y - vy_last_) / dt < -ay_max_)
324  cmd_vel.linear.y = vy_last_ - ay_max_ * dt;
325  }
326  if (fabs(cmd_vel.angular.z) > fabs(vtheta_last_))
327  {
328  if ((cmd_vel.angular.z - vtheta_last_) / dt > atheta_max_)
329  cmd_vel.angular.z = vtheta_last_ + atheta_max_ * dt;
330  else if ((cmd_vel.angular.z - vtheta_last_) / dt < -atheta_max_)
331  cmd_vel.angular.z = vtheta_last_ - atheta_max_ * dt;
332  }
333 
334  pthread_mutex_lock(&m_mutex);
335  vx_last_ = cmd_vel.linear.x;
336  vy_last_ = cmd_vel.linear.y;
337  vtheta_last_ = cmd_vel.angular.z;
338  pthread_mutex_unlock(&m_mutex);
339 
340  velocity_limited_marker_.publishMarkers(cmd_vel_in.linear.x, cmd_vel.linear.x, cmd_vel_in.linear.y, cmd_vel.linear.y,
341  cmd_vel_in.angular.z, cmd_vel.angular.z);
342 
343  // if closest obstacle is within stop_threshold, then do not move
345  {
346  stopMovement();
347  }
348  else
349  {
350  // publish adjusted velocity
351  topic_pub_command_.publish(cmd_vel);
352  }
353  return;
354 }
355 
357 {
358  pthread_mutex_lock(&m_mutex);
360  pthread_mutex_unlock(&m_mutex);
361 
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;
371 
372  //Calculate corner angles in robot_frame:
373  double corner_front_left, corner_rear_left, corner_rear_right, corner_front_right;
374  corner_front_left = atan2(footprint_left_, footprint_front_);
375  corner_rear_left = atan2(footprint_left_, footprint_rear_);
376  corner_rear_right = atan2(footprint_right_, footprint_rear_);
377  corner_front_right = atan2(footprint_right_, footprint_front_);
378 
379  //Decide, whether circumscribed or tube argument should be used for filtering:
380  if (fabs(robot_twist_linear_.x) <= 0.005f && fabs(robot_twist_linear_.y) <= 0.005f)
381  {
382  use_tube = false;
383  //disable tube filter at very slow velocities
384  }
385  if (!use_tube)
386  {
387  if (fabs(robot_twist_angular_.z) <= 0.01f)
388  {
389  use_circumscribed = false;
390  } //when tube filter inactive, start circumscribed filter at very low rot-velocities
391  }
392  else
393  {
395  {
396  use_circumscribed = false;
397  } //when tube filter running, disable circum-filter in a wider range of rot-velocities
398  }
399 
400  //Calculation of tube in driving-dir considered for obstacle avoidence
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;
407 
408  for (unsigned i = 0; i < robot_footprint_.size(); i++)
409  {
410  corner_dist = sqrt(robot_footprint_[i].x * robot_footprint_[i].x + robot_footprint_[i].y * robot_footprint_[i].y);
411  if (corner_dist > circumscribed_radius)
412  circumscribed_radius = corner_dist;
413  }
414 
415  if (use_tube)
416  {
417  //use commanded vel-value for vel-vector direction.. ?
418  velocity_angle = atan2(robot_twist_linear_.y, robot_twist_linear_.x);
419  velocity_ortho_angle = velocity_angle + M_PI / 2.0f;
420 
421  for (unsigned i = 0; i < robot_footprint_.size(); i++)
422  {
423  corner_angle = atan2(robot_footprint_[i].y, robot_footprint_[i].x);
424  delta_corner_angle = velocity_ortho_angle - corner_angle;
425  corner_dist = sqrt(robot_footprint_[i].x * robot_footprint_[i].x + robot_footprint_[i].y * robot_footprint_[i].y);
426  ortho_corner_dist = cos(delta_corner_angle) * corner_dist;
427 
428  if (ortho_corner_dist < tube_right_border)
429  {
430  tube_right_border = ortho_corner_dist;
431  tube_right_origin = sin(delta_corner_angle) * corner_dist;
432  }
433  else if (ortho_corner_dist > tube_left_border)
434  {
435  tube_left_border = ortho_corner_dist;
436  tube_left_origin = sin(delta_corner_angle) * corner_dist;
437  }
438  }
439  }
440 
441  //find relevant obstacles
442  pthread_mutex_lock(&m_mutex);
443  relevant_obstacles_.header.frame_id = global_frame_;
444  relevant_obstacles_.header.stamp = ros::Time::now();
448  relevant_obstacles_.info.origin.position.x = anti_collision_costmap_->getCostmap()->getOriginX();
449  relevant_obstacles_.info.origin.position.y = anti_collision_costmap_->getCostmap()->getOriginY();
450  relevant_obstacles_.info.origin.orientation.w = 1.0;
451  relevant_obstacles_.data.clear();
452  for (unsigned int i = 0;
453  i
454  < anti_collision_costmap_->getCostmap()->getSizeInCellsX()
456  {
457  if (anti_collision_costmap_->getCostmap()->getCharMap()[i] == -1)
458  {
459  relevant_obstacles_.data.push_back(-1);
460  }
462  { // add trshold
463  relevant_obstacles_.data.push_back(0);
464  }
465  else
466  {
467 
468  // calculate cell in 2D space where robot is is point (0, 0)
469  geometry_msgs::Point cell;
470  cell.x = (i % (int)(anti_collision_costmap_->getCostmap()->getSizeInCellsX()))
473  cell.y = (i / (int)(anti_collision_costmap_->getCostmap()->getSizeInCellsX()))
476  cell.z = 0.0f;
477 
478  cur_obstacle_relevant = false;
479  cur_distance_to_center = getDistance2d(zero_position, cell);
480  //check whether current obstacle lies inside the circumscribed_radius of the robot -> prevent collisions while rotating
481  if (use_circumscribed && cur_distance_to_center <= circumscribed_radius)
482  {
483  cur_obstacle_robot = cell;
484 
485  if (obstacleValid(cur_obstacle_robot.x, cur_obstacle_robot.y))
486  {
487  cur_obstacle_relevant = true;
488  obstacle_theta_robot = atan2(cur_obstacle_robot.y, cur_obstacle_robot.x);
489  }
490 
491  //for each obstacle, now check whether it lies in the tube or not:
492  }
493  else if (use_tube && cur_distance_to_center < influence_radius_)
494  {
495  cur_obstacle_robot = cell;
496 
497  if (obstacleValid(cur_obstacle_robot.x, cur_obstacle_robot.y))
498  {
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;
502 
503  if (obstacle_dist_vel_dir <= tube_left_border && obstacle_dist_vel_dir >= tube_right_border)
504  {
505  //found obstacle that lies inside of observation tube
506 
507  if (sign(obstacle_dist_vel_dir) >= 0)
508  {
509  if (cos(obstacle_delta_theta_robot) * cur_distance_to_center >= tube_left_origin)
510  {
511  //relevant obstacle in tube found
512  cur_obstacle_relevant = true;
513  }
514  }
515  else
516  { // obstacle in right part of tube
517  if (cos(obstacle_delta_theta_robot) * cur_distance_to_center >= tube_right_origin)
518  {
519  //relevant obstacle in tube found
520  cur_obstacle_relevant = true;
521  }
522  }
523  }
524  }
525  }
526 
527  if (cur_obstacle_relevant)
528  {
529  ROS_DEBUG_STREAM_NAMED("obstacleHandler", "[cob_collision_velocity_filter] Detected an obstacle");
530  //relevant obstacle in tube found
531  relevant_obstacles_.data.push_back(100);
532 
533  //now calculate distance of current, relevant obstacle to robot
534  if (obstacle_theta_robot >= corner_front_right && obstacle_theta_robot < corner_front_left)
535  {
536  //obstacle in front:
537  cur_distance_to_border = cur_distance_to_center - fabs(footprint_front_) / fabs(cos(obstacle_theta_robot));
538  }
539  else if (obstacle_theta_robot >= corner_front_left && obstacle_theta_robot < corner_rear_left)
540  {
541  //obstacle left:
542  cur_distance_to_border = cur_distance_to_center - fabs(footprint_left_) / fabs(sin(obstacle_theta_robot));
543  }
544  else if (obstacle_theta_robot >= corner_rear_left || obstacle_theta_robot < corner_rear_right)
545  {
546  //obstacle in rear:
547  cur_distance_to_border = cur_distance_to_center - fabs(footprint_rear_) / fabs(cos(obstacle_theta_robot));
548  }
549  else
550  {
551  //obstacle right:
552  cur_distance_to_border = cur_distance_to_center - fabs(footprint_right_) / fabs(sin(obstacle_theta_robot));
553  }
554 
555  if (cur_distance_to_border < closest_obstacle_dist_)
556  {
557  closest_obstacle_dist_ = cur_distance_to_border;
558  closest_obstacle_angle_ = obstacle_theta_robot;
559  }
560  }
561  else
562  {
563  relevant_obstacles_.data.push_back(0);
564  }
565  }
566  }
567  pthread_mutex_unlock(&m_mutex);
568 
570  ROS_DEBUG_STREAM_NAMED("obstacleHandler",
571  "[cob_collision_velocity_filter] closest_obstacle_dist_ = " << closest_obstacle_dist_);
572 }
573 
574 double CollisionVelocityFilter::getDistance2d(geometry_msgs::Point a, geometry_msgs::Point b)
575 {
576  return sqrt(pow(a.x - b.x, 2) + pow(a.y - b.y, 2));
577 }
578 
580 {
581  if (x >= 0.0f)
582  return 1.0f;
583  else
584  return -1.0f;
585 }
586 
587 bool CollisionVelocityFilter::obstacleValid(double x_obstacle, double y_obstacle)
588 {
589  if (x_obstacle < footprint_front_ && x_obstacle > footprint_rear_ && y_obstacle > footprint_right_
590  && y_obstacle < footprint_left_)
591  {
592  ROS_WARN("Found an obstacle inside robot_footprint: Skip!");
593  return false;
594  }
595 
596  return true;
597 }
598 
600 {
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;
608  topic_pub_command_.publish(stop_twist);
609  vx_last_ = 0.0;
610  vy_last_ = 0.0;
611  vtheta_last_ = 0.0;
612 }
613 
614 //#######################
615 //#### main programm ####
616 int main(int argc, char** argv)
617 {
618  // initialize ROS, spezify name of node
619  ros::init(argc, argv, "cob_collision_velocity_filter");
620 
621  // create nodeClass
622 
623 #if ROS_VERSION_MINIMUM(1, 14, 0)
625  tf2_ros::TransformListener tf_listener(tf);
626 #else
628 #endif
629  costmap_2d::Costmap2DROS* costmap = new costmap_2d::Costmap2DROS("anti_collision_costmap", tf);
630  CollisionVelocityFilter collisionVelocityFilter(costmap);
631 
632  ros::spin();
633 
634  return 0;
635 }
636 
CollisionVelocityFilter::dynCB_
dynamic_reconfigure::Server< cob_collision_velocity_filter::CollisionVelocityFilterConfig >::CallbackType dynCB_
Definition: cob_collision_velocity_filter.h:120
CollisionVelocityFilter::use_circumscribed_threshold_
double use_circumscribed_threshold_
Definition: cob_collision_velocity_filter.h:185
CollisionVelocityFilter::obstacleValid
bool obstacleValid(double x_obstacle, double y_obstacle)
checks if obstacle lies already within footprint -> this is ignored due to sensor readings of the hul...
Definition: cob_collision_velocity_filter.cpp:587
CollisionVelocityFilter::getDistance2d
double getDistance2d(geometry_msgs::Point a, geometry_msgs::Point b)
computes distance between two points
Definition: cob_collision_velocity_filter.cpp:574
CollisionVelocityFilter::vy_last_
double vy_last_
Definition: cob_collision_velocity_filter.h:191
CollisionVelocityFilter::vtheta_last_
double vtheta_last_
Definition: cob_collision_velocity_filter.h:191
CollisionVelocityFilter::sign
double sign(double x)
returns the sign of x
Definition: cob_collision_velocity_filter.cpp:579
CollisionVelocityFilter::ay_max_
double ay_max_
Definition: cob_collision_velocity_filter.h:177
CollisionVelocityFilter::footprint_right_initial_
double footprint_right_initial_
Definition: cob_collision_velocity_filter.h:182
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
main
int main(int argc, char **argv)
Definition: cob_collision_velocity_filter.cpp:616
ROS_DEBUG_STREAM_NAMED
#define ROS_DEBUG_STREAM_NAMED(name, args)
CollisionVelocityFilter::robot_twist_angular_
geometry_msgs::Vector3 robot_twist_angular_
Definition: cob_collision_velocity_filter.h:175
cob_collision_velocity_filter::VelocityLimitedMarker::publishMarkers
void publishMarkers(double vel_x_desired, double vel_x_actual, double vel_y_desired, double vel_y_actual, double vel_theta_desired, double vel_theta_actual)
Creates all the markers, the method is called from the constructor.
Definition: velocity_limited_marker.cpp:253
ros::NodeHandle::getParam
bool getParam(const std::string &key, bool &b) const
CollisionVelocityFilter::vx_last_
double vx_last_
Definition: cob_collision_velocity_filter.h:191
CollisionVelocityFilter::joystickVelocityCB
void joystickVelocityCB(const geometry_msgs::Twist::ConstPtr &twist)
reads twist command from teleop device (joystick, teleop_keyboard, ...) and calls functions for colli...
Definition: cob_collision_velocity_filter.cpp:156
CollisionVelocityFilter::velocity_limited_marker_
cob_collision_velocity_filter::VelocityLimitedMarker velocity_limited_marker_
Definition: cob_collision_velocity_filter.h:195
CollisionVelocityFilter::topic_pub_command_
ros::Publisher topic_pub_command_
declaration of publisher
Definition: cob_collision_velocity_filter.h:112
TimeBase< Time, Duration >::toSec
double toSec() const
CollisionVelocityFilter::robot_footprint_
std::vector< geometry_msgs::Point > robot_footprint_
Definition: cob_collision_velocity_filter.h:180
costmap_2d::Costmap2D::getCharMap
unsigned char * getCharMap() const
costmap_2d::Costmap2D::getOriginX
double getOriginX() const
CollisionVelocityFilter::footprint_left_
double footprint_left_
Definition: cob_collision_velocity_filter.h:181
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
CollisionVelocityFilter::closest_obstacle_dist_
double closest_obstacle_dist_
Definition: cob_collision_velocity_filter.h:186
f
f
CollisionVelocityFilter::ax_max_
double ax_max_
Definition: cob_collision_velocity_filter.h:177
CollisionVelocityFilter::joystick_velocity_sub_
ros::Subscriber joystick_velocity_sub_
declaration of subscriber
Definition: cob_collision_velocity_filter.h:116
tf2_ros::TransformListener
CollisionVelocityFilter::getFootprint
void getFootprint(const ros::TimerEvent &)
Timer callback, calls GetFootprint Service and adjusts footprint.
Definition: cob_collision_velocity_filter.cpp:175
CollisionVelocityFilter::kv_
double kv_
Definition: cob_collision_velocity_filter.h:190
costmap_2d::Costmap2DROS::getRobotFootprint
const std::vector< geometry_msgs::Point > & getRobotFootprint() const noexcept
CollisionVelocityFilter::anti_collision_costmap_
costmap_2d::Costmap2DROS * anti_collision_costmap_
Definition: cob_collision_velocity_filter.h:125
CollisionVelocityFilter::stop_threshold_
double stop_threshold_
Definition: cob_collision_velocity_filter.h:185
ROS_DEBUG
#define ROS_DEBUG(...)
CollisionVelocityFilter::kp_
double kp_
Definition: cob_collision_velocity_filter.h:190
ROS_DEBUG_NAMED
#define ROS_DEBUG_NAMED(name,...)
CollisionVelocityFilter::dynamicReconfigureCB
void dynamicReconfigureCB(const cob_collision_velocity_filter::CollisionVelocityFilterConfig &config, const uint32_t level)
Dynamic reconfigure callback.
Definition: cob_collision_velocity_filter.cpp:206
CollisionVelocityFilter::virt_mass_
double virt_mass_
Definition: cob_collision_velocity_filter.h:192
CollisionVelocityFilter::dyn_server_
dynamic_reconfigure::Server< cob_collision_velocity_filter::CollisionVelocityFilterConfig > dyn_server_
dynamic reconfigure
Definition: cob_collision_velocity_filter.h:119
costmap_2d::Costmap2D::getSizeInCellsY
unsigned int getSizeInCellsY() const
CollisionVelocityFilter::footprint_front_
double footprint_front_
Definition: cob_collision_velocity_filter.h:181
ros::NodeHandle::hasParam
bool hasParam(const std::string &key) const
CollisionVelocityFilter::vtheta_max_
double vtheta_max_
Definition: cob_collision_velocity_filter.h:176
ROS_WARN
#define ROS_WARN(...)
tf2_ros::Buffer
CollisionVelocityFilter::costmap_obstacle_treshold_
int costmap_obstacle_treshold_
Definition: cob_collision_velocity_filter.h:169
CollisionVelocityFilter::stopMovement
void stopMovement()
stops movement of the robot
Definition: cob_collision_velocity_filter.cpp:599
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
ros::TimerEvent
CollisionVelocityFilter::closest_obstacle_angle_
double closest_obstacle_angle_
Definition: cob_collision_velocity_filter.h:186
CollisionVelocityFilter::footprint_rear_initial_
double footprint_rear_initial_
Definition: cob_collision_velocity_filter.h:182
CollisionVelocityFilter::robot_twist_linear_
geometry_msgs::Vector3 robot_twist_linear_
Definition: cob_collision_velocity_filter.h:175
CollisionVelocityFilter::obstacle_damping_dist_
double obstacle_damping_dist_
Definition: cob_collision_velocity_filter.h:185
XmlRpc::XmlRpcValue::getType
const Type & getType() const
CollisionVelocityFilter::footprint_front_initial_
double footprint_front_initial_
Definition: cob_collision_velocity_filter.h:182
CollisionVelocityFilter::~CollisionVelocityFilter
~CollisionVelocityFilter()
Destructor.
Definition: cob_collision_velocity_filter.cpp:149
XmlRpc::XmlRpcValue::TypeArray
TypeArray
CollisionVelocityFilter
checks for obstacles in driving direction and stops the robot
Definition: cob_collision_velocity_filter.h:61
transform_listener.h
costmap_2d::Costmap2D::getOriginY
double getOriginY() const
CollisionVelocityFilter::last_time_
double last_time_
Definition: cob_collision_velocity_filter.h:189
CollisionVelocityFilter::nh_
ros::NodeHandle nh_
create a handle for this node, initialize node
Definition: cob_collision_velocity_filter.h:103
CollisionVelocityFilter::pnh_
ros::NodeHandle pnh_
Definition: cob_collision_velocity_filter.h:106
CollisionVelocityFilter::footprint_left_initial_
double footprint_left_initial_
Definition: cob_collision_velocity_filter.h:182
CollisionVelocityFilter::m_mutex
pthread_mutex_t m_mutex
Definition: cob_collision_velocity_filter.h:166
CollisionVelocityFilter::footprint_right_
double footprint_right_
Definition: cob_collision_velocity_filter.h:181
tf::TransformListener
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
costmap_2d::Costmap2D::getResolution
double getResolution() const
CollisionVelocityFilter::global_frame_
std::string global_frame_
Definition: cob_collision_velocity_filter.h:172
CollisionVelocityFilter::robot_frame_
std::string robot_frame_
Definition: cob_collision_velocity_filter.h:172
CollisionVelocityFilter::footprint_rear_
double footprint_rear_
Definition: cob_collision_velocity_filter.h:181
CollisionVelocityFilter::performControllerStep
void performControllerStep()
checks distance to obstacles in driving direction and slows down/stops robot and publishes command ve...
Definition: cob_collision_velocity_filter.cpp:234
ros::spin
ROSCPP_DECL void spin()
tf
CollisionVelocityFilter::obstacleHandler
void obstacleHandler()
checks for obstacles in driving direction of the robot (rotation included) and publishes relevant obs...
Definition: cob_collision_velocity_filter.cpp:356
CollisionVelocityFilter::get_footprint_timer_
ros::Timer get_footprint_timer_
Timer for periodically calling GetFootprint Service.
Definition: cob_collision_velocity_filter.h:109
CollisionVelocityFilter::v_max_
double v_max_
Definition: cob_collision_velocity_filter.h:176
CollisionVelocityFilter::atheta_max_
double atheta_max_
Definition: cob_collision_velocity_filter.h:177
costmap_2d::Costmap2DROS::getCostmap
Costmap2D * getCostmap() const
costmap_2d::Costmap2D::getSizeInCellsX
unsigned int getSizeInCellsX() const
CollisionVelocityFilter::relevant_obstacles_
nav_msgs::OccupancyGrid relevant_obstacles_
Definition: cob_collision_velocity_filter.h:184
ROS_ASSERT
#define ROS_ASSERT(cond)
ros::NodeHandle::createTimer
Timer createTimer(Duration period, const TimerCallback &callback, bool oneshot=false, bool autostart=true) const
CollisionVelocityFilter::topic_pub_relevant_obstacles_
ros::Publisher topic_pub_relevant_obstacles_
Definition: cob_collision_velocity_filter.h:113
ros::Duration
CollisionVelocityFilter::influence_radius_
double influence_radius_
Definition: cob_collision_velocity_filter.h:185
costmap_2d::Costmap2DROS
XmlRpc::XmlRpcValue
ros::NodeHandle
cob_collision_velocity_filter.h
ros::Time::now
static Time now()
CollisionVelocityFilter::CollisionVelocityFilter
CollisionVelocityFilter(costmap_2d::Costmap2DROS *costmap)
Constructor.
Definition: cob_collision_velocity_filter.cpp:30


cob_collision_velocity_filter
Author(s): Matthias Gruhler, Michal Spanel
autogenerated on Thu Feb 22 2024 03:43:55