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  NavfnROS::NavfnROS()
51  : costmap_(NULL), planner_(), initialized_(false), allow_unknown_(true) {}
52 
53  NavfnROS::NavfnROS(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
54  : costmap_(NULL), planner_(), initialized_(false), allow_unknown_(true) {
55  //initialize the planner
56  initialize(name, costmap_ros);
57  }
58 
59  NavfnROS::NavfnROS(std::string name, costmap_2d::Costmap2D* costmap, std::string global_frame)
60  : costmap_(NULL), planner_(), initialized_(false), allow_unknown_(true) {
61  //initialize the planner
62  initialize(name, costmap, global_frame);
63  }
64 
65  void NavfnROS::initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string global_frame){
66  if(!initialized_){
67  costmap_ = costmap;
68  global_frame_ = global_frame;
70 
71  ros::NodeHandle private_nh("~/" + name);
72 
73  plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
74 
75  private_nh.param("visualize_potential", visualize_potential_, false);
76 
77  //if we're going to visualize the potential array we need to advertise
79  potarr_pub_.advertise(private_nh, "potential", 1);
80 
81  private_nh.param("allow_unknown", allow_unknown_, true);
82  private_nh.param("planner_window_x", planner_window_x_, 0.0);
83  private_nh.param("planner_window_y", planner_window_y_, 0.0);
84  private_nh.param("default_tolerance", default_tolerance_, 0.0);
85 
86  //get the tf prefix
87  ros::NodeHandle prefix_nh;
88  tf_prefix_ = tf::getPrefixParam(prefix_nh);
89 
90  make_plan_srv_ = private_nh.advertiseService("make_plan", &NavfnROS::makePlanService, this);
91 
92  initialized_ = true;
93  }
94  else
95  ROS_WARN("This planner has already been initialized, you can't call it twice, doing nothing");
96  }
97 
98  void NavfnROS::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
99  initialize(name, costmap_ros->getCostmap(), costmap_ros->getGlobalFrameID());
100  }
101 
102  bool NavfnROS::validPointPotential(const geometry_msgs::Point& world_point){
103  return validPointPotential(world_point, default_tolerance_);
104  }
105 
106  bool NavfnROS::validPointPotential(const geometry_msgs::Point& world_point, double tolerance){
107  if(!initialized_){
108  ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
109  return false;
110  }
111 
112  double resolution = costmap_->getResolution();
113  geometry_msgs::Point p;
114  p = world_point;
115 
116  p.y = world_point.y - tolerance;
117 
118  while(p.y <= world_point.y + tolerance){
119  p.x = world_point.x - tolerance;
120  while(p.x <= world_point.x + tolerance){
121  double potential = getPointPotential(p);
122  if(potential < POT_HIGH){
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  if(!initialized_){
135  ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
136  return -1.0;
137  }
138 
139  unsigned int mx, my;
140  if(!costmap_->worldToMap(world_point.x, world_point.y, mx, my))
141  return DBL_MAX;
142 
143  unsigned int index = my * planner_->nx + mx;
144  return planner_->potarr[index];
145  }
146 
147  bool NavfnROS::computePotential(const geometry_msgs::Point& world_point){
148  if(!initialized_){
149  ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
150  return false;
151  }
152 
153  //make sure to resize the underlying array that Navfn uses
155  planner_->setCostmap(costmap_->getCharMap(), true, allow_unknown_);
156 
157  unsigned int mx, my;
158  if(!costmap_->worldToMap(world_point.x, world_point.y, mx, my))
159  return false;
160 
161  int map_start[2];
162  map_start[0] = 0;
163  map_start[1] = 0;
164 
165  int map_goal[2];
166  map_goal[0] = mx;
167  map_goal[1] = my;
168 
169  planner_->setStart(map_start);
170  planner_->setGoal(map_goal);
171 
172  return planner_->calcNavFnDijkstra();
173  }
174 
175  void NavfnROS::clearRobotCell(const tf::Stamped<tf::Pose>& global_pose, unsigned int mx, unsigned int my){
176  if(!initialized_){
177  ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
178  return;
179  }
180 
181  //set the associated costs in the cost map to be free
183  }
184 
185  bool NavfnROS::makePlanService(nav_msgs::GetPlan::Request& req, nav_msgs::GetPlan::Response& resp){
186  makePlan(req.start, req.goal, resp.plan.poses);
187 
188  resp.plan.header.stamp = ros::Time::now();
189  resp.plan.header.frame_id = global_frame_;
190 
191  return true;
192  }
193 
194  void NavfnROS::mapToWorld(double mx, double my, double& wx, double& wy) {
195  wx = costmap_->getOriginX() + mx * costmap_->getResolution();
196  wy = costmap_->getOriginY() + my * costmap_->getResolution();
197  }
198 
199  bool NavfnROS::makePlan(const geometry_msgs::PoseStamped& start,
200  const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan){
201  return makePlan(start, goal, default_tolerance_, plan);
202  }
203 
204  bool NavfnROS::makePlan(const geometry_msgs::PoseStamped& start,
205  const geometry_msgs::PoseStamped& goal, double tolerance, std::vector<geometry_msgs::PoseStamped>& plan){
206  boost::mutex::scoped_lock lock(mutex_);
207  if(!initialized_){
208  ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
209  return false;
210  }
211 
212  //clear the plan, just in case
213  plan.clear();
214 
215  ros::NodeHandle n;
216 
217  //until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame
218  if(tf::resolve(tf_prefix_, goal.header.frame_id) != tf::resolve(tf_prefix_, global_frame_)){
219  ROS_ERROR("The goal pose passed to this planner must be in the %s frame. It is instead in the %s frame.",
220  tf::resolve(tf_prefix_, global_frame_).c_str(), tf::resolve(tf_prefix_, goal.header.frame_id).c_str());
221  return false;
222  }
223 
224  if(tf::resolve(tf_prefix_, start.header.frame_id) != tf::resolve(tf_prefix_, global_frame_)){
225  ROS_ERROR("The start pose passed to this planner must be in the %s frame. It is instead in the %s frame.",
226  tf::resolve(tf_prefix_, global_frame_).c_str(), tf::resolve(tf_prefix_, start.header.frame_id).c_str());
227  return false;
228  }
229 
230  double wx = start.pose.position.x;
231  double wy = start.pose.position.y;
232 
233  unsigned int mx, my;
234  if(!costmap_->worldToMap(wx, wy, mx, my)){
235  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?");
236  return false;
237  }
238 
239  //clear the starting cell within the costmap because we know it can't be an obstacle
240  tf::Stamped<tf::Pose> start_pose;
241  tf::poseStampedMsgToTF(start, start_pose);
242  clearRobotCell(start_pose, mx, my);
243 
244  //make sure to resize the underlying array that Navfn uses
246  planner_->setCostmap(costmap_->getCharMap(), true, allow_unknown_);
247 
248  int map_start[2];
249  map_start[0] = mx;
250  map_start[1] = my;
251 
252  wx = goal.pose.position.x;
253  wy = goal.pose.position.y;
254 
255  if(!costmap_->worldToMap(wx, wy, mx, my)){
256  if(tolerance <= 0.0){
257  ROS_WARN_THROTTLE(1.0, "The goal sent to the navfn planner is off the global costmap. Planning will always fail to this goal.");
258  return false;
259  }
260  mx = 0;
261  my = 0;
262  }
263 
264  int map_goal[2];
265  map_goal[0] = mx;
266  map_goal[1] = my;
267 
268  planner_->setStart(map_goal);
269  planner_->setGoal(map_start);
270 
271  //bool success = planner_->calcNavFnAstar();
272  planner_->calcNavFnDijkstra(true);
273 
274  double resolution = costmap_->getResolution();
275  geometry_msgs::PoseStamped p, best_pose;
276  p = goal;
277 
278  bool found_legal = false;
279  double best_sdist = DBL_MAX;
280 
281  p.pose.position.y = goal.pose.position.y - tolerance;
282 
283  while(p.pose.position.y <= goal.pose.position.y + tolerance){
284  p.pose.position.x = goal.pose.position.x - tolerance;
285  while(p.pose.position.x <= goal.pose.position.x + tolerance){
286  double potential = getPointPotential(p.pose.position);
287  double sdist = sq_distance(p, goal);
288  if(potential < POT_HIGH && sdist < best_sdist){
289  best_sdist = sdist;
290  best_pose = p;
291  found_legal = true;
292  }
293  p.pose.position.x += resolution;
294  }
295  p.pose.position.y += resolution;
296  }
297 
298  if(found_legal){
299  //extract the plan
300  if(getPlanFromPotential(best_pose, plan)){
301  //make sure the goal we push on has the same timestamp as the rest of the plan
302  geometry_msgs::PoseStamped goal_copy = best_pose;
303  goal_copy.header.stamp = ros::Time::now();
304  plan.push_back(goal_copy);
305  }
306  else{
307  ROS_ERROR("Failed to get a plan from potential when a legal potential was found. This shouldn't happen.");
308  }
309  }
310 
312  //publish potential array
313  pcl::PointCloud<PotarrPoint> pot_area;
314  pot_area.header.frame_id = global_frame_;
315  pot_area.points.clear();
316  std_msgs::Header header;
317  pcl_conversions::fromPCL(pot_area.header, header);
318  header.stamp = ros::Time::now();
319  pot_area.header = pcl_conversions::toPCL(header);
320 
321  PotarrPoint pt;
322  float *pp = planner_->potarr;
323  double pot_x, pot_y;
324  for (unsigned int i = 0; i < (unsigned int)planner_->ny*planner_->nx ; i++)
325  {
326  if (pp[i] < 10e7)
327  {
328  mapToWorld(i%planner_->nx, i/planner_->nx, pot_x, pot_y);
329  pt.x = pot_x;
330  pt.y = pot_y;
331  pt.z = pp[i]/pp[planner_->start[1]*planner_->nx + planner_->start[0]]*20;
332  pt.pot_value = pp[i];
333  pot_area.push_back(pt);
334  }
335  }
336  potarr_pub_.publish(pot_area);
337  }
338 
339  //publish the plan for visualization purposes
340  publishPlan(plan, 0.0, 1.0, 0.0, 0.0);
341 
342  return !plan.empty();
343  }
344 
345  void NavfnROS::publishPlan(const std::vector<geometry_msgs::PoseStamped>& path, double r, double g, double b, double a){
346  if(!initialized_){
347  ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
348  return;
349  }
350 
351  //create a message for the plan
352  nav_msgs::Path gui_path;
353  gui_path.poses.resize(path.size());
354 
355  if(!path.empty())
356  {
357  gui_path.header.frame_id = path[0].header.frame_id;
358  gui_path.header.stamp = path[0].header.stamp;
359  }
360 
361  // Extract the plan in world co-ordinates, we assume the path is all in the same frame
362  for(unsigned int i=0; i < path.size(); i++){
363  gui_path.poses[i] = path[i];
364  }
365 
366  plan_pub_.publish(gui_path);
367  }
368 
369  bool NavfnROS::getPlanFromPotential(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan){
370  if(!initialized_){
371  ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
372  return false;
373  }
374 
375  //clear the plan, just in case
376  plan.clear();
377 
378  //until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame
379  if(tf::resolve(tf_prefix_, goal.header.frame_id) != tf::resolve(tf_prefix_, global_frame_)){
380  ROS_ERROR("The goal pose passed to this planner must be in the %s frame. It is instead in the %s frame.",
381  tf::resolve(tf_prefix_, global_frame_).c_str(), tf::resolve(tf_prefix_, goal.header.frame_id).c_str());
382  return false;
383  }
384 
385  double wx = goal.pose.position.x;
386  double wy = goal.pose.position.y;
387 
388  //the potential has already been computed, so we won't update our copy of the costmap
389  unsigned int mx, my;
390  if(!costmap_->worldToMap(wx, wy, mx, my)){
391  ROS_WARN_THROTTLE(1.0, "The goal sent to the navfn planner is off the global costmap. Planning will always fail to this goal.");
392  return false;
393  }
394 
395  int map_goal[2];
396  map_goal[0] = mx;
397  map_goal[1] = my;
398 
399  planner_->setStart(map_goal);
400 
401  planner_->calcPath(costmap_->getSizeInCellsX() * 4);
402 
403  //extract the plan
404  float *x = planner_->getPathX();
405  float *y = planner_->getPathY();
406  int len = planner_->getPathLen();
407  ros::Time plan_time = ros::Time::now();
408 
409  for(int i = len - 1; i >= 0; --i){
410  //convert the plan to world coordinates
411  double world_x, world_y;
412  mapToWorld(x[i], y[i], world_x, world_y);
413 
414  geometry_msgs::PoseStamped pose;
415  pose.header.stamp = plan_time;
416  pose.header.frame_id = global_frame_;
417  pose.pose.position.x = world_x;
418  pose.pose.position.y = world_y;
419  pose.pose.position.z = 0.0;
420  pose.pose.orientation.x = 0.0;
421  pose.pose.orientation.y = 0.0;
422  pose.pose.orientation.z = 0.0;
423  pose.pose.orientation.w = 1.0;
424  plan.push_back(pose);
425  }
426 
427  //publish the plan for visualization purposes
428  publishPlan(plan, 0.0, 1.0, 0.0, 0.0);
429  return !plan.empty();
430  }
431 };
void setCost(unsigned int mx, unsigned int my, unsigned char cost)
Definition: navfn.h:81
bool visualize_potential_
Definition: navfn_ros.h:171
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:345
boost::shared_ptr< NavFn > planner_
Definition: navfn_ros.h:168
void publish(const boost::shared_ptr< M > &message) const
std::string getPrefixParam(ros::NodeHandle &nh)
ros::Publisher plan_pub_
Definition: navfn_ros.h:169
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:147
std::string getGlobalFrameID()
PLUGINLIB_EXPORT_CLASS(BAGReader, nodelet::Nodelet)
costmap_2d::Costmap2D * costmap_
Store a copy of the current costmap in costmap. Called by makePlan.
Definition: navfn_ros.h:167
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:171
double getOriginX() const
std::string tf_prefix_
Definition: navfn_ros.h:184
void clearRobotCell(const tf::Stamped< tf::Pose > &global_pose, unsigned int mx, unsigned int my)
Definition: navfn_ros.cpp:175
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:102
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:199
TFSIMD_FORCE_INLINE const tfScalar & y() const
static const unsigned char FREE_SPACE
#define ROS_WARN(...)
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)
ros::ServiceServer make_plan_srv_
Definition: navfn_ros.h:186
#define ROS_WARN_THROTTLE(period,...)
double sq_distance(const geometry_msgs::PoseStamped &p1, const geometry_msgs::PoseStamped &p2)
Definition: navfn_ros.h:175
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:185
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:369
double getOriginY() const
bool makePlanService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp)
Definition: navfn_ros.cpp:185
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
unsigned int getSizeInCellsY() const
double default_tolerance_
Definition: navfn_ros.h:183
double planner_window_y_
Definition: navfn_ros.h:183
double planner_window_x_
Definition: navfn_ros.h:183
NavfnROS()
Default constructor for the NavFnROS object.
Definition: navfn_ros.cpp:50
unsigned int getSizeInCellsX() const
void mapToWorld(double mx, double my, double &wx, double &wy)
Definition: navfn_ros.cpp:194
static Time now()
static void poseStampedMsgToTF(const geometry_msgs::PoseStamped &msg, Stamped< Pose > &bt)
const std::string header
pcl_ros::Publisher< PotarrPoint > potarr_pub_
Definition: navfn_ros.h:170
std::string global_frame_
Definition: navfn_ros.h:187
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:98
void toPCL(const ros::Time &stamp, pcl::uint64_t &pcl_stamp)


navfn
Author(s): Kurt Konolige, Eitan Marder-Eppstein, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:44:39