navfn_ros.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 *
3 * Software License Agreement (BSD License)
4 *
5 * Copyright (c) 2008, Willow Garage, Inc.
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of the Willow Garage nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *
35 * Author: Eitan Marder-Eppstein
36 *********************************************************************/
37 #include <navfn/navfn_ros.h>
39 #include <tf/transform_listener.h>
40 #include <costmap_2d/cost_values.h>
41 #include <costmap_2d/costmap_2d.h>
42 
44 
45 //register this planner as a BaseGlobalPlanner plugin
47 
48 namespace navfn
49 {
50 
51  NavfnROS::NavfnROS()
52  : costmap_ros_(NULL), planner_(), initialized_(false), allow_unknown_(true) {}
53 
54  NavfnROS::NavfnROS(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
55  : costmap_ros_(NULL), planner_(), initialized_(false), allow_unknown_(true)
56  {
57  //initialize the planner
58  initialize(name, costmap_ros);
59  }
60 
61  void NavfnROS::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
62  {
63  if(!initialized_)
64  {
65  costmap_ros_ = costmap_ros;
68 
69  ros::NodeHandle private_nh("~/Navfn");
70 
71  plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
72 
73  private_nh.param("visualize_potential", visualize_potential_, false);
74 
75  //if we're going to visualize the potential array we need to advertise
77  potarr_pub_.advertise(private_nh, "potential", 1);
78 
79  private_nh.param("allow_unknown", allow_unknown_, true);
80  private_nh.param("planner_window_x", planner_window_x_, 0.0);
81  private_nh.param("planner_window_y", planner_window_y_, 0.0);
82  private_nh.param("default_tolerance", default_tolerance_, 0.0);
83 
84  //get the tf prefix
85  ros::NodeHandle prefix_nh;
86  tf_prefix_ = tf::getPrefixParam(prefix_nh);
87 
88  make_plan_srv_ = private_nh.advertiseService("make_plan", &NavfnROS::makePlanService, this);
89 
90  initialized_ = true;
91  }
92  else
93  ROS_WARN("This planner has already been initialized, you can't call it twice, doing nothing");
94  }
95 
96  bool NavfnROS::validPointPotential(const geometry_msgs::Point& world_point)
97  {
98  return validPointPotential(world_point, default_tolerance_);
99  }
100 
101  bool NavfnROS::validPointPotential(const geometry_msgs::Point& world_point, double tolerance)
102  {
103  if(!initialized_)
104  {
105  ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
106  return false;
107  }
108 
109  double resolution = costmap_ros_->getCostmap()->getResolution();
110  geometry_msgs::Point p;
111  p = world_point;
112 
113  p.y = world_point.y - tolerance;
114 
115  while(p.y <= world_point.y + tolerance)
116  {
117  p.x = world_point.x - tolerance;
118  while(p.x <= world_point.x + tolerance)
119  {
120  double potential = getPointPotential(p);
121  if(potential < POT_HIGH)
122  {
123  return true;
124  }
125  p.x += resolution;
126  }
127  p.y += resolution;
128  }
129 
130  return false;
131  }
132 
133  double NavfnROS::getPointPotential(const geometry_msgs::Point& world_point)
134  {
135  if(!initialized_)
136  {
137  ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
138  return -1.0;
139  }
140 
141  unsigned int mx, my;
142  if(!costmap_ros_->getCostmap()->worldToMap(world_point.x, world_point.y, mx, my))
143  return DBL_MAX;
144 
145  unsigned int index = my * planner_->nx + mx;
146  return planner_->potarr[index];
147  }
148 
149  bool NavfnROS::computePotential(const geometry_msgs::Point& world_point)
150  {
151  if(!initialized_)
152  {
153  ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
154  return false;
155  }
156 
158 
159  //make sure to resize the underlying array that Navfn uses
160  planner_->setNavArr(costmap->getSizeInCellsX(), costmap->getSizeInCellsY());
161  planner_->setCostmap(costmap->getCharMap(), true, allow_unknown_);
162 
163  unsigned int mx, my;
164  if(!costmap->worldToMap(world_point.x, world_point.y, mx, my))
165  return false;
166 
167  int map_start[2];
168  map_start[0] = 0;
169  map_start[1] = 0;
170 
171  int map_goal[2];
172  map_goal[0] = mx;
173  map_goal[1] = my;
174 
175  planner_->setStart(map_start);
176  planner_->setGoal(map_goal);
177 
178  return planner_->calcNavFnDijkstra();
179  }
180 
181  void NavfnROS::clearRobotCell(const tf::Stamped<tf::Pose>& global_pose, unsigned int mx, unsigned int my)
182  {
183  if(!initialized_)
184  {
185  ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
186  return;
187  }
188 
189  //set the associated costs in the cost map to be free
191  }
192 
193  bool NavfnROS::makePlanService(nav_msgs::GetPlan::Request& req, nav_msgs::GetPlan::Response& resp)
194  {
195  makePlan(req.start, req.goal, resp.plan.poses);
196 
197  resp.plan.header.stamp = ros::Time::now();
198  resp.plan.header.frame_id = costmap_ros_->getGlobalFrameID();
199 
200  return true;
201  }
202 
203  void NavfnROS::mapToWorld(double mx, double my, double& wx, double& wy)
204  {
206  wx = costmap->getOriginX() + mx * costmap->getResolution();
207  wy = costmap->getOriginY() + my * costmap->getResolution();
208  }
209 
210  bool NavfnROS::makePlan(const geometry_msgs::PoseStamped& start,
211  const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan)
212  {
213  return makePlan(start, goal, default_tolerance_, plan);
214  }
215 
216  bool NavfnROS::makePlan(const geometry_msgs::PoseStamped& start,
217  const geometry_msgs::PoseStamped& goal, double tolerance, std::vector<geometry_msgs::PoseStamped>& plan)
218  {
219  boost::mutex::scoped_lock lock(mutex_);
220  if(!initialized_)
221  {
222  ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
223  return false;
224  }
225 
226  //clear the plan, just in case
227  plan.clear();
228 
229  ros::NodeHandle n;
231  std::string global_frame = costmap_ros_->getGlobalFrameID();
232 
233  //until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame
234  if(tf::resolve(tf_prefix_, goal.header.frame_id) != tf::resolve(tf_prefix_, global_frame))
235  {
236  ROS_ERROR("The goal pose passed to this planner must be in the %s frame. It is instead in the %s frame.",
237  tf::resolve(tf_prefix_, global_frame).c_str(), tf::resolve(tf_prefix_, goal.header.frame_id).c_str());
238  return false;
239  }
240 
241  if(tf::resolve(tf_prefix_, start.header.frame_id) != tf::resolve(tf_prefix_, global_frame))
242  {
243  ROS_ERROR("The start pose passed to this planner must be in the %s frame. It is instead in the %s frame.",
244  tf::resolve(tf_prefix_, global_frame).c_str(), tf::resolve(tf_prefix_, start.header.frame_id).c_str());
245  return false;
246  }
247 
248  double wx = start.pose.position.x;
249  double wy = start.pose.position.y;
250 
251  unsigned int mx, my;
252  if(!costmap->worldToMap(wx, wy, mx, my))
253  {
254  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?");
255  return false;
256  }
257 
258  //clear the starting cell within the costmap because we know it can't be an obstacle
259  // tf::Stamped<tf::Pose> start_pose;
260  // tf::poseStampedMsgToTF(start, start_pose);
261  // clearRobotCell(start_pose, mx, my);
262 
263 #if 0
264  {
265  static int n = 0;
266  static char filename[1000];
267  snprintf( filename, 1000, "navfnros-makeplan-costmapB-%04d.pgm", n++ );
268  costmap->saveRawMap( std::string( filename ));
269  }
270 #endif
271 
272  //make sure to resize the underlying array that Navfn uses
273  planner_->setNavArr(costmap->getSizeInCellsX(), costmap->getSizeInCellsY());
274  planner_->setCostmap(costmap->getCharMap(), true, allow_unknown_);
275 
276 #if 0
277  {
278  static int n = 0;
279  static char filename[1000];
280  snprintf( filename, 1000, "navfnros-makeplan-costmapC-%04d", n++ );
281  planner_->savemap( filename );
282  }
283 #endif
284 
285  int map_start[2];
286  map_start[0] = mx;
287  map_start[1] = my;
288 
289  wx = goal.pose.position.x;
290  wy = goal.pose.position.y;
291 
292  if(!costmap->worldToMap(wx, wy, mx, my))
293  {
294  if(tolerance <= 0.0)
295  {
296  ROS_WARN_THROTTLE(1.0, "The goal sent to the navfn planner is off the global costmap. Planning will always fail to this goal.");
297  return false;
298  }
299  mx = 0;
300  my = 0;
301  }
302 
303  int map_goal[2];
304  map_goal[0] = mx;
305  map_goal[1] = my;
306 
307  planner_->setStart(map_goal);
308  planner_->setGoal(map_start);
309 
310  //bool success = planner_->calcNavFnAstar();
311  planner_->calcNavFnDijkstra(true);
312 
313  double resolution = costmap->getResolution();
314  geometry_msgs::PoseStamped p, best_pose;
315  p = goal;
316 
317  bool found_legal = false;
318  double best_sdist = DBL_MAX;
319 
320  p.pose.position.y = goal.pose.position.y - tolerance;
321 
322  while(p.pose.position.y <= goal.pose.position.y + tolerance)
323  {
324  p.pose.position.x = goal.pose.position.x - tolerance;
325  while(p.pose.position.x <= goal.pose.position.x + tolerance)
326  {
327  double potential = getPointPotential(p.pose.position);
328  double sdist = sq_distance(p, goal);
329  if(potential < POT_HIGH && sdist < best_sdist)
330  {
331  best_sdist = sdist;
332  best_pose = p;
333  found_legal = true;
334  }
335  p.pose.position.x += resolution;
336  }
337  p.pose.position.y += resolution;
338  }
339 
340  if(found_legal)
341  {
342  //extract the plan
343  if(getPlanFromPotential(best_pose, plan))
344  {
345  //make sure the goal we push on has the same timestamp as the rest of the plan
346  geometry_msgs::PoseStamped goal_copy = best_pose;
347  goal_copy.header.stamp = ros::Time::now();
348  plan.push_back(goal_copy);
349  }
350  else
351  {
352  ROS_ERROR("Failed to get a plan from potential when a legal potential was found. This shouldn't happen.");
353  }
354  }
355 
357  {
358  //publish potential array
359  pcl::PointCloud<PotarrPoint> pot_area;
360  pot_area.header.frame_id = global_frame;
361  pot_area.points.clear();
362  std_msgs::Header header;
363  pcl_conversions::fromPCL(pot_area.header, header);
364  header.stamp = ros::Time::now();
365  pot_area.header = pcl_conversions::toPCL(header);
366 
367  PotarrPoint pt;
368  float *pp = planner_->potarr;
369  double pot_x, pot_y;
370  for (unsigned int i = 0; i < (unsigned int)planner_->ny*planner_->nx ; i++)
371  {
372  if (pp[i] < 10e7)
373  {
374  mapToWorld(i%planner_->nx, i/planner_->nx, pot_x, pot_y);
375  pt.x = pot_x;
376  pt.y = pot_y;
377  pt.z = pp[i]/pp[planner_->start[1]*planner_->nx + planner_->start[0]]*20;
378  pt.pot_value = pp[i];
379  pot_area.push_back(pt);
380  }
381  }
382  potarr_pub_.publish(pot_area);
383  }
384 
385 
386 
387  unsigned int mx2;
388  unsigned int my2;
389  ROS_DEBUG("Navfn: Gloal position: x %f, y %f",plan.back().pose.position.x,plan.back().pose.position.y );
390  costmap->worldToMap(plan.back().pose.position.x, plan.back().pose.position.y, mx2, my2);
391 
392  if(costmap->getCost(mx2,my2) > 180)
393  {
394  ROS_DEBUG("Navfn: Goal in obstabel.");
395  while(costmap->getCost(mx2,my2) > 180){
396  plan.pop_back();
397  costmap->worldToMap(plan.back().pose.position.x, plan.back().pose.position.y, mx2, my2);
398  }
399  plan.back().pose.orientation = goal.pose.orientation;
400  }
401  //publish the plan for visualization purposes
402  publishPlan(plan, 0.0, 1.0, 0.0, 0.0);
403 
404  return !plan.empty();
405  }
406 
407  void NavfnROS::publishPlan(const std::vector<geometry_msgs::PoseStamped>& path, double r, double g, double b, double a)
408  {
409  if(!initialized_)
410  {
411  ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
412  return;
413  }
414 
415  //create a message for the plan
416  nav_msgs::Path gui_path;
417  gui_path.poses.resize(path.size());
418 
419  if(!path.empty())
420  {
421  gui_path.header.frame_id = path[0].header.frame_id;
422  gui_path.header.stamp = path[0].header.stamp;
423  }
424 
425  // Extract the plan in world co-ordinates, we assume the path is all in the same frame
426  for(unsigned int i=0; i < path.size(); i++)
427  {
428  gui_path.poses[i] = path[i];
429  }
430 
431  plan_pub_.publish(gui_path);
432  }
433 
434  bool NavfnROS::getPlanFromPotential(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan)
435  {
436  if(!initialized_)
437  {
438  ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
439  return false;
440  }
441 
443  std::string global_frame = costmap_ros_->getGlobalFrameID();
444 
445  //clear the plan, just in case
446  plan.clear();
447 
448  //until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame
449  if(tf::resolve(tf_prefix_, goal.header.frame_id) != tf::resolve(tf_prefix_, global_frame))
450  {
451  ROS_ERROR("The goal pose passed to this planner must be in the %s frame. It is instead in the %s frame.",
452  tf::resolve(tf_prefix_, global_frame).c_str(), tf::resolve(tf_prefix_, goal.header.frame_id).c_str());
453  return false;
454  }
455 
456  double wx = goal.pose.position.x;
457  double wy = goal.pose.position.y;
458 
459  //the potential has already been computed, so we won't update our copy of the costmap
460  unsigned int mx, my;
461  if(!costmap->worldToMap(wx, wy, mx, my))
462  {
463  ROS_WARN_THROTTLE(1.0, "The goal sent to the navfn planner is off the global costmap. Planning will always fail to this goal.");
464  return false;
465  }
466 
467  int map_goal[2];
468  map_goal[0] = mx;
469  map_goal[1] = my;
470 
471  planner_->setStart(map_goal);
472 
473  planner_->calcPath(costmap->getSizeInCellsX() * 4);
474 
475  //extract the plan
476  float *x = planner_->getPathX();
477  float *y = planner_->getPathY();
478  int len = planner_->getPathLen();
479  ros::Time plan_time = ros::Time::now();
480 
481  for(int i = len - 1; i >= 0; --i)
482  {
483  //convert the plan to world coordinates
484  double world_x, world_y;
485  mapToWorld(x[i], y[i], world_x, world_y);
486 
487  geometry_msgs::PoseStamped pose;
488  pose.header.stamp = plan_time;
489  pose.header.frame_id = global_frame;
490  pose.pose.position.x = world_x;
491  pose.pose.position.y = world_y;
492  pose.pose.position.z = 0.0;
493  pose.pose.orientation.x = 0.0;
494  pose.pose.orientation.y = 0.0;
495  pose.pose.orientation.z = 0.0;
496  pose.pose.orientation.w = 1.0;
497  plan.push_back(pose);
498  }
499 
500  //publish the plan for visualization purposes
501  publishPlan(plan, 0.0, 1.0, 0.0, 0.0);
502  return !plan.empty();
503  }
504 };
void setCost(unsigned int mx, unsigned int my, unsigned char cost)
filename
Definition: navfn.h:81
bool visualize_potential_
Definition: navfn_ros.h:155
void publishPlan(const std::vector< geometry_msgs::PoseStamped > &path, double r, double g, double b, double a)
Publish a path for visualization purposes.
Definition: navfn_ros.cpp:407
#define ROS_WARN_THROTTLE(rate,...)
boost::shared_ptr< NavFn > planner_
Definition: navfn_ros.h:152
void publish(const boost::shared_ptr< M > &message) const
std::string getPrefixParam(ros::NodeHandle &nh)
ros::Publisher plan_pub_
Definition: navfn_ros.h:153
bool computePotential(const geometry_msgs::Point &world_point)
Computes the full navigation function for the map given a point in the world to start from...
Definition: navfn_ros.cpp:149
costmap_2d::Costmap2DROS * costmap_ros_
Store a copy of the current costmap in costmap. Called by makePlan.
Definition: navfn_ros.h:151
std::string getGlobalFrameID()
Provides a ROS wrapper for the navfn planner which runs a fast, interpolated navigation function on a...
Definition: navfn_ros.h:58
bool allow_unknown_
Definition: navfn_ros.h:155
double getOriginX() const
std::string tf_prefix_
Definition: navfn_ros.h:168
void clearRobotCell(const tf::Stamped< tf::Pose > &global_pose, unsigned int mx, unsigned int my)
Definition: navfn_ros.cpp:181
bool validPointPotential(const geometry_msgs::Point &world_point)
Check for a valid potential value at a given point in the world (Note: You should call computePotenti...
Definition: navfn_ros.cpp:96
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
bool makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, std::vector< geometry_msgs::PoseStamped > &plan)
Given a goal pose in the world, compute a plan.
Definition: navfn_ros.cpp:210
TFSIMD_FORCE_INLINE const tfScalar & y() const
static const unsigned char FREE_SPACE
#define ROS_WARN(...)
#define PLUGINLIB_DECLARE_CLASS(pkg, class_name, class_type, base_class_type)
unsigned char * getCharMap() const
std::string resolve(const std::string &prefix, const std::string &frame_name)
void fromPCL(const pcl::uint64_t &pcl_stamp, ros::Time &stamp)
unsigned char getCost(unsigned int mx, unsigned int my) const
ros::ServiceServer make_plan_srv_
Definition: navfn_ros.h:170
double sq_distance(const geometry_msgs::PoseStamped &p1, const geometry_msgs::PoseStamped &p2)
Definition: navfn_ros.h:159
Navigation function class. Holds buffers for costmap, navfn map. Maps are pixel-based. Origin is upper left, x is right, y is down.
Definition: navfn.h:106
bool param(const std::string &param_name, T &param_val, const T &default_val) const
boost::mutex mutex_
Definition: navfn_ros.h:169
double getPointPotential(const geometry_msgs::Point &world_point)
Get the potential, or naviagation cost, at a given point in the world (Note: You should call computeP...
Definition: navfn_ros.cpp:133
TFSIMD_FORCE_INLINE const tfScalar & x() const
bool getPlanFromPotential(const geometry_msgs::PoseStamped &goal, std::vector< geometry_msgs::PoseStamped > &plan)
Compute a plan to a goal after the potential for a start point has already been computed (Note: You s...
Definition: navfn_ros.cpp:434
double getOriginY() const
bool makePlanService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp)
Definition: navfn_ros.cpp:193
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
unsigned int getSizeInCellsY() const
double default_tolerance_
Definition: navfn_ros.h:167
double planner_window_y_
Definition: navfn_ros.h:167
double planner_window_x_
Definition: navfn_ros.h:167
NavfnROS()
Default constructor for the NavFnROS object.
Definition: navfn_ros.cpp:51
unsigned int getSizeInCellsX() const
void mapToWorld(double mx, double my, double &wx, double &wy)
Definition: navfn_ros.cpp:203
static Time now()
pcl_ros::Publisher< PotarrPoint > potarr_pub_
Definition: navfn_ros.h:154
double getResolution() const
Costmap2D * getCostmap()
#define ROS_ERROR(...)
bool worldToMap(double wx, double wy, unsigned int &mx, unsigned int &my) const
void initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros)
Initialization function for the NavFnROS object.
Definition: navfn_ros.cpp:61
void toPCL(const ros::Time &stamp, pcl::uint64_t &pcl_stamp)
#define ROS_DEBUG(...)


asr_navfn
Author(s): Kurt Konolige, Eitan Marder-Eppstein, contradict@gmail.com, Felix Marek
autogenerated on Mon Jun 10 2019 12:43:25