trajectory_planner_ros.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: Eitan Marder-Eppstein
36 *********************************************************************/
37 
39 
40 #include <sys/time.h>
41 #include <boost/tokenizer.hpp>
42 
43 #include <Eigen/Core>
44 #include <cmath>
45 
46 #include <ros/console.h>
47 
49 
51 #include <nav_msgs/Path.h>
52 
53 
54 
55 //register this planner as a BaseLocalPlanner plugin
57 
58 namespace base_local_planner {
59 
60  void TrajectoryPlannerROS::reconfigureCB(BaseLocalPlannerConfig &config, uint32_t level) {
61  if (setup_ && config.restore_defaults) {
62  config = default_config_;
63  //Avoid looping
64  config.restore_defaults = false;
65  }
66  if ( ! setup_) {
67  default_config_ = config;
68  setup_ = true;
69  }
70  tc_->reconfigure(config);
71  reached_goal_ = false;
72  }
73 
74  TrajectoryPlannerROS::TrajectoryPlannerROS() :
75  world_model_(NULL), tc_(NULL), costmap_ros_(NULL), tf_(NULL), setup_(false), initialized_(false), odom_helper_("odom") {}
76 
78  world_model_(NULL), tc_(NULL), costmap_ros_(NULL), tf_(NULL), setup_(false), initialized_(false), odom_helper_("odom") {
79 
80  //initialize the planner
81  initialize(name, tf, costmap_ros);
82  }
83 
85  std::string name,
87  costmap_2d::Costmap2DROS* costmap_ros){
88  if (! isInitialized()) {
89 
90  ros::NodeHandle private_nh("~/" + name);
91  g_plan_pub_ = private_nh.advertise<nav_msgs::Path>("global_plan", 1);
92  l_plan_pub_ = private_nh.advertise<nav_msgs::Path>("local_plan", 1);
93 
94 
95  tf_ = tf;
96  costmap_ros_ = costmap_ros;
97  rot_stopped_velocity_ = 1e-2;
99  double sim_time, sim_granularity, angular_sim_granularity;
100  int vx_samples, vtheta_samples;
101  double pdist_scale, gdist_scale, occdist_scale, heading_lookahead, oscillation_reset_dist, escape_reset_dist, escape_reset_theta;
102  bool holonomic_robot, dwa, simple_attractor, heading_scoring;
103  double heading_scoring_timestep;
104  double max_vel_x, min_vel_x;
105  double backup_vel;
106  double stop_time_buffer;
107  std::string world_model_type;
108  rotating_to_goal_ = false;
109 
110  //initialize the copy of the costmap the controller will use
112 
113 
116  private_nh.param("prune_plan", prune_plan_, true);
117 
118  private_nh.param("yaw_goal_tolerance", yaw_goal_tolerance_, 0.05);
119  private_nh.param("xy_goal_tolerance", xy_goal_tolerance_, 0.10);
120  private_nh.param("acc_lim_x", acc_lim_x_, 2.5);
121  private_nh.param("acc_lim_y", acc_lim_y_, 2.5);
122  private_nh.param("acc_lim_theta", acc_lim_theta_, 3.2);
123 
124  private_nh.param("stop_time_buffer", stop_time_buffer, 0.2);
125 
126  private_nh.param("latch_xy_goal_tolerance", latch_xy_goal_tolerance_, false);
127 
128  //Since I screwed up nicely in my documentation, I'm going to add errors
129  //informing the user if they've set one of the wrong parameters
130  if(private_nh.hasParam("acc_limit_x"))
131  ROS_ERROR("You are using acc_limit_x where you should be using acc_lim_x. Please change your configuration files appropriately. The documentation used to be wrong on this, sorry for any confusion.");
132 
133  if(private_nh.hasParam("acc_limit_y"))
134  ROS_ERROR("You are using acc_limit_y where you should be using acc_lim_y. Please change your configuration files appropriately. The documentation used to be wrong on this, sorry for any confusion.");
135 
136  if(private_nh.hasParam("acc_limit_th"))
137  ROS_ERROR("You are using acc_limit_th where you should be using acc_lim_th. Please change your configuration files appropriately. The documentation used to be wrong on this, sorry for any confusion.");
138 
139  //Assuming this planner is being run within the navigation stack, we can
140  //just do an upward search for the frequency at which its being run. This
141  //also allows the frequency to be overwritten locally.
142  std::string controller_frequency_param_name;
143  if(!private_nh.searchParam("controller_frequency", controller_frequency_param_name))
144  sim_period_ = 0.05;
145  else
146  {
147  double controller_frequency = 0;
148  private_nh.param(controller_frequency_param_name, controller_frequency, 20.0);
149  if(controller_frequency > 0)
150  sim_period_ = 1.0 / controller_frequency;
151  else
152  {
153  ROS_WARN("A controller_frequency less than 0 has been set. Ignoring the parameter, assuming a rate of 20Hz");
154  sim_period_ = 0.05;
155  }
156  }
157  ROS_INFO("Sim period is set to %.2f", sim_period_);
158 
159  private_nh.param("sim_time", sim_time, 1.0);
160  private_nh.param("sim_granularity", sim_granularity, 0.025);
161  private_nh.param("angular_sim_granularity", angular_sim_granularity, sim_granularity);
162  private_nh.param("vx_samples", vx_samples, 3);
163  private_nh.param("vtheta_samples", vtheta_samples, 20);
164 
165  private_nh.param("path_distance_bias", pdist_scale, 0.6);
166  private_nh.param("goal_distance_bias", gdist_scale, 0.8);
167  private_nh.param("occdist_scale", occdist_scale, 0.01);
168 
169  bool meter_scoring;
170  if ( ! private_nh.hasParam("meter_scoring")) {
171  ROS_WARN("Trajectory Rollout planner initialized with param meter_scoring not set. Set it to true to make your settins robust against changes of costmap resolution.");
172  } else {
173  private_nh.param("meter_scoring", meter_scoring, false);
174 
175  if(meter_scoring) {
176  //if we use meter scoring, then we want to multiply the biases by the resolution of the costmap
177  double resolution = costmap_->getResolution();
178  gdist_scale *= resolution;
179  pdist_scale *= resolution;
180  occdist_scale *= resolution;
181  } else {
182  ROS_WARN("Trajectory Rollout planner initialized with param meter_scoring set to false. Set it to true to make your settins robust against changes of costmap resolution.");
183  }
184  }
185 
186  private_nh.param("heading_lookahead", heading_lookahead, 0.325);
187  private_nh.param("oscillation_reset_dist", oscillation_reset_dist, 0.05);
188  private_nh.param("escape_reset_dist", escape_reset_dist, 0.10);
189  private_nh.param("escape_reset_theta", escape_reset_theta, M_PI_4);
190  private_nh.param("holonomic_robot", holonomic_robot, true);
191  private_nh.param("max_vel_x", max_vel_x, 0.5);
192  private_nh.param("min_vel_x", min_vel_x, 0.1);
193 
194  double max_rotational_vel;
195  private_nh.param("max_rotational_vel", max_rotational_vel, 1.0);
196  max_vel_th_ = max_rotational_vel;
197  min_vel_th_ = -1.0 * max_rotational_vel;
198  private_nh.param("min_in_place_rotational_vel", min_in_place_vel_th_, 0.4);
199  reached_goal_ = false;
200  backup_vel = -0.1;
201  if(private_nh.getParam("backup_vel", backup_vel))
202  ROS_WARN("The backup_vel parameter has been deprecated in favor of the escape_vel parameter. To switch, just change the parameter name in your configuration files.");
203 
204  //if both backup_vel and escape_vel are set... we'll use escape_vel
205  private_nh.getParam("escape_vel", backup_vel);
206 
207  if(backup_vel >= 0.0)
208  ROS_WARN("You've specified a positive escape velocity. This is probably not what you want and will cause the robot to move forward instead of backward. You should probably change your escape_vel parameter to be negative");
209 
210  private_nh.param("world_model", world_model_type, std::string("costmap"));
211  private_nh.param("dwa", dwa, true);
212  private_nh.param("heading_scoring", heading_scoring, false);
213  private_nh.param("heading_scoring_timestep", heading_scoring_timestep, 0.8);
214 
215  simple_attractor = false;
216 
217  //parameters for using the freespace controller
218  double min_pt_separation, max_obstacle_height, grid_resolution;
219  private_nh.param("point_grid/max_sensor_range", max_sensor_range_, 2.0);
220  private_nh.param("point_grid/min_pt_separation", min_pt_separation, 0.01);
221  private_nh.param("point_grid/max_obstacle_height", max_obstacle_height, 2.0);
222  private_nh.param("point_grid/grid_resolution", grid_resolution, 0.2);
223 
224  ROS_ASSERT_MSG(world_model_type == "costmap", "At this time, only costmap world models are supported by this controller");
226  std::vector<double> y_vels = loadYVels(private_nh);
227 
229 
231  acc_lim_x_, acc_lim_y_, acc_lim_theta_, sim_time, sim_granularity, vx_samples, vtheta_samples, pdist_scale,
232  gdist_scale, occdist_scale, heading_lookahead, oscillation_reset_dist, escape_reset_dist, escape_reset_theta, holonomic_robot,
233  max_vel_x, min_vel_x, max_vel_th_, min_vel_th_, min_in_place_vel_th_, backup_vel,
234  dwa, heading_scoring, heading_scoring_timestep, meter_scoring, simple_attractor, y_vels, stop_time_buffer, sim_period_, angular_sim_granularity);
235 
236  map_viz_.initialize(name, global_frame_, boost::bind(&TrajectoryPlanner::getCellCosts, tc_, _1, _2, _3, _4, _5, _6));
237  initialized_ = true;
238 
239  dsrv_ = new dynamic_reconfigure::Server<BaseLocalPlannerConfig>(private_nh);
240  dynamic_reconfigure::Server<BaseLocalPlannerConfig>::CallbackType cb = boost::bind(&TrajectoryPlannerROS::reconfigureCB, this, _1, _2);
241  dsrv_->setCallback(cb);
242 
243  } else {
244  ROS_WARN("This planner has already been initialized, doing nothing");
245  }
246  }
247 
249  std::vector<double> y_vels;
250 
251  std::string y_vel_list;
252  if(node.getParam("y_vels", y_vel_list)){
253  typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
254  boost::char_separator<char> sep("[], ");
255  tokenizer tokens(y_vel_list, sep);
256 
257  for(tokenizer::iterator i = tokens.begin(); i != tokens.end(); i++){
258  y_vels.push_back(atof((*i).c_str()));
259  }
260  }
261  else{
262  //if no values are passed in, we'll provide defaults
263  y_vels.push_back(-0.3);
264  y_vels.push_back(-0.1);
265  y_vels.push_back(0.1);
266  y_vels.push_back(0.3);
267  }
268 
269  return y_vels;
270  }
271 
273  //make sure to clean things up
274  delete dsrv_;
275 
276  if(tc_ != NULL)
277  delete tc_;
278 
279  if(world_model_ != NULL)
280  delete world_model_;
281  }
282 
283  bool TrajectoryPlannerROS::stopWithAccLimits(const tf::Stamped<tf::Pose>& global_pose, const tf::Stamped<tf::Pose>& robot_vel, geometry_msgs::Twist& cmd_vel){
284  //slow down with the maximum possible acceleration... we should really use the frequency that we're running at to determine what is feasible
285  //but we'll use a tenth of a second to be consistent with the implementation of the local planner.
286  double vx = sign(robot_vel.getOrigin().x()) * std::max(0.0, (fabs(robot_vel.getOrigin().x()) - acc_lim_x_ * sim_period_));
287  double vy = sign(robot_vel.getOrigin().y()) * std::max(0.0, (fabs(robot_vel.getOrigin().y()) - acc_lim_y_ * sim_period_));
288 
289  double vel_yaw = tf::getYaw(robot_vel.getRotation());
290  double vth = sign(vel_yaw) * std::max(0.0, (fabs(vel_yaw) - acc_lim_theta_ * sim_period_));
291 
292  //we do want to check whether or not the command is valid
293  double yaw = tf::getYaw(global_pose.getRotation());
294  bool valid_cmd = tc_->checkTrajectory(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), yaw,
295  robot_vel.getOrigin().getX(), robot_vel.getOrigin().getY(), vel_yaw, vx, vy, vth);
296 
297  //if we have a valid command, we'll pass it on, otherwise we'll command all zeros
298  if(valid_cmd){
299  ROS_DEBUG("Slowing down... using vx, vy, vth: %.2f, %.2f, %.2f", vx, vy, vth);
300  cmd_vel.linear.x = vx;
301  cmd_vel.linear.y = vy;
302  cmd_vel.angular.z = vth;
303  return true;
304  }
305 
306  cmd_vel.linear.x = 0.0;
307  cmd_vel.linear.y = 0.0;
308  cmd_vel.angular.z = 0.0;
309  return false;
310  }
311 
312  bool TrajectoryPlannerROS::rotateToGoal(const tf::Stamped<tf::Pose>& global_pose, const tf::Stamped<tf::Pose>& robot_vel, double goal_th, geometry_msgs::Twist& cmd_vel){
313  double yaw = tf::getYaw(global_pose.getRotation());
314  double vel_yaw = tf::getYaw(robot_vel.getRotation());
315  cmd_vel.linear.x = 0;
316  cmd_vel.linear.y = 0;
317  double ang_diff = angles::shortest_angular_distance(yaw, goal_th);
318 
319  double v_theta_samp = ang_diff > 0.0 ? std::min(max_vel_th_,
320  std::max(min_in_place_vel_th_, ang_diff)) : std::max(min_vel_th_,
321  std::min(-1.0 * min_in_place_vel_th_, ang_diff));
322 
323  //take the acceleration limits of the robot into account
324  double max_acc_vel = fabs(vel_yaw) + acc_lim_theta_ * sim_period_;
325  double min_acc_vel = fabs(vel_yaw) - acc_lim_theta_ * sim_period_;
326 
327  v_theta_samp = sign(v_theta_samp) * std::min(std::max(fabs(v_theta_samp), min_acc_vel), max_acc_vel);
328 
329  //we also want to make sure to send a velocity that allows us to stop when we reach the goal given our acceleration limits
330  double max_speed_to_stop = sqrt(2 * acc_lim_theta_ * fabs(ang_diff));
331 
332  v_theta_samp = sign(v_theta_samp) * std::min(max_speed_to_stop, fabs(v_theta_samp));
333 
334  // Re-enforce min_in_place_vel_th_. It is more important than the acceleration limits.
335  v_theta_samp = v_theta_samp > 0.0
336  ? std::min( max_vel_th_, std::max( min_in_place_vel_th_, v_theta_samp ))
337  : std::max( min_vel_th_, std::min( -1.0 * min_in_place_vel_th_, v_theta_samp ));
338 
339  //we still want to lay down the footprint of the robot and check if the action is legal
340  bool valid_cmd = tc_->checkTrajectory(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), yaw,
341  robot_vel.getOrigin().getX(), robot_vel.getOrigin().getY(), vel_yaw, 0.0, 0.0, v_theta_samp);
342 
343  ROS_DEBUG("Moving to desired goal orientation, th cmd: %.2f, valid_cmd: %d", v_theta_samp, valid_cmd);
344 
345  if(valid_cmd){
346  cmd_vel.angular.z = v_theta_samp;
347  return true;
348  }
349 
350  cmd_vel.angular.z = 0.0;
351  return false;
352 
353  }
354 
355  bool TrajectoryPlannerROS::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan){
356  if (! isInitialized()) {
357  ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
358  return false;
359  }
360 
361  //reset the global plan
362  global_plan_.clear();
363  global_plan_ = orig_global_plan;
364 
365  //when we get a new plan, we also want to clear any latch we may have on goal tolerances
366  xy_tolerance_latch_ = false;
367  //reset the at goal flag
368  reached_goal_ = false;
369  return true;
370  }
371 
372  bool TrajectoryPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel){
373  if (! isInitialized()) {
374  ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
375  return false;
376  }
377 
378  std::vector<geometry_msgs::PoseStamped> local_plan;
379  tf::Stamped<tf::Pose> global_pose;
380  if (!costmap_ros_->getRobotPose(global_pose)) {
381  return false;
382  }
383 
384  std::vector<geometry_msgs::PoseStamped> transformed_plan;
385  //get the global plan in our frame
386  if (!transformGlobalPlan(*tf_, global_plan_, global_pose, *costmap_, global_frame_, transformed_plan)) {
387  ROS_WARN("Could not transform the global plan to the frame of the controller");
388  return false;
389  }
390 
391  //now we'll prune the plan based on the position of the robot
392  if(prune_plan_)
393  prunePlan(global_pose, transformed_plan, global_plan_);
394 
395  tf::Stamped<tf::Pose> drive_cmds;
396  drive_cmds.frame_id_ = robot_base_frame_;
397 
398  tf::Stamped<tf::Pose> robot_vel;
399  odom_helper_.getRobotVel(robot_vel);
400 
401  /* For timing uncomment
402  struct timeval start, end;
403  double start_t, end_t, t_diff;
404  gettimeofday(&start, NULL);
405  */
406 
407  //if the global plan passed in is empty... we won't do anything
408  if(transformed_plan.empty())
409  return false;
410 
411  tf::Stamped<tf::Pose> goal_point;
412  tf::poseStampedMsgToTF(transformed_plan.back(), goal_point);
413  //we assume the global goal is the last point in the global plan
414  double goal_x = goal_point.getOrigin().getX();
415  double goal_y = goal_point.getOrigin().getY();
416 
417  double yaw = tf::getYaw(goal_point.getRotation());
418 
419  double goal_th = yaw;
420 
421  //check to see if we've reached the goal position
422  if (xy_tolerance_latch_ || (getGoalPositionDistance(global_pose, goal_x, goal_y) <= xy_goal_tolerance_)) {
423 
424  //if the user wants to latch goal tolerance, if we ever reach the goal location, we'll
425  //just rotate in place
427  xy_tolerance_latch_ = true;
428  }
429 
430  double angle = getGoalOrientationAngleDifference(global_pose, goal_th);
431  //check to see if the goal orientation has been reached
432  if (fabs(angle) <= yaw_goal_tolerance_) {
433  //set the velocity command to zero
434  cmd_vel.linear.x = 0.0;
435  cmd_vel.linear.y = 0.0;
436  cmd_vel.angular.z = 0.0;
437  rotating_to_goal_ = false;
438  xy_tolerance_latch_ = false;
439  reached_goal_ = true;
440  } else {
441  //we need to call the next two lines to make sure that the trajectory
442  //planner updates its path distance and goal distance grids
443  tc_->updatePlan(transformed_plan);
444  Trajectory path = tc_->findBestPath(global_pose, robot_vel, drive_cmds);
446 
447  //copy over the odometry information
448  nav_msgs::Odometry base_odom;
449  odom_helper_.getOdom(base_odom);
450 
451  //if we're not stopped yet... we want to stop... taking into account the acceleration limits of the robot
453  if ( ! stopWithAccLimits(global_pose, robot_vel, cmd_vel)) {
454  return false;
455  }
456  }
457  //if we're stopped... then we want to rotate to goal
458  else{
459  //set this so that we know its OK to be moving
460  rotating_to_goal_ = true;
461  if(!rotateToGoal(global_pose, robot_vel, goal_th, cmd_vel)) {
462  return false;
463  }
464  }
465  }
466 
467  //publish an empty plan because we've reached our goal position
468  publishPlan(transformed_plan, g_plan_pub_);
469  publishPlan(local_plan, l_plan_pub_);
470 
471  //we don't actually want to run the controller when we're just rotating to goal
472  return true;
473  }
474 
475  tc_->updatePlan(transformed_plan);
476 
477  //compute what trajectory to drive along
478  Trajectory path = tc_->findBestPath(global_pose, robot_vel, drive_cmds);
479 
481  /* For timing uncomment
482  gettimeofday(&end, NULL);
483  start_t = start.tv_sec + double(start.tv_usec) / 1e6;
484  end_t = end.tv_sec + double(end.tv_usec) / 1e6;
485  t_diff = end_t - start_t;
486  ROS_INFO("Cycle time: %.9f", t_diff);
487  */
488 
489  //pass along drive commands
490  cmd_vel.linear.x = drive_cmds.getOrigin().getX();
491  cmd_vel.linear.y = drive_cmds.getOrigin().getY();
492  cmd_vel.angular.z = tf::getYaw(drive_cmds.getRotation());
493 
494  //if we cannot move... tell someone
495  if (path.cost_ < 0) {
496  ROS_DEBUG_NAMED("trajectory_planner_ros",
497  "The rollout planner failed to find a valid plan. This means that the footprint of the robot was in collision for all simulated trajectories.");
498  local_plan.clear();
499  publishPlan(transformed_plan, g_plan_pub_);
500  publishPlan(local_plan, l_plan_pub_);
501  return false;
502  }
503 
504  ROS_DEBUG_NAMED("trajectory_planner_ros", "A valid velocity command of (%.2f, %.2f, %.2f) was found for this cycle.",
505  cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z);
506 
507  // Fill out the local plan
508  for (unsigned int i = 0; i < path.getPointsSize(); ++i) {
509  double p_x, p_y, p_th;
510  path.getPoint(i, p_x, p_y, p_th);
514  tf::Point(p_x, p_y, 0.0)),
515  ros::Time::now(),
516  global_frame_);
517  geometry_msgs::PoseStamped pose;
518  tf::poseStampedTFToMsg(p, pose);
519  local_plan.push_back(pose);
520  }
521 
522  //publish information to the visualizer
523  publishPlan(transformed_plan, g_plan_pub_);
524  publishPlan(local_plan, l_plan_pub_);
525  return true;
526  }
527 
528  bool TrajectoryPlannerROS::checkTrajectory(double vx_samp, double vy_samp, double vtheta_samp, bool update_map){
529  tf::Stamped<tf::Pose> global_pose;
530  if(costmap_ros_->getRobotPose(global_pose)){
531  if(update_map){
532  //we need to give the planne some sort of global plan, since we're only checking for legality
533  //we'll just give the robots current position
534  std::vector<geometry_msgs::PoseStamped> plan;
535  geometry_msgs::PoseStamped pose_msg;
536  tf::poseStampedTFToMsg(global_pose, pose_msg);
537  plan.push_back(pose_msg);
538  tc_->updatePlan(plan, true);
539  }
540 
541  //copy over the odometry information
542  nav_msgs::Odometry base_odom;
543  {
544  boost::recursive_mutex::scoped_lock lock(odom_lock_);
545  base_odom = base_odom_;
546  }
547 
548  return tc_->checkTrajectory(global_pose.getOrigin().x(), global_pose.getOrigin().y(), tf::getYaw(global_pose.getRotation()),
549  base_odom.twist.twist.linear.x,
550  base_odom.twist.twist.linear.y,
551  base_odom.twist.twist.angular.z, vx_samp, vy_samp, vtheta_samp);
552 
553  }
554  ROS_WARN("Failed to get the pose of the robot. No trajectories will pass as legal in this case.");
555  return false;
556  }
557 
558 
559  double TrajectoryPlannerROS::scoreTrajectory(double vx_samp, double vy_samp, double vtheta_samp, bool update_map){
560  // Copy of checkTrajectory that returns a score instead of True / False
561  tf::Stamped<tf::Pose> global_pose;
562  if(costmap_ros_->getRobotPose(global_pose)){
563  if(update_map){
564  //we need to give the planne some sort of global plan, since we're only checking for legality
565  //we'll just give the robots current position
566  std::vector<geometry_msgs::PoseStamped> plan;
567  geometry_msgs::PoseStamped pose_msg;
568  tf::poseStampedTFToMsg(global_pose, pose_msg);
569  plan.push_back(pose_msg);
570  tc_->updatePlan(plan, true);
571  }
572 
573  //copy over the odometry information
574  nav_msgs::Odometry base_odom;
575  {
576  boost::recursive_mutex::scoped_lock lock(odom_lock_);
577  base_odom = base_odom_;
578  }
579 
580  return tc_->scoreTrajectory(global_pose.getOrigin().x(), global_pose.getOrigin().y(), tf::getYaw(global_pose.getRotation()),
581  base_odom.twist.twist.linear.x,
582  base_odom.twist.twist.linear.y,
583  base_odom.twist.twist.angular.z, vx_samp, vy_samp, vtheta_samp);
584 
585  }
586  ROS_WARN("Failed to get the pose of the robot. No trajectories will pass as legal in this case.");
587  return -1.0;
588  }
589 
591  if (! isInitialized()) {
592  ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
593  return false;
594  }
595  //return flag set in controller
596  return reached_goal_;
597  }
598 };
Computes control velocities for a robot given a costmap, a plan, and the robot&#39;s position in the worl...
void initialize(std::string name, tf::TransformListener *tf, costmap_2d::Costmap2DROS *costmap_ros)
Constructs the ros wrapper.
nav_msgs::Odometry base_odom_
Used to get the velocity of the robot.
bool setPlan(const std::vector< geometry_msgs::PoseStamped > &orig_global_plan)
Set the plan that the controller is following.
std::string robot_base_frame_
Used as the base frame id of the robot.
std::vector< double > loadYVels(ros::NodeHandle node)
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 getRobotVel(tf::Stamped< tf::Pose > &robot_vel)
double scoreTrajectory(double x, double y, double theta, double vx, double vy, double vtheta, double vx_samp, double vy_samp, double vtheta_samp)
Generate and score a single trajectory.
bool isGoalReached()
Check if the goal pose has been achieved.
std::vector< geometry_msgs::Point > footprint_spec_
bool getRobotPose(tf::Stamped< tf::Pose > &global_pose) const
double max_sensor_range_
Keep track of the effective maximum range of our sensors.
void publishCostCloud(const costmap_2d::Costmap2D *costmap_p_)
Build and publish a PointCloud if the publish_cost_grid_pc parameter was true. Only include points fo...
bool computeVelocityCommands(geometry_msgs::Twist &cmd_vel)
Given the current position, orientation, and velocity of the robot, compute velocity commands to send...
double getGoalPositionDistance(const tf::Stamped< tf::Pose > &global_pose, double goal_x, double goal_y)
return squared distance to check if the goal position has been achieved
bool rotateToGoal(const tf::Stamped< tf::Pose > &global_pose, const tf::Stamped< tf::Pose > &robot_vel, double goal_th, geometry_msgs::Twist &cmd_vel)
Once a goal position is reached... rotate to the goal orientation.
static void poseStampedTFToMsg(const Stamped< Pose > &bt, geometry_msgs::PoseStamped &msg)
static double getYaw(const Quaternion &bt_q)
std::string getGlobalFrameID()
PLUGINLIB_EXPORT_CLASS(BAGReader, nodelet::Nodelet)
void getOdom(nav_msgs::Odometry &base_odom)
void prunePlan(const tf::Stamped< tf::Pose > &global_pose, std::vector< geometry_msgs::PoseStamped > &plan, std::vector< geometry_msgs::PoseStamped > &global_plan)
Trim off parts of the global plan that are far enough behind the robot.
bool checkTrajectory(double x, double y, double theta, double vx, double vy, double vtheta, double vx_samp, double vy_samp, double vtheta_samp)
Generate and score a single trajectory.
dynamic_reconfigure::Server< BaseLocalPlannerConfig > * dsrv_
void getPoint(unsigned int index, double &x, double &y, double &th) const
Get a point within the trajectory.
Definition: trajectory.cpp:47
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)
Initializes the MapGridVisualizer.
double cost_
The cost/score of the trajectory.
Definition: trajectory.h:62
tf::TransformListener * tf_
costmap_2d::Costmap2D * costmap_
The costmap the controller will use.
#define ROS_WARN(...)
bool stopped(const nav_msgs::Odometry &base_odom, const double &rot_stopped_velocity, const double &trans_stopped_velocity)
Check whether the robot is stopped or not.
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
std::string global_frame_
The frame in which the controller will run.
tf::Transform Pose
#define ROS_DEBUG_NAMED(name,...)
static Quaternion createQuaternionFromYaw(double yaw)
bool transformGlobalPlan(const tf::TransformListener &tf, const std::vector< geometry_msgs::PoseStamped > &global_plan, const tf::Stamped< tf::Pose > &global_robot_pose, const costmap_2d::Costmap2D &costmap, const std::string &global_frame, std::vector< geometry_msgs::PoseStamped > &transformed_plan)
Transforms the global plan of the robot from the planner frame to the frame of the costmap...
#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
double scoreTrajectory(double vx_samp, double vy_samp, double vtheta_samp, bool update_map=true)
Generate and score a single trajectory.
#define ROS_ASSERT_MSG(cond,...)
MapGridVisualizer map_viz_
The map grid visualizer for outputting the potential field generated by the cost function.
std::string getBaseFrameID()
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
TrajectoryPlanner * tc_
The trajectory controller.
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
unsigned int getPointsSize() const
Return the number of points in the trajectory.
Definition: trajectory.cpp:77
void updatePlan(const std::vector< geometry_msgs::PoseStamped > &new_plan, bool compute_dists=false)
Update the plan that the controller is following.
void publishPlan(const std::vector< geometry_msgs::PoseStamped > &path, const ros::Publisher &pub)
Publish a plan for visualization purposes.
bool stopWithAccLimits(const tf::Stamped< tf::Pose > &global_pose, const tf::Stamped< tf::Pose > &robot_vel, geometry_msgs::Twist &cmd_vel)
Stop the robot taking into account acceleration limits.
bool checkTrajectory(double vx_samp, double vy_samp, double vtheta_samp, bool update_map=true)
Generate and score a single trajectory.
std::string frame_id_
base_local_planner::OdometryHelperRos odom_helper_
bool getParam(const std::string &key, std::string &s) const
void reconfigureCB(BaseLocalPlannerConfig &config, uint32_t level)
Callback to update the local planner&#39;s parameters based on dynamic reconfigure.
WorldModel * world_model_
The world model that the controller will use.
Trajectory findBestPath(tf::Stamped< tf::Pose > global_pose, tf::Stamped< tf::Pose > global_vel, tf::Stamped< tf::Pose > &drive_velocities)
Given the current position, orientation, and velocity of the robot, return a trajectory to follow...
static Time now()
std::vector< geometry_msgs::PoseStamped > global_plan_
static void poseStampedMsgToTF(const geometry_msgs::PoseStamped &msg, Stamped< Pose > &bt)
TrajectoryPlannerROS()
Default constructor for the ros wrapper.
tf::TransformListener * tf_
Used for transforming point clouds.
A class that implements the WorldModel interface to provide grid based collision checks for the traje...
Definition: costmap_model.h:50
double getResolution() const
Costmap2D * getCostmap()
bool hasParam(const std::string &key) const
~TrajectoryPlannerROS()
Destructor for the wrapper.
std::vector< geometry_msgs::Point > getRobotFootprint()
A ROS wrapper for the trajectory controller that queries the param server to construct a controller...
#define ROS_ERROR(...)
costmap_2d::Costmap2DROS * costmap_ros_
The ROS wrapper for the costmap the controller will use.
def shortest_angular_distance(from_angle, to_angle)
double getGoalOrientationAngleDifference(const tf::Stamped< tf::Pose > &global_pose, double goal_th)
return angle difference to goal to check if the goal orientation has been achieved ...
Holds a trajectory generated by considering an x, y, and theta velocity.
Definition: trajectory.h:44
#define ROS_DEBUG(...)


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:49