planner_core.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2008, 2013, 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 Willow Garage, Inc. 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  * David V. Lu!!
37  *********************************************************************/
40 #include <tf/transform_listener.h>
41 #include <costmap_2d/cost_values.h>
42 #include <costmap_2d/costmap_2d.h>
43 
45 #include <global_planner/astar.h>
49 
50 //register this planner as a BaseGlobalPlanner plugin
52 
53 namespace global_planner {
54 
55 void GlobalPlanner::outlineMap(unsigned char* costarr, int nx, int ny, unsigned char value) {
56  unsigned char* pc = costarr;
57  for (int i = 0; i < nx; i++)
58  *pc++ = value;
59  pc = costarr + (ny - 1) * nx;
60  for (int i = 0; i < nx; i++)
61  *pc++ = value;
62  pc = costarr;
63  for (int i = 0; i < ny; i++, pc += nx)
64  *pc = value;
65  pc = costarr + nx - 1;
66  for (int i = 0; i < ny; i++, pc += nx)
67  *pc = value;
68 }
69 
70 GlobalPlanner::GlobalPlanner() :
71  costmap_(NULL), initialized_(false), allow_unknown_(true) {
72 }
73 
74 GlobalPlanner::GlobalPlanner(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id) :
75  costmap_(NULL), initialized_(false), allow_unknown_(true) {
76  //initialize the planner
77  initialize(name, costmap, frame_id);
78 }
79 
81  if (p_calc_)
82  delete p_calc_;
83  if (planner_)
84  delete planner_;
85  if (path_maker_)
86  delete path_maker_;
87  if (dsrv_)
88  delete dsrv_;
89 }
90 
91 void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) {
92  initialize(name, costmap_ros->getCostmap(), costmap_ros->getGlobalFrameID());
93 }
94 
95 void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id) {
96  if (!initialized_) {
97  ros::NodeHandle private_nh("~/" + name);
98  costmap_ = costmap;
99  frame_id_ = frame_id;
100 
101  unsigned int cx = costmap->getSizeInCellsX(), cy = costmap->getSizeInCellsY();
102 
103  private_nh.param("old_navfn_behavior", old_navfn_behavior_, false);
105  convert_offset_ = 0.5;
106  else
107  convert_offset_ = 0.0;
108 
109  bool use_quadratic;
110  private_nh.param("use_quadratic", use_quadratic, true);
111  if (use_quadratic)
112  p_calc_ = new QuadraticCalculator(cx, cy);
113  else
114  p_calc_ = new PotentialCalculator(cx, cy);
115 
116  bool use_dijkstra;
117  private_nh.param("use_dijkstra", use_dijkstra, true);
118  if (use_dijkstra)
119  {
120  DijkstraExpansion* de = new DijkstraExpansion(p_calc_, cx, cy);
122  de->setPreciseStart(true);
123  planner_ = de;
124  }
125  else
126  planner_ = new AStarExpansion(p_calc_, cx, cy);
127 
128  bool use_grid_path;
129  private_nh.param("use_grid_path", use_grid_path, false);
130  if (use_grid_path)
132  else
134 
136 
137  plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
138  potential_pub_ = private_nh.advertise<nav_msgs::OccupancyGrid>("potential", 1);
139 
140  private_nh.param("allow_unknown", allow_unknown_, true);
142  private_nh.param("planner_window_x", planner_window_x_, 0.0);
143  private_nh.param("planner_window_y", planner_window_y_, 0.0);
144  private_nh.param("default_tolerance", default_tolerance_, 0.0);
145  private_nh.param("publish_scale", publish_scale_, 100);
146 
147  //get the tf prefix
148  ros::NodeHandle prefix_nh;
149  tf_prefix_ = tf::getPrefixParam(prefix_nh);
150 
151  make_plan_srv_ = private_nh.advertiseService("make_plan", &GlobalPlanner::makePlanService, this);
152 
153  dsrv_ = new dynamic_reconfigure::Server<global_planner::GlobalPlannerConfig>(ros::NodeHandle("~/" + name));
154  dynamic_reconfigure::Server<global_planner::GlobalPlannerConfig>::CallbackType cb = boost::bind(
155  &GlobalPlanner::reconfigureCB, this, _1, _2);
156  dsrv_->setCallback(cb);
157 
158  initialized_ = true;
159  } else
160  ROS_WARN("This planner has already been initialized, you can't call it twice, doing nothing");
161 
162 }
163 
164 void GlobalPlanner::reconfigureCB(global_planner::GlobalPlannerConfig& config, uint32_t level) {
165  planner_->setLethalCost(config.lethal_cost);
166  path_maker_->setLethalCost(config.lethal_cost);
167  planner_->setNeutralCost(config.neutral_cost);
168  planner_->setFactor(config.cost_factor);
169  publish_potential_ = config.publish_potential;
170  orientation_filter_->setMode(config.orientation_mode);
171  orientation_filter_->setWindowSize(config.orientation_window_size);
172 }
173 
174 void GlobalPlanner::clearRobotCell(const tf::Stamped<tf::Pose>& global_pose, unsigned int mx, unsigned int my) {
175  if (!initialized_) {
176  ROS_ERROR(
177  "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 GlobalPlanner::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 = frame_id_;
190 
191  return true;
192 }
193 
194 void GlobalPlanner::mapToWorld(double mx, double my, double& wx, double& wy) {
197 }
198 
199 bool GlobalPlanner::worldToMap(double wx, double wy, double& mx, double& my) {
200  double origin_x = costmap_->getOriginX(), origin_y = costmap_->getOriginY();
201  double resolution = costmap_->getResolution();
202 
203  if (wx < origin_x || wy < origin_y)
204  return false;
205 
206  mx = (wx - origin_x) / resolution - convert_offset_;
207  my = (wy - origin_y) / resolution - convert_offset_;
208 
209  if (mx < costmap_->getSizeInCellsX() && my < costmap_->getSizeInCellsY())
210  return true;
211 
212  return false;
213 }
214 
215 bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
216  std::vector<geometry_msgs::PoseStamped>& plan) {
217  return makePlan(start, goal, default_tolerance_, plan);
218 }
219 
220 bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
221  double tolerance, std::vector<geometry_msgs::PoseStamped>& plan) {
222  boost::mutex::scoped_lock lock(mutex_);
223  if (!initialized_) {
224  ROS_ERROR(
225  "This planner has not been initialized yet, but it is being used, please call initialize() before use");
226  return false;
227  }
228 
229  //clear the plan, just in case
230  plan.clear();
231 
232  ros::NodeHandle n;
233  std::string global_frame = frame_id_;
234 
235  //until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame
236  if (tf::resolve(tf_prefix_, goal.header.frame_id) != tf::resolve(tf_prefix_, global_frame)) {
237  ROS_ERROR(
238  "The goal pose passed to this planner must be in the %s frame. It is instead in the %s frame.", tf::resolve(tf_prefix_, global_frame).c_str(), tf::resolve(tf_prefix_, goal.header.frame_id).c_str());
239  return false;
240  }
241 
242  if (tf::resolve(tf_prefix_, start.header.frame_id) != tf::resolve(tf_prefix_, global_frame)) {
243  ROS_ERROR(
244  "The start pose passed to this planner must be in the %s frame. It is instead in the %s frame.", 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 start_x_i, start_y_i, goal_x_i, goal_y_i;
252  double start_x, start_y, goal_x, goal_y;
253 
254  if (!costmap_->worldToMap(wx, wy, start_x_i, start_y_i)) {
255  ROS_WARN(
256  "The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?");
257  return false;
258  }
260  start_x = start_x_i;
261  start_y = start_y_i;
262  }else{
263  worldToMap(wx, wy, start_x, start_y);
264  }
265 
266  wx = goal.pose.position.x;
267  wy = goal.pose.position.y;
268 
269  if (!costmap_->worldToMap(wx, wy, goal_x_i, goal_y_i)) {
270  ROS_WARN_THROTTLE(1.0,
271  "The goal sent to the global planner is off the global costmap. Planning will always fail to this goal.");
272  return false;
273  }
275  goal_x = goal_x_i;
276  goal_y = goal_y_i;
277  }else{
278  worldToMap(wx, wy, goal_x, goal_y);
279  }
280 
281  //clear the starting cell within the costmap because we know it can't be an obstacle
282  tf::Stamped<tf::Pose> start_pose;
283  tf::poseStampedMsgToTF(start, start_pose);
284  clearRobotCell(start_pose, start_x_i, start_y_i);
285 
286  int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();
287 
288  //make sure to resize the underlying array that Navfn uses
289  p_calc_->setSize(nx, ny);
290  planner_->setSize(nx, ny);
291  path_maker_->setSize(nx, ny);
292  potential_array_ = new float[nx * ny];
293 
295 
296  bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(), start_x, start_y, goal_x, goal_y,
297  nx * ny * 2, potential_array_);
298 
300  planner_->clearEndpoint(costmap_->getCharMap(), potential_array_, goal_x_i, goal_y_i, 2);
303 
304  if (found_legal) {
305  //extract the plan
306  if (getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, plan)) {
307  //make sure the goal we push on has the same timestamp as the rest of the plan
308  geometry_msgs::PoseStamped goal_copy = goal;
309  goal_copy.header.stamp = ros::Time::now();
310  plan.push_back(goal_copy);
311  } else {
312  ROS_ERROR("Failed to get a plan from potential when a legal potential was found. This shouldn't happen.");
313  }
314  }else{
315  ROS_ERROR("Failed to get a plan.");
316  }
317 
318  // add orientations if needed
319  orientation_filter_->processPath(start, plan);
320 
321  //publish the plan for visualization purposes
322  publishPlan(plan);
323  delete potential_array_;
324  return !plan.empty();
325 }
326 
327 void GlobalPlanner::publishPlan(const std::vector<geometry_msgs::PoseStamped>& path) {
328  if (!initialized_) {
329  ROS_ERROR(
330  "This planner has not been initialized yet, but it is being used, please call initialize() before use");
331  return;
332  }
333 
334  //create a message for the plan
335  nav_msgs::Path gui_path;
336  gui_path.poses.resize(path.size());
337 
338  gui_path.header.frame_id = frame_id_;
339  gui_path.header.stamp = ros::Time::now();
340 
341  // Extract the plan in world co-ordinates, we assume the path is all in the same frame
342  for (unsigned int i = 0; i < path.size(); i++) {
343  gui_path.poses[i] = path[i];
344  }
345 
346  plan_pub_.publish(gui_path);
347 }
348 
349 bool GlobalPlanner::getPlanFromPotential(double start_x, double start_y, double goal_x, double goal_y,
350  const geometry_msgs::PoseStamped& goal,
351  std::vector<geometry_msgs::PoseStamped>& plan) {
352  if (!initialized_) {
353  ROS_ERROR(
354  "This planner has not been initialized yet, but it is being used, please call initialize() before use");
355  return false;
356  }
357 
358  std::string global_frame = frame_id_;
359 
360  //clear the plan, just in case
361  plan.clear();
362 
363  std::vector<std::pair<float, float> > path;
364 
365  if (!path_maker_->getPath(potential_array_, start_x, start_y, goal_x, goal_y, path)) {
366  ROS_ERROR("NO PATH!");
367  return false;
368  }
369 
370  ros::Time plan_time = ros::Time::now();
371  for (int i = path.size() -1; i>=0; i--) {
372  std::pair<float, float> point = path[i];
373  //convert the plan to world coordinates
374  double world_x, world_y;
375  mapToWorld(point.first, point.second, world_x, world_y);
376 
377  geometry_msgs::PoseStamped pose;
378  pose.header.stamp = plan_time;
379  pose.header.frame_id = global_frame;
380  pose.pose.position.x = world_x;
381  pose.pose.position.y = world_y;
382  pose.pose.position.z = 0.0;
383  pose.pose.orientation.x = 0.0;
384  pose.pose.orientation.y = 0.0;
385  pose.pose.orientation.z = 0.0;
386  pose.pose.orientation.w = 1.0;
387  plan.push_back(pose);
388  }
390  plan.push_back(goal);
391  }
392  return !plan.empty();
393 }
394 
395 void GlobalPlanner::publishPotential(float* potential)
396 {
397  int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();
398  double resolution = costmap_->getResolution();
399  nav_msgs::OccupancyGrid grid;
400  // Publish Whole Grid
401  grid.header.frame_id = frame_id_;
402  grid.header.stamp = ros::Time::now();
403  grid.info.resolution = resolution;
404 
405  grid.info.width = nx;
406  grid.info.height = ny;
407 
408  double wx, wy;
409  costmap_->mapToWorld(0, 0, wx, wy);
410  grid.info.origin.position.x = wx - resolution / 2;
411  grid.info.origin.position.y = wy - resolution / 2;
412  grid.info.origin.position.z = 0.0;
413  grid.info.origin.orientation.w = 1.0;
414 
415  grid.data.resize(nx * ny);
416 
417  float max = 0.0;
418  for (unsigned int i = 0; i < grid.data.size(); i++) {
419  float potential = potential_array_[i];
420  if (potential < POT_HIGH) {
421  if (potential > max) {
422  max = potential;
423  }
424  }
425  }
426 
427  for (unsigned int i = 0; i < grid.data.size(); i++) {
428  if (potential_array_[i] >= POT_HIGH) {
429  grid.data[i] = -1;
430  } else
431  grid.data[i] = potential_array_[i] * publish_scale_ / max;
432  }
433  potential_pub_.publish(grid);
434 }
435 
436 } //end namespace global_planner
void setCost(unsigned int mx, unsigned int my, unsigned char cost)
void clearRobotCell(const tf::Stamped< tf::Pose > &global_pose, unsigned int mx, unsigned int my)
void publishPotential(float *potential)
~GlobalPlanner()
Default deconstructor for the PlannerCore object.
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.
void mapToWorld(double mx, double my, double &wx, double &wy)
#define ROS_WARN_THROTTLE(rate,...)
void publish(const boost::shared_ptr< M > &message) const
std::string getPrefixParam(ros::NodeHandle &nh)
void mapToWorld(unsigned int mx, unsigned int my, double &wx, double &wy) const
virtual void processPath(const geometry_msgs::PoseStamped &start, std::vector< geometry_msgs::PoseStamped > &path)
void setLethalCost(unsigned char lethal_cost)
Definition: expander.h:64
void outlineMap(unsigned char *costarr, int nx, int ny, unsigned char value)
virtual void setSize(int nx, int ny)
Sets or resets the size of the map.
Definition: expander.h:59
void initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros)
Initialization function for the PlannerCore object.
std::string getGlobalFrameID()
PLUGINLIB_EXPORT_CLASS(BAGReader, nodelet::Nodelet)
double getOriginX() const
costmap_2d::Costmap2D * costmap_
Store a copy of the current costmap in costmap. Called by makePlan.
Definition: planner_core.h:172
virtual bool getPath(float *potential, double start_x, double start_y, double end_x, double end_y, std::vector< std::pair< float, float > > &path)=0
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
void reconfigureCB(global_planner::GlobalPlannerConfig &config, uint32_t level)
static const unsigned char FREE_SPACE
#define ROS_WARN(...)
PotentialCalculator * p_calc_
Definition: planner_core.h:188
unsigned char * getCharMap() const
#define POT_HIGH
Definition: planner_core.h:40
void clearEndpoint(unsigned char *costs, float *potential, int gx, int gy, int s)
Definition: expander.h:77
std::string resolve(const std::string &prefix, const std::string &frame_name)
void publishPlan(const std::vector< geometry_msgs::PoseStamped > &path)
Publish a path for visualization purposes.
bool getPlanFromPotential(double start_x, double start_y, double end_x, double end_y, 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...
void setPreciseStart(bool precise)
Definition: dijkstra.h:75
void setLethalCost(unsigned char lethal_cost)
Definition: traceback.h:57
bool param(const std::string &param_name, T &param_val, const T &default_val) const
bool worldToMap(double wx, double wy, double &mx, double &my)
double getOriginY() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
unsigned int getSizeInCellsY() const
GlobalPlanner()
Default constructor for the PlannerCore object.
virtual void setSize(int xs, int ys)
Definition: traceback.h:50
OrientationFilter * orientation_filter_
Definition: planner_core.h:191
void setHasUnknown(bool unknown)
Definition: expander.h:73
unsigned int getSizeInCellsX() const
bool makePlanService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp)
ros::ServiceServer make_plan_srv_
Definition: planner_core.h:186
static const unsigned char LETHAL_OBSTACLE
static Time now()
virtual bool calculatePotentials(unsigned char *costs, double start_x, double start_y, double end_x, double end_y, int cycles, float *potential)=0
void setMode(OrientationMode new_mode)
static void poseStampedMsgToTF(const geometry_msgs::PoseStamped &msg, Stamped< Pose > &bt)
void setNeutralCost(unsigned char neutral_cost)
Definition: expander.h:67
double getResolution() const
Costmap2D * getCostmap()
#define ROS_ERROR(...)
void setWindowSize(size_t window_size)
void setFactor(float factor)
Definition: expander.h:70
bool worldToMap(double wx, double wy, unsigned int &mx, unsigned int &my) const
dynamic_reconfigure::Server< global_planner::GlobalPlannerConfig > * dsrv_
Definition: planner_core.h:205
virtual void setSize(int nx, int ny)
Sets or resets the size of the map.


global_planner
Author(s): David Lu!!
autogenerated on Thu Mar 12 2020 04:10:23