Hello,

I'm experiencing weird behaviour with a real robot running move_base. My setup consists of two Intel D435i cameras for obstacle avoidance and one Intel T265 camera for odometry, running on Ubuntu 20.04 Focal with ROS Noetic.

I can currently map an area using RTABMap, save the database of the map and launch RTABMap's localization mode.

Next thing I do is launch move_base. It all goes as expected, no errors nor warnings and most trajectories get executed succesfully.

Except sometimes, the robot gets stuck/stays indecisive inexplicably. It happens when I send a new goal and the robot is stationary, close to an obstacle but with enough space to operate (close in the sense that the obstacle is within the 4m x 4m local costmap). The weird thing is, if the local costmap gets updated with a new obstacle (me getting close to it for example), the robot reacts immediately and starts executing the planned path as it should.

I have tried playing with the acceleration values of the baselocalplanner and update and planning frequencies but with no luck.

Has anyone experienced this behaviour? Or have some tips? Thanks for your time.

Here are my configuration files.

costmapcommonparams.yaml:

robot_base_frame: base_link
transform tolerance: 0.3                   #def 0.2
robot_radius: 0.6

obstacle_layer:
  enabled:                  true
  voxel_decay:              10             #seconds if linear, e^n if exponential
  decay_model:              0              #0=linear, 1=exponential, -1=persistent
  voxel_size:               0.05           #meters
  track_unknown_space:      true           #default space is unknown
  observation_persistence:  0.0            #seconds
  max_obstacle_height:      1.2            #meters
  unknown_threshold:        24             #nums voxels
  mark_threshold:           2              #num voxels
  update_footprint_enabled: true
  combination_method:       1              #1=max, 0=override
  obstacle_range:           2.5            #meters
  origin_z:                 0.0            #meters
  publish_voxel_map:        true           #default off
  transform_tolerance:      0.3            #seconds
  mapping_mode:             false          #default off, saves map not for navigation
  map_save_duration:        60             #default 60s, how often to autosave
  observation_sources:      pcl_mark pcl_clear_1 pcl_clear_2

  pcl_mark:
    data_type:               PointCloud2
    topic:                   /local_grid_obstacle
    marking:                 true
    clearing:                false
    min_obstacle_height:     0.0           #default 0, meters
    max_obstacle_height:     1.1           #defaule 3, meters
    expected_update_rate:    2.5           #default 0, if not updating at this rate at least, remove from buffer
    observation_persistence: 0.0           #default 0, use all measurements taken during now-value, 0=latest 
    inf_is_valid:            false         #default false, for laser scans
    clear_after_reading:     true          #default false, clear the buffer after the layer gets readings from it
    filter:                  "passthrough" #default passthrough, apply "voxel", "passthrough", or no filter to sensor data, recommended to have at one filter on   TRY NO FILTER SINCE INPUT ALREADY FILTERED
    voxel_min_points:        0             #default 0, minimum points per voxel for voxel filter

  pcl_clear_1:
    enabled:                 true          #default true, can be toggled on/off with associated service call
    data_type:               PointCloud2
    topic:                   /d4351/depth/color/points
    marking:                 false
    clearing:                true
    min_z:                   -100.0        #default 0, meters
    max_z:                   100.0         #default 10, meters
    vertical_fov_angle:      1.14          #default 0.7, radians    65.5 deg
    horizontal_fov_angle:    1.59          #default 1.04, radians   91.2 deg 
    decay_acceleration:      1.            #default 0, 1/s^2. If laser scanner MUST be 0
    model_type:              0             #default 0 (depth camera). Use 1 for 3D 

  pcl_clear_2:
    enabled:                 true          #default true, can be toggled on/off with associated service call
    data_type:               PointCloud2
    topic:                   /d4352/depth/color/points
    marking:                 false
    clearing:                true
    min_z:                   -100.0        #default 0, meters
    max_z:                   100.0         #default 10, meters
    vertical_fov_angle:      1.14          #default 0.7, radians    65.5 deg
    horizontal_fov_angle:    1.59          #default 1.04, radians   91.2 deg 
    decay_acceleration:      1.            #default 0, 1/s^2. If laser scanner MUST be 0
    model_type:              0             #default 0 (depth camera). Use 1 for 3D


inflation_layer:
#see http://kaiyuzheng.me/documents/navguide.pdf
  enabled:                   true
  inflation_radius:          1.75
  cost_scaling_factor:       2.58          #def 10

localcostmapparams.yaml:

local_costmap:
  global_frame: odom_ground
  rolling_window: true
  update_frequency: 8.0                 #def 5.0
  publish_frequency: 1.0                #def 0.0
  width: 4.0
  height: 4.0
  resolution: 0.05

  plugins:
    - {name: obstacle_layer,      type: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer"}
    - {name: inflation_layer,     type: "costmap_2d::InflationLayer"}

globalcostmapparams.yaml:

global_costmap:
  global_frame: map
  rolling_window: true
  update_frequency: 5.0                 #def 5.0
  publish_frequency: 1.0                #def 0.0
  width: 50
  height: 50
  resolution: 0.05

  static_layer:
    enabled:               true
    unknown_cost_value:    -1         #def -1
    lethal_cost_threshold: 100        #def 100
    map_topic:             /grid_map
    first_map_only:        false      #default false
    subscribe_to_updates:  false      #default false
    track_unknown_space:   true       #default true
    use_maximum:           false      #default false
    trinary_costmap:       true       #default true

  plugins:
     - {name: static_layer,            type: "costmap_2d::StaticLayer"}
     - {name: obstacle_layer,          type: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer"}
     - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}

baselocalplanner_params.yaml:

TrajectoryPlannerROS:
    #velocity
    #see http://kaiyuzheng.me/documents/navguide.pdf
      acc_lim_x: 0.40           #def 2.5
      acc_lim_y: 0.0            #diff
      acc_lim_theta: 0.35       #def 3.2

      max_vel_trans: 0.5
      min_vel_trans: 0.1        #def 0.1

      max_vel_x:  0.5
      min_vel_x: -0.5           #0.1
      max_vel_y: 0.0           #diff
      min_vel_y: 0.0           #diff
      max_vel_theta:  0.4       #def 1.0
      min_vel_theta: -0.4       #def 0.4 
      min_in_place_vel_theta: 0.1  #def 0.4

      escape_vel: -0.1          #def -0.1, speed for backing up
      holonomic_robot: false


    #goal tolerance
      yaw_goal_tolerance: 0.25         #def 0.05, tolerance ins rads 
      xy_goal_tolerance: 0.1           #def 0.1
      latch_xy_goal_tolerance: false   #def false


    #forward simulation
    #see http://kaiyuzheng.me/documents/navguide.pdf
      sim_time: 4.0               #def 1.7
      sim _granularity: 0.025     #def 0.025 (good for turtlebot sized robots), step size in m to take between points on given traj
      angular_sim_granularity: 0.025  #def = sim_granularity, step size in rads to take between points on given trajectory
      vx_samples: 20              #def 3
      vy_samples: 0               #diff drive robot no samples
      vtheta_samples: 40          #def 20
      controller_frequency: 20.0  #def 20.0

    #trajectory scoring
    #see http://kaiyuzheng.me/documents/navguide.pdf
      meter_scoring: true              #def false, use cells or meters   OJO a lo mejor false es mejor
      path_distance_bias: 0.6          #def 0.6
      goal_distance_bias: 0.8          #def 0.8
      occdist_scale: 0.01              #def 0.01

      heading_lookahead: 0.325         #def 0.325
      heading_scoring: false           #def false
      heading_scoring_timestep: 0.8    #def 0.8
      dwa: false                       #def true, but false gives you Trajectory Rollout, which is said to work best with low acceleration limits

      publish_cost_grid_pc: true         #def false
      global_frame_id: odom_ground    #t265_odom_frame   #def same as local costmaps's global frame

    #oscillation
      oscillation_reset_dist: 0.05     #def 0.05, how far to travel before resetting oscillation flags


    #global plan
      prune_plan: true                 #def true, points will fall off the end of the plan once the robot moves 1m past them

globalplannerparams.yaml:

GlobalPlanner:
  allow_unknown: false                          #def true but need track_unknown_space in obstacle_layer true
  default_tolerance: 0.0                        #def 0, might want to increase for occupied goals
  visualize_potential: true                     #def false (potential area pointcloud2)
  use_dijkstra: true                            #def true
  use_quadratic: true                           #def true
  use_grid_path: false                          #def false
  old_navfn_behavior: false                     #def false
  lethal_cost: 253                              #def 253  see http://kaiyuzheng.me/documents/navguide.pdf
  neutral_cost: 66                              #def 50
  cost_factor: 0.55                             #def 0.8
  publish_potential: true                       #def true
  orientation_mode: 0                           #def 0 -> None (no orientation)
  orientation_window_size: 1                    #def 1

movebaseparams.yaml:

base_global_planner: "global_planner/GlobalPlanner"
base_local_planner: "base_local_planner/TrajectoryPlannerROS"

controller_frequency: 20.0            #def 20, rate send vel cmds
planner_patience: 20.0                #def 5, how long planner will wait in attempt to find valid plan before space-clearing operations
controller_patience: 15.0             #def 15, how long controller will wait w/out receiving valid control before space-clearing operations

conservative_reset_dist: 3.0         #def 3, distance which obstacles will be cleared from the costmap when space-clearing operations. ONLY used when def recovery behaviors used

recovery_behavior_enabled: true      #def true
clearing_rotation_allowed: true      #def true. ONLY used when def recovery behaviors used
shutdown_costmaps: false             #def false, shutdown when move_base inactive
oscillation_timeout: 15.0            #def 0.0 (infinite), how long allow oscillation before recovery behaviors
oscillation_distance: 0.5            #def 0.5, how far move to be considered not oscillating, resets oscillation counter
planner_frequency: 1.0               #def 0.0 (only when new goal received), rate run global planning loop
max_planning_retries: 10             #def -1 (infinite), how many retries before recovery behaviors