40 #ifdef HAVE_SYS_TIME_H 44 #include <boost/tokenizer.hpp> 54 #include <nav_msgs/Path.h> 65 if (
setup_ && config.restore_defaults) {
68 config.restore_defaults =
false;
103 double sim_time, sim_granularity, angular_sim_granularity;
104 int vx_samples, vtheta_samples;
105 double path_distance_bias, goal_distance_bias, occdist_scale, heading_lookahead, oscillation_reset_dist, escape_reset_dist, escape_reset_theta;
106 bool holonomic_robot, dwa, simple_attractor, heading_scoring;
107 double heading_scoring_timestep;
108 double max_vel_x, min_vel_x;
110 double stop_time_buffer;
111 std::string world_model_type;
128 private_nh.
param(
"stop_time_buffer", stop_time_buffer, 0.2);
134 if(private_nh.
hasParam(
"acc_limit_x"))
135 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.");
137 if(private_nh.
hasParam(
"acc_limit_y"))
138 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.");
140 if(private_nh.
hasParam(
"acc_limit_th"))
141 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.");
146 std::string controller_frequency_param_name;
147 if(!private_nh.
searchParam(
"controller_frequency", controller_frequency_param_name))
151 double controller_frequency = 0;
152 private_nh.
param(controller_frequency_param_name, controller_frequency, 20.0);
153 if(controller_frequency > 0)
157 ROS_WARN(
"A controller_frequency less than 0 has been set. Ignoring the parameter, assuming a rate of 20Hz");
163 private_nh.
param(
"sim_time", sim_time, 1.0);
164 private_nh.
param(
"sim_granularity", sim_granularity, 0.025);
165 private_nh.
param(
"angular_sim_granularity", angular_sim_granularity, sim_granularity);
166 private_nh.
param(
"vx_samples", vx_samples, 3);
167 private_nh.
param(
"vtheta_samples", vtheta_samples, 20);
170 "path_distance_bias",
174 "goal_distance_bias",
179 if (private_nh.
hasParam(
"pdist_scale") & !private_nh.
hasParam(
"path_distance_bias"))
181 private_nh.
setParam(
"path_distance_bias", path_distance_bias);
183 if (private_nh.
hasParam(
"gdist_scale") & !private_nh.
hasParam(
"goal_distance_bias"))
185 private_nh.
setParam(
"goal_distance_bias", goal_distance_bias);
188 private_nh.
param(
"occdist_scale", occdist_scale, 0.01);
191 if ( ! private_nh.
hasParam(
"meter_scoring")) {
192 ROS_WARN(
"Trajectory Rollout planner initialized with param meter_scoring not set. Set it to true to make your settings robust against changes of costmap resolution.");
194 private_nh.
param(
"meter_scoring", meter_scoring,
false);
199 goal_distance_bias *= resolution;
200 path_distance_bias *= resolution;
202 ROS_WARN(
"Trajectory Rollout planner initialized with param meter_scoring set to false. Set it to true to make your settings robust against changes of costmap resolution.");
206 private_nh.
param(
"heading_lookahead", heading_lookahead, 0.325);
207 private_nh.
param(
"oscillation_reset_dist", oscillation_reset_dist, 0.05);
208 private_nh.
param(
"escape_reset_dist", escape_reset_dist, 0.10);
209 private_nh.
param(
"escape_reset_theta", escape_reset_theta, M_PI_4);
210 private_nh.
param(
"holonomic_robot", holonomic_robot,
true);
211 private_nh.
param(
"max_vel_x", max_vel_x, 0.5);
212 private_nh.
param(
"min_vel_x", min_vel_x, 0.1);
214 double max_rotational_vel;
215 private_nh.
param(
"max_rotational_vel", max_rotational_vel, 1.0);
220 "min_in_place_vel_theta",
221 "min_in_place_rotational_vel", 0.4);
224 if(private_nh.
getParam(
"backup_vel", backup_vel))
225 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.");
228 private_nh.
getParam(
"escape_vel", backup_vel);
230 if(backup_vel >= 0.0)
231 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");
233 private_nh.
param(
"world_model", world_model_type, std::string(
"costmap"));
234 private_nh.
param(
"dwa", dwa,
true);
235 private_nh.
param(
"heading_scoring", heading_scoring,
false);
236 private_nh.
param(
"heading_scoring_timestep", heading_scoring_timestep, 0.8);
238 simple_attractor =
false;
241 double min_pt_separation, max_obstacle_height, grid_resolution;
243 private_nh.
param(
"point_grid/min_pt_separation", min_pt_separation, 0.01);
244 private_nh.
param(
"point_grid/max_obstacle_height", max_obstacle_height, 2.0);
245 private_nh.
param(
"point_grid/grid_resolution", grid_resolution, 0.2);
247 ROS_ASSERT_MSG(world_model_type ==
"costmap",
"At this time, only costmap world models are supported by this controller");
249 std::vector<double> y_vels =
loadYVels(private_nh);
255 goal_distance_bias, occdist_scale, heading_lookahead, oscillation_reset_dist, escape_reset_dist, escape_reset_theta, holonomic_robot,
257 dwa, heading_scoring, heading_scoring_timestep, meter_scoring, simple_attractor, y_vels, stop_time_buffer,
sim_period_, angular_sim_granularity);
262 dsrv_ =
new dynamic_reconfigure::Server<BaseLocalPlannerConfig>(private_nh);
264 dsrv_->setCallback(cb);
267 ROS_WARN(
"This planner has already been initialized, doing nothing");
272 std::vector<double> y_vels;
274 std::string y_vel_list;
275 if(node.
getParam(
"y_vels", y_vel_list)){
276 typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
277 boost::char_separator<char> sep(
"[], ");
278 tokenizer tokens(y_vel_list, sep);
280 for(tokenizer::iterator i = tokens.begin(); i != tokens.end(); i++){
281 y_vels.push_back(atof((*i).c_str()));
286 y_vels.push_back(-0.3);
287 y_vels.push_back(-0.1);
288 y_vels.push_back(0.1);
289 y_vels.push_back(0.3);
309 double vx =
sign(robot_vel.pose.position.x) * std::max(0.0, (fabs(robot_vel.pose.position.x) -
acc_lim_x_ *
sim_period_));
310 double vy =
sign(robot_vel.pose.position.y) * std::max(0.0, (fabs(robot_vel.pose.position.y) -
acc_lim_y_ * sim_period_));
312 double vel_yaw =
tf2::getYaw(robot_vel.pose.orientation);
313 double vth =
sign(vel_yaw) * std::max(0.0, (fabs(vel_yaw) -
acc_lim_theta_ * sim_period_));
316 double yaw =
tf2::getYaw(global_pose.pose.orientation);
317 bool valid_cmd =
tc_->
checkTrajectory(global_pose.pose.position.x, global_pose.pose.position.y, yaw,
318 robot_vel.pose.position.x, robot_vel.pose.position.y, vel_yaw, vx, vy, vth);
322 ROS_DEBUG(
"Slowing down... using vx, vy, vth: %.2f, %.2f, %.2f", vx, vy, vth);
323 cmd_vel.linear.x = vx;
324 cmd_vel.linear.y = vy;
325 cmd_vel.angular.z = vth;
329 cmd_vel.linear.x = 0.0;
330 cmd_vel.linear.y = 0.0;
331 cmd_vel.angular.z = 0.0;
336 double yaw =
tf2::getYaw(global_pose.pose.orientation);
337 double vel_yaw =
tf2::getYaw(robot_vel.pose.orientation);
338 cmd_vel.linear.x = 0;
339 cmd_vel.linear.y = 0;
342 double v_theta_samp = ang_diff > 0.0 ? std::min(
max_vel_th_,
350 v_theta_samp =
sign(v_theta_samp) * std::min(std::max(fabs(v_theta_samp), min_acc_vel), max_acc_vel);
355 v_theta_samp =
sign(v_theta_samp) * std::min(max_speed_to_stop, fabs(v_theta_samp));
358 v_theta_samp = v_theta_samp > 0.0
363 bool valid_cmd =
tc_->
checkTrajectory(global_pose.pose.position.x, global_pose.pose.position.y, yaw,
364 robot_vel.pose.position.x, robot_vel.pose.position.y, vel_yaw, 0.0, 0.0, v_theta_samp);
366 ROS_DEBUG(
"Moving to desired goal orientation, th cmd: %.2f, valid_cmd: %d", v_theta_samp, valid_cmd);
369 cmd_vel.angular.z = v_theta_samp;
373 cmd_vel.angular.z = 0.0;
380 ROS_ERROR(
"This planner has not been initialized, please call initialize() before using this planner");
397 ROS_ERROR(
"This planner has not been initialized, please call initialize() before using this planner");
401 std::vector<geometry_msgs::PoseStamped> local_plan;
402 geometry_msgs::PoseStamped global_pose;
407 std::vector<geometry_msgs::PoseStamped> transformed_plan;
410 ROS_WARN(
"Could not transform the global plan to the frame of the controller");
418 geometry_msgs::PoseStamped drive_cmds;
421 geometry_msgs::PoseStamped robot_vel;
431 if(transformed_plan.empty())
434 const geometry_msgs::PoseStamped& goal_point = transformed_plan.back();
436 const double goal_x = goal_point.pose.position.x;
437 const double goal_y = goal_point.pose.position.y;
439 const double yaw =
tf2::getYaw(goal_point.pose.orientation);
441 double goal_th = yaw;
456 cmd_vel.linear.x = 0.0;
457 cmd_vel.linear.y = 0.0;
458 cmd_vel.angular.z = 0.0;
470 nav_msgs::Odometry base_odom;
483 if(!
rotateToGoal(global_pose, robot_vel, goal_th, cmd_vel)) {
512 cmd_vel.linear.x = drive_cmds.pose.position.x;
513 cmd_vel.linear.y = drive_cmds.pose.position.y;
514 cmd_vel.angular.z =
tf2::getYaw(drive_cmds.pose.orientation);
517 if (path.
cost_ < 0) {
519 "The rollout planner failed to find a valid plan. This means that the footprint of the robot was in collision for all simulated trajectories.");
526 ROS_DEBUG_NAMED(
"trajectory_planner_ros",
"A valid velocity command of (%.2f, %.2f, %.2f) was found for this cycle.",
527 cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z);
531 double p_x, p_y, p_th;
533 geometry_msgs::PoseStamped pose;
536 pose.pose.position.x = p_x;
537 pose.pose.position.y = p_y;
538 pose.pose.position.z = 0.0;
542 local_plan.push_back(pose);
552 geometry_msgs::PoseStamped global_pose;
557 std::vector<geometry_msgs::PoseStamped> plan;
558 plan.push_back(global_pose);
563 nav_msgs::Odometry base_odom;
565 boost::recursive_mutex::scoped_lock lock(
odom_lock_);
570 base_odom.twist.twist.linear.x,
571 base_odom.twist.twist.linear.y,
572 base_odom.twist.twist.angular.z, vx_samp, vy_samp, vtheta_samp);
575 ROS_WARN(
"Failed to get the pose of the robot. No trajectories will pass as legal in this case.");
582 geometry_msgs::PoseStamped global_pose;
587 std::vector<geometry_msgs::PoseStamped> plan;
588 plan.push_back(global_pose);
593 nav_msgs::Odometry base_odom;
595 boost::recursive_mutex::scoped_lock lock(
odom_lock_);
600 base_odom.twist.twist.linear.x,
601 base_odom.twist.twist.linear.y,
602 base_odom.twist.twist.angular.z, vx_samp, vy_samp, vtheta_samp);
605 ROS_WARN(
"Failed to get the pose of the robot. No trajectories will pass as legal in this case.");
611 ROS_ERROR(
"This planner has not been initialized, please call initialize() before using this planner");
Computes control velocities for a robot given a costmap, a plan, and the robot's position in the worl...
bool transformGlobalPlan(const tf2_ros::Buffer &tf, const std::vector< geometry_msgs::PoseStamped > &global_plan, const geometry_msgs::PoseStamped &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...
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
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.
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_
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...
ros::Publisher g_plan_pub_
std::string getGlobalFrameID()
void getOdom(nav_msgs::Odometry &base_odom)
boost::recursive_mutex odom_lock_
unsigned int getPointsSize() const
Return the number of points in the trajectory.
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.
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.
costmap_2d::Costmap2D * costmap_
The costmap the controller will use.
double getGoalPositionDistance(const geometry_msgs::PoseStamped &global_pose, double goal_x, double goal_y)
return squared distance to check if the goal position has been achieved
double rot_stopped_velocity_
bool latch_xy_goal_tolerance_
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.
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...
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
double yaw_goal_tolerance_
std::string global_frame_
The frame in which the controller will run.
base_local_planner::BaseLocalPlannerConfig default_config_
#define ROS_DEBUG_NAMED(name,...)
double getGoalOrientationAngleDifference(const geometry_msgs::PoseStamped &global_pose, double goal_th)
return angle difference to goal to check if the goal orientation has been achieved ...
bool stopWithAccLimits(const geometry_msgs::PoseStamped &global_pose, const geometry_msgs::PoseStamped &robot_vel, geometry_msgs::Twist &cmd_vel)
Stop the robot taking into account acceleration limits.
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
double xy_goal_tolerance_
bool getParam(const std::string &key, std::string &s) 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()
bool getRobotPose(geometry_msgs::PoseStamped &global_pose) const
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)
bool hasParam(const std::string &key) const
double getYaw(const A &a)
double trans_stopped_velocity_
bool searchParam(const std::string &key, std::string &result) const
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 checkTrajectory(double vx_samp, double vy_samp, double vtheta_samp, bool update_map=true)
Generate and score a single trajectory.
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const
base_local_planner::OdometryHelperRos odom_helper_
void reconfigureCB(BaseLocalPlannerConfig &config, uint32_t level)
Callback to update the local planner's parameters based on dynamic reconfigure.
WorldModel * world_model_
The world model that the controller will use.
void initialize(std::string name, tf2_ros::Buffer *tf, costmap_2d::Costmap2DROS *costmap_ros)
Constructs the ros wrapper.
void convert(const A &a, B &b)
double getResolution() const
std::vector< geometry_msgs::PoseStamped > global_plan_
TrajectoryPlannerROS()
Default constructor for the ros wrapper.
A class that implements the WorldModel interface to provide grid based collision checks for the traje...
void getRobotVel(geometry_msgs::PoseStamped &robot_vel)
~TrajectoryPlannerROS()
Destructor for the wrapper.
std::vector< geometry_msgs::Point > getRobotFootprint()
void reconfigure(BaseLocalPlannerConfig &cfg)
Reconfigures the trajectory planner.
param_t loadParameterWithDeprecation(const ros::NodeHandle &nh, const std::string current_name, const std::string old_name, const param_t &default_value)
A ROS wrapper for the trajectory controller that queries the param server to construct a controller...
tf2_ros::Buffer * tf_
Used for transforming point clouds.
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
costmap_2d::Costmap2DROS * costmap_ros_
The ROS wrapper for the costmap the controller will use.
ros::Publisher l_plan_pub_
void prunePlan(const geometry_msgs::PoseStamped &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 rotateToGoal(const geometry_msgs::PoseStamped &global_pose, const geometry_msgs::PoseStamped &robot_vel, double goal_th, geometry_msgs::Twist &cmd_vel)
Once a goal position is reached... rotate to the goal orientation.
def shortest_angular_distance(from_angle, to_angle)
Holds a trajectory generated by considering an x, y, and theta velocity.
double min_in_place_vel_th_