dwa_planner.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 *
3 * Software License Agreement (BSD License)
4 *
5 * Copyright (c) 2009, 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 *********************************************************************/
40 #include <cmath>
41 
42 //for computing path distance
43 #include <queue>
44 
45 #include <angles/angles.h>
46 
47 #include <ros/ros.h>
48 
50 
51 namespace dwa_local_planner {
52  void DWAPlanner::reconfigure(DWAPlannerConfig &config)
53  {
54 
55  boost::mutex::scoped_lock l(configuration_mutex_);
56 
58  config.sim_time,
59  config.sim_granularity,
60  config.angular_sim_granularity,
61  config.use_dwa,
62  sim_period_);
63 
64  double resolution = planner_util_->getCostmap()->getResolution();
65  pdist_scale_ = config.path_distance_bias;
66  // pdistscale used for both path and alignment, set forward_point_distance to zero to discard alignment
67  path_costs_.setScale(resolution * pdist_scale_ * 0.5);
68  alignment_costs_.setScale(resolution * pdist_scale_ * 0.5);
69 
70  gdist_scale_ = config.goal_distance_bias;
71  goal_costs_.setScale(resolution * gdist_scale_ * 0.5);
72  goal_front_costs_.setScale(resolution * gdist_scale_ * 0.5);
73 
74  occdist_scale_ = config.occdist_scale;
76 
77  stop_time_buffer_ = config.stop_time_buffer;
78  oscillation_costs_.setOscillationResetDist(config.oscillation_reset_dist, config.oscillation_reset_angle);
79  forward_point_distance_ = config.forward_point_distance;
82 
83  // obstacle costs can vary due to scaling footprint feature
84  obstacle_costs_.setParams(config.max_trans_vel, config.max_scaling_factor, config.scaling_speed);
85 
86  twirling_costs_.setScale(config.twirling_scale);
87 
88  int vx_samp, vy_samp, vth_samp;
89  vx_samp = config.vx_samples;
90  vy_samp = config.vy_samples;
91  vth_samp = config.vth_samples;
92 
93  if (vx_samp <= 0) {
94  ROS_WARN("You've specified that you don't want any samples in the x dimension. We'll at least assume that you want to sample one value... so we're going to set vx_samples to 1 instead");
95  vx_samp = 1;
96  config.vx_samples = vx_samp;
97  }
98 
99  if (vy_samp <= 0) {
100  ROS_WARN("You've specified that you don't want any samples in the y dimension. We'll at least assume that you want to sample one value... so we're going to set vy_samples to 1 instead");
101  vy_samp = 1;
102  config.vy_samples = vy_samp;
103  }
104 
105  if (vth_samp <= 0) {
106  ROS_WARN("You've specified that you don't want any samples in the th dimension. We'll at least assume that you want to sample one value... so we're going to set vth_samples to 1 instead");
107  vth_samp = 1;
108  config.vth_samples = vth_samp;
109  }
110 
111  vsamples_[0] = vx_samp;
112  vsamples_[1] = vy_samp;
113  vsamples_[2] = vth_samp;
114 
115 
116  }
117 
119  planner_util_(planner_util),
120  obstacle_costs_(planner_util->getCostmap()),
121  path_costs_(planner_util->getCostmap()),
122  goal_costs_(planner_util->getCostmap(), 0.0, 0.0, true),
123  goal_front_costs_(planner_util->getCostmap(), 0.0, 0.0, true),
124  alignment_costs_(planner_util->getCostmap())
125  {
126  ros::NodeHandle private_nh("~/" + name);
127 
130 
131  //Assuming this planner is being run within the navigation stack, we can
132  //just do an upward search for the frequency at which its being run. This
133  //also allows the frequency to be overwritten locally.
134  std::string controller_frequency_param_name;
135  if(!private_nh.searchParam("controller_frequency", controller_frequency_param_name)) {
136  sim_period_ = 0.05;
137  } else {
138  double controller_frequency = 0;
139  private_nh.param(controller_frequency_param_name, controller_frequency, 20.0);
140  if(controller_frequency > 0) {
141  sim_period_ = 1.0 / controller_frequency;
142  } else {
143  ROS_WARN("A controller_frequency less than 0 has been set. Ignoring the parameter, assuming a rate of 20Hz");
144  sim_period_ = 0.05;
145  }
146  }
147  ROS_INFO("Sim period is set to %.2f", sim_period_);
148 
150 
151  bool sum_scores;
152  private_nh.param("sum_scores", sum_scores, false);
153  obstacle_costs_.setSumScores(sum_scores);
154 
155 
156  private_nh.param("publish_cost_grid_pc", publish_cost_grid_pc_, false);
157  map_viz_.initialize(name, planner_util->getGlobalFrame(), boost::bind(&DWAPlanner::getCellCosts, this, _1, _2, _3, _4, _5, _6));
158 
159  std::string frame_id;
160  private_nh.param("global_frame_id", frame_id, std::string("odom"));
161 
162  traj_cloud_ = new pcl::PointCloud<base_local_planner::MapGridCostPoint>;
163  traj_cloud_->header.frame_id = frame_id;
164  traj_cloud_pub_.advertise(private_nh, "trajectory_cloud", 1);
165  private_nh.param("publish_traj_pc", publish_traj_pc_, false);
166 
167  // set up all the cost functions that will be applied in order
168  // (any function returning negative values will abort scoring, so the order can improve performance)
169  std::vector<base_local_planner::TrajectoryCostFunction*> critics;
170  critics.push_back(&oscillation_costs_); // discards oscillating motions (assisgns cost -1)
171  critics.push_back(&obstacle_costs_); // discards trajectories that move into obstacles
172  critics.push_back(&goal_front_costs_); // prefers trajectories that make the nose go towards (local) nose goal
173  critics.push_back(&alignment_costs_); // prefers trajectories that keep the robot nose on nose path
174  critics.push_back(&path_costs_); // prefers trajectories on global path
175  critics.push_back(&goal_costs_); // prefers trajectories that go towards (local) goal, based on wave propagation
176  critics.push_back(&twirling_costs_); // optionally prefer trajectories that don't spin
177 
178  // trajectory generators
179  std::vector<base_local_planner::TrajectorySampleGenerator*> generator_list;
180  generator_list.push_back(&generator_);
181 
183 
184  private_nh.param("cheat_factor", cheat_factor_, 1.0);
185  }
186 
187  // used for visualization only, total_costs are not really total costs
188  bool DWAPlanner::getCellCosts(int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost) {
189 
190  path_cost = path_costs_.getCellCosts(cx, cy);
191  goal_cost = goal_costs_.getCellCosts(cx, cy);
192  occ_cost = planner_util_->getCostmap()->getCost(cx, cy);
193  if (path_cost == path_costs_.obstacleCosts() ||
194  path_cost == path_costs_.unreachableCellCosts() ||
196  return false;
197  }
198 
199  double resolution = planner_util_->getCostmap()->getResolution();
200  total_cost =
201  pdist_scale_ * resolution * path_cost +
202  gdist_scale_ * resolution * goal_cost +
203  occdist_scale_ * occ_cost;
204  return true;
205  }
206 
207  bool DWAPlanner::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) {
209  return planner_util_->setPlan(orig_global_plan);
210  }
211 
217  Eigen::Vector3f pos,
218  Eigen::Vector3f vel,
219  Eigen::Vector3f vel_samples){
222  geometry_msgs::PoseStamped goal_pose = global_plan_.back();
223  Eigen::Vector3f goal(goal_pose.pose.position.x, goal_pose.pose.position.y, tf::getYaw(goal_pose.pose.orientation));
226  vel,
227  goal,
228  &limits,
229  vsamples_);
230  generator_.generateTrajectory(pos, vel, vel_samples, traj);
231  double cost = scored_sampling_planner_.scoreTrajectory(traj, -1);
232  //if the trajectory is a legal one... the check passes
233  if(cost >= 0) {
234  return true;
235  }
236  ROS_WARN("Invalid Trajectory %f, %f, %f, cost: %f", vel_samples[0], vel_samples[1], vel_samples[2], cost);
237 
238  //otherwise the check fails
239  return false;
240  }
241 
242 
244  tf::Stamped<tf::Pose> global_pose,
245  const std::vector<geometry_msgs::PoseStamped>& new_plan,
246  const std::vector<geometry_msgs::Point>& footprint_spec) {
247  global_plan_.resize(new_plan.size());
248  for (unsigned int i = 0; i < new_plan.size(); ++i) {
249  global_plan_[i] = new_plan[i];
250  }
251 
252  obstacle_costs_.setFootprint(footprint_spec);
253 
254  // costs for going away from path
256 
257  // costs for not going towards the local goal as much as possible
259 
260  // alignment costs
261  geometry_msgs::PoseStamped goal_pose = global_plan_.back();
262 
263  Eigen::Vector3f pos(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), tf::getYaw(global_pose.getRotation()));
264  double sq_dist =
265  (pos[0] - goal_pose.pose.position.x) * (pos[0] - goal_pose.pose.position.x) +
266  (pos[1] - goal_pose.pose.position.y) * (pos[1] - goal_pose.pose.position.y);
267 
268  // we want the robot nose to be drawn to its final position
269  // (before robot turns towards goal orientation), not the end of the
270  // path for the robot center. Choosing the final position after
271  // turning towards goal orientation causes instability when the
272  // robot needs to make a 180 degree turn at the end
273  std::vector<geometry_msgs::PoseStamped> front_global_plan = global_plan_;
274  double angle_to_goal = atan2(goal_pose.pose.position.y - pos[1], goal_pose.pose.position.x - pos[0]);
275  front_global_plan.back().pose.position.x = front_global_plan.back().pose.position.x +
276  forward_point_distance_ * cos(angle_to_goal);
277  front_global_plan.back().pose.position.y = front_global_plan.back().pose.position.y + forward_point_distance_ *
278  sin(angle_to_goal);
279 
280  goal_front_costs_.setTargetPoses(front_global_plan);
281 
282  // keeping the nose on the path
284  double resolution = planner_util_->getCostmap()->getResolution();
285  alignment_costs_.setScale(resolution * pdist_scale_ * 0.5);
286  // costs for robot being aligned with path (nose on path, not ju
288  } else {
289  // once we are close to goal, trying to keep the nose close to anything destabilizes behavior.
291  }
292  }
293 
294 
295  /*
296  * given the current state of the robot, find a good trajectory
297  */
299  tf::Stamped<tf::Pose> global_pose,
300  tf::Stamped<tf::Pose> global_vel,
301  tf::Stamped<tf::Pose>& drive_velocities) {
302 
303  //make sure that our configuration doesn't change mid-run
304  boost::mutex::scoped_lock l(configuration_mutex_);
305 
306  Eigen::Vector3f pos(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), tf::getYaw(global_pose.getRotation()));
307  Eigen::Vector3f vel(global_vel.getOrigin().getX(), global_vel.getOrigin().getY(), tf::getYaw(global_vel.getRotation()));
308  geometry_msgs::PoseStamped goal_pose = global_plan_.back();
309  Eigen::Vector3f goal(goal_pose.pose.position.x, goal_pose.pose.position.y, tf::getYaw(goal_pose.pose.orientation));
311 
312  // prepare cost functions and generators for this run
314  vel,
315  goal,
316  &limits,
317  vsamples_);
318 
319  result_traj_.cost_ = -7;
320  // find best trajectory by sampling and scoring the samples
321  std::vector<base_local_planner::Trajectory> all_explored;
323 
324  if(publish_traj_pc_)
325  {
327  traj_cloud_->points.clear();
328  traj_cloud_->width = 0;
329  traj_cloud_->height = 0;
330  std_msgs::Header header;
331  pcl_conversions::fromPCL(traj_cloud_->header, header);
332  header.stamp = ros::Time::now();
333  traj_cloud_->header = pcl_conversions::toPCL(header);
334  for(std::vector<base_local_planner::Trajectory>::iterator t=all_explored.begin(); t != all_explored.end(); ++t)
335  {
336  if(t->cost_<0)
337  continue;
338  // Fill out the plan
339  for(unsigned int i = 0; i < t->getPointsSize(); ++i) {
340  double p_x, p_y, p_th;
341  t->getPoint(i, p_x, p_y, p_th);
342  pt.x=p_x;
343  pt.y=p_y;
344  pt.z=0;
345  pt.path_cost=p_th;
346  pt.total_cost=t->cost_;
347  traj_cloud_->push_back(pt);
348  }
349  }
351  }
352 
353  // verbose publishing of point clouds
354  if (publish_cost_grid_pc_) {
355  //we'll publish the visualization of the costs to rviz before returning our best trajectory
357  }
358 
359  // debrief stateful scoring functions
361 
362  //if we don't have a legal trajectory, we'll just command zero
363  if (result_traj_.cost_ < 0) {
364  drive_velocities.setIdentity();
365  } else {
367  drive_velocities.setOrigin(start);
368  tf::Matrix3x3 matrix;
370  drive_velocities.setBasis(matrix);
371  }
372 
373  return result_traj_;
374  }
375 };
void initialise(const Eigen::Vector3f &pos, const Eigen::Vector3f &vel, const Eigen::Vector3f &goal, base_local_planner::LocalPlannerLimits *limits, const Eigen::Vector3f &vsamples, std::vector< Eigen::Vector3f > additional_samples, bool discretize_by_time=false)
void setFootprint(std::vector< geometry_msgs::Point > footprint_spec)
void reconfigure(DWAPlannerConfig &cfg)
Reconfigures the trajectory planner.
Definition: dwa_planner.cpp:52
pcl_ros::Publisher< base_local_planner::MapGridCostPoint > traj_cloud_pub_
Definition: dwa_planner.h:170
bool setPlan(const std::vector< geometry_msgs::PoseStamped > &orig_global_plan)
bool generateTrajectory(Eigen::Vector3f pos, Eigen::Vector3f vel, Eigen::Vector3f sample_target_vel, base_local_planner::Trajectory &traj)
base_local_planner::MapGridCostFunction goal_front_costs_
Definition: dwa_planner.h:184
void publishCostCloud(const costmap_2d::Costmap2D *costmap_p_)
base_local_planner::MapGridCostFunction goal_costs_
Definition: dwa_planner.h:183
double sim_period_
The number of seconds to use to compute max/min vels for dwa.
Definition: dwa_planner.h:161
static double getYaw(const Quaternion &bt_q)
costmap_2d::Costmap2D * getCostmap()
base_local_planner::OscillationCostFunction oscillation_costs_
Definition: dwa_planner.h:180
base_local_planner::LocalPlannerUtil * planner_util_
Definition: dwa_planner.h:155
bool publish_cost_grid_pc_
Whether or not to build and publish a PointCloud.
Definition: dwa_planner.h:171
bool getCellCosts(int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost)
Compute the components and total cost for a map grid cell.
void initialize(const std::string &name, std::string frame, boost::function< bool(int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost)> cost_function)
base_local_planner::TwirlingCostFunction twirling_costs_
Definition: dwa_planner.h:186
#define ROS_WARN(...)
std::vector< geometry_msgs::PoseStamped > global_plan_
Definition: dwa_planner.h:166
pcl::PointCloud< base_local_planner::MapGridCostPoint > * traj_cloud_
Definition: dwa_planner.h:169
double scoreTrajectory(Trajectory &traj, double best_traj_cost)
void setStopOnFailure(bool stop_on_failure)
void setRotation(const Quaternion &q)
base_local_planner::MapGridCostFunction alignment_costs_
Definition: dwa_planner.h:185
DWAPlanner(std::string name, base_local_planner::LocalPlannerUtil *planner_util)
Constructor for the planner.
base_local_planner::SimpleScoredSamplingPlanner scored_sampling_planner_
Definition: dwa_planner.h:188
bool findBestTrajectory(Trajectory &traj, std::vector< Trajectory > *all_explored=0)
void fromPCL(const pcl::uint64_t &pcl_stamp, ros::Time &stamp)
unsigned char getCost(unsigned int mx, unsigned int my) const
static const unsigned char INSCRIBED_INFLATED_OBSTACLE
static Quaternion createQuaternionFromYaw(double yaw)
base_local_planner::ObstacleCostFunction obstacle_costs_
Definition: dwa_planner.h:181
base_local_planner::MapGridVisualizer map_viz_
The map grid visualizer for outputting the potential field generated by the cost function.
Definition: dwa_planner.h:176
#define ROS_INFO(...)
bool searchParam(const std::string &key, std::string &result) const
bool param(const std::string &param_name, T &param_val, const T &default_val) const
LocalPlannerLimits getCurrentLimits()
void publish(const boost::shared_ptr< const pcl::PointCloud< PointT > > &point_cloud) const
base_local_planner::MapGridCostFunction path_costs_
Definition: dwa_planner.h:182
void updatePlanAndLocalCosts(tf::Stamped< tf::Pose > global_pose, const std::vector< geometry_msgs::PoseStamped > &new_plan, const std::vector< geometry_msgs::Point > &footprint_spec)
Update the cost functions before planning.
void setParameters(double sim_time, double sim_granularity, double angular_sim_granularity, bool use_dwa=false, double sim_period=0.0)
bool setPlan(const std::vector< geometry_msgs::PoseStamped > &orig_global_plan)
base_local_planner::Trajectory findBestPath(tf::Stamped< tf::Pose > global_pose, tf::Stamped< tf::Pose > global_vel, tf::Stamped< tf::Pose > &drive_velocities)
Given the current position and velocity of the robot, find the best trajectory to exectue...
void setParams(double max_trans_vel, double max_scaling_factor, double scaling_speed)
void setTargetPoses(std::vector< geometry_msgs::PoseStamped > target_poses)
base_local_planner::Trajectory result_traj_
Definition: dwa_planner.h:162
base_local_planner::SimpleTrajectoryGenerator generator_
Definition: dwa_planner.h:179
static Time now()
double stop_time_buffer_
How long before hitting something we&#39;re going to enforce that the robot stop.
Definition: dwa_planner.h:157
const std::string header
double getCellCosts(unsigned int cx, unsigned int cy)
double getResolution() const
void advertise(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size)
void toPCL(const ros::Time &stamp, pcl::uint64_t &pcl_stamp)
void updateOscillationFlags(Eigen::Vector3f pos, base_local_planner::Trajectory *traj, double min_vel_trans)
void setOscillationResetDist(double dist, double angle)
bool checkTrajectory(const Eigen::Vector3f pos, const Eigen::Vector3f vel, const Eigen::Vector3f vel_samples)
Check if a trajectory is legal for a position/velocity pair.


dwa_local_planner
Author(s): Eitan Marder-Eppstein, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:44:35