teb_config.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2016,
6  * TU Dortmund - Institute of Control Theory and Systems Engineering.
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the institute nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * Author: Christoph Rösmann
37  *********************************************************************/
38 
40 
41 namespace teb_local_planner
42 {
43 
45 {
46 
47  nh.param("odom_topic", odom_topic, odom_topic);
48  nh.param("map_frame", map_frame, map_frame);
49 
50  // Trajectory
52  nh.param("dt_ref", trajectory.dt_ref, trajectory.dt_ref);
58  nh.getParam("global_plan_via_point_sep", trajectory.global_plan_viapoint_sep); // deprecated, see checkDeprecated()
59  if (!nh.param("global_plan_viapoint_sep", trajectory.global_plan_viapoint_sep, trajectory.global_plan_viapoint_sep))
60  nh.setParam("global_plan_viapoint_sep", trajectory.global_plan_viapoint_sep); // write deprecated value to param server
67 
68  // Robot
69  nh.param("max_vel_x", robot.max_vel_x, robot.max_vel_x);
70  nh.param("max_vel_x_backwards", robot.max_vel_x_backwards, robot.max_vel_x_backwards);
71  nh.param("max_vel_y", robot.max_vel_y, robot.max_vel_y);
72  nh.param("max_vel_theta", robot.max_vel_theta, robot.max_vel_theta);
73  nh.param("acc_lim_x", robot.acc_lim_x, robot.acc_lim_x);
74  nh.param("acc_lim_y", robot.acc_lim_y, robot.acc_lim_y);
75  nh.param("acc_lim_theta", robot.acc_lim_theta, robot.acc_lim_theta);
76  nh.param("min_turning_radius", robot.min_turning_radius, robot.min_turning_radius);
77  nh.param("wheelbase", robot.wheelbase, robot.wheelbase);
78  nh.param("cmd_angle_instead_rotvel", robot.cmd_angle_instead_rotvel, robot.cmd_angle_instead_rotvel);
79  nh.param("is_footprint_dynamic", robot.is_footprint_dynamic, robot.is_footprint_dynamic);
80 
81  // GoalTolerance
86 
87  // Obstacles
89  nh.param("inflation_dist", obstacles.inflation_dist, obstacles.inflation_dist);
100 
101  // Optimization
102  nh.param("no_inner_iterations", optim.no_inner_iterations, optim.no_inner_iterations);
103  nh.param("no_outer_iterations", optim.no_outer_iterations, optim.no_outer_iterations);
104  nh.param("optimization_activate", optim.optimization_activate, optim.optimization_activate);
105  nh.param("optimization_verbose", optim.optimization_verbose, optim.optimization_verbose);
106  nh.param("penalty_epsilon", optim.penalty_epsilon, optim.penalty_epsilon);
107  nh.param("weight_max_vel_x", optim.weight_max_vel_x, optim.weight_max_vel_x);
108  nh.param("weight_max_vel_y", optim.weight_max_vel_y, optim.weight_max_vel_y);
109  nh.param("weight_max_vel_theta", optim.weight_max_vel_theta, optim.weight_max_vel_theta);
110  nh.param("weight_acc_lim_x", optim.weight_acc_lim_x, optim.weight_acc_lim_x);
111  nh.param("weight_acc_lim_y", optim.weight_acc_lim_y, optim.weight_acc_lim_y);
112  nh.param("weight_acc_lim_theta", optim.weight_acc_lim_theta, optim.weight_acc_lim_theta);
113  nh.param("weight_kinematics_nh", optim.weight_kinematics_nh, optim.weight_kinematics_nh);
114  nh.param("weight_kinematics_forward_drive", optim.weight_kinematics_forward_drive, optim.weight_kinematics_forward_drive);
115  nh.param("weight_kinematics_turning_radius", optim.weight_kinematics_turning_radius, optim.weight_kinematics_turning_radius);
116  nh.param("weight_optimaltime", optim.weight_optimaltime, optim.weight_optimaltime);
117  nh.param("weight_obstacle", optim.weight_obstacle, optim.weight_obstacle);
118  nh.param("weight_inflation", optim.weight_inflation, optim.weight_inflation);
119  nh.param("weight_dynamic_obstacle", optim.weight_dynamic_obstacle, optim.weight_dynamic_obstacle);
120  nh.param("weight_dynamic_obstacle_inflation", optim.weight_dynamic_obstacle_inflation, optim.weight_dynamic_obstacle_inflation);
121  nh.param("weight_viapoint", optim.weight_viapoint, optim.weight_viapoint);
122  nh.param("weight_prefer_rotdir", optim.weight_prefer_rotdir, optim.weight_prefer_rotdir);
123  nh.param("weight_adapt_factor", optim.weight_adapt_factor, optim.weight_adapt_factor);
124 
125  // Homotopy Class Planner
126  nh.param("enable_homotopy_class_planning", hcp.enable_homotopy_class_planning, hcp.enable_homotopy_class_planning);
127  nh.param("enable_multithreading", hcp.enable_multithreading, hcp.enable_multithreading);
128  nh.param("simple_exploration", hcp.simple_exploration, hcp.simple_exploration);
129  nh.param("max_number_classes", hcp.max_number_classes, hcp.max_number_classes);
130  nh.param("selection_obst_cost_scale", hcp.selection_obst_cost_scale, hcp.selection_obst_cost_scale);
131  nh.param("selection_prefer_initial_plan", hcp.selection_prefer_initial_plan, hcp.selection_prefer_initial_plan);
132  nh.param("selection_viapoint_cost_scale", hcp.selection_viapoint_cost_scale, hcp.selection_viapoint_cost_scale);
133  nh.param("selection_cost_hysteresis", hcp.selection_cost_hysteresis, hcp.selection_cost_hysteresis);
134  nh.param("selection_alternative_time_cost", hcp.selection_alternative_time_cost, hcp.selection_alternative_time_cost);
135  nh.param("switching_blocking_period", hcp.switching_blocking_period, hcp.switching_blocking_period);
136  nh.param("roadmap_graph_samples", hcp.roadmap_graph_no_samples, hcp.roadmap_graph_no_samples);
137  nh.param("roadmap_graph_area_width", hcp.roadmap_graph_area_width, hcp.roadmap_graph_area_width);
138  nh.param("roadmap_graph_area_length_scale", hcp.roadmap_graph_area_length_scale, hcp.roadmap_graph_area_length_scale);
139  nh.param("h_signature_prescaler", hcp.h_signature_prescaler, hcp.h_signature_prescaler);
140  nh.param("h_signature_threshold", hcp.h_signature_threshold, hcp.h_signature_threshold);
141  nh.param("obstacle_keypoint_offset", hcp.obstacle_keypoint_offset, hcp.obstacle_keypoint_offset);
142  nh.param("obstacle_heading_threshold", hcp.obstacle_heading_threshold, hcp.obstacle_heading_threshold);
143  nh.param("viapoints_all_candidates", hcp.viapoints_all_candidates, hcp.viapoints_all_candidates);
144  nh.param("visualize_hc_graph", hcp.visualize_hc_graph, hcp.visualize_hc_graph);
145  nh.param("visualize_with_time_as_z_axis_scale", hcp.visualize_with_time_as_z_axis_scale, hcp.visualize_with_time_as_z_axis_scale);
146 
147  // Recovery
148 
149  nh.param("shrink_horizon_backup", recovery.shrink_horizon_backup, recovery.shrink_horizon_backup);
151  nh.param("oscillation_recovery", recovery.oscillation_recovery, recovery.oscillation_recovery);
152  nh.param("oscillation_v_eps", recovery.oscillation_v_eps, recovery.oscillation_v_eps);
153  nh.param("oscillation_omega_eps", recovery.oscillation_omega_eps, recovery.oscillation_omega_eps);
156 
157  checkParameters();
158  checkDeprecated(nh);
159 }
160 
161 void TebConfig::reconfigure(TebLocalPlannerReconfigureConfig& cfg)
162 {
163  boost::mutex::scoped_lock l(config_mutex_);
164 
165  // Trajectory
166  trajectory.teb_autosize = cfg.teb_autosize;
167  trajectory.dt_ref = cfg.dt_ref;
168  trajectory.dt_hysteresis = cfg.dt_hysteresis;
169  trajectory.global_plan_overwrite_orientation = cfg.global_plan_overwrite_orientation;
170  trajectory.allow_init_with_backwards_motion = cfg.allow_init_with_backwards_motion;
171  trajectory.global_plan_viapoint_sep = cfg.global_plan_viapoint_sep;
172  trajectory.via_points_ordered = cfg.via_points_ordered;
173  trajectory.max_global_plan_lookahead_dist = cfg.max_global_plan_lookahead_dist;
174  trajectory.exact_arc_length = cfg.exact_arc_length;
175  trajectory.force_reinit_new_goal_dist = cfg.force_reinit_new_goal_dist;
176  trajectory.feasibility_check_no_poses = cfg.feasibility_check_no_poses;
177  trajectory.publish_feedback = cfg.publish_feedback;
178 
179  // Robot
180  robot.max_vel_x = cfg.max_vel_x;
181  robot.max_vel_x_backwards = cfg.max_vel_x_backwards;
182  robot.max_vel_y = cfg.max_vel_y;
183  robot.max_vel_theta = cfg.max_vel_theta;
184  robot.acc_lim_x = cfg.acc_lim_x;
185  robot.acc_lim_y = cfg.acc_lim_y;
186  robot.acc_lim_theta = cfg.acc_lim_theta;
187  robot.min_turning_radius = cfg.min_turning_radius;
188  robot.wheelbase = cfg.wheelbase;
189  robot.cmd_angle_instead_rotvel = cfg.cmd_angle_instead_rotvel;
190 
191  // GoalTolerance
192  goal_tolerance.xy_goal_tolerance = cfg.xy_goal_tolerance;
193  goal_tolerance.yaw_goal_tolerance = cfg.yaw_goal_tolerance;
194  goal_tolerance.free_goal_vel = cfg.free_goal_vel;
195 
196  // Obstacles
197  obstacles.min_obstacle_dist = cfg.min_obstacle_dist;
198  obstacles.inflation_dist = cfg.inflation_dist;
199  obstacles.dynamic_obstacle_inflation_dist = cfg.dynamic_obstacle_inflation_dist;
200  obstacles.include_dynamic_obstacles = cfg.include_dynamic_obstacles;
201  obstacles.include_costmap_obstacles = cfg.include_costmap_obstacles;
202  obstacles.legacy_obstacle_association = cfg.legacy_obstacle_association;
203  obstacles.obstacle_association_force_inclusion_factor = cfg.obstacle_association_force_inclusion_factor;
204  obstacles.obstacle_association_cutoff_factor = cfg.obstacle_association_cutoff_factor;
205  obstacles.costmap_obstacles_behind_robot_dist = cfg.costmap_obstacles_behind_robot_dist;
206  obstacles.obstacle_poses_affected = cfg.obstacle_poses_affected;
207 
208 
209  // Optimization
210  optim.no_inner_iterations = cfg.no_inner_iterations;
211  optim.no_outer_iterations = cfg.no_outer_iterations;
212  optim.optimization_activate = cfg.optimization_activate;
213  optim.optimization_verbose = cfg.optimization_verbose;
214  optim.penalty_epsilon = cfg.penalty_epsilon;
215  optim.weight_max_vel_x = cfg.weight_max_vel_x;
216  optim.weight_max_vel_y = cfg.weight_max_vel_y;
217  optim.weight_max_vel_theta = cfg.weight_max_vel_theta;
218  optim.weight_acc_lim_x = cfg.weight_acc_lim_x;
219  optim.weight_acc_lim_y = cfg.weight_acc_lim_y;
220  optim.weight_acc_lim_theta = cfg.weight_acc_lim_theta;
221  optim.weight_kinematics_nh = cfg.weight_kinematics_nh;
222  optim.weight_kinematics_forward_drive = cfg.weight_kinematics_forward_drive;
223  optim.weight_kinematics_turning_radius = cfg.weight_kinematics_turning_radius;
224  optim.weight_optimaltime = cfg.weight_optimaltime;
225  optim.weight_obstacle = cfg.weight_obstacle;
226  optim.weight_inflation = cfg.weight_inflation;
227  optim.weight_dynamic_obstacle = cfg.weight_dynamic_obstacle;
228  optim.weight_dynamic_obstacle_inflation = cfg.weight_dynamic_obstacle_inflation;
229  optim.weight_viapoint = cfg.weight_viapoint;
230  optim.weight_adapt_factor = cfg.weight_adapt_factor;
231 
232  // Homotopy Class Planner
233  hcp.enable_multithreading = cfg.enable_multithreading;
234  hcp.max_number_classes = cfg.max_number_classes;
235  hcp.selection_cost_hysteresis = cfg.selection_cost_hysteresis;
236  hcp.selection_prefer_initial_plan = cfg.selection_prefer_initial_plan;
237  hcp.selection_obst_cost_scale = cfg.selection_obst_cost_scale;
238  hcp.selection_viapoint_cost_scale = cfg.selection_viapoint_cost_scale;
239  hcp.selection_alternative_time_cost = cfg.selection_alternative_time_cost;
240  hcp.switching_blocking_period = cfg.switching_blocking_period;
241 
242  hcp.obstacle_heading_threshold = cfg.obstacle_heading_threshold;
243  hcp.roadmap_graph_no_samples = cfg.roadmap_graph_no_samples;
244  hcp.roadmap_graph_area_width = cfg.roadmap_graph_area_width;
245  hcp.roadmap_graph_area_length_scale = cfg.roadmap_graph_area_length_scale;
246  hcp.h_signature_prescaler = cfg.h_signature_prescaler;
247  hcp.h_signature_threshold = cfg.h_signature_threshold;
248  hcp.viapoints_all_candidates = cfg.viapoints_all_candidates;
249  hcp.visualize_hc_graph = cfg.visualize_hc_graph;
250  hcp.visualize_with_time_as_z_axis_scale = cfg.visualize_with_time_as_z_axis_scale;
251 
252  // Recovery
253 
254  recovery.shrink_horizon_backup = cfg.shrink_horizon_backup;
255  recovery.oscillation_recovery = cfg.oscillation_recovery;
256 
257  checkParameters();
258 }
259 
260 
262 {
263  // positive backward velocity?
264  if (robot.max_vel_x_backwards <= 0)
265  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.");
266 
267  // bounds smaller than penalty epsilon
269  ROS_WARN("TebLocalPlannerROS() Param Warning: max_vel_x <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!");
270 
272  ROS_WARN("TebLocalPlannerROS() Param Warning: max_vel_x_backwards <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!");
273 
275  ROS_WARN("TebLocalPlannerROS() Param Warning: max_vel_theta <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!");
276 
278  ROS_WARN("TebLocalPlannerROS() Param Warning: acc_lim_x <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!");
279 
281  ROS_WARN("TebLocalPlannerROS() Param Warning: acc_lim_theta <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!");
282 
283  // dt_ref and dt_hyst
285  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!");
286 
287  // min number of samples
288  if (trajectory.min_samples <3)
289  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 ...");
290 
291  // costmap obstacle behind robot
293  ROS_WARN("TebLocalPlannerROS() Param Warning: parameter 'costmap_obstacles_behind_robot_dist' should be positive or zero.");
294 
295  // hcp: obstacle heading threshold
297  ROS_WARN("TebLocalPlannerROS() Param Warning: parameter obstacle_heading_threshold must be in the interval ]0,1[. 0=0deg opening angle, 1=90deg opening angle.");
298 
299  // carlike
301  ROS_WARN("TebLocalPlannerROS() Param Warning: parameter cmd_angle_instead_rotvel is non-zero but wheelbase is set to zero: undesired behavior.");
302 
304  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");
305 
306  // positive weight_adapt_factor
307  if (optim.weight_adapt_factor < 1.0)
308  ROS_WARN("TebLocalPlannerROS() Param Warning: parameter weight_adapt_factor shoud be >= 1.0");
309 
311  ROS_WARN("TebLocalPlannerROS() Param Warning: parameter oscillation_filter_duration must be >= 0");
312 
313 }
314 
316 {
317  if (nh.hasParam("line_obstacle_poses_affected") || nh.hasParam("polygon_obstacle_poses_affected"))
318  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'.");
319 
320  if (nh.hasParam("weight_point_obstacle") || nh.hasParam("weight_line_obstacle") || nh.hasParam("weight_poly_obstacle"))
321  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'.");
322 
323  if (nh.hasParam("costmap_obstacles_front_only"))
324  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.");
325 
326  if (nh.hasParam("costmap_emergency_stop_dist"))
327  ROS_WARN("TebLocalPlannerROS() Param Warning: 'costmap_emergency_stop_dist' is deprecated. You can safely remove it from your parameter config.");
328 
329  if (nh.hasParam("alternative_time_cost"))
330  ROS_WARN("TebLocalPlannerROS() Param Warning: 'alternative_time_cost' is deprecated. It has been replaced by 'selection_alternative_time_cost'.");
331 
332  if (nh.hasParam("global_plan_via_point_sep"))
333  ROS_WARN("TebLocalPlannerROS() Param Warning: 'global_plan_via_point_sep' is deprecated. It has been replaced by 'global_plan_viapoint_sep' due to consistency reasons.");
334 }
335 
336 
337 } // namespace teb_local_planner
boost::mutex config_mutex_
Mutex for config accesses and changes.
Definition: teb_config.h:378
bool include_costmap_obstacles
Specify whether the obstacles in the costmap should be taken into account directly.
Definition: teb_config.h:119
bool cmd_angle_instead_rotvel
Substitute the rotational velocity in the commanded velocity message by the corresponding steering an...
Definition: teb_config.h:99
std::string map_frame
Global planning frame.
Definition: teb_config.h:66
double max_vel_y
Maximum strafing velocity of the robot (should be zero for non-holonomic robots!) ...
Definition: teb_config.h:92
int max_samples
Maximum number of samples; Warning: if too small the discretization/resolution might not be sufficien...
Definition: teb_config.h:75
double weight_adapt_factor
Some special weights (currently &#39;weight_obstacle&#39;) are repeatedly scaled by this factor in each outer...
Definition: teb_config.h:159
bool allow_init_with_backwards_motion
If true, the underlying trajectories might be initialized with backwards motions in case the goal is ...
Definition: teb_config.h:77
double acc_lim_x
Maximum translational acceleration of the robot.
Definition: teb_config.h:94
double min_obstacle_dist
Minimum desired separation from obstacles.
Definition: teb_config.h:115
double weight_prefer_rotdir
Optimization weight for preferring a specific turning direction (-> currently only activated if an os...
Definition: teb_config.h:157
bool shrink_horizon_backup
Allows the planner to shrink the horizon temporary (50%) in case of automatically detected issues...
Definition: teb_config.h:194
double roadmap_graph_area_width
< Specify the number of samples generated for creating the roadmap graph, if simple_exploration is tu...
Definition: teb_config.h:177
double dt_hysteresis
Hysteresis for automatic resizing depending on the current temporal resolution (dt): usually 10% of d...
Definition: teb_config.h:73
double weight_max_vel_theta
Optimization weight for satisfying the maximum allowed angular velocity.
Definition: teb_config.h:144
bool selection_alternative_time_cost
If true, time cost is replaced by the total transition time.
Definition: teb_config.h:173
double xy_goal_tolerance
Allowed final euclidean distance to the goal position.
Definition: teb_config.h:107
double global_plan_viapoint_sep
Min. separation between each two consecutive via-points extracted from the global plan (if negative: ...
Definition: teb_config.h:78
double obstacle_heading_threshold
Specify the value of the normalized scalar product between obstacle heading and goal heading in order...
Definition: teb_config.h:183
struct teb_local_planner::TebConfig::Robot robot
Robot related parameters.
double yaw_goal_tolerance
Allowed final orientation error.
Definition: teb_config.h:106
double min_turning_radius
Minimum turning radius of a carlike robot (diff-drive robot: zero);.
Definition: teb_config.h:97
void checkDeprecated(const ros::NodeHandle &nh) const
Check if some deprecated parameters are found and print warnings.
Definition: teb_config.cpp:315
struct teb_local_planner::TebConfig::Trajectory trajectory
Trajectory related parameters.
double weight_max_vel_y
Optimization weight for satisfying the maximum allowed strafing velocity (in use only for holonomic r...
Definition: teb_config.h:143
#define ROS_WARN(...)
double weight_kinematics_turning_radius
Optimization weight for enforcing a minimum turning radius (carlike robots)
Definition: teb_config.h:150
bool legacy_obstacle_association
If true, the old association strategy is used (for each obstacle, find the nearest TEB pose)...
Definition: teb_config.h:122
double weight_acc_lim_x
Optimization weight for satisfying the maximum allowed translational acceleration.
Definition: teb_config.h:145
double visualize_with_time_as_z_axis_scale
If this value is bigger than 0, the trajectory and obstacles are visualized in 3d using the time as t...
Definition: teb_config.h:188
double weight_acc_lim_y
Optimization weight for satisfying the maximum allowed strafing acceleration (in use only for holonom...
Definition: teb_config.h:146
bool optimization_activate
Activate the optimization.
Definition: teb_config.h:137
int feasibility_check_no_poses
Specify up to which pose on the predicted plan the feasibility should be checked each sampling interv...
Definition: teb_config.h:83
double costmap_obstacles_behind_robot_dist
Limit the occupied local costmap obstacles taken into account for planning behind the robot (specify ...
Definition: teb_config.h:120
struct teb_local_planner::TebConfig::GoalTolerance goal_tolerance
Goal tolerance related parameters.
double teb_autosize
Enable automatic resizing of the trajectory w.r.t to the temporal resolution (recommended) ...
Definition: teb_config.h:71
bool via_points_ordered
If true, the planner adheres to the order of via-points in the storage container. ...
Definition: teb_config.h:79
double obstacle_association_force_inclusion_factor
The non-legacy obstacle association technique tries to connect only relevant obstacles with the discr...
Definition: teb_config.h:123
void reconfigure(TebLocalPlannerReconfigureConfig &cfg)
Reconfigure parameters from the dynamic_reconfigure config. Change parameters dynamically (e...
Definition: teb_config.cpp:161
double weight_max_vel_x
Optimization weight for satisfying the maximum allowed translational velocity.
Definition: teb_config.h:142
double force_reinit_new_goal_dist
Reinitialize the trajectory if a previous goal is updated with a seperation of more than the specifie...
Definition: teb_config.h:82
double dt_ref
Desired temporal resolution of the trajectory (should be in the magniture of the underlying control r...
Definition: teb_config.h:72
double acc_lim_theta
Maximum angular acceleration of the robot.
Definition: teb_config.h:96
bool enable_multithreading
Activate multiple threading for planning multiple trajectories in parallel.
Definition: teb_config.h:166
void loadRosParamFromNodeHandle(const ros::NodeHandle &nh)
Load parmeters from the ros param server.
Definition: teb_config.cpp:44
double switching_blocking_period
Specify a time duration in seconds that needs to be expired before a switch to new equivalence class ...
Definition: teb_config.h:174
struct teb_local_planner::TebConfig::HomotopyClasses hcp
std::string odom_topic
Topic name of the odometry message, provided by the robot driver or simulator.
Definition: teb_config.h:65
bool param(const std::string &param_name, T &param_val, const T &default_val) const
double weight_inflation
Optimization weight for the inflation penalty (should be small)
Definition: teb_config.h:153
bool visualize_hc_graph
Visualize the graph that is created for exploring new homotopy classes.
Definition: teb_config.h:187
bool simple_exploration
If true, distinctive trajectories are explored using a simple left-right approach (pass each obstacle...
Definition: teb_config.h:167
double h_signature_prescaler
Scale number of obstacle value in order to allow huge number of obstacles. Do not choose it extremly ...
Definition: teb_config.h:179
double selection_cost_hysteresis
Specify how much trajectory cost must a new candidate have w.r.t. a previously selected trajectory in...
Definition: teb_config.h:169
double weight_obstacle
Optimization weight for satisfying a minimum separation from obstacles.
Definition: teb_config.h:152
bool enable_homotopy_class_planning
Activate homotopy class planning (Requires much more resources that simple planning, since multiple trajectories are optimized at once).
Definition: teb_config.h:165
bool free_goal_vel
Allow the robot&#39;s velocity to be nonzero (usally max_vel) for planning purposes.
Definition: teb_config.h:108
double max_vel_theta
Maximum angular velocity of the robot.
Definition: teb_config.h:93
double oscillation_v_eps
Threshold for the average normalized linear velocity: if oscillation_v_eps and oscillation_omega_eps ...
Definition: teb_config.h:197
double penalty_epsilon
Add a small safety margin to penalty functions for hard-constraint approximations.
Definition: teb_config.h:140
void checkParameters() const
Check parameters and print warnings in case of discrepancies.
Definition: teb_config.cpp:261
bool exact_arc_length
If true, the planner uses the exact arc length in velocity, acceleration and turning rate computation...
Definition: teb_config.h:81
double oscillation_filter_duration
Filter length/duration [sec] for the detection of oscillations.
Definition: teb_config.h:200
double obstacle_association_cutoff_factor
See obstacle_association_force_inclusion_factor, but beyond a multiple of [value]*min_obstacle_dist a...
Definition: teb_config.h:124
double selection_viapoint_cost_scale
Extra scaling of via-point cost terms just for selecting the &#39;best&#39; candidate.
Definition: teb_config.h:172
int no_inner_iterations
Number of solver iterations called in each outerloop iteration.
Definition: teb_config.h:134
double weight_acc_lim_theta
Optimization weight for satisfying the maximum allowed angular acceleration.
Definition: teb_config.h:147
double weight_dynamic_obstacle_inflation
Optimization weight for the inflation penalty of dynamic obstacles (should be small) ...
Definition: teb_config.h:155
double shrink_horizon_min_duration
Specify minimum duration for the reduced horizon in case an infeasible trajectory is detected...
Definition: teb_config.h:195
bool costmap_converter_spin_thread
If true, the costmap converter invokes its callback queue in a different thread.
Definition: teb_config.h:126
int obstacle_poses_affected
The obstacle position is attached to the closest pose on the trajectory to reduce computational effor...
Definition: teb_config.h:121
double obstacle_keypoint_offset
If simple_exploration is turned on, this parameter determines the distance on the left and right side...
Definition: teb_config.h:182
double weight_dynamic_obstacle
Optimization weight for satisfying a minimum separation from dynamic obstacles.
Definition: teb_config.h:154
double roadmap_graph_area_length_scale
The length of the rectangular region is determined by the distance between start and goal...
Definition: teb_config.h:178
std::string costmap_converter_plugin
Define a plugin name of the costmap_converter package (costmap cells are converted to points/lines/po...
Definition: teb_config.h:125
bool viapoints_all_candidates
If true, all trajectories of different topologies are attached to the current set of via-points...
Definition: teb_config.h:185
double weight_optimaltime
Optimization weight for contracting the trajectory w.r.t transition time.
Definition: teb_config.h:151
bool getParam(const std::string &key, std::string &s) const
bool global_plan_overwrite_orientation
Overwrite orientation of local subgoals provided by the global planner.
Definition: teb_config.h:76
double weight_kinematics_nh
Optimization weight for satisfying the non-holonomic kinematics.
Definition: teb_config.h:148
struct teb_local_planner::TebConfig::Recovery recovery
Parameters related to recovery and backup strategies.
bool include_dynamic_obstacles
Specify whether the movement of dynamic obstacles should be predicted by a constant velocity model (t...
Definition: teb_config.h:118
double max_vel_x_backwards
Maximum translational velocity of the robot for driving backwards.
Definition: teb_config.h:91
double weight_viapoint
Optimization weight for minimizing the distance to via-points.
Definition: teb_config.h:156
double oscillation_recovery_min_duration
Minumum duration [sec] for which the recovery mode is activated after an oscillation is detected...
Definition: teb_config.h:199
bool hasParam(const std::string &key) const
int no_outer_iterations
Each outerloop iteration automatically resizes the trajectory and invokes the internal optimizer with...
Definition: teb_config.h:135
double dynamic_obstacle_inflation_dist
Buffer zone around predicted locations of dynamic obstacles with non-zero penalty costs (should be la...
Definition: teb_config.h:117
struct teb_local_planner::TebConfig::Optimization optim
Optimization related parameters.
double selection_obst_cost_scale
Extra scaling of obstacle cost terms just for selecting the &#39;best&#39; candidate.
Definition: teb_config.h:171
bool publish_feedback
Publish planner feedback containing the full trajectory and a list of active obstacles (should be ena...
Definition: teb_config.h:84
double acc_lim_y
Maximum strafing acceleration of the robot.
Definition: teb_config.h:95
double h_signature_threshold
Two h-signatures are assumed to be equal, if both the difference of real parts and complex parts are ...
Definition: teb_config.h:180
double oscillation_omega_eps
Threshold for the average normalized angular velocity: if oscillation_v_eps and oscillation_omega_eps...
Definition: teb_config.h:198
double wheelbase
The distance between the drive shaft and steering axle (only required for a carlike robot with &#39;cmd_a...
Definition: teb_config.h:98
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const
double max_global_plan_lookahead_dist
Specify maximum length (cumulative Euclidean distances) of the subset of the global plan taken into a...
Definition: teb_config.h:80
bool oscillation_recovery
Try to detect and resolve oscillations between multiple solutions in the same equivalence class (robo...
Definition: teb_config.h:196
int max_number_classes
Specify the maximum number of allowed alternative homotopy classes (limits computational effort) ...
Definition: teb_config.h:168
double inflation_dist
buffer zone around obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in ...
Definition: teb_config.h:116
struct teb_local_planner::TebConfig::Obstacles obstacles
Obstacle related parameters.
double weight_kinematics_forward_drive
Optimization weight for forcing the robot to choose only forward directions (positive transl...
Definition: teb_config.h:149
int min_samples
Minimum number of samples (should be always greater than 2)
Definition: teb_config.h:74
double max_vel_x
Maximum translational velocity of the robot.
Definition: teb_config.h:90
bool optimization_verbose
Print verbose information.
Definition: teb_config.h:138
double selection_prefer_initial_plan
Specify a cost reduction in the interval (0,1) for the trajectory in the equivalence class of the ini...
Definition: teb_config.h:170


teb_local_planner
Author(s): Christoph Rösmann
autogenerated on Wed Jun 5 2019 19:25:10