00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #include <cob_collision_velocity_filter.h>
00019
00020 #include <visualization_msgs/Marker.h>
00021
00022
00023 CollisionVelocityFilter::CollisionVelocityFilter(costmap_2d::Costmap2DROS * costmap)
00024 {
00025
00026 nh_ = ros::NodeHandle("");
00027 pnh_ = ros::NodeHandle("~");
00028
00029 m_mutex = PTHREAD_MUTEX_INITIALIZER;
00030
00031 anti_collision_costmap_ = costmap;
00032
00033 pnh_.param("costmap_obstacle_treshold", costmap_obstacle_treshold_, 250);
00034
00035
00036 topic_pub_command_ = nh_.advertise<geometry_msgs::Twist>("command", 1);
00037 topic_pub_relevant_obstacles_ = nh_.advertise<nav_msgs::OccupancyGrid>("relevant_obstacles_grid", 1);
00038
00039
00040 joystick_velocity_sub_ = nh_.subscribe<geometry_msgs::Twist>("command_in", 10,
00041 &CollisionVelocityFilter::joystickVelocityCB, this);
00042
00043
00044 double footprint_update_frequency;
00045 if (!pnh_.hasParam("footprint_update_frequency"))
00046 ROS_WARN("Used default parameter for footprint_update_frequency [1.0 Hz].");
00047 pnh_.param("footprint_update_frequency", footprint_update_frequency, 1.0);
00048 get_footprint_timer_ = pnh_.createTimer(ros::Duration(1 / footprint_update_frequency),
00049 &CollisionVelocityFilter::getFootprint, this);
00050
00051
00052 if (!pnh_.hasParam("global_frame"))
00053 ROS_WARN("Used default parameter for global_frame [/base_link]");
00054 pnh_.param("global_frame", global_frame_, std::string("/base_link"));
00055
00056 if (!pnh_.hasParam("robot_base_frame"))
00057 ROS_WARN("Used default parameter for robot_frame [/base_link]");
00058 pnh_.param("robot_base_frame", robot_frame_, std::string("/base_link"));
00059
00060 if (!pnh_.hasParam("influence_radius"))
00061 ROS_WARN("Used default parameter for influence_radius [1.5 m]");
00062 pnh_.param("influence_radius", influence_radius_, 1.5);
00063 closest_obstacle_dist_ = influence_radius_;
00064 closest_obstacle_angle_ = 0.0;
00065
00066
00067 if (!pnh_.hasParam("stop_threshold"))
00068 ROS_WARN("Used default parameter for stop_threshold [0.1 m]");
00069 pnh_.param("stop_threshold", stop_threshold_, 0.10);
00070
00071 if (!nh_.hasParam("obstacle_damping_dist"))
00072 ROS_WARN("Used default parameter for obstacle_damping_dist [5.0 m]");
00073 pnh_.param("obstacle_damping_dist", obstacle_damping_dist_, 5.0);
00074 if (obstacle_damping_dist_ <= stop_threshold_)
00075 {
00076 obstacle_damping_dist_ = stop_threshold_ + 0.01;
00077 ROS_WARN("obstacle_damping_dist <= stop_threshold -> robot will stop without deceleration!");
00078 }
00079
00080 if (!pnh_.hasParam("use_circumscribed_threshold"))
00081 ROS_WARN("Used default parameter for use_circumscribed_threshold_ [0.2 rad/s]");
00082 pnh_.param("use_circumscribed_threshold", use_circumscribed_threshold_, 0.20);
00083
00084 if (!pnh_.hasParam("pot_ctrl_vmax"))
00085 ROS_WARN("Used default parameter for pot_ctrl_vmax [0.6]");
00086 pnh_.param("pot_ctrl_vmax", v_max_, 0.6);
00087
00088 if (!pnh_.hasParam("pot_ctrl_vtheta_max"))
00089 ROS_WARN("Used default parameter for pot_ctrl_vtheta_max [0.8]");
00090 pnh_.param("pot_ctrl_vtheta_max", vtheta_max_, 0.8);
00091
00092 if (!pnh_.hasParam("pot_ctrl_kv"))
00093 ROS_WARN("Used default parameter for pot_ctrl_kv [1.0]");
00094 pnh_.param("pot_ctrl_kv", kv_, 1.0);
00095
00096 if (!pnh_.hasParam("pot_ctrl_kp"))
00097 ROS_WARN("Used default parameter for pot_ctrl_kp [2.0]");
00098 pnh_.param("pot_ctrl_kp", kp_, 2.0);
00099
00100 if (!pnh_.hasParam("pot_ctrl_virt_mass"))
00101 ROS_WARN("Used default parameter for pot_ctrl_virt_mass [0.8]");
00102 pnh_.param("pot_ctrl_virt_mass", virt_mass_, 0.8);
00103
00104 robot_footprint_ = anti_collision_costmap_->getRobotFootprint();
00105
00106 if (robot_footprint_.size() > 4)
00107 ROS_WARN(
00108 "You have set more than 4 points as robot_footprint, cob_collision_velocity_filter can deal only with rectangular footprints so far!");
00109
00110
00111 if (!pnh_.hasParam("max_acceleration"))
00112 ROS_WARN("Used default parameter for max_acceleration [0.5, 0.5, 0.7]");
00113 XmlRpc::XmlRpcValue max_acc;
00114 if (pnh_.getParam("max_acceleration", max_acc))
00115 {
00116 ROS_ASSERT(max_acc.getType() == XmlRpc::XmlRpcValue::TypeArray);
00117 ax_max_ = (double)max_acc[0];
00118 ay_max_ = (double)max_acc[1];
00119 atheta_max_ = (double)max_acc[2];
00120 }
00121 else
00122 {
00123 ax_max_ = 0.5;
00124 ay_max_ = 0.5;
00125 atheta_max_ = 0.7;
00126 }
00127
00128 last_time_ = ros::Time::now().toSec();
00129 vx_last_ = 0.0;
00130 vy_last_ = 0.0;
00131 vtheta_last_ = 0.0;
00132
00133
00134 dynCB_ = boost::bind(&CollisionVelocityFilter::dynamicReconfigureCB, this, _1, _2);
00135 dyn_server_.setCallback(dynCB_);
00136 ROS_DEBUG("[cob_collision_velocity_filter] Initialized");
00137 }
00138
00139
00140 CollisionVelocityFilter::~CollisionVelocityFilter()
00141 {
00142
00143
00144 }
00145
00146
00147 void CollisionVelocityFilter::joystickVelocityCB(const geometry_msgs::Twist::ConstPtr &twist)
00148 {
00149
00150 ROS_DEBUG_NAMED("joystickVelocityCB", "[cob_collision_velocity_filter] Received command");
00151 pthread_mutex_lock(&m_mutex);
00152
00153 robot_twist_linear_ = twist->linear;
00154 robot_twist_angular_ = twist->angular;
00155
00156 pthread_mutex_unlock(&m_mutex);
00157
00158
00159 obstacleHandler();
00160
00161 performControllerStep();
00162
00163 }
00164
00165
00166 void CollisionVelocityFilter::getFootprint(const ros::TimerEvent& event)
00167 {
00168 ROS_DEBUG("[cob_collision_velocity_filter] Update footprint");
00169
00170 std::vector<geometry_msgs::Point> footprint;
00171 footprint = anti_collision_costmap_->getRobotFootprint();
00172
00173 pthread_mutex_lock(&m_mutex);
00174
00175 footprint_front_ = footprint_front_initial_;
00176 footprint_rear_ = footprint_rear_initial_;
00177 footprint_left_ = footprint_left_initial_;
00178 footprint_right_ = footprint_right_initial_;
00179
00180 robot_footprint_ = footprint;
00181 for (unsigned int i = 0; i < footprint.size(); i++)
00182 {
00183 if (footprint[i].x > footprint_front_)
00184 footprint_front_ = footprint[i].x;
00185 if (footprint[i].x < footprint_rear_)
00186 footprint_rear_ = footprint[i].x;
00187 if (footprint[i].y > footprint_left_)
00188 footprint_left_ = footprint[i].y;
00189 if (footprint[i].y < footprint_right_)
00190 footprint_right_ = footprint[i].y;
00191 }
00192
00193 pthread_mutex_unlock(&m_mutex);
00194
00195 }
00196
00197 void CollisionVelocityFilter::dynamicReconfigureCB(
00198 const cob_collision_velocity_filter::CollisionVelocityFilterConfig &config, const uint32_t level)
00199 {
00200 pthread_mutex_lock(&m_mutex);
00201
00202 stop_threshold_ = config.stop_threshold;
00203 obstacle_damping_dist_ = config.obstacle_damping_dist;
00204 if (obstacle_damping_dist_ <= stop_threshold_)
00205 {
00206 obstacle_damping_dist_ = stop_threshold_ + 0.01;
00207 ROS_WARN("obstacle_damping_dist <= stop_threshold -> robot will stop without deceleration!");
00208 }
00209
00210 if (obstacle_damping_dist_ > config.influence_radius || stop_threshold_ > config.influence_radius)
00211 {
00212 ROS_WARN("Not changing influence_radius since obstacle_damping_dist and/or stop_threshold is bigger!");
00213 }
00214 else
00215 {
00216 influence_radius_ = config.influence_radius;
00217 }
00218
00219 if (stop_threshold_ <= 0.0 || influence_radius_ <= 0.0)
00220 ROS_WARN("Turned off obstacle avoidance!");
00221 pthread_mutex_unlock(&m_mutex);
00222 }
00223
00224
00225 void CollisionVelocityFilter::performControllerStep()
00226 {
00227
00228 double dt;
00229 double vx_max, vy_max;
00230 geometry_msgs::Twist cmd_vel, cmd_vel_in;
00231
00232 cmd_vel_in.linear = robot_twist_linear_;
00233 cmd_vel_in.angular = robot_twist_angular_;
00234
00235 cmd_vel.linear = robot_twist_linear_;
00236 cmd_vel.angular = robot_twist_angular_;
00237 dt = ros::Time::now().toSec() - last_time_;
00238 last_time_ = ros::Time::now().toSec();
00239
00240 double vel_angle = atan2(cmd_vel.linear.y, cmd_vel.linear.x);
00241 vx_max = v_max_ * fabs(cos(vel_angle));
00242 if (vx_max > fabs(cmd_vel.linear.x))
00243 vx_max = fabs(cmd_vel.linear.x);
00244 vy_max = v_max_ * fabs(sin(vel_angle));
00245 if (vy_max > fabs(cmd_vel.linear.y))
00246 vy_max = fabs(cmd_vel.linear.y);
00247
00248
00249 if (closest_obstacle_dist_ < influence_radius_)
00250 {
00251 double F_x, F_y;
00252 double vx_d, vy_d, vx_factor, vy_factor;
00253 double kv_obst = kv_, vx_max_obst = vx_max, vy_max_obst = vy_max;
00254
00255
00256 double obstacle_linear_slope_x = vx_max / (obstacle_damping_dist_ - stop_threshold_);
00257 vx_max_obst = (closest_obstacle_dist_ - stop_threshold_ + stop_threshold_ / 10.0f) * obstacle_linear_slope_x;
00258 if (vx_max_obst > vx_max)
00259 vx_max_obst = vx_max;
00260 else if (vx_max_obst < 0.0f)
00261 vx_max_obst = 0.0f;
00262
00263 double obstacle_linear_slope_y = vy_max / (obstacle_damping_dist_ - stop_threshold_);
00264 vy_max_obst = (closest_obstacle_dist_ - stop_threshold_ + stop_threshold_ / 10.0f) * obstacle_linear_slope_y;
00265 if (vy_max_obst > vy_max)
00266 vy_max_obst = vy_max;
00267 else if (vy_max_obst < 0.0f)
00268 vy_max_obst = 0.0f;
00269
00270
00271
00272 double closest_obstacle_dist_x = closest_obstacle_dist_ * cos(closest_obstacle_angle_);
00273 double closest_obstacle_dist_y = closest_obstacle_dist_ * sin(closest_obstacle_angle_);
00274 vx_d = kp_ / kv_obst * closest_obstacle_dist_x;
00275 vy_d = kp_ / kv_obst * closest_obstacle_dist_y;
00276 vx_factor = vx_max_obst / sqrt(vy_d * vy_d + vx_d * vx_d);
00277 vy_factor = vy_max_obst / sqrt(vy_d * vy_d + vx_d * vx_d);
00278 if (vx_factor > 1.0)
00279 vx_factor = 1.0;
00280 if (vy_factor > 1.0)
00281 vy_factor = 1.0;
00282
00283 F_x = -kv_obst * vx_last_ + vx_factor * kp_ * closest_obstacle_dist_x;
00284 F_y = -kv_obst * vy_last_ + vy_factor * kp_ * closest_obstacle_dist_y;
00285
00286 cmd_vel.linear.x = vx_last_ + F_x / virt_mass_ * dt;
00287 cmd_vel.linear.y = vy_last_ + F_y / virt_mass_ * dt;
00288
00289 }
00290
00291
00292 if (fabs(cmd_vel.linear.x) > vx_max)
00293 cmd_vel.linear.x = sign(cmd_vel.linear.x) * vx_max;
00294 if (fabs(cmd_vel.linear.y) > vy_max)
00295 cmd_vel.linear.y = sign(cmd_vel.linear.y) * vy_max;
00296 if (fabs(cmd_vel.angular.z) > vtheta_max_)
00297 cmd_vel.angular.z = sign(cmd_vel.angular.z) * vtheta_max_;
00298
00299
00300
00301
00302
00303 if (fabs(cmd_vel.linear.x) > fabs(vx_last_))
00304 {
00305 if ((cmd_vel.linear.x - vx_last_) / dt > ax_max_)
00306 cmd_vel.linear.x = vx_last_ + ax_max_ * dt;
00307 else if ((cmd_vel.linear.x - vx_last_) / dt < -ax_max_)
00308 cmd_vel.linear.x = vx_last_ - ax_max_ * dt;
00309 }
00310 if (fabs(cmd_vel.linear.y) > fabs(vy_last_))
00311 {
00312 if ((cmd_vel.linear.y - vy_last_) / dt > ay_max_)
00313 cmd_vel.linear.y = vy_last_ + ay_max_ * dt;
00314 else if ((cmd_vel.linear.y - vy_last_) / dt < -ay_max_)
00315 cmd_vel.linear.y = vy_last_ - ay_max_ * dt;
00316 }
00317 if (fabs(cmd_vel.angular.z) > fabs(vtheta_last_))
00318 {
00319 if ((cmd_vel.angular.z - vtheta_last_) / dt > atheta_max_)
00320 cmd_vel.angular.z = vtheta_last_ + atheta_max_ * dt;
00321 else if ((cmd_vel.angular.z - vtheta_last_) / dt < -atheta_max_)
00322 cmd_vel.angular.z = vtheta_last_ - atheta_max_ * dt;
00323 }
00324
00325 pthread_mutex_lock(&m_mutex);
00326 vx_last_ = cmd_vel.linear.x;
00327 vy_last_ = cmd_vel.linear.y;
00328 vtheta_last_ = cmd_vel.angular.z;
00329 pthread_mutex_unlock(&m_mutex);
00330
00331 velocity_limited_marker_.publishMarkers(cmd_vel_in.linear.x, cmd_vel.linear.x, cmd_vel_in.linear.y, cmd_vel.linear.y,
00332 cmd_vel_in.angular.z, cmd_vel.angular.z);
00333
00334
00335 if (closest_obstacle_dist_ < stop_threshold_)
00336 {
00337 stopMovement();
00338 }
00339 else
00340 {
00341
00342 topic_pub_command_.publish(cmd_vel);
00343 }
00344 return;
00345 }
00346
00347 void CollisionVelocityFilter::obstacleHandler()
00348 {
00349 pthread_mutex_lock(&m_mutex);
00350 closest_obstacle_dist_ = influence_radius_;
00351 pthread_mutex_unlock(&m_mutex);
00352
00353 double cur_distance_to_center, cur_distance_to_border;
00354 double obstacle_theta_robot, obstacle_delta_theta_robot, obstacle_dist_vel_dir;
00355 bool cur_obstacle_relevant;
00356 geometry_msgs::Point cur_obstacle_robot;
00357 geometry_msgs::Point zero_position;
00358 zero_position.x = 0.0f;
00359 zero_position.y = 0.0f;
00360 zero_position.z = 0.0f;
00361 bool use_circumscribed = true, use_tube = true;
00362
00363
00364 double corner_front_left, corner_rear_left, corner_rear_right, corner_front_right;
00365 corner_front_left = atan2(footprint_left_, footprint_front_);
00366 corner_rear_left = atan2(footprint_left_, footprint_rear_);
00367 corner_rear_right = atan2(footprint_right_, footprint_rear_);
00368 corner_front_right = atan2(footprint_right_, footprint_front_);
00369
00370
00371 if (fabs(robot_twist_linear_.x) <= 0.005f && fabs(robot_twist_linear_.y) <= 0.005f)
00372 {
00373 use_tube = false;
00374
00375 }
00376 if (!use_tube)
00377 {
00378 if (fabs(robot_twist_angular_.z) <= 0.01f)
00379 {
00380 use_circumscribed = false;
00381 }
00382 }
00383 else
00384 {
00385 if (fabs(robot_twist_angular_.z) <= use_circumscribed_threshold_)
00386 {
00387 use_circumscribed = false;
00388 }
00389 }
00390
00391
00392 double velocity_angle = 0.0f, velocity_ortho_angle;
00393 double corner_angle, delta_corner_angle;
00394 double ortho_corner_dist;
00395 double tube_left_border = 0.0f, tube_right_border = 0.0f;
00396 double tube_left_origin = 0.0f, tube_right_origin = 0.0f;
00397 double corner_dist, circumscribed_radius = 0.0f;
00398
00399 for (unsigned i = 0; i < robot_footprint_.size(); i++)
00400 {
00401 corner_dist = sqrt(robot_footprint_[i].x * robot_footprint_[i].x + robot_footprint_[i].y * robot_footprint_[i].y);
00402 if (corner_dist > circumscribed_radius)
00403 circumscribed_radius = corner_dist;
00404 }
00405
00406 if (use_tube)
00407 {
00408
00409 velocity_angle = atan2(robot_twist_linear_.y, robot_twist_linear_.x);
00410 velocity_ortho_angle = velocity_angle + M_PI / 2.0f;
00411
00412 for (unsigned i = 0; i < robot_footprint_.size(); i++)
00413 {
00414 corner_angle = atan2(robot_footprint_[i].y, robot_footprint_[i].x);
00415 delta_corner_angle = velocity_ortho_angle - corner_angle;
00416 corner_dist = sqrt(robot_footprint_[i].x * robot_footprint_[i].x + robot_footprint_[i].y * robot_footprint_[i].y);
00417 ortho_corner_dist = cos(delta_corner_angle) * corner_dist;
00418
00419 if (ortho_corner_dist < tube_right_border)
00420 {
00421 tube_right_border = ortho_corner_dist;
00422 tube_right_origin = sin(delta_corner_angle) * corner_dist;
00423 }
00424 else if (ortho_corner_dist > tube_left_border)
00425 {
00426 tube_left_border = ortho_corner_dist;
00427 tube_left_origin = sin(delta_corner_angle) * corner_dist;
00428 }
00429 }
00430 }
00431
00432
00433 pthread_mutex_lock(&m_mutex);
00434 relevant_obstacles_.header.frame_id = global_frame_;
00435 relevant_obstacles_.header.stamp = ros::Time::now();
00436 relevant_obstacles_.data.clear();
00437 for (unsigned int i = 0;
00438 i
00439 < anti_collision_costmap_->getCostmap()->getSizeInCellsX()
00440 * anti_collision_costmap_->getCostmap()->getSizeInCellsY(); i++)
00441 {
00442 if (anti_collision_costmap_->getCostmap()->getCharMap()[i] == -1)
00443 {
00444 relevant_obstacles_.data.push_back(-1);
00445 }
00446 else if (anti_collision_costmap_->getCostmap()->getCharMap()[i] < costmap_obstacle_treshold_)
00447 {
00448 relevant_obstacles_.data.push_back(0);
00449 }
00450 else
00451 {
00452
00453
00454 geometry_msgs::Point cell;
00455 cell.x = (i % (int)(anti_collision_costmap_->getCostmap()->getSizeInCellsX()))
00456 * anti_collision_costmap_->getCostmap()->getResolution()
00457 + anti_collision_costmap_->getCostmap()->getOriginX();
00458 cell.y = (i / (int)(anti_collision_costmap_->getCostmap()->getSizeInCellsX()))
00459 * anti_collision_costmap_->getCostmap()->getResolution()
00460 + anti_collision_costmap_->getCostmap()->getOriginY();
00461 cell.z = 0.0f;
00462
00463 cur_obstacle_relevant = false;
00464 cur_distance_to_center = getDistance2d(zero_position, cell);
00465
00466 if (use_circumscribed && cur_distance_to_center <= circumscribed_radius)
00467 {
00468 cur_obstacle_robot = cell;
00469
00470 if (obstacleValid(cur_obstacle_robot.x, cur_obstacle_robot.y))
00471 {
00472 cur_obstacle_relevant = true;
00473 obstacle_theta_robot = atan2(cur_obstacle_robot.y, cur_obstacle_robot.x);
00474 }
00475
00476
00477 }
00478 else if (use_tube && cur_distance_to_center < influence_radius_)
00479 {
00480 cur_obstacle_robot = cell;
00481
00482 if (obstacleValid(cur_obstacle_robot.x, cur_obstacle_robot.y))
00483 {
00484 obstacle_theta_robot = atan2(cur_obstacle_robot.y, cur_obstacle_robot.x);
00485 obstacle_delta_theta_robot = obstacle_theta_robot - velocity_angle;
00486 obstacle_dist_vel_dir = sin(obstacle_delta_theta_robot) * cur_distance_to_center;
00487
00488 if (obstacle_dist_vel_dir <= tube_left_border && obstacle_dist_vel_dir >= tube_right_border)
00489 {
00490
00491
00492 if (sign(obstacle_dist_vel_dir) >= 0)
00493 {
00494 if (cos(obstacle_delta_theta_robot) * cur_distance_to_center >= tube_left_origin)
00495 {
00496
00497 cur_obstacle_relevant = true;
00498 }
00499 }
00500 else
00501 {
00502 if (cos(obstacle_delta_theta_robot) * cur_distance_to_center >= tube_right_origin)
00503 {
00504
00505 cur_obstacle_relevant = true;
00506 }
00507 }
00508 }
00509 }
00510 }
00511
00512 if (cur_obstacle_relevant)
00513 {
00514 ROS_DEBUG_STREAM_NAMED("obstacleHandler", "[cob_collision_velocity_filter] Detected an obstacle");
00515
00516 relevant_obstacles_.data.push_back(100);
00517
00518
00519 if (obstacle_theta_robot >= corner_front_right && obstacle_theta_robot < corner_front_left)
00520 {
00521
00522 cur_distance_to_border = cur_distance_to_center - fabs(footprint_front_) / fabs(cos(obstacle_theta_robot));
00523 }
00524 else if (obstacle_theta_robot >= corner_front_left && obstacle_theta_robot < corner_rear_left)
00525 {
00526
00527 cur_distance_to_border = cur_distance_to_center - fabs(footprint_left_) / fabs(sin(obstacle_theta_robot));
00528 }
00529 else if (obstacle_theta_robot >= corner_rear_left || obstacle_theta_robot < corner_rear_right)
00530 {
00531
00532 cur_distance_to_border = cur_distance_to_center - fabs(footprint_rear_) / fabs(cos(obstacle_theta_robot));
00533 }
00534 else
00535 {
00536
00537 cur_distance_to_border = cur_distance_to_center - fabs(footprint_right_) / fabs(sin(obstacle_theta_robot));
00538 }
00539
00540 if (cur_distance_to_border < closest_obstacle_dist_)
00541 {
00542 closest_obstacle_dist_ = cur_distance_to_border;
00543 closest_obstacle_angle_ = obstacle_theta_robot;
00544 }
00545 }
00546 else
00547 {
00548 relevant_obstacles_.data.push_back(0);
00549 }
00550 }
00551 }
00552 pthread_mutex_unlock(&m_mutex);
00553
00554 topic_pub_relevant_obstacles_.publish(relevant_obstacles_);
00555 ROS_DEBUG_STREAM_NAMED("obstacleHandler",
00556 "[cob_collision_velocity_filter] closest_obstacle_dist_ = " << closest_obstacle_dist_);
00557 }
00558
00559 double CollisionVelocityFilter::getDistance2d(geometry_msgs::Point a, geometry_msgs::Point b)
00560 {
00561 return sqrt(pow(a.x - b.x, 2) + pow(a.y - b.y, 2));
00562 }
00563
00564 double CollisionVelocityFilter::sign(double x)
00565 {
00566 if (x >= 0.0f)
00567 return 1.0f;
00568 else
00569 return -1.0f;
00570 }
00571
00572 bool CollisionVelocityFilter::obstacleValid(double x_obstacle, double y_obstacle)
00573 {
00574 if (x_obstacle < footprint_front_ && x_obstacle > footprint_rear_ && y_obstacle > footprint_right_
00575 && y_obstacle < footprint_left_)
00576 {
00577 ROS_WARN("Found an obstacle inside robot_footprint: Skip!");
00578 return false;
00579 }
00580
00581 return true;
00582 }
00583
00584 void CollisionVelocityFilter::stopMovement()
00585 {
00586 geometry_msgs::Twist stop_twist;
00587 stop_twist.linear.x = 0.0f;
00588 stop_twist.linear.y = 0.0f;
00589 stop_twist.linear.z = 0.0f;
00590 stop_twist.angular.x = 0.0f;
00591 stop_twist.angular.y = 0.0f;
00592 stop_twist.linear.z = 0.0f;
00593 topic_pub_command_.publish(stop_twist);
00594 vx_last_ = 0.0;
00595 vy_last_ = 0.0;
00596 vtheta_last_ = 0.0;
00597 }
00598
00599
00600
00601 int main(int argc, char** argv)
00602 {
00603
00604 ros::init(argc, argv, "cob_collision_velocity_filter");
00605
00606
00607 tf::TransformListener tf(ros::Duration(10));
00608 costmap_2d::Costmap2DROS* costmap = new costmap_2d::Costmap2DROS("anti_collision_costmap", tf);
00609 CollisionVelocityFilter collisionVelocityFilter(costmap);
00610
00611 ros::spin();
00612
00613 return 0;
00614 }
00615