47 #include <boost/algorithm/string.hpp> 61 void TrajectoryPlanner::reconfigure(BaseLocalPlannerConfig &cfg)
63 BaseLocalPlannerConfig config(cfg);
65 boost::mutex::scoped_lock l(configuration_mutex_);
67 acc_lim_x_ = config.acc_lim_x;
68 acc_lim_y_ = config.acc_lim_y;
69 acc_lim_theta_ = config.acc_lim_theta;
71 max_vel_x_ = config.max_vel_x;
72 min_vel_x_ = config.min_vel_x;
74 max_vel_th_ = config.max_vel_theta;
75 min_vel_th_ = config.min_vel_theta;
76 min_in_place_vel_th_ = config.min_in_place_vel_theta;
78 sim_time_ = config.sim_time;
79 sim_granularity_ = config.sim_granularity;
80 angular_sim_granularity_ = config.angular_sim_granularity;
82 path_distance_bias_ = config.path_distance_bias;
83 goal_distance_bias_ = config.goal_distance_bias;
84 occdist_scale_ = config.occdist_scale;
88 double resolution = costmap_.getResolution();
89 goal_distance_bias_ *= resolution;
90 path_distance_bias_ *= resolution;
93 oscillation_reset_dist_ = config.oscillation_reset_dist;
94 escape_reset_dist_ = config.escape_reset_dist;
95 escape_reset_theta_ = config.escape_reset_theta;
97 vx_samples_ = config.vx_samples;
98 vtheta_samples_ = config.vtheta_samples;
100 if (vx_samples_ <= 0) {
101 config.vx_samples = 1;
102 vx_samples_ = config.vx_samples;
103 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");
105 if(vtheta_samples_ <= 0) {
106 config.vtheta_samples = 1;
107 vtheta_samples_ = config.vtheta_samples;
108 ROS_WARN(
"You've specified that you don't want any samples in the theta dimension. We'll at least assume that you want to sample one value... so we're going to set vtheta_samples to 1 instead");
111 heading_lookahead_ = config.heading_lookahead;
113 holonomic_robot_ = config.holonomic_robot;
115 backup_vel_ = config.escape_vel;
119 heading_scoring_ = config.heading_scoring;
120 heading_scoring_timestep_ = config.heading_scoring_timestep;
122 simple_attractor_ = config.simple_attractor;
125 string y_string = config.y_vels;
126 vector<string> y_strs;
127 boost::split(y_strs, y_string, boost::is_any_of(
", "), boost::token_compress_on);
129 vector<double>y_vels;
130 for(vector<string>::iterator it=y_strs.begin(); it != y_strs.end(); ++it) {
131 istringstream iss(*it);
134 y_vels.push_back(temp);
142 TrajectoryPlanner::TrajectoryPlanner(
WorldModel& world_model,
144 std::vector<geometry_msgs::Point> footprint_spec,
145 double acc_lim_x,
double acc_lim_y,
double acc_lim_theta,
146 double sim_time,
double sim_granularity,
147 int vx_samples,
int vtheta_samples,
148 double path_distance_bias,
double goal_distance_bias,
double occdist_scale,
149 double heading_lookahead,
double oscillation_reset_dist,
150 double escape_reset_dist,
double escape_reset_theta,
151 bool holonomic_robot,
152 double max_vel_x,
double min_vel_x,
153 double max_vel_th,
double min_vel_th,
double min_in_place_vel_th,
155 bool dwa,
bool heading_scoring,
double heading_scoring_timestep,
bool meter_scoring,
bool simple_attractor,
156 vector<double> y_vels,
double stop_time_buffer,
double sim_period,
double angular_sim_granularity)
157 : path_map_(costmap.getSizeInCellsX(), costmap.getSizeInCellsY()),
158 goal_map_(costmap.getSizeInCellsX(), costmap.getSizeInCellsY()),
160 world_model_(world_model), footprint_spec_(footprint_spec),
161 sim_time_(sim_time), sim_granularity_(sim_granularity), angular_sim_granularity_(angular_sim_granularity),
162 vx_samples_(vx_samples), vtheta_samples_(vtheta_samples),
163 path_distance_bias_(path_distance_bias), goal_distance_bias_(goal_distance_bias), occdist_scale_(occdist_scale),
164 acc_lim_x_(acc_lim_x), acc_lim_y_(acc_lim_y), acc_lim_theta_(acc_lim_theta),
165 prev_x_(0), prev_y_(0), escape_x_(0), escape_y_(0), escape_theta_(0), heading_lookahead_(heading_lookahead),
166 oscillation_reset_dist_(oscillation_reset_dist), escape_reset_dist_(escape_reset_dist),
167 escape_reset_theta_(escape_reset_theta), holonomic_robot_(holonomic_robot),
168 max_vel_x_(max_vel_x), min_vel_x_(min_vel_x),
169 max_vel_th_(max_vel_th), min_vel_th_(min_vel_th), min_in_place_vel_th_(min_in_place_vel_th),
170 backup_vel_(backup_vel),
171 dwa_(dwa), heading_scoring_(heading_scoring), heading_scoring_timestep_(heading_scoring_timestep),
172 simple_attractor_(simple_attractor), y_vels_(y_vels), stop_time_buffer_(stop_time_buffer), sim_period_(sim_period)
215 double x,
double y,
double theta,
216 double vx,
double vy,
double vtheta,
217 double vx_samp,
double vy_samp,
double vtheta_samp,
218 double acc_x,
double acc_y,
double acc_theta,
219 double impossible_cost,
227 double theta_i = theta;
229 double vx_i, vy_i, vtheta_i;
236 double vmag =
hypot(vx_samp, vy_samp);
262 double path_dist = 0.0;
263 double goal_dist = 0.0;
264 double occ_cost = 0.0;
265 double heading_diff = 0.0;
267 for(
int i = 0; i < num_steps; ++i){
269 unsigned int cell_x, cell_y;
281 if(footprint_cost < 0){
307 occ_cost = std::max(std::max(occ_cost, footprint_cost),
double(
costmap_.
getCost(cell_x, cell_y)));
317 bool update_path_and_goal_distances =
true;
323 heading_diff =
headingDiff(cell_x, cell_y, x_i, y_i, theta_i);
325 update_path_and_goal_distances =
false;
329 if (update_path_and_goal_distances) {
331 path_dist =
path_map_(cell_x, cell_y).target_dist;
332 goal_dist =
goal_map_(cell_x, cell_y).target_dist;
335 if(impossible_cost <= goal_dist || impossible_cost <= path_dist){
373 unsigned int goal_cell_x, goal_cell_y;
378 if (
lineCost(cell_x, goal_cell_x, cell_y, goal_cell_y) >= 0) {
392 int deltax =
abs(x1 - x0);
393 int deltay =
abs(y1 - y0);
397 int xinc1, xinc2, yinc1, yinc2;
398 int den, num, numadd, numpixels;
400 double line_cost = 0.0;
401 double point_cost = -1.0;
425 if (deltax >= deltay)
442 for (
int curpixel = 0; curpixel <= numpixels; curpixel++) {
445 if (point_cost < 0) {
449 if (line_cost < point_cost) {
450 line_cost = point_cost;
469 if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE || cost == NO_INFORMATION){
478 for(
unsigned int i = 0; i < new_plan.size(); ++i){
499 ROS_DEBUG(
"Path/Goal distance computed");
504 double vtheta,
double vx_samp,
double vy_samp,
double vtheta_samp){
507 double cost =
scoreTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp);
513 ROS_WARN(
"Invalid Trajectory %f, %f, %f, cost: %f", vx_samp, vy_samp, vtheta_samp, cost);
520 double vtheta,
double vx_samp,
double vy_samp,
double vtheta_samp) {
525 vx_samp, vy_samp, vtheta_samp,
530 return double( t.
cost_ );
537 double vx,
double vy,
double vtheta,
538 double acc_x,
double acc_y,
double acc_theta) {
541 double min_vel_x, min_vel_theta;
545 max_vel_x =
min( max_vel_x, final_goal_dist /
sim_time_ );
553 max_vel_theta =
min(
max_vel_th_, vtheta + acc_theta * sim_period_);
554 min_vel_theta =
max(
min_vel_th_, vtheta - acc_theta * sim_period_);
565 double dvx = (max_vel_x - min_vel_x) / (
vx_samples_ - 1);
566 double dvtheta = (max_vel_theta - min_vel_theta) / (
vtheta_samples_ - 1);
568 double vx_samp = min_vel_x;
569 double vtheta_samp = min_vel_theta;
570 double vy_samp = 0.0;
574 best_traj->
cost_ = -1.0;
577 comp_traj->
cost_ = -1.0;
591 acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
596 best_traj = comp_traj;
600 vtheta_samp = min_vel_theta;
604 acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
609 best_traj = comp_traj;
612 vtheta_samp += dvtheta;
624 acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
629 best_traj = comp_traj;
637 acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
642 best_traj = comp_traj;
649 vtheta_samp = min_vel_theta;
661 generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp_limited,
662 acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
666 if(comp_traj->
cost_ >= 0
667 && (comp_traj->
cost_ <= best_traj->
cost_ || best_traj->
cost_ < 0 || best_traj->
yv_ != 0.0)
668 && (vtheta_samp > dvtheta || vtheta_samp < -1 * dvtheta)){
669 double x_r, y_r, th_r;
673 unsigned int cell_x, cell_y;
677 double ahead_gdist =
goal_map_(cell_x, cell_y).target_dist;
678 if (ahead_gdist < heading_dist) {
682 best_traj = comp_traj;
684 heading_dist = ahead_gdist;
689 best_traj = comp_traj;
691 heading_dist = ahead_gdist;
697 vtheta_samp += dvtheta;
701 if (best_traj->
cost_ >= 0) {
703 if ( ! (best_traj->
xv_ > 0)) {
709 }
else if (best_traj->
thetav_ > 0) {
714 }
else if(best_traj->
yv_ > 0) {
719 }
else if(best_traj->
yv_ < 0){
755 vtheta_samp = min_vel_theta;
759 for(
unsigned int i = 0; i <
y_vels_.size(); ++i){
764 acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
768 double x_r, y_r, th_r;
772 unsigned int cell_x, cell_y;
776 double ahead_gdist =
goal_map_(cell_x, cell_y).target_dist;
777 if (ahead_gdist < heading_dist) {
781 best_traj = comp_traj;
783 heading_dist = ahead_gdist;
788 best_traj = comp_traj;
790 heading_dist = ahead_gdist;
799 if (best_traj->
cost_ >= 0) {
800 if (!(best_traj->
xv_ > 0)) {
806 }
else if(best_traj->
thetav_ > 0) {
811 }
else if(best_traj->
yv_ > 0) {
816 }
else if(best_traj->
yv_ < 0) {
854 acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
867 best_traj = comp_traj;
899 if(best_traj->
cost_ == -1.0)
900 best_traj->
cost_ = 1.0;
908 geometry_msgs::PoseStamped& global_vel, geometry_msgs::PoseStamped& drive_velocities) {
910 Eigen::Vector3f pos(global_pose.pose.position.x, global_pose.pose.position.y,
tf2::getYaw(global_pose.pose.orientation));
911 Eigen::Vector3f vel(global_vel.pose.position.x, global_vel.pose.position.y,
tf2::getYaw(global_vel.pose.orientation));
918 std::vector<base_local_planner::Position2DInt> footprint_list =
926 for (
unsigned int i = 0; i < footprint_list.size(); ++i) {
927 path_map_(footprint_list[i].x, footprint_list[i].y).within_robot =
true;
933 ROS_DEBUG(
"Path/Goal distance computed");
937 vel[0], vel[1], vel[2],
967 drive_velocities.pose.position.x = 0;
968 drive_velocities.pose.position.y = 0;
969 drive_velocities.pose.position.z = 0;
970 drive_velocities.pose.orientation.w = 1;
971 drive_velocities.pose.orientation.x = 0;
972 drive_velocities.pose.orientation.y = 0;
973 drive_velocities.pose.orientation.z = 0;
976 drive_velocities.pose.position.x = best.
xv_;
977 drive_velocities.pose.position.y = best.
yv_;
978 drive_velocities.pose.position.z = 0;
double unreachableCellCosts()
int vx_samples_
The number of samples we'll take in the x dimenstion of the control space.
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
double angular_sim_granularity_
The distance between angular simulation points.
virtual double footprintCost(const geometry_msgs::Point &position, const std::vector< geometry_msgs::Point > &footprint, double inscribed_radius, double circumscribed_radius)=0
Subclass will implement this method to check a footprint at a given position and orientation for lega...
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'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.
void setTargetCells(const costmap_2d::Costmap2D &costmap, const std::vector< geometry_msgs::PoseStamped > &global_plan)
Update what cells are considered path based on the global plan.
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.
bool within_robot
Mark for cells within the robot footprint.
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...
double goal_y_
The goal distance was last computed from.
~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.
INLINE Rall1d< T, V, S > hypot(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
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 cost_
The cost/score of the trajectory.
double backup_vel_
The velocity to use while backing up.
geometry_msgs::TransformStamped t
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.
Trajectory findBestPath(const geometry_msgs::PoseStamped &global_pose, geometry_msgs::PoseStamped &global_vel, geometry_msgs::PoseStamped &drive_velocities)
Given the current position, orientation, and velocity of the robot, return a trajectory to follow...
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.
void resetPoints()
Clear the trajectory's points.
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.
static const unsigned char INSCRIBED_INFLATED_OBSTACLE
boost::mutex configuration_mutex_
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's cost function.
MapGrid goal_map_
The local map grid where we propagate goal distance.
bool escaping_
Boolean to keep track of whether we're in escape mode.
double headingDiff(int cell_x, int cell_y, double x, double y, double heading)
double thetav_
The x, y, and theta velocities of the trajectory.
double sim_granularity_
The distance between simulation points.
double acc_lim_theta_
The acceleration limits of the robot.
double min(double a, double b)
double circumscribed_radius_
double pointCost(int x, int y)
double path_distance_bias_
double escape_reset_dist_
void addPoint(double x, double y, double th)
Add a point to the end of a trajectory.
Trajectory traj_two
Used for scoring trajectories.
double getYaw(const A &a)
void updatePlan(const std::vector< geometry_msgs::PoseStamped > &new_plan, bool compute_dists=false)
Update the plan that the controller is following.
double target_dist
Distance to planner's path.
An interface the trajectory controller uses to interact with the world regardless of the underlying w...
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.
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
bool heading_scoring_
Should we score based on the rollout approach or the heading approach.
bool worldToMap(double wx, double wy, unsigned int &mx, unsigned int &my) const
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
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.
Stores path distance and goal distance information used for scoring trajectories. ...
void convert(const A &a, B &b)
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.
void getEndpoint(double &x, double &y, double &th) const
Get the last point of the trajectory.
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 setLocalGoal(const costmap_2d::Costmap2D &costmap, const std::vector< geometry_msgs::PoseStamped > &global_plan)
Update what cell is considered the next local goal.
double goal_distance_bias_
void swap(scoped_ptr< T > &, scoped_ptr< T > &)
double max(double a, double b)
void resetPathDist()
reset path distance fields for all cells
void mapToWorld(unsigned int mx, unsigned int my, double &wx, double &wy) const
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.
def shortest_angular_distance(from_angle, to_angle)
unsigned char getCost(unsigned int mx, unsigned int my) const
Holds a trajectory generated by considering an x, y, and theta velocity.
void calculateMinAndMaxDistances(const std::vector< geometry_msgs::Point > &footprint, double &min_dist, double &max_dist)