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

1
toid_navigation/CMakeLists.txt
View File

@@ -16,6 +16,7 @@ install(
launch
maps
params
behaviors
rviz
DESTINATION share/${PROJECT_NAME}
)

51
toid_navigation/behaviors/navigate_to_pose_w_backtracking.xml Normal file
View File

@@ -0,0 +1,51 @@
<!--
This Behavior Tree replans the global path periodically at 1 Hz and it also has
recovery actions specific to planning / control as well as general system issues.
This will be continuous if a kinematically valid planner is selected.
-->
<root BTCPP_format="4" main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
<PipelineSequence name="NavigateWithReplanning">
<ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>
<PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>
<RateController hz="1.0">
<RecoveryNode number_of_retries="1" name="ComputePathToPose">
<ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}"/>
<Sequence>
<WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
</Sequence>
</RecoveryNode>
</RateController>
<RecoveryNode number_of_retries="1" name="FollowPath">
<FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}"/>
<Sequence>
<WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
</Sequence>
</RecoveryNode>
</PipelineSequence>
<Sequence>
<Fallback>
<WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
<WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
</Fallback>
<ReactiveFallback name="RecoveryFallback">
<GoalUpdated/>
<RoundRobin name="RecoveryActions">
<Sequence name="ClearingActions">
<ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
</Sequence>
<BackUp backup_dist="0.04" backup_speed="0.05" error_code_id="{backup_code_id}"/>
<BackUp backup_dist="-0.08" backup_speed="0.05" error_code_id="{backup_code_id}"/>
<BackUp backup_dist="0.012" backup_speed="0.05" error_code_id="{backup_code_id}"/>
<BackUp backup_dist="-0.16" backup_speed="0.05" error_code_id="{backup_code_id}"/>
</RoundRobin>
</ReactiveFallback>
</Sequence>
</RecoveryNode>
</BehaviorTree>
</root>

8
toid_navigation/launch/navigation_launch.py
View File

@@ -13,6 +13,8 @@ def generate_launch_description():
map = os.path.join(pkg_share, 'maps', 'mapb2_5cm.yaml')
launch_dir = os.path.join(pkg_share, 'launch')
default_rviz_config_path = os.path.join(pkg_share, 'rviz', 'nav2.rviz')
bt_path = os.path.join(pkg_share, "behaviors", "navigate_to_pose_w_backtracking.xml")
lattice_path = os.path.join(pkg_share, "params", "output.json")
position = "0.756 0.225 0 0 0 0".split(' ')
#position = "0 0 0 0 0 0".split(' ')
@@ -54,7 +56,7 @@ def generate_launch_description():
executable='planner_server',
name='planner_server',
output='screen',
parameters=[params]
parameters=[{'GridBased':{'lattice_filepath': lattice_path}}, params]
)
controller_server = Node(
@@ -70,9 +72,11 @@ def generate_launch_description():
executable='bt_navigator',
name='bt_navigator',
output='screen',
parameters=[params]
parameters=[{'default_nav_to_pose_bt_xml': bt_path}, params]
)
# default_nav_to_pose_bt_xml: "$(find-pkg-share toid_navigation)/behaviors/navigate_to_pose_w_backtracking.xml"
behavior_server = Node(
package='nav2_behaviors',
executable='behavior_server',

2
toid_navigation/package.xml
View File

@@ -14,7 +14,7 @@
<depend>nav2_planner</depend>
<depend>nav2_controller</depend>
<depend>nav2_bt_navigator</depend>
<depend>nav2_lifecycle_manger</depend>
<depend>nav2_lifecycle_manager</depend>
<depend>nav2_bringup</depend>
<depend>ros2_control</depend>
<depend>rviz2</depend>

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:

53
toid_navigation/rviz/nav2.rviz
View File

@@ -4,6 +4,7 @@ Panels:
Name: Displays
Property Tree Widget:
Expanded:
- /RobotModel1
- /GlobalCostMap1/Topic1
Splitter Ratio: 0.5
Tree Height: 592
@@ -46,7 +47,7 @@ Visualization Manager:
Plane Cell Count: 64
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
- Alpha: 0.20000000298023224
Class: rviz_default_plugins/RobotModel
Collision Enabled: false
Description File: ""
@@ -193,8 +194,8 @@ Visualization Manager:
Class: rviz_default_plugins/Path
Color: 25; 255; 0
Enabled: true
Head Diameter: 0.30000001192092896
Head Length: 0.20000000298023224
Head Diameter: 0.029999999329447746
Head Length: 0.019999999552965164
Length: 0.30000001192092896
Line Style: Lines
Line Width: 0.029999999329447746
@@ -204,10 +205,10 @@ Visualization Manager:
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: None
Pose Style: Arrows
Radius: 0.029999999329447746
Shaft Diameter: 0.10000000149011612
Shaft Length: 0.10000000149011612
Shaft Diameter: 0.009999999776482582
Shaft Length: 0.009999999776482582
Topic:
Depth: 5
Durability Policy: Volatile
@@ -216,6 +217,36 @@ Visualization Manager:
Reliability Policy: Reliable
Value: /plan
Value: true
- Alpha: 0.20000000298023224
Class: rviz_default_plugins/PointStamped
Color: 249; 240; 107
Enabled: true
History Length: 1
Name: PointStamped
Radius: 0.05000000074505806
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /lookahead_point
Value: true
- Alpha: 1
Class: rviz_default_plugins/PointStamped
Color: 38; 162; 105
Enabled: true
History Length: 1
Name: PointStamped
Radius: 0.05000000074505806
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /start_point
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
@@ -262,16 +293,16 @@ Visualization Manager:
Views:
Current:
Class: rviz_default_plugins/XYOrbit
Distance: 1.4671714305877686
Distance: 3.863811731338501
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.8760442137718201
Y: -0.15811485052108765
Z: -8.940696716308594e-07
X: 0.1778966784477234
Y: -1.1747734546661377
Z: 1.7285346984863281e-06
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
@@ -288,7 +319,7 @@ Window Geometry:
Height: 896
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd000000040000000000000254000002defc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f000002de000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002defc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f000002de000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005d30000003efc0100000002fb0000000800540069006d00650100000000000005d3000002de00fffffffb0000000800540069006d0065010000000000000450000000000000000000000264000002de00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
QMainWindow State: 000000ff00000000fd000000040000000000000254000002defc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f000002de000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002defc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f000002de000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005d30000003efc0100000002fb0000000800540069006d00650100000000000005d30000027b00fffffffb0000000800540069006d0065010000000000000450000000000000000000000264000002de00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time: