0
$\begingroup$

Setup:

  • ROS2 version: Humble
  • Nav2 version: 1.1.15
  • Hardware: i7-10700TE CPU with 16 GB of RAM

I would like to use the Nav 2 MPPI controller to drive a forklift through a warehouse. I created a virtual corridor to test and tune the controller parameters safely.

The virtual corridor is 3.1-3.2 m wide. The robot's footprint is 2.07 x 1.08 m. It is required that the robot be able to change direction in the corridor.

Photo about the environment

The problem is that if I enable the consider_footprint parameter, the controller can't keep up the 30 Hz controller frequency and the robot drives significantly worse. I know that the SMAC planner considers the robot's footprint during planning so it is not required to enable this footprint checking in the controller. However, I would like to keep it enabled for safety reasons because the controller does not always follow the global plan.

These are my related controller, planner and costmap parameters:

planner_server:
  ros__parameters:
    planner_plugins: ["GridBased"]
    expected_planner_frequency: 1.0
    use_sim_time: False
    GridBased:
      plugin: "nav2_smac_planner/SmacPlannerLattice"
      downsample_costmap: False
      downsampling_factor: 1
      allow_unknown: False 
      tolerance: 0.25
      max_iterations: 1000000 
      max_on_approach_iterations: 1000
      max_planning_time: 10.0 
      motion_model_for_search: "REEDS_SHEPP"
      analytic_expansion_ratio: 3.5
      analytic_expansion_max_length: 3.0
      reverse_penalty: 2.0
      change_penalty: 0.15 
      cost_penalty: 2.0
      rotation_penalty: 5.0
      retrospective_penalty: 0.025
      lattice_filepath: "acker_minturn_05_costres_005.json"
      cache_obstacle_heuristic: True
      allow_reverse_expansion: True 
      smooth_path: True
      smoother:
        max_iterations: 1000
        w_smooth: 0.3
        w_data: 0.2
        tolerance: 0.0000000001 # 1.0e-10
        do_refinement: True
        refinement_num: 2

controller_server:
  ros__parameters:
    controller_frequency: 30.0
    use_sim_time: False
    min_x_velocity_threshold: 0.001
    min_theta_velocity_threshold: 0.001
    odom_topic: /odometry/filtered
    progress_checker_plugin: "progress_checker"
    goal_checker_plugins: ["general_goal_checker"]
    controller_plugin_types: ["nav2_mppi_controller::MPPIController"]
    controller_plugins: ["FollowPath"]
    # Progress checker parameters
    progress_checker:
      plugin: "nav2_controller::PoseProgressChecker" 
      required_movement_radius: 0.25
      required_movement_angle: 0.25
      movement_time_allowance: 10.0
    general_goal_checker:
      plugin: "nav2_controller::SimpleGoalChecker"
      stateful: True 
      xy_goal_tolerance: 0.05 
      yaw_goal_tolerance: 0.05 
    stopped_goal_checker:
      plugin: "nav2_controller::StoppedGoalChecker"
      trans_stopped_velocity: 0.05 
      rot_stopped_velocity: 0.05
    FollowPath:
      plugin: "nav2_mppi_controller::MPPIController"
      time_steps: 90
      model_dt: 0.05
      batch_size: 2000
      vx_std: 0.15 
      vy_std: 0.0
      wz_std: 0.15
      vx_max: 0.5 
      vx_min: -0.5
      vy_max: 0.0
      wz_max: 0.4
      iteration_count: 1
      transform_tolerance: 0.1 
      prune_distance: 3.0
      temperature: 0.3
      gamma: 0.015
      motion_model: "DiffDrive"
      visualize: false
      reset_period: 1.0 
      regenerate_noise: false
      TrajectoryVisualizer:
        trajectory_step: 5
        time_step: 3
      max_robot_pose_search_dist: 5.0
      enforce_path_inversion: True
      inversion_xy_tolerance: 0.2
      inversion_yaw_tolerance: 0.4
      critics:
        [
          "ConstraintCritic",
          "GoalCritic",
          "GoalAngleCritic",
          "CostCritic",
          "PathAlignCritic",
          "PathFollowCritic",
          "PathAngleCritic",
        ]
      ConstraintCritic:
        enabled: true
        cost_power: 1
        cost_weight: 4.0
      GoalCritic:
        enabled: true
        cost_power: 1
        cost_weight: 8.0
        threshold_to_consider: 1.4
      GoalAngleCritic:
        enabled: true
        cost_power: 1
        cost_weight: 3.0
        threshold_to_consider: 0.5
      CostCritic:
        enabled: true
        cost_power: 1
        cost_weight: 3.81
        critical_cost: 300.0
        consider_footprint: true
        collision_cost: 1000000.0
        near_goal_distance: 0.5
      PathAlignCritic:
        enabled: true
        use_path_orientations: True
        cost_power: 1
        cost_weight: 10.0
        max_path_occupancy_ratio: 0.05
        trajectory_point_step: 3 
        threshold_to_consider: 0.5
        offset_from_furthest: 20
      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
        forward_preference: false
        cost_weight: 2.2 
        offset_from_furthest: 20
        threshold_to_consider: 1.4
        max_angle_to_furthest: 0.25

global_costmap:
  global_costmap:
    ros__parameters:
      always_send_full_costmap: False
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      footprint: "[ [-0.52,-0.35], [1.55,-0.35], [ 1.55, 0.73], [-0.52, 0.73] ]"
      footprint_padding: 0.08
      robot_radius: 0.22
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      filters: ["keepout_filter", "inflation_layer"]
      keepout_filter:
        plugin: "nav2_costmap_2d::KeepoutFilter"
        enabled: True
        filter_info_topic: "/costmap_filter_info"
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: "scan"
        footprint_clearing_enabled: true
        scan:
          topic: /scan
          data_type: "LaserScan"
          expected_update_rate: 0.07
          max_obstacle_height: 2.0
          marking: True
          clearing: True
          inf_is_valid: False
          raytrace_max_range: 8.0
          raytrace_min_range: 0.1
          obstacle_max_range: 8.0
          obstacle_min_range: 0.1
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
        enabled: true
        subscribe_to_updates: true
        transform_tolerance: 0.1
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        enabled: True
        cost_scaling_factor: 1.5
        inflation_radius: 3.9 
        inflate_unknown: false
        inflate_around_unknown: true

local_costmap:
  local_costmap:
    ros__parameters:
      always_send_full_costmap: False
      update_frequency: 5.0
      publish_frequency: 2.0 
      transform_tolerance: 0.3 
      global_frame: odom
      robot_base_frame: base_link
      rolling_window: true
      width: 8 
      height: 8 
      resolution: 0.05 
      robot_radius: 0.22
      footprint: "[ [-0.52,-0.35], [1.55,-0.35], [ 1.55, 0.73], [-0.52, 0.73] ]"
      footprint_padding: 0.05
      plugins: ["obstacle_layer", "inflation_layer"]
      filters: ["keepout_filter", "inflation_layer"]
      keepout_filter:
        plugin: "nav2_costmap_2d::KeepoutFilter"
        enabled: True
        filter_info_topic: "/costmap_filter_info"
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        footprint_clearing_enabled: True
        map_subscribe_transient_local: True
        observation_sources: "scan"
        scan:
          topic: /scan
          data_type: "LaserScan"
          expected_update_rate: 0.07
          max_obstacle_height: 2.0
          marking: True
          clearing: True
          inf_is_valid: True
          raytrace_max_range: 8.0
          raytrace_min_range: 0.1
          obstacle_max_range: 7.9
          obstacle_min_range: 0.1
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        enabled: True
        cost_scaling_factor: 1.5 
        inflation_radius: 3.9
        inflate_around_unknown: true 

From what I heard the IPC should be able to run the MPPI controller with 30 Hz and if I check the CPU loads, it is not 100% when it prints this: Control loop missed its desired rate of 30.0000Hz.

What can I do in this situation?

New contributor
vLevente is a new contributor to this site. Take care in asking for clarification, commenting, and answering. Check out our Code of Conduct.
$\endgroup$

0

Your Answer

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

Browse other questions tagged or ask your own question.