TEB turns around multiple times while going towards goal.

[akashjinandra]

Hello All, We've been using TEB for while and noticed issues where it will randomly decide to turn the robot around multiple times as it gets toward the goal. In the picture below the robot is should have straight path. The global path is always straight, but I've noticed sometimes the local plan has a loop, but normally it will straight itself out before the robot gets close. I would say this issue happens once out of every 20 or 30 plans. This screenshot is from testing in gazebo, but this happens on the real vehicle as well.

Here is another example of it happening:

Here is my yaml file:

TebLocalPlannerROS:

odom_topic: odom

Trajectory

teb_autosize: True dt_ref: 1.0 dt_hysteresis: 0.5 global_plan_overwrite_orientation: False allow_init_with_backwards_motion: True max_global_plan_lookahead_dist: 0.0 feasibility_check_no_poses: 3.0 force_reinit_new_goal_dist: 1.0 global_plan_viapoint_sep: 0.5

Robot

max_vel_x: 1.0 max_vel_x_backwards: 1.0 max_vel_y: 0.0 max_vel_theta: 0.5 acc_lim_x: 1.0 acc_lim_y: 0 acc_lim_theta: 0.05

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

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

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

footprint_model: type: "line" line_start: [0.0, 0.0] # for type "line" line_end: [1.2, 0.0] # for type "line"

GoalTolerance

xy_goal_tolerance: 0.05 yaw_goal_tolerance: 0.05 free_goal_vel: False

Obstacles

min_obstacle_dist: 0.6 # This value must also include our robot's expansion, since footprint_model is set to "line". inflation_dist: 0.7 include_costmap_obstacles: True costmap_obstacles_behind_robot_dist: 4.0 obstacle_poses_affected: 30 costmap_converter_plugin: "" costmap_converter_spin_thread: True costmap_converter_rate: 5

Optimization

no_inner_iterations: 10 no_outer_iterations: 5 optimization_activate: True optimization_verbose: False penalty_epsilon: 0.04 weight_max_vel_x: 10 weight_max_vel_y: 0 weight_max_vel_theta: 10 #50 weight_acc_lim_x: 10 weight_acc_lim_y: 0 weight_acc_lim_theta: 10 weight_kinematics_nh: 7000 weight_kinematics_forward_drive: 1 weight_kinematics_turning_radius: 10 #300 weight_optimaltime: 50 weight_obstacle: 500 weight_dynamic_obstacle: 10 # not in use yet weight_viapoint: 10.0

Homotopy Class Planner

enable_homotopy_class_planning: True enable_multithreading: True simple_exploration: False max_number_classes: 4 selection_cost_hysteresis: 0.8 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

[zengxiaolei]

Maybe you can try to set acc_lim_theta to some higher value. According to my experience, 0.05 is too low, and it can cause odd behavior when robot wants turn. By the way, your dt_ref seems to be some large. Hope it helps.

[corot]

Same problem here, even with acc_lim_theta set to 1. @akashjinandra, happens equally going forward and backward? Looks so in your captures.

[akashjinandra]

Ah, so I left the dt ref large so my robot can go faster, up to 3 m/s a second. I've seen this behavior with dt ref at 0.3 and the hysteresis at 0.1 like the default. I also kept the accel limit at 0.05 because the robot will swerve when arriving at the goal when driving in thur negative direction. It doesn't happen when going in the positive direction. Thanks for your inputs

[croesmann]

Hi, the issue could be related to the local minimum problem and the initialization of the global plan. see here for more details. The global planner does not provide any information on the orientation by default so the planner specifies some heuristics and usually assumes forward motions (see the link). This could explain your first scenario. However, in the second scenario, the optimizer might choose this solution as time-optimal because the backward speed is quite large (actually similar to the forward speed). Since your linear acceleration is large as well, switching directions could be faster than steering around the corner with smaller angular velocity.

[corot]

Actually, I got (mostly) rid of this problem by using global_planner instead of navfn. I set orientation_mode = Forward and invert the path myself if that reduces the total rotation-in-place from start pose to path's first pose + path's last pose to goal pose. If this solution is generally applicable, I'll PR the pertinent change on global_planner (by now I do separately in Python)

[akashjinandra]

Sorry I haven't responded here yet, I am using global planner, not navfn. I also have the orientation constraint set at interpolate so that the robot can travel in both directions at full speed. Is there anyway to make it so the planner cares more about staying within the bounds of the obstacles(which it doesn't when it turns around multiple times), rather than be time optimal?

[corot]

interpolate is intended for omnidirectional bases... what I use is forward

For the question, planner actually cares about obstacles, but there are some parameters that affects how it does. Obstacles well ahead in the path are ignored, afaik, unless you increase obstacle_poses_affected parameter

[alliansmover]

Can you solve this problem?

[feifeiwei]

xy_goal_tolerance: 0.05 maybe it will work by increase the value.. e.g.0.5

[akashjinandra]

I don't think we can get away with .5 xy_goal tolerance

[yangjiongfeng]

You can set weight_kinematics_forward_drive to a very large number。 weight_kinematics_forward_drive：1000