trajectory diverged and robot slightly moving forward when resetting planner

Question

trajectory diverged and robot slightly moving forward when resetting planner

jaykorea opened this issue 2 years ago · 0 comments

jaykorea commented 2 years ago

Hello.

I'm building non-holonomic diff robot with TEB local planner.

The problem occurs when local planning trajectory is disturbed by some obstacles.

Divergence detection message occurs and warning message keep saying "Resetting planner"

I acknowlede of this sequence, however as the planner reset, robot slightly moves forward and bump to the obstacle...

I think cmd_vel is jerking and transmitting for a short time when planner is resetting as the local planner doesn't care of the costmap..

My project goal is for robot to make it keep wait until the obstacle moves away when there is no other feasible trajectory to the goal point( using move_base_flex to keep tyring forever to finish the goal ).

I want teb local planner keep resetting unless the trajectory is feasible, especially not transmitting any cmd_vel when trajectory diverged detected or not feasible(not jerking -> this is the big issue now).

What is the reason of this problem and how can I solve this problem.

Thank you sincerley

p.s. Below is my param setting & short video

IMG_7634.mp4

controllers:
  - name: 'TebLocalPlannerROS'
    type: 'teb_local_planner/TebLocalPlannerROS'
  # - name: 'DWBLocalPlanner'
  #   type: 'nav_core_adapter::LocalPlannerAdapter'     
 
TebLocalPlannerROS:
  odom_topic: /odom_md
  map_frame: map # map
     
  # Trajectory
   
  teb_autosize: True
  dt_ref: 0.35
  dt_hysteresis: 0.05 # default: 0.11
  min_samples: 3
  global_plan_overwrite_orientation: true # defualt : true
  max_global_plan_lookahead_dist: 2.0 #default 2.0
  feasibility_check_no_poses: 4
  publish_feedback: False 
  allow_init_with_backwards_motion: False
 
  # Robot
          
  max_vel_x: 0.7 # default 0.25
  max_vel_y: 0 # default 0.25
  max_vel_x_backwards: 0.25
  max_vel_theta: 0.5 # default 0.45
  acc_lim_x: 0.07 # default 0.5
  acc_lim_y: 0
  acc_lim_theta: 0.315 # default : 0.4
  min_turning_radius: 0
  footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
   type: "polygon" #"line"
   vertices: [[-0.34, -0.28], [-0.34, 0.28], [0.14, 0.28], [0.14, -0.28]] # paddinged 0.05
   #radius: 0.2 # for type "circular"
   #line_start: [-0.15, 0.0] # for type "line"
   #line_end: [0.03, 0.0] # for type "line"
   #front_offset: 0.2 # for type "two_circles"
   #front_radius: 0.2 # for type "two_circles"
   #rear_offset: 0.2 # for type "two_circles"
   #rear_radius: 0.2 # for type "two_circles"
   #vertices: [ [0.25, -0.05], [0.18, -0.05], [0.18, -0.18], [-0.19, -0.18], [-0.25, 0], [-0.19, 0.18], [0.18, 0.18], [0.18, 0.05], [0.25, 0.05] ] # for type "polygon"
 
  # GoalTolerance  
  xy_goal_tolerance: 0.8
  yaw_goal_tolerance: 0.55
  trans_stopped_vel: 0.1
  theta_stopped_vel: 0.1
  free_goal_vel: False
  complete_global_plan: true
  divergence_detection_enable: true
  divergence_detection_max_chi_squared: 55.0
 
  exact_arc_length: true
  control_look_ahead_poses; 1

  min_obstacle_dist: 0.1 # This value must also include our robot radius, since footprint_model is set to "point" or "line".
  inflation_dist: 0.6
  include_costmap_obstacles: True
  include_dynamic_obstacles: True
  costmap_obstacles_behind_robot_dist: 1.0 #default : 1.0
  obstacle_poses_affected: 100

#   costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSRANSAC"
#   costmap_converter_spin_thread: True
#   costmap_converter_rate: 5 # default : 5
 
#  # Configure plugins (namespace move_base/TebLocalPlannerROS/PLUGINNAME)
#   #The parameters must be added for each plugin separately
#   costmap_converter/CostmapToLinesDBSRANSAC:
#     cluster_max_distance: 0.4
#     cluster_min_pts: 2
#     ransac_inlier_distance: 0.15
#     ransac_min_inliers: 10
#     ransac_no_iterations: 2000
#     ransac_remainig_outliers: 3
#     ransac_convert_outlier_pts: True
#     ransac_filter_remaining_outlier_pts: False
#     convex_hull_min_pt_separation: 0.1
#    ## Costmap converter plugin
#   #costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH"   
#   #costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSRANSAC"
#   #costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSMCCH"
#   #costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSConcaveHull"
#   #costmap_converter_plugin: "" # deactivate plugin

  costmap_converter_plugin: "costmap_converter::CostmapToDynamicObstacles"
  costmap_converter_spin_thread: True
  costmap_converter_rate: 5
 
  ## Configure plugins (namespace move_base/CostmapToDynamicObstacles)
  costmap_converter/CostmapToDynamicObstacles:
    alpha_slow: 0.3
    alpha_fast: 0.85
    beta: 0.85
    min_sep_between_slow_and_fast_filter: 80
    min_occupancy_probability: 180
    max_occupancy_neighbors: 100
    morph_size: 1
    filter_by_area: True
    min_area: 3
    max_area: 300
    filter_by_circularity: True
    min_circularity: 0.2
    max_circularity: 1.0
    filter_by_inertia: True
    min_intertia_ratio: 0.2
    max_inertia_ratio: 1.0
    filter_by_convexity: False
    min_convexity: 0.0
    max_convexity: 1.0
    dt: 0.2
    dist_thresh: 60.0
    max_allowed_skipped_frames: 3
    max_trace_length: 10
    static_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH"
 
  #Reduce_velocity_near_obstacles:
  obstacle_proximity_ratio_max_vel: 1.0
  obstacle_proximity_lower_bound: 0.2
  obstacle_proximity_upper_bound: 3

  # Optimization
  no_inner_iterations: 5
  no_outer_iterations: 4
  optimization_activate: True
  optimization_verbose: False
  penalty_epsilon: 0.05
  weight_max_vel_x: 2
  weight_max_vel_theta: 1
  weight_acc_lim_x: 5
  weight_acc_lim_theta: 1
  weight_kinematics_nh: 1000
  weight_kinematics_forward_drive: 1000
  weight_kinematics_turning_radius: 1
  weight_inflation: 0.1
  weight_optimaltime: 1
  weight_obstacle: 50
  weight_velocity_obstacle_ratio: 0.7
  weight_viapoint: 0.0           #10
  #weight_dynamic_obstacle: 100 # not in use yet
 #  alternative_time_cost: False # not in use yet
 
 # viapoints setup
  global_plan_viapoint_sep: -0.1 #0.5
  via_points_ordered: false      #true
 
  # Homotopy Class Planner
  enable_homotopy_class_planning: false # I turned it of because of  #[ WARN] [1654158502.701863979]: Control loop missed its desired rate of 10.0000Hz... the loop actually took 0.1477 seconds
  enable_multithreading: True
  simple_exploration: False
  max_number_classes: 1
  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

# Recovery
  shrink_horizon_backup: True
  shrink_horizon_min_duration: 10
  oscillation_recovery: True
  oscillation_v_eps: 0.1
  oscillation_omega_eps: 0.1
  oscillation_recovery_min_duration: 100
  oscillation_filter_duration: 100