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

6
.clangd Normal file
View File

@@ -0,0 +1,6 @@
---
Diagnostics:
UnusedIncludes: None
MissingIncludes: None
ClangTidy:
FastCheckFilter: Loose

1
.gitignore vendored
View File

@@ -1,3 +1,4 @@
.cache
build
install
log

6
colcon_defaults.yaml Normal file
View File

@@ -0,0 +1,6 @@
{
"build": {
"cmake-args": ["-DCMAKE_EXPORT_COMPILE_COMMANDS=1"],
"symlink-install": true
}
}

6
toid_bot_description/src/toid_bot_description.urdf
View File

@@ -3,8 +3,8 @@
<xacro:arg name="use_mock" default="true" />
<xacro:property name="base_width" value="0.31"/>
<xacro:property name="base_length" value="0.25"/>
<xacro:property name="base_width" value="0.30"/>
<xacro:property name="base_length" value="0.30"/>
<xacro:property name="base_height" value="0.34"/>
<xacro:property name="wheel_radius" value="0.04"/>
@@ -53,7 +53,7 @@
<joint name="base_joint" type="fixed">
<child link="base_link"/>
<parent link="base_footprint"/>
<origin xyz="0.0 0.0 ${base_zoff}" rpy="0 0 0"/>
<origin xyz="${-wheel_xoff} 0.0 ${base_zoff}" rpy="0 0 0"/>
</joint>
<joint name="lidar_joint" type="fixed">

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:

68
toid_spinner_controller/CMakeLists.txt Normal file
View File

@@ -0,0 +1,68 @@
cmake_minimum_required(VERSION 3.8)
project(toid_spinner_controller)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
set(library_name toid_spinner_controller)
set(PACKAGE_DEPS
rclcpp
geometry_msgs
nav2_costmap_2d
pluginlib
nav_msgs
nav2_util
nav2_core
tf2
tf2_ros
angles
nav2_controller
)
# find dependencies
find_package(ament_cmake REQUIRED)
foreach(PACKAGE ${PACKAGE_DEPS})
find_package(${PACKAGE} REQUIRED)
endforeach()
add_library(${library_name} SHARED
src/toid_spinner_controller.cpp
src/path_handler.cpp)
target_include_directories(${library_name}
PRIVATE
include
)
ament_target_dependencies(${library_name}
${PACKAGE_DEPS}
)
install(TARGETS ${library_name}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
install(DIRECTORY include/
DESTINATION include/
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
set(ament_cmake_copyright_FOUND TRUE)
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_export_include_directories(include)
ament_export_libraries(${library_name})
ament_export_dependencies(${PACKAGE_DEPS})
pluginlib_export_plugin_description_file(nav2_core toid_spinner_controller.xml)
ament_package()

17
toid_spinner_controller/LICENSE Normal file
View File

@@ -0,0 +1,17 @@
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.

49
toid_spinner_controller/include/toid_spinner_controller/path_handler.hpp Normal file
View File

@@ -0,0 +1,49 @@
#pragma once
#include <memory>
#include <rclcpp/logger.hpp>
#include <tf2_ros/buffer.hpp>
#include "geometry_msgs/msg/pose.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_core/controller_exceptions.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "nav_msgs/msg/path.hpp"
#include "tf2/time.hpp"
namespace toid {
class PathHandler {
public:
PathHandler(tf2::Duration tranform_tolerance,
std::shared_ptr<tf2_ros::Buffer> tf,
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros);
~PathHandler() = default;
nav_msgs::msg::Path
transformGlobalPlan(const geometry_msgs::msg::PoseStamped &pose,
double max_robot_pose_search_dist,
double angle_tolerance);
bool tranformPose(const std::string frame,
const geometry_msgs::msg::PoseStamped &in_pose,
geometry_msgs::msg::PoseStamped &out_pose);
void setPlan(const nav_msgs::msg::Path &path) { global_plan_ = path; }
nav_msgs::msg::Path getPlan() { return global_plan_; }
protected:
double dist2(const geometry_msgs::msg::PoseStamped &p1, const geometry_msgs::msg::PoseStamped &p2);
rclcpp::Logger logger_{rclcpp::get_logger("MultiPlexPathHandler")};
tf2::Duration transform_tolerance_;
std::shared_ptr<tf2_ros::Buffer> tf_;
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
nav_msgs::msg::Path global_plan_;
};
} // namespace toid

82
toid_spinner_controller/include/toid_spinner_controller/toid_spinner_controller.hpp Normal file
View File

@@ -0,0 +1,82 @@
#pragma once
#include <memory>
#include "geometry_msgs/msg/point_stamped.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/twist_stamped.hpp"
#include "nav2_controller/plugins/position_goal_checker.hpp"
#include "nav2_core/controller.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "nav2_costmap_2d/footprint_collision_checker.hpp"
#include "nav2_util/node_utils.hpp"
#include "pluginlib/class_list_macros.hpp"
#include "pluginlib/class_loader.hpp"
#include "rclcpp/logger.hpp"
#include "rclcpp/parameter_value.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "rclcpp_lifecycle/lifecycle_publisher.hpp"
#include "tf2_ros/buffer.hpp"
#include "toid_spinner_controller/path_handler.hpp"
namespace toid {
class SpinnerController : public nav2_core::Controller {
public:
SpinnerController();
~SpinnerController() override = default;
void configure(const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent,
std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
std::shared_ptr<nav2_costmap_2d::Costmap2DROS>) override;
void cleanup() override;
void activate() override;
void deactivate() override;
void setPlan(const nav_msgs::msg::Path &path) override;
bool cancel() override;
geometry_msgs::msg::TwistStamped
computeVelocityCommands(const geometry_msgs::msg::PoseStamped &pose,
const geometry_msgs::msg::Twist &velocity,
nav2_core::GoalChecker *goal_checker) override;
geometry_msgs::msg::TwistStamped
computeRotateToHeadingCommand(const double &angular_distance_to_heading,
const geometry_msgs::msg::PoseStamped &pose);
void setSpeedLimit(const double &speed_limit,
const bool &percentage) override;
protected:
rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
rclcpp::Logger logger_{rclcpp::get_logger("SpinnerController")};
rclcpp::Clock::SharedPtr clock_;
std::unique_ptr<PathHandler> path_handler_;
std::unique_ptr<nav2_controller::PositionGoalChecker> position_goal_checker_;
std::unique_ptr<
nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>>
collision_checker_;
rclcpp_lifecycle::LifecyclePublisher<
geometry_msgs::msg::PointStamped>::SharedPtr closest_pt_pub_;
std::shared_ptr<tf2_ros::Buffer> tf_;
std::string plugin_name_;
bool new_path_recieved = false;
double last_angular_vel_;
pluginlib::ClassLoader<nav2_core::Controller> lp_loader_;
nav2_core::Controller::Ptr primary_controller_;
// Params
double angular_dist_threshold_;
double angular_disengage_threshold_;
double forward_sampling_distance_;
double max_angular_accel_;
double rotate_to_heading_;
double control_duration_;
};
} // namespace toid

31
toid_spinner_controller/package.xml Normal file
View File

@@ -0,0 +1,31 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>toid_spinner_controller</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="82343504+pimpest@users.noreply.github.com">petar</maintainer>
<license>MIT</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>angles</depend>
<depend>geometry_msgs</depend>
<depend>nav_msgs</depend>
<depend>nav2_costmap_2d</depend>
<depend>nav2_msgs</depend>
<depend>nav2_util</depend>
<depend>nav2_core</depend>
<depend>nav2_controller</depend>
<depend>pluginlib</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
<nav2_core plugin="${prefix}/toid_spinner_controller.xml" />
</export>
</package>

131
toid_spinner_controller/src/path_handler.cpp Normal file
View File

@@ -0,0 +1,131 @@
#include "toid_spinner_controller/path_handler.hpp"
#include "angles/angles.h"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_core/controller_exceptions.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "nav_msgs/msg/path.hpp"
#include "tf2/LinearMath/Quaternion.hpp"
#include "tf2/LinearMath/Vector3.hpp"
#include "tf2/exceptions.hpp"
#include <algorithm>
#include <cmath>
#include <cstdint>
#include <iterator>
#include <memory>
#include <numeric>
namespace toid {
PathHandler::PathHandler(
tf2::Duration tranform_tolerance, std::shared_ptr<tf2_ros::Buffer> tf,
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
: transform_tolerance_(tranform_tolerance), tf_(tf),
costmap_ros_(costmap_ros) {}
nav_msgs::msg::Path
PathHandler::transformGlobalPlan(const geometry_msgs::msg::PoseStamped &pose,
double max_robot_pose_search_dist,
double angle_tolerance) {
if (global_plan_.poses.empty()) {
throw nav2_core::InvalidPath("Received plan with length of one");
}
geometry_msgs::msg::PoseStamped robot_pose;
if (!tranformPose(global_plan_.header.frame_id, pose, robot_pose)) {
throw nav2_core::ControllerTFError(
"Unable to transform robot pose into global plan's frame");
}
auto closest_pose_upper_bound =
nav2_util::geometry_utils::first_after_integrated_distance(
global_plan_.poses.begin(), global_plan_.poses.end(),
max_robot_pose_search_dist);
const double robot_orientation = tf2::getYaw(robot_pose.pose.orientation);
// Calculate the closeset pose and keep track of the next rotation point
auto closest_pose = global_plan_.poses.begin();
auto last_rotation = global_plan_.poses.end();
auto previous_it = global_plan_.poses.begin();
auto it = global_plan_.poses.begin();
double closest_dist = INFINITY;
for (; it != closest_pose_upper_bound; it++) {
const double it_orientation = tf2::getYaw(it->pose.orientation);
const double angle_diff = std::abs(
angles::shortest_angular_distance(it_orientation, robot_orientation));
if (previous_it->pose.position == it->pose.position) {
last_rotation = it + 1;
}
if (angle_diff <= angle_tolerance) {
double dist = dist2(robot_pose, *it);
if (dist <= closest_dist) {
closest_dist = dist;
closest_pose = it;
last_rotation = global_plan_.poses.end();
}
}
previous_it = it;
}
if(last_rotation == closest_pose_upper_bound || last_rotation == global_plan_.poses.end()) {
for (; it != global_plan_.poses.end(); it++) {
if (previous_it->pose.position == it->pose.position) {
last_rotation = it + 1;
} else if (last_rotation != global_plan_.poses.end()) {
break;
}
previous_it = it;
}
}
auto transformGlobalPoseToLocal = [&](const auto &global_plan_pose) {
geometry_msgs::msg::PoseStamped stamped_pose;
stamped_pose.header.frame_id = global_plan_.header.frame_id;
stamped_pose.header.stamp = robot_pose.header.stamp;
stamped_pose.pose = global_plan_pose.pose;
stamped_pose.pose.position.z = 0.0;
return stamped_pose;
};
nav_msgs::msg::Path out_path;
std::transform(closest_pose, last_rotation,
std::back_inserter(out_path.poses),
transformGlobalPoseToLocal);
out_path.header.frame_id = global_plan_.header.frame_id;
out_path.header.stamp = robot_pose.header.stamp;
global_plan_.poses.erase(begin(global_plan_.poses), closest_pose);
return out_path;
}
bool PathHandler::tranformPose(const std::string frame,
const geometry_msgs::msg::PoseStamped &in_pose,
geometry_msgs::msg::PoseStamped &out_pose) {
if (in_pose.header.frame_id == frame) {
out_pose = in_pose;
return true;
}
try {
tf_->transform(in_pose, out_pose, frame, transform_tolerance_);
out_pose.header.frame_id = frame;
return true;
} catch (tf2::TransformException &ex) {
RCLCPP_ERROR(logger_, "Exception in tranformPose: %s", ex.what());
}
return false;
}
double PathHandler::dist2(const geometry_msgs::msg::PoseStamped &p1,
const geometry_msgs::msg::PoseStamped &p2) {
const double x = p1.pose.position.x - p2.pose.position.x;
const double y = p1.pose.position.y - p2.pose.position.y;
return x * x + y * y;
}
} // namespace toid

206
toid_spinner_controller/src/toid_spinner_controller.cpp Normal file
View File

@@ -0,0 +1,206 @@
#include "toid_spinner_controller/toid_spinner_controller.hpp"
#include "geometry_msgs/msg/point_stamped.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/twist_stamped.hpp"
#include "pluginlib/class_list_macros.hpp"
#include "pluginlib/exceptions.hpp"
#include "rclcpp/logging.hpp"
#include "toid_spinner_controller/path_handler.hpp"
#include <algorithm>
#include <angles/angles.h>
#include <cmath>
#include <limits>
#include <memory>
#include <rclcpp/duration.hpp>
#include <tf2/time.hpp>
#include <tf2/utils.hpp>
namespace toid {
SpinnerController::SpinnerController()
: lp_loader_("nav2_core", "nav2_core::Controller") {}
void SpinnerController::configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent, std::string name,
std::shared_ptr<tf2_ros::Buffer> tf,
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) {
position_goal_checker_ =
std::make_unique<nav2_controller::PositionGoalChecker>();
position_goal_checker_->initialize(parent, ".position_checker", costmap_ros);
plugin_name_ = name;
node_ = parent;
auto node = node_.lock();
tf_ = tf;
logger_ = node->get_logger();
clock_ = node->get_clock();
std::string primary_controller;
double control_frequency;
nav2_util::declare_parameter_if_not_declared(
node, plugin_name_ + ".angular_dist_threshold",
rclcpp::ParameterValue(0.785));
nav2_util::declare_parameter_if_not_declared(
node, plugin_name_ + ".angular_disengage_threshold",
rclcpp::ParameterValue(0.785 / 2.0));
nav2_util::declare_parameter_if_not_declared(
node, plugin_name_ + ".forward_sampling_distance",
rclcpp::ParameterValue(0.5));
nav2_util::declare_parameter_if_not_declared(
node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2));
nav2_util::declare_parameter_if_not_declared(
node, plugin_name_ + ".rotate_to_heading", rclcpp::ParameterValue(1.0));
nav2_util::declare_parameter_if_not_declared(
node, plugin_name_ + ".primary_controller.plugin",
rclcpp::PARAMETER_STRING);
node->get_parameter(plugin_name_ + ".angular_dist_threshold",
angular_dist_threshold_);
node->get_parameter(plugin_name_ + ".angular_disengage_threshold",
angular_disengage_threshold_);
node->get_parameter(plugin_name_ + ".forward_sampling_distance",
forward_sampling_distance_);
node->get_parameter(plugin_name_ + ".max_angular_accel", max_angular_accel_);
node->get_parameter(plugin_name_ + ".rotate_to_heading", rotate_to_heading_);
primary_controller =
node->get_parameter(plugin_name_ + ".primary_controller.plugin")
.as_string();
node->get_parameter("controller_frequency", control_frequency);
control_duration_ = 1.0 / control_frequency;
try {
primary_controller_ = lp_loader_.createUniqueInstance(primary_controller);
RCLCPP_INFO(logger_,
"Created internal controller for multiplexer: %s of type %s",
plugin_name_.c_str(), primary_controller.c_str());
} catch (const pluginlib::PluginlibException &ex) {
RCLCPP_FATAL(
logger_,
"Failed to create primary controller for multiplexer. Exception: %s",
ex.what());
return;
}
primary_controller_->configure(parent, name + ".primary_controller", tf,
costmap_ros);
path_handler_ = std::make_unique<PathHandler>(tf2::durationFromSec(0.1), tf_,
costmap_ros);
closest_pt_pub_ = node->create_publisher<geometry_msgs::msg::PointStamped>(
"start_point", 1);
}
void SpinnerController::cleanup() {
RCLCPP_INFO(logger_, "Cleaning SpinnerController");
primary_controller_->cleanup();
primary_controller_->reset();
position_goal_checker_.reset();
path_handler_.reset();
closest_pt_pub_.reset();
}
void SpinnerController::activate() {
RCLCPP_INFO(logger_, "Activating SpinnerController");
last_angular_vel_ = std::numeric_limits<double>::max();
new_path_recieved = false;
auto node = node_.lock();
closest_pt_pub_->on_activate();
primary_controller_->activate();
position_goal_checker_->reset();
}
void SpinnerController::deactivate() {
RCLCPP_INFO(logger_, "Deactivating SpinnerController");
closest_pt_pub_.reset();
primary_controller_->deactivate();
closest_pt_pub_->on_deactivate();
}
void SpinnerController::setPlan(const nav_msgs::msg::Path &path) {
path_handler_->setPlan(path);
new_path_recieved = true;
primary_controller_->setPlan(path);
}
geometry_msgs::msg::TwistStamped SpinnerController::computeVelocityCommands(
const geometry_msgs::msg::PoseStamped &pose,
const geometry_msgs::msg::Twist &velocity,
nav2_core::GoalChecker *goal_checker) {
auto path = path_handler_->transformGlobalPlan(pose, 1.0, M_PI / 8);
if (path.poses.size() > 0) {
geometry_msgs::msg::PointStamped pt;
const auto &pose = path.poses.back();
pt.point = pose.pose.position;
pt.header.frame_id = path.header.frame_id;
pt.header.stamp = path.header.stamp;
closest_pt_pub_->publish(pt);
}
geometry_msgs::msg::PoseStamped robot_pose;
path_handler_->tranformPose(path.header.frame_id, pose, robot_pose);
const double robot_angle = tf2::getYaw(robot_pose.pose.orientation);
if (path.poses.front().pose.position == path.poses.back().pose.position) {
const double path_angle = tf2::getYaw(path.poses.back().pose.orientation);
const double angle =
angles::shortest_angular_distance(robot_angle, path_angle);
return computeRotateToHeadingCommand(angle, pose);
}
if (new_path_recieved) {
new_path_recieved = false;
primary_controller_->setPlan(path);
}
auto twist = primary_controller_->computeVelocityCommands(pose, velocity,
goal_checker);
last_angular_vel_ = twist.twist.angular.z;
return twist;
}
geometry_msgs::msg::TwistStamped
SpinnerController::computeRotateToHeadingCommand(
const double &angular_distance_to_heading,
const geometry_msgs::msg::PoseStamped &pose) {
geometry_msgs::msg::TwistStamped cmd;
const double last_angular_vel =
(last_angular_vel_ == std::numeric_limits<double>::max())
? 0
: last_angular_vel_;
const double sign = angular_distance_to_heading > 0.0 ? 1.0 : -1.0;
const double speed_upper_bound =
last_angular_vel + control_duration_ * max_angular_accel_;
const double speed_lower_bound =
last_angular_vel - control_duration_ * max_angular_accel_;
const double speed_until_overshoot =
0.9 * std::sqrt(2.0 * max_angular_accel_ *
std::fabs(angular_distance_to_heading));
const double requested_speed = sign * std::fmin(speed_until_overshoot,rotate_to_heading_);
const double speed =
std::clamp(requested_speed, speed_lower_bound, speed_upper_bound);
cmd.header.frame_id = pose.header.frame_id;
cmd.header.stamp = pose.header.stamp;
cmd.twist.angular.z = speed;
last_angular_vel_ = speed;
return cmd;
}
void SpinnerController::setSpeedLimit(const double &speed_limit,
const bool &percentage) {
primary_controller_->setSpeedLimit(speed_limit, percentage);
}
bool SpinnerController::cancel() { return primary_controller_->cancel(); }
} // namespace toid
PLUGINLIB_EXPORT_CLASS(toid::SpinnerController, nav2_core::Controller)

9
toid_spinner_controller/toid_spinner_controller.xml Normal file
View File

@@ -0,0 +1,9 @@
<class_libraries>
<library path="toid_spinner_controller">
<class type="toid::SpinnerController" base_class_type="nav2_core::Controller">
<description>
toid_SpinnerController
</description>
</class>
</library>
</class_libraries>