trajectory_planner.h
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 #ifndef TRAJECTORY_ROLLOUT_TRAJECTORY_PLANNER_H_
38 #define TRAJECTORY_ROLLOUT_TRAJECTORY_PLANNER_H_
39 
40 #include <vector>
41 #include <cmath>
42 
43 //for obstacle data access
44 #include <costmap_2d/costmap_2d.h>
45 #include <costmap_2d/cost_values.h>
47 
50 #include <base_local_planner/Position2DInt.h>
51 #include <base_local_planner/BaseLocalPlannerConfig.h>
52 
53 //we'll take in a path as a vector of poses
54 #include <geometry_msgs/PoseStamped.h>
55 #include <geometry_msgs/Point.h>
56 
57 //for some datatypes
58 #include <tf/transform_datatypes.h>
59 
60 //for creating a local cost grid
63 
64 namespace base_local_planner {
70  friend class TrajectoryPlannerTest; //Need this for gtest to work
71  public:
108  TrajectoryPlanner(WorldModel& world_model,
109  const costmap_2d::Costmap2D& costmap,
110  std::vector<geometry_msgs::Point> footprint_spec,
111  double acc_lim_x = 1.0, double acc_lim_y = 1.0, double acc_lim_theta = 1.0,
112  double sim_time = 1.0, double sim_granularity = 0.025,
113  int vx_samples = 20, int vtheta_samples = 20,
114  double pdist_scale = 0.6, double gdist_scale = 0.8, double occdist_scale = 0.2,
115  double heading_lookahead = 0.325, double oscillation_reset_dist = 0.05,
116  double escape_reset_dist = 0.10, double escape_reset_theta = M_PI_2,
117  bool holonomic_robot = true,
118  double max_vel_x = 0.5, double min_vel_x = 0.1,
119  double max_vel_th = 1.0, double min_vel_th = -1.0, double min_in_place_vel_th = 0.4,
120  double backup_vel = -0.1,
121  bool dwa = false, bool heading_scoring = false, double heading_scoring_timestep = 0.1,
122  bool meter_scoring = true,
123  bool simple_attractor = false,
124  std::vector<double> y_vels = std::vector<double>(0),
125  double stop_time_buffer = 0.2,
126  double sim_period = 0.1, double angular_sim_granularity = 0.025);
127 
132 
136  void reconfigure(BaseLocalPlannerConfig &cfg);
137 
146  tf::Stamped<tf::Pose>& drive_velocities);
147 
153  void updatePlan(const std::vector<geometry_msgs::PoseStamped>& new_plan, bool compute_dists = false);
154 
160  void getLocalGoal(double& x, double& y);
161 
175  bool checkTrajectory(double x, double y, double theta, double vx, double vy,
176  double vtheta, double vx_samp, double vy_samp, double vtheta_samp);
177 
191  double scoreTrajectory(double x, double y, double theta, double vx, double vy,
192  double vtheta, double vx_samp, double vy_samp, double vtheta_samp);
193 
204  bool getCellCosts(int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost);
205 
207  void setFootprint( std::vector<geometry_msgs::Point> footprint ) { footprint_spec_ = footprint; }
208 
210  geometry_msgs::Polygon getFootprintPolygon() const { return costmap_2d::toPolygon(footprint_spec_); }
211  std::vector<geometry_msgs::Point> getFootprint() const { return footprint_spec_; }
212 
213  private:
227  Trajectory createTrajectories(double x, double y, double theta, double vx, double vy, double vtheta,
228  double acc_x, double acc_y, double acc_theta);
229 
247  void generateTrajectory(double x, double y, double theta, double vx, double vy,
248  double vtheta, double vx_samp, double vy_samp, double vtheta_samp, double acc_x, double acc_y,
249  double acc_theta, double impossible_cost, Trajectory& traj);
250 
258  double footprintCost(double x_i, double y_i, double theta_i);
259 
261 
266 
267  std::vector<geometry_msgs::Point> footprint_spec_;
268 
269  std::vector<geometry_msgs::PoseStamped> global_plan_;
270 
273 
276 
277  bool escaping_;
279 
280  double goal_x_,goal_y_;
281 
284 
285  double sim_time_;
288 
291 
294 
295  double prev_x_, prev_y_;
297 
299 
304 
306 
307  double backup_vel_;
308 
309  bool dwa_;
313 
314  std::vector<double> y_vels_;
315 
317  double sim_period_;
318 
320 
321  boost::mutex configuration_mutex_;
322 
332  inline double computeNewXPosition(double xi, double vx, double vy, double theta, double dt){
333  return xi + (vx * cos(theta) + vy * cos(M_PI_2 + theta)) * dt;
334  }
335 
345  inline double computeNewYPosition(double yi, double vx, double vy, double theta, double dt){
346  return yi + (vx * sin(theta) + vy * sin(M_PI_2 + theta)) * dt;
347  }
348 
356  inline double computeNewThetaPosition(double thetai, double vth, double dt){
357  return thetai + vth * dt;
358  }
359 
360  //compute velocity based on acceleration
369  inline double computeNewVelocity(double vg, double vi, double a_max, double dt){
370  if((vg - vi) >= 0) {
371  return std::min(vg, vi + a_max * dt);
372  }
373  return std::max(vg, vi - a_max * dt);
374  }
375 
376  void getMaxSpeedToStopInTime(double time, double& vx, double& vy, double& vth){
377  vx = acc_lim_x_ * std::max(time, 0.0);
378  vy = acc_lim_y_ * std::max(time, 0.0);
379  vth = acc_lim_theta_ * std::max(time, 0.0);
380  }
381 
382  double lineCost(int x0, int x1, int y0, int y1);
383  double pointCost(int x, int y);
384  double headingDiff(int cell_x, int cell_y, double x, double y, double heading);
385  };
386 };
387 
388 #endif
int vx_samples_
The number of samples we&#39;ll take in the x dimenstion of the control space.
double stop_time_buffer_
How long before hitting something we&#39;re going to enforce that the robot stop.
Computes control velocities for a robot given a costmap, a plan, and the robot&#39;s position in the worl...
double angular_sim_granularity_
The distance between angular simulation points.
double computeNewYPosition(double yi, double vx, double vy, double theta, double dt)
Compute y position based on velocity.
double heading_scoring_timestep_
How far to look ahead in time when we score a heading.
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.
int vtheta_samples_
The number of samples we&#39;ll take in the theta dimension of the control space.
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.
base_local_planner::FootprintHelper footprint_helper_
double computeNewXPosition(double xi, double vx, double vy, double theta, double dt)
Compute x position based on velocity.
double oscillation_reset_dist_
The distance the robot must travel before it can explore rotational velocities that were unsuccessful...
bool final_goal_position_valid_
True if final_goal_x_ and final_goal_y_ have valid data. Only false if an empty path is sent...
bool stuck_right
Booleans to keep the robot from oscillating during rotation.
const costmap_2d::Costmap2D & costmap_
Provides access to cost map information.
double heading_lookahead_
How far the robot should look ahead of itself when differentiating between different rotational veloc...
void setFootprint(std::vector< geometry_msgs::Point > footprint)
Set the footprint specification of the robot.
double goal_y_
Storage for the local goal the robot is pursuing.
~TrajectoryPlanner()
Destructs a trajectory controller.
bool holonomic_robot_
Is the robot holonomic or not?
double min_in_place_vel_th_
Velocity limits for the controller.
double computeNewThetaPosition(double thetai, double vth, double dt)
Compute orientation based on velocity.
double lineCost(int x0, int x1, int y0, int y1)
MapGrid path_map_
The local map grid where we propagate path distance.
double prev_y_
Used to calculate the distance the robot has traveled before reseting oscillation booleans...
bool dwa_
Should we use the dynamic window approach?
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.
double sim_time_
The number of seconds each trajectory is "rolled-out".
double backup_vel_
The velocity to use while backing up.
TFSIMD_FORCE_INLINE const tfScalar & y() const
double sim_period_
The number of seconds to use to compute max/min vels for dwa.
bool simple_attractor_
Enables simple attraction to a goal point.
std::vector< double > y_vels_
Y velocities to explore.
double escape_reset_theta_
The distance the robot must travel before it can leave escape mode.
Trajectory createTrajectories(double x, double y, double theta, double vx, double vy, double vtheta, double acc_x, double acc_y, double acc_theta)
Create the trajectories we wish to explore, score them, and return the best option.
A grid of MapCell cells that is used to propagate path and goal distances for the trajectory controll...
Definition: map_grid.h:52
double escape_theta_
Used to calculate the distance the robot has traveled before reseting escape booleans.
WorldModel & world_model_
The world model that the controller uses for collision detection.
std::vector< geometry_msgs::PoseStamped > global_plan_
The global path for the robot to follow.
double footprintCost(double x_i, double y_i, double theta_i)
Checks the legality of the robot footprint at a position and orientation using the world model...
double occdist_scale_
Scaling factors for the controller&#39;s cost function.
MapGrid goal_map_
The local map grid where we propagate goal distance.
bool escaping_
Boolean to keep track of whether we&#39;re in escape mode.
double headingDiff(int cell_x, int cell_y, double x, double y, double heading)
std::vector< geometry_msgs::Point > getFootprint() const
TFSIMD_FORCE_INLINE const tfScalar & x() const
double sim_granularity_
The distance between simulation points.
double acc_lim_theta_
The acceleration limits of the robot.
Trajectory traj_two
Used for scoring trajectories.
geometry_msgs::Polygon toPolygon(std::vector< geometry_msgs::Point > pts)
void updatePlan(const std::vector< geometry_msgs::PoseStamped > &new_plan, bool compute_dists=false)
Update the plan that the controller is following.
An interface the trajectory controller uses to interact with the world regardless of the underlying w...
Definition: world_model.h:52
double final_goal_y_
The end position of the plan.
void generateTrajectory(double x, double y, double theta, double vx, double vy, double vtheta, double vx_samp, double vy_samp, double vtheta_samp, double acc_x, double acc_y, double acc_theta, double impossible_cost, Trajectory &traj)
Generate and score a single trajectory.
bool heading_scoring_
Should we score based on the rollout approach or the heading approach.
bool stuck_right_strafe
Booleans to keep the robot from oscillating during strafing.
std::vector< geometry_msgs::Point > footprint_spec_
The footprint specification of the robot.
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...
double computeNewVelocity(double vg, double vi, double a_max, double dt)
Compute velocity based on acceleration.
void getLocalGoal(double &x, double &y)
Accessor for the goal the robot is currently pursuing in world corrdinates.
bool strafe_left
Booleans to keep track of strafe direction for the robot.
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
void reconfigure(BaseLocalPlannerConfig &cfg)
Reconfigures the trajectory planner.
TrajectoryPlanner(WorldModel &world_model, const costmap_2d::Costmap2D &costmap, std::vector< geometry_msgs::Point > footprint_spec, double acc_lim_x=1.0, double acc_lim_y=1.0, double acc_lim_theta=1.0, double sim_time=1.0, double sim_granularity=0.025, int vx_samples=20, int vtheta_samples=20, double pdist_scale=0.6, double gdist_scale=0.8, double occdist_scale=0.2, double heading_lookahead=0.325, double oscillation_reset_dist=0.05, double escape_reset_dist=0.10, double escape_reset_theta=M_PI_2, bool holonomic_robot=true, double max_vel_x=0.5, double min_vel_x=0.1, double max_vel_th=1.0, double min_vel_th=-1.0, double min_in_place_vel_th=0.4, double backup_vel=-0.1, bool dwa=false, bool heading_scoring=false, double heading_scoring_timestep=0.1, bool meter_scoring=true, bool simple_attractor=false, std::vector< double > y_vels=std::vector< double >(0), double stop_time_buffer=0.2, double sim_period=0.1, double angular_sim_granularity=0.025)
Constructs a trajectory controller.
void getMaxSpeedToStopInTime(double time, double &vx, double &vy, double &vth)
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
bool rotating_right
Booleans to keep track of the direction of rotation for the robot.
Holds a trajectory generated by considering an x, y, and theta velocity.
Definition: trajectory.h:44
geometry_msgs::Polygon getFootprintPolygon() const
Return the footprint specification of the robot.


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