sbpl_lattice_planner.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: Mike Phillips
36 *********************************************************************/
37 
40 #include <nav_msgs/Path.h>
41 #include <sbpl_lattice_planner/SBPLLatticePlannerStats.h>
42 
45 
46 using namespace std;
47 using namespace ros;
48 
49 
51 
52 namespace geometry_msgs {
53  bool operator== (const Point &p1, const Point &p2)
54  {
55  return p1.x == p2.x && p1.y == p2.y && p1.z == p2.z;
56  }
57 }
58 
59 namespace sbpl_lattice_planner{
60 
61 class LatticeSCQ : public StateChangeQuery{
62  public:
63  LatticeSCQ(EnvironmentNAVXYTHETALAT* env, std::vector<nav2dcell_t> const & changedcellsV)
64  : env_(env), changedcellsV_(changedcellsV) {
65  }
66 
67  // lazy init, because we do not always end up calling this method
68  virtual std::vector<int> const * getPredecessors() const{
69  if(predsOfChangedCells_.empty() && !changedcellsV_.empty())
70  env_->GetPredsofChangedEdges(&changedcellsV_, &predsOfChangedCells_);
71  return &predsOfChangedCells_;
72  }
73 
74  // lazy init, because we do not always end up calling this method
75  virtual std::vector<int> const * getSuccessors() const{
76  if(succsOfChangedCells_.empty() && !changedcellsV_.empty())
77  env_->GetSuccsofChangedEdges(&changedcellsV_, &succsOfChangedCells_);
78  return &succsOfChangedCells_;
79  }
80 
81  EnvironmentNAVXYTHETALAT * env_;
82  std::vector<nav2dcell_t> const & changedcellsV_;
83  mutable std::vector<int> predsOfChangedCells_;
84  mutable std::vector<int> succsOfChangedCells_;
85 };
86 
87 SBPLLatticePlanner::SBPLLatticePlanner()
88  : initialized_(false), costmap_ros_(NULL){
89 }
90 
92  : initialized_(false), costmap_ros_(NULL){
93  initialize(name, costmap_ros);
94 }
95 
96 
97 void SBPLLatticePlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
98  if(!initialized_){
99  ros::NodeHandle private_nh("~/"+name);
100 
101  ROS_INFO("Name is %s", name.c_str());
102 
103  private_nh.param("planner_type", planner_type_, string("ARAPlanner"));
104  private_nh.param("allocated_time", allocated_time_, 10.0);
105  private_nh.param("initial_epsilon",initial_epsilon_,3.0);
106  private_nh.param("environment_type", environment_type_, string("XYThetaLattice"));
107  private_nh.param("forward_search", forward_search_, bool(false));
108  private_nh.param("primitive_filename",primitive_filename_,string(""));
109  private_nh.param("force_scratch_limit",force_scratch_limit_,500);
110 
111  double nominalvel_mpersecs, timetoturn45degsinplace_secs;
112  private_nh.param("nominalvel_mpersecs", nominalvel_mpersecs, 0.4);
113  private_nh.param("timetoturn45degsinplace_secs", timetoturn45degsinplace_secs, 0.6);
114 
115  int lethal_obstacle;
116  private_nh.param("lethal_obstacle",lethal_obstacle,20);
117  lethal_obstacle_ = (unsigned char) lethal_obstacle;
120  ROS_DEBUG("SBPL: lethal: %uz, inscribed inflated: %uz, multiplier: %uz",lethal_obstacle,inscribed_inflated_obstacle_,sbpl_cost_multiplier_);
121 
122  name_ = name;
123  costmap_ros_ = costmap_ros;
124 
126 
127  if ("XYThetaLattice" == environment_type_){
128  ROS_DEBUG("Using a 3D costmap for theta lattice\n");
129  env_ = new EnvironmentNAVXYTHETALAT();
130  }
131  else{
132  ROS_ERROR("XYThetaLattice is currently the only supported environment!\n");
133  exit(1);
134  }
135 
137 
138  if (circumscribed_cost_ == 0) {
139  // Unfortunately, the inflation_radius is not taken into account by
140  // inflation_layer->computeCost(). If inflation_radius is smaller than
141  // the circumscribed radius, SBPL will ignore some obstacles, but we
142  // cannot detect this problem. If the cost_scaling_factor is too large,
143  // SBPL won't run into obstacles, but will always perform an expensive
144  // footprint check, no matter how far the nearest obstacle is.
145  ROS_WARN("The costmap value at the robot's circumscribed radius (%f m) is 0.", costmap_ros_->getLayeredCostmap()->getCircumscribedRadius());
146  ROS_WARN("SBPL performance will suffer.");
147  ROS_WARN("Please decrease the costmap's cost_scaling_factor.");
148  }
149  if(!env_->SetEnvParameter("cost_inscribed_thresh",costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE))){
150  ROS_ERROR("Failed to set cost_inscribed_thresh parameter");
151  exit(1);
152  }
153  if(!env_->SetEnvParameter("cost_possibly_circumscribed_thresh", circumscribed_cost_)){
154  ROS_ERROR("Failed to set cost_possibly_circumscribed_thresh parameter");
155  exit(1);
156  }
157  int obst_cost_thresh = costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE);
158  vector<sbpl_2Dpt_t> perimeterptsV;
159  perimeterptsV.reserve(footprint_.size());
160  for (size_t ii(0); ii < footprint_.size(); ++ii) {
161  sbpl_2Dpt_t pt;
162  pt.x = footprint_[ii].x;
163  pt.y = footprint_[ii].y;
164  perimeterptsV.push_back(pt);
165  }
166 
167  bool ret;
168  try{
169  ret = env_->InitializeEnv(costmap_ros_->getCostmap()->getSizeInCellsX(), // width
170  costmap_ros_->getCostmap()->getSizeInCellsY(), // height
171  0, // mapdata
172  0, 0, 0, // start (x, y, theta, t)
173  0, 0, 0, // goal (x, y, theta)
174  0, 0, 0, //goal tolerance
175  perimeterptsV, costmap_ros_->getCostmap()->getResolution(), nominalvel_mpersecs,
176  timetoturn45degsinplace_secs, obst_cost_thresh,
177  primitive_filename_.c_str());
180  }
181  catch(SBPL_Exception *e){
182  ROS_ERROR("SBPL encountered a fatal exception: %s", e->what());
183  ret = false;
184  }
185  if(!ret){
186  ROS_ERROR("SBPL initialization failed!");
187  exit(1);
188  }
189  for (ssize_t ix(0); ix < costmap_ros_->getCostmap()->getSizeInCellsX(); ++ix)
190  for (ssize_t iy(0); iy < costmap_ros_->getCostmap()->getSizeInCellsY(); ++iy)
191  env_->UpdateCost(ix, iy, costMapCostToSBPLCost(costmap_ros_->getCostmap()->getCost(ix,iy)));
192 
193  if ("ARAPlanner" == planner_type_){
194  ROS_INFO("Planning with ARA*");
195  planner_ = new ARAPlanner(env_, forward_search_);
196  }
197  else if ("ADPlanner" == planner_type_){
198  ROS_INFO("Planning with AD*");
199  planner_ = new ADPlanner(env_, forward_search_);
200  }
201  else{
202  ROS_ERROR("ARAPlanner and ADPlanner are currently the only supported planners!\n");
203  exit(1);
204  }
205 
206  ROS_INFO("[sbpl_lattice_planner] Initialized successfully");
207  plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
208  stats_publisher_ = private_nh.advertise<sbpl_lattice_planner::SBPLLatticePlannerStats>("sbpl_lattice_planner_stats", 1);
209 
210  initialized_ = true;
211  }
212 }
213 
214 //Taken from Sachin's sbpl_cart_planner
215 //This rescales the costmap according to a rosparam which sets the obstacle cost
216 unsigned char SBPLLatticePlanner::costMapCostToSBPLCost(unsigned char newcost){
217  if(newcost == costmap_2d::LETHAL_OBSTACLE)
218  return lethal_obstacle_;
219  else if(newcost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
221  else if(newcost == 0 || newcost == costmap_2d::NO_INFORMATION)
222  return 0;
223  else {
224  unsigned char sbpl_cost = newcost / sbpl_cost_multiplier_;
225  if (sbpl_cost == 0)
226  sbpl_cost = 1;
227  return sbpl_cost;
228  }
229 }
230 
231 void SBPLLatticePlanner::publishStats(int solution_cost, int solution_size,
232  const geometry_msgs::PoseStamped& start,
233  const geometry_msgs::PoseStamped& goal){
234  // Fill up statistics and publish
235  sbpl_lattice_planner::SBPLLatticePlannerStats stats;
236  stats.initial_epsilon = initial_epsilon_;
237  stats.plan_to_first_solution = false;
238  stats.final_number_of_expands = planner_->get_n_expands();
239  stats.allocated_time = allocated_time_;
240 
241  stats.time_to_first_solution = planner_->get_initial_eps_planning_time();
242  stats.actual_time = planner_->get_final_eps_planning_time();
243  stats.number_of_expands_initial_solution = planner_->get_n_expands_init_solution();
244  stats.final_epsilon = planner_->get_final_epsilon();
245 
246  stats.solution_cost = solution_cost;
247  stats.path_size = solution_size;
248  stats.start = start;
249  stats.goal = goal;
250  stats_publisher_.publish(stats);
251 }
252 
254  unsigned char result = 0;
255 
256  if (!costmap_ros_) {
257  ROS_ERROR("Costmap is not initialized");
258  return 0;
259  }
260 
261  // check if the costmap has an inflation layer
262  for(std::vector<boost::shared_ptr<costmap_2d::Layer> >::const_iterator layer = costmap_ros_->getLayeredCostmap()->getPlugins()->begin();
263  layer != costmap_ros_->getLayeredCostmap()->getPlugins()->end();
264  ++layer) {
265  boost::shared_ptr<costmap_2d::InflationLayer> inflation_layer = boost::dynamic_pointer_cast<costmap_2d::InflationLayer>(*layer);
266  if (!inflation_layer) continue;
267 
269  }
270  return result;
271 }
272 
273 bool SBPLLatticePlanner::makePlan(const geometry_msgs::PoseStamped& start,
274  const geometry_msgs::PoseStamped& goal,
275  std::vector<geometry_msgs::PoseStamped>& plan){
276  if(!initialized_){
277  ROS_ERROR("Global planner is not initialized");
278  return false;
279  }
280 
281  bool do_init = false;
284  ROS_INFO("Costmap dimensions have changed from (%d x %d) to (%d x %d), reinitializing sbpl_lattice_planner.",
287  do_init = true;
288  }
289  else if (footprint_ != costmap_ros_->getRobotFootprint()) {
290  ROS_INFO("Robot footprint has changed, reinitializing sbpl_lattice_planner.");
291  do_init = true;
292  }
294  ROS_INFO("Cost at circumscribed radius has changed, reinitializing sbpl_lattice_planner.");
295  do_init = true;
296  }
297 
298  if (do_init) {
299  initialized_ = false;
300  delete planner_;
301  planner_ = NULL;
302  delete env_;
303  env_ = NULL;
305  }
306 
307  plan.clear();
308 
309  ROS_INFO("[sbpl_lattice_planner] getting start point (%g,%g) goal point (%g,%g)",
310  start.pose.position.x, start.pose.position.y,goal.pose.position.x, goal.pose.position.y);
311  double theta_start = 2 * atan2(start.pose.orientation.z, start.pose.orientation.w);
312  double theta_goal = 2 * atan2(goal.pose.orientation.z, goal.pose.orientation.w);
313 
314  try{
315  int ret = env_->SetStart(start.pose.position.x - costmap_ros_->getCostmap()->getOriginX(), start.pose.position.y - costmap_ros_->getCostmap()->getOriginY(), theta_start);
316  if(ret < 0 || planner_->set_start(ret) == 0){
317  ROS_ERROR("ERROR: failed to set start state\n");
318  return false;
319  }
320  }
321  catch(SBPL_Exception *e){
322  ROS_ERROR("SBPL encountered a fatal exception while setting the start state");
323  return false;
324  }
325 
326  try{
327  int ret = env_->SetGoal(goal.pose.position.x - costmap_ros_->getCostmap()->getOriginX(), goal.pose.position.y - costmap_ros_->getCostmap()->getOriginY(), theta_goal);
328  if(ret < 0 || planner_->set_goal(ret) == 0){
329  ROS_ERROR("ERROR: failed to set goal state\n");
330  return false;
331  }
332  }
333  catch(SBPL_Exception *e){
334  ROS_ERROR("SBPL encountered a fatal exception while setting the goal state");
335  return false;
336  }
337 
338  int offOnCount = 0;
339  int onOffCount = 0;
340  int allCount = 0;
341  vector<nav2dcell_t> changedcellsV;
342 
343  for(unsigned int ix = 0; ix < costmap_ros_->getCostmap()->getSizeInCellsX(); ix++) {
344  for(unsigned int iy = 0; iy < costmap_ros_->getCostmap()->getSizeInCellsY(); iy++) {
345 
346  unsigned char oldCost = env_->GetMapCost(ix,iy);
347  unsigned char newCost = costMapCostToSBPLCost(costmap_ros_->getCostmap()->getCost(ix,iy));
348 
349  if(oldCost == newCost) continue;
350 
351  allCount++;
352 
353  //first case - off cell goes on
354 
357  offOnCount++;
358  }
359 
362  onOffCount++;
363  }
364  env_->UpdateCost(ix, iy, costMapCostToSBPLCost(costmap_ros_->getCostmap()->getCost(ix,iy)));
365 
366  nav2dcell_t nav2dcell;
367  nav2dcell.x = ix;
368  nav2dcell.y = iy;
369  changedcellsV.push_back(nav2dcell);
370  }
371  }
372 
373  try{
374  if(!changedcellsV.empty()){
375  StateChangeQuery* scq = new LatticeSCQ(env_, changedcellsV);
376  planner_->costs_changed(*scq);
377  delete scq;
378  }
379 
380  if(allCount > force_scratch_limit_)
381  planner_->force_planning_from_scratch();
382  }
383  catch(SBPL_Exception *e){
384  ROS_ERROR("SBPL failed to update the costmap");
385  return false;
386  }
387 
388  //setting planner parameters
389  ROS_DEBUG("allocated:%f, init eps:%f\n",allocated_time_,initial_epsilon_);
390  planner_->set_initialsolution_eps(initial_epsilon_);
391  planner_->set_search_mode(false);
392 
393  ROS_DEBUG("[sbpl_lattice_planner] run planner");
394  vector<int> solution_stateIDs;
395  int solution_cost;
396  try{
397  int ret = planner_->replan(allocated_time_, &solution_stateIDs, &solution_cost);
398  if(ret)
399  ROS_DEBUG("Solution is found\n");
400  else{
401  ROS_INFO("Solution not found\n");
402  publishStats(solution_cost, 0, start, goal);
403  return false;
404  }
405  }
406  catch(SBPL_Exception *e){
407  ROS_ERROR("SBPL encountered a fatal exception while planning");
408  return false;
409  }
410 
411  ROS_DEBUG("size of solution=%d", (int)solution_stateIDs.size());
412 
413  vector<EnvNAVXYTHETALAT3Dpt_t> sbpl_path;
414  try{
415  env_->ConvertStateIDPathintoXYThetaPath(&solution_stateIDs, &sbpl_path);
416  }
417  catch(SBPL_Exception *e){
418  ROS_ERROR("SBPL encountered a fatal exception while reconstructing the path");
419  return false;
420  }
421  // if the plan has zero points, add a single point to make move_base happy
422  if( sbpl_path.size() == 0 ) {
423  EnvNAVXYTHETALAT3Dpt_t s(
424  start.pose.position.x - costmap_ros_->getCostmap()->getOriginX(),
425  start.pose.position.y - costmap_ros_->getCostmap()->getOriginY(),
426  theta_start);
427  sbpl_path.push_back(s);
428  }
429 
430  ROS_DEBUG("Plan has %d points.\n", (int)sbpl_path.size());
431  ros::Time plan_time = ros::Time::now();
432 
433  //create a message for the plan
434  nav_msgs::Path gui_path;
435  gui_path.poses.resize(sbpl_path.size());
436  gui_path.header.frame_id = costmap_ros_->getGlobalFrameID();
437  gui_path.header.stamp = plan_time;
438  for(unsigned int i=0; i<sbpl_path.size(); i++){
439  geometry_msgs::PoseStamped pose;
440  pose.header.stamp = plan_time;
441  pose.header.frame_id = costmap_ros_->getGlobalFrameID();
442 
443  pose.pose.position.x = sbpl_path[i].x + costmap_ros_->getCostmap()->getOriginX();
444  pose.pose.position.y = sbpl_path[i].y + costmap_ros_->getCostmap()->getOriginY();
445  pose.pose.position.z = start.pose.position.z;
446 
447  tf2::Quaternion temp;
448  temp.setRPY(0,0,sbpl_path[i].theta);
449  pose.pose.orientation.x = temp.getX();
450  pose.pose.orientation.y = temp.getY();
451  pose.pose.orientation.z = temp.getZ();
452  pose.pose.orientation.w = temp.getW();
453 
454  plan.push_back(pose);
455 
456  gui_path.poses[i] = plan[i];
457  }
458  plan_pub_.publish(gui_path);
459  publishStats(solution_cost, sbpl_path.size(), start, goal);
460 
461  return true;
462 }
463 };
LatticeSCQ(EnvironmentNAVXYTHETALAT *env, std::vector< nav2dcell_t > const &changedcellsV)
virtual void initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros)
Initialization function for the SBPLLatticePlanner object.
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
void publish(const boost::shared_ptr< M > &message) const
virtual std::vector< int > const * getPredecessors() const
TF2SIMD_FORCE_INLINE const tf2Scalar & getW() const
XmlRpcServer s
void publishStats(int solution_cost, int solution_size, const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal)
std::string getGlobalFrameID()
PLUGINLIB_EXPORT_CLASS(BAGReader, nodelet::Nodelet)
double getOriginX() const
std::vector< boost::shared_ptr< Layer > > * getPlugins()
#define ROS_WARN(...)
bool operator==(const Point &p1, const Point &p2)
std::vector< nav2dcell_t > const & changedcellsV_
TFSIMD_FORCE_INLINE const tfScalar & x() const
virtual std::vector< int > const * getSuccessors() const
unsigned char getCost(unsigned int mx, unsigned int my) const
static const unsigned char INSCRIBED_INFLATED_OBSTACLE
TFSIMD_FORCE_INLINE const tfScalar & z() const
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
TFSIMD_FORCE_INLINE const tfScalar & y() const
double getOriginY() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
virtual 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.
unsigned int getSizeInCellsY() const
unsigned char costMapCostToSBPLCost(unsigned char newcost)
unsigned int getSizeInCellsX() const
static const unsigned char LETHAL_OBSTACLE
static const unsigned char NO_INFORMATION
static Time now()
double getResolution() const
Costmap2D * getCostmap()
std::vector< geometry_msgs::Point > getRobotFootprint()
#define ROS_ERROR(...)
std::vector< geometry_msgs::Point > footprint_
SBPLLatticePlanner()
Default constructor for the NavFnROS object.
LayeredCostmap * getLayeredCostmap()
#define ROS_DEBUG(...)


sbpl_lattice_planner
Author(s): Michael Phillips
autogenerated on Mon Jun 22 2020 03:18:17