Inefficient Planning

[TwoBit01]

Hi,
im currently working with TEB and a tricycle drive. TEB is most of the time working very good and driving the best solution. But sometimes, I could not determine why, he is planning and executing the inefficient way you could think of. Here an example of what i mean:

So the current goal is Node 1108 and the heading is in the same direction as the robot. So the best solution would be to drive straight forward, but teb is planning and executing a solution with two turns. I decreased the tolerances, checked the influence of the weight parameters and changed min_turning_radius, also like vel_theta and nothing worked.

Here my default config parameters:

`TebLocalPlannerROS:

odom_topic: agv_1/odom

Trajectory

teb_autosize: True
dt_ref: 0.275
dt_hysteresis: 0.1
global_plan_overwrite_orientation: true
allow_init_with_backwards_motion: true
max_global_plan_lookahead_dist: 5.0
feasibility_check_no_poses: 5
global_plan_viapoint_sep: -0.1
via_points_ordered: false

Robot

max_vel_x: 1.5
max_vel_x_backwards: 0.75
max_vel_theta: 1.0 # the angular velocity is also bounded by min_turning_radius in case of a carlike robot (r = v / omega)
max_vel_y: 0.0
acc_lim_x: 1.0
acc_lim_theta: 0.5

********************** Carlike robot parameters ********************

min_turning_radius: 0.5 # Min turning radius of the carlike robot (compute value using a model or adjust with rqt_reconfigure manually)
wheelbase: -1.204 # Wheelbase of our robot
cmd_angle_instead_rotvel: True # stage simulator takes the angle instead of the rotvel as input (twist message)

********************************************************************

footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
type: "polygon"
radius: 0.4 # for type "circular"
line_start: [-1.6, 0.0] # for type "line"
line_end: [1.440, 0.0] # for type "line"
front_offset: 0.75 # for type "two_circles"
front_radius: 0.75 # for type "two_circles"
rear_offset: 0.65 # for type "two_circles"
rear_radius: 0.65 # for type "two_circles"
vertices: [ [-1.6,-0.520], [1.440,-0.520], [1.440,0.520], [-1.6,0.520] ] # for type "polygon"

GoalTolerance

xy_goal_tolerance: 0.2
yaw_goal_tolerance: 0.05
free_goal_vel: True

Obstacles

min_obstacle_dist: 0.27 # This value must also include our robot's expansion, since footprint_model is set to "line".
include_costmap_obstacles: True
costmap_obstacles_behind_robot_dist: 1.0
obstacle_poses_affected: 5
inflation_dist: 1.2

Optimization

no_inner_iterations: 5
no_outer_iterations: 4
optimization_activate: true
optimization_verbose: false
penalty_epsilon: 0.1

weight_max_vel_x: 2
weight_max_vel_theta: 1
weight_acc_lim_x: 1
weight_acc_lim_theta: 1
weight_kinematics_nh: 1000
weight_kinematics_forward_drive: 0
weight_kinematics_turning_radius: 5
weight_optimaltime: 50
weight_obstacle: 50
weight_inflation: 0.1
weight_dynamic_obstacle: 10 # not in use yet
weight_adapt_factor: 2

Homotopy Class Planner

enable_homotopy_class_planning: True
enable_multithreading: True
simple_exploration: False
max_number_classes: 4
selection_cost_hysteresis: 1.0
selection_obst_cost_scale: 1.0
selection_alternative_time_cost: False
roadmap_graph_no_samples: 15
roadmap_graph_area_width: 5
h_signature_prescaler: 0.5
h_signature_threshold: 0.1
obstacle_keypoint_offset: 0.1
obstacle_heading_threshold: 0.45
visualize_hc_graph: False
`

Would be very happy about some advices,
Cheer Rob

[croesmann]

Hi @TwoBit01,

sorry for the late response, I was traveling the last 2 weeks.

Which distritbution are you are using? Indigo, kinetic? Can you please clone the most recent kinetic code (either if you are on indigo, jade or kinetic) and test if the same behavior occurs.

The issue might be related to the initialization of the trajectory leading to a different local minimum.
Can you try to set global_plan_overwrite_orientation to false and use the interpolation strategy for orientations from the GlobalPlanner (see rqt_reconfigure for the GlobalPlanner) for testing?

[TwoBit01]

Hey Christoph,
so I tested the indigo 0.4.4 source version and at the first sight, it helped but over a longer testing time, sometimes its still happing.

Im using navgraph to navigate through fixed global paths in the environment. Im calculating for each next navigation point the orientation and pass it as a goal to the move_base:

current_edge_.setX( waypoints_.peek_next().x() - waypoints_.current().x() );
current_edge_.setY( waypoints_.peek_next().y() - waypoints_.current().y() );
yaw = std::atan2( current_edge_.getY(), current_edge_.getX() );

Always checked the orientation when this problem occurred, but the last vector was pointing always to the next nav point, as it should.

[TwoBit01]

Hi,
I tested Teb now with the global_planner instead of navfn and tried all orientation methods. But its still not working better. But I could determine that the solution differ from navfn. Situations where Teb is finding the "correct" solution with navfn, are not working anymore with the global_planner. Would be happy for any suggestions.

[TwoBit01]

Hi,
I was testing a lot in the last weeks and could not really determine, what was causing the behaviour in some situations (star/goal combination with goal orientation) and in some not that seem the same. Especially where the solution should be obvious a straight line. Now I could observe that decreasing or increasing the max_global_lookhead_distance is solving the problem. So I'm dynamically adjusting for each waypoint that parameter and gained a stable driving behaviour for over 200 navpoints in various constellations. I think that is one of the most interesting parameters, due to that it is generating trajectories that are considering the goal and its orientation in advance. In parking situations very useful, so I usually want to have lookahead_distances > 3m. But larger lookaheads are causing more "singularity" or not well-behaving solutions.