dwa_planner_ros.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 *********************************************************************/
37 
39 #include <Eigen/Core>
40 #include <cmath>
41 
42 #include <ros/console.h>
43 
45 
47 #include <nav_msgs/Path.h>
48 
49 //register this planner as a BaseLocalPlanner plugin
51 
52 namespace dwa_local_planner {
53 
54  void DWAPlannerROS::reconfigureCB(DWAPlannerConfig &config, uint32_t level) {
55  if (setup_ && config.restore_defaults) {
56  config = default_config_;
57  config.restore_defaults = false;
58  }
59  if ( ! setup_) {
60  default_config_ = config;
61  setup_ = true;
62  }
63 
64  // update generic local planner params
66  limits.max_trans_vel = config.max_trans_vel;
67  limits.min_trans_vel = config.min_trans_vel;
68  limits.max_vel_x = config.max_vel_x;
69  limits.min_vel_x = config.min_vel_x;
70  limits.max_vel_y = config.max_vel_y;
71  limits.min_vel_y = config.min_vel_y;
72  limits.max_rot_vel = config.max_rot_vel;
73  limits.min_rot_vel = config.min_rot_vel;
74  limits.acc_lim_x = config.acc_lim_x;
75  limits.acc_lim_y = config.acc_lim_y;
76  limits.acc_lim_theta = config.acc_lim_theta;
77  limits.acc_limit_trans = config.acc_limit_trans;
78  limits.xy_goal_tolerance = config.xy_goal_tolerance;
79  limits.yaw_goal_tolerance = config.yaw_goal_tolerance;
80  limits.prune_plan = config.prune_plan;
81  limits.trans_stopped_vel = config.trans_stopped_vel;
82  limits.rot_stopped_vel = config.rot_stopped_vel;
83  planner_util_.reconfigureCB(limits, config.restore_defaults);
84 
85  // update dwa specific configuration
86  dp_->reconfigure(config);
87  }
88 
89  DWAPlannerROS::DWAPlannerROS() : initialized_(false),
90  odom_helper_("odom"), setup_(false) {
91 
92  }
93 
95  std::string name,
97  costmap_2d::Costmap2DROS* costmap_ros) {
98  if (! isInitialized()) {
99 
100  ros::NodeHandle private_nh("~/" + name);
101  g_plan_pub_ = private_nh.advertise<nav_msgs::Path>("global_plan", 1);
102  l_plan_pub_ = private_nh.advertise<nav_msgs::Path>("local_plan", 1);
103  tf_ = tf;
104  costmap_ros_ = costmap_ros;
106 
107  // make sure to update the costmap we'll use for this cycle
109 
111 
112  //create the actual planner that we'll use.. it'll configure itself from the parameter server
114 
115  if( private_nh.getParam( "odom_topic", odom_topic_ ))
116  {
118  }
119 
121 
122  initialized_ = true;
123 
124  dsrv_ = new dynamic_reconfigure::Server<DWAPlannerConfig>(private_nh);
125  dynamic_reconfigure::Server<DWAPlannerConfig>::CallbackType cb = boost::bind(&DWAPlannerROS::reconfigureCB, this, _1, _2);
126  dsrv_->setCallback(cb);
127  }
128  else{
129  ROS_WARN("This planner has already been initialized, doing nothing.");
130  }
131  }
132 
133  bool DWAPlannerROS::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) {
134  if (! isInitialized()) {
135  ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
136  return false;
137  }
138  //when we get a new plan, we also want to clear any latch we may have on goal tolerances
140 
141  ROS_INFO("Got new plan");
142  return dp_->setPlan(orig_global_plan);
143  }
144 
146  if (! isInitialized()) {
147  ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
148  return false;
149  }
151  ROS_ERROR("Could not get robot pose");
152  return false;
153  }
154 
156  ROS_INFO("Goal reached");
157  return true;
158  } else {
159  return false;
160  }
161  }
162 
163  void DWAPlannerROS::publishLocalPlan(std::vector<geometry_msgs::PoseStamped>& path) {
165  }
166 
167 
168  void DWAPlannerROS::publishGlobalPlan(std::vector<geometry_msgs::PoseStamped>& path) {
170  }
171 
173  //make sure to clean things up
174  delete dsrv_;
175  }
176 
177 
178 
179  bool DWAPlannerROS::dwaComputeVelocityCommands(tf::Stamped<tf::Pose> &global_pose, geometry_msgs::Twist& cmd_vel) {
180  // dynamic window sampling approach to get useful velocity commands
181  if(! isInitialized()){
182  ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
183  return false;
184  }
185 
186  tf::Stamped<tf::Pose> robot_vel;
187  odom_helper_.getRobotVel(robot_vel);
188 
189  /* For timing uncomment
190  struct timeval start, end;
191  double start_t, end_t, t_diff;
192  gettimeofday(&start, NULL);
193  */
194 
195  //compute what trajectory to drive along
196  tf::Stamped<tf::Pose> drive_cmds;
197  drive_cmds.frame_id_ = costmap_ros_->getBaseFrameID();
198 
199  // call with updated footprint
200  base_local_planner::Trajectory path = dp_->findBestPath(global_pose, robot_vel, drive_cmds);
201  //ROS_ERROR("Best: %.2f, %.2f, %.2f, %.2f", path.xv_, path.yv_, path.thetav_, path.cost_);
202 
203  /* For timing uncomment
204  gettimeofday(&end, NULL);
205  start_t = start.tv_sec + double(start.tv_usec) / 1e6;
206  end_t = end.tv_sec + double(end.tv_usec) / 1e6;
207  t_diff = end_t - start_t;
208  ROS_INFO("Cycle time: %.9f", t_diff);
209  */
210 
211  //pass along drive commands
212  cmd_vel.linear.x = drive_cmds.getOrigin().getX();
213  cmd_vel.linear.y = drive_cmds.getOrigin().getY();
214  cmd_vel.angular.z = tf::getYaw(drive_cmds.getRotation());
215 
216  //if we cannot move... tell someone
217  std::vector<geometry_msgs::PoseStamped> local_plan;
218  if(path.cost_ < 0) {
219  ROS_DEBUG_NAMED("dwa_local_planner",
220  "The dwa local planner failed to find a valid plan, cost functions discarded all candidates. This can mean there is an obstacle too close to the robot.");
221  local_plan.clear();
222  publishLocalPlan(local_plan);
223  return false;
224  }
225 
226  ROS_DEBUG_NAMED("dwa_local_planner", "A valid velocity command of (%.2f, %.2f, %.2f) was found for this cycle.",
227  cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z);
228 
229  // Fill out the local plan
230  for(unsigned int i = 0; i < path.getPointsSize(); ++i) {
231  double p_x, p_y, p_th;
232  path.getPoint(i, p_x, p_y, p_th);
233 
237  tf::Point(p_x, p_y, 0.0)),
238  ros::Time::now(),
240  geometry_msgs::PoseStamped pose;
241  tf::poseStampedTFToMsg(p, pose);
242  local_plan.push_back(pose);
243  }
244 
245  //publish information to the visualizer
246 
247  publishLocalPlan(local_plan);
248  return true;
249  }
250 
251 
252 
253 
254  bool DWAPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) {
255  // dispatches to either dwa sampling control or stop and rotate control, depending on whether we have been close enough to goal
257  ROS_ERROR("Could not get robot pose");
258  return false;
259  }
260  std::vector<geometry_msgs::PoseStamped> transformed_plan;
261  if ( ! planner_util_.getLocalPlan(current_pose_, transformed_plan)) {
262  ROS_ERROR("Could not get local plan");
263  return false;
264  }
265 
266  //if the global plan passed in is empty... we won't do anything
267  if(transformed_plan.empty()) {
268  ROS_WARN_NAMED("dwa_local_planner", "Received an empty transformed plan.");
269  return false;
270  }
271  ROS_DEBUG_NAMED("dwa_local_planner", "Received a transformed plan with %zu points.", transformed_plan.size());
272 
273  // update plan in dwa_planner even if we just stop and rotate, to allow checkTrajectory
274  dp_->updatePlanAndLocalCosts(current_pose_, transformed_plan, costmap_ros_->getRobotFootprint());
275 
277  //publish an empty plan because we've reached our goal position
278  std::vector<geometry_msgs::PoseStamped> local_plan;
279  std::vector<geometry_msgs::PoseStamped> transformed_plan;
280  publishGlobalPlan(transformed_plan);
281  publishLocalPlan(local_plan);
284  cmd_vel,
285  limits.getAccLimits(),
286  dp_->getSimPeriod(),
287  &planner_util_,
288  odom_helper_,
290  boost::bind(&DWAPlanner::checkTrajectory, dp_, _1, _2, _3));
291  } else {
292  bool isOk = dwaComputeVelocityCommands(current_pose_, cmd_vel);
293  if (isOk) {
294  publishGlobalPlan(transformed_plan);
295  } else {
296  ROS_WARN_NAMED("dwa_local_planner", "DWA planner failed to produce path.");
297  std::vector<geometry_msgs::PoseStamped> empty_plan;
298  publishGlobalPlan(empty_plan);
299  }
300  return isOk;
301  }
302  }
303 
304 
305 };
boost::shared_ptr< DWAPlanner > dp_
The trajectory controller.
void reconfigureCB(DWAPlannerConfig &config, uint32_t level)
Callback to update the local planner&#39;s parameters based on dynamic reconfigure.
void getRobotVel(tf::Stamped< tf::Pose > &robot_vel)
bool getRobotPose(tf::Stamped< tf::Pose > &global_pose) const
#define ROS_WARN_NAMED(name,...)
dynamic_reconfigure::Server< DWAPlannerConfig > * dsrv_
static void poseStampedTFToMsg(const Stamped< Pose > &bt, geometry_msgs::PoseStamped &msg)
void initialize(std::string name, tf::TransformListener *tf, costmap_2d::Costmap2DROS *costmap_ros)
Constructs the ros wrapper.
static double getYaw(const Quaternion &bt_q)
bool getLocalPlan(tf::Stamped< tf::Pose > &global_pose, std::vector< geometry_msgs::PoseStamped > &transformed_plan)
bool isGoalReached(LocalPlannerUtil *planner_util, OdometryHelperRos &odom_helper, tf::Stamped< tf::Pose > global_pose)
std::string getGlobalFrameID()
PLUGINLIB_EXPORT_CLASS(BAGReader, nodelet::Nodelet)
base_local_planner::OdometryHelperRos odom_helper_
void getPoint(unsigned int index, double &x, double &y, double &th) const
#define ROS_WARN(...)
base_local_planner::LatchedStopRotateController latchedStopRotateController_
ROS Wrapper for the DWAPlanner that adheres to the BaseLocalPlanner interface and can be used as a pl...
void initialize(tf::TransformListener *tf, costmap_2d::Costmap2D *costmap, std::string global_frame)
tf::Transform Pose
#define ROS_DEBUG_NAMED(name,...)
bool dwaComputeVelocityCommands(tf::Stamped< tf::Pose > &global_pose, geometry_msgs::Twist &cmd_vel)
Given the current position, orientation, and velocity of the robot, compute velocity commands to send...
bool computeVelocityCommandsStopRotate(geometry_msgs::Twist &cmd_vel, Eigen::Vector3f acc_lim, double sim_period, LocalPlannerUtil *planner_util, OdometryHelperRos &odom_helper, tf::Stamped< tf::Pose > global_pose, boost::function< bool(Eigen::Vector3f pos, Eigen::Vector3f vel, Eigen::Vector3f vel_samples)> obstacle_check)
tf::TransformListener * tf_
Used for transforming point clouds.
A class implementing a local planner using the Dynamic Window Approach.
Definition: dwa_planner.h:71
costmap_2d::Costmap2DROS * costmap_ros_
static Quaternion createQuaternionFromYaw(double yaw)
bool computeVelocityCommands(geometry_msgs::Twist &cmd_vel)
Given the current position, orientation, and velocity of the robot, compute velocity commands to send...
#define ROS_INFO(...)
LocalPlannerLimits getCurrentLimits()
std::string getBaseFrameID()
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void initialize(const std::string &name="")
bool setPlan(const std::vector< geometry_msgs::PoseStamped > &orig_global_plan)
Set the plan that the controller is following.
unsigned int getPointsSize() const
void publishPlan(const std::vector< geometry_msgs::PoseStamped > &path, const ros::Publisher &pub)
std::string frame_id_
void publishLocalPlan(std::vector< geometry_msgs::PoseStamped > &path)
bool getParam(const std::string &key, std::string &s) const
static Time now()
void setOdomTopic(std::string odom_topic)
void publishGlobalPlan(std::vector< geometry_msgs::PoseStamped > &path)
~DWAPlannerROS()
Destructor for the wrapper.
Costmap2D * getCostmap()
std::vector< geometry_msgs::Point > getRobotFootprint()
bool isGoalReached()
Check if the goal pose has been achieved.
tf::Stamped< tf::Pose > current_pose_
#define ROS_ERROR(...)
base_local_planner::LocalPlannerUtil planner_util_
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.
bool isPositionReached(LocalPlannerUtil *planner_util, tf::Stamped< tf::Pose > global_pose)


dwa_local_planner
Author(s): Eitan Marder-Eppstein, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:59