$search
00001 /********************************************************************* 00002 * 00003 * Software License Agreement (BSD License) 00004 * 00005 * Copyright (c) 2008, Willow Garage, Inc. 00006 * All rights reserved. 00007 * 00008 * Redistribution and use in source and binary forms, with or without 00009 * modification, are permitted provided that the following conditions 00010 * are met: 00011 * 00012 * * Redistributions of source code must retain the above copyright 00013 * notice, this list of conditions and the following disclaimer. 00014 * * Redistributions in binary form must reproduce the above 00015 * copyright notice, this list of conditions and the following 00016 * disclaimer in the documentation and/or other materials provided 00017 * with the distribution. 00018 * * Neither the name of the Willow Garage nor the names of its 00019 * contributors may be used to endorse or promote products derived 00020 * from this software without specific prior written permission. 00021 * 00022 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00023 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00024 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00025 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00026 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00027 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00028 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00029 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00030 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00031 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00032 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00033 * POSSIBILITY OF SUCH DAMAGE. 00034 * 00035 * Author: Eitan Marder-Eppstein 00036 *********************************************************************/ 00037 #include <navfn/navfn_ros.h> 00038 #include <pluginlib/class_list_macros.h> 00039 00040 //register this planner as a BaseGlobalPlanner plugin 00041 PLUGINLIB_DECLARE_CLASS(navfn, NavfnROS, navfn::NavfnROS, nav_core::BaseGlobalPlanner) 00042 00043 namespace navfn { 00044 00045 NavfnROS::NavfnROS() 00046 : costmap_ros_(NULL), planner_(), initialized_(false), allow_unknown_(true), costmap_publisher_(NULL) {} 00047 00048 NavfnROS::NavfnROS(std::string name, costmap_2d::Costmap2DROS* costmap_ros) 00049 : costmap_ros_(NULL), planner_(), initialized_(false), allow_unknown_(true) { 00050 //initialize the planner 00051 initialize(name, costmap_ros); 00052 } 00053 00054 void NavfnROS::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros){ 00055 if(!initialized_){ 00056 costmap_ros_ = costmap_ros; 00057 planner_ = boost::shared_ptr<NavFn>(new NavFn(costmap_ros->getSizeInCellsX(), costmap_ros->getSizeInCellsY())); 00058 00059 //get an initial copy of the costmap 00060 costmap_ros_->getCostmapCopy(costmap_); 00061 00062 ros::NodeHandle private_nh("~/" + name); 00063 00064 plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1); 00065 00066 private_nh.param("visualize_potential", visualize_potential_, false); 00067 00068 //if we're going to visualize the potential array we need to advertise 00069 if(visualize_potential_) 00070 potarr_pub_.advertise(private_nh, "potential", 1); 00071 00072 private_nh.param("allow_unknown", allow_unknown_, true); 00073 private_nh.param("planner_window_x", planner_window_x_, 0.0); 00074 private_nh.param("planner_window_y", planner_window_y_, 0.0); 00075 private_nh.param("default_tolerance", default_tolerance_, 0.0); 00076 00077 double costmap_pub_freq; 00078 private_nh.param("planner_costmap_publish_frequency", costmap_pub_freq, 0.0); 00079 00080 //read parameters for the planner 00081 std::string global_frame = costmap_ros_->getGlobalFrameID(); 00082 00083 //we'll get the parameters for the robot radius from the costmap we're associated with 00084 inscribed_radius_ = costmap_ros_->getInscribedRadius(); 00085 circumscribed_radius_ = costmap_ros_->getCircumscribedRadius(); 00086 inflation_radius_ = costmap_ros_->getInflationRadius(); 00087 00088 //initialize the costmap publisher 00089 costmap_publisher_ = new costmap_2d::Costmap2DPublisher(ros::NodeHandle(private_nh, "NavfnROS_costmap"), costmap_pub_freq, global_frame); 00090 00091 //get the tf prefix 00092 ros::NodeHandle prefix_nh; 00093 tf_prefix_ = tf::getPrefixParam(prefix_nh); 00094 00095 make_plan_srv_ = private_nh.advertiseService("make_plan", &NavfnROS::makePlanService, this); 00096 00097 initialized_ = true; 00098 } 00099 else 00100 ROS_WARN("This planner has already been initialized, you can't call it twice, doing nothing"); 00101 } 00102 00103 bool NavfnROS::validPointPotential(const geometry_msgs::Point& world_point){ 00104 return validPointPotential(world_point, default_tolerance_); 00105 } 00106 00107 bool NavfnROS::validPointPotential(const geometry_msgs::Point& world_point, double tolerance){ 00108 if(!initialized_){ 00109 ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use"); 00110 return false; 00111 } 00112 00113 double resolution = costmap_ros_->getResolution(); 00114 geometry_msgs::Point p; 00115 p = world_point; 00116 00117 p.y = world_point.y - tolerance; 00118 00119 while(p.y <= world_point.y + tolerance){ 00120 p.x = world_point.x - tolerance; 00121 while(p.x <= world_point.x + tolerance){ 00122 double potential = getPointPotential(p); 00123 if(potential < POT_HIGH){ 00124 return true; 00125 } 00126 p.x += resolution; 00127 } 00128 p.y += resolution; 00129 } 00130 00131 return false; 00132 } 00133 00134 double NavfnROS::getPointPotential(const geometry_msgs::Point& world_point){ 00135 if(!initialized_){ 00136 ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use"); 00137 return -1.0; 00138 } 00139 00140 unsigned int mx, my; 00141 if(!costmap_.worldToMap(world_point.x, world_point.y, mx, my)) 00142 return DBL_MAX; 00143 00144 unsigned int index = my * planner_->nx + mx; 00145 return planner_->potarr[index]; 00146 } 00147 00148 bool NavfnROS::computePotential(const geometry_msgs::Point& world_point){ 00149 if(!initialized_){ 00150 ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use"); 00151 return false; 00152 } 00153 00154 //make sure that we have the latest copy of the costmap and that we clear the footprint of obstacles 00155 getCostmap(costmap_); 00156 00157 //make sure to resize the underlying array that Navfn uses 00158 planner_->setNavArr(costmap_.getSizeInCellsX(), costmap_.getSizeInCellsY()); 00159 planner_->setCostmap(costmap_.getCharMap(), true, allow_unknown_); 00160 00161 unsigned int mx, my; 00162 if(!costmap_.worldToMap(world_point.x, world_point.y, mx, my)) 00163 return false; 00164 00165 int map_start[2]; 00166 map_start[0] = 0; 00167 map_start[1] = 0; 00168 00169 int map_goal[2]; 00170 map_goal[0] = mx; 00171 map_goal[1] = my; 00172 00173 planner_->setStart(map_start); 00174 planner_->setGoal(map_goal); 00175 00176 return planner_->calcNavFnDijkstra(); 00177 } 00178 00179 void NavfnROS::clearRobotCell(const tf::Stamped<tf::Pose>& global_pose, unsigned int mx, unsigned int my){ 00180 if(!initialized_){ 00181 ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use"); 00182 return; 00183 } 00184 00185 //set the associated costs in the cost map to be free 00186 costmap_.setCost(mx, my, costmap_2d::FREE_SPACE); 00187 00188 double max_inflation_dist = inflation_radius_ + inscribed_radius_; 00189 00190 //make sure to re-inflate obstacles in the affected region 00191 costmap_.reinflateWindow(global_pose.getOrigin().x(), global_pose.getOrigin().y(), max_inflation_dist, max_inflation_dist); 00192 00193 //just in case we inflate over the point we just cleared 00194 costmap_.setCost(mx, my, costmap_2d::FREE_SPACE); 00195 00196 } 00197 00198 void NavfnROS::getCostmap(costmap_2d::Costmap2D& costmap) 00199 { 00200 if(!initialized_){ 00201 ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use"); 00202 return; 00203 } 00204 00205 costmap_ros_->clearRobotFootprint(); 00206 00207 //if the user has requested that the planner use only a window of the costmap, we'll get that window here 00208 if(planner_window_x_ > 1e-6 && planner_window_y_ > 1e-6){ 00209 costmap_ros_->getCostmapWindowCopy(planner_window_x_, planner_window_y_, costmap_); 00210 } 00211 else{ 00212 costmap_ros_->getCostmapCopy(costmap); 00213 } 00214 00215 //if the user wants to publish information about the planner's costmap, we'll do that here 00216 if(costmap_publisher_->active()){ 00217 std::vector<geometry_msgs::Point> oriented_footprint; 00218 costmap_ros_->getOrientedFootprint(oriented_footprint); 00219 tf::Stamped<tf::Pose> global_pose; 00220 costmap_ros_->getRobotPose(global_pose); 00221 costmap_publisher_->updateCostmapData(costmap, oriented_footprint, global_pose); 00222 } 00223 00224 } 00225 00226 bool NavfnROS::makePlanService(nav_msgs::GetPlan::Request& req, nav_msgs::GetPlan::Response& resp){ 00227 makePlan(req.start, req.goal, resp.plan.poses); 00228 00229 resp.plan.header.stamp = ros::Time::now(); 00230 resp.plan.header.frame_id = costmap_ros_->getGlobalFrameID(); 00231 00232 return true; 00233 } 00234 00235 void NavfnROS::mapToWorld(double mx, double my, double& wx, double& wy) { 00236 wx = costmap_.getOriginX() + mx * costmap_.getResolution(); 00237 wy = costmap_.getOriginY() + my * costmap_.getResolution(); 00238 } 00239 00240 bool NavfnROS::makePlan(const geometry_msgs::PoseStamped& start, 00241 const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan){ 00242 return makePlan(start, goal, default_tolerance_, plan); 00243 } 00244 00245 bool NavfnROS::makePlan(const geometry_msgs::PoseStamped& start, 00246 const geometry_msgs::PoseStamped& goal, double tolerance, std::vector<geometry_msgs::PoseStamped>& plan){ 00247 boost::mutex::scoped_lock lock(mutex_); 00248 if(!initialized_){ 00249 ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use"); 00250 return false; 00251 } 00252 00253 //clear the plan, just in case 00254 plan.clear(); 00255 00256 //make sure that we have the latest copy of the costmap and that we clear the footprint of obstacles 00257 getCostmap(costmap_); 00258 00259 ros::NodeHandle n; 00260 00261 //until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame 00262 if(tf::resolve(tf_prefix_, goal.header.frame_id) != tf::resolve(tf_prefix_, costmap_ros_->getGlobalFrameID())){ 00263 ROS_ERROR("The goal pose passed to this planner must be in the %s frame. It is instead in the %s frame.", 00264 tf::resolve(tf_prefix_, costmap_ros_->getGlobalFrameID()).c_str(), tf::resolve(tf_prefix_, goal.header.frame_id).c_str()); 00265 return false; 00266 } 00267 00268 if(tf::resolve(tf_prefix_, start.header.frame_id) != tf::resolve(tf_prefix_, costmap_ros_->getGlobalFrameID())){ 00269 ROS_ERROR("The start pose passed to this planner must be in the %s frame. It is instead in the %s frame.", 00270 tf::resolve(tf_prefix_, costmap_ros_->getGlobalFrameID()).c_str(), tf::resolve(tf_prefix_, start.header.frame_id).c_str()); 00271 return false; 00272 } 00273 00274 double wx = start.pose.position.x; 00275 double wy = start.pose.position.y; 00276 00277 unsigned int mx, my; 00278 if(!costmap_.worldToMap(wx, wy, mx, my)){ 00279 ROS_WARN("The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?"); 00280 return false; 00281 } 00282 00283 //clear the starting cell within the costmap because we know it can't be an obstacle 00284 tf::Stamped<tf::Pose> start_pose; 00285 tf::poseStampedMsgToTF(start, start_pose); 00286 clearRobotCell(start_pose, mx, my); 00287 00288 //make sure to resize the underlying array that Navfn uses 00289 planner_->setNavArr(costmap_.getSizeInCellsX(), costmap_.getSizeInCellsY()); 00290 planner_->setCostmap(costmap_.getCharMap(), true, allow_unknown_); 00291 00292 int map_start[2]; 00293 map_start[0] = mx; 00294 map_start[1] = my; 00295 00296 wx = goal.pose.position.x; 00297 wy = goal.pose.position.y; 00298 00299 if(!costmap_.worldToMap(wx, wy, mx, my)){ 00300 if(tolerance <= 0.0){ 00301 ROS_WARN("The goal sent to the navfn planner is off the global costmap. Planning will always fail to this goal."); 00302 return false; 00303 } 00304 mx = 0; 00305 my = 0; 00306 } 00307 00308 int map_goal[2]; 00309 map_goal[0] = mx; 00310 map_goal[1] = my; 00311 00312 planner_->setStart(map_goal); 00313 planner_->setGoal(map_start); 00314 00315 //bool success = planner_->calcNavFnAstar(); 00316 planner_->calcNavFnDijkstra(true); 00317 00318 double resolution = costmap_ros_->getResolution(); 00319 geometry_msgs::PoseStamped p, best_pose; 00320 p = goal; 00321 00322 bool found_legal = false; 00323 double best_sdist = DBL_MAX; 00324 00325 p.pose.position.y = goal.pose.position.y - tolerance; 00326 00327 while(p.pose.position.y <= goal.pose.position.y + tolerance){ 00328 p.pose.position.x = goal.pose.position.x - tolerance; 00329 while(p.pose.position.x <= goal.pose.position.x + tolerance){ 00330 double potential = getPointPotential(p.pose.position); 00331 double sdist = sq_distance(p, goal); 00332 if(potential < POT_HIGH && sdist < best_sdist){ 00333 best_sdist = sdist; 00334 best_pose = p; 00335 found_legal = true; 00336 } 00337 p.pose.position.x += resolution; 00338 } 00339 p.pose.position.y += resolution; 00340 } 00341 00342 if(found_legal){ 00343 //extract the plan 00344 if(getPlanFromPotential(best_pose, plan)){ 00345 //make sure the goal we push on has the same timestamp as the rest of the plan 00346 geometry_msgs::PoseStamped goal_copy = best_pose; 00347 goal_copy.header.stamp = ros::Time::now(); 00348 plan.push_back(goal_copy); 00349 } 00350 else{ 00351 ROS_ERROR("Failed to get a plan from potential when a legal potential was found. This shouldn't happen."); 00352 } 00353 } 00354 00355 if (visualize_potential_){ 00356 //publish potential array 00357 pcl::PointCloud<PotarrPoint> pot_area; 00358 pot_area.header.frame_id = costmap_ros_->getGlobalFrameID(); 00359 pot_area.points.clear(); 00360 pot_area.header.stamp = ros::Time::now(); 00361 00362 PotarrPoint pt; 00363 float *pp = planner_->potarr; 00364 double pot_x, pot_y; 00365 for (unsigned int i = 0; i < (unsigned int)planner_->ny*planner_->nx ; i++) 00366 { 00367 if (pp[i] < 10e7) 00368 { 00369 mapToWorld(i%planner_->nx, i/planner_->nx, pot_x, pot_y); 00370 pt.x = pot_x; 00371 pt.y = pot_y; 00372 pt.z = pp[i]/pp[planner_->start[1]*planner_->nx + planner_->start[0]]*20; 00373 pt.pot_value = pp[i]; 00374 pot_area.push_back(pt); 00375 } 00376 } 00377 potarr_pub_.publish(pot_area); 00378 } 00379 00380 //publish the plan for visualization purposes 00381 publishPlan(plan, 0.0, 1.0, 0.0, 0.0); 00382 00383 return !plan.empty(); 00384 } 00385 00386 void NavfnROS::publishPlan(const std::vector<geometry_msgs::PoseStamped>& path, double r, double g, double b, double a){ 00387 if(!initialized_){ 00388 ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use"); 00389 return; 00390 } 00391 00392 //create a message for the plan 00393 nav_msgs::Path gui_path; 00394 gui_path.poses.resize(path.size()); 00395 00396 if(!path.empty()) 00397 { 00398 gui_path.header.frame_id = path[0].header.frame_id; 00399 gui_path.header.stamp = path[0].header.stamp; 00400 } 00401 00402 // Extract the plan in world co-ordinates, we assume the path is all in the same frame 00403 for(unsigned int i=0; i < path.size(); i++){ 00404 gui_path.poses[i] = path[i]; 00405 } 00406 00407 plan_pub_.publish(gui_path); 00408 } 00409 00410 bool NavfnROS::getPlanFromPotential(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan){ 00411 if(!initialized_){ 00412 ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use"); 00413 return false; 00414 } 00415 00416 //clear the plan, just in case 00417 plan.clear(); 00418 00419 //until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame 00420 if(tf::resolve(tf_prefix_, goal.header.frame_id) != tf::resolve(tf_prefix_, costmap_ros_->getGlobalFrameID())){ 00421 ROS_ERROR("The goal pose passed to this planner must be in the %s frame. It is instead in the %s frame.", 00422 tf::resolve(tf_prefix_, costmap_ros_->getGlobalFrameID()).c_str(), tf::resolve(tf_prefix_, goal.header.frame_id).c_str()); 00423 return false; 00424 } 00425 00426 double wx = goal.pose.position.x; 00427 double wy = goal.pose.position.y; 00428 00429 //the potential has already been computed, so we won't update our copy of the costmap 00430 unsigned int mx, my; 00431 if(!costmap_.worldToMap(wx, wy, mx, my)){ 00432 ROS_WARN("The goal sent to the navfn planner is off the global costmap. Planning will always fail to this goal."); 00433 return false; 00434 } 00435 00436 int map_goal[2]; 00437 map_goal[0] = mx; 00438 map_goal[1] = my; 00439 00440 planner_->setStart(map_goal); 00441 00442 planner_->calcPath(costmap_ros_->getSizeInCellsX() * 4); 00443 00444 //extract the plan 00445 float *x = planner_->getPathX(); 00446 float *y = planner_->getPathY(); 00447 int len = planner_->getPathLen(); 00448 ros::Time plan_time = ros::Time::now(); 00449 std::string global_frame = costmap_ros_->getGlobalFrameID(); 00450 for(int i = len - 1; i >= 0; --i){ 00451 //convert the plan to world coordinates 00452 double world_x, world_y; 00453 mapToWorld(x[i], y[i], world_x, world_y); 00454 00455 geometry_msgs::PoseStamped pose; 00456 pose.header.stamp = plan_time; 00457 pose.header.frame_id = global_frame; 00458 pose.pose.position.x = world_x; 00459 pose.pose.position.y = world_y; 00460 pose.pose.position.z = 0.0; 00461 pose.pose.orientation.x = 0.0; 00462 pose.pose.orientation.y = 0.0; 00463 pose.pose.orientation.z = 0.0; 00464 pose.pose.orientation.w = 1.0; 00465 plan.push_back(pose); 00466 } 00467 00468 //publish the plan for visualization purposes 00469 publishPlan(plan, 0.0, 1.0, 0.0, 0.0); 00470 return !plan.empty(); 00471 } 00472 };