0
$\begingroup$

I'm using an MPPI controller and Smac Hybrid Planner for the Ackermann steering robot to navigate in a confined space. The problem is my long base_footprint: consider the follower robot behind + pallet on the other side. That makes the minimum turning radius = 2.5 , quite large. According to the map, it requires the planner to plan the path considering base_footprint but the visualized path I can see on rviz2 didn't seem like it. Moreover, it seems like sometimes local plan and global plan conflicts

environment

path

have any configuration advice? or should I change planner or controller here's my nav2 param in the controller and planner part:

controller_server:
ros__parameters:
    controller_frequency: 30.0
    FollowPath:
        plugin: "nav2_mppi_controller::MPPIController"
        time_steps: 56
        model_dt: 0.05
        batch_size: 2000
        vx_std: 0.2
        vy_std: 0.2
        wz_std: 0.4
        vx_max: 0.5
        vx_min: -0.55
        vy_max: 0.5
        wz_max: 1.9
        iteration_count: 1
        prune_distance: 2.5
        transform_tolerance: 0.1
        temperature: 0.3
        gamma: 0.015
        motion_model: "Ackermann"
        visualize: false
        reset_period: 1.0 # (only in Humble)
        regenerate_noises: false
        TrajectoryVisualizer:
            trajectory_step: 5
            time_step: 3
        AckermannConstraints:
            min_turning_r: 2.5
        critics:
            [
                "ConstraintCritic",
                "ObstaclesCritic",
                "GoalCritic",
                "GoalAngleCritic",
                "CostCritic",
                "PathAlignCritic",
                "PathFollowCritic",
                "PathAngleCritic",
                "PreferForwardCritic",
            ]
        ConstraintCritic:
            enabled: true
            cost_power: 1
            cost_weight: 4.0
        GoalCritic:
            enabled: true
            cost_power: 1
            cost_weight: 5.0
            threshold_to_consider: 1.4
        GoalAngleCritic:
            enabled: true
            cost_power: 1
            cost_weight: 3.0
            threshold_to_consider: 0.5
        PreferForwardCritic:
            enabled: false
            cost_power: 1
            cost_weight: 5.0
            threshold_to_consider: 0.5
        # ObstaclesCritic:
        #     enabled: true
        #     cost_power: 1
        #     repulsion_weight: 1.5
        #     critical_weight: 20.0
        #     consider_footprint: true
        #     collision_cost: 10000.0
        #     collision_margin_distance: 0.15
        #     near_goal_distance: 0.5
        #     inflation_radius: 0.25 # (only in Humble)
        #     cost_scaling_factor: 10.0 # (only in Humble)
        PathAlignCritic:
            enabled: true
            cost_power: 1
            cost_weight: 14.0
            max_path_occupancy_ratio: 0.05
            trajectory_point_step: 3
            threshold_to_consider: 0.5
            offset_from_furthest: 20
            use_path_orientations: true
        CostCritic:
          enabled: true
          cost_power: 3
          cost_weight: 3.81
          critical_cost: 300.0
          consider_footprint: true
          collision_cost: 1000000.0
          near_goal_distance: 1.0
        PathFollowCritic:
            enabled: true
            cost_power: 1
            cost_weight: 5.0
            offset_from_furthest: 5
            threshold_to_consider: 1.4
        PathAngleCritic:
            enabled: true
            cost_power: 1
            cost_weight: 2.0
            offset_from_furthest: 4
            threshold_to_consider: 0.5
            max_angle_to_furthest: 1.0
            forward_preference: false
            mode: 2

planner part:

 planner_server:
  ros__parameters:
    expected_planner_frequency: 20.0
    use_sim_time: True
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_smac_planner/SmacPlannerHybrid"
      tolerance: 0.5 
      downsample_costmap: true    
      downsampling_factor: 1          
      allow_unknown: True    
      max_iterations: 1000000       
      max_on_approach_iterations: 1000 
      max_planning_time: 2.0        
      smooth_path: true
      motion_model_for_search: "REEDS_SHEPP"
      angle_quantization_bins: 72    
      minimum_turning_radius: 2.5     
      reverse_penalty: 1.0  #For Reeds-Shepp model: penalty to apply if motion is reversing, must be => 1
      change_penalty: 0.0      
      non_straight_penalty: 0.0     
      cost_penalty: 1.5 
      lookup_table_size: 20.0    
      cache_obstacle_heuristic: False
      debug_visualizations: False

global and local costmap:

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 20.0
      publish_frequency: 20.0
      global_frame: odom_diff
      robot_base_frame: base_footprint_diff
      use_sim_time: True
      rolling_window: true
      width: 2
      height: 2
      resolution: 0.05
      footprint: "[ [1.90, 0.3], [1.90, -0.3], [-0.50, -0.3], [-0.50, 0.3] ]" 
      # footprint: "[ [1.0, 0.25], [1.0, -0.25], [-1.0, -0.25], [-1.0, 0.25] ]"   
      origin_x: 0.0  
      origin_y: 0.0
      plugins: ["voxel_layer", "inflation_layer"]
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 5.0
        inflation_radius: 0.15
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 3.5
          obstacle_min_range: 0.2

      always_send_full_costmap: True

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_footprint_diff
      use_sim_time: True
      # robot_radius: 0.22
      footprint: "[ [1.90, 0.3], [1.90, -0.3], [-0.5, -0.3], [-0.5, 0.3] ]" 
      resolution: 0.05
      # footprint: "[ [1.0, 0.25], [1.0, -0.25], [-1.0, -0.25], [-1.0, 0.25] ]"   
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 3.5
          obstacle_min_range: 0.8
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.15
      always_send_full_costmap: True

I have tried changing the motion model for search in planner, and I consume a lot of time trying tuning Planner Critics value, but it still does not work. Thanks for the guidance in advance.

$\endgroup$

1 Answer 1

1
$\begingroup$

Increase your reverse_penalty so that it doesn't want to reverse as much to get those jaggedy paths. When you set it to 1, you're not penalizing reversing at all, its identical to moving forward. Increase that to 2-3-4-5 and see how it goes. For reference:

https://github.com/ros-planning/navigation2/blob/main/nav2_smac_planner/src/node_hybrid.cpp#L431

N *= 1

Its faster for the robot to just do a little wiggle going straight at the goal than it is to arc around and approach at a better heading due to that setting not penalizing it. Your orientation of the goal can also play a factor since the robot obviously can't reorient itself once in that hallway, so it needs to maintain the heading to be achievable of the goal. If that's the heading you actually need, it should be possible, but just FYI that the orientation of everything involved here has a real impact. If you were to - say - have that orientation in the opposite direction, I could see the robot doing a nice drive-past-the-hallway, stop, and back into it in reverse to obtain the goal pretty nicely (for instance).

$\endgroup$
3
  • $\begingroup$ Thanks for your reply. Following your comment, I've tried edit reverse_penalty to 10. Honestly, the path and the behavior of my robot didn't change that much. I'm still facing the stuttering problem, is there any conflict between my controller and planner, my planner keep warning "control loop missed its desired rate" I've provide the video while my robot running nav2 for a clearer explanation : youtu.be/8GKw-6WMOXk $\endgroup$
    – FunkFang
    Mar 6 at 4:32
  • $\begingroup$ Are you sure its possible any other way? $\endgroup$ Mar 6 at 19:03
  • $\begingroup$ Would I better use a simpler planner like NavFN Planner to generate just a simple plan for the goal and use an MPPI controller to perform a feasible control command align the path? I'm trying it now but I'm not sure about it. BTW, I am still confused about how the global and local planners work with each other. According to my output, Is it normal for global planner to plan the path without base_footprint consideration? or is it too hard for it to calculate the feasible path from my scenario which is the very tight space and the large base_footprint? $\endgroup$
    – FunkFang
    Mar 6 at 19:40

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.