Pimpest/toid
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Created path segmenter controller

Browse Source
This commit is contained in:
pimpest
2026-01-11 22:57:26 +01:00
committed by Pimpest
parent 3a6907a1fe
commit eca8d2532f
19 changed files with 4411 additions and 169 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3620
toid_navigation/params/output.json Normal file
View File

File diff suppressed because it is too large Load Diff

233
toid_navigation/params/toid_general_params.yaml
View File

@@ -22,22 +22,25 @@ behavior_server:
local_footprint_topic: local_costmap/published_footprint
global_footprint_topic: global_costmap/published_footprint
cycle_frequency: 10.0
behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
behavior_plugins: ["backup", "spin", "drive_on_heading", "assisted_teleop", "wait"]
spin:
plugin: "nav2_behaviors::Spin"
backup:
plugin: "nav2_behaviors::BackUp"
enable_stamped_cmd_vel: true
simulate_ahead_time: 0.2
drive_on_heading:
plugin: "nav2_behaviors::DriveOnHeading"
wait:
plugin: "nav2_behaviors::Wait"
assisted_teleop:
plugin: "nav2_behaviors::AssistedTeleop"
local_frame: odom
local_frame: map
global_frame: map
robot_base_frame: base_footprint
enable_stamped_cmd_vel: true
transform_tolerance: 0.1
simulate_ahead_time: 2.0
simulate_ahead_time: 0.2
max_rotational_vel: 1.0
min_rotational_vel: 0.1
rotational_acc_lim: 3.2
@@ -45,22 +48,23 @@ behavior_server:
global_costmap:
global_costmap:
ros__parameters:
update_frequency: 1.0
update_frequency: 5.0
publish_frequency: 1.0
global_frame: map
robot_base_frame: base_footprint
robot_radius: 0.18
footprint: "[[-0.08, 0.15], [0.22, 0.15], [0.22, -0.15], [-0.08, -0.15]]"
footprint_padding: 0.02
track_unknown_space: false
rolling_window: false
plugins: ["static_layer", "inflation_layer"]
static_layer:
plugin: "nav2_costmap_2d::StaticLayer"
footprint_clearing_enabled: false
footprint_clearing_enabled: true
map_subscribe_transient_local: True
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
footprint_clearing_enabled: false
cost_scaling_factor: 12.0
footprint_clearing_enabled: true
cost_scaling_factor: 6.0
inflation_radius: 0.40
always_send_full_costmap: True
@@ -69,8 +73,8 @@ local_costmap:
ros__parameters:
update_frequency: 5.0
publish_frequency: 2.0
footprint: "[[-0.13, 0.16], [0.13, 0.16], [0.13, -0.16], [-0.13, -0.16]]"
footprint_padding: 0.02
footprint: "[[-0.08, 0.15], [0.22, 0.15], [0.22, -0.15], [-0.08, -0.15]]"
footprint_padding: 0.01
#robot_radius: 0.18
global_frame: map
robot_base_frame: base_footprint
@@ -82,25 +86,57 @@ local_costmap:
plugins: ["static_layer", "inflation_layer"]
static_layer:
plugin: "nav2_costmap_2d::StaticLayer"
footprint_clearing_enabled: true
map_subscribe_transient_local: True
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 10.0
inflation_radius: 0.45
cost_scaling_factor: 3.0
inflation_radius: 0.40
planner_server:
ros__parameters:
allow_partial_planning: false
expected_planner_frequency: 20.0
expected_planner_frequency: 0.02
costmap_update_timeout: 1.0
introspection_mode: "disabled"
planner_plugins: ['GridBased']
GridBased:
plugin: 'nav2_navfn_planner::NavfnPlanner'
plugin: "nav2_smac_planner::SmacPlannerLattice" # In Iron and older versions, "/" was used instead of "::"
allow_unknown: true # Allow traveling in unknown space
tolerance: 0.05 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found.
max_iterations: 1000000 # Maximum total iterations to search for before failing (in case unreachable), set to -1 to disable
max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution
max_planning_time: 5.0 # Max time in s for planner to plan, smooth
analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach.
analytic_expansion_max_length: 0.1 # For Hybrid/Lattice nodes The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting
analytic_expansion_max_cost: 50.0 # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal
analytic_expansion_max_cost_override: false # Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required)
downsample_obstacle_heuristic: true # Downsample the obstacle map dynamic programming distance expansion heuristic to speed up search at the cost of some path quality
use_quadratic_cost_penalty: false # Use quadratic cost penalty for traversal and heuristic cost computations rather than linear
reverse_penalty: 1.0 # Penalty to apply if motion is reversing, must be => 1
change_penalty: 0.00 # Penalty to apply if motion is changing directions (L to R), must be >= 0
non_straight_penalty: 1.2 # Penalty to apply if motion is non-straight, must be => 1
cost_penalty: 2.5 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable.
rotation_penalty: 4.0 # Penalty to apply to in-place rotations, if minimum control set contains them
retrospective_penalty: 0.015
lookup_table_size: 1.0 # Size of the dubin/reeds-sheep distance window to cache, in meters.
cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static.
allow_reverse_expansion: true # If true, allows the robot to use the primitives to expand in the mirrored opposite direction of the current robot's orientation (to reverse).
coarse_search_resolution: 1 # Number of bins to skip when doing a coarse search for the path. Only used for all_direction goal heading mode.
goal_heading_mode: "DEFAULT" # DEFAULT, BIDIRECTIONAL, ALL_DIRECTION
smooth_path: True # If true, does a simple and quick smoothing post-processing to the path
smoother:
max_iterations: 1000
w_smooth: 0.3
w_data: 0.2
tolerance: 1.0e-10
do_refinement: true
refinement_num: 2
controller_server:
ros__parameters:
controller_frequency: 20.0
controller_frequency: 100.0
costmap_update_timeout: 0.3
failure_tolerance: 0.3
odom_topic: "odom"
@@ -115,146 +151,39 @@ controller_server:
movement_time_allowance: 5.0
goal_checker:
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.10
xy_goal_tolerance: 0.04
yaw_goal_tolerance: 0.10
stateful: True
FollowPath:
# plugin: "dwb_core::DWBLocalPlanner"
# debug_trajectory_details: True
# min_vel_x: 0.0
# min_vel_y: 0.0
# max_vel_x: 0.26
# max_vel_y: 0.0
# max_vel_theta: 1.0
# min_speed_xy: 0.0
# max_speed_xy: 0.26
# min_speed_theta: 0.0
# acc_lim_x: 2.5
# acc_lim_y: 0.0
# acc_lim_theta: 3.2
# decel_lim_x: -2.5
# decel_lim_y: 0.0
# decel_lim_theta: -3.2
# vx_samples: 20
# vtheta_samples: 20
# sim_time: 1.7
# linear_granularity: 0.05
# angular_granularity: 0.025
# transform_tolerance: 0.2
# xy_goal_tolerance: 0.10
# trans_stopped_velocity: 0.25
# short_circuit_trajectory_evaluation: True
# limit_vel_cmd_in_traj: False
# stateful: True
# critics: ["RotateToGoal", "Oscillation", "ObstacleFootprint", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
# ObstacleFootprint.scale: 1.00
# PathAlign.scale: 32.0
# GoalAlign.scale: 24.0
# PathAlign.forward_point_distance: 0.05
# GoalAlign.forward_point_distance: 0.05
# PathDist.scale: 32.0
# GoalDist.scale: 24.0
# RotateToGoal.scale: 32.0
# RotateToGoal.slowing_factor: 5.0
# RotateToGoal.lookahead_time: -1.0
plugin: "nav2_mppi_controller::MPPIController"
time_steps: 56
model_dt: 0.05
batch_size: 2000
ax_max: 3.0
ax_min: -3.0
ay_max: 3.0
ay_min: -3.0
az_max: 3.5
vx_std: 0.2
vy_std: 0.2
wz_std: 0.4
vx_max: 0.5
vx_min: -0.35
vy_max: 0.5
wz_max: 1.9
iteration_count: 1
prune_distance: 1.7
transform_tolerance: 0.1
temperature: 0.3
gamma: 0.015
motion_model: "DiffDrive"
visualize: true
publish_optimal_trajectory: true
regenerate_noises: true
TrajectoryVisualizer:
trajectory_step: 5
time_step: 3
TrajectoryValidator:
# The validator can be configured with additional parameters if needed.
plugin: "mppi::DefaultOptimalTrajectoryValidator"
collision_lookahead_time: 1.0 # Time to look ahead to check for collisions
consider_footprint: true # Whether to consider the full robot's footprint (or circle) in trajectory validation collision checks
AckermannConstraints:
min_turning_r: 0.2
critics:
[
"ConstraintCritic",
"CostCritic",
"GoalCritic",
"GoalAngleCritic",
"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: true
cost_power: 1
cost_weight: 5.0
threshold_to_consider: 0.5
CostCritic:
enabled: true
cost_power: 1
cost_weight: 3.81
near_collision_cost: 253
critical_cost: 300.0
consider_footprint: true
collision_cost: 1000000.0
near_goal_distance: 1.0
trajectory_point_step: 2
PathAlignCritic:
enabled: true
cost_power: 1
cost_weight: 14.0
max_path_occupancy_ratio: 0.05
trajectory_point_step: 4
threshold_to_consider: 0.5
offset_from_furthest: 20
use_path_orientations: false
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
mode: 0
plugin: "toid::SpinnerController"
primary_controller:
plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
desired_linear_vel: 0.50
lookahead_dist: 0.1
min_lookahead_dist: 0.08
max_lookahead_dist: 0.15
lookahead_time: 0.2
rotate_to_heading_angular_vel: 0.8
use_velocity_scaled_lookahead_dist: true
min_approach_linear_velocity: 0.1
approach_velocity_scaling_dist: 0.6
use_collision_detection: true
max_allowed_time_to_collision_up_to_carrot: 0.3
use_regulated_linear_velocity_scaling: true
use_fixed_curvature_lookahead: false
curvature_lookahead_dist: 0.1
use_cost_regulated_linear_velocity_scaling: true
cost_scaling_dist: 0.1
cost_scaling_gain: 1.5
regulated_linear_scaling_min_radius: 0.9
regulated_linear_scaling_min_speed: 0.25
use_rotate_to_heading: false
allow_reversing: true
max_angular_accel: 3.2
min_distance_to_obstacle: 0.0
max_robot_pose_search_dist: 0.5
use_cancel_deceleration: true
stateful: true
lifecycle_manager:
ros__parameters: