$search
00001 /**************************************************************** 00002 * 00003 * Copyright (c) 2012 00004 * 00005 * Fraunhofer Institute for Manufacturing Engineering 00006 * and Automation (IPA) 00007 * 00008 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00009 * 00010 * Project name: care-o-bot 00011 * ROS stack name: cob_navigation 00012 * ROS package name: cob_collision_velocity_filter 00013 * 00014 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00015 * 00016 * Author: Matthias Gruhler, email:Matthias.Gruhler@ipa.fhg.de 00017 * 00018 * Date of creation: February 2012 00019 * 00020 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00021 * 00022 * Redistribution and use in source and binary forms, with or without 00023 * modification, are permitted provided that the following conditions are met: 00024 * 00025 * * Redistributions of source code must retain the above copyright 00026 * notice, this list of conditions and the following disclaimer. 00027 * * Redistributions in binary form must reproduce the above copyright 00028 * notice, this list of conditions and the following disclaimer in the 00029 * documentation and/or other materials provided with the distribution. 00030 * * Neither the name of the Fraunhofer Institute for Manufacturing 00031 * Engineering and Automation (IPA) nor the names of its 00032 * contributors may be used to endorse or promote products derived from 00033 * this software without specific prior written permission. 00034 * 00035 * This program is free software: you can redistribute it and/or modify 00036 * it under the terms of the GNU Lesser General Public License LGPL as 00037 * published by the Free Software Foundation, either version 3 of the 00038 * License, or (at your option) any later version. 00039 * 00040 * This program is distributed in the hope that it will be useful, 00041 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00042 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00043 * GNU Lesser General Public License LGPL for more details. 00044 * 00045 * You should have received a copy of the GNU Lesser General Public 00046 * License LGPL along with this program. 00047 * If not, see <http://www.gnu.org/licenses/>. 00048 * 00049 ****************************************************************/ 00050 #include <cob_collision_velocity_filter.h> 00051 00052 #include <visualization_msgs/Marker.h> 00053 00054 // Constructor 00055 CollisionVelocityFilter::CollisionVelocityFilter() 00056 { 00057 // create node handle 00058 nh_ = ros::NodeHandle("~"); 00059 00060 m_mutex = PTHREAD_MUTEX_INITIALIZER; 00061 00062 // node handle to get footprint from parameter server 00063 std::string costmap_parameter_source; 00064 if(!nh_.hasParam("costmap_parameter_source")) ROS_WARN("Checking default source [/local_costmap_node/costmap] for costmap parameters"); 00065 nh_.param("costmap_parameter_source",costmap_parameter_source, std::string("/local_costmap_node/costmap")); 00066 00067 ros::NodeHandle local_costmap_nh_(costmap_parameter_source); 00068 00069 // implementation of topics to publish (command for base and list of relevant obstacles) 00070 topic_pub_command_ = nh_.advertise<geometry_msgs::Twist>("command", 1); 00071 topic_pub_relevant_obstacles_ = nh_.advertise<nav_msgs::GridCells>("relevant_obstacles", 1); 00072 00073 // subscribe to twist-movement of teleop 00074 joystick_velocity_sub_ = nh_.subscribe<geometry_msgs::Twist>("teleop_twist", 1, boost::bind(&CollisionVelocityFilter::joystickVelocityCB, this, _1)); 00075 // subscribe to the costmap to receive inflated cells 00076 obstacles_sub_ = nh_.subscribe<nav_msgs::GridCells>("obstacles", 1, boost::bind(&CollisionVelocityFilter::obstaclesCB, this, _1)); 00077 00078 // create service client 00079 srv_client_get_footprint_ = nh_.serviceClient<cob_footprint_observer::GetFootprint>("/get_footprint"); 00080 00081 // create Timer and call getFootprint Service periodically 00082 double footprint_update_frequency; 00083 if(!nh_.hasParam("footprint_update_frequency")) ROS_WARN("Used default parameter for footprint_update_frequency [1.0 Hz]."); 00084 nh_.param("footprint_update_frequency",footprint_update_frequency,1.0); 00085 get_footprint_timer_ = nh_.createTimer(ros::Duration(1/footprint_update_frequency), boost::bind(&CollisionVelocityFilter::getFootprintServiceCB, this, _1)); 00086 00087 // read parameters from parameter server 00088 // parameters from costmap 00089 if(!local_costmap_nh_.hasParam(costmap_parameter_source+"/global_frame")) ROS_WARN("Used default parameter for global_frame [/base_link]"); 00090 local_costmap_nh_.param(costmap_parameter_source+"/global_frame", global_frame_, std::string("/base_link")); 00091 00092 if(!local_costmap_nh_.hasParam(costmap_parameter_source+"/robot_base_frame")) ROS_WARN("Used default parameter for robot_frame [/base_link]"); 00093 local_costmap_nh_.param(costmap_parameter_source+"/robot_base_frame", robot_frame_, std::string("/base_link")); 00094 00095 if(!nh_.hasParam("influence_radius")) ROS_WARN("Used default parameter for influence_radius [1.5 m]"); 00096 nh_.param("influence_radius", influence_radius_, 1.5); 00097 closest_obstacle_dist_ = influence_radius_; 00098 closest_obstacle_angle_ = 0.0; 00099 00100 // parameters for obstacle avoidence and velocity adjustment 00101 if(!nh_.hasParam("stop_threshold")) ROS_WARN("Used default parameter for stop_threshold [0.1 m]"); 00102 nh_.param("stop_threshold", stop_threshold_, 0.10); 00103 00104 if(!nh_.hasParam("obstacle_damping_dist")) ROS_WARN("Used default parameter for obstacle_damping_dist [5.0 m]"); 00105 nh_.param("obstacle_damping_dist", obstacle_damping_dist_, 5.0); 00106 if(obstacle_damping_dist_ <= stop_threshold_) { 00107 obstacle_damping_dist_ = stop_threshold_ + 0.01; // set to stop_threshold_+0.01 to avoid divide by zero error 00108 ROS_WARN("obstacle_damping_dist <= stop_threshold -> robot will stop without decceleration!"); 00109 } 00110 00111 if(!nh_.hasParam("use_circumscribed_threshold")) ROS_WARN("Used default parameter for use_circumscribed_threshold_ [0.2 rad/s]"); 00112 nh_.param("use_circumscribed_threshold", use_circumscribed_threshold_, 0.20); 00113 00114 if(!nh_.hasParam("pot_ctrl_vmax")) ROS_WARN("Used default parameter for pot_ctrl_vmax [0.6]"); 00115 nh_.param("pot_ctrl_vmax", v_max_, 0.6); 00116 00117 if(!nh_.hasParam("pot_ctrl_vtheta_max")) ROS_WARN("Used default parameter for pot_ctrl_vtheta_max [0.8]"); 00118 nh_.param("pot_ctrl_vtheta_max", vtheta_max_, 0.8); 00119 00120 if(!nh_.hasParam("pot_ctrl_kv")) ROS_WARN("Used default parameter for pot_ctrl_kv [1.0]"); 00121 nh_.param("pot_ctrl_kv", kv_, 1.0); 00122 00123 if(!nh_.hasParam("pot_ctrl_kp")) ROS_WARN("Used default parameter for pot_ctrl_kp [2.0]"); 00124 nh_.param("pot_ctrl_kp", kp_, 2.0); 00125 00126 if(!nh_.hasParam("pot_ctrl_virt_mass")) ROS_WARN("Used default parameter for pot_ctrl_virt_mass [0.8]"); 00127 nh_.param("pot_ctrl_virt_mass", virt_mass_, 0.8); 00128 00129 //load the robot footprint from the parameter server if its available in the local costmap namespace 00130 robot_footprint_ = loadRobotFootprint(local_costmap_nh_); 00131 if(robot_footprint_.size() > 4) 00132 ROS_WARN("You have set more than 4 points as robot_footprint, cob_collision_velocity_filter can deal only with rectangular footprints so far!"); 00133 00134 // try to geht the max_acceleration values from the parameter server 00135 if(!nh_.hasParam("max_acceleration")) ROS_WARN("Used default parameter for max_acceleration [0.5, 0.5, 0.7]"); 00136 XmlRpc::XmlRpcValue max_acc; 00137 if(nh_.getParam("max_acceleration", max_acc)) { 00138 ROS_ASSERT(max_acc.getType() == XmlRpc::XmlRpcValue::TypeArray); 00139 ax_max_ = (double)max_acc[0]; 00140 ay_max_ = (double)max_acc[1]; 00141 atheta_max_ = (double)max_acc[2]; 00142 } else { 00143 ax_max_ = 0.5; 00144 ay_max_ = 0.5; 00145 atheta_max_ = 0.7; 00146 } 00147 00148 last_time_ = ros::Time::now().toSec(); 00149 vx_last_ = 0.0; 00150 vy_last_ = 0.0; 00151 vtheta_last_ = 0.0; 00152 00153 // dynamic reconfigure 00154 dynCB_ = boost::bind(&CollisionVelocityFilter::dynamicReconfigureCB, this, _1, _2); 00155 dyn_server_.setCallback(dynCB_); 00156 } 00157 00158 // Destructor 00159 CollisionVelocityFilter::~CollisionVelocityFilter(){} 00160 00161 // joystick_velocityCB reads twist command from joystick 00162 void CollisionVelocityFilter::joystickVelocityCB(const geometry_msgs::Twist::ConstPtr &twist){ 00163 pthread_mutex_lock(&m_mutex); 00164 00165 robot_twist_linear_ = twist->linear; 00166 robot_twist_angular_ = twist->angular; 00167 00168 pthread_mutex_unlock(&m_mutex); 00169 00170 // check for relevant obstacles 00171 obstacleHandler(); 00172 // stop if we are about to run in an obstacle 00173 performControllerStep(); 00174 00175 } 00176 00177 // obstaclesCB reads obstacles from costmap 00178 void CollisionVelocityFilter::obstaclesCB(const nav_msgs::GridCells::ConstPtr &obstacles){ 00179 pthread_mutex_lock(&m_mutex); 00180 00181 if(obstacles->cells.size()!=0) costmap_received_ = true; 00182 last_costmap_received_ = * obstacles; 00183 00184 if(stop_threshold_ < obstacles->cell_width / 2.0f || stop_threshold_ < obstacles->cell_height / 2.0f) 00185 ROS_WARN("You specified a stop_threshold that is smaller than resolution of received costmap!"); 00186 00187 pthread_mutex_unlock(&m_mutex); 00188 } 00189 00190 // timer callback for periodically checking footprint 00191 void CollisionVelocityFilter::getFootprintServiceCB(const ros::TimerEvent&) 00192 { 00193 cob_footprint_observer::GetFootprint srv = cob_footprint_observer::GetFootprint(); 00194 // check if service is reachable 00195 if (srv_client_get_footprint_.call(srv)) 00196 { 00197 // adjust footprint 00198 geometry_msgs::PolygonStamped footprint_poly = srv.response.footprint; 00199 std::vector<geometry_msgs::Point> footprint; 00200 geometry_msgs::Point pt; 00201 00202 for(unsigned int i=0; i<footprint_poly.polygon.points.size(); ++i) { 00203 pt.x = footprint_poly.polygon.points[i].x; 00204 pt.y = footprint_poly.polygon.points[i].y; 00205 pt.z = footprint_poly.polygon.points[i].z; 00206 footprint.push_back(pt); 00207 } 00208 00209 pthread_mutex_lock(&m_mutex); 00210 00211 footprint_front_ = footprint_front_initial_; 00212 footprint_rear_ = footprint_rear_initial_; 00213 footprint_left_ = footprint_left_initial_; 00214 footprint_right_ = footprint_right_initial_; 00215 00216 robot_footprint_ = footprint; 00217 for(unsigned int i=0; i<footprint.size(); i++) { 00218 if(footprint[i].x > footprint_front_) footprint_front_ = footprint[i].x; 00219 if(footprint[i].x < footprint_rear_) footprint_rear_ = footprint[i].x; 00220 if(footprint[i].y > footprint_left_) footprint_left_ = footprint[i].y; 00221 if(footprint[i].y < footprint_right_) footprint_right_ = footprint[i].y; 00222 } 00223 00224 pthread_mutex_unlock(&m_mutex); 00225 00226 } else { 00227 ROS_WARN("Cannot reach service /get_footprint"); 00228 } 00229 00230 } 00231 00232 void 00233 CollisionVelocityFilter::dynamicReconfigureCB(const cob_collision_velocity_filter::CollisionVelocityFilterConfig &config, 00234 const uint32_t level) 00235 { 00236 pthread_mutex_lock(&m_mutex); 00237 00238 stop_threshold_ = config.stop_threshold; 00239 obstacle_damping_dist_ = config.obstacle_damping_dist; 00240 if(obstacle_damping_dist_ <= stop_threshold_) { 00241 obstacle_damping_dist_ = stop_threshold_ + 0.01; // set to stop_threshold_+0.01 to avoid divide by zero error 00242 ROS_WARN("obstacle_damping_dist <= stop_threshold -> robot will stop without decceleration!"); 00243 } 00244 00245 if(obstacle_damping_dist_ > config.influence_radius || stop_threshold_ > config.influence_radius) 00246 { 00247 ROS_WARN("Not changing influence_radius since obstacle_damping_dist and/or stop_threshold is bigger!"); 00248 } else { 00249 influence_radius_ = config.influence_radius; 00250 } 00251 00252 if (stop_threshold_ <= 0.0 || influence_radius_ <=0.0) 00253 ROS_WARN("Turned off obstacle avoidance!"); 00254 pthread_mutex_unlock(&m_mutex); 00255 } 00256 00257 // sets corrected velocity of joystick command 00258 void CollisionVelocityFilter::performControllerStep() { 00259 00260 double dt; 00261 double vx_max, vy_max; 00262 geometry_msgs::Twist cmd_vel,cmd_vel_in; 00263 00264 cmd_vel_in.linear = robot_twist_linear_; 00265 cmd_vel_in.angular = robot_twist_angular_; 00266 00267 cmd_vel.linear = robot_twist_linear_; 00268 cmd_vel.angular = robot_twist_angular_; 00269 dt = ros::Time::now().toSec() - last_time_; 00270 last_time_ = ros::Time::now().toSec(); 00271 00272 double vel_angle = atan2(cmd_vel.linear.y,cmd_vel.linear.x); 00273 vx_max = v_max_ * fabs(cos(vel_angle)); 00274 if (vx_max > fabs(cmd_vel.linear.x)) vx_max = fabs(cmd_vel.linear.x); 00275 vy_max = v_max_ * fabs(sin(vel_angle)); 00276 if (vy_max > fabs(cmd_vel.linear.y)) vy_max = fabs(cmd_vel.linear.y); 00277 00278 //Slow down in any way while approximating an obstacle: 00279 if(closest_obstacle_dist_ < influence_radius_) { 00280 double F_x, F_y; 00281 double vx_d, vy_d, vx_factor, vy_factor; 00282 double kv_obst=kv_, vx_max_obst=vx_max, vy_max_obst=vy_max; 00283 00284 //implementation for linear decrease of v_max: 00285 double obstacle_linear_slope_x = vx_max / (obstacle_damping_dist_ - stop_threshold_); 00286 vx_max_obst = (closest_obstacle_dist_- stop_threshold_ + stop_threshold_/10.0f) * obstacle_linear_slope_x; 00287 if(vx_max_obst > vx_max) vx_max_obst = vx_max; 00288 else if(vx_max_obst < 0.0f) vx_max_obst = 0.0f; 00289 00290 double obstacle_linear_slope_y = vy_max / (obstacle_damping_dist_ - stop_threshold_); 00291 vy_max_obst = (closest_obstacle_dist_- stop_threshold_ + stop_threshold_/10.0f) * obstacle_linear_slope_y; 00292 if(vy_max_obst > vy_max) vy_max_obst = vy_max; 00293 else if(vy_max_obst < 0.0f) vy_max_obst = 0.0f; 00294 00295 //Translational movement 00296 //calculation of v factor to limit maxspeed 00297 double closest_obstacle_dist_x = closest_obstacle_dist_ * cos(closest_obstacle_angle_); 00298 double closest_obstacle_dist_y = closest_obstacle_dist_ * sin(closest_obstacle_angle_); 00299 vx_d = kp_/kv_obst * closest_obstacle_dist_x; 00300 vy_d = kp_/kv_obst * closest_obstacle_dist_y; 00301 vx_factor = vx_max_obst / sqrt(vy_d*vy_d + vx_d*vx_d); 00302 vy_factor = vy_max_obst / sqrt(vy_d*vy_d + vx_d*vx_d); 00303 if(vx_factor > 1.0) vx_factor = 1.0; 00304 if(vy_factor > 1.0) vy_factor = 1.0; 00305 00306 F_x = - kv_obst * vx_last_ + vx_factor * kp_ * closest_obstacle_dist_x; 00307 F_y = - kv_obst * vy_last_ + vy_factor * kp_ * closest_obstacle_dist_y; 00308 00309 cmd_vel.linear.x = vx_last_ + F_x / virt_mass_ * dt; 00310 cmd_vel.linear.y = vy_last_ + F_y / virt_mass_ * dt; 00311 00312 } 00313 00314 // make sure, the computed and commanded velocities are not greater than the specified max velocities 00315 if (fabs(cmd_vel.linear.x) > vx_max) cmd_vel.linear.x = sign(cmd_vel.linear.x) * vx_max; 00316 if (fabs(cmd_vel.linear.y) > vy_max) cmd_vel.linear.y = sign(cmd_vel.linear.y) * vy_max; 00317 if (fabs(cmd_vel.angular.z) > vtheta_max_) cmd_vel.angular.z = sign(cmd_vel.angular.z) * vtheta_max_; 00318 00319 // limit acceleration: 00320 // only acceleration (in terms of speeding up in any direction) is limited, 00321 // deceleration (in terms of slowing down) is handeled either by cob_teleop or the potential field 00322 // like slow-down behaviour above 00323 if (fabs(cmd_vel.linear.x) > fabs(vx_last_)) 00324 { 00325 if ((cmd_vel.linear.x - vx_last_)/dt > ax_max_) 00326 cmd_vel.linear.x = vx_last_ + ax_max_ * dt; 00327 else if((cmd_vel.linear.x - vx_last_)/dt < -ax_max_) 00328 cmd_vel.linear.x = vx_last_ - ax_max_ * dt; 00329 } 00330 if (fabs(cmd_vel.linear.y) > fabs(vy_last_)) 00331 { 00332 if ((cmd_vel.linear.y - vy_last_)/dt > ay_max_) 00333 cmd_vel.linear.y = vy_last_ + ay_max_ * dt; 00334 else if ((cmd_vel.linear.y - vy_last_)/dt < -ay_max_) 00335 cmd_vel.linear.y = vy_last_ - ay_max_ * dt; 00336 } 00337 if (fabs(cmd_vel.angular.z) > fabs(vtheta_last_)) 00338 { 00339 if ((cmd_vel.angular.z - vtheta_last_)/dt > atheta_max_) 00340 cmd_vel.angular.z = vtheta_last_ + atheta_max_ * dt; 00341 else if ((cmd_vel.angular.z - vtheta_last_)/dt < -atheta_max_) 00342 cmd_vel.angular.z = vtheta_last_ - atheta_max_ * dt; 00343 } 00344 00345 pthread_mutex_lock(&m_mutex); 00346 vx_last_ = cmd_vel.linear.x; 00347 vy_last_ = cmd_vel.linear.y; 00348 vtheta_last_ = cmd_vel.angular.z; 00349 pthread_mutex_unlock(&m_mutex); 00350 00351 velocity_limited_marker_.publishMarkers(cmd_vel_in.linear.x, cmd_vel.linear.x, cmd_vel_in.linear.y, cmd_vel.linear.y, cmd_vel_in.angular.z, cmd_vel.angular.z); 00352 00353 // if closest obstacle is within stop_threshold, then do not move 00354 if( closest_obstacle_dist_ < stop_threshold_ ) { 00355 stopMovement(); 00356 } 00357 else 00358 { 00359 // publish adjusted velocity 00360 topic_pub_command_.publish(cmd_vel); 00361 } 00362 return; 00363 } 00364 00365 void CollisionVelocityFilter::obstacleHandler() { 00366 pthread_mutex_lock(&m_mutex); 00367 if(!costmap_received_) { 00368 ROS_WARN("No costmap has been received by cob_collision_velocity_filter, the robot will drive without obstacle avoidance!"); 00369 closest_obstacle_dist_ = influence_radius_; 00370 00371 pthread_mutex_unlock(&m_mutex); 00372 return; 00373 } 00374 closest_obstacle_dist_ = influence_radius_; 00375 pthread_mutex_unlock(&m_mutex); 00376 00377 double cur_distance_to_center, cur_distance_to_border; 00378 double obstacle_theta_robot, obstacle_delta_theta_robot, obstacle_dist_vel_dir; 00379 bool cur_obstacle_relevant; 00380 geometry_msgs::Point cur_obstacle_robot; 00381 geometry_msgs::Point zero_position; 00382 zero_position.x=0.0f; 00383 zero_position.y=0.0f; 00384 zero_position.z=0.0f; 00385 bool use_circumscribed=true, use_tube=true; 00386 00387 //Calculate corner angles in robot_frame: 00388 double corner_front_left, corner_rear_left, corner_rear_right, corner_front_right; 00389 corner_front_left = atan2(footprint_left_, footprint_front_); 00390 corner_rear_left = atan2(footprint_left_, footprint_rear_); 00391 corner_rear_right = atan2(footprint_right_, footprint_rear_); 00392 corner_front_right = atan2(footprint_right_, footprint_front_); 00393 00394 //Decide, whether circumscribed or tube argument should be used for filtering: 00395 if(fabs(robot_twist_linear_.x) <= 0.005f && fabs(robot_twist_linear_.y) <= 0.005f) { 00396 use_tube = false; 00397 //disable tube filter at very slow velocities 00398 } 00399 if(!use_tube) { 00400 if( fabs(robot_twist_angular_.z) <= 0.01f) { 00401 use_circumscribed = false; 00402 } //when tube filter inactive, start circumscribed filter at very low rot-velocities 00403 } else { 00404 if( fabs(robot_twist_angular_.z) <= use_circumscribed_threshold_) { 00405 use_circumscribed = false; 00406 } //when tube filter running, disable circum-filter in a wider range of rot-velocities 00407 } 00408 00409 //Calculation of tube in driving-dir considered for obstacle avoidence 00410 double velocity_angle=0.0f, velocity_ortho_angle; 00411 double corner_angle, delta_corner_angle; 00412 double ortho_corner_dist; 00413 double tube_left_border = 0.0f, tube_right_border = 0.0f; 00414 double tube_left_origin = 0.0f, tube_right_origin = 0.0f; 00415 double corner_dist, circumscribed_radius = 0.0f; 00416 00417 for(unsigned i = 0; i<robot_footprint_.size(); i++) { 00418 corner_dist = sqrt(robot_footprint_[i].x*robot_footprint_[i].x + robot_footprint_[i].y*robot_footprint_[i].y); 00419 if(corner_dist > circumscribed_radius) circumscribed_radius = corner_dist; 00420 } 00421 00422 if(use_tube) { 00423 //use commanded vel-value for vel-vector direction.. ? 00424 velocity_angle = atan2(robot_twist_linear_.y, robot_twist_linear_.x); 00425 velocity_ortho_angle = velocity_angle + M_PI / 2.0f; 00426 00427 for(unsigned i = 0; i<robot_footprint_.size(); i++) { 00428 corner_angle = atan2(robot_footprint_[i].y, robot_footprint_[i].x); 00429 delta_corner_angle = velocity_ortho_angle - corner_angle; 00430 corner_dist = sqrt(robot_footprint_[i].x*robot_footprint_[i].x + robot_footprint_[i].y*robot_footprint_[i].y); 00431 ortho_corner_dist = cos(delta_corner_angle) * corner_dist; 00432 00433 if(ortho_corner_dist < tube_right_border) { 00434 tube_right_border = ortho_corner_dist; 00435 tube_right_origin = sin(delta_corner_angle) * corner_dist; 00436 } else if(ortho_corner_dist > tube_left_border) { 00437 tube_left_border = ortho_corner_dist; 00438 tube_left_origin = sin(delta_corner_angle) * corner_dist; 00439 } 00440 } 00441 } 00442 00443 //find relevant obstacles 00444 pthread_mutex_lock(&m_mutex); 00445 relevant_obstacles_.header = last_costmap_received_.header; 00446 relevant_obstacles_.cell_width = last_costmap_received_.cell_width; 00447 relevant_obstacles_.cell_height = last_costmap_received_.cell_height; 00448 relevant_obstacles_.cells.clear(); 00449 00450 for(unsigned int i = 0; i < last_costmap_received_.cells.size(); i++) { 00451 cur_obstacle_relevant = false; 00452 cur_distance_to_center = getDistance2d(zero_position, last_costmap_received_.cells[i]); 00453 //check whether current obstacle lies inside the circumscribed_radius of the robot -> prevent collisions while rotating 00454 if(use_circumscribed && cur_distance_to_center <= circumscribed_radius) { 00455 cur_obstacle_robot = last_costmap_received_.cells[i]; 00456 00457 if( obstacleValid(cur_obstacle_robot.x, cur_obstacle_robot.y) ) { 00458 cur_obstacle_relevant = true; 00459 relevant_obstacles_.cells.push_back(last_costmap_received_.cells[i]); 00460 obstacle_theta_robot = atan2(cur_obstacle_robot.y, cur_obstacle_robot.x); 00461 } 00462 00463 //for each obstacle, now check whether it lies in the tube or not: 00464 } else if(use_tube && cur_distance_to_center < influence_radius_) { 00465 cur_obstacle_robot = last_costmap_received_.cells[i]; 00466 00467 if( obstacleValid(cur_obstacle_robot.x, cur_obstacle_robot.y) ) { 00468 obstacle_theta_robot = atan2(cur_obstacle_robot.y, cur_obstacle_robot.x); 00469 obstacle_delta_theta_robot = obstacle_theta_robot - velocity_angle; 00470 obstacle_dist_vel_dir = sin(obstacle_delta_theta_robot) * cur_distance_to_center; 00471 00472 if(obstacle_dist_vel_dir <= tube_left_border && obstacle_dist_vel_dir >= tube_right_border) { 00473 //found obstacle that lies inside of observation tube 00474 00475 if( sign(obstacle_dist_vel_dir) >= 0) { 00476 if(cos(obstacle_delta_theta_robot) * cur_distance_to_center >= tube_left_origin) { 00477 //relevant obstacle in tube found 00478 cur_obstacle_relevant = true; 00479 relevant_obstacles_.cells.push_back(last_costmap_received_.cells[i]); 00480 } 00481 } else { // obstacle in right part of tube 00482 if(cos(obstacle_delta_theta_robot) * cur_distance_to_center >= tube_right_origin) { 00483 //relevant obstacle in tube found 00484 cur_obstacle_relevant = true; 00485 relevant_obstacles_.cells.push_back(last_costmap_received_.cells[i]); 00486 } 00487 } 00488 } 00489 } 00490 } 00491 00492 if(cur_obstacle_relevant) { //now calculate distance of current, relevant obstacle to robot 00493 if(obstacle_theta_robot >= corner_front_right && obstacle_theta_robot < corner_front_left) { 00494 //obstacle in front: 00495 cur_distance_to_border = cur_distance_to_center - fabs(footprint_front_) / fabs(cos(obstacle_theta_robot)); 00496 } else if(obstacle_theta_robot >= corner_front_left && obstacle_theta_robot < corner_rear_left) { 00497 //obstacle left: 00498 cur_distance_to_border = cur_distance_to_center - fabs(footprint_left_) / fabs(sin(obstacle_theta_robot)); 00499 } else if(obstacle_theta_robot >= corner_rear_left || obstacle_theta_robot < corner_rear_right) { 00500 //obstacle in rear: 00501 cur_distance_to_border = cur_distance_to_center - fabs(footprint_rear_) / fabs(cos(obstacle_theta_robot)); 00502 } else { 00503 //obstacle right: 00504 cur_distance_to_border = cur_distance_to_center - fabs(footprint_right_) / fabs(sin(obstacle_theta_robot)); 00505 } 00506 00507 if(cur_distance_to_border < closest_obstacle_dist_) { 00508 closest_obstacle_dist_ = cur_distance_to_border; 00509 closest_obstacle_angle_ = obstacle_theta_robot; 00510 } 00511 } 00512 } 00513 pthread_mutex_unlock(&m_mutex); 00514 00515 topic_pub_relevant_obstacles_.publish(relevant_obstacles_); 00516 } 00517 00518 // load robot footprint from costmap_2d_ros to keep same footprint format 00519 std::vector<geometry_msgs::Point> CollisionVelocityFilter::loadRobotFootprint(ros::NodeHandle node){ 00520 std::vector<geometry_msgs::Point> footprint; 00521 geometry_msgs::Point pt; 00522 double padding; 00523 00524 std::string padding_param, footprint_param; 00525 if(!node.searchParam("footprint_padding", padding_param)) 00526 padding = 0.01; 00527 else 00528 node.param(padding_param, padding, 0.01); 00529 00530 //grab the footprint from the parameter server if possible 00531 XmlRpc::XmlRpcValue footprint_list; 00532 std::string footprint_string; 00533 std::vector<std::string> footstring_list; 00534 if(node.searchParam("footprint", footprint_param)){ 00535 node.getParam(footprint_param, footprint_list); 00536 if(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeString) { 00537 footprint_string = std::string(footprint_list); 00538 00539 //if there's just an empty footprint up there, return 00540 if(footprint_string == "[]" || footprint_string == "") 00541 return footprint; 00542 00543 boost::erase_all(footprint_string, " "); 00544 00545 boost::char_separator<char> sep("[]"); 00546 boost::tokenizer<boost::char_separator<char> > tokens(footprint_string, sep); 00547 footstring_list = std::vector<std::string>(tokens.begin(), tokens.end()); 00548 } 00549 //make sure we have a list of lists 00550 if(!(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeArray && footprint_list.size() > 2) && !(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeString && footstring_list.size() > 5)){ 00551 ROS_FATAL("The footprint must be specified as list of lists on the parameter server, %s was specified as %s", footprint_param.c_str(), std::string(footprint_list).c_str()); 00552 throw std::runtime_error("The footprint must be specified as list of lists on the parameter server with at least 3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]"); 00553 } 00554 00555 if(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeArray) { 00556 for(int i = 0; i < footprint_list.size(); ++i){ 00557 //make sure we have a list of lists of size 2 00558 XmlRpc::XmlRpcValue point = footprint_list[i]; 00559 if(!(point.getType() == XmlRpc::XmlRpcValue::TypeArray && point.size() == 2)){ 00560 ROS_FATAL("The footprint must be specified as list of lists on the parameter server eg: [[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form"); 00561 throw std::runtime_error("The footprint must be specified as list of lists on the parameter server eg: [[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form"); 00562 } 00563 00564 //make sure that the value we're looking at is either a double or an int 00565 if(!(point[0].getType() == XmlRpc::XmlRpcValue::TypeInt || point[0].getType() == XmlRpc::XmlRpcValue::TypeDouble)){ 00566 ROS_FATAL("Values in the footprint specification must be numbers"); 00567 throw std::runtime_error("Values in the footprint specification must be numbers"); 00568 } 00569 pt.x = point[0].getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(point[0]) : (double)(point[0]); 00570 pt.x += sign(pt.x) * padding; 00571 00572 //make sure that the value we're looking at is either a double or an int 00573 if(!(point[1].getType() == XmlRpc::XmlRpcValue::TypeInt || point[1].getType() == XmlRpc::XmlRpcValue::TypeDouble)){ 00574 ROS_FATAL("Values in the footprint specification must be numbers"); 00575 throw std::runtime_error("Values in the footprint specification must be numbers"); 00576 } 00577 pt.y = point[1].getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(point[1]) : (double)(point[1]); 00578 pt.y += sign(pt.y) * padding; 00579 00580 footprint.push_back(pt); 00581 00582 node.deleteParam(footprint_param); 00583 std::ostringstream oss; 00584 bool first = true; 00585 BOOST_FOREACH(geometry_msgs::Point p, footprint) { 00586 if(first) { 00587 oss << "[[" << p.x << "," << p.y << "]"; 00588 first = false; 00589 } 00590 else { 00591 oss << ",[" << p.x << "," << p.y << "]"; 00592 } 00593 } 00594 oss << "]"; 00595 node.setParam(footprint_param, oss.str().c_str()); 00596 node.setParam("footprint", oss.str().c_str()); 00597 } 00598 } 00599 00600 else if(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeString) { 00601 std::vector<geometry_msgs::Point> footprint_spec; 00602 bool valid_foot = true; 00603 BOOST_FOREACH(std::string t, footstring_list) { 00604 if( t != "," ) { 00605 boost::erase_all(t, " "); 00606 boost::char_separator<char> pt_sep(","); 00607 boost::tokenizer<boost::char_separator<char> > pt_tokens(t, pt_sep); 00608 std::vector<std::string> point(pt_tokens.begin(), pt_tokens.end()); 00609 00610 if(point.size() != 2) { 00611 ROS_WARN("Each point must have exactly 2 coordinates"); 00612 valid_foot = false; 00613 break; 00614 } 00615 00616 std::vector<double>tmp_pt; 00617 BOOST_FOREACH(std::string p, point) { 00618 std::istringstream iss(p); 00619 double temp; 00620 if(iss >> temp) { 00621 tmp_pt.push_back(temp); 00622 } 00623 else { 00624 ROS_WARN("Each coordinate must convert to a double."); 00625 valid_foot = false; 00626 break; 00627 } 00628 } 00629 if(!valid_foot) 00630 break; 00631 00632 geometry_msgs::Point pt; 00633 pt.x = tmp_pt[0]; 00634 pt.y = tmp_pt[1]; 00635 00636 footprint_spec.push_back(pt); 00637 } 00638 } 00639 if (valid_foot) { 00640 footprint = footprint_spec; 00641 node.setParam("footprint", footprint_string); 00642 } 00643 else { 00644 ROS_FATAL("This footprint is not vaid it must be specified as a list of lists with at least 3 points, you specified %s", footprint_string.c_str()); 00645 throw std::runtime_error("The footprint must be specified as list of lists on the parameter server with at least 3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]"); 00646 } 00647 } 00648 } 00649 00650 footprint_right_ = 0.0f; footprint_left_ = 0.0f; footprint_front_ = 0.0f; footprint_rear_ = 0.0f; 00651 //extract rectangular borders for simplifying: 00652 for(unsigned int i=0; i<footprint.size(); i++) { 00653 if(footprint[i].x > footprint_front_) footprint_front_ = footprint[i].x; 00654 if(footprint[i].x < footprint_rear_) footprint_rear_ = footprint[i].x; 00655 if(footprint[i].y > footprint_left_) footprint_left_ = footprint[i].y; 00656 if(footprint[i].y < footprint_right_) footprint_right_ = footprint[i].y; 00657 } 00658 ROS_DEBUG("Extracted rectangular footprint for cob_collision_velocity_filter: Front: %f, Rear %f, Left: %f, Right %f", footprint_front_, footprint_rear_, footprint_left_, footprint_right_); 00659 00660 return footprint; 00661 } 00662 00663 double CollisionVelocityFilter::getDistance2d(geometry_msgs::Point a, geometry_msgs::Point b) { 00664 return sqrt( pow(a.x - b.x,2) + pow(a.y - b.y,2) ); 00665 } 00666 00667 double CollisionVelocityFilter::sign(double x) { 00668 if(x >= 0.0f) return 1.0f; 00669 else return -1.0f; 00670 } 00671 00672 bool CollisionVelocityFilter::obstacleValid(double x_obstacle, double y_obstacle) { 00673 if(x_obstacle<footprint_front_ && x_obstacle>footprint_rear_ && y_obstacle>footprint_right_ && y_obstacle<footprint_left_) { 00674 ROS_WARN("Found an obstacle inside robot_footprint: Skip!"); 00675 return false; 00676 } 00677 00678 return true; 00679 } 00680 00681 void CollisionVelocityFilter::stopMovement() { 00682 geometry_msgs::Twist stop_twist; 00683 stop_twist.linear.x = 0.0f; stop_twist.linear.y = 0.0f; stop_twist.linear.z = 0.0f; 00684 stop_twist.angular.x = 0.0f; stop_twist.angular.y = 0.0f; stop_twist.linear.z = 0.0f; 00685 topic_pub_command_.publish(stop_twist); 00686 vx_last_ = 0.0; 00687 vy_last_ = 0.0; 00688 vtheta_last_ = 0.0; 00689 } 00690 00691 //####################### 00692 //#### main programm #### 00693 int main(int argc, char** argv) 00694 { 00695 // initialize ROS, spezify name of node 00696 ros::init(argc, argv, "cob_collision_velocity_filter"); 00697 00698 // create nodeClass 00699 CollisionVelocityFilter collisionVelocityFilter; 00700 00701 00702 ros::spin(); 00703 00704 return 0; 00705 } 00706