search

rst-tu-dortmund / teb_local_planner

An optimal trajectory planner considering distinctive topologies for mobile robots based on Timed-Elastic-Bands (ROS Package)
http://wiki.ros.org/teb_local_planner
BSD 3-Clause "New" or "Revised" License
1.04k stars 548 forks source link

allow_init_with_backwards_motion is false but plan starts going backwards #151

Closed awesomebytes closed 5 years ago

awesomebytes commented 5 years ago

I'm finding a case where the plan very often returns a -X velocity at the start of the plan even with the option allow_init_with_backwards_motion set to false.

I added some prints on the calculated speed:

[DEBUG] [1561537470.346395194]: Unsaturated Velocity command: x,y,theta: -0.472014 -0.0304646 -25.9747
[DEBUG] [1561537470.347284826]:   Saturated Velocity command: x,y,theta: -0.1 -0.0304646 -1

I don't want my robot to move backwards at the start (even though I'm fine with it rotating over itself)... And I don't understand why this is happening or what can I do to avoid it. Any hint would be appreciated.

My current config:

rosparam get /move_base_flex/teb_controller
acc_lim_theta: 0.25
acc_lim_x: 0.5
acc_lim_y: 0.5
allow_init_with_backwards_motion: false
cmd_angle_instead_rotvel: false
costmap_converter:
  CostmapToLinesDBSRANSAC: {cluster_max_distance: 0.4, cluster_min_pts: 2, convex_hull_min_pt_separation: 0.1,
    ransac_convert_outlier_pts: true, ransac_filter_remaining_outlier_pts: false,
    ransac_inlier_distance: 0.15, ransac_min_inliers: 10, ransac_no_iterations: 2000,
    ransac_remaining_outliers: 3}
  CostmapToPolygonsDBSMCCH: {cluster_max_distance: 0.4, cluster_max_pts: 30, cluster_min_pts: 2,
    convex_hull_min_pt_separation: 0.1}
costmap_converter_plugin: costmap_converter::CostmapToPolygonsDBSMCCH
costmap_converter_rate: 5
costmap_converter_spin_thread: true
costmap_obstacles_behind_robot_dist: 1.0
dt_hysteresis: 0.1
dt_ref: 0.2
dynamic_obstacle_inflation_dist: 0.6
enable_multithreading: true
exact_arc_length: true
feasibility_check_no_poses: 5
footprint_model: {radius: 0.2, type: circular}
force_reinit_new_goal_dist: 1.0
free_goal_vel: false
global_plan_overwrite_orientation: true
global_plan_viapoint_sep: 0.5
h_signature_prescaler: 1.0
h_signature_threshold: 0.1
include_costmap_obstacles: true
include_dynamic_obstacles: false
inflation_dist: 1.0
is_footprint_dynamic: false
legacy_obstacle_association: false
map_frame: /odom
max_global_plan_lookahead_dist: 4.0
max_number_classes: 1
max_vel_theta: 1.0
max_vel_x: 0.5
max_vel_x_backwards: 0.1
max_vel_y: 0.5
min_obstacle_dist: 0.1
min_turning_radius: 0.0
no_inner_iterations: 5
no_outer_iterations: 4
obstacle_association_cutoff_factor: 5.0
obstacle_association_force_inclusion_factor: 1.5
obstacle_heading_threshold: 0.45
obstacle_poses_affected: 30
odom_topic: /pepper_odom
optimization_activate: true
optimization_verbose: false
oscillation_recovery: true
penalty_epsilon: 0.04
publish_feedback: true
roadmap_graph_area_length_scale: 1.0
roadmap_graph_area_width: 5.0
roadmap_graph_no_samples: 15
selection_alternative_time_cost: false
selection_cost_hysteresis: 1.0
selection_obst_cost_scale: 100.0
selection_prefer_initial_plan: 0.95
selection_viapoint_cost_scale: 1.0
shrink_horizon_backup: true
switching_blocking_period: 0.0
teb_autosize: true
via_points_ordered: false
viapoints_all_candidates: true
visualize_hc_graph: false
visualize_with_time_as_z_axis_scale: 0.0
weight_acc_lim_theta: 1.0
weight_acc_lim_x: 1.0
weight_acc_lim_y: 1.0
weight_adapt_factor: 2.0
weight_dynamic_obstacle: 50.0
weight_dynamic_obstacle_inflation: 0.1
weight_inflation: 0.1
weight_kinematics_forward_drive: 100.0
weight_kinematics_nh: 1000.0
weight_kinematics_turning_radius: 0.1
weight_max_vel_theta: 1.0
weight_max_vel_x: 2.0
weight_max_vel_y: 2.0
weight_obstacle: 50.0
weight_optimaltime: 1.0
weight_viapoint: 25.0
wheelbase: 0.25
xy_goal_tolerance: 0.4
yaw_goal_tolerance: 0.2

I'm running version 0.6.11 in Kinetic.

croesmann commented 5 years ago

Hi, parameter allow_init_with_backwards_motion affects only the initialization, not the optimization result. The optimizer is still allowed to find backward motions according to the time-optimal objective. However, enforcing only forward motions is difficult since the optimization framework does not support hard constraints and hence small violations are still allowed. You might read #85 and this. You should definitely increase weight_kinematics_forward_drive and make sure that your angular velocity and acceleration limits are properly set.

I close this issue in favor of #85.

awesomebytes commented 5 years ago

@croesmann Thank you very much for your helpful and informing answer, it is very appreciated. Now I understand better what allow_init_with_backwards_motion does. And thanks for the tip on weight_kinematics_forward_drive, makes a lot of sense.