dlux_global_planner.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2018, Locus Robotics
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
37 #include <nav_core2/exceptions.h>
38 #include <nav_2d_utils/tf_help.h>
39 #include <nav_2d_utils/path_ops.h>
41 #include <memory>
42 #include <string>
43 
44 namespace dlux_global_planner
45 {
47  calc_loader_("dlux_global_planner", "dlux_global_planner::PotentialCalculator"),
48  traceback_loader_("dlux_global_planner", "dlux_global_planner::Traceback"),
49  potential_grid_(HIGH_POTENTIAL), cached_goal_x_(-1), cached_goal_y_(-1), potential_pub_(potential_grid_)
50 {
51 }
52 
53 void DluxGlobalPlanner::initialize(const ros::NodeHandle& parent, const std::string& name,
55 {
56  tf_ = tf;
57  ros::NodeHandle planner_nh(parent, name);
58  costmap_ = costmap;
59  potential_grid_.setInfo(costmap_->getInfo());
60 
61  cost_interpreter_ = std::make_shared<CostInterpreter>();
62  cost_interpreter_->initialize(planner_nh, costmap_);
63 
64  std::string plugin_name;
65  planner_nh.param("potential_calculator", plugin_name, std::string("dlux_plugins::AStar"));
66  ROS_INFO_NAMED("DluxGlobalPlanner", "Using PotentialCalculator \"%s\"", plugin_name.c_str());
67  calculator_ = calc_loader_.createInstance(plugin_name);
68  calculator_->initialize(planner_nh, costmap, cost_interpreter_);
69 
70  planner_nh.param("traceback", plugin_name, std::string("dlux_plugins::GradientPath"));
71  ROS_INFO_NAMED("DluxGlobalPlanner", "Using Traceback \"%s\"", plugin_name.c_str());
72  traceback_ = traceback_loader_.createInstance(plugin_name);
73  traceback_->initialize(planner_nh, cost_interpreter_);
74 
75  planner_nh.param("path_caching", path_caching_, false);
76  planner_nh.param("improvement_threshold", improvement_threshold_, -1.0);
77  cached_path_cost_ = -1.0;
78 
79  bool publish_potential;
80  planner_nh.param("publish_potential", publish_potential, false);
81  if (publish_potential)
82  potential_pub_.init(planner_nh, "potential_grid", "potential");
83 
84  planner_nh.param("print_statistics", print_statistics_, false);
85 }
86 
87 bool DluxGlobalPlanner::isPlanValid(const nav_2d_msgs::Path2D& path) const
88 {
89  // NB: Only checks for obstacles at each pose
90  unsigned int x, y;
91  nav_core2::Costmap& costmap = *costmap_;
92  const nav_grid::NavGridInfo& info = costmap.getInfo();
93  for (geometry_msgs::Pose2D pose : path.poses)
94  {
95  if (!worldToGridBounded(info, pose.x, pose.y, x, y) || costmap(x, y) >= costmap.INSCRIBED_INFLATED_OBSTACLE)
96  {
97  return false;
98  }
99  }
100  return true;
101 }
102 
103 nav_2d_msgs::Path2D DluxGlobalPlanner::makePlan(const nav_2d_msgs::Pose2DStamped& start,
104  const nav_2d_msgs::Pose2DStamped& goal)
105 {
106  if (potential_grid_.getInfo() != costmap_->getInfo())
107  potential_grid_.setInfo(costmap_->getInfo());
108 
109  geometry_msgs::Pose2D local_start = nav_2d_utils::transformStampedPose(tf_, start, potential_grid_.getFrameId());
110  geometry_msgs::Pose2D local_goal = nav_2d_utils::transformStampedPose(tf_, goal, potential_grid_.getFrameId());
111 
112  nav_core2::Costmap& costmap = *costmap_;
113  const nav_grid::NavGridInfo& info = costmap.getInfo();
114 
115  // Check Start / Goal Quality
116  unsigned int x, y;
117  if (!worldToGridBounded(info, local_start.x, local_start.y, x, y))
118  {
119  cached_path_cost_ = -1.0;
120  throw nav_core2::StartBoundsException(start);
121  }
122  if (costmap(x, y) >= costmap.INSCRIBED_INFLATED_OBSTACLE)
123  {
124  cached_path_cost_ = -1.0;
126  }
127  if (!worldToGridBounded(info, local_goal.x, local_goal.y, x, y))
128  {
129  cached_path_cost_ = -1.0;
130  throw nav_core2::GoalBoundsException(goal);
131  }
132  if (costmap(x, y) >= costmap.INSCRIBED_INFLATED_OBSTACLE)
133  {
134  cached_path_cost_ = -1.0;
136  }
137 
138  bool cached_plan_available = false;
139  if (path_caching_ && hasValidCachedPath(local_goal, x, y))
140  {
142  {
143  return cached_path_;
144  }
145  cached_plan_available = true;
146  }
147 
148  // Commence path planning.
149  unsigned int n_updated = calculator_->updatePotentials(potential_grid_, local_start, local_goal);
151  double path_cost = 0.0; // right now we don't do anything with the cost
152  nav_2d_msgs::Path2D path = traceback_->getPath(potential_grid_, start.pose, goal.pose, path_cost);
153  if (print_statistics_)
154  {
155  ROS_INFO_NAMED("DluxGlobalPlanner",
156  "Got plan! Cost: %.2f, %d updated potentials, path of length %.2f with %zu poses.",
157  path_cost, n_updated, nav_2d_utils::getPlanLength(path), path.poses.size());
158  }
159 
160  // If there is a cached path available and the new path cost has not sufficiently improved
161  if (cached_plan_available && !shouldReturnNewPath(path, path_cost))
162  {
163  return cached_path_;
164  }
165  cached_path_cost_ = path_cost;
166  cached_path_ = path;
167  return path;
168 }
169 
170 
171 bool DluxGlobalPlanner::hasValidCachedPath(const geometry_msgs::Pose2D& local_goal,
172  unsigned int goal_x, unsigned int goal_y)
173 {
174  bool ret = cached_path_cost_ >= 0 && cached_goal_x_ == goal_x && cached_goal_y_ == goal_y &&
176  cached_goal_x_ = goal_x;
177  cached_goal_y_ = goal_y;
178  return ret;
179 }
180 
182 {
183  // If we don't care if the plan improves, return immediately.
184  return improvement_threshold_ < 0.0;
185 }
186 
187 bool DluxGlobalPlanner::shouldReturnNewPath(const nav_2d_msgs::Path2D& new_path, const double new_path_cost) const
188 {
189  return new_path_cost + improvement_threshold_ <= cached_path_cost_;
190 }
191 
193 
194 } // namespace dlux_global_planner
boost::shared_ptr< Traceback > traceback_
#define ROS_INFO_NAMED(name,...)
virtual bool shouldReturnCachedPathImmediately() const
Whether the planner should always return a valid cached path without running the planning algorithm...
static const unsigned char INSCRIBED_INFLATED_OBSTACLE
TFSIMD_FORCE_INLINE const tfScalar & y() const
boost::shared_ptr< PotentialCalculator > calculator_
pluginlib::ClassLoader< PotentialCalculator > calc_loader_
void init(ros::NodeHandle &nh, const std::string &nav_grid_topic="grid", const std::string &occupancy_grid_topic="costmap", const std::string &update_area_topic="update_area", bool publish_updates=true, ros::Duration full_publish_cycle=ros::Duration(0), ros::Duration update_publish_cycle=ros::Duration(0))
std::string getFrameId() const
const float HIGH_POTENTIAL
Definition: potential.h:46
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void setInfo(const NavGridInfo &new_info) override
TFSIMD_FORCE_INLINE const tfScalar & x() const
virtual bool hasValidCachedPath(const geometry_msgs::Pose2D &local_goal, unsigned int goal_x, unsigned int goal_y)
Check whether there is a valid cached path where the goal hasn&#39;t changed.
Plugin-based global wavefront planner that conforms to the nav_core2 GlobalPlanner interface...
nav_grid_pub_sub::ScaleGridPublisher< float > potential_pub_
pluginlib::ClassLoader< Traceback > traceback_loader_
double getPlanLength(const nav_2d_msgs::Path2D &plan, const unsigned int start_index=0)
virtual bool shouldReturnNewPath(const nav_2d_msgs::Path2D &new_path, const double new_path_cost) const
Given a cached path is available and a new path, should the new path be the one returned?
nav_2d_msgs::Path2D makePlan(const nav_2d_msgs::Pose2DStamped &start, const nav_2d_msgs::Pose2DStamped &goal) override
NavGridInfo getInfo() const
virtual bool isPlanValid(const nav_2d_msgs::Path2D &path) const
Check the costmap for any obstacles on this path.
geometry_msgs::Pose2D transformStampedPose(const TFListenerPtr tf, const nav_2d_msgs::Pose2DStamped &pose, const std::string &frame_id)
void initialize(const ros::NodeHandle &parent, const std::string &name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override
std::shared_ptr< Costmap > Ptr
std::shared_ptr< tf::TransformListener > TFListenerPtr
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
bool worldToGridBounded(const NavGridInfo &info, double wx, double wy, unsigned int &mx, unsigned int &my)


dlux_global_planner
Author(s):
autogenerated on Sun Jan 10 2021 04:08:52