MPPI navigation regression from 1.1.17 to 1.1.18 on Humble #5503
Description
Bug report
Required Info:
- Operating System:
- Ubuntu 22.04 LTS
- Computer:
- Intel i7-10700TE
- ROS2 Version:
- ROS2 Humble binaries
- Version or commit hash:
- 1.1.18
- DDS implementation:
- Cyclone DDS
Steps to reproduce issue
Create a global path with a cusp.
MPPI config:
controller_server:
ros__parameters:
controller_frequency: 10.0
use_sim_time: False
min_x_velocity_threshold: 0.001
min_theta_velocity_threshold: 0.001
failure_tolerance: 0.3
odom_topic: /odometry/filtered
progress_checker_plugin: "progress_checker"
goal_checker_plugins: ["stopped_goal_checker"]
controller_plugin_types: ["nav2_mppi_controller::MPPIController"]
controller_plugins: ["FollowPath"]
progress_checker:
plugin: "nav2_controller::PoseProgressChecker"
required_movement_radius: 0.25
required_movement_angle: 0.25
movement_time_allowance: 20.0
general_goal_checker:
plugin: "nav2_controller::SimpleGoalChecker"
stateful: True
xy_goal_tolerance: 0.08
yaw_goal_tolerance: 0.05
stopped_goal_checker:
plugin: "nav2_controller::StoppedGoalChecker"
trans_stopped_velocity: 0.01
rot_stopped_velocity: 0.01
xy_goal_tolerance: 0.08
yaw_goal_tolerance: 0.05
FollowPath:
plugin: "nav2_mppi_controller::MPPIController"
time_steps: 56
model_dt: 0.1
batch_size: 10000
vx_std: 0.2
vy_std: 0.0
wz_std: 0.15
vx_max: 0.8
vx_min: -0.5
vy_max: 0.0
wz_max: 0.3
iteration_count: 1
prune_distance: 4.0
transform_tolerance: 0.1
temperature: 0.3
gamma: 0.015
motion_model: "DiffDrive"
visualize: False
reset_period: 1.0 # (only in Humble)
TrajectoryVisualizer:
trajectory_step: 5
time_step: 3
max_robot_pose_search_dist: 5.0 # Costmap size / 2
enforce_path_inversion: True
inversion_xy_tolerance: 0.2
inversion_yaw_tolerance: 0.2
AckermannConstrains:
min_turning_r: 0.5
critics: [
"ConstraintCritic",
"GoalCritic",
"GoalAngleCritic",
"ObstaclesCritic",
"PathAlignCritic",
"PathFollowCritic",
"PathAngleCritic",
]
ConstraintCritic:
enabled: true
cost_power: 1
cost_weight: 5.0
GoalCritic:
enabled: true
cost_power: 1
cost_weight: 7.0
threshold_to_consider: 1.4
GoalAngleCritic:
enabled: true
cost_power: 1
cost_weight: 3.0
threshold_to_consider: 0.5
ObstaclesCritic:
enabled: true
cost_power: 1
repulsion_weight: 1.5
critical_weight: 20.0
consider_footprint: false
collision_cost: 10000.0
collision_margin_distance: 0.2
near_goal_distance: 0.5
inflation_radius: 3.9 # (only in Humble)
cost_scaling_factor: 1.5 #! 10.0 # (only in Humble)
PathAlignCritic:
enabled: true
use_path_orientations: true
cost_power: 1
cost_weight: 9.0
max_path_occupancy_ratio: 0.05
trajectory_point_step: 4
threshold_to_consider: 0.5
offset_from_furthest: 4
PathFollowCritic:
enabled: true
cost_power: 1
cost_weight: 10.0
offset_from_furthest: 5
threshold_to_consider: 1.4
PathAngleCritic:
enabled: true
cost_power: 1
forward_preference: False
mode: 2
cost_weight: 2.0
offset_from_furthest: 20
threshold_to_consider: 1.4
max_angle_to_furthest: 0.25
Expected behavior
The robot can follow the global plan and change direction at cusp points.
Actual behavior
On 1.1.17, the robot properly decelerated before the cusp point, reoriented smoothly, and continued to the goal. I created a YouTube video to demonstrate the expected behaviour with the 1.1.17 version. Link
On 1.1.18, the robot stops before the cusp, and a recovery is needed to reach the goal. I also made a video to show this.
Reproduction instructions
We updated one of our robots from 1.1.17 to 1.1.18, and the robot's navigation changed drastically for the worse.