Navigation2 Controller plugin. Currently testing on ros2 foxy.
This is a controller (local trajectory planner) that implements model predictive path integral control to track a path with collision avoidance.
The main idea of the algorithm is to sample batch of control sequences with specified time step for each control, Having inital state of robot (pose, velocity) and batch of controls use iteratively "model" to predict real velocities for each time step in batch.
this can be explained as follows V(t+1) = M(t), where
- V(t+1) - predicted velocities of batch at time step t + 1
- M(T) - Function that predicts real velocities at t + 1 step, by given velocities and control actions at t step.
Then velocities integrated to get trajectories. For each trajectory, the cost function is calculated. All control sequences are weighted by trajectories costs using softmax function to get final control sequence.
MPPIc package requires a modern C++ compiler supporting C++17, and Conan C++ package manager:
pip install conan
| Parameter | Type | Definition |
|---|---|---|
| iteration_count | int | Iteration count in MPPI algorithm |
| lookahead_dist | double | Max lenght of the global plan that considered by local planner |
| batch_size | int | Count of randomly sampled trajectories |
| time_steps | int | Number of time steps (points) in each sampled trajectory |
| model_dt | double | Time interval between two sampled points in trajectories |
| vx_std | double | Sampling standart deviation for VX |
| vy_std | double | Sampling standart deviation for VY |
| wx_std | double | Sampling standart deviation for WX |
| vx_max | double | Max VX |
| vy_max | double | Max VY |
| wz_max | double | Max WZ |
| temperature | double | Selectiveness of trajectories by their costs (The closer this value to 0, the "more" we take in considiration controls with less cost), 0 mean use control with best cost, huge value will lead to just taking mean of all trajectories withou cost consideration |
| visualize | bool | Use visualization |
| motion_model | string | Type of model [diff, omni, carlike] |
| Parameter | Type | Definition |
|---|---|---|
| critics_type | string | Type of controller [float, double] |
| critics_names | string | Critics (plugins) names |
| Parameter | Type | Definition |
|---|---|---|
| goal_weight | double | |
| goal_power | int |
| Parameter | Type | Definition |
|---|---|---|
| goal_angle_cost_weight | double | |
| goal_angle_cost_power | int | |
| threshold_to_consider_goal_angle | double | Minimal distance between robot and goal above which angle goal cost considered |
| Parameter | Type | Definition |
|---|---|---|
| angle_to_goal_cost_weight | double | |
| angle_to_goal_cost_power | int |
| Parameter | Type | Definition |
|---|---|---|
| reference_cost_weight | double | |
| reference_cost_power | int |
| Parameter | Type | Definition |
|---|---|---|
| consider_footprint | bool | |
| obstacle_cost_weight | double | |
| obstacle_cost_power | int | |
| inflation_cost_scaling_factor | int | Must be set accurately according to inflation layer params |
| inflation_radius | double | Must be set accurately according to inflation layer params |
controller_server:
ros__parameters:
FollowPath:
plugin: "mppi::Controller<float>"
time_steps: 15
model_dt: 0.1
batch_size: 300
vx_std: 0.1
vy_std: 0.1
wz_std: 0.40
vx_max: 0.5
vy_max: 0.5
wz_max: 1.3
iteration_count: 2
temperature: 0.25
motion_model: "diff"
visualize: true
CriticScorer:
critics_type: "float"
critics_names: [ "GoalCritic", "GoalAngleCritic", "AngleToGoalCritic", "ReferenceTrajectoryCritic", "ObstaclesCritic" ]
GoalCritic:
goal_cost_power: 1
goal_cost_weight: 15
GoalAngleCritic:
goal_angle_cost_power: 1
goal_angle_cost_weight: 15
threshold_to_consider_goal_angle: 0.20
AngleToGoalCritic:
angle_to_goal_cost_power: 2
angle_to_goal_cost_weight: 2
ReferenceTrajectoryCritic:
reference_cost_power: 1
reference_cost_weight: 5
ObstaclesCritic:
consider_footprint: true
obstacle_cost_power: 1
obstacle_cost_weight: 20
inflation_cost_scaling_factor: 3.0
inflation_radius: 0.75
| Topic | Type | Description |
|---|---|---|
trajectories |
visualization_msgs/MarkerArray |
Randomly generated trajectories, including resulting control sequence |
transformed_global_plan |
nav_msgs/Path |
Part of global plan considered by local planner |