00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 #include <teb_local_planner/teb_config.h>
00040
00041 namespace teb_local_planner
00042 {
00043
00044 void TebConfig::loadRosParamFromNodeHandle(const ros::NodeHandle& nh)
00045 {
00046
00047 nh.param("odom_topic", odom_topic, odom_topic);
00048 nh.param("map_frame", map_frame, map_frame);
00049
00050
00051 nh.param("teb_autosize", trajectory.teb_autosize, trajectory.teb_autosize);
00052 nh.param("dt_ref", trajectory.dt_ref, trajectory.dt_ref);
00053 nh.param("dt_hysteresis", trajectory.dt_hysteresis, trajectory.dt_hysteresis);
00054 nh.param("min_samples", trajectory.min_samples, trajectory.min_samples);
00055 nh.param("global_plan_overwrite_orientation", trajectory.global_plan_overwrite_orientation, trajectory.global_plan_overwrite_orientation);
00056 nh.param("global_plan_via_point_sep", trajectory.global_plan_viapoint_sep, trajectory.global_plan_viapoint_sep);
00057 nh.param("via_points_ordered", trajectory.via_points_ordered, trajectory.via_points_ordered);
00058 nh.param("max_global_plan_lookahead_dist", trajectory.max_global_plan_lookahead_dist, trajectory.max_global_plan_lookahead_dist);
00059 nh.param("force_reinit_new_goal_dist", trajectory.force_reinit_new_goal_dist, trajectory.force_reinit_new_goal_dist);
00060 nh.param("feasibility_check_no_poses", trajectory.feasibility_check_no_poses, trajectory.feasibility_check_no_poses);
00061 nh.param("publish_feedback", trajectory.publish_feedback, trajectory.publish_feedback);
00062 nh.param("shrink_horizon_backup", trajectory.shrink_horizon_backup, trajectory.shrink_horizon_backup);
00063
00064
00065 nh.param("max_vel_x", robot.max_vel_x, robot.max_vel_x);
00066 nh.param("max_vel_x_backwards", robot.max_vel_x_backwards, robot.max_vel_x_backwards);
00067 nh.param("max_vel_theta", robot.max_vel_theta, robot.max_vel_theta);
00068 nh.param("acc_lim_x", robot.acc_lim_x, robot.acc_lim_x);
00069 nh.param("acc_lim_theta", robot.acc_lim_theta, robot.acc_lim_theta);
00070 nh.param("min_turning_radius", robot.min_turning_radius, robot.min_turning_radius);
00071 nh.param("wheelbase", robot.wheelbase, robot.wheelbase);
00072 nh.param("cmd_angle_instead_rotvel", robot.cmd_angle_instead_rotvel, robot.cmd_angle_instead_rotvel);
00073
00074
00075 nh.param("xy_goal_tolerance", goal_tolerance.xy_goal_tolerance, goal_tolerance.xy_goal_tolerance);
00076 nh.param("yaw_goal_tolerance", goal_tolerance.yaw_goal_tolerance, goal_tolerance.yaw_goal_tolerance);
00077 nh.param("free_goal_vel", goal_tolerance.free_goal_vel, goal_tolerance.free_goal_vel);
00078
00079
00080 nh.param("min_obstacle_dist", obstacles.min_obstacle_dist, obstacles.min_obstacle_dist);
00081 nh.param("inflation_dist", obstacles.inflation_dist, obstacles.inflation_dist);
00082 nh.param("include_costmap_obstacles", obstacles.include_costmap_obstacles, obstacles.include_costmap_obstacles);
00083 nh.param("costmap_obstacles_behind_robot_dist", obstacles.costmap_obstacles_behind_robot_dist, obstacles.costmap_obstacles_behind_robot_dist);
00084 nh.param("obstacle_poses_affected", obstacles.obstacle_poses_affected, obstacles.obstacle_poses_affected);
00085 nh.param("costmap_converter_plugin", obstacles.costmap_converter_plugin, obstacles.costmap_converter_plugin);
00086 nh.param("costmap_converter_spin_thread", obstacles.costmap_converter_spin_thread, obstacles.costmap_converter_spin_thread);
00087
00088
00089 nh.param("no_inner_iterations", optim.no_inner_iterations, optim.no_inner_iterations);
00090 nh.param("no_outer_iterations", optim.no_outer_iterations, optim.no_outer_iterations);
00091 nh.param("optimization_activate", optim.optimization_activate, optim.optimization_activate);
00092 nh.param("optimization_verbose", optim.optimization_verbose, optim.optimization_verbose);
00093 nh.param("penalty_epsilon", optim.penalty_epsilon, optim.penalty_epsilon);
00094 nh.param("weight_max_vel_x", optim.weight_max_vel_x, optim.weight_max_vel_x);
00095 nh.param("weight_max_vel_theta", optim.weight_max_vel_theta, optim.weight_max_vel_theta);
00096 nh.param("weight_acc_lim_x", optim.weight_acc_lim_x, optim.weight_acc_lim_x);
00097 nh.param("weight_acc_lim_theta", optim.weight_acc_lim_theta, optim.weight_acc_lim_theta);
00098 nh.param("weight_kinematics_nh", optim.weight_kinematics_nh, optim.weight_kinematics_nh);
00099 nh.param("weight_kinematics_forward_drive", optim.weight_kinematics_forward_drive, optim.weight_kinematics_forward_drive);
00100 nh.param("weight_kinematics_turning_radius", optim.weight_kinematics_turning_radius, optim.weight_kinematics_turning_radius);
00101 nh.param("weight_optimaltime", optim.weight_optimaltime, optim.weight_optimaltime);
00102 nh.param("weight_obstacle", optim.weight_obstacle, optim.weight_obstacle);
00103 nh.param("weight_inflation", optim.weight_inflation, optim.weight_inflation);
00104 nh.param("weight_dynamic_obstacle", optim.weight_dynamic_obstacle, optim.weight_dynamic_obstacle);
00105 nh.param("weight_viapoint", optim.weight_viapoint, optim.weight_viapoint);
00106
00107
00108 nh.param("enable_homotopy_class_planning", hcp.enable_homotopy_class_planning, hcp.enable_homotopy_class_planning);
00109 nh.param("enable_multithreading", hcp.enable_multithreading, hcp.enable_multithreading);
00110 nh.param("simple_exploration", hcp.simple_exploration, hcp.simple_exploration);
00111 nh.param("max_number_classes", hcp.max_number_classes, hcp.max_number_classes);
00112 nh.param("selection_obst_cost_scale", hcp.selection_obst_cost_scale, hcp.selection_obst_cost_scale);
00113 nh.param("selection_viapoint_cost_scale", hcp.selection_viapoint_cost_scale, hcp.selection_viapoint_cost_scale);
00114 nh.param("selection_cost_hysteresis", hcp.selection_cost_hysteresis, hcp.selection_cost_hysteresis);
00115 nh.param("selection_alternative_time_cost", hcp.selection_alternative_time_cost, hcp.selection_alternative_time_cost);
00116 nh.param("roadmap_graph_samples", hcp.roadmap_graph_no_samples, hcp.roadmap_graph_no_samples);
00117 nh.param("roadmap_graph_area_width", hcp.roadmap_graph_area_width, hcp.roadmap_graph_area_width);
00118 nh.param("h_signature_prescaler", hcp.h_signature_prescaler, hcp.h_signature_prescaler);
00119 nh.param("h_signature_threshold", hcp.h_signature_threshold, hcp.h_signature_threshold);
00120 nh.param("obstacle_keypoint_offset", hcp.obstacle_keypoint_offset, hcp.obstacle_keypoint_offset);
00121 nh.param("obstacle_heading_threshold", hcp.obstacle_heading_threshold, hcp.obstacle_heading_threshold);
00122 nh.param("viapoints_all_candidates", hcp.viapoints_all_candidates, hcp.viapoints_all_candidates);
00123 nh.param("visualize_hc_graph", hcp.visualize_hc_graph, hcp.visualize_hc_graph);
00124
00125 checkParameters();
00126 checkDeprecated(nh);
00127 }
00128
00129 void TebConfig::reconfigure(TebLocalPlannerReconfigureConfig& cfg)
00130 {
00131 boost::mutex::scoped_lock l(config_mutex_);
00132
00133
00134 trajectory.teb_autosize = cfg.teb_autosize;
00135 trajectory.dt_ref = cfg.dt_ref;
00136 trajectory.dt_hysteresis = cfg.dt_hysteresis;
00137 trajectory.global_plan_overwrite_orientation = cfg.global_plan_overwrite_orientation;
00138 trajectory.global_plan_viapoint_sep = cfg.global_plan_viapoint_sep;
00139 trajectory.via_points_ordered = cfg.via_points_ordered;
00140 trajectory.max_global_plan_lookahead_dist = cfg.max_global_plan_lookahead_dist;
00141 trajectory.force_reinit_new_goal_dist = cfg.force_reinit_new_goal_dist;
00142 trajectory.feasibility_check_no_poses = cfg.feasibility_check_no_poses;
00143 trajectory.publish_feedback = cfg.publish_feedback;
00144 trajectory.shrink_horizon_backup = cfg.shrink_horizon_backup;
00145
00146
00147 robot.max_vel_x = cfg.max_vel_x;
00148 robot.max_vel_x_backwards = cfg.max_vel_x_backwards;
00149 robot.max_vel_theta = cfg.max_vel_theta;
00150 robot.acc_lim_x = cfg.acc_lim_x;
00151 robot.acc_lim_theta = cfg.acc_lim_theta;
00152 robot.min_turning_radius = cfg.min_turning_radius;
00153 robot.wheelbase = cfg.wheelbase;
00154 robot.cmd_angle_instead_rotvel = cfg.cmd_angle_instead_rotvel;
00155
00156
00157 goal_tolerance.xy_goal_tolerance = cfg.xy_goal_tolerance;
00158 goal_tolerance.yaw_goal_tolerance = cfg.yaw_goal_tolerance;
00159 goal_tolerance.free_goal_vel = cfg.free_goal_vel;
00160
00161
00162 obstacles.min_obstacle_dist = cfg.min_obstacle_dist;
00163 obstacles.inflation_dist = cfg.inflation_dist;
00164 obstacles.include_costmap_obstacles = cfg.include_costmap_obstacles;
00165 obstacles.costmap_obstacles_behind_robot_dist = cfg.costmap_obstacles_behind_robot_dist;
00166 obstacles.obstacle_poses_affected = cfg.obstacle_poses_affected;
00167
00168
00169
00170 optim.no_inner_iterations = cfg.no_inner_iterations;
00171 optim.no_outer_iterations = cfg.no_outer_iterations;
00172 optim.optimization_activate = cfg.optimization_activate;
00173 optim.optimization_verbose = cfg.optimization_verbose;
00174 optim.penalty_epsilon = cfg.penalty_epsilon;
00175 optim.weight_max_vel_x = cfg.weight_max_vel_x;
00176 optim.weight_max_vel_theta = cfg.weight_max_vel_theta;
00177 optim.weight_acc_lim_x = cfg.weight_acc_lim_x;
00178 optim.weight_acc_lim_theta = cfg.weight_acc_lim_theta;
00179 optim.weight_kinematics_nh = cfg.weight_kinematics_nh;
00180 optim.weight_kinematics_forward_drive = cfg.weight_kinematics_forward_drive;
00181 optim.weight_kinematics_turning_radius = cfg.weight_kinematics_turning_radius;
00182 optim.weight_optimaltime = cfg.weight_optimaltime;
00183 optim.weight_obstacle = cfg.weight_obstacle;
00184 optim.weight_inflation = cfg.weight_inflation;
00185 optim.weight_dynamic_obstacle = cfg.weight_dynamic_obstacle;
00186 optim.weight_viapoint = cfg.weight_viapoint;
00187
00188
00189 hcp.enable_multithreading = cfg.enable_multithreading;
00190 hcp.simple_exploration = cfg.simple_exploration;
00191 hcp.max_number_classes = cfg.max_number_classes;
00192 hcp.selection_cost_hysteresis = cfg.selection_cost_hysteresis;
00193 hcp.selection_obst_cost_scale = cfg.selection_obst_cost_scale;
00194 hcp.selection_viapoint_cost_scale = cfg.selection_viapoint_cost_scale;
00195 hcp.selection_alternative_time_cost = cfg.selection_alternative_time_cost;
00196
00197 hcp.obstacle_keypoint_offset = cfg.obstacle_keypoint_offset;
00198 hcp.obstacle_heading_threshold = cfg.obstacle_heading_threshold;
00199 hcp.roadmap_graph_no_samples = cfg.roadmap_graph_no_samples;
00200 hcp.roadmap_graph_area_width = cfg.roadmap_graph_area_width;
00201 hcp.h_signature_prescaler = cfg.h_signature_prescaler;
00202 hcp.h_signature_threshold = cfg.h_signature_threshold;
00203 hcp.viapoints_all_candidates = cfg.viapoints_all_candidates;
00204 hcp.visualize_hc_graph = cfg.visualize_hc_graph;
00205
00206 checkParameters();
00207 }
00208
00209
00210 void TebConfig::checkParameters() const
00211 {
00212
00213 if (robot.max_vel_x_backwards <= 0)
00214 ROS_WARN("TebLocalPlannerROS() Param Warning: Do not choose max_vel_x_backwards to be <=0. Disable backwards driving by increasing the optimization weight for penalyzing backwards driving.");
00215
00216
00217 if (robot.max_vel_x <= optim.penalty_epsilon)
00218 ROS_WARN("TebLocalPlannerROS() Param Warning: max_vel_x <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!");
00219
00220 if (robot.max_vel_x_backwards <= optim.penalty_epsilon)
00221 ROS_WARN("TebLocalPlannerROS() Param Warning: max_vel_x_backwards <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!");
00222
00223 if (robot.max_vel_theta <= optim.penalty_epsilon)
00224 ROS_WARN("TebLocalPlannerROS() Param Warning: max_vel_theta <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!");
00225
00226 if (robot.acc_lim_x <= optim.penalty_epsilon)
00227 ROS_WARN("TebLocalPlannerROS() Param Warning: acc_lim_x <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!");
00228
00229 if (robot.acc_lim_theta <= optim.penalty_epsilon)
00230 ROS_WARN("TebLocalPlannerROS() Param Warning: acc_lim_theta <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!");
00231
00232
00233 if (trajectory.dt_ref <= trajectory.dt_hysteresis)
00234 ROS_WARN("TebLocalPlannerROS() Param Warning: dt_ref <= dt_hysteresis. The hysteresis is not allowed to be greater or equal!. Undefined behavior... Change at least one of them!");
00235
00236
00237 if (trajectory.min_samples <3)
00238 ROS_WARN("TebLocalPlannerROS() Param Warning: parameter min_samples is smaller than 3! Sorry, I haven't enough degrees of freedom to plan a trajectory for you. Please increase ...");
00239
00240
00241 if (obstacles.costmap_obstacles_behind_robot_dist < 0)
00242 ROS_WARN("TebLocalPlannerROS() Param Warning: parameter 'costmap_obstacles_behind_robot_dist' should be positive or zero.");
00243
00244
00245 if (hcp.obstacle_keypoint_offset>=1 || hcp.obstacle_keypoint_offset<=0)
00246 ROS_WARN("TebLocalPlannerROS() Param Warning: parameter obstacle_heading_threshold must be in the interval ]0,1[. 0=0deg opening angle, 1=90deg opening angle.");
00247
00248
00249 if (robot.cmd_angle_instead_rotvel && robot.wheelbase==0)
00250 ROS_WARN("TebLocalPlannerROS() Param Warning: parameter cmd_angle_instead_rotvel is non-zero but wheelbase is set to zero: undesired behavior.");
00251
00252 if (robot.cmd_angle_instead_rotvel && robot.min_turning_radius==0)
00253 ROS_WARN("TebLocalPlannerROS() Param Warning: parameter cmd_angle_instead_rotvel is non-zero but min_turning_radius is set to zero: undesired behavior. You are mixing a carlike and a diffdrive robot");
00254
00255 }
00256
00257 void TebConfig::checkDeprecated(const ros::NodeHandle& nh) const
00258 {
00259 if (nh.hasParam("line_obstacle_poses_affected") || nh.hasParam("polygon_obstacle_poses_affected"))
00260 ROS_WARN("TebLocalPlannerROS() Param Warning: 'line_obstacle_poses_affected' and 'polygon_obstacle_poses_affected' are deprecated. They share now the common parameter 'obstacle_poses_affected'.");
00261
00262 if (nh.hasParam("weight_point_obstacle") || nh.hasParam("weight_line_obstacle") || nh.hasParam("weight_poly_obstacle"))
00263 ROS_WARN("TebLocalPlannerROS() Param Warning: 'weight_point_obstacle', 'weight_line_obstacle' and 'weight_poly_obstacle' are deprecated. They are replaced by the single param 'weight_obstacle'.");
00264
00265 if (nh.hasParam("costmap_obstacles_front_only"))
00266 ROS_WARN("TebLocalPlannerROS() Param Warning: 'costmap_obstacles_front_only' is deprecated. It is replaced by 'costmap_obstacles_behind_robot_dist' to define the actual area taken into account.");
00267
00268 if (nh.hasParam("costmap_emergency_stop_dist"))
00269 ROS_WARN("TebLocalPlannerROS() Param Warning: 'costmap_emergency_stop_dist' is deprecated. You can safely remove it from your parameter config.");
00270
00271 if (nh.hasParam("alternative_time_cost"))
00272 ROS_WARN("TebLocalPlannerROS() Param Warning: 'alternative_time_cost' is deprecated. It has been replaced by 'selection_alternative_time_cost'.");
00273 }
00274
00275
00276 }