forked from zbw/yiliao2026
迁移了原来的nav2包；新增基于obstacle_scanner构建costmap的nav2，暂未实际测试
This commit is contained in:
29
src/navigation/cyy_navigation2/CMakeLists.txt
Normal file
29
src/navigation/cyy_navigation2/CMakeLists.txt
Normal file
@@ -0,0 +1,29 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(cyy_navigation2)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(nav2_bringup REQUIRED)
|
||||
install(
|
||||
DIRECTORY launch param maps
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
|
||||
21
src/navigation/cyy_navigation2/bt/follow_point.xml
Normal file
21
src/navigation/cyy_navigation2/bt/follow_point.xml
Normal file
@@ -0,0 +1,21 @@
|
||||
<!--
|
||||
This Behavior Tree follows a dynamic pose to a certain distance
|
||||
-->
|
||||
|
||||
<root main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
<PipelineSequence name="NavigateWithReplanning">
|
||||
<RateController hz="1.0">
|
||||
<Sequence>
|
||||
<GoalUpdater input_goal="{goal}" output_goal="{updated_goal}">
|
||||
<ComputePathToPose goal="{updated_goal}" path="{path}" planner_id="GridBased"/>
|
||||
</GoalUpdater>
|
||||
<TruncatePath distance="1.0" input_path="{path}" output_path="{truncated_path}"/>
|
||||
</Sequence>
|
||||
</RateController>
|
||||
<KeepRunningUntilFailure>
|
||||
<FollowPath path="{truncated_path}" controller_id="FollowPath"/>
|
||||
</KeepRunningUntilFailure>
|
||||
</PipelineSequence>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
@@ -0,0 +1,46 @@
|
||||
<!--
|
||||
This Behavior Tree replans the global path once every 15 seconds or if the path becomes invalid. 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 main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
|
||||
<PipelineSequence name="NavigateWithReplanning">
|
||||
<RateController hz="2.0">
|
||||
<RecoveryNode number_of_retries="1" name="ComputePathToPose">
|
||||
<Fallback>
|
||||
<ReactiveSequence>
|
||||
<Inverter>
|
||||
<PathExpiringTimer seconds="10" path="{path}"/>
|
||||
</Inverter>
|
||||
<Inverter>
|
||||
<GlobalUpdatedGoal/>
|
||||
</Inverter>
|
||||
<IsPathValid path="{path}"/>
|
||||
</ReactiveSequence>
|
||||
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||
</Fallback>
|
||||
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||
</RecoveryNode>
|
||||
</RateController>
|
||||
<RecoveryNode number_of_retries="1" name="FollowPath">
|
||||
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||
</RecoveryNode>
|
||||
</PipelineSequence>
|
||||
<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>
|
||||
<Spin spin_dist="1.57"/>
|
||||
<Wait wait_duration="5"/>
|
||||
<BackUp backup_dist="0.30" backup_speed="0.05"/>
|
||||
</RoundRobin>
|
||||
</ReactiveFallback>
|
||||
</RecoveryNode>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
@@ -0,0 +1,38 @@
|
||||
|
||||
<!--
|
||||
This Behavior Tree replans the global path periodically at 1 Hz through an array of poses continuously
|
||||
and it also has recovery actions specific to planning / control as well as general system issues.
|
||||
-->
|
||||
<root main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
|
||||
<PipelineSequence name="NavigateWithReplanning">
|
||||
<RateController hz="0.333">
|
||||
<RecoveryNode number_of_retries="1" name="ComputePathThroughPoses">
|
||||
<ReactiveSequence>
|
||||
<RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7"/>
|
||||
<ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="GridBased"/>
|
||||
</ReactiveSequence>
|
||||
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||
</RecoveryNode>
|
||||
</RateController>
|
||||
<RecoveryNode number_of_retries="1" name="FollowPath">
|
||||
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||
</RecoveryNode>
|
||||
</PipelineSequence>
|
||||
<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>
|
||||
<Spin spin_dist="1.57"/>
|
||||
<Wait wait_duration="5"/>
|
||||
<BackUp backup_dist="0.30" backup_speed="0.05"/>
|
||||
</RoundRobin>
|
||||
</ReactiveFallback>
|
||||
</RecoveryNode>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
@@ -0,0 +1,36 @@
|
||||
|
||||
<!--
|
||||
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 main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
|
||||
<PipelineSequence name="NavigateWithReplanning">
|
||||
<RateController hz="1.0">
|
||||
<RecoveryNode number_of_retries="1" name="ComputePathToPose">
|
||||
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||
</RecoveryNode>
|
||||
</RateController>
|
||||
<RecoveryNode number_of_retries="1" name="FollowPath">
|
||||
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||
</RecoveryNode>
|
||||
</PipelineSequence>
|
||||
<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>
|
||||
<Spin spin_dist="1.57"/>
|
||||
<Wait wait_duration="5"/>
|
||||
<BackUp backup_dist="0.30" backup_speed="0.05"/>
|
||||
</RoundRobin>
|
||||
</ReactiveFallback>
|
||||
</RecoveryNode>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
@@ -0,0 +1,47 @@
|
||||
|
||||
<!--
|
||||
This BT has all the functionalities of navigate_to_pose_w_replanning_and_recovery.xml,
|
||||
with an additional feature to cancel the control closer to the goal proximity and
|
||||
make the robot wait for a specific time, to see if the obstacle clears out before
|
||||
navigating along a significantly longer path to reach the goal location.
|
||||
-->
|
||||
<root main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
|
||||
<PipelineSequence name="NavigateWithReplanning">
|
||||
<RateController hz="1.0">
|
||||
<RecoveryNode number_of_retries="1" name="ComputePathToPose">
|
||||
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||
</RecoveryNode>
|
||||
</RateController>
|
||||
<ReactiveSequence name="MonitorAndFollowPath">
|
||||
<PathLongerOnApproach path="{path}" prox_len="3.0" length_factor="2.0">
|
||||
<RetryUntilSuccessful num_attempts="1">
|
||||
<SequenceStar name="CancelingControlAndWait">
|
||||
<CancelControl name="ControlCancel"/>
|
||||
<Wait wait_duration="5"/>
|
||||
</SequenceStar>
|
||||
</RetryUntilSuccessful>
|
||||
</PathLongerOnApproach>
|
||||
<RecoveryNode number_of_retries="1" name="FollowPath">
|
||||
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||
</RecoveryNode>
|
||||
</ReactiveSequence>
|
||||
</PipelineSequence>
|
||||
<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>
|
||||
<Spin spin_dist="1.57"/>
|
||||
<Wait wait_duration="5"/>
|
||||
<BackUp backup_dist="0.30" backup_speed="0.05"/>
|
||||
</RoundRobin>
|
||||
</ReactiveFallback>
|
||||
</RecoveryNode>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
@@ -0,0 +1,44 @@
|
||||
|
||||
<!--
|
||||
This Behavior Tree replans the global path only if the path becomes invalid 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 main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
|
||||
<PipelineSequence>
|
||||
<RateController hz="1.0" name="RateControllerComputePathToPose">
|
||||
<RecoveryNode number_of_retries="1" name="RecoveryComputePathToPose">
|
||||
<Fallback name="FallbackComputePathToPose">
|
||||
<ReactiveSequence name="CheckIfNewPathNeeded">
|
||||
<Inverter>
|
||||
<GlobalUpdatedGoal/>
|
||||
</Inverter>
|
||||
<IsPathValid path="{path}"/>
|
||||
</ReactiveSequence>
|
||||
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||
</Fallback>
|
||||
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||
</RecoveryNode>
|
||||
</RateController>
|
||||
<RecoveryNode number_of_retries="1" name="RecoveryFollowPath">
|
||||
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||
</RecoveryNode>
|
||||
</PipelineSequence>
|
||||
<ReactiveFallback name="FallbackRecoveries">
|
||||
<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>
|
||||
<Spin name="SpinRecovery" spin_dist="1.57"/>
|
||||
<Wait name="WaitRecovery" wait_duration="5"/>
|
||||
<BackUp name="BackUpRecovery" backup_dist="0.30" backup_speed="0.05"/>
|
||||
</RoundRobin>
|
||||
</ReactiveFallback>
|
||||
</RecoveryNode>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
@@ -0,0 +1,14 @@
|
||||
<!--
|
||||
This Behavior Tree replans the global path after every 1m.
|
||||
-->
|
||||
|
||||
<root main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
<PipelineSequence name="NavigateWithReplanning">
|
||||
<DistanceController distance="1.0">
|
||||
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||
</DistanceController>
|
||||
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||
</PipelineSequence>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
@@ -0,0 +1,14 @@
|
||||
<!--
|
||||
This Behavior Tree replans the global path only when the goal is updated.
|
||||
-->
|
||||
|
||||
<root main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
<PipelineSequence name="NavigateWithReplanning">
|
||||
<GoalUpdatedController>
|
||||
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||
</GoalUpdatedController>
|
||||
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||
</PipelineSequence>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
@@ -0,0 +1,21 @@
|
||||
<!--
|
||||
This Behavior Tree replans the global path only if the path becomes invalid
|
||||
-->
|
||||
<root main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
<PipelineSequence name="NavigateWithReplanning">
|
||||
<RateController hz="1.0">
|
||||
<Fallback>
|
||||
<ReactiveSequence>
|
||||
<Inverter>
|
||||
<GlobalUpdatedGoal/>
|
||||
</Inverter>
|
||||
<IsPathValid path="{path}"/>
|
||||
</ReactiveSequence>
|
||||
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||
</Fallback>
|
||||
</RateController>
|
||||
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||
</PipelineSequence>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
@@ -0,0 +1,14 @@
|
||||
<!--
|
||||
This Behavior Tree replans the global path periodically proprortional to speed.
|
||||
-->
|
||||
|
||||
<root main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
<PipelineSequence name="NavigateWithReplanning">
|
||||
<SpeedController min_rate="0.1" max_rate="1.0" min_speed="0.0" max_speed="0.26">
|
||||
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||
</SpeedController>
|
||||
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||
</PipelineSequence>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
@@ -0,0 +1,14 @@
|
||||
<!--
|
||||
This Behavior Tree replans the global path periodically at 1 Hz.
|
||||
-->
|
||||
|
||||
<root main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
<PipelineSequence name="NavigateWithReplanning">
|
||||
<RateController hz="1.0">
|
||||
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||
</RateController>
|
||||
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||
</PipelineSequence>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
20
src/navigation/cyy_navigation2/bt/odometry_calibration.xml
Normal file
20
src/navigation/cyy_navigation2/bt/odometry_calibration.xml
Normal file
@@ -0,0 +1,20 @@
|
||||
<!--
|
||||
his Behavior Tree drives in a square for odometry calibration experiments
|
||||
-->
|
||||
|
||||
<root main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
<Repeat num_cycles="3">
|
||||
<Sequence name="Drive in a square">
|
||||
<DriveOnHeading dist_to_travel="2.0" speed="0.2" time_allowance="12"/>
|
||||
<Spin spin_dist="1.570796" is_recovery="false"/>
|
||||
<DriveOnHeading dist_to_travel="2.0" speed="0.2" time_allowance="12"/>
|
||||
<Spin spin_dist="1.570796" is_recovery="false"/>
|
||||
<DriveOnHeading dist_to_travel="2.0" speed="0.2" time_allowance="12"/>
|
||||
<Spin spin_dist="1.570796" is_recovery="false"/>
|
||||
<DriveOnHeading dist_to_travel="2.0" speed="0.2" time_allowance="12"/>
|
||||
<Spin spin_dist="1.570796" is_recovery="false"/>
|
||||
</Sequence>
|
||||
</Repeat>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
348
src/navigation/cyy_navigation2/config/nav2_params.yaml
Normal file
348
src/navigation/cyy_navigation2/config/nav2_params.yaml
Normal file
@@ -0,0 +1,348 @@
|
||||
amcl:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
alpha1: 0.5
|
||||
alpha2: 0.5
|
||||
alpha3: 0.5
|
||||
alpha4: 0.5
|
||||
alpha5: 0.5
|
||||
base_frame_id: "base_link"
|
||||
beam_skip_distance: 0.5
|
||||
beam_skip_error_threshold: 0.9
|
||||
beam_skip_threshold: 0.3
|
||||
do_beamskip: true
|
||||
global_frame_id: "map"
|
||||
lambda_short: 0.1
|
||||
laser_likelihood_max_dist: 4.0
|
||||
laser_max_range: 100.0
|
||||
laser_min_range: -1.0
|
||||
laser_model_type: "likelihood_field"
|
||||
max_beams: 120
|
||||
max_particles: 4000
|
||||
min_particles: 1000
|
||||
odom_frame_id: "odom"
|
||||
pf_err: 0.01
|
||||
pf_z: 0.99
|
||||
recovery_alpha_fast: 0.0
|
||||
recovery_alpha_slow: 0.0
|
||||
resample_interval: 2
|
||||
robot_model_type: "nav2_amcl::DifferentialMotionModel"
|
||||
save_pose_rate: 0.5
|
||||
sigma_hit: 0.2
|
||||
tf_broadcast: true
|
||||
transform_tolerance: 1.0
|
||||
update_min_a: 0.2
|
||||
update_min_d: 0.25
|
||||
z_hit: 0.9
|
||||
z_max: 0.05
|
||||
z_rand: 0.1
|
||||
z_short: 0.05
|
||||
scan_topic: scan
|
||||
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
odom_topic: /odom
|
||||
bt_loop_duration: 10
|
||||
default_server_timeout: 20
|
||||
wait_for_service_timeout: 1000
|
||||
# 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
|
||||
# nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
|
||||
# nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
|
||||
# They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
|
||||
plugin_lib_names:
|
||||
- nav2_compute_path_to_pose_action_bt_node
|
||||
- nav2_compute_path_through_poses_action_bt_node
|
||||
- nav2_smooth_path_action_bt_node
|
||||
- nav2_follow_path_action_bt_node
|
||||
- nav2_spin_action_bt_node
|
||||
- nav2_wait_action_bt_node
|
||||
- nav2_assisted_teleop_action_bt_node
|
||||
- nav2_back_up_action_bt_node
|
||||
- nav2_drive_on_heading_bt_node
|
||||
- nav2_clear_costmap_service_bt_node
|
||||
- nav2_is_stuck_condition_bt_node
|
||||
- nav2_goal_reached_condition_bt_node
|
||||
- nav2_goal_updated_condition_bt_node
|
||||
- nav2_globally_updated_goal_condition_bt_node
|
||||
- nav2_is_path_valid_condition_bt_node
|
||||
- nav2_initial_pose_received_condition_bt_node
|
||||
- nav2_reinitialize_global_localization_service_bt_node
|
||||
- nav2_rate_controller_bt_node
|
||||
- nav2_distance_controller_bt_node
|
||||
- nav2_speed_controller_bt_node
|
||||
- nav2_truncate_path_action_bt_node
|
||||
- nav2_truncate_path_local_action_bt_node
|
||||
- nav2_goal_updater_node_bt_node
|
||||
- nav2_recovery_node_bt_node
|
||||
- nav2_pipeline_sequence_bt_node
|
||||
- nav2_round_robin_node_bt_node
|
||||
- nav2_transform_available_condition_bt_node
|
||||
- nav2_time_expired_condition_bt_node
|
||||
- nav2_path_expiring_timer_condition
|
||||
- nav2_distance_traveled_condition_bt_node
|
||||
- nav2_single_trigger_bt_node
|
||||
- nav2_goal_updated_controller_bt_node
|
||||
- nav2_is_battery_low_condition_bt_node
|
||||
- nav2_navigate_through_poses_action_bt_node
|
||||
- nav2_navigate_to_pose_action_bt_node
|
||||
- nav2_remove_passed_goals_action_bt_node
|
||||
- nav2_planner_selector_bt_node
|
||||
- nav2_controller_selector_bt_node
|
||||
- nav2_goal_checker_selector_bt_node
|
||||
- nav2_controller_cancel_bt_node
|
||||
- nav2_path_longer_on_approach_bt_node
|
||||
- nav2_wait_cancel_bt_node
|
||||
- nav2_spin_cancel_bt_node
|
||||
- nav2_back_up_cancel_bt_node
|
||||
- nav2_assisted_teleop_cancel_bt_node
|
||||
- nav2_drive_on_heading_cancel_bt_node
|
||||
- nav2_is_battery_charging_condition_bt_node
|
||||
|
||||
bt_navigator_navigate_through_poses_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
bt_navigator_navigate_to_pose_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
controller_frequency: 20.0
|
||||
min_x_velocity_threshold: 0.001
|
||||
min_y_velocity_threshold: 0.5
|
||||
min_theta_velocity_threshold: 0.001
|
||||
failure_tolerance: 0.3
|
||||
progress_checker_plugin: "progress_checker"
|
||||
goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
|
||||
controller_plugins: ["FollowPath"]
|
||||
|
||||
# Progress checker parameters
|
||||
progress_checker:
|
||||
plugin: "nav2_controller::SimpleProgressChecker"
|
||||
required_movement_radius: 0.5
|
||||
movement_time_allowance: 10.0
|
||||
# Goal checker parameters
|
||||
#precise_goal_checker:
|
||||
# plugin: "nav2_controller::SimpleGoalChecker"
|
||||
# xy_goal_tolerance: 0.25
|
||||
# yaw_goal_tolerance: 0.25
|
||||
# stateful: True
|
||||
general_goal_checker:
|
||||
stateful: True
|
||||
plugin: "nav2_controller::SimpleGoalChecker"
|
||||
xy_goal_tolerance: 0.25
|
||||
yaw_goal_tolerance: 0.25
|
||||
# DWB parameters
|
||||
FollowPath:
|
||||
plugin: "dwb_core::DWBLocalPlanner"
|
||||
debug_trajectory_details: True
|
||||
min_vel_x: 0.0
|
||||
min_vel_y: 0.0
|
||||
max_vel_x: 0.50
|
||||
max_vel_y: 0.0
|
||||
max_vel_theta: 3.0
|
||||
min_speed_xy: 0.0
|
||||
max_speed_xy: 0.50
|
||||
min_speed_theta: 0.0
|
||||
# Add high threshold velocity for turtlebot 3 issue.
|
||||
# https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
|
||||
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
|
||||
vy_samples: 5
|
||||
vtheta_samples: 20
|
||||
sim_time: 1.7
|
||||
linear_granularity: 0.05
|
||||
angular_granularity: 0.025
|
||||
transform_tolerance: 0.2
|
||||
xy_goal_tolerance: 0.25
|
||||
trans_stopped_velocity: 0.25
|
||||
short_circuit_trajectory_evaluation: True
|
||||
stateful: True
|
||||
critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
|
||||
BaseObstacle.scale: 0.02
|
||||
PathAlign.scale: 32.0
|
||||
PathAlign.forward_point_distance: 0.1
|
||||
GoalAlign.scale: 24.0
|
||||
GoalAlign.forward_point_distance: 0.1
|
||||
PathDist.scale: 32.0
|
||||
GoalDist.scale: 24.0
|
||||
RotateToGoal.scale: 32.0
|
||||
RotateToGoal.slowing_factor: 5.0
|
||||
RotateToGoal.lookahead_time: -1.0
|
||||
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 5.0
|
||||
publish_frequency: 2.0
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: True
|
||||
rolling_window: true
|
||||
width: 3
|
||||
height: 3
|
||||
resolution: 0.05
|
||||
robot_radius: 0.20
|
||||
plugins: ["voxel_layer", "inflation_layer"]
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.30
|
||||
voxel_layer:
|
||||
plugin: "nav2_costmap_2d::VoxelLayer"
|
||||
enabled: True
|
||||
publish_voxel_map: True
|
||||
origin_z: 0.0
|
||||
z_resolution: 0.05
|
||||
z_voxels: 16
|
||||
max_obstacle_height: 2.0
|
||||
mark_threshold: 0
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
always_send_full_costmap: True
|
||||
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 1.0
|
||||
publish_frequency: 1.0
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: True
|
||||
robot_radius: 0.20
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
|
||||
obstacle_layer:
|
||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
||||
enabled: True
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.30
|
||||
always_send_full_costmap: True
|
||||
|
||||
map_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
# Overridden in launch by the "map" launch configuration or provided default value.
|
||||
# To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below.
|
||||
yaml_filename: ""
|
||||
|
||||
map_saver:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
save_map_timeout: 5.0
|
||||
free_thresh_default: 0.25
|
||||
occupied_thresh_default: 0.65
|
||||
map_subscribe_transient_local: True
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
expected_planner_frequency: 20.0
|
||||
use_sim_time: True
|
||||
planner_plugins: ["GridBased"]
|
||||
GridBased:
|
||||
plugin: "nav2_navfn_planner/NavfnPlanner"
|
||||
tolerance: 0.5
|
||||
use_astar: false
|
||||
allow_unknown: true
|
||||
|
||||
smoother_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
smoother_plugins: ["simple_smoother"]
|
||||
simple_smoother:
|
||||
plugin: "nav2_smoother::SimpleSmoother"
|
||||
tolerance: 1.0e-10
|
||||
max_its: 1000
|
||||
do_refinement: True
|
||||
|
||||
behavior_server:
|
||||
ros__parameters:
|
||||
costmap_topic: local_costmap/costmap_raw
|
||||
footprint_topic: local_costmap/published_footprint
|
||||
cycle_frequency: 10.0
|
||||
behavior_plugins: ["backup", "drive_on_heading", "assisted_teleop", "wait"]
|
||||
backup:
|
||||
plugin: "nav2_behaviors/BackUp"
|
||||
drive_on_heading:
|
||||
plugin: "nav2_behaviors/DriveOnHeading"
|
||||
wait:
|
||||
plugin: "nav2_behaviors/Wait"
|
||||
assisted_teleop:
|
||||
plugin: "nav2_behaviors/AssistedTeleop"
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
transform_tolerance: 0.3
|
||||
use_sim_time: true
|
||||
simulate_ahead_time: 2.0
|
||||
max_rotational_vel: 1.0
|
||||
min_rotational_vel: 0.4
|
||||
rotational_acc_lim: 3.2
|
||||
|
||||
robot_state_publisher:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
waypoint_follower:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
loop_rate: 20
|
||||
stop_on_failure: false
|
||||
waypoint_task_executor_plugin: "wait_at_waypoint"
|
||||
wait_at_waypoint:
|
||||
plugin: "nav2_waypoint_follower::WaitAtWaypoint"
|
||||
enabled: True
|
||||
waypoint_pause_duration: 200
|
||||
|
||||
velocity_smoother:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
smoothing_frequency: 20.0
|
||||
scale_velocities: False
|
||||
feedback: "OPEN_LOOP"
|
||||
max_velocity: [0.26, 0.0, 1.0]
|
||||
min_velocity: [-0.26, 0.0, -1.0]
|
||||
max_accel: [2.5, 0.0, 3.2]
|
||||
max_decel: [-2.5, 0.0, -3.2]
|
||||
odom_topic: "odom"
|
||||
odom_duration: 0.1
|
||||
deadband_velocity: [0.0, 0.0, 0.0]
|
||||
velocity_timeout: 1.0
|
||||
108
src/navigation/cyy_navigation2/launch/car_bringup.launch.py
Normal file
108
src/navigation/cyy_navigation2/launch/car_bringup.launch.py
Normal file
@@ -0,0 +1,108 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
小车导航系统启动文件
|
||||
启动所有必要的硬件和导航节点
|
||||
"""
|
||||
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, TimerAction, LogInfo
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource, FrontendLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
from launch.conditions import IfCondition, UnlessCondition
|
||||
|
||||
def generate_launch_description():
|
||||
# 获取包路径
|
||||
cyy_navigation2_dir = get_package_share_directory('cyy_navigation2')
|
||||
|
||||
# 声明启动参数
|
||||
use_base = LaunchConfiguration('use_base', default='true')
|
||||
use_lidar = LaunchConfiguration('use_lidar', default='true')
|
||||
use_description = LaunchConfiguration('use_description', default='true')
|
||||
use_bridge = LaunchConfiguration('use_bridge', default='true')
|
||||
|
||||
# 启动参数列表
|
||||
launch_arguments = [
|
||||
DeclareLaunchArgument(
|
||||
'use_base',
|
||||
default_value='true',
|
||||
description='启动底盘控制'
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
'use_lidar',
|
||||
default_value='true',
|
||||
description='启动激光雷达'
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
'use_description',
|
||||
default_value='true',
|
||||
description='启动URDF显示'
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
'use_bridge',
|
||||
default_value='true',
|
||||
description='启动ROS Bridge'
|
||||
),
|
||||
]
|
||||
|
||||
# 创建启动描述
|
||||
ld = LaunchDescription(launch_arguments)
|
||||
|
||||
# 1. 启动底盘控制
|
||||
base_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([
|
||||
FindPackageShare('origincar_base'), '/launch/base_serial.launch.py'
|
||||
]),
|
||||
condition=IfCondition(use_base)
|
||||
)
|
||||
|
||||
# 添加延迟，确保底盘先启动
|
||||
ld.add_action(TimerAction(
|
||||
period=1.0,
|
||||
actions=[LogInfo(msg='启动底盘控制...'), base_launch]
|
||||
))
|
||||
|
||||
# 2. 启动激光雷达
|
||||
lidar_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([
|
||||
FindPackageShare('lslidar_driver'), '/launch/lsn10_launch.py'
|
||||
]),
|
||||
condition=IfCondition(use_lidar)
|
||||
)
|
||||
|
||||
ld.add_action(TimerAction(
|
||||
period=2.0,
|
||||
actions=[LogInfo(msg='启动激光雷达...'), lidar_launch]
|
||||
))
|
||||
|
||||
# 3. 启动URDF显示
|
||||
description_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([
|
||||
FindPackageShare('origincar_description'), '/launch/display.launch.py'
|
||||
]),
|
||||
condition=IfCondition(use_description)
|
||||
)
|
||||
|
||||
ld.add_action(TimerAction(
|
||||
period=3.0,
|
||||
actions=[LogInfo(msg='启动URDF显示...'), description_launch]
|
||||
))
|
||||
|
||||
# 4. 启动ROS Bridge
|
||||
bridge_launch = IncludeLaunchDescription(
|
||||
FrontendLaunchDescriptionSource([
|
||||
FindPackageShare('rosbridge_server'), '/launch/rosbridge_websocket_launch.xml'
|
||||
]),
|
||||
condition=IfCondition(use_bridge)
|
||||
)
|
||||
|
||||
ld.add_action(TimerAction(
|
||||
period=4.0,
|
||||
actions=[LogInfo(msg='启动ROS Bridge...'), bridge_launch]
|
||||
))
|
||||
|
||||
|
||||
return ld
|
||||
41
src/navigation/cyy_navigation2/launch/cyy_nav.launch.py
Normal file
41
src/navigation/cyy_navigation2/launch/cyy_nav.launch.py
Normal file
@@ -0,0 +1,41 @@
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import IncludeLaunchDescription
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
# =============================1.定位到包的地址=============================================================
|
||||
cyy_navigation2_dir = get_package_share_directory('cyy_navigation2')
|
||||
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
|
||||
slam = LaunchConfiguration('slam', default='True')
|
||||
# =============================2.声明参数，获取配置文件路径===================================================
|
||||
# use_sim_time 这里要设置成true,因为gazebo是仿真环境，其时间是通过/clock话题获取，而不是系统时间
|
||||
use_sim_time = LaunchConfiguration('use_sim_time', default='false')
|
||||
map_yaml_path = LaunchConfiguration('map',
|
||||
default=os.path.join(cyy_navigation2_dir, 'maps', 'cyy_map1.yaml'))
|
||||
nav2_param_path = LaunchConfiguration('params_file',
|
||||
default=os.path.join(cyy_navigation2_dir, 'param', 'nav2_params.yaml'))
|
||||
rviz_config_dir = os.path.join(nav2_bringup_dir, 'rviz', 'nav2_default_view.rviz')
|
||||
|
||||
# =============================3.声明启动launch文件，传入：地图路径、是否使用仿真时间以及nav2参数文件==============
|
||||
nav2_bringup_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([nav2_bringup_dir, '/launch', '/bringup_launch.py']),
|
||||
launch_arguments={
|
||||
'map': map_yaml_path,
|
||||
'use_sim_time': use_sim_time,
|
||||
'params_file': nav2_param_path,
|
||||
'slam': slam}.items(),
|
||||
)
|
||||
rviz_node = Node(
|
||||
package='rviz2',
|
||||
executable='rviz2',
|
||||
name='rviz2',
|
||||
arguments=['-d', rviz_config_dir],
|
||||
parameters=[{'use_sim_time': use_sim_time}],
|
||||
output='screen')
|
||||
|
||||
return LaunchDescription([nav2_bringup_launch])
|
||||
86
src/navigation/cyy_navigation2/launch/cyy_nav_box.launch.py
Normal file
86
src/navigation/cyy_navigation2/launch/cyy_nav_box.launch.py
Normal file
@@ -0,0 +1,86 @@
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def generate_launch_description():
|
||||
# 获取包路径
|
||||
cyy_navigation2_dir = get_package_share_directory('cyy_navigation2')
|
||||
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
|
||||
|
||||
# 声明参数
|
||||
use_sim_time = LaunchConfiguration('use_sim_time', default='False')
|
||||
slam = LaunchConfiguration('slam', default='False')
|
||||
localization = LaunchConfiguration('localization', default='False')
|
||||
|
||||
# 定义文件路径
|
||||
map_yaml_path = os.path.join(cyy_navigation2_dir, 'maps', 'cyy_map.yaml')
|
||||
nav2_param_path = os.path.join(cyy_navigation2_dir, 'param', 'nav2_params.yaml')
|
||||
slam_params_file = os.path.join(cyy_navigation2_dir, 'param', 'slam_toolbox_localization.yaml')
|
||||
|
||||
# 定义地图基础路径（用于 .posegraph 文件）
|
||||
map_base_path = os.path.join(cyy_navigation2_dir, 'maps', 'cyy_map')
|
||||
|
||||
# 检查 .posegraph 文件是否存在
|
||||
posegraph_path = map_base_path + '.posegraph'
|
||||
if not os.path.exists(posegraph_path):
|
||||
print(f"警告: .posegraph 文件不存在: {posegraph_path}")
|
||||
print("将使用 AMCL 替代")
|
||||
use_slam_toolbox = False
|
||||
else:
|
||||
use_slam_toolbox = True
|
||||
print(f"找到 .posegraph 文件: {posegraph_path}")
|
||||
|
||||
# 创建启动描述
|
||||
ld = LaunchDescription([
|
||||
DeclareLaunchArgument('use_sim_time', default_value='False'),
|
||||
DeclareLaunchArgument('slam', default_value='False'),
|
||||
DeclareLaunchArgument('localization', default_value='False'),
|
||||
])
|
||||
|
||||
# 如果使用 slam_toolbox
|
||||
if use_slam_toolbox:
|
||||
slam_toolbox_node = Node(
|
||||
package='slam_toolbox',
|
||||
executable='localization_slam_toolbox_node',
|
||||
name='slam_toolbox',
|
||||
output='screen',
|
||||
parameters=[
|
||||
slam_params_file,
|
||||
{'use_sim_time': use_sim_time},
|
||||
{'map_file_name': map_base_path},
|
||||
{'map_start_pose': [0.0, 0.0, 0.0]}
|
||||
],
|
||||
remappings=[('/scan', '/scan')]
|
||||
)
|
||||
ld.add_action(slam_toolbox_node)
|
||||
# 使用 slam_toolbox 时，Nav2 不启用定位
|
||||
localization_arg = 'False'
|
||||
else:
|
||||
# 使用 AMCL
|
||||
localization_arg = 'True'
|
||||
|
||||
# Nav2 启动 - 使用正确的路径
|
||||
nav2_launch_path = os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')
|
||||
if not os.path.exists(nav2_launch_path):
|
||||
print(f"错误: 找不到 Nav2 启动文件: {nav2_launch_path}")
|
||||
return ld
|
||||
|
||||
nav2_bringup_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(nav2_launch_path),
|
||||
launch_arguments={
|
||||
'map': map_yaml_path,
|
||||
'use_sim_time': use_sim_time,
|
||||
'params_file': nav2_param_path,
|
||||
'slam': slam,
|
||||
'localization': localization_arg,
|
||||
'autostart': 'True',
|
||||
}.items(),
|
||||
)
|
||||
|
||||
ld.add_action(nav2_bringup_launch)
|
||||
|
||||
return ld
|
||||
BIN
src/navigation/cyy_navigation2/maps/cyy_map.data
Normal file
BIN
src/navigation/cyy_navigation2/maps/cyy_map.data
Normal file
Binary file not shown.
BIN
src/navigation/cyy_navigation2/maps/cyy_map.pgm
Normal file
BIN
src/navigation/cyy_navigation2/maps/cyy_map.pgm
Normal file
Binary file not shown.
7
src/navigation/cyy_navigation2/maps/cyy_map.yaml
Normal file
7
src/navigation/cyy_navigation2/maps/cyy_map.yaml
Normal file
@@ -0,0 +1,7 @@
|
||||
image: cyy_map.pgm
|
||||
mode: trinary
|
||||
resolution: 0.01
|
||||
origin: [-2.27, -1.63, 0]
|
||||
negate: 0
|
||||
occupied_thresh: 0.65
|
||||
free_thresh: 0.25
|
||||
BIN
src/navigation/cyy_navigation2/maps/cyy_map1.pgm
Normal file
BIN
src/navigation/cyy_navigation2/maps/cyy_map1.pgm
Normal file
Binary file not shown.
7
src/navigation/cyy_navigation2/maps/cyy_map1.yaml
Normal file
7
src/navigation/cyy_navigation2/maps/cyy_map1.yaml
Normal file
@@ -0,0 +1,7 @@
|
||||
image: cyy_map1.pgm
|
||||
mode: trinary
|
||||
resolution: 0.01
|
||||
origin: [-3.49, -3.24, 0]
|
||||
negate: 0
|
||||
occupied_thresh: 0.65
|
||||
free_thresh: 0.25
|
||||
20
src/navigation/cyy_navigation2/package.xml
Normal file
20
src/navigation/cyy_navigation2/package.xml
Normal file
@@ -0,0 +1,20 @@
|
||||
<?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>cyy_navigation2</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="root@todo.todo">root</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>nav2_bringup</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
353
src/navigation/cyy_navigation2/param/nav2_params.yaml
Normal file
353
src/navigation/cyy_navigation2/param/nav2_params.yaml
Normal file
@@ -0,0 +1,353 @@
|
||||
amcl:
|
||||
ros__parameters:
|
||||
enabled: False
|
||||
use_sim_time: True
|
||||
alpha1: 0.2
|
||||
alpha2: 0.2
|
||||
alpha3: 0.2
|
||||
alpha4: 0.2
|
||||
alpha5: 0.2
|
||||
base_frame_id: "base_link"
|
||||
beam_skip_distance: 0.5
|
||||
beam_skip_error_threshold: 0.9
|
||||
beam_skip_threshold: 0.3
|
||||
do_beamskip: false
|
||||
global_frame_id: "map"
|
||||
lambda_short: 0.1
|
||||
laser_likelihood_max_dist: 2.0
|
||||
laser_max_range: 100.0
|
||||
laser_min_range: -1.0
|
||||
laser_model_type: "likelihood_field"
|
||||
max_beams: 60
|
||||
max_particles: 2000
|
||||
min_particles: 500
|
||||
odom_frame_id: "odom"
|
||||
pf_err: 0.05
|
||||
pf_z: 0.99
|
||||
recovery_alpha_fast: 0.0
|
||||
recovery_alpha_slow: 0.0
|
||||
resample_interval: 1
|
||||
robot_model_type: "nav2_amcl::DifferentialMotionModel"
|
||||
save_pose_rate: 0.5
|
||||
sigma_hit: 0.2
|
||||
tf_broadcast: true
|
||||
transform_tolerance: 1.0
|
||||
update_min_a: 0.2
|
||||
update_min_d: 0.25
|
||||
z_hit: 0.5
|
||||
z_max: 0.05
|
||||
z_rand: 0.5
|
||||
z_short: 0.05
|
||||
scan_topic: scan
|
||||
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
odom_topic: /odom
|
||||
bt_loop_duration: 10
|
||||
default_server_timeout: 20
|
||||
wait_for_service_timeout: 1000
|
||||
# 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
|
||||
# nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
|
||||
# nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
|
||||
# They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
|
||||
plugin_lib_names:
|
||||
- nav2_compute_path_to_pose_action_bt_node
|
||||
- nav2_compute_path_through_poses_action_bt_node
|
||||
- nav2_smooth_path_action_bt_node
|
||||
- nav2_follow_path_action_bt_node
|
||||
- nav2_spin_action_bt_node
|
||||
- nav2_wait_action_bt_node
|
||||
- nav2_assisted_teleop_action_bt_node
|
||||
- nav2_back_up_action_bt_node
|
||||
- nav2_drive_on_heading_bt_node
|
||||
- nav2_clear_costmap_service_bt_node
|
||||
- nav2_is_stuck_condition_bt_node
|
||||
- nav2_goal_reached_condition_bt_node
|
||||
- nav2_goal_updated_condition_bt_node
|
||||
- nav2_globally_updated_goal_condition_bt_node
|
||||
- nav2_is_path_valid_condition_bt_node
|
||||
- nav2_initial_pose_received_condition_bt_node
|
||||
- nav2_reinitialize_global_localization_service_bt_node
|
||||
- nav2_rate_controller_bt_node
|
||||
- nav2_distance_controller_bt_node
|
||||
- nav2_speed_controller_bt_node
|
||||
- nav2_truncate_path_action_bt_node
|
||||
- nav2_truncate_path_local_action_bt_node
|
||||
- nav2_goal_updater_node_bt_node
|
||||
- nav2_recovery_node_bt_node
|
||||
- nav2_pipeline_sequence_bt_node
|
||||
- nav2_round_robin_node_bt_node
|
||||
- nav2_transform_available_condition_bt_node
|
||||
- nav2_time_expired_condition_bt_node
|
||||
- nav2_path_expiring_timer_condition
|
||||
- nav2_distance_traveled_condition_bt_node
|
||||
- nav2_single_trigger_bt_node
|
||||
- nav2_goal_updated_controller_bt_node
|
||||
- nav2_is_battery_low_condition_bt_node
|
||||
- nav2_navigate_through_poses_action_bt_node
|
||||
- nav2_navigate_to_pose_action_bt_node
|
||||
- nav2_remove_passed_goals_action_bt_node
|
||||
- nav2_planner_selector_bt_node
|
||||
- nav2_controller_selector_bt_node
|
||||
- nav2_goal_checker_selector_bt_node
|
||||
- nav2_controller_cancel_bt_node
|
||||
- nav2_path_longer_on_approach_bt_node
|
||||
- nav2_wait_cancel_bt_node
|
||||
- nav2_spin_cancel_bt_node
|
||||
- nav2_back_up_cancel_bt_node
|
||||
- nav2_assisted_teleop_cancel_bt_node
|
||||
- nav2_drive_on_heading_cancel_bt_node
|
||||
- nav2_is_battery_charging_condition_bt_node
|
||||
|
||||
bt_navigator_navigate_through_poses_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
bt_navigator_navigate_to_pose_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
controller_frequency: 20.0
|
||||
min_x_velocity_threshold: 0.001
|
||||
min_y_velocity_threshold: 0.5
|
||||
min_theta_velocity_threshold: 0.001
|
||||
failure_tolerance: 0.3
|
||||
progress_checker_plugin: "progress_checker"
|
||||
goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
|
||||
controller_plugins: ["FollowPath"]
|
||||
|
||||
# Progress checker parameters
|
||||
progress_checker:
|
||||
plugin: "nav2_controller::SimpleProgressChecker"
|
||||
required_movement_radius: 0.5
|
||||
movement_time_allowance: 10.0
|
||||
# Goal checker parameters
|
||||
#precise_goal_checker:
|
||||
# plugin: "nav2_controller::SimpleGoalChecker"
|
||||
# xy_goal_tolerance: 0.25
|
||||
# yaw_goal_tolerance: 0.25
|
||||
# stateful: True
|
||||
general_goal_checker:
|
||||
stateful: True
|
||||
plugin: "nav2_controller::SimpleGoalChecker"
|
||||
xy_goal_tolerance: 0.25
|
||||
yaw_goal_tolerance: 6.28
|
||||
# DWB parameters
|
||||
FollowPath:
|
||||
plugin: "dwb_core::DWBLocalPlanner"
|
||||
debug_trajectory_details: True
|
||||
min_vel_x: -0.50
|
||||
min_vel_y: 0.0
|
||||
max_vel_x: 0.50
|
||||
max_vel_y: 0.0
|
||||
max_vel_theta: 1.0
|
||||
min_speed_xy: 0.0
|
||||
max_speed_xy: 0.50
|
||||
min_speed_theta: 0.0
|
||||
# Add high threshold velocity for turtlebot 3 issue.
|
||||
# https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
|
||||
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
|
||||
vy_samples: 0
|
||||
vtheta_samples: 20
|
||||
sim_time: 1.7
|
||||
linear_granularity: 0.05
|
||||
angular_granularity: 0.025
|
||||
transform_tolerance: 0.2
|
||||
xy_goal_tolerance: 0.25
|
||||
trans_stopped_velocity: 0.25
|
||||
short_circuit_trajectory_evaluation: True
|
||||
stateful: True
|
||||
critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
|
||||
BaseObstacle.scale: 0.01
|
||||
PathAlign.scale: 32.0
|
||||
PathAlign.forward_point_distance: 0.1
|
||||
GoalAlign.scale: 24.0
|
||||
GoalAlign.forward_point_distance: 0.1
|
||||
PathDist.scale: 32.0
|
||||
GoalDist.scale: 24.0
|
||||
RotateToGoal.scale: 32.0
|
||||
RotateToGoal.slowing_factor: 5.0
|
||||
RotateToGoal.lookahead_time: -1.0
|
||||
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 5.0
|
||||
publish_frequency: 2.0
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: True
|
||||
rolling_window: true
|
||||
width: 3
|
||||
height: 3
|
||||
resolution: 0.05
|
||||
footprint: "[[0.2, 0.15], [0.2, -0.15], [-0.2, -0.15], [-0.2, 0.15]]"
|
||||
robot_radius: 0.00
|
||||
plugins: ["voxel_layer", "inflation_layer"]
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.10
|
||||
voxel_layer:
|
||||
plugin: "nav2_costmap_2d::VoxelLayer"
|
||||
enabled: True
|
||||
publish_voxel_map: True
|
||||
origin_z: 0.0
|
||||
z_resolution: 0.05
|
||||
z_voxels: 16
|
||||
max_obstacle_height: 2.0
|
||||
mark_threshold: 0
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
always_send_full_costmap: True
|
||||
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 1.0
|
||||
publish_frequency: 1.0
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: True
|
||||
footprint: "[[0.2, 0.15], [0.2, -0.15], [-0.2, -0.15], [-0.2, 0.15]]"
|
||||
robot_radius: 0.0
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
|
||||
obstacle_layer:
|
||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
||||
enabled: True
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.30
|
||||
always_send_full_costmap: True
|
||||
|
||||
map_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
# Overridden in launch by the "map" launch configuration or provided default value.
|
||||
# To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below.
|
||||
yaml_filename: ""
|
||||
|
||||
map_saver:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
save_map_timeout: 5.0
|
||||
free_thresh_default: 0.25
|
||||
occupied_thresh_default: 0.65
|
||||
map_subscribe_transient_local: True
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
expected_planner_frequency: 20.0
|
||||
use_sim_time: True
|
||||
planner_plugins: ["GridBased"]
|
||||
GridBased:
|
||||
plugin: "nav2_navfn_planner/NavfnPlanner"
|
||||
tolerance: 0.5
|
||||
use_astar: false
|
||||
allow_unknown: true
|
||||
|
||||
smoother_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
smoother_plugins: ["simple_smoother"]
|
||||
simple_smoother:
|
||||
plugin: "nav2_smoother::SimpleSmoother"
|
||||
tolerance: 1.0e-10
|
||||
max_its: 1000
|
||||
do_refinement: True
|
||||
|
||||
behavior_server:
|
||||
ros__parameters:
|
||||
costmap_topic: local_costmap/costmap_raw
|
||||
footprint_topic: local_costmap/published_footprint
|
||||
cycle_frequency: 10.0
|
||||
behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
|
||||
spin:
|
||||
plugin: "nav2_behaviors/Spin"
|
||||
backup:
|
||||
plugin: "nav2_behaviors/BackUp"
|
||||
drive_on_heading:
|
||||
plugin: "nav2_behaviors/DriveOnHeading"
|
||||
wait:
|
||||
plugin: "nav2_behaviors/Wait"
|
||||
assisted_teleop:
|
||||
plugin: "nav2_behaviors/AssistedTeleop"
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
transform_tolerance: 0.3
|
||||
use_sim_time: true
|
||||
simulate_ahead_time: 2.0
|
||||
max_rotational_vel: 1.0
|
||||
min_rotational_vel: 0.4
|
||||
rotational_acc_lim: 3.2
|
||||
|
||||
robot_state_publisher:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
waypoint_follower:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
loop_rate: 20
|
||||
stop_on_failure: false
|
||||
waypoint_task_executor_plugin: "wait_at_waypoint"
|
||||
wait_at_waypoint:
|
||||
plugin: "nav2_waypoint_follower::WaitAtWaypoint"
|
||||
enabled: True
|
||||
waypoint_pause_duration: 200
|
||||
|
||||
velocity_smoother:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
smoothing_frequency: 20.0
|
||||
scale_velocities: False
|
||||
feedback: "OPEN_LOOP"
|
||||
max_velocity: [0.26, 0.0, 1.0]
|
||||
min_velocity: [-0.26, 0.0, -1.0]
|
||||
max_accel: [2.5, 0.0, 3.2]
|
||||
max_decel: [-2.5, 0.0, -3.2]
|
||||
odom_topic: "odom"
|
||||
odom_duration: 0.1
|
||||
deadband_velocity: [0.0, 0.0, 0.0]
|
||||
velocity_timeout: 1.0
|
||||
@@ -0,0 +1,54 @@
|
||||
slam_toolbox:
|
||||
ros__parameters:
|
||||
# 核心模式设置
|
||||
mode: "localization" # 重要：设置为定位模式
|
||||
map_file_name: "" # 由启动参数传入
|
||||
map_start_pose: [0.0, 0.0, 0.0]
|
||||
# ROS框架设置
|
||||
odom_frame: "odom"
|
||||
map_frame: "map"
|
||||
base_frame: "base_link"
|
||||
scan_topic: "/scan"
|
||||
|
||||
# 求解器设置
|
||||
solver_plugin: "solver_plugins::CeresSolver"
|
||||
ceres_linear_solver: "SPARSE_NORMAL_CHOLESKY"
|
||||
ceres_preconditioner: "SCHUR_JACOBI"
|
||||
ceres_trust_strategy: "LEVENBERG_MARQUARDT"
|
||||
ceres_dogleg_type: "TRADITIONAL_DOGLEG"
|
||||
ceres_loss_function: "None"
|
||||
|
||||
# 定位优化参数
|
||||
use_scan_matching: true
|
||||
use_scan_barycenter: true
|
||||
minimum_travel_distance: 0.05
|
||||
minimum_travel_heading: 0.087
|
||||
|
||||
# 扫描处理
|
||||
scan_buffer_size: 20
|
||||
throttle_scans: 1
|
||||
|
||||
# 回环检测（定位模式可关闭）
|
||||
do_loop_closing: false
|
||||
|
||||
# 搜索参数
|
||||
link_match_minimum_response_fine: 0.2
|
||||
link_scan_maximum_distance: 2.0
|
||||
loop_search_maximum_distance: 5.0
|
||||
|
||||
# 性能参数
|
||||
minimum_time_interval: 0.1
|
||||
transform_publish_period: 0.02
|
||||
map_update_interval: 0.5
|
||||
|
||||
# 地图参数
|
||||
resolution: 0.05
|
||||
max_laser_range: 15.0
|
||||
min_laser_range: 0.2
|
||||
|
||||
# 初始位姿（可选，可以通过RViz设置）
|
||||
# map_start_pose: [0.0, 0.0, 0.0]
|
||||
|
||||
# 调试
|
||||
debug_logging: false
|
||||
enable_interactive_mode: true
|
||||
61
src/navigation/cyy_slamtoolbox/CMakeLists.txt
Normal file
61
src/navigation/cyy_slamtoolbox/CMakeLists.txt
Normal file
@@ -0,0 +1,61 @@
|
||||
# 设置CMake最低版本要求
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
# 定义项目名称（应与package.xml中的<name>一致）
|
||||
project(cyy_slamtoolbox)
|
||||
|
||||
# 如果未设置，默认使用C++17标准
|
||||
if(NOT CMAKE_CXX_STANDARD)
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
endif()
|
||||
|
||||
# 查找编译依赖
|
||||
find_package(ament_cmake REQUIRED)
|
||||
# 查找slam_toolbox包
|
||||
find_package(slam_toolbox REQUIRED)
|
||||
# 查找其他可能的依赖，如传感器消息、几何库等
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(sensor_msgs REQUIRED)
|
||||
find_package(nav_msgs REQUIRED)
|
||||
find_package(tf2 REQUIRED)
|
||||
find_package(tf2_ros REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
|
||||
# 消息输出，用于调试
|
||||
message(STATUS "开始配置项目: ${PROJECT_NAME}")
|
||||
|
||||
# 包含头文件目录
|
||||
include_directories(
|
||||
include
|
||||
${ament_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
# 如果有自定义的C++节点源文件，在此添加可执行文件
|
||||
# 例如，如果您有自定义的SLAM处理节点：
|
||||
# add_executable(origincar_slam_node src/origincar_slam_node.cpp)
|
||||
# ament_target_dependencies(origincar_slam_node
|
||||
# rclcpp std_msgs sensor_msgs nav_msgs tf2 tf2_ros geometry_msgs slam_toolbox)
|
||||
#
|
||||
# 安装目标（将可执行文件安装到lib/<package_name>）
|
||||
# install(TARGETS origincar_slam_node
|
||||
# DESTINATION lib/${PROJECT_NAME})
|
||||
|
||||
# 安装launch文件到share/<package_name>/launch目录
|
||||
install(DIRECTORY launch
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
PATTERN "*.py"
|
||||
)
|
||||
|
||||
# 安装配置文件（如YAML参数文件）到share/<package_name>/config目录
|
||||
install(DIRECTORY config
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
PATTERN "*.yaml"
|
||||
PATTERN "*.yml"
|
||||
)
|
||||
|
||||
|
||||
# 声明ament包依赖关系（确保在package.xml中已声明这些依赖）
|
||||
ament_export_dependencies(slam_toolbox rclcpp std_msgs sensor_msgs nav_msgs tf2 tf2_ros geometry_msgs)
|
||||
|
||||
# 必须调用此函数来生成必要的CMake配置
|
||||
ament_package()
|
||||
@@ -0,0 +1,9 @@
|
||||
scan_to_scan_filter_chain:
|
||||
ros__parameters:
|
||||
filter1:
|
||||
name: angle
|
||||
type: laser_filters/LaserScanAngularBoundsFilter
|
||||
params:
|
||||
lower_angle: -1.52
|
||||
upper_angle: 1.52
|
||||
|
||||
@@ -0,0 +1,15 @@
|
||||
scan_to_scan_filter_chain:
|
||||
ros__parameters:
|
||||
filter1:
|
||||
name: box_filter
|
||||
type: laser_filters/LaserScanBoxFilter
|
||||
params:
|
||||
box_frame: base_link
|
||||
max_x: 0.5
|
||||
max_y: 0.5
|
||||
max_z: 0.5
|
||||
min_x: -0.5
|
||||
min_y: -0.5
|
||||
min_z: -0.5
|
||||
|
||||
invert: false # activate to remove all points outside of the box
|
||||
@@ -0,0 +1,7 @@
|
||||
scan_to_scan_filter_chain:
|
||||
ros__parameters:
|
||||
filter1:
|
||||
name: footprint_filter
|
||||
type: laser_filters/LaserScanFootprintFilter
|
||||
params:
|
||||
inscribed_radius: 0.325
|
||||
@@ -0,0 +1,9 @@
|
||||
scan_to_scan_filter_chain:
|
||||
ros__parameters:
|
||||
filter1:
|
||||
name: intensity
|
||||
type: laser_filters/LaserScanIntensityFilter
|
||||
params:
|
||||
lower_threshold: 8000.
|
||||
upper_threshold: 100000.
|
||||
disp_histogram: 0
|
||||
@@ -0,0 +1,19 @@
|
||||
scan_filter_chain:
|
||||
- name: angle_bounds
|
||||
type: laser_filters/LaserScanAngularBoundsFilter
|
||||
params:
|
||||
# 示例：如果您只想处理前方180度的数据，可以设置为：
|
||||
lower_angle: -1.57 # 约-90度
|
||||
upper_angle: 1.57 # 约90度
|
||||
|
||||
- name: range_filter
|
||||
type: laser_filters/LaserScanRangeFilter
|
||||
params:
|
||||
# 过滤掉无效的距离值，这对于SLAM非常重要
|
||||
lower_threshold: 0.2 # 忽略小于0.2米的测量（如雷达自身的噪声）
|
||||
upper_threshold: 20.0 # 忽略大于20.0米的测量（通常是不稳定的数据）
|
||||
use_message_range_limits: false
|
||||
|
||||
- name: interpolation
|
||||
type: laser_filters/InterpolationFilter
|
||||
# 此滤波器没有参数，它会自动对因过滤产生的无效值（如NaN）进行插值修复[1](@ref)
|
||||
@@ -0,0 +1,87 @@
|
||||
|
||||
slam_toolbox:
|
||||
ros__parameters:
|
||||
|
||||
# Plugin params
|
||||
solver_plugin: solver_plugins::CeresSolver
|
||||
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
|
||||
ceres_preconditioner: SCHUR_JACOBI
|
||||
ceres_trust_strategy: LEVENBERG_MARQUARDT
|
||||
ceres_dogleg_type: TRADITIONAL_DOGLEG
|
||||
ceres_loss_function: None
|
||||
|
||||
# ROS Parameters
|
||||
odom_frame: odom
|
||||
map_frame: map
|
||||
base_frame: base_footprint
|
||||
scan_topic: /scan
|
||||
use_map_saver: true
|
||||
mode: mapping
|
||||
|
||||
# lifelong params
|
||||
lifelong_search_use_tree: false
|
||||
lifelong_minimum_score: 0.1
|
||||
lifelong_iou_match: 0.85
|
||||
lifelong_node_removal_score: 0.04
|
||||
lifelong_overlap_score_scale: 0.06
|
||||
lifelong_constraint_multiplier: 0.08
|
||||
lifelong_nearby_penalty: 0.001
|
||||
lifelong_candidates_scale: 0.03
|
||||
|
||||
# if you'd like to immediately start continuing a map at a given pose
|
||||
# or at the dock, but they are mutually exclusive, if pose is given
|
||||
# will use pose
|
||||
#map_file_name: test_steve
|
||||
#map_start_pose: [0.0, 0.0, 0.0]
|
||||
#map_start_at_dock: true
|
||||
|
||||
debug_logging: false
|
||||
throttle_scans: 1
|
||||
transform_publish_period: 0.02 #if 0 never publishes odometry
|
||||
map_update_interval: 5.0
|
||||
resolution: 0.05
|
||||
min_laser_range: 0.0 #for rastering images
|
||||
max_laser_range: 20.0 #for rastering images
|
||||
minimum_time_interval: 0.5
|
||||
transform_timeout: 0.2
|
||||
tf_buffer_duration: 10.
|
||||
stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
|
||||
|
||||
# General Parameters
|
||||
use_scan_matching: true
|
||||
use_scan_barycenter: true
|
||||
minimum_travel_distance: 0.5
|
||||
minimum_travel_heading: 0.5
|
||||
scan_buffer_size: 10
|
||||
scan_buffer_maximum_scan_distance: 10.0
|
||||
link_match_minimum_response_fine: 0.1
|
||||
link_scan_maximum_distance: 1.5
|
||||
loop_search_maximum_distance: 3.0
|
||||
do_loop_closing: true
|
||||
loop_match_minimum_chain_size: 10
|
||||
loop_match_maximum_variance_coarse: 3.0
|
||||
loop_match_minimum_response_coarse: 0.35
|
||||
loop_match_minimum_response_fine: 0.45
|
||||
|
||||
# Correlation Parameters - Correlation Parameters
|
||||
correlation_search_space_dimension: 0.5
|
||||
correlation_search_space_resolution: 0.01
|
||||
correlation_search_space_smear_deviation: 0.1
|
||||
|
||||
# Correlation Parameters - Loop Closure Parameters
|
||||
loop_search_space_dimension: 8.0
|
||||
loop_search_space_resolution: 0.05
|
||||
loop_search_space_smear_deviation: 0.03
|
||||
|
||||
# Scan Matcher Parameters
|
||||
distance_variance_penalty: 0.5
|
||||
angle_variance_penalty: 1.0
|
||||
|
||||
fine_search_angle_offset: 0.00349
|
||||
coarse_search_angle_offset: 0.349
|
||||
coarse_angle_resolution: 0.0349
|
||||
minimum_angle_penalty: 0.9
|
||||
minimum_distance_penalty: 0.5
|
||||
use_response_expansion: true
|
||||
min_pass_through: 2
|
||||
occupancy_threshold: 0.1
|
||||
@@ -0,0 +1,70 @@
|
||||
slam_toolbox:
|
||||
ros__parameters:
|
||||
solver_plugin: solver_plugins::CeresSolver
|
||||
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
|
||||
ceres_preconditioner: SCHUR_JACOBI
|
||||
ceres_trust_strategy: LEVENBERG_MARQUARDT
|
||||
ceres_dogleg_type: TRADITIONAL_DOGLEG
|
||||
ceres_loss_function: None
|
||||
|
||||
# ROS Parameters
|
||||
odom_frame: odom
|
||||
map_frame: map
|
||||
base_frame: base_link
|
||||
scan_topic: /scan
|
||||
mode: localization #localization
|
||||
|
||||
# if you'd like to start localizing on bringup in a map and pose
|
||||
#map_file_name: test_steve
|
||||
#map_start_pose: [5.0, 1.0, 0.0]
|
||||
|
||||
debug_logging: false
|
||||
throttle_scans: 1
|
||||
transform_publish_period: 0.02 #if 0 never publishes odometry
|
||||
map_update_interval: 5.0
|
||||
resolution: 0.05
|
||||
min_laser_range: 0.0 #for rastering images
|
||||
max_laser_range: 20.0 #for rastering images
|
||||
minimum_time_interval: 0.5
|
||||
transform_timeout: 0.2
|
||||
tf_buffer_duration: 30.
|
||||
stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
|
||||
|
||||
# General Parameters
|
||||
use_scan_matching: true
|
||||
use_scan_barycenter: true
|
||||
minimum_travel_distance: 0.5
|
||||
minimum_travel_heading: 0.5
|
||||
scan_buffer_size: 3
|
||||
scan_buffer_maximum_scan_distance: 10.0
|
||||
link_match_minimum_response_fine: 0.1
|
||||
link_scan_maximum_distance: 1.5
|
||||
do_loop_closing: true
|
||||
loop_match_minimum_chain_size: 3
|
||||
loop_match_maximum_variance_coarse: 3.0
|
||||
loop_match_minimum_response_coarse: 0.35
|
||||
loop_match_minimum_response_fine: 0.45
|
||||
|
||||
# Correlation Parameters - Correlation Parameters
|
||||
correlation_search_space_dimension: 0.5
|
||||
correlation_search_space_resolution: 0.01
|
||||
correlation_search_space_smear_deviation: 0.1
|
||||
|
||||
# Correlation Parameters - Loop Closure Parameters
|
||||
loop_search_space_dimension: 8.0
|
||||
loop_search_space_resolution: 0.05
|
||||
loop_search_space_smear_deviation: 0.03
|
||||
loop_search_maximum_distance: 3.0
|
||||
|
||||
# Scan Matcher Parameters
|
||||
distance_variance_penalty: 0.5
|
||||
angle_variance_penalty: 1.0
|
||||
|
||||
fine_search_angle_offset: 0.00349
|
||||
coarse_search_angle_offset: 0.349
|
||||
coarse_angle_resolution: 0.0349
|
||||
minimum_angle_penalty: 0.9
|
||||
minimum_distance_penalty: 0.5
|
||||
use_response_expansion: true
|
||||
min_pass_through: 2
|
||||
occupancy_threshold: 0.1
|
||||
@@ -0,0 +1,69 @@
|
||||
slam_toolbox:
|
||||
ros__parameters:
|
||||
|
||||
# Plugin params
|
||||
solver_plugin: solver_plugins::CeresSolver
|
||||
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
|
||||
ceres_preconditioner: SCHUR_JACOBI
|
||||
ceres_trust_strategy: LEVENBERG_MARQUARDT
|
||||
ceres_dogleg_type: TRADITIONAL_DOGLEG
|
||||
ceres_loss_function: None
|
||||
|
||||
# ROS Parameters
|
||||
odom_frame: odom
|
||||
map_frame: map
|
||||
base_frame: base_footprint
|
||||
scan_topic: /scan
|
||||
use_map_saver: true
|
||||
mode: mapping #localization
|
||||
debug_logging: false
|
||||
throttle_scans: 1
|
||||
transform_publish_period: 0.02 #if 0 never publishes odometry
|
||||
map_update_interval: 10.0
|
||||
resolution: 0.05
|
||||
min_laser_range: 0.0 #for rastering images
|
||||
max_laser_range: 20.0 #for rastering images
|
||||
minimum_time_interval: 0.5
|
||||
transform_timeout: 0.2
|
||||
tf_buffer_duration: 14400.
|
||||
stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
|
||||
enable_interactive_mode: true
|
||||
|
||||
# General Parameters
|
||||
use_scan_matching: true
|
||||
use_scan_barycenter: true
|
||||
minimum_travel_distance: 0.5
|
||||
minimum_travel_heading: 0.5
|
||||
scan_buffer_size: 10
|
||||
scan_buffer_maximum_scan_distance: 10.0
|
||||
link_match_minimum_response_fine: 0.1
|
||||
link_scan_maximum_distance: 1.5
|
||||
loop_search_maximum_distance: 3.0
|
||||
do_loop_closing: true
|
||||
loop_match_minimum_chain_size: 10
|
||||
loop_match_maximum_variance_coarse: 3.0
|
||||
loop_match_minimum_response_coarse: 0.35
|
||||
loop_match_minimum_response_fine: 0.45
|
||||
|
||||
# Correlation Parameters - Correlation Parameters
|
||||
correlation_search_space_dimension: 0.5
|
||||
correlation_search_space_resolution: 0.01
|
||||
correlation_search_space_smear_deviation: 0.1
|
||||
|
||||
# Correlation Parameters - Loop Closure Parameters
|
||||
loop_search_space_dimension: 8.0
|
||||
loop_search_space_resolution: 0.05
|
||||
loop_search_space_smear_deviation: 0.03
|
||||
|
||||
# Scan Matcher Parameters
|
||||
distance_variance_penalty: 0.5
|
||||
angle_variance_penalty: 1.0
|
||||
|
||||
fine_search_angle_offset: 0.00349
|
||||
coarse_search_angle_offset: 0.349
|
||||
coarse_angle_resolution: 0.0349
|
||||
minimum_angle_penalty: 0.9
|
||||
minimum_distance_penalty: 0.5
|
||||
use_response_expansion: true
|
||||
min_pass_through: 2
|
||||
occupancy_threshold: 0.1
|
||||
@@ -0,0 +1,78 @@
|
||||
slam_toolbox:
|
||||
ros__parameters:
|
||||
|
||||
# Plugin params
|
||||
solver_plugin: solver_plugins::CeresSolver
|
||||
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
|
||||
ceres_preconditioner: SCHUR_JACOBI
|
||||
ceres_trust_strategy: LEVENBERG_MARQUARDT
|
||||
ceres_dogleg_type: TRADITIONAL_DOGLEG
|
||||
ceres_loss_function: None
|
||||
|
||||
# ROS Parameters
|
||||
odom_frame: odom
|
||||
map_frame: map
|
||||
base_frame: base_link
|
||||
scan_topic: /scan
|
||||
use_map_saver: true
|
||||
mode: mapping #localization
|
||||
samples: 440
|
||||
# if you'd like to immediately start continuing a map at a given pose
|
||||
# or at the dock, but they are mutually exclusive, if pose is given
|
||||
# will use pose
|
||||
#map_file_name: test_steve
|
||||
# map_start_pose: [0.0, 0.0, 0.0]
|
||||
#map_start_at_dock: true
|
||||
|
||||
debug_logging: false
|
||||
throttle_scans: 5
|
||||
transform_publish_period: 0.02 #if 0 never publishes odometry
|
||||
map_update_interval: 0.01
|
||||
resolution: 0.01
|
||||
min_laser_range: 0.21 #for rastering images
|
||||
max_laser_range: 15.0 #for rastering images
|
||||
minimum_time_interval: 0.1
|
||||
transform_timeout: 2.0
|
||||
tf_buffer_duration: 30.
|
||||
stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
|
||||
enable_interactive_mode: true
|
||||
|
||||
# General Parameters
|
||||
use_scan_matching: true
|
||||
use_scan_barycenter: true
|
||||
minimum_travel_distance: 0.2
|
||||
minimum_travel_heading: 0.2
|
||||
scan_buffer_size: 10
|
||||
samples: 440
|
||||
scan_buffer_maximum_scan_distance: 10.0
|
||||
link_match_minimum_response_fine: 0.1
|
||||
link_scan_maximum_distance: 1.5
|
||||
loop_search_maximum_distance: 3.0
|
||||
do_loop_closing: true
|
||||
loop_match_minimum_chain_size: 10
|
||||
loop_match_maximum_variance_coarse: 3.0
|
||||
loop_match_minimum_response_coarse: 0.35
|
||||
loop_match_minimum_response_fine: 0.45
|
||||
|
||||
# Correlation Parameters - Correlation Parameters
|
||||
correlation_search_space_dimension: 0.5
|
||||
correlation_search_space_resolution: 0.05
|
||||
correlation_search_space_smear_deviation: 0.1
|
||||
|
||||
# Correlation Parameters - Loop Closure Parameters
|
||||
loop_search_space_dimension: 8.0
|
||||
loop_search_space_resolution: 0.05
|
||||
loop_search_space_smear_deviation: 0.03
|
||||
|
||||
# Scan Matcher Parameters
|
||||
distance_variance_penalty: 0.5
|
||||
angle_variance_penalty: 1.0
|
||||
|
||||
fine_search_angle_offset: 0.00349
|
||||
coarse_search_angle_offset: 0.349
|
||||
coarse_angle_resolution: 0.0349
|
||||
minimum_angle_penalty: 0.9
|
||||
minimum_distance_penalty: 0.5
|
||||
use_response_expansion: true
|
||||
min_pass_through: 2
|
||||
occupancy_threshold: 0.1
|
||||
@@ -0,0 +1,77 @@
|
||||
slam_toolbox:
|
||||
ros__parameters:
|
||||
|
||||
# Plugin params
|
||||
solver_plugin: solver_plugins::CeresSolver
|
||||
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
|
||||
ceres_preconditioner: SCHUR_JACOBI
|
||||
ceres_trust_strategy: LEVENBERG_MARQUARDT
|
||||
ceres_dogleg_type: TRADITIONAL_DOGLEG
|
||||
ceres_loss_function: None
|
||||
|
||||
# ROS Parameters
|
||||
odom_frame: odom
|
||||
map_frame: map
|
||||
base_frame: base_footprint
|
||||
scan_topic: /scan
|
||||
use_map_saver: true
|
||||
mode: mapping #localization
|
||||
|
||||
# if you'd like to immediately start continuing a map at a given pose
|
||||
# or at the dock, but they are mutually exclusive, if pose is given
|
||||
# will use pose
|
||||
#map_file_name: test_steve
|
||||
#map_start_pose: [0.0, 0.0, 0.0]
|
||||
#map_start_at_dock: true
|
||||
|
||||
debug_logging: false
|
||||
throttle_scans: 1
|
||||
transform_publish_period: 0.02 #if 0 never publishes odometry
|
||||
map_update_interval: 5.0
|
||||
resolution: 0.05
|
||||
min_laser_range: 0.0 #for rastering images
|
||||
max_laser_range: 20.0 #for rastering images
|
||||
minimum_time_interval: 0.5
|
||||
transform_timeout: 0.2
|
||||
tf_buffer_duration: 30.
|
||||
stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
|
||||
enable_interactive_mode: true
|
||||
|
||||
# General Parameters
|
||||
use_scan_matching: true
|
||||
use_scan_barycenter: true
|
||||
minimum_travel_distance: 0.5
|
||||
minimum_travel_heading: 0.5
|
||||
scan_buffer_size: 10
|
||||
scan_buffer_maximum_scan_distance: 10.0
|
||||
link_match_minimum_response_fine: 0.1
|
||||
link_scan_maximum_distance: 1.5
|
||||
loop_search_maximum_distance: 3.0
|
||||
do_loop_closing: true
|
||||
loop_match_minimum_chain_size: 10
|
||||
loop_match_maximum_variance_coarse: 3.0
|
||||
loop_match_minimum_response_coarse: 0.35
|
||||
loop_match_minimum_response_fine: 0.45
|
||||
|
||||
# Correlation Parameters - Correlation Parameters
|
||||
correlation_search_space_dimension: 0.5
|
||||
correlation_search_space_resolution: 0.01
|
||||
correlation_search_space_smear_deviation: 0.1
|
||||
|
||||
# Correlation Parameters - Loop Closure Parameters
|
||||
loop_search_space_dimension: 8.0
|
||||
loop_search_space_resolution: 0.05
|
||||
loop_search_space_smear_deviation: 0.03
|
||||
|
||||
# Scan Matcher Parameters
|
||||
distance_variance_penalty: 0.5
|
||||
angle_variance_penalty: 1.0
|
||||
|
||||
fine_search_angle_offset: 0.00349
|
||||
coarse_search_angle_offset: 0.349
|
||||
coarse_angle_resolution: 0.0349
|
||||
minimum_angle_penalty: 0.9
|
||||
minimum_distance_penalty: 0.5
|
||||
use_response_expansion: true
|
||||
min_pass_through: 2
|
||||
occupancy_threshold: 0.1
|
||||
@@ -0,0 +1,16 @@
|
||||
scan_to_scan_filter_chain:
|
||||
ros__parameters:
|
||||
filter1:
|
||||
name: mask
|
||||
type: laser_filters/LaserScanMaskFilter
|
||||
params:
|
||||
masks:
|
||||
laser:
|
||||
- 100.
|
||||
- 101.
|
||||
- 102.
|
||||
- 103.
|
||||
- 104.
|
||||
- 1000.
|
||||
- 1001.
|
||||
- 1002.
|
||||
@@ -0,0 +1,20 @@
|
||||
scan_to_scan_filter_chain:
|
||||
ros__parameters:
|
||||
filter1:
|
||||
type: laser_filters/LaserArrayFilter
|
||||
name: median
|
||||
params:
|
||||
range_filter_chain:
|
||||
filter1:
|
||||
name: median
|
||||
type: filters/MultiChannelMedianFilterFloat
|
||||
params:
|
||||
number_of_observations: 5
|
||||
unused: 10
|
||||
intensity_filter_chain:
|
||||
filter1:
|
||||
name: median
|
||||
type: filters/MultiChannelMedianFilterFloat
|
||||
params:
|
||||
number_of_observations: 5
|
||||
unused: 10
|
||||
@@ -0,0 +1,17 @@
|
||||
scan_to_scan_filter_chain:
|
||||
ros__parameters:
|
||||
filter1:
|
||||
name: median_spatial
|
||||
type: laser_filters/LaserScanMedianSpatialFilter
|
||||
params:
|
||||
window_size: 31
|
||||
filter2:
|
||||
name: median_filter
|
||||
type: laser_filters/LaserArrayFilter
|
||||
params:
|
||||
range_filter_chain:
|
||||
filter1:
|
||||
name: median
|
||||
type: filters/MultiChannelMedianFilterFloat
|
||||
params:
|
||||
number_of_observations: 3
|
||||
@@ -0,0 +1,42 @@
|
||||
scan_to_scan_filter_chain:
|
||||
ros__parameters:
|
||||
filter1:
|
||||
type: laser_filters/LaserArrayFilter
|
||||
name: laser_median_5
|
||||
params:
|
||||
range_filter_chain:
|
||||
filter1:
|
||||
name: median_5
|
||||
type: filters/MultiChannelMedianFilterFloat
|
||||
params:
|
||||
number_of_observations: 5
|
||||
unused: 10
|
||||
intensity_filter_chain:
|
||||
filter2:
|
||||
name: median_5
|
||||
type: filters/MultiChannelMedianFilterFloat
|
||||
params:
|
||||
number_of_observations: 5
|
||||
unused: 10
|
||||
filter2:
|
||||
name: intensity
|
||||
type: laser_filters/LaserScanIntensityFilter
|
||||
params:
|
||||
lower_threshold: 8000.
|
||||
upper_threshold: 100000.
|
||||
disp_histogram: 0
|
||||
filter3:
|
||||
name: shadows
|
||||
type: laser_filters/ScanShadowsFilter
|
||||
params:
|
||||
min_angle: 10.
|
||||
max_angle: 170.
|
||||
neighbors: 20
|
||||
window: 0
|
||||
filter4:
|
||||
name: dark_shadows
|
||||
type: laser_filters/LaserScanIntensityFilter
|
||||
params:
|
||||
lower_threshold: 100.
|
||||
upper_threshold: 10000.
|
||||
disp_histogram: 0
|
||||
@@ -0,0 +1,10 @@
|
||||
scan_to_scan_filter_chain:
|
||||
ros__parameters:
|
||||
filter1:
|
||||
name: polygon_filter
|
||||
type: laser_filters/LaserScanPolygonFilter
|
||||
params:
|
||||
polygon_frame: base_link
|
||||
polygon: '[[0.0, 0.0], [0.1, 0.1], [0.1, 0.0], [0.0, -0.1]]'
|
||||
invert: false
|
||||
footprint_topic: base_footprint_exclude
|
||||
@@ -0,0 +1,11 @@
|
||||
scan_to_scan_filter_chain:
|
||||
ros__parameters:
|
||||
filter1:
|
||||
name: box_filter
|
||||
type: laser_filters/LaserScanRangeFilter
|
||||
params:
|
||||
use_message_range_limits: false # if not specified defaults to false
|
||||
lower_threshold: 0.5 # if not specified defaults to 0.0
|
||||
upper_threshold: 1.0 # if not specified defaults to 100000.0
|
||||
lower_replacement_value: -.inf # if not specified defaults to NaN
|
||||
upper_replacement_value: .inf # if not specified defaults to NaN
|
||||
@@ -0,0 +1,8 @@
|
||||
scan_to_scan_filter_chain:
|
||||
ros__parameters:
|
||||
filter1:
|
||||
name: scan_blob_filter
|
||||
type: laser_filters/ScanBlobFilter
|
||||
params:
|
||||
max_radius: 0.25 # maximum radius to be considered as blob object
|
||||
min_points: 6 # min scan points to be considered as blob object
|
||||
@@ -0,0 +1,12 @@
|
||||
scan_to_scan_filter_chain:
|
||||
ros__parameters:
|
||||
filter1:
|
||||
name: scan_filter
|
||||
type: laser_filters/LaserScanSectorFilter
|
||||
params:
|
||||
angle_min: 2.54 # if not specified defaults to 0.0
|
||||
angle_max: -2.54 # if not specified defaults to 0.0
|
||||
range_min: 0.2 # if not specified defaults to 0.0
|
||||
range_max: 2.0 # if not specified defaults to 100000.0
|
||||
clear_inside: true # if not specified defaults to true
|
||||
invert: false # (!clear_inside) if not specified defaults to false
|
||||
@@ -0,0 +1,18 @@
|
||||
scan_to_scan_filter_chain:
|
||||
ros__parameters:
|
||||
filter1:
|
||||
name: shadows
|
||||
type: laser_filters/ScanShadowsFilter
|
||||
params:
|
||||
min_angle: 10.
|
||||
max_angle: 170.
|
||||
neighbors: 20
|
||||
window: 1
|
||||
filter2:
|
||||
name: dark_shadows
|
||||
type: laser_filters/LaserScanIntensityFilter
|
||||
params:
|
||||
lower_threshold: 100.
|
||||
upper_threshold: 10000.
|
||||
disp_histogram: 0
|
||||
|
||||
137
src/navigation/cyy_slamtoolbox/config/slam_toolbox_default.rviz
Normal file
137
src/navigation/cyy_slamtoolbox/config/slam_toolbox_default.rviz
Normal file
@@ -0,0 +1,137 @@
|
||||
Panels:
|
||||
- Class: rviz_common/Displays
|
||||
Help Height: 78
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /Status1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 154
|
||||
- Class: rviz_common/Selection
|
||||
Name: Selection
|
||||
- Class: rviz_common/Tool Properties
|
||||
Expanded:
|
||||
- /2D Goal Pose1
|
||||
- /Publish Point1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.5886790156364441
|
||||
- Class: rviz_common/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
- Class: rviz_common/Time
|
||||
Experimental: false
|
||||
Name: Time
|
||||
SyncMode: 0
|
||||
SyncSource: ""
|
||||
- Class: slam_toolbox::SlamToolboxPlugin
|
||||
Name: SlamToolboxPlugin
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz_default_plugins/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.029999999329447746
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Plane: XY
|
||||
Plane Cell Count: 10
|
||||
Reference Frame: <Fixed Frame>
|
||||
Value: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Fixed Frame: map
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
- Class: rviz_default_plugins/Interact
|
||||
Hide Inactive Objects: true
|
||||
- Class: rviz_default_plugins/MoveCamera
|
||||
- Class: rviz_default_plugins/Select
|
||||
- Class: rviz_default_plugins/FocusCamera
|
||||
- Class: rviz_default_plugins/Measure
|
||||
Line color: 128; 128; 0
|
||||
- Class: rviz_default_plugins/SetInitialPose
|
||||
Covariance x: 0.25
|
||||
Covariance y: 0.25
|
||||
Covariance yaw: 0.06853891909122467
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /initialpose
|
||||
- Class: rviz_default_plugins/SetGoal
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /goal_pose
|
||||
- Class: rviz_default_plugins/PublishPoint
|
||||
Single click: true
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /clicked_point
|
||||
Transformation:
|
||||
Current:
|
||||
Class: rviz_default_plugins/TF
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz_default_plugins/Orbit
|
||||
Distance: 10
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Focal Shape Fixed Size: true
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 0.785398006439209
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 0.785398006439209
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 846
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
QMainWindow State: 000000ff00000000fd000000040000000000000217000002b0fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000125000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000220053006c0061006d0054006f006f006c0062006f00780050006c007500670069006e0100000168000001850000018500ffffff000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000017e000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
SlamToolboxPlugin:
|
||||
collapsed: false
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1200
|
||||
X: 72
|
||||
Y: 60
|
||||
@@ -0,0 +1,19 @@
|
||||
scan_filter_chain:
|
||||
- name: speckle_filter
|
||||
type: laser_filters/LaserScanSpeckleFilter
|
||||
params:
|
||||
# Select which filter type to use.
|
||||
# 0: Range based filtering (distance between consecutive points)
|
||||
# 1: Euclidean filtering based on radius outlier search
|
||||
filter_type: 0
|
||||
|
||||
# Only ranges smaller than this range are taken into account
|
||||
max_range: 2.0
|
||||
|
||||
# filter_type[0] (Distance): max distance between consecutive points
|
||||
# filter_type[1] (RadiusOutlier): max distance between points
|
||||
max_range_difference: 0.1
|
||||
|
||||
# filter_type[0] (Distance): Number of consecutive ranges that will be tested for max_distance
|
||||
# filter_type[1] (RadiusOutlier): Minimum number of neighbors
|
||||
filter_window: 2
|
||||
@@ -0,0 +1,24 @@
|
||||
#!/usr/bin/env python3
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
import os
|
||||
|
||||
def generate_launch_description():
|
||||
# 获取参数文件路径
|
||||
pkg_share = get_package_share_directory('cyy_slamtoolbox')
|
||||
params_file = os.path.join(pkg_share, 'config', 'mapper_params_online_async.yaml')
|
||||
|
||||
# 创建SLAM Toolbox节点
|
||||
slam_toolbox_node = Node(
|
||||
package='slam_toolbox',
|
||||
executable='async_slam_toolbox_node', # 或 sync_slam_toolbox_node
|
||||
name='slam_toolbox',
|
||||
output='screen',
|
||||
parameters=[params_file]
|
||||
)
|
||||
|
||||
|
||||
return LaunchDescription([
|
||||
slam_toolbox_node
|
||||
])
|
||||
@@ -0,0 +1,24 @@
|
||||
#!/usr/bin/env python3
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
import os
|
||||
|
||||
def generate_launch_description():
|
||||
# 获取参数文件路径
|
||||
pkg_share = get_package_share_directory('cyy_slamtoolbox')
|
||||
params_file = os.path.join(pkg_share, 'config', 'mapper_params_localization.yaml')
|
||||
|
||||
# 创建SLAM Toolbox节点
|
||||
slam_toolbox_node = Node(
|
||||
package='slam_toolbox',
|
||||
executable='localization_slam_toolbox_node', # 或 sync_slam_toolbox_node
|
||||
name='slam_toolbox',
|
||||
output='screen',
|
||||
parameters=[params_file]
|
||||
)
|
||||
|
||||
|
||||
return LaunchDescription([
|
||||
slam_toolbox_node
|
||||
])
|
||||
24
src/navigation/cyy_slamtoolbox/launch/filter.launch.py
Normal file
24
src/navigation/cyy_slamtoolbox/launch/filter.launch.py
Normal file
@@ -0,0 +1,24 @@
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
import os
|
||||
|
||||
def generate_launch_description():
|
||||
# 假设配置文件路径，你需要根据实际情况修改
|
||||
config_path = os.path.join(
|
||||
get_package_share_directory('cyy_slamtoolbox'),
|
||||
'config',
|
||||
'angular_filter_example.yaml'
|
||||
)
|
||||
|
||||
laser_filter_node = Node(
|
||||
package='laser_filters',
|
||||
executable='scan_to_scan_filter_chain',
|
||||
name='laser_filter',
|
||||
output='screen',
|
||||
parameters=[config_path],
|
||||
# 重新映射话题：从/base_scan原始数据过滤后发布到/scan
|
||||
remappings=[('scan', '/base_scan'), ('scan_filtered', '/scan')]
|
||||
)
|
||||
|
||||
return LaunchDescription([laser_filter_node])
|
||||
18
src/navigation/cyy_slamtoolbox/package.xml
Normal file
18
src/navigation/cyy_slamtoolbox/package.xml
Normal file
@@ -0,0 +1,18 @@
|
||||
<?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>cyy_slamtoolbox</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="root@todo.todo">root</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
23
src/navigation/gc_navigation2_real/CMakeLists.txt
Normal file
23
src/navigation/gc_navigation2_real/CMakeLists.txt
Normal file
@@ -0,0 +1,23 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(gc_navigation2_real)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
install(
|
||||
DIRECTORY launch config params maps behavior_tree
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
@@ -0,0 +1,37 @@
|
||||
<!--
|
||||
阿克曼底盘自定义行为树：移除了 Spin（原地旋转），
|
||||
恢复行为改为：清除代价地图 → 后退 → 等待
|
||||
实车版本 — 与仿真版相同逻辑
|
||||
-->
|
||||
<root main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
|
||||
<PipelineSequence name="NavigateWithReplanning">
|
||||
<RateController hz="1.0">
|
||||
<RecoveryNode number_of_retries="1" name="ComputePathToPose">
|
||||
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||
</RecoveryNode>
|
||||
</RateController>
|
||||
<RecoveryNode number_of_retries="1" name="FollowPath">
|
||||
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||
</RecoveryNode>
|
||||
</PipelineSequence>
|
||||
<ReactiveFallback name="RecoveryFallback">
|
||||
<GoalUpdated/>
|
||||
<RoundRobin name="RecoveryActions">
|
||||
<!-- 1. 清除代价地图，去掉短暂障碍 -->
|
||||
<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>
|
||||
<!-- 2. 快速后退 0.8m，给重新规划留空间（阿克曼无法原地旋转） -->
|
||||
<BackUp backup_dist="0.80" backup_speed="0.18"/>
|
||||
<!-- 3. 短暂等待 0.3s，让新路径规划完成 -->
|
||||
<Wait wait_duration="0.3"/>
|
||||
</RoundRobin>
|
||||
</ReactiveFallback>
|
||||
</RecoveryNode>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
@@ -0,0 +1,78 @@
|
||||
slam_toolbox:
|
||||
ros__parameters:
|
||||
|
||||
# ============================ Solver 插件参数 ======================================
|
||||
solver_plugin: solver_plugins::CeresSolver
|
||||
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
|
||||
ceres_preconditioner: SCHUR_JACOBI
|
||||
ceres_trust_strategy: LEVENBERG_MARQUARDT
|
||||
ceres_dogleg_type: TRADITIONAL_DOGLEG
|
||||
ceres_loss_function: None
|
||||
|
||||
# ============================ ROS 基础参数（实车）===================================
|
||||
odom_frame: odom_combined # 实车：EKF 融合后的里程计坐标系
|
||||
map_frame: map # 地图坐标系
|
||||
base_frame: base_footprint # 机器人基座坐标系（与 URDF 根帧一致）
|
||||
scan_topic: /scan # 激光雷达话题（实车镭神雷达）
|
||||
use_map_saver: true # 启用地图保存功能
|
||||
mode: mapping # mapping = 定位 + 实时建图
|
||||
|
||||
# ============================ 已有地图加载 ==========================================
|
||||
# map_file_name: "" # 从零开始建图，留空
|
||||
map_start_pose: [0.0, 0.0, 0.0] # 初始位姿 [x, y, yaw]
|
||||
|
||||
# ============================ 调试与性能 ===========================================
|
||||
debug_logging: false
|
||||
throttle_scans: 1 # 每 N 帧处理一次（实车每秒10帧左右）
|
||||
transform_publish_period: 0.02 # TF 发布周期（秒）
|
||||
map_update_interval: 3.0 # /map 话题更新间隔（秒）
|
||||
resolution: 0.05 # 地图分辨率（米/像素）
|
||||
restamp_tf: false
|
||||
min_laser_range: 0.15 # 实车雷达盲区约 0.12m，设 0.15 过滤近处噪点
|
||||
max_laser_range: 12.0 # 实车雷达最大有效距离（镭神 N10 ≈ 10-15m）
|
||||
minimum_time_interval: 0.3 # 最小处理时间间隔（实车可适当放宽）
|
||||
transform_timeout: 0.2 # TF 查找超时
|
||||
tf_buffer_duration: 30.0 # TF 缓冲区时长
|
||||
stack_size_to_use: 40000000 # 栈大小（序列化大地图需要）
|
||||
enable_interactive_mode: true # 交互模式：允许外部工具编辑地图
|
||||
|
||||
# ============================ 通用建图参数 =========================================
|
||||
use_scan_matching: true
|
||||
use_scan_barycenter: true
|
||||
minimum_travel_distance: 0.3 # 实车最小移动距离（米），小于仿真以更频繁更新
|
||||
minimum_travel_heading: 0.3 # 最小转向角度（弧度）
|
||||
check_min_dist_and_heading_precisely: false
|
||||
scan_buffer_size: 10 # 扫描缓冲区大小
|
||||
scan_buffer_maximum_scan_distance: 10.0
|
||||
link_match_minimum_response_fine: 0.1
|
||||
link_scan_maximum_distance: 1.5
|
||||
|
||||
# ============================ 回环检测参数 =========================================
|
||||
do_loop_closing: true # 启用回环检测
|
||||
loop_match_minimum_chain_size: 10
|
||||
loop_match_maximum_variance_coarse: 3.0
|
||||
loop_match_minimum_response_coarse: 0.35
|
||||
loop_match_minimum_response_fine: 0.45
|
||||
loop_search_maximum_distance: 3.0
|
||||
|
||||
# ============================ 相关性匹配参数 =======================================
|
||||
correlation_search_space_dimension: 0.5
|
||||
correlation_search_space_resolution: 0.01
|
||||
correlation_search_space_smear_deviation: 0.1
|
||||
|
||||
loop_search_space_dimension: 8.0
|
||||
loop_search_space_resolution: 0.05
|
||||
loop_search_space_smear_deviation: 0.03
|
||||
|
||||
# ============================ 扫描匹配器参数 ========================================
|
||||
distance_variance_penalty: 0.5
|
||||
angle_variance_penalty: 1.0
|
||||
|
||||
fine_search_angle_offset: 0.00349
|
||||
coarse_search_angle_offset: 0.349
|
||||
coarse_angle_resolution: 0.0349
|
||||
minimum_angle_penalty: 0.9
|
||||
minimum_distance_penalty: 0.5
|
||||
use_response_expansion: true
|
||||
min_pass_through: 2
|
||||
occupancy_threshold: 0.1
|
||||
@@ -0,0 +1,77 @@
|
||||
slam_toolbox:
|
||||
ros__parameters:
|
||||
|
||||
# ============================ Solver 插件参数 ======================================
|
||||
solver_plugin: solver_plugins::CeresSolver
|
||||
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
|
||||
ceres_preconditioner: SCHUR_JACOBI
|
||||
ceres_trust_strategy: LEVENBERG_MARQUARDT
|
||||
ceres_dogleg_type: TRADITIONAL_DOGLEG
|
||||
ceres_loss_function: None
|
||||
|
||||
# ============================ ROS 基础参数（实车定位模式）============================
|
||||
odom_frame: odom_combined # 实车：EKF 融合后的里程计坐标系
|
||||
map_frame: map
|
||||
base_frame: base_footprint
|
||||
scan_topic: /scan
|
||||
mode: localization # 纯定位模式，不更新地图
|
||||
|
||||
# ============================ 已有地图加载 ==========================================
|
||||
# 由 launch 文件通过 map_file_name 参数传入 .posegraph 路径
|
||||
# map_file_name: ""
|
||||
map_start_pose: [0.0, 0.0, 0.0]
|
||||
|
||||
# ============================ 调试与性能 ===========================================
|
||||
debug_logging: false
|
||||
throttle_scans: 2 # 每2帧处理一次（实车激光较慢时使用）
|
||||
transform_publish_period: 0.02
|
||||
map_update_interval: 5.0
|
||||
resolution: 0.05
|
||||
restamp_tf: false
|
||||
min_laser_range: 0.15 # 过滤雷达盲区噪点
|
||||
max_laser_range: 12.0 # 实车雷达最大有效距离
|
||||
minimum_time_interval: 0.3
|
||||
transform_timeout: 0.2
|
||||
tf_buffer_duration: 30.0
|
||||
stack_size_to_use: 40000000
|
||||
|
||||
# ============================ 通用参数 =============================================
|
||||
use_scan_matching: true
|
||||
use_scan_barycenter: true
|
||||
minimum_travel_distance: 0.3
|
||||
minimum_travel_heading: 0.3
|
||||
check_min_dist_and_heading_precisely: false
|
||||
scan_buffer_size: 10
|
||||
scan_buffer_maximum_scan_distance: 10.0
|
||||
link_match_minimum_response_fine: 0.1
|
||||
link_scan_maximum_distance: 1.5
|
||||
|
||||
# 定位模式关闭回环检测
|
||||
do_loop_closing: false
|
||||
loop_match_minimum_chain_size: 10
|
||||
loop_match_maximum_variance_coarse: 3.0
|
||||
loop_match_minimum_response_coarse: 0.35
|
||||
loop_match_minimum_response_fine: 0.45
|
||||
loop_search_maximum_distance: 3.0
|
||||
|
||||
# ============================ 相关性匹配参数 =======================================
|
||||
correlation_search_space_dimension: 0.5
|
||||
correlation_search_space_resolution: 0.01
|
||||
correlation_search_space_smear_deviation: 0.1
|
||||
|
||||
loop_search_space_dimension: 8.0
|
||||
loop_search_space_resolution: 0.05
|
||||
loop_search_space_smear_deviation: 0.03
|
||||
|
||||
# ============================ 扫描匹配器参数 ========================================
|
||||
distance_variance_penalty: 0.5
|
||||
angle_variance_penalty: 1.0
|
||||
|
||||
fine_search_angle_offset: 0.00349
|
||||
coarse_search_angle_offset: 0.349
|
||||
coarse_angle_resolution: 0.0349
|
||||
minimum_angle_penalty: 0.9
|
||||
minimum_distance_penalty: 0.5
|
||||
use_response_expansion: true
|
||||
min_pass_through: 2
|
||||
occupancy_threshold: 0.1
|
||||
@@ -0,0 +1,54 @@
|
||||
#!/usr/bin/env python3
|
||||
# ============================================================================
|
||||
# real_bringup.launch.py
|
||||
# 功能：实车基础 bringup — 底盘驱动 + 激光雷达 + TF 树
|
||||
#
|
||||
# 适用场景：
|
||||
# - 单独调试底盘通信和传感器数据
|
||||
# - 遥控测试（无导航）
|
||||
# - 作为其他 launch 文件的基础模块
|
||||
#
|
||||
# 启动内容：
|
||||
# origincar_base bringup — 串口驱动 + EKF + IMU 滤波 + TF + robot_state_publisher
|
||||
# lslidar_driver — 镭神激光雷达 → /scan
|
||||
#
|
||||
# 使用方式：
|
||||
# ros2 launch gc_navigation2_real real_bringup.launch.py
|
||||
# ============================================================================
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
"""生成 LaunchDescription，启动实车底盘 + 雷达 + TF（无导航）"""
|
||||
|
||||
# ============================ 1. 包路径 =========================================
|
||||
origincar_base_dir = get_package_share_directory('origincar_base')
|
||||
|
||||
# ============================ 2. 实车底盘 bringup ===============================
|
||||
# 注：origincar_bringup 已包含 robot_state_publisher + joint_state_publisher + 静态 TF
|
||||
origincar_bringup = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
[origincar_base_dir, '/launch', '/origincar_bringup.launch.py']),
|
||||
)
|
||||
|
||||
# ============================ 3. 镭神激光雷达驱动 ===============================
|
||||
lslidar_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
[get_package_share_directory('lslidar_driver'),
|
||||
'/launch', '/lsn10_launch.py']),
|
||||
)
|
||||
|
||||
# ============================ 4. 组装 LaunchDescription =========================
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time',
|
||||
default_value='false',
|
||||
description='使用仿真时间（实车必须为 false）'),
|
||||
|
||||
origincar_bringup,
|
||||
lslidar_launch,
|
||||
])
|
||||
103
src/navigation/gc_navigation2_real/launch/real_mapping.launch.py
Normal file
103
src/navigation/gc_navigation2_real/launch/real_mapping.launch.py
Normal file
@@ -0,0 +1,103 @@
|
||||
#!/usr/bin/env python3
|
||||
# ============================================================================
|
||||
# real_slam_mapping.launch.py
|
||||
# 功能：实车 SLAM 建图启动文件（无 Gazebo 仿真）
|
||||
#
|
||||
# 适用场景：
|
||||
# - 在实际场地遥控机器人建立环境地图
|
||||
# - 在已有地图基础上增量更新
|
||||
# - 单独调试 slam_toolbox 建图参数
|
||||
#
|
||||
# 架构：
|
||||
# origincar_base (bringup) — 底盘串口驱动 + EKF 里程计 + IMU 滤波 + TF
|
||||
# lslidar_driver — 镭神激光雷达驱动 → /scan
|
||||
# async_slam_toolbox_node — 激光 SLAM 建图（mapping 模式）
|
||||
# 发布 map→odom_combined TF
|
||||
# 发布 /map 话题（实时地图）
|
||||
#
|
||||
# 使用方式：
|
||||
# ros2 launch gc_navigation2_real real_slam_mapping.launch.py
|
||||
#
|
||||
# 保存地图：
|
||||
# ros2 service call /slam_toolbox/serialize_map slam_toolbox/srv/SerializePoseGraph \
|
||||
# "{filename: '/home/guoch/test_ws/src/gc_navigation2_real/maps/my_map'}"
|
||||
# ros2 run nav2_map_server map_saver_cli -f <map_name>
|
||||
# ============================================================================
|
||||
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
"""生成 LaunchDescription，启动实车底盘 + 雷达 + slam_toolbox 建图"""
|
||||
|
||||
# ============================ 1. 包路径 =========================================
|
||||
pkg_dir = get_package_share_directory('gc_navigation2_real')
|
||||
origincar_base_dir = get_package_share_directory('origincar_base')
|
||||
|
||||
# ============================ 2. 声明启动参数 ====================================
|
||||
use_sim_time = LaunchConfiguration('use_sim_time', default='false')
|
||||
|
||||
# slam_params_file: slam_toolbox 配置文件
|
||||
# slam_params_file = LaunchConfiguration(
|
||||
# 'slam_params_file',
|
||||
# default=os.path.join(pkg_dir, 'config', 'slam_toolbox_async_real.yaml'))
|
||||
|
||||
# map_file_name: 可选，加载已有序列化地图继续建图
|
||||
map_file_name = LaunchConfiguration('map_file_name', default='')
|
||||
|
||||
# ============================ 3. 实车底盘 bringup ===============================
|
||||
# 启动 origincar_base：串口驱动 + EKF + IMU 滤波 + TF 发布
|
||||
# 注：origincar_bringup 已包含 robot_state_publisher + joint_state_publisher + 静态 TF
|
||||
origincar_bringup = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
[origincar_base_dir, '/launch', '/origincar_bringup.launch.py']),
|
||||
)
|
||||
|
||||
# ============================ 4. 镭神激光雷达驱动 ===============================
|
||||
# 启动 lslidar_driver → /scan 话题
|
||||
lslidar_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
[get_package_share_directory('lslidar_driver'),
|
||||
'/launch', '/lsn10_launch.py']),
|
||||
)
|
||||
|
||||
# ============================ 5. slam_toolbox 建图节点 ============================
|
||||
slam_toolbox_node = Node(
|
||||
package='slam_toolbox',
|
||||
executable='async_slam_toolbox_node',
|
||||
name='slam_toolbox',
|
||||
output='screen',
|
||||
parameters=[
|
||||
slam_params_file,
|
||||
{
|
||||
'use_sim_time': False,
|
||||
'map_file_name': map_file_name,
|
||||
},
|
||||
],
|
||||
)
|
||||
|
||||
# ============================ 6. 组装 LaunchDescription =========================
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time',
|
||||
default_value='false',
|
||||
description='使用仿真时间（实车必须为 false）'),
|
||||
DeclareLaunchArgument(
|
||||
'slam_params_file',
|
||||
default_value=os.path.join(pkg_dir, 'config', 'slam_toolbox_async_real.yaml'),
|
||||
description='slam_toolbox 参数配置文件路径'),
|
||||
DeclareLaunchArgument(
|
||||
'map_file_name',
|
||||
default_value='',
|
||||
description='已有序列化地图路径（.posegraph），留空则从零开始建图'),
|
||||
|
||||
origincar_bringup,
|
||||
lslidar_launch,
|
||||
slam_toolbox_node,
|
||||
])
|
||||
@@ -0,0 +1,110 @@
|
||||
#!/usr/bin/env python3
|
||||
# ============================================================================
|
||||
# real_nav2_slam.launch.py
|
||||
# 功能：实车导航 — 已知静态地图 + SLAM 定位 + Nav2 导航
|
||||
#
|
||||
# 架构：
|
||||
# origincar_base (bringup) — 底盘串口驱动 + EKF 里程计 + IMU 滤波 + TF
|
||||
# lslidar_driver — 镭神激光雷达驱动 → /scan
|
||||
# map_server — 加载 yaml+pgm 全量静态地图 → /map
|
||||
# slam_toolbox (localization) — 加载 .posegraph，激光扫描匹配定位
|
||||
# 发布 map→odom_combined TF（替代 AMCL）
|
||||
# ⚠️ /map 话题重映射为 /slam_map，避免冲突
|
||||
# navigation_launch — Nav2 导航栈
|
||||
# global_costmap 从 /map 获取全量静态地图
|
||||
#
|
||||
# 使用方式：
|
||||
# ros2 launch gc_navigation2_real real_nav2_slam.launch.py
|
||||
# ============================================================================
|
||||
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
"""生成 LaunchDescription，启动实车底盘 + 雷达 + SLAM定位 + Nav2 导航"""
|
||||
ld = LaunchDescription()
|
||||
|
||||
# =============================1. 包路径===========================================
|
||||
pkg_dir = get_package_share_directory('gc_navigation2_real')
|
||||
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
|
||||
origincar_base_dir = get_package_share_directory('origincar_base')
|
||||
|
||||
# =============================2. 参数=============================================
|
||||
use_sim_time = LaunchConfiguration('use_sim_time', default='false')
|
||||
map_yaml_path = LaunchConfiguration('map', default=os.path.join(
|
||||
pkg_dir, 'maps', 'my_map.yaml'))
|
||||
nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
|
||||
pkg_dir, 'params', 'gc_navigation_slam_real.yaml'))
|
||||
slam_params_file = os.path.join(
|
||||
pkg_dir, 'config', 'slam_toolbox_localization_real.yaml')
|
||||
posegraph_path = os.path.join(
|
||||
pkg_dir, 'maps', 'my_map.posegraph')
|
||||
|
||||
# =============================3. 实车底盘 bringup ================================
|
||||
origincar_bringup = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
[origincar_base_dir, '/launch', '/origincar_bringup.launch.py']),
|
||||
)
|
||||
|
||||
# =============================4. 镭神激光雷达驱动 ================================
|
||||
lslidar_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
[get_package_share_directory('lslidar_driver'),
|
||||
'/launch', '/lsn10_launch.py']),
|
||||
)
|
||||
|
||||
# =============================5. map_server：全量静态地图 ==========================
|
||||
map_server_node = Node(
|
||||
package='nav2_map_server',
|
||||
executable='map_server',
|
||||
name='map_server',
|
||||
output='screen',
|
||||
parameters=[{'yaml_filename': map_yaml_path, 'use_sim_time': False}],
|
||||
)
|
||||
map_server_lifecycle = Node(
|
||||
package='nav2_lifecycle_manager',
|
||||
executable='lifecycle_manager',
|
||||
name='lifecycle_manager_map',
|
||||
output='screen',
|
||||
parameters=[{'use_sim_time': False, 'autostart': True,
|
||||
'node_names': ['map_server']}],
|
||||
)
|
||||
|
||||
# =============================6. slam_toolbox 定位模式（替代 AMCL）==================
|
||||
# ⚠️ /map 重映射到 /slam_map，避免覆盖 map_server 的全量地图
|
||||
slam_toolbox_node = Node(
|
||||
package='slam_toolbox',
|
||||
executable='localization_slam_toolbox_node',
|
||||
name='slam_toolbox',
|
||||
output='screen',
|
||||
parameters=[slam_params_file,
|
||||
{'use_sim_time': False,
|
||||
'map_file_name': posegraph_path}],
|
||||
remappings=[('/map', '/slam_map')],
|
||||
)
|
||||
|
||||
# =============================7. Nav2 导航栈 =====================================
|
||||
navigation_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
[nav2_bringup_dir, '/launch', '/navigation_launch.py']),
|
||||
launch_arguments={
|
||||
'use_sim_time': use_sim_time,
|
||||
'params_file': nav2_param_path,
|
||||
}.items(),
|
||||
)
|
||||
|
||||
# =============================8. 组装=============================================
|
||||
ld.add_action(origincar_bringup)
|
||||
ld.add_action(lslidar_launch)
|
||||
ld.add_action(map_server_node)
|
||||
ld.add_action(map_server_lifecycle)
|
||||
ld.add_action(slam_toolbox_node)
|
||||
ld.add_action(navigation_launch)
|
||||
|
||||
return ld
|
||||
@@ -0,0 +1,135 @@
|
||||
#!/usr/bin/env python3
|
||||
# ============================================================================
|
||||
# real_nav2_slam_online.launch.py
|
||||
# 功能：实车导航 — 已知全局地图 + SLAM 在线更新 + Nav2 导航
|
||||
#
|
||||
# 架构：
|
||||
# ┌─ /map（全量静态地图）
|
||||
# map_server ──────────┤
|
||||
# └─ 来自 my_map.yaml + my_map.pgm
|
||||
#
|
||||
# ┌─ map→odom_combined TF（激光扫描匹配定位）
|
||||
# slam_toolbox ────────┤
|
||||
# (async mapping) └─ /slam_map（实时建图结果，重映射避免冲突）
|
||||
#
|
||||
# navigation ┌─ global_costmap/static_layer ← /map（全量地图）
|
||||
# ├─ local_costmap/obstacle_layer ← /scan（实时避障）
|
||||
# └─ planner + controller + behavior
|
||||
#
|
||||
# 关键设计：
|
||||
# 1. map_server 提供全量静态底图 → Foxglove 中看到完整房间布局
|
||||
# 2. slam_toolbox 定位 + 实时建图，/slam_map 可单独查看更新效果
|
||||
# 3. 实时避障通过 costmap 的 obstacle_layer 实现（不依赖地图更新）
|
||||
# 4. 定期调用 /slam_toolbox/serialize_map 保存更新后的地图
|
||||
#
|
||||
# 与仿真版的区别：
|
||||
# - 无 Gazebo，无 spawn_entity
|
||||
# - 使用 origincar_base bringup（串口驱动 + EKF + IMU）
|
||||
# - 使用 lslidar_driver（镭神实车雷达）
|
||||
# - use_sim_time: False
|
||||
# - TF 帧适配实车（odom_combined, base_footprint）
|
||||
#
|
||||
# 使用方式：
|
||||
# ros2 launch gc_navigation2_real real_nav2_slam_online.launch.py
|
||||
# ============================================================================
|
||||
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
from launch.actions import ExecuteProcess
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
"""启动实车底盘 + 雷达 + 静态地图 + SLAM 在线定位建图 + Nav2 导航"""
|
||||
ld = LaunchDescription()
|
||||
|
||||
# =============================1. 包路径===========================================
|
||||
pkg_dir = get_package_share_directory('gc_navigation2_real')
|
||||
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
|
||||
origincar_base_dir = get_package_share_directory('origincar_base')
|
||||
|
||||
# =============================2. 参数=============================================
|
||||
use_sim_time = LaunchConfiguration('use_sim_time', default='false')
|
||||
map_yaml_path = os.path.join(pkg_dir, 'maps', 'my_map.yaml')
|
||||
nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
|
||||
pkg_dir, 'params', 'gc_navigation_slam_real.yaml'))
|
||||
slam_params_file = os.path.join(pkg_dir, 'config', 'slam_toolbox_async_real.yaml')
|
||||
posegraph_path = os.path.join(pkg_dir, 'maps', 'my_map.posegraph')
|
||||
|
||||
# =============================3. 实车底盘 bringup ================================
|
||||
origincar_bringup = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
[origincar_base_dir, '/launch', '/origincar_bringup.launch.py']),
|
||||
)
|
||||
|
||||
# =============================4. 镭神激光雷达驱动 ================================
|
||||
lslidar_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
[get_package_share_directory('lslidar_driver'),
|
||||
'/launch', '/lsn10_launch.py']),
|
||||
)
|
||||
|
||||
# =============================5. map_server：全量静态地图 ==========================
|
||||
map_server_node = Node(
|
||||
package='nav2_map_server',
|
||||
executable='map_server',
|
||||
name='map_server',
|
||||
output='screen',
|
||||
parameters=[{'yaml_filename': map_yaml_path, 'use_sim_time': False}],
|
||||
)
|
||||
map_server_lifecycle = Node(
|
||||
package='nav2_lifecycle_manager',
|
||||
executable='lifecycle_manager',
|
||||
name='lifecycle_manager_map',
|
||||
output='screen',
|
||||
parameters=[{'use_sim_time': False, 'autostart': True,
|
||||
'node_names': ['map_server']}],
|
||||
)
|
||||
|
||||
# =============================6. slam_toolbox：定位 + 实时建图========================
|
||||
# ⚠️ /map 重映射到 /slam_map，避免覆盖 map_server 的全量地图
|
||||
slam_toolbox_node = Node(
|
||||
package='slam_toolbox',
|
||||
executable='async_slam_toolbox_node',
|
||||
name='slam_toolbox',
|
||||
output='screen',
|
||||
parameters=[slam_params_file, {'use_sim_time': False}],
|
||||
remappings=[('/map', '/slam_map')],
|
||||
)
|
||||
|
||||
# =============================7. 加载已有序列化地图 =================================
|
||||
load_map_cmd = ExecuteProcess(
|
||||
cmd=[
|
||||
'bash', '-c',
|
||||
'sleep 8 && '
|
||||
'ros2 service call /slam_toolbox/deserialize_map '
|
||||
'slam_toolbox/srv/DeserializePoseGraph '
|
||||
"'{filename: \"%s\", match_type: 1}'" % posegraph_path,
|
||||
],
|
||||
output='screen',
|
||||
)
|
||||
|
||||
# =============================8. Nav2 导航栈 =====================================
|
||||
navigation_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
[nav2_bringup_dir, '/launch', '/navigation_launch.py']),
|
||||
launch_arguments={
|
||||
'use_sim_time': use_sim_time,
|
||||
'params_file': nav2_param_path,
|
||||
}.items(),
|
||||
)
|
||||
|
||||
# =============================9. 组装============================================
|
||||
ld.add_action(origincar_bringup)
|
||||
ld.add_action(lslidar_launch)
|
||||
ld.add_action(map_server_node)
|
||||
ld.add_action(map_server_lifecycle)
|
||||
ld.add_action(slam_toolbox_node)
|
||||
ld.add_action(load_map_cmd)
|
||||
ld.add_action(navigation_launch)
|
||||
|
||||
return ld
|
||||
@@ -0,0 +1,103 @@
|
||||
#!/usr/bin/env python3
|
||||
# ============================================================================
|
||||
# real_slam_mapping.launch.py
|
||||
# 功能：实车 SLAM 建图启动文件（无 Gazebo 仿真）
|
||||
#
|
||||
# 适用场景：
|
||||
# - 在实际场地遥控机器人建立环境地图
|
||||
# - 在已有地图基础上增量更新
|
||||
# - 单独调试 slam_toolbox 建图参数
|
||||
#
|
||||
# 架构：
|
||||
# origincar_base (bringup) — 底盘串口驱动 + EKF 里程计 + IMU 滤波 + TF
|
||||
# lslidar_driver — 镭神激光雷达驱动 → /scan
|
||||
# async_slam_toolbox_node — 激光 SLAM 建图（mapping 模式）
|
||||
# 发布 map→odom_combined TF
|
||||
# 发布 /map 话题（实时地图）
|
||||
#
|
||||
# 使用方式：
|
||||
# ros2 launch gc_navigation2_real real_slam_mapping.launch.py
|
||||
#
|
||||
# 保存地图：
|
||||
# ros2 service call /slam_toolbox/serialize_map slam_toolbox/srv/SerializePoseGraph \
|
||||
# "{filename: '/home/guoch/test_ws/src/gc_navigation2_real/maps/my_map'}"
|
||||
# ros2 run nav2_map_server map_saver_cli -f <map_name>
|
||||
# ============================================================================
|
||||
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
"""生成 LaunchDescription，启动实车底盘 + 雷达 + slam_toolbox 建图"""
|
||||
|
||||
# ============================ 1. 包路径 =========================================
|
||||
pkg_dir = get_package_share_directory('gc_navigation2_real')
|
||||
origincar_base_dir = get_package_share_directory('origincar_base')
|
||||
|
||||
# ============================ 2. 声明启动参数 ====================================
|
||||
use_sim_time = LaunchConfiguration('use_sim_time', default='false')
|
||||
|
||||
# slam_params_file: slam_toolbox 配置文件
|
||||
slam_params_file = LaunchConfiguration(
|
||||
'slam_params_file',
|
||||
default=os.path.join(pkg_dir, 'config', 'slam_toolbox_async_real.yaml'))
|
||||
|
||||
# map_file_name: 可选，加载已有序列化地图继续建图
|
||||
map_file_name = LaunchConfiguration('map_file_name', default='')
|
||||
|
||||
# ============================ 3. 实车底盘 bringup ===============================
|
||||
# 启动 origincar_base：串口驱动 + EKF + IMU 滤波 + TF 发布
|
||||
# 注：origincar_bringup 已包含 robot_state_publisher + joint_state_publisher + 静态 TF
|
||||
origincar_bringup = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
[origincar_base_dir, '/launch', '/origincar_bringup.launch.py']),
|
||||
)
|
||||
|
||||
# ============================ 4. 镭神激光雷达驱动 ===============================
|
||||
# 启动 lslidar_driver → /scan 话题
|
||||
lslidar_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
[get_package_share_directory('lslidar_driver'),
|
||||
'/launch', '/lsn10_launch.py']),
|
||||
)
|
||||
|
||||
# ============================ 5. slam_toolbox 建图节点 ============================
|
||||
slam_toolbox_node = Node(
|
||||
package='slam_toolbox',
|
||||
executable='async_slam_toolbox_node',
|
||||
name='slam_toolbox',
|
||||
output='screen',
|
||||
parameters=[
|
||||
slam_params_file,
|
||||
{
|
||||
'use_sim_time': False,
|
||||
'map_file_name': map_file_name,
|
||||
},
|
||||
],
|
||||
)
|
||||
|
||||
# ============================ 6. 组装 LaunchDescription =========================
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time',
|
||||
default_value='false',
|
||||
description='使用仿真时间（实车必须为 false）'),
|
||||
DeclareLaunchArgument(
|
||||
'slam_params_file',
|
||||
default_value=os.path.join(pkg_dir, 'config', 'slam_toolbox_async_real.yaml'),
|
||||
description='slam_toolbox 参数配置文件路径'),
|
||||
DeclareLaunchArgument(
|
||||
'map_file_name',
|
||||
default_value='',
|
||||
description='已有序列化地图路径（.posegraph），留空则从零开始建图'),
|
||||
|
||||
origincar_bringup,
|
||||
lslidar_launch,
|
||||
slam_toolbox_node,
|
||||
])
|
||||
3
src/navigation/gc_navigation2_real/maps/Readme.txt
Normal file
3
src/navigation/gc_navigation2_real/maps/Readme.txt
Normal file
@@ -0,0 +1,3 @@
|
||||
# 实车地图存放目录
|
||||
# 使用 map_saver_cli 保存地图到此目录：
|
||||
# ros2 run nav2_map_server map_saver_cli -f ~/test_ws/src/gc_navigation2_real/maps/your_map_name
|
||||
31
src/navigation/gc_navigation2_real/package.xml
Normal file
31
src/navigation/gc_navigation2_real/package.xml
Normal 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>gc_navigation2_real</name>
|
||||
<version>0.0.0</version>
|
||||
<description>Real-world navigation package: SLAM + Nav2 for OriginCar on physical robot (no Gazebo)</description>
|
||||
<maintainer email="guoch@todo.todo">guoch</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<exec_depend>rclcpp</exec_depend>
|
||||
<exec_depend>slam_toolbox</exec_depend>
|
||||
<exec_depend>nav2_bringup</exec_depend>
|
||||
<exec_depend>origincar_base</exec_depend>
|
||||
<exec_depend>origincar_description</exec_depend>
|
||||
<exec_depend>origincar_msg</exec_depend>
|
||||
<exec_depend>robot_state_publisher</exec_depend>
|
||||
<exec_depend>joint_state_publisher</exec_depend>
|
||||
<exec_depend>robot_localization</exec_depend>
|
||||
<exec_depend>imu_filter_madgwick</exec_depend>
|
||||
<exec_depend>tf2_ros</exec_depend>
|
||||
<exec_depend>lslidar_driver</exec_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,353 @@
|
||||
# ============================================================================
|
||||
# gc_navigation_slam_real.yaml
|
||||
# 实车 Nav2 导航参数（SLAM 定位方案，无 AMCL）
|
||||
#
|
||||
# 与仿真版的区别：
|
||||
# - use_sim_time: False（使用系统时钟）
|
||||
# - TF 帧适配实车（odom_combined, base_footprint）
|
||||
# - 速度/加速度参数适当降低以保证实车安全
|
||||
# - 激光雷达参数匹配镭神 N10 实车性能
|
||||
# ============================================================================
|
||||
|
||||
slam_toolbox:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
global_frame: map
|
||||
robot_base_frame: base_footprint
|
||||
odom_topic: /odom_combined
|
||||
bt_loop_duration: 50
|
||||
default_server_timeout: 20
|
||||
# 阿克曼底盘专用 BT：移除 Spin，恢复行为 = 清代价地图 → 后退 → 等待
|
||||
default_nav_to_pose_bt_xml: /home/guoch/test_ws/install/gc_navigation2_real/share/gc_navigation2_real/behavior_tree/nav_to_pose_ackermann.xml
|
||||
plugin_lib_names:
|
||||
- nav2_compute_path_to_pose_action_bt_node
|
||||
- nav2_compute_path_through_poses_action_bt_node
|
||||
- nav2_smooth_path_action_bt_node
|
||||
- nav2_follow_path_action_bt_node
|
||||
- nav2_spin_action_bt_node
|
||||
- nav2_wait_action_bt_node
|
||||
- nav2_back_up_action_bt_node
|
||||
- nav2_drive_on_heading_bt_node
|
||||
- nav2_clear_costmap_service_bt_node
|
||||
- nav2_is_stuck_condition_bt_node
|
||||
- nav2_goal_reached_condition_bt_node
|
||||
- nav2_goal_updated_condition_bt_node
|
||||
- nav2_globally_updated_goal_condition_bt_node
|
||||
- nav2_is_path_valid_condition_bt_node
|
||||
- nav2_initial_pose_received_condition_bt_node
|
||||
- nav2_reinitialize_global_localization_service_bt_node
|
||||
- nav2_rate_controller_bt_node
|
||||
- nav2_distance_controller_bt_node
|
||||
- nav2_speed_controller_bt_node
|
||||
- nav2_truncate_path_action_bt_node
|
||||
- nav2_truncate_path_local_action_bt_node
|
||||
- nav2_goal_updater_node_bt_node
|
||||
- nav2_recovery_node_bt_node
|
||||
- nav2_pipeline_sequence_bt_node
|
||||
- nav2_round_robin_node_bt_node
|
||||
- nav2_transform_available_condition_bt_node
|
||||
- nav2_time_expired_condition_bt_node
|
||||
- nav2_path_expiring_timer_condition
|
||||
- nav2_distance_traveled_condition_bt_node
|
||||
- nav2_single_trigger_bt_node
|
||||
- nav2_is_battery_low_condition_bt_node
|
||||
- nav2_navigate_through_poses_action_bt_node
|
||||
- nav2_navigate_to_pose_action_bt_node
|
||||
- nav2_remove_passed_goals_action_bt_node
|
||||
- nav2_planner_selector_bt_node
|
||||
- nav2_controller_selector_bt_node
|
||||
- nav2_goal_checker_selector_bt_node
|
||||
- nav2_controller_cancel_bt_node
|
||||
- nav2_path_longer_on_approach_bt_node
|
||||
- nav2_wait_cancel_bt_node
|
||||
- nav2_spin_cancel_bt_node
|
||||
- nav2_back_up_cancel_bt_node
|
||||
- nav2_drive_on_heading_cancel_bt_node
|
||||
|
||||
bt_navigator_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
controller_frequency: 20.0
|
||||
FollowPath:
|
||||
plugin: "nav2_mppi_controller::MPPIController"
|
||||
time_steps: 36
|
||||
model_dt: 0.05
|
||||
batch_size: 1000
|
||||
vx_std: 0.2
|
||||
vy_std: 0.0
|
||||
wz_std: 0.4
|
||||
vx_max: 0.5 # 第一阶段提速：0.4→0.5（仿真已验证安全）
|
||||
vx_min: -0.30 # 第一阶段提速：-0.25→-0.30
|
||||
vy_max: 0.0
|
||||
wz_max: 1.5 # 实车最大角速度降低
|
||||
iteration_count: 1
|
||||
temperature: 0.3
|
||||
gamma: 0.015
|
||||
motion_model: "Ackermann"
|
||||
visualize: false
|
||||
TrajectoryVisualizer:
|
||||
trajectory_step: 5
|
||||
time_step: 3
|
||||
AckermannConstraints:
|
||||
min_turning_r: 0.4
|
||||
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
|
||||
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
|
||||
forward_preference: true
|
||||
|
||||
controller_server_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 5.0
|
||||
publish_frequency: 2.0
|
||||
transform_tolerance: 0.5
|
||||
global_frame: odom_combined # 实车：EKF 融合后的里程计坐标系
|
||||
robot_base_frame: base_footprint # 实车：URDF 根帧
|
||||
use_sim_time: False
|
||||
rolling_window: true
|
||||
width: 3
|
||||
height: 3
|
||||
resolution: 0.05
|
||||
footprint: "[[0.14, 0.085],
|
||||
[0.14, -0.085],
|
||||
[-0.14, -0.085],
|
||||
[-0.14, 0.085]]"
|
||||
footprint_padding: 0.02
|
||||
plugins: ["voxel_layer", "inflation_layer"]
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.55
|
||||
voxel_layer:
|
||||
plugin: "nav2_costmap_2d::VoxelLayer"
|
||||
enabled: True
|
||||
publish_voxel_map: True
|
||||
origin_z: 0.0
|
||||
z_resolution: 0.05
|
||||
z_voxels: 16
|
||||
max_obstacle_height: 2.0
|
||||
mark_threshold: 0
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
static_layer:
|
||||
map_subscribe_transient_local: True
|
||||
always_send_full_costmap: True
|
||||
local_costmap_client:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
local_costmap_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
transform_tolerance: 0.5
|
||||
update_frequency: 1.0
|
||||
publish_frequency: 1.0
|
||||
global_frame: map
|
||||
robot_base_frame: base_footprint # 实车：与 URDF 根帧一致
|
||||
footprint: "[[0.14, 0.085],
|
||||
[0.14, -0.085],
|
||||
[-0.14, -0.085],
|
||||
[-0.14, 0.085]]"
|
||||
footprint_padding: 0.02
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
|
||||
obstacle_layer:
|
||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
||||
enabled: True
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.55
|
||||
always_send_full_costmap: True
|
||||
global_costmap_client:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
global_costmap_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
map_saver:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
save_map_timeout: 5.0
|
||||
free_thresh_default: 0.25
|
||||
occupied_thresh_default: 0.65
|
||||
map_subscribe_transient_local: True
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
planner_plugins: ["GridBased"]
|
||||
use_sim_time: False
|
||||
|
||||
GridBased:
|
||||
plugin: "nav2_smac_planner/SmacPlannerHybrid"
|
||||
downsample_costmap: false
|
||||
downsampling_factor: 1
|
||||
tolerance: 0.25
|
||||
allow_unknown: true
|
||||
max_iterations: 1000000
|
||||
max_on_approach_iterations: 1000
|
||||
max_planning_time: 5.0
|
||||
motion_model_for_search: "REEDS_SHEPP"
|
||||
angle_quantization_bins: 72
|
||||
analytic_expansion_ratio: 3.5
|
||||
analytic_expansion_max_length: 3.0
|
||||
minimum_turning_radius: 0.40
|
||||
reverse_penalty: 1.3
|
||||
change_penalty: 0.0
|
||||
non_straight_penalty: 1.2
|
||||
cost_penalty: 2.0
|
||||
retrospective_penalty: 0.015
|
||||
lookup_table_size: 20.0
|
||||
cache_obstacle_heuristic: false
|
||||
viz_expansions: false
|
||||
smooth_path: True
|
||||
|
||||
smoother:
|
||||
max_iterations: 1000
|
||||
w_smooth: 0.3
|
||||
w_data: 0.2
|
||||
tolerance: 1.0e-10
|
||||
do_refinement: true
|
||||
refinement_num: 2
|
||||
|
||||
planner_server_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
smoother_server:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
smoother_plugins: ["simple_smoother"]
|
||||
simple_smoother:
|
||||
plugin: "nav2_smoother::SimpleSmoother"
|
||||
tolerance: 1.0e-10
|
||||
max_its: 1000
|
||||
do_refinement: True
|
||||
|
||||
behavior_server:
|
||||
ros__parameters:
|
||||
costmap_topic: local_costmap/costmap_raw
|
||||
footprint_topic: local_costmap/published_footprint
|
||||
cycle_frequency: 10.0
|
||||
behavior_plugins: ["spin", "backup", "wait"]
|
||||
spin:
|
||||
plugin: "nav2_behaviors/Spin"
|
||||
backup:
|
||||
plugin: "nav2_behaviors/BackUp"
|
||||
backup_dist: 0.8
|
||||
backup_speed: 0.18
|
||||
wait:
|
||||
plugin: "nav2_behaviors/Wait"
|
||||
wait_duration: 0.5
|
||||
global_frame: odom_combined # 实车：EKF 融合后的里程计坐标系
|
||||
robot_base_frame: base_footprint # 实车：URDF 根帧
|
||||
transform_tolerance: 0.5
|
||||
use_sim_time: False
|
||||
simulate_ahead_time: 2.0
|
||||
max_rotational_vel: 1.0
|
||||
min_rotational_vel: 0.4
|
||||
rotational_acc_lim: 3.2
|
||||
|
||||
robot_state_publisher:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
waypoint_follower:
|
||||
ros__parameters:
|
||||
loop_rate: 20
|
||||
use_sim_time: False
|
||||
stop_on_failure: false
|
||||
waypoint_task_executor_plugin: "wait_at_waypoint"
|
||||
wait_at_waypoint:
|
||||
plugin: "nav2_waypoint_follower::WaitAtWaypoint"
|
||||
enabled: True
|
||||
waypoint_pause_duration: 200
|
||||
31
src/navigation/gc_navigation2_slamtoolbox/CMakeLists.txt
Normal file
31
src/navigation/gc_navigation2_slamtoolbox/CMakeLists.txt
Normal file
@@ -0,0 +1,31 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(gc_navigation2_slamtoolbox)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
# uncomment the following section in order to fill in
|
||||
# further dependencies manually.
|
||||
# find_package(<dependency> REQUIRED)
|
||||
# 查找slam_toolbox包
|
||||
find_package(slam_toolbox REQUIRED)
|
||||
install(
|
||||
DIRECTORY launch config params maps world
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
@@ -0,0 +1,89 @@
|
||||
|
||||
slam_toolbox:
|
||||
ros__parameters:
|
||||
|
||||
# Plugin params
|
||||
solver_plugin: solver_plugins::CeresSolver
|
||||
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
|
||||
ceres_preconditioner: SCHUR_JACOBI
|
||||
ceres_trust_strategy: LEVENBERG_MARQUARDT
|
||||
ceres_dogleg_type: TRADITIONAL_DOGLEG
|
||||
ceres_loss_function: None
|
||||
|
||||
# ROS Parameters
|
||||
odom_frame: odom
|
||||
map_frame: map
|
||||
base_frame: base_link
|
||||
scan_topic: /scan
|
||||
use_map_saver: true
|
||||
mode: mapping
|
||||
|
||||
# lifelong params
|
||||
lifelong_search_use_tree: false
|
||||
lifelong_minimum_score: 0.1
|
||||
lifelong_iou_match: 0.85
|
||||
lifelong_node_removal_score: 0.04
|
||||
lifelong_overlap_score_scale: 0.06
|
||||
lifelong_constraint_multiplier: 0.08
|
||||
lifelong_nearby_penalty: 0.001
|
||||
lifelong_candidates_scale: 0.03
|
||||
|
||||
# if you'd like to immediately start continuing a map at a given pose
|
||||
# or at the dock, but they are mutually exclusive, if pose is given
|
||||
# will use pose
|
||||
#map_file_name: test_steve
|
||||
#map_start_pose: [0.0, 0.0, 0.0]
|
||||
#map_start_at_dock: true
|
||||
|
||||
debug_logging: false
|
||||
throttle_scans: 1
|
||||
transform_publish_period: 0.02 #if 0 never publishes odometry
|
||||
map_update_interval: 5.0
|
||||
resolution: 0.05
|
||||
restamp_tf: false
|
||||
min_laser_range: 0.0 #for rastering images
|
||||
max_laser_range: 20.0 #for rastering images
|
||||
minimum_time_interval: 0.5
|
||||
transform_timeout: 0.2
|
||||
tf_buffer_duration: 10.
|
||||
stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
|
||||
|
||||
# General Parameters
|
||||
use_scan_matching: true
|
||||
use_scan_barycenter: true
|
||||
minimum_travel_distance: 0.5
|
||||
minimum_travel_heading: 0.5
|
||||
check_min_dist_and_heading_precisely: false
|
||||
scan_buffer_size: 10
|
||||
scan_buffer_maximum_scan_distance: 10.0
|
||||
link_match_minimum_response_fine: 0.1
|
||||
link_scan_maximum_distance: 1.5
|
||||
loop_search_maximum_distance: 3.0
|
||||
do_loop_closing: true
|
||||
loop_match_minimum_chain_size: 10
|
||||
loop_match_maximum_variance_coarse: 3.0
|
||||
loop_match_minimum_response_coarse: 0.35
|
||||
loop_match_minimum_response_fine: 0.45
|
||||
|
||||
# Correlation Parameters - Correlation Parameters
|
||||
correlation_search_space_dimension: 0.5
|
||||
correlation_search_space_resolution: 0.01
|
||||
correlation_search_space_smear_deviation: 0.1
|
||||
|
||||
# Correlation Parameters - Loop Closure Parameters
|
||||
loop_search_space_dimension: 8.0
|
||||
loop_search_space_resolution: 0.05
|
||||
loop_search_space_smear_deviation: 0.03
|
||||
|
||||
# Scan Matcher Parameters
|
||||
distance_variance_penalty: 0.5
|
||||
angle_variance_penalty: 1.0
|
||||
|
||||
fine_search_angle_offset: 0.00349
|
||||
coarse_search_angle_offset: 0.349
|
||||
coarse_angle_resolution: 0.0349
|
||||
minimum_angle_penalty: 0.9
|
||||
minimum_distance_penalty: 0.5
|
||||
use_response_expansion: true
|
||||
min_pass_through: 2
|
||||
occupancy_threshold: 0.1
|
||||
@@ -0,0 +1,72 @@
|
||||
slam_toolbox:
|
||||
ros__parameters:
|
||||
solver_plugin: solver_plugins::CeresSolver
|
||||
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
|
||||
ceres_preconditioner: SCHUR_JACOBI
|
||||
ceres_trust_strategy: LEVENBERG_MARQUARDT
|
||||
ceres_dogleg_type: TRADITIONAL_DOGLEG
|
||||
ceres_loss_function: None
|
||||
|
||||
# ROS Parameters
|
||||
odom_frame: odom
|
||||
map_frame: map
|
||||
base_frame: base_link
|
||||
scan_topic: /scan
|
||||
mode: localization #localization
|
||||
|
||||
# if you'd like to start localizing on bringup in a map and pose
|
||||
map_file_name: /home/guoch/test_ws/src/gc_navigation2_slamtoolbox/maps/my_map.posegraph
|
||||
map_start_pose: [0.0, 0.0, 0.0]
|
||||
|
||||
debug_logging: false
|
||||
throttle_scans: 1
|
||||
transform_publish_period: 0.02 #if 0 never publishes odometry
|
||||
map_update_interval: 5.0
|
||||
resolution: 0.05
|
||||
restamp_tf: false
|
||||
min_laser_range: 0.0 #for rastering images
|
||||
max_laser_range: 20.0 #for rastering images
|
||||
minimum_time_interval: 0.5
|
||||
transform_timeout: 0.2
|
||||
tf_buffer_duration: 30.
|
||||
stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
|
||||
|
||||
# General Parameters
|
||||
use_scan_matching: true
|
||||
use_scan_barycenter: true
|
||||
minimum_travel_distance: 0.5
|
||||
minimum_travel_heading: 0.5
|
||||
check_min_dist_and_heading_precisely: false
|
||||
scan_buffer_size: 3
|
||||
scan_buffer_maximum_scan_distance: 10.0
|
||||
link_match_minimum_response_fine: 0.1
|
||||
link_scan_maximum_distance: 1.5
|
||||
do_loop_closing: true
|
||||
loop_match_minimum_chain_size: 3
|
||||
loop_match_maximum_variance_coarse: 3.0
|
||||
loop_match_minimum_response_coarse: 0.35
|
||||
loop_match_minimum_response_fine: 0.45
|
||||
|
||||
# Correlation Parameters - Correlation Parameters
|
||||
correlation_search_space_dimension: 0.5
|
||||
correlation_search_space_resolution: 0.01
|
||||
correlation_search_space_smear_deviation: 0.1
|
||||
|
||||
# Correlation Parameters - Loop Closure Parameters
|
||||
loop_search_space_dimension: 8.0
|
||||
loop_search_space_resolution: 0.05
|
||||
loop_search_space_smear_deviation: 0.03
|
||||
loop_search_maximum_distance: 3.0
|
||||
|
||||
# Scan Matcher Parameters
|
||||
distance_variance_penalty: 0.5
|
||||
angle_variance_penalty: 1.0
|
||||
|
||||
fine_search_angle_offset: 0.00349
|
||||
coarse_search_angle_offset: 0.349
|
||||
coarse_angle_resolution: 0.0349
|
||||
minimum_angle_penalty: 0.9
|
||||
minimum_distance_penalty: 0.5
|
||||
use_response_expansion: true
|
||||
min_pass_through: 2
|
||||
occupancy_threshold: 0.1
|
||||
@@ -0,0 +1,71 @@
|
||||
slam_toolbox:
|
||||
ros__parameters:
|
||||
|
||||
# Plugin params
|
||||
solver_plugin: solver_plugins::CeresSolver
|
||||
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
|
||||
ceres_preconditioner: SCHUR_JACOBI
|
||||
ceres_trust_strategy: LEVENBERG_MARQUARDT
|
||||
ceres_dogleg_type: TRADITIONAL_DOGLEG
|
||||
ceres_loss_function: None
|
||||
|
||||
# ROS Parameters
|
||||
odom_frame: odom
|
||||
map_frame: map
|
||||
base_frame: base_footprint
|
||||
scan_topic: /scan
|
||||
use_map_saver: true
|
||||
mode: mapping #localization
|
||||
debug_logging: false
|
||||
throttle_scans: 1
|
||||
transform_publish_period: 0.02 #if 0 never publishes odometry
|
||||
map_update_interval: 10.0
|
||||
resolution: 0.05
|
||||
restamp_tf: false
|
||||
min_laser_range: 0.0 #for rastering images
|
||||
max_laser_range: 20.0 #for rastering images
|
||||
minimum_time_interval: 0.5
|
||||
transform_timeout: 0.2
|
||||
tf_buffer_duration: 14400.
|
||||
stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
|
||||
enable_interactive_mode: true
|
||||
|
||||
# General Parameters
|
||||
use_scan_matching: true
|
||||
use_scan_barycenter: true
|
||||
minimum_travel_distance: 0.5
|
||||
minimum_travel_heading: 0.5
|
||||
check_min_dist_and_heading_precisely: false
|
||||
scan_buffer_size: 10
|
||||
scan_buffer_maximum_scan_distance: 10.0
|
||||
link_match_minimum_response_fine: 0.1
|
||||
link_scan_maximum_distance: 1.5
|
||||
loop_search_maximum_distance: 3.0
|
||||
do_loop_closing: true
|
||||
loop_match_minimum_chain_size: 10
|
||||
loop_match_maximum_variance_coarse: 3.0
|
||||
loop_match_minimum_response_coarse: 0.35
|
||||
loop_match_minimum_response_fine: 0.45
|
||||
|
||||
# Correlation Parameters - Correlation Parameters
|
||||
correlation_search_space_dimension: 0.5
|
||||
correlation_search_space_resolution: 0.01
|
||||
correlation_search_space_smear_deviation: 0.1
|
||||
|
||||
# Correlation Parameters - Loop Closure Parameters
|
||||
loop_search_space_dimension: 8.0
|
||||
loop_search_space_resolution: 0.05
|
||||
loop_search_space_smear_deviation: 0.03
|
||||
|
||||
# Scan Matcher Parameters
|
||||
distance_variance_penalty: 0.5
|
||||
angle_variance_penalty: 1.0
|
||||
|
||||
fine_search_angle_offset: 0.00349
|
||||
coarse_search_angle_offset: 0.349
|
||||
coarse_angle_resolution: 0.0349
|
||||
minimum_angle_penalty: 0.9
|
||||
minimum_distance_penalty: 0.5
|
||||
use_response_expansion: true
|
||||
min_pass_through: 2
|
||||
occupancy_threshold: 0.1
|
||||
@@ -0,0 +1,74 @@
|
||||
$(var namespace)/slam_toolbox:
|
||||
ros__parameters:
|
||||
|
||||
# Plugin params
|
||||
solver_plugin: solver_plugins::CeresSolver
|
||||
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
|
||||
ceres_preconditioner: SCHUR_JACOBI
|
||||
ceres_trust_strategy: LEVENBERG_MARQUARDT
|
||||
ceres_dogleg_type: TRADITIONAL_DOGLEG
|
||||
ceres_loss_function: None
|
||||
|
||||
# ROS Parameters
|
||||
odom_frame: global_odom
|
||||
map_frame: map
|
||||
base_frame: base_footprint
|
||||
scan_topic: scan
|
||||
mode: mapping #localization
|
||||
|
||||
# if you'd like to immediately start continuing a map at a given pose
|
||||
# or at the dock, but they are mutually exclusive, if pose is given
|
||||
# will use pose
|
||||
#map_file_name: test_steve
|
||||
# map_start_pose: [0.0, 0.0, 0.0]
|
||||
#map_start_at_dock: true
|
||||
|
||||
debug_logging: false
|
||||
throttle_scans: 1
|
||||
transform_publish_period: 0.02 #if 0 never publishes odometry
|
||||
map_update_interval: 5.0
|
||||
resolution: 0.05
|
||||
max_laser_range: 20.0 #for rastering images
|
||||
minimum_time_interval: 0.5
|
||||
transform_timeout: 0.2
|
||||
tf_buffer_duration: 30.0
|
||||
stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
|
||||
enable_interactive_mode: true
|
||||
|
||||
# General Parameters
|
||||
use_scan_matching: true
|
||||
use_scan_barycenter: true
|
||||
minimum_travel_distance: 0.5
|
||||
minimum_travel_heading: 0.5
|
||||
check_min_dist_and_heading_precisely: false
|
||||
scan_buffer_size: 10
|
||||
scan_buffer_maximum_scan_distance: 10.0
|
||||
link_match_minimum_response_fine: 0.1
|
||||
link_scan_maximum_distance: 1.5
|
||||
loop_search_maximum_distance: 3.0
|
||||
do_loop_closing: true
|
||||
loop_match_minimum_chain_size: 10
|
||||
loop_match_maximum_variance_coarse: 3.0
|
||||
loop_match_minimum_response_coarse: 0.35
|
||||
loop_match_minimum_response_fine: 0.45
|
||||
|
||||
# Correlation Parameters - Correlation Parameters
|
||||
correlation_search_space_dimension: 0.5
|
||||
correlation_search_space_resolution: 0.01
|
||||
correlation_search_space_smear_deviation: 0.1
|
||||
|
||||
# Correlation Parameters - Loop Closure Parameters
|
||||
loop_search_space_dimension: 8.0
|
||||
loop_search_space_resolution: 0.05
|
||||
loop_search_space_smear_deviation: 0.03
|
||||
|
||||
# Scan Matcher Parameters
|
||||
distance_variance_penalty: 0.5
|
||||
angle_variance_penalty: 1.0
|
||||
|
||||
fine_search_angle_offset: 0.00349
|
||||
coarse_search_angle_offset: 0.349
|
||||
coarse_angle_resolution: 0.0349
|
||||
minimum_angle_penalty: 0.9
|
||||
minimum_distance_penalty: 0.5
|
||||
use_response_expansion: true
|
||||
@@ -0,0 +1,79 @@
|
||||
slam_toolbox:
|
||||
ros__parameters:
|
||||
|
||||
# Plugin params
|
||||
solver_plugin: solver_plugins::CeresSolver
|
||||
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
|
||||
ceres_preconditioner: SCHUR_JACOBI
|
||||
ceres_trust_strategy: LEVENBERG_MARQUARDT
|
||||
ceres_dogleg_type: TRADITIONAL_DOGLEG
|
||||
ceres_loss_function: None
|
||||
|
||||
# ROS Parameters
|
||||
odom_frame: odom
|
||||
map_frame: map
|
||||
base_frame: base_footprint
|
||||
scan_topic: /scan
|
||||
use_map_saver: true
|
||||
mode: mapping #localization
|
||||
|
||||
# if you'd like to immediately start continuing a map at a given pose
|
||||
# or at the dock, but they are mutually exclusive, if pose is given
|
||||
# will use pose
|
||||
#map_file_name: test_steve
|
||||
#map_start_pose: [0.0, 0.0, 0.0]
|
||||
#map_start_at_dock: true
|
||||
|
||||
debug_logging: false
|
||||
throttle_scans: 1
|
||||
transform_publish_period: 0.02 #if 0 never publishes odometry
|
||||
map_update_interval: 5.0
|
||||
resolution: 0.05
|
||||
restamp_tf: false
|
||||
min_laser_range: 0.0 #for rastering images
|
||||
max_laser_range: 20.0 #for rastering images
|
||||
minimum_time_interval: 0.5
|
||||
transform_timeout: 0.2
|
||||
tf_buffer_duration: 30.
|
||||
stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
|
||||
enable_interactive_mode: true
|
||||
|
||||
# General Parameters
|
||||
use_scan_matching: true
|
||||
use_scan_barycenter: true
|
||||
minimum_travel_distance: 0.5
|
||||
minimum_travel_heading: 0.5
|
||||
check_min_dist_and_heading_precisely: false
|
||||
scan_buffer_size: 10
|
||||
scan_buffer_maximum_scan_distance: 10.0
|
||||
link_match_minimum_response_fine: 0.1
|
||||
link_scan_maximum_distance: 1.5
|
||||
loop_search_maximum_distance: 3.0
|
||||
do_loop_closing: true
|
||||
loop_match_minimum_chain_size: 10
|
||||
loop_match_maximum_variance_coarse: 3.0
|
||||
loop_match_minimum_response_coarse: 0.35
|
||||
loop_match_minimum_response_fine: 0.45
|
||||
|
||||
# Correlation Parameters - Correlation Parameters
|
||||
correlation_search_space_dimension: 0.5
|
||||
correlation_search_space_resolution: 0.01
|
||||
correlation_search_space_smear_deviation: 0.1
|
||||
|
||||
# Correlation Parameters - Loop Closure Parameters
|
||||
loop_search_space_dimension: 8.0
|
||||
loop_search_space_resolution: 0.05
|
||||
loop_search_space_smear_deviation: 0.03
|
||||
|
||||
# Scan Matcher Parameters
|
||||
distance_variance_penalty: 0.5
|
||||
angle_variance_penalty: 1.0
|
||||
|
||||
fine_search_angle_offset: 0.00349
|
||||
coarse_search_angle_offset: 0.349
|
||||
coarse_angle_resolution: 0.0349
|
||||
minimum_angle_penalty: 0.9
|
||||
minimum_distance_penalty: 0.5
|
||||
use_response_expansion: true
|
||||
min_pass_through: 2
|
||||
occupancy_threshold: 0.1
|
||||
@@ -0,0 +1,29 @@
|
||||
<!--
|
||||
Ackermann navigate-through-poses behavior tree.
|
||||
Keeps periodic replanning while following. Recovery is intentionally short:
|
||||
local controller clear on path-follow failure, then BackUp only at the top level.
|
||||
-->
|
||||
<root main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
|
||||
<PipelineSequence name="NavigateWithReplanning">
|
||||
<RateController hz="1.0">
|
||||
<ReactiveSequence>
|
||||
<RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7"/>
|
||||
<ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="GridBased"/>
|
||||
</ReactiveSequence>
|
||||
</RateController>
|
||||
<RecoveryNode number_of_retries="1" name="FollowPath">
|
||||
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||
</RecoveryNode>
|
||||
</PipelineSequence>
|
||||
<ReactiveFallback name="RecoveryFallback">
|
||||
<GoalUpdated/>
|
||||
<RoundRobin name="RecoveryActions">
|
||||
<BackUp backup_dist="0.80" backup_speed="0.18"/>
|
||||
</RoundRobin>
|
||||
</ReactiveFallback>
|
||||
</RecoveryNode>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
@@ -0,0 +1,26 @@
|
||||
<!--
|
||||
Ackermann navigation behavior tree.
|
||||
Keeps 1 Hz replanning while following. Recovery is intentionally short:
|
||||
local controller clear on path-follow failure, then BackUp only at the top level.
|
||||
-->
|
||||
<root main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
|
||||
<PipelineSequence name="NavigateWithReplanning">
|
||||
<RateController hz="0.5">
|
||||
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||
</RateController>
|
||||
<RecoveryNode number_of_retries="1" name="FollowPath">
|
||||
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||
</RecoveryNode>
|
||||
</PipelineSequence>
|
||||
<ReactiveFallback name="RecoveryFallback">
|
||||
<GoalUpdated/>
|
||||
<RoundRobin name="RecoveryActions">
|
||||
<BackUp backup_dist="0.80" backup_speed="0.18"/>
|
||||
</RoundRobin>
|
||||
</ReactiveFallback>
|
||||
</RecoveryNode>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
@@ -0,0 +1,82 @@
|
||||
slam_toolbox:
|
||||
ros__parameters:
|
||||
|
||||
# ============================ Solver 插件参数 ======================================
|
||||
solver_plugin: solver_plugins::CeresSolver
|
||||
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
|
||||
ceres_preconditioner: SCHUR_JACOBI
|
||||
ceres_trust_strategy: LEVENBERG_MARQUARDT
|
||||
ceres_dogleg_type: TRADITIONAL_DOGLEG
|
||||
ceres_loss_function: None
|
||||
|
||||
# ============================ ROS 基础参数 =========================================
|
||||
odom_frame: odom # 里程计坐标系
|
||||
map_frame: map # 地图坐标系
|
||||
base_frame: base_link # 机器人基座坐标系（与 URDF 根帧一致）
|
||||
scan_topic: /scan # 激光雷达话题
|
||||
use_map_saver: true # 启用地图保存功能
|
||||
mode: mapping # mapping = 定位 + 实时建图（地图可更新）
|
||||
|
||||
# ============================ 已有地图加载（在其基础上继续更新）==========================
|
||||
# 启动时暂不加载（注释掉），通过服务在启动后手动加载：
|
||||
# ros2 service call /slam_toolbox/deserialize_map slam_toolbox/srv/DeserializePoseGraph "{filename: '/home/guoch/test_ws/src/gc_navigation2_slamtoolbox/maps/my_map.posegraph', match_type: 1}"
|
||||
# map_file_name: /home/guoch/test_ws/src/gc_navigation2_slamtoolbox/maps/my_map.posegraph
|
||||
map_start_pose: [-2.0, -2.0, 0.0] # 初始位姿 [x, y, yaw]（map 坐标系下）
|
||||
# map_start_at_dock: true # 或从 docking 位姿启动（与 pose 互斥）
|
||||
|
||||
# ============================ 调试与性能 ===========================================
|
||||
debug_logging: false # 调试日志（生产环境关闭）
|
||||
throttle_scans: 10 # 每 N 帧激光处理一次（1=全部处理）
|
||||
transform_publish_period: 0.02 # TF 发布周期（秒），0=不发布里程计
|
||||
map_update_interval: 3.0 # /map 话题更新间隔（秒），越小越实时
|
||||
resolution: 0.05 # 地图分辨率（米/像素）
|
||||
restamp_tf: false # 是否重新打时间戳
|
||||
min_laser_range: 0.0 # 激光最小有效距离（米）
|
||||
max_laser_range: 20.0 # 激光最大有效距离（米）
|
||||
minimum_time_interval: 0.5 # 最小处理时间间隔（秒）
|
||||
transform_timeout: 0.2 # TF 查找超时（秒）
|
||||
tf_buffer_duration: 30.0 # TF 缓冲区时长（秒）
|
||||
stack_size_to_use: 40000000 # 栈大小（序列化大地图需要）
|
||||
enable_interactive_mode: true # 交互模式：允许外部工具编辑地图
|
||||
|
||||
# ============================ 通用建图参数 =========================================
|
||||
use_scan_matching: true # 启用扫描匹配
|
||||
use_scan_barycenter: true # 使用扫描重心
|
||||
minimum_travel_distance: 0.1 # 最小移动距离触发处理（米）
|
||||
minimum_travel_heading: 0.1 # 最小转向角度触发处理（弧度）
|
||||
check_min_dist_and_heading_precisely: false
|
||||
scan_buffer_size: 5 # 扫描缓冲区大小（越大匹配越稳定）
|
||||
scan_buffer_maximum_scan_distance: 5.0 # 缓冲区最大扫描距离
|
||||
link_match_minimum_response_fine: 0.1 # 精细匹配最小响应
|
||||
link_scan_maximum_distance: 1.5 # 链接扫描最大距离
|
||||
|
||||
# ============================ 回环检测参数 =========================================
|
||||
do_loop_closing: true # 启用回环检测（消除累积误差）
|
||||
loop_match_minimum_chain_size: 10 # 最小回环链大小
|
||||
loop_match_maximum_variance_coarse: 3.0 # 粗匹配最大方差
|
||||
loop_match_minimum_response_coarse: 0.35 # 粗匹配最小响应
|
||||
loop_match_minimum_response_fine: 0.45 # 精细匹配最小响应
|
||||
loop_search_maximum_distance: 3.0 # 回环搜索最大距离
|
||||
|
||||
# ============================ 相关性匹配 - 扫描匹配参数 ==============================
|
||||
correlation_search_space_dimension: 0.5 # 搜索空间维度
|
||||
correlation_search_space_resolution: 0.01 # 搜索空间分辨率
|
||||
correlation_search_space_smear_deviation: 0.1 # 搜索空间平滑偏差
|
||||
|
||||
# ============================ 相关性匹配 - 回环检测参数 ==============================
|
||||
loop_search_space_dimension: 8.0 # 回环搜索空间维度
|
||||
loop_search_space_resolution: 0.05 # 回环搜索分辨率
|
||||
loop_search_space_smear_deviation: 0.03 # 回环搜索平滑偏差
|
||||
|
||||
# ============================ 扫描匹配器参数 ========================================
|
||||
distance_variance_penalty: 0.5 # 距离方差惩罚
|
||||
angle_variance_penalty: 1.0 # 角度方差惩罚
|
||||
|
||||
fine_search_angle_offset: 0.00349 # 精细搜索角度偏移
|
||||
coarse_search_angle_offset: 0.349 # 粗搜索角度偏移
|
||||
coarse_angle_resolution: 0.0349 # 粗搜索角度分辨率
|
||||
minimum_angle_penalty: 0.9 # 最小角度惩罚
|
||||
minimum_distance_penalty: 0.5 # 最小距离惩罚
|
||||
use_response_expansion: true # 使用响应扩展
|
||||
min_pass_through: 2 # 最小通过次数
|
||||
occupancy_threshold: 0.1 # 占据阈值
|
||||
@@ -0,0 +1,55 @@
|
||||
slam_toolbox:
|
||||
ros__parameters:
|
||||
# 核心模式设置
|
||||
use_sim_time: true
|
||||
mode: "localization" # 重要：设置为定位模式
|
||||
map_file_name: "" # 由启动参数传入
|
||||
map_start_pose: [0.0,0.0,0.0]
|
||||
# ROS框架设置
|
||||
odom_frame: "odom"
|
||||
map_frame: "map"
|
||||
base_frame: "base_footprint"
|
||||
scan_topic: "/scan"
|
||||
|
||||
# 求解器设置
|
||||
solver_plugin: "solver_plugins::CeresSolver"
|
||||
ceres_linear_solver: "SPARSE_NORMAL_CHOLESKY"
|
||||
ceres_preconditioner: "SCHUR_JACOBI"
|
||||
ceres_trust_strategy: "LEVENBERG_MARQUARDT"
|
||||
ceres_dogleg_type: "TRADITIONAL_DOGLEG"
|
||||
ceres_loss_function: "None"
|
||||
|
||||
# 定位优化参数
|
||||
use_scan_matching: true
|
||||
use_scan_barycenter: true
|
||||
minimum_travel_distance: 0.05
|
||||
minimum_travel_heading: 0.087
|
||||
|
||||
# 扫描处理
|
||||
scan_buffer_size: 50
|
||||
throttle_scans: 2
|
||||
|
||||
# 回环检测（定位模式可关闭）
|
||||
do_loop_closing: false
|
||||
|
||||
# 搜索参数
|
||||
link_match_minimum_response_fine: 0.2
|
||||
link_scan_maximum_distance: 2.0
|
||||
loop_search_maximum_distance: 5.0
|
||||
|
||||
# 性能参数
|
||||
minimum_time_interval: 0.1
|
||||
transform_publish_period: 0.02
|
||||
map_update_interval: 0.5
|
||||
|
||||
# 地图参数
|
||||
resolution: 0.05
|
||||
max_laser_range: 15.0
|
||||
min_laser_range: 0.2
|
||||
|
||||
# 初始位姿（可选，可以通过RViz设置）
|
||||
# map_start_pose: [0.0, 0.0, 0.0]
|
||||
|
||||
# 调试
|
||||
debug_logging: false
|
||||
enable_interactive_mode: true
|
||||
@@ -0,0 +1,73 @@
|
||||
slam_toolbox:
|
||||
ros__parameters:
|
||||
|
||||
# === Solver ===
|
||||
solver_plugin: solver_plugins::CeresSolver
|
||||
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
|
||||
ceres_preconditioner: SCHUR_JACOBI
|
||||
ceres_trust_strategy: LEVENBERG_MARQUARDT
|
||||
ceres_dogleg_type: TRADITIONAL_DOGLEG
|
||||
ceres_loss_function: None
|
||||
|
||||
# === ROS 基础 ===
|
||||
odom_frame: odom
|
||||
map_frame: map
|
||||
base_frame: base_link
|
||||
scan_topic: /scan
|
||||
mode: mapping
|
||||
use_map_saver: true
|
||||
|
||||
# === 调试与性能 ===
|
||||
debug_logging: false
|
||||
throttle_scans: 2
|
||||
transform_publish_period: 0.02
|
||||
map_update_interval: 0.8
|
||||
resolution: 0.05
|
||||
min_laser_range: 0.15
|
||||
max_laser_range: 10.0
|
||||
minimum_time_interval: 0.7
|
||||
transform_timeout: 0.2
|
||||
tf_buffer_duration: 30.0
|
||||
stack_size_to_use: 40000000
|
||||
enable_interactive_mode: true
|
||||
|
||||
# === 建图参数 ===
|
||||
use_scan_matching: true
|
||||
use_scan_barycenter: true
|
||||
minimum_travel_distance: 0.5
|
||||
minimum_travel_heading: 0.1
|
||||
scan_buffer_size: 10
|
||||
scan_buffer_maximum_scan_distance: 10.0
|
||||
link_match_minimum_response_fine: 0.1
|
||||
link_scan_maximum_distance: 1.5
|
||||
|
||||
# === 回环检测 ===
|
||||
do_loop_closing: true
|
||||
loop_match_minimum_chain_size: 10
|
||||
loop_match_maximum_variance_coarse: 3.0
|
||||
loop_match_minimum_response_coarse: 0.35
|
||||
loop_match_minimum_response_fine: 0.45
|
||||
loop_search_maximum_distance: 3.0
|
||||
|
||||
# === 扫描匹配 ===
|
||||
correlation_search_space_dimension: 0.5
|
||||
correlation_search_space_resolution: 0.01
|
||||
correlation_search_space_smear_deviation: 0.1
|
||||
loop_search_space_dimension: 8.0
|
||||
loop_search_space_resolution: 0.05
|
||||
loop_search_space_smear_deviation: 0.03
|
||||
|
||||
# === 匹配器参数 ===
|
||||
distance_variance_penalty: 0.5
|
||||
angle_variance_penalty: 1.0
|
||||
fine_search_angle_offset: 0.00349
|
||||
coarse_search_angle_offset: 0.349
|
||||
coarse_angle_resolution: 0.0349
|
||||
minimum_angle_penalty: 0.9
|
||||
minimum_distance_penalty: 0.5
|
||||
use_response_expansion: true
|
||||
min_pass_through: 2
|
||||
occupancy_threshold: 0.1
|
||||
|
||||
# MessageFilter queue size (default 1, too small for high-rate lidar)
|
||||
scan_queue_size: 20
|
||||
@@ -0,0 +1,156 @@
|
||||
# ============================================================================
|
||||
# gc_nav2_with_amcl.launch.py
|
||||
# 功能：基于 AMCL（自适应蒙特卡洛定位）的导航启动文件
|
||||
#
|
||||
# 架构：
|
||||
# Gazebo 仿真环境
|
||||
# ├── robot_state_publisher — 发布机器人 TF 树（URDF 模型）
|
||||
# └── nav2_bringup_launch — Nav2 一体化启动（包含以下子模块）：
|
||||
# ├── map_server — 加载 my_map.yaml,发布全量静态地图到 /map
|
||||
# ├── AMCL — 粒子滤波定位,发布 map→odom TF
|
||||
# │ 需要手动设置 /initialpose 初始位姿
|
||||
# ├── planner_server — 全局/局部路径规划器（SmacHybrid）
|
||||
# ├── controller_server — 路径跟踪控制器（MPPI Ackermann）
|
||||
# ├── behavior_server — 恢复行为服务器（spin/backup/wait）
|
||||
# └── bt_navigator — 行为树导航编排器
|
||||
#
|
||||
# 与 SLAM 定位方案的区别：
|
||||
# AMCL： 粒子滤波定位,需要手动设置初始位姿,地图固定不更新
|
||||
# SLAM定位： 激光扫描匹配定位,自动定位,可加载 .posegraph 序列化地图
|
||||
# ============================================================================
|
||||
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, TimerAction
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.parameter_descriptions import ParameterValue
|
||||
from launch.substitutions import Command
|
||||
from launch.actions import ExecuteProcess
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
"""生成 LaunchDescription,启动 Gazebo + AMCL定位 + Nav2 导航"""
|
||||
ld = LaunchDescription()
|
||||
|
||||
# =============================1.定位到包的地址=============================================================
|
||||
# 通过 ament_index 获取各功能包的安装路径
|
||||
gc_navigation_fish_dir = get_package_share_directory(
|
||||
'gc_navigation2_slamtoolbox')
|
||||
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
|
||||
origincar_urdf_dir = get_package_share_directory(
|
||||
'origincar_description')
|
||||
|
||||
# =============================2.声明参数,获取配置文件路径===================================================
|
||||
# use_sim_time: 仿真时间开关。Gazebo 下必须为 True,
|
||||
# 因为仿真环境通过 /clock 话题提供时间,而非系统时间
|
||||
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
|
||||
|
||||
# map_yaml_path: 全量静态地图 yaml 文件路径
|
||||
# 传给 bringup_launch → map_server 加载并发布到 /map
|
||||
map_yaml_path = LaunchConfiguration('map', default=os.path.join(
|
||||
gc_navigation_fish_dir, 'maps', 'my_map.yaml'))
|
||||
|
||||
# nav2_param_path: Nav2 导航栈参数文件（含 AMCL 粒子滤波配置）
|
||||
# 注意：此处用 gc_navigation_amcl.yaml,包含 AMCL 参数（粒子数、运动模型等）
|
||||
# SLAM 方案则用 gc_navigation_slam.yaml（不含 AMCL）
|
||||
nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
|
||||
gc_navigation_fish_dir, 'params', 'gc_navigation_amcl.yaml'))
|
||||
|
||||
# =============================3.声明启动launch文件==========================================================
|
||||
# bringup_launch.py 是 Nav2 的一体化启动入口,内部自动处理：
|
||||
# 1. 声明所有 launch 参数（map, slam, use_sim_time, params_file 等）
|
||||
# 2. 根据 slam 参数决定启动模式：
|
||||
# slam=False（默认）→ localization_launch（map_server + AMCL）
|
||||
# slam=True → slam_launch（slam_toolbox 在线建图）
|
||||
# 3. 启动 navigation_launch（planner + controller + behavior + bt_navigator）
|
||||
# 4. 通过 RewrittenYaml 实现参数文件中的变量替换
|
||||
#
|
||||
# 传入参数：
|
||||
# map: 地图 yaml 文件路径（map_server 加载,AMCL 定位）
|
||||
# use_sim_time: 仿真时间模式（Gazebo 下必须为 true）
|
||||
# params_file: Nav2 全部节点的参数配置
|
||||
nav2_bringup_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
[nav2_bringup_dir, '/launch', '/bringup_launch.py']),
|
||||
launch_arguments={
|
||||
'map': map_yaml_path,
|
||||
'use_sim_time': use_sim_time,
|
||||
'params_file': nav2_param_path}.items(),
|
||||
)
|
||||
|
||||
# =============================4.Gazebo仿真设置==========================================================
|
||||
# robot_name_in_model: Gazebo 中机器人的模型名称,需与 URDF 中一致
|
||||
robot_name_in_model = 'mycar'
|
||||
|
||||
# default_model_path: 机器人 URDF 模型文件路径
|
||||
# 使用 xacro 宏展开生成最终的 URDF
|
||||
default_model_path = os.path.join(
|
||||
origincar_urdf_dir, "urdf", "origincar.urdf")
|
||||
|
||||
# 声明 model 启动参数,支持命令行覆盖：ros2 launch ... model:=/path/to/custom.urdf
|
||||
model = DeclareLaunchArgument(
|
||||
name="model", default_value=default_model_path)
|
||||
|
||||
# Gazebo 仿真世界文件路径
|
||||
gazebo_world_path = os.path.join(origincar_urdf_dir, 'world/test.world')
|
||||
|
||||
# 启动 Gazebo 仿真器进程
|
||||
# --verbose: 输出详细日志
|
||||
# -s libgazebo_ros_init.so: 加载 ROS ↔ Gazebo 通信初始化插件
|
||||
# -s libgazebo_ros_factory.so: 加载模型生成（spawn_entity）插件
|
||||
start_gazebo_cmd = ExecuteProcess(
|
||||
cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so',
|
||||
'-s', 'libgazebo_ros_factory.so', gazebo_world_path],
|
||||
output='screen')
|
||||
|
||||
# 在 Gazebo 世界中生成机器人模型
|
||||
# -entity: 模型在 Gazebo 中的实例名（mycar）
|
||||
# -file: 要加载的 URDF 模型文件
|
||||
# 生成后 Gazebo 会为模型创建对应的关节状态话题等
|
||||
spawn_entity_cmd = Node(
|
||||
package='gazebo_ros',
|
||||
executable='spawn_entity.py',
|
||||
arguments=['-entity', robot_name_in_model,
|
||||
'-file', default_model_path],
|
||||
output='screen'
|
||||
)
|
||||
|
||||
# robot_description: 将 XACRO/URDF 模型字符串设为 ROS 参数
|
||||
# robot_state_publisher 读取此参数发布各连杆的 TF 坐标变换
|
||||
robot_description = ParameterValue(
|
||||
Command(["xacro ", LaunchConfiguration("model")]), value_type=str)
|
||||
|
||||
# robot_state_publisher: 发布机器人各关节、连杆的 TF 变换
|
||||
# - 订阅 joint_states 话题获取关节角度
|
||||
# - 根据 URDF 模型计算 base_link → laser_link, wheel_link 等变换
|
||||
# - publish_frequency=30Hz 确保 TF 更新平滑
|
||||
# - use_sim_time=True 使用 /clock 仿真时间
|
||||
robot_state_publisher = Node(
|
||||
package="robot_state_publisher",
|
||||
executable="robot_state_publisher",
|
||||
parameters=[{"robot_description": robot_description,
|
||||
'use_sim_time': True, 'publish_frequency': 30.0}]
|
||||
)
|
||||
|
||||
# joint_state_publisher: 发布非固定关节的默认状态
|
||||
# 如果 Gazebo 已经发布 joint_states,此节点可以注释掉避免冲突
|
||||
# joint_state_publisher = Node(
|
||||
# package="joint_state_publisher",
|
||||
# executable="joint_state_publisher"
|
||||
# )
|
||||
|
||||
# =============================5.将所有 Action 添加到 LaunchDescription ==================================
|
||||
# 注意：nav2_bringup_launch 必须在 Gazebo 启动之后,
|
||||
# 否则会出现 /clock 话题未就绪的问题
|
||||
# 可通过 TimerAction 添加延迟,或依赖 launch 系统的自动排序
|
||||
ld.add_action(model) # 1. 声明模型路径参数
|
||||
ld.add_action(nav2_bringup_launch) # 2. 启动 Nav2 一体化（map_server + AMCL + 导航）
|
||||
ld.add_action(start_gazebo_cmd) # 3. 启动 Gazebo 仿真器
|
||||
ld.add_action(spawn_entity_cmd) # 4. 在 Gazebo 中生成机器人
|
||||
ld.add_action(robot_state_publisher) # 5. 发布机器人 TF 树
|
||||
# ld.add_action(joint_state_publisher) # 6. (可选) 默认关节状态发布
|
||||
|
||||
return ld
|
||||
@@ -0,0 +1,196 @@
|
||||
# ============================================================================
|
||||
# gc_nav2_with_slam.launch.py
|
||||
# 功能：基于 slam_toolbox 定位的导航启动文件
|
||||
#
|
||||
# 架构：
|
||||
# Gazebo 仿真环境
|
||||
# ├── robot_state_publisher — 发布机器人 TF 树（URDF 模型）
|
||||
# ├── map_server — 加载 my_map.yaml，发布全量静态地图到 /map
|
||||
# ├── slam_toolbox — 加载 my_map.posegraph，激光扫描匹配定位
|
||||
# │ 发布 map→odom TF，替代 AMCL 的粒子滤波
|
||||
# │ ⚠️ /map 话题重映射为 /slam_map，避免冲突
|
||||
# └── navigation_launch — Nav2 导航栈（路径规划 + 运动控制）
|
||||
# global_costmap 从 /map 获取全量静态地图
|
||||
#
|
||||
# 与 AMCL 方案的区别：
|
||||
# AMCL： 粒子滤波定位，需要手动设置初始位姿
|
||||
# SLAM定位： 激光扫描匹配定位，自动根据地图特征定位
|
||||
# ============================================================================
|
||||
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.parameter_descriptions import ParameterValue
|
||||
from launch.substitutions import Command
|
||||
from launch.actions import ExecuteProcess
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
"""生成 LaunchDescription，启动 Gazebo + SLAM定位 + Nav2 导航"""
|
||||
ld = LaunchDescription()
|
||||
|
||||
# =============================1.定位到包的地址=============================================================
|
||||
# 通过 ament_index 获取各功能包的安装路径
|
||||
pkg_dir = get_package_share_directory('gc_navigation2_slamtoolbox')
|
||||
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
|
||||
origincar_urdf_dir = get_package_share_directory('origincar_description')
|
||||
|
||||
# =============================2.声明参数===============================================================
|
||||
# use_sim_time: 仿真时间开关。Gazebo 下必须为 True，
|
||||
# 因为仿真环境通过 /clock 话题提供时间，而非系统时间
|
||||
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
|
||||
|
||||
# map_yaml_path: 全量静态地图 yaml 文件路径
|
||||
# 由 map_server 加载，发布到 /map 供 global_costmap 使用
|
||||
map_yaml_path = LaunchConfiguration('map', default=os.path.join(
|
||||
pkg_dir, 'maps', 'my_map.yaml'))
|
||||
|
||||
# nav2_param_path: Nav2 导航栈参数文件（不含 AMCL，含 slam_toolbox）
|
||||
nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
|
||||
pkg_dir, 'params', 'gc_navigation_slam.yaml'))
|
||||
|
||||
# slam_params_file: slam_toolbox 定位模式的参数文件
|
||||
# mode: localization — 只定位不建图
|
||||
# 包含 solver 配置、激光匹配参数、TF 帧定义等
|
||||
slam_params_file = os.path.join(
|
||||
pkg_dir, 'config', 'mapper_params_localization.yaml')
|
||||
|
||||
# posegraph_path: slam_toolbox 定位所需的序列化地图文件
|
||||
# .posegraph 文件保存了 slam_toolbox 建图时的位姿图
|
||||
# 配合 .data 文件，slam_toolbox 可以还原完整地图用于激光匹配
|
||||
posegraph_path = os.path.join(
|
||||
pkg_dir, 'maps', 'my_map.posegraph')
|
||||
|
||||
# =============================3.slam_toolbox 定位模式（替代 AMCL）=========================================
|
||||
# localization_slam_toolbox_node:
|
||||
# - 加载 .posegraph 序列化地图用于激光扫描匹配
|
||||
# - 发布 map→odom 坐标变换（替代 AMCL 的粒子滤波定位）
|
||||
# - 发布 /pose 话题（当前定位位姿）
|
||||
#
|
||||
# ⚠️ 关键：将 slam_toolbox 的 /map 重映射为 /slam_map
|
||||
# 避免覆盖 map_server 发布到 /map 的全量静态地图
|
||||
# 确保 global_costmap 的 static_layer 能获取完整地图边界
|
||||
slam_toolbox_node = Node(
|
||||
package='slam_toolbox',
|
||||
executable='localization_slam_toolbox_node',
|
||||
name='slam_toolbox',
|
||||
output='screen',
|
||||
parameters=[slam_params_file,
|
||||
{'use_sim_time': True,
|
||||
'map_file_name': posegraph_path}],
|
||||
remappings=[('/map', '/slam_map')],
|
||||
)
|
||||
|
||||
# =============================4.map_server 提供全量静态地图给 global_costmap =============================
|
||||
# map_server:
|
||||
# - 加载 my_map.yaml + my_map.pgm 格式的传统静态地图
|
||||
# - 发布完整地图到 /map 话题（199×266 像素，分辨率 0.05 m）
|
||||
# - 这是 global_costmap 静态层的唯一数据来源
|
||||
# - 是 lifecycle 节点，需要 lifecycle_manager 激活
|
||||
map_server_node = Node(
|
||||
package='nav2_map_server',
|
||||
executable='map_server',
|
||||
name='map_server',
|
||||
output='screen',
|
||||
parameters=[{'yaml_filename': map_yaml_path,
|
||||
'use_sim_time': True}],
|
||||
)
|
||||
|
||||
# lifecycle_manager: 管理 map_server 的生命周期（configure → activate）
|
||||
# autostart=True 表示启动后自动激活 map_server
|
||||
map_server_lifecycle = Node(
|
||||
package='nav2_lifecycle_manager',
|
||||
executable='lifecycle_manager',
|
||||
name='lifecycle_manager_map',
|
||||
output='screen',
|
||||
parameters=[{'use_sim_time': True,
|
||||
'autostart': True,
|
||||
'node_names': ['map_server']}],
|
||||
)
|
||||
|
||||
# =============================5.仅启动 nav2 navigation（不含 localization）=================================
|
||||
# 为什么用 navigation_launch 而不是 bringup_launch？
|
||||
# bringup_launch 包含 localization_launch（AMCL + map_server）
|
||||
# 由于 slam_toolbox 已经替代了 AMCL，只需 navigation_launch 的：
|
||||
# - planner_server — 全局/局部路径规划
|
||||
# - controller_server — 路径跟踪控制（MPPI）
|
||||
# - behavior_server — 行为树（spin/backup/wait 恢复）
|
||||
# - bt_navigator — 行为树导航编排
|
||||
# - costmap 层 — global_costmap + local_costmap
|
||||
navigation_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
[nav2_bringup_dir, '/launch', '/navigation_launch.py']),
|
||||
launch_arguments={
|
||||
'use_sim_time': use_sim_time,
|
||||
'params_file': nav2_param_path}.items(),
|
||||
)
|
||||
|
||||
# =============================6.Gazebo仿真设置==========================================================
|
||||
# robot_name_in_model: Gazebo 中机器人的模型名称，需与 URDF 中一致
|
||||
robot_name_in_model = 'mycar'
|
||||
|
||||
# default_model_path: 机器人 URDF 模型文件路径
|
||||
# 使用 xacro 预处理生成最终的 URDF
|
||||
default_model_path = os.path.join(
|
||||
origincar_urdf_dir, "urdf", "origincar.urdf")
|
||||
|
||||
# 声明 model 启动参数，支持命令行覆盖 URDF 路径
|
||||
model = DeclareLaunchArgument(
|
||||
name="model", default_value=default_model_path)
|
||||
|
||||
# Gazebo 世界文件路径
|
||||
gazebo_world_path = os.path.join(origincar_urdf_dir, 'world/test.world')
|
||||
|
||||
# 启动 Gazebo 仿真器
|
||||
# --verbose: 详细日志输出
|
||||
# -s libgazebo_ros_init.so: 加载 ROS-Gazebo 初始化插件
|
||||
# -s libgazebo_ros_factory.so: 加载模型生成插件
|
||||
start_gazebo_cmd = ExecuteProcess(
|
||||
cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so',
|
||||
'-s', 'libgazebo_ros_factory.so', gazebo_world_path],
|
||||
output='screen')
|
||||
|
||||
# 在 Gazebo 中生成机器人模型
|
||||
# -entity: 模型实例名称
|
||||
# -file: URDF 模型文件
|
||||
spawn_entity_cmd = Node(
|
||||
package='gazebo_ros',
|
||||
executable='spawn_entity.py',
|
||||
arguments=['-entity', robot_name_in_model,
|
||||
'-file', default_model_path],
|
||||
output='screen'
|
||||
)
|
||||
|
||||
# robot_description: 将 URDF/XACRO 模型转换为 ROS 参数
|
||||
# robot_state_publisher 订阅此参数发布 TF 树
|
||||
robot_description = ParameterValue(
|
||||
Command(["xacro ", LaunchConfiguration("model")]), value_type=str)
|
||||
|
||||
# robot_state_publisher: 发布机器人各连杆的 TF 变换
|
||||
# base_link → laser_link, wheel_link 等关节状态
|
||||
# publish_frequency=30Hz 保证平滑的 TF 更新
|
||||
robot_state_publisher = Node(
|
||||
package="robot_state_publisher",
|
||||
executable="robot_state_publisher",
|
||||
parameters=[{"robot_description": robot_description,
|
||||
'use_sim_time': True, 'publish_frequency': 30.0}]
|
||||
)
|
||||
|
||||
# =============================7.将所有 Action 添加到 LaunchDescription ==================================
|
||||
# 启动顺序由 Launch 系统自动管理依赖
|
||||
ld.add_action(model) # 1. 声明模型参数
|
||||
ld.add_action(start_gazebo_cmd) # 2. 启动 Gazebo 仿真器
|
||||
ld.add_action(spawn_entity_cmd) # 3. 生成机器人模型
|
||||
ld.add_action(robot_state_publisher) # 4. 发布 TF 树
|
||||
ld.add_action(map_server_node) # 5. 加载全量静态地图 → /map
|
||||
ld.add_action(map_server_lifecycle) # 6. 激活 map_server
|
||||
ld.add_action(slam_toolbox_node) # 7. 启动 SLAM 定位 → map→odom TF
|
||||
ld.add_action(navigation_launch) # 8. 启动 Nav2 导航栈
|
||||
|
||||
return ld
|
||||
|
||||
|
||||
@@ -0,0 +1,181 @@
|
||||
# ============================================================================
|
||||
# gc_nav2_with_slam_online.launch.py
|
||||
# 功能：已知全局地图 + SLAM 在线更新 + 导航
|
||||
#
|
||||
# 架构：
|
||||
# ┌─ /map（全量静态地图，199×266）
|
||||
# map_server ──────────┤
|
||||
# └─ 来自 my_map.yaml + my_map.pgm
|
||||
#
|
||||
# ┌─ map→odom TF（激光扫描匹配定位）
|
||||
# slam_toolbox ────────┤
|
||||
# (async mapping) └─ /slam_map（实时建图结果，重映射避免冲突）
|
||||
#
|
||||
# navigation ┌─ global_costmap/static_layer ← /map（全量地图）
|
||||
# ├─ local_costmap/obstacle_layer ← /scan（实时避障）
|
||||
# └─ planner + controller + behavior
|
||||
#
|
||||
# 关键设计：
|
||||
# 1. map_server 提供全量静态底图 → Foxglove 中看到完整房间布局
|
||||
# 2. slam_toolbox 定位 + 实时建图，/slam_map 可单独查看更新效果
|
||||
# 3. 实时避障通过 costmap 的 obstacle_layer 实现（不依赖地图更新）
|
||||
# 4. 定期调用 /slam_toolbox/serialize_map 保存更新后的地图
|
||||
# ============================================================================
|
||||
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, SetEnvironmentVariable
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.parameter_descriptions import ParameterValue
|
||||
from launch.substitutions import Command
|
||||
from launch.actions import ExecuteProcess
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
"""启动 Gazebo + 已知全局地图 + SLAM 在线定位建图 + Nav2 导航"""
|
||||
ld = LaunchDescription()
|
||||
|
||||
# =============================1.包路径================================================================
|
||||
pkg_dir = get_package_share_directory('gc_navigation2_slamtoolbox')
|
||||
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
|
||||
origincar_urdf_dir = get_package_share_directory('origincar_description')
|
||||
|
||||
# =============================2.参数==================================================================
|
||||
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
|
||||
map_yaml_path = os.path.join(pkg_dir, 'maps', 'zhihui.yaml')
|
||||
nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
|
||||
pkg_dir, 'params', 'gc_navigation_slam.yaml'))
|
||||
slam_params_file = os.path.join(pkg_dir, 'config', 'slam_toolbox_async.yaml')
|
||||
posegraph_path = os.path.join(pkg_dir, 'maps', 'zhihui.posegraph')
|
||||
|
||||
# =============================3.map_server：全量静态地图（Foxglove 看到的全局地图）========================
|
||||
map_server_node = Node(
|
||||
package='nav2_map_server',
|
||||
executable='map_server',
|
||||
name='map_server',
|
||||
output='screen',
|
||||
parameters=[{'yaml_filename': map_yaml_path, 'use_sim_time': True}],
|
||||
)
|
||||
map_server_lifecycle = Node(
|
||||
package='nav2_lifecycle_manager',
|
||||
executable='lifecycle_manager',
|
||||
name='lifecycle_manager_map',
|
||||
output='screen',
|
||||
parameters=[{'use_sim_time': True, 'autostart': True,
|
||||
'node_names': ['map_server']}],
|
||||
)
|
||||
|
||||
# =============================4.slam_toolbox：定位 + 实时建图=============================================
|
||||
# ⚠️ /map 重映射到 /slam_map，避免覆盖 map_server 的全量地图
|
||||
slam_toolbox_node = Node(
|
||||
package='slam_toolbox',
|
||||
executable='async_slam_toolbox_node',
|
||||
name='slam_toolbox',
|
||||
output='screen',
|
||||
parameters=[slam_params_file, {'use_sim_time': True}],
|
||||
remappings=[('/map', '/slam_map')],
|
||||
)
|
||||
|
||||
# =============================5.加载已有序列化地图=======================================================
|
||||
# 通过 ros2 service call 在 slam_toolbox 启动后加载
|
||||
import launch
|
||||
load_map_cmd = ExecuteProcess(
|
||||
cmd=[
|
||||
'bash', '-c',
|
||||
# 等待 slam_toolbox 服务就绪后加载地图
|
||||
'sleep 8 && '
|
||||
'ros2 service call /slam_toolbox/deserialize_map '
|
||||
'slam_toolbox/srv/DeserializePoseGraph '
|
||||
"'{filename: \"%s\", match_type: 1}'" % posegraph_path,
|
||||
],
|
||||
output='screen',
|
||||
)
|
||||
|
||||
# =============================6.Nav2 导航栈============================================================
|
||||
navigation_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
[nav2_bringup_dir, '/launch', '/navigation_launch.py']),
|
||||
launch_arguments={
|
||||
'use_sim_time': use_sim_time,
|
||||
'params_file': nav2_param_path,
|
||||
}.items(),
|
||||
)
|
||||
|
||||
# =============================7.Gazebo 仿真============================================================
|
||||
robot_name_in_model = 'mycar'
|
||||
default_model_path = os.path.join(origincar_urdf_dir, 'urdf', 'origincar.urdf')
|
||||
model = DeclareLaunchArgument(name='model', default_value=default_model_path)
|
||||
gazebo_world_path = os.path.join(origincar_urdf_dir, 'world', 'zhihui.world')
|
||||
start_gazebo_cmd = ExecuteProcess(
|
||||
cmd=['gazebo', '--verbose',
|
||||
'-s', 'libgazebo_ros_init.so',
|
||||
'-s', 'libgazebo_ros_factory.so',
|
||||
gazebo_world_path],
|
||||
output='screen',
|
||||
)
|
||||
|
||||
# =============================7a.机器人初始位姿参数=====================================================
|
||||
# 通过命令行参数覆盖，例如：
|
||||
# ros2 launch ... spawn_x:=1.0 spawn_y:=-2.0 spawn_yaw:=1.57
|
||||
spawn_x = LaunchConfiguration('spawn_x', default='-4.311092')
|
||||
spawn_y = LaunchConfiguration('spawn_y', default='-4.299756')
|
||||
spawn_z = LaunchConfiguration('spawn_z', default='0.0')
|
||||
spawn_yaw = LaunchConfiguration('spawn_yaw', default='0.0')
|
||||
|
||||
set_spawn_x = SetEnvironmentVariable('SPAWN_X', spawn_x)
|
||||
set_spawn_y = SetEnvironmentVariable('SPAWN_Y', spawn_y)
|
||||
set_spawn_z = SetEnvironmentVariable('SPAWN_Z', spawn_z)
|
||||
set_spawn_yaw = SetEnvironmentVariable('SPAWN_YAW', spawn_yaw)
|
||||
|
||||
# =============================7b.在 Gazebo 中生成机器人=================================================
|
||||
# 通过 bash 读取环境变量，确保位姿参数正确传递
|
||||
spawn_entity_cmd = ExecuteProcess(
|
||||
cmd=['bash', '-c',
|
||||
'ros2 run gazebo_ros spawn_entity.py '
|
||||
'-entity mycar '
|
||||
f'-file {default_model_path} '
|
||||
'-x $SPAWN_X -y $SPAWN_Y -z $SPAWN_Z -Y $SPAWN_YAW'],
|
||||
output='screen',
|
||||
)
|
||||
|
||||
robot_description = ParameterValue(
|
||||
Command(['xacro ', LaunchConfiguration('model')]), value_type=str,
|
||||
)
|
||||
robot_state_publisher = Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
parameters=[{'robot_description': robot_description,
|
||||
'use_sim_time': True, 'publish_frequency': 30.0}],
|
||||
)
|
||||
|
||||
# =============================8.组装==================================================================
|
||||
ld.add_action(model)
|
||||
ld.add_action(DeclareLaunchArgument(
|
||||
'spawn_x', default_value='-4.311092',
|
||||
description='机器人初始 x 坐标（m）'))
|
||||
ld.add_action(DeclareLaunchArgument(
|
||||
'spawn_y', default_value='-4.299756',
|
||||
description='机器人初始 y 坐标（m）'))
|
||||
ld.add_action(DeclareLaunchArgument(
|
||||
'spawn_z', default_value='0.0',
|
||||
description='机器人初始 z 坐标（m）'))
|
||||
ld.add_action(DeclareLaunchArgument(
|
||||
'spawn_yaw', default_value='0.0',
|
||||
description='机器人初始偏航角（rad）'))
|
||||
ld.add_action(start_gazebo_cmd)
|
||||
ld.add_action(set_spawn_x)
|
||||
ld.add_action(set_spawn_y)
|
||||
ld.add_action(set_spawn_z)
|
||||
ld.add_action(set_spawn_yaw)
|
||||
ld.add_action(spawn_entity_cmd)
|
||||
ld.add_action(robot_state_publisher)
|
||||
ld.add_action(map_server_node)
|
||||
ld.add_action(map_server_lifecycle)
|
||||
ld.add_action(slam_toolbox_node)
|
||||
ld.add_action(load_map_cmd)
|
||||
ld.add_action(navigation_launch)
|
||||
|
||||
return ld
|
||||
@@ -0,0 +1,56 @@
|
||||
# ============================================================================
|
||||
# gc_nav2_with_slam_online_real.launch.py
|
||||
# 功能：真机实时建图 — slam_toolbox 从零建图 + Nav2 导航
|
||||
# 依赖：需先启动 origincar_bringup（底盘驱动 + EKF）
|
||||
# ============================================================================
|
||||
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
"""真机实时建图：slam_toolbox 从零建图 + Nav2 导航"""
|
||||
ld = LaunchDescription()
|
||||
|
||||
# === 1. 包路径 ===
|
||||
pkg_dir = get_package_share_directory('gc_navigation2_slamtoolbox')
|
||||
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
|
||||
|
||||
# === 2. 参数 ===
|
||||
use_sim_time = LaunchConfiguration('use_sim_time', default='false')
|
||||
nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
|
||||
pkg_dir, 'params', 'gc_navigation_slam.yaml'))
|
||||
slam_params_file = os.path.join(pkg_dir, 'config', 'slam_toolbox_mapping.yaml')
|
||||
|
||||
# === 3. slam_toolbox：实时建图 → 发布 map→odom ===
|
||||
slam_toolbox_node = Node(
|
||||
package='slam_toolbox',
|
||||
executable='async_slam_toolbox_node',
|
||||
name='slam_toolbox',
|
||||
output='screen',
|
||||
parameters=[slam_params_file,
|
||||
{'use_sim_time': use_sim_time}],
|
||||
)
|
||||
|
||||
# === 4. Nav2 导航栈 ===
|
||||
navigation_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
[nav2_bringup_dir, '/launch', '/navigation_launch.py']),
|
||||
launch_arguments={
|
||||
'use_sim_time': use_sim_time,
|
||||
'params_file': nav2_param_path,
|
||||
}.items(),
|
||||
)
|
||||
|
||||
# === 5. TF 树由底盘启动文件（origincar_base）提供，此处不再重复发布 ===
|
||||
|
||||
# === 6. 组装 ===
|
||||
ld.add_action(slam_toolbox_node)
|
||||
ld.add_action(navigation_launch)
|
||||
|
||||
return ld
|
||||
@@ -0,0 +1,25 @@
|
||||
#!/usr/bin/env python3
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
import os
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
# 获取参数文件路径
|
||||
pkg_share = get_package_share_directory('gc_navigation2_slamtoolbox')
|
||||
params_file = os.path.join(
|
||||
pkg_share, 'config', 'mapper_params_lifelong.yaml')
|
||||
|
||||
# 创建SLAM Toolbox节点
|
||||
slam_toolbox_node = Node(
|
||||
package='slam_toolbox',
|
||||
executable='async_slam_toolbox_node', # 或 sync_slam_toolbox_node
|
||||
name='slam_toolbox',
|
||||
output='screen',
|
||||
parameters=[params_file]
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
slam_toolbox_node
|
||||
])
|
||||
@@ -0,0 +1,25 @@
|
||||
#!/usr/bin/env python3
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
import os
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
# 获取参数文件路径
|
||||
pkg_share = get_package_share_directory('gc_navigation2_slamtoolbox')
|
||||
params_file = os.path.join(
|
||||
pkg_share, 'config', 'mapper_params_localization.yaml')
|
||||
|
||||
# 创建SLAM Toolbox节点
|
||||
slam_toolbox_node = Node(
|
||||
package='slam_toolbox',
|
||||
executable='localization_slam_toolbox_node', # 或 sync_slam_toolbox_node
|
||||
name='slam_toolbox',
|
||||
output='screen',
|
||||
parameters=[params_file]
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
slam_toolbox_node
|
||||
])
|
||||
@@ -0,0 +1,25 @@
|
||||
#!/usr/bin/env python3
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
import os
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
# 获取参数文件路径
|
||||
pkg_share = get_package_share_directory('gc_navigation2_slamtoolbox')
|
||||
params_file = os.path.join(
|
||||
pkg_share, 'config', 'mapper_params_online_async.yaml')
|
||||
|
||||
# 创建SLAM Toolbox节点
|
||||
slam_toolbox_node = Node(
|
||||
package='slam_toolbox',
|
||||
executable='async_slam_toolbox_node', # 或 sync_slam_toolbox_node
|
||||
name='slam_toolbox',
|
||||
output='screen',
|
||||
parameters=[params_file]
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
slam_toolbox_node
|
||||
])
|
||||
@@ -0,0 +1,196 @@
|
||||
#!/usr/bin/env python3
|
||||
# ============================================================================
|
||||
# gc_slam_mapping.launch.py
|
||||
# 功能：Gazebo 仿真环境 + SLAM 建图启动文件
|
||||
#
|
||||
# 适用场景：
|
||||
# - 在 Gazebo 仿真中遥控机器人建立环境地图
|
||||
# - 在已有地图基础上增量更新
|
||||
# - 单独调试 slam_toolbox 建图参数
|
||||
#
|
||||
# 架构：
|
||||
# Gazebo 仿真器 — 加载 zhihui.world（智慧楼墙体环境）
|
||||
# spawn_entity — 在 Gazebo 中生成机器人模型
|
||||
# robot_state_publisher — 发布机器人 TF 树（URDF 模型）
|
||||
# joint_state_publisher — 发布关节状态
|
||||
# async_slam_toolbox_node — 激光 SLAM 建图（mapping 模式）
|
||||
# 发布 map→odom TF
|
||||
# 发布 /map 话题（实时地图）
|
||||
#
|
||||
# 使用方式：
|
||||
# ros2 launch gc_navigation2_slamtoolbox gc_slam_mapping.launch.py
|
||||
#
|
||||
# 保存地图：
|
||||
# ros2 service call /slam_toolbox/serialize_map slam_toolbox/srv/SerializePoseGraph \
|
||||
# "{filename: '/home/guoch/test_ws/src/gc_navigation2_slamtoolbox/maps/my_map'}"
|
||||
# ros2 run nav2_map_server map_saver_cli -f <map_name>
|
||||
# ============================================================================
|
||||
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, ExecuteProcess, SetEnvironmentVariable
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.parameter_descriptions import ParameterValue
|
||||
from launch.substitutions import Command
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
"""生成 LaunchDescription，启动 Gazebo + slam_toolbox 建图"""
|
||||
|
||||
# ============================ 1. 包路径 =========================================
|
||||
pkg_dir = get_package_share_directory('gc_navigation2_slamtoolbox')
|
||||
origincar_urdf_dir = get_package_share_directory('origincar_description')
|
||||
|
||||
# ============================ 2. 声明启动参数 ====================================
|
||||
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
|
||||
|
||||
# slam_params_file: slam_toolbox 配置文件
|
||||
slam_params_file = LaunchConfiguration(
|
||||
'slam_params_file',
|
||||
default=os.path.join(pkg_dir, 'config', 'slam_toolbox_async.yaml'))
|
||||
|
||||
# map_file_name: 可选，加载已有序列化地图继续建图
|
||||
map_file_name = LaunchConfiguration('map_file_name', default='')
|
||||
|
||||
# model: 机器人 URDF 模型路径（用于 xacro 处理）
|
||||
# origincar.urdf 是纯 URDF（不含 xacro 宏），xacro 会原样透传
|
||||
default_model_path = os.path.join(
|
||||
origincar_urdf_dir, 'urdf', 'origincar.urdf')
|
||||
model = DeclareLaunchArgument(
|
||||
name='model', default_value=default_model_path)
|
||||
|
||||
# spawn_model: Gazebo 中生成机器人用的 URDF（不含 xacro 宏）
|
||||
spawn_model_path = os.path.join(
|
||||
origincar_urdf_dir, 'urdf', 'origincar.urdf')
|
||||
|
||||
# ============================ 3. 设置 GAZEBO_MODEL_PATH ==========================
|
||||
# 让 Gazebo 能找到 zhihui 模型，同时保留标准模型路径
|
||||
gazebo_model_path = os.path.join(pkg_dir, 'world')
|
||||
existing_gazebo_path = os.environ.get('GAZEBO_MODEL_PATH', '')
|
||||
if existing_gazebo_path:
|
||||
full_gazebo_path = f'{gazebo_model_path}:{existing_gazebo_path}'
|
||||
else:
|
||||
full_gazebo_path = gazebo_model_path
|
||||
set_gazebo_model_path = SetEnvironmentVariable(
|
||||
name='GAZEBO_MODEL_PATH',
|
||||
value=full_gazebo_path
|
||||
)
|
||||
|
||||
# ============================ 4. 启动 Gazebo 仿真器 ==============================
|
||||
gazebo_world_path = os.path.join(pkg_dir, 'world', 'zhihui.world')
|
||||
start_gazebo_cmd = ExecuteProcess(
|
||||
cmd=['gazebo', '--verbose',
|
||||
'-s', 'libgazebo_ros_init.so',
|
||||
'-s', 'libgazebo_ros_factory.so',
|
||||
gazebo_world_path],
|
||||
output='screen'
|
||||
)
|
||||
|
||||
# ============================ 5. 机器人初始位姿参数 ==============================
|
||||
# 通过命令行参数覆盖，例如：
|
||||
# ros2 launch ... spawn_x:=1.0 spawn_y:=-2.0 spawn_yaw:=1.57
|
||||
spawn_x = LaunchConfiguration('spawn_x', default='-4.311092')
|
||||
spawn_y = LaunchConfiguration('spawn_y', default='-4.299756')
|
||||
spawn_z = LaunchConfiguration('spawn_z', default='0.0')
|
||||
spawn_yaw = LaunchConfiguration('spawn_yaw', default='0.0')
|
||||
|
||||
# 将位姿参数设为环境变量，供 spawn 命令使用
|
||||
set_spawn_x = SetEnvironmentVariable('SPAWN_X', spawn_x)
|
||||
set_spawn_y = SetEnvironmentVariable('SPAWN_Y', spawn_y)
|
||||
set_spawn_z = SetEnvironmentVariable('SPAWN_Z', spawn_z)
|
||||
set_spawn_yaw = SetEnvironmentVariable('SPAWN_YAW', spawn_yaw)
|
||||
|
||||
# ============================ 6. 在 Gazebo 中生成机器人 ==========================
|
||||
# 通过 bash 读取环境变量，确保位姿参数正确传递
|
||||
robot_name_in_model = 'mycar'
|
||||
spawn_entity_cmd = ExecuteProcess(
|
||||
cmd=['bash', '-c',
|
||||
'ros2 run gazebo_ros spawn_entity.py '
|
||||
'-entity mycar '
|
||||
f'-file {spawn_model_path} '
|
||||
'-x $SPAWN_X -y $SPAWN_Y -z $SPAWN_Z -Y $SPAWN_YAW'],
|
||||
output='screen'
|
||||
)
|
||||
|
||||
# ============================ 7. robot_state_publisher ===========================
|
||||
robot_description = ParameterValue(
|
||||
Command(['xacro ', LaunchConfiguration('model')]),
|
||||
value_type=str
|
||||
)
|
||||
robot_state_publisher_node = Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
name='robot_state_publisher',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'robot_description': robot_description,
|
||||
'use_sim_time': True,
|
||||
'publish_frequency': 30.0,
|
||||
}],
|
||||
)
|
||||
|
||||
# ============================ 8. joint_state_publisher ===========================
|
||||
joint_state_publisher_node = Node(
|
||||
package='joint_state_publisher',
|
||||
executable='joint_state_publisher',
|
||||
name='joint_state_publisher',
|
||||
output='screen',
|
||||
parameters=[{'use_sim_time': use_sim_time}],
|
||||
)
|
||||
|
||||
# ============================ 9. slam_toolbox 建图节点 ============================
|
||||
slam_toolbox_node = Node(
|
||||
package='slam_toolbox',
|
||||
executable='async_slam_toolbox_node',
|
||||
name='slam_toolbox',
|
||||
output='screen',
|
||||
parameters=[
|
||||
slam_params_file,
|
||||
{
|
||||
'use_sim_time': use_sim_time,
|
||||
'map_file_name': map_file_name,
|
||||
},
|
||||
],
|
||||
)
|
||||
|
||||
# ============================ 10. 组装 LaunchDescription =========================
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time',
|
||||
default_value='true',
|
||||
description='使用仿真时间（Gazebo=true, 实车=false)'),
|
||||
DeclareLaunchArgument(
|
||||
'slam_params_file',
|
||||
default_value=os.path.join(pkg_dir, 'config', 'slam_toolbox_async.yaml'),
|
||||
description='slam_toolbox 参数配置文件路径'),
|
||||
DeclareLaunchArgument(
|
||||
'map_file_name',
|
||||
default_value='',
|
||||
description='已有序列化地图路径（.posegraph），留空则从零开始建图'),
|
||||
DeclareLaunchArgument(
|
||||
'spawn_x', default_value='-4.311092',
|
||||
description='机器人初始 x 坐标（m）'),
|
||||
DeclareLaunchArgument(
|
||||
'spawn_y', default_value='-4.299756',
|
||||
description='机器人初始 y 坐标（m）'),
|
||||
DeclareLaunchArgument(
|
||||
'spawn_z', default_value='0.0',
|
||||
description='机器人初始 z 坐标（m）'),
|
||||
DeclareLaunchArgument(
|
||||
'spawn_yaw', default_value='0.0',
|
||||
description='机器人初始偏航角（rad）'),
|
||||
model,
|
||||
|
||||
set_gazebo_model_path,
|
||||
set_spawn_x,
|
||||
set_spawn_y,
|
||||
set_spawn_z,
|
||||
set_spawn_yaw,
|
||||
start_gazebo_cmd,
|
||||
spawn_entity_cmd,
|
||||
robot_state_publisher_node,
|
||||
joint_state_publisher_node,
|
||||
slam_toolbox_node,
|
||||
])
|
||||
BIN
src/navigation/gc_navigation2_slamtoolbox/maps/my_map.data
Normal file
BIN
src/navigation/gc_navigation2_slamtoolbox/maps/my_map.data
Normal file
Binary file not shown.
BIN
src/navigation/gc_navigation2_slamtoolbox/maps/my_map.pgm
Normal file
BIN
src/navigation/gc_navigation2_slamtoolbox/maps/my_map.pgm
Normal file
Binary file not shown.
BIN
src/navigation/gc_navigation2_slamtoolbox/maps/my_map.posegraph
Normal file
BIN
src/navigation/gc_navigation2_slamtoolbox/maps/my_map.posegraph
Normal file
Binary file not shown.
@@ -0,0 +1,7 @@
|
||||
image: my_map.pgm
|
||||
mode: trinary
|
||||
resolution: 0.05
|
||||
origin: [-2.15, -5.81, 0]
|
||||
negate: 0
|
||||
occupied_thresh: 0.65
|
||||
free_thresh: 0.25
|
||||
BIN
src/navigation/gc_navigation2_slamtoolbox/maps/zhihui.data
Normal file
BIN
src/navigation/gc_navigation2_slamtoolbox/maps/zhihui.data
Normal file
Binary file not shown.
BIN
src/navigation/gc_navigation2_slamtoolbox/maps/zhihui.pgm
Normal file
BIN
src/navigation/gc_navigation2_slamtoolbox/maps/zhihui.pgm
Normal file
Binary file not shown.
BIN
src/navigation/gc_navigation2_slamtoolbox/maps/zhihui.posegraph
Normal file
BIN
src/navigation/gc_navigation2_slamtoolbox/maps/zhihui.posegraph
Normal file
Binary file not shown.
@@ -0,0 +1,7 @@
|
||||
image: zhihui.pgm
|
||||
mode: trinary
|
||||
resolution: 0.05
|
||||
origin: [-4.98, -4.71, 0]
|
||||
negate: 0
|
||||
occupied_thresh: 0.65
|
||||
free_thresh: 0.25
|
||||
18
src/navigation/gc_navigation2_slamtoolbox/package.xml
Normal file
18
src/navigation/gc_navigation2_slamtoolbox/package.xml
Normal file
@@ -0,0 +1,18 @@
|
||||
<?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>gc_navigation2_slamtoolbox</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="guoch@todo.todo">guoch</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,413 @@
|
||||
amcl:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
alpha1: 0.2
|
||||
alpha2: 0.2
|
||||
alpha3: 0.2
|
||||
alpha4: 0.2
|
||||
alpha5: 0.2
|
||||
base_frame_id: "base_link"
|
||||
beam_skip_distance: 0.5
|
||||
beam_skip_error_threshold: 0.9
|
||||
beam_skip_threshold: 0.3
|
||||
do_beamskip: false
|
||||
global_frame_id: "map"
|
||||
lambda_short: 0.1
|
||||
laser_likelihood_max_dist: 2.0
|
||||
laser_max_range: 100.0
|
||||
laser_min_range: -1.0
|
||||
laser_model_type: "likelihood_field"
|
||||
max_beams: 60
|
||||
max_particles: 2000
|
||||
min_particles: 500
|
||||
odom_frame_id: "odom"
|
||||
pf_err: 0.05
|
||||
pf_z: 0.99
|
||||
recovery_alpha_fast: 0.0
|
||||
recovery_alpha_slow: 0.0
|
||||
resample_interval: 1
|
||||
robot_model_type: "nav2_amcl::DifferentialMotionModel"
|
||||
save_pose_rate: 0.5
|
||||
sigma_hit: 0.2
|
||||
tf_broadcast: true
|
||||
transform_tolerance: 10.0
|
||||
update_min_a: 0.2
|
||||
update_min_d: 0.25
|
||||
z_hit: 0.5
|
||||
z_max: 0.05
|
||||
z_rand: 0.5
|
||||
z_short: 0.05
|
||||
scan_topic: scan
|
||||
|
||||
amcl_map_client:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
amcl_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
odom_topic: /odom
|
||||
bt_loop_duration: 20
|
||||
default_server_timeout: 20
|
||||
# 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
|
||||
# nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
|
||||
# nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
|
||||
# They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
|
||||
plugin_lib_names:
|
||||
- nav2_compute_path_to_pose_action_bt_node
|
||||
- nav2_compute_path_through_poses_action_bt_node
|
||||
- nav2_smooth_path_action_bt_node
|
||||
- nav2_follow_path_action_bt_node
|
||||
- nav2_spin_action_bt_node
|
||||
- nav2_wait_action_bt_node
|
||||
- nav2_back_up_action_bt_node
|
||||
- nav2_drive_on_heading_bt_node
|
||||
- nav2_clear_costmap_service_bt_node
|
||||
- nav2_is_stuck_condition_bt_node
|
||||
- nav2_goal_reached_condition_bt_node
|
||||
- nav2_goal_updated_condition_bt_node
|
||||
- nav2_globally_updated_goal_condition_bt_node
|
||||
- nav2_is_path_valid_condition_bt_node
|
||||
- nav2_initial_pose_received_condition_bt_node
|
||||
- nav2_reinitialize_global_localization_service_bt_node
|
||||
- nav2_rate_controller_bt_node
|
||||
- nav2_distance_controller_bt_node
|
||||
- nav2_speed_controller_bt_node
|
||||
- nav2_truncate_path_action_bt_node
|
||||
- nav2_truncate_path_local_action_bt_node
|
||||
- nav2_goal_updater_node_bt_node
|
||||
- nav2_recovery_node_bt_node
|
||||
- nav2_pipeline_sequence_bt_node
|
||||
- nav2_round_robin_node_bt_node
|
||||
- nav2_transform_available_condition_bt_node
|
||||
- nav2_time_expired_condition_bt_node
|
||||
- nav2_path_expiring_timer_condition
|
||||
- nav2_distance_traveled_condition_bt_node
|
||||
- nav2_single_trigger_bt_node
|
||||
- nav2_is_battery_low_condition_bt_node
|
||||
- nav2_navigate_through_poses_action_bt_node
|
||||
- nav2_navigate_to_pose_action_bt_node
|
||||
- nav2_remove_passed_goals_action_bt_node
|
||||
- nav2_planner_selector_bt_node
|
||||
- nav2_controller_selector_bt_node
|
||||
- nav2_goal_checker_selector_bt_node
|
||||
- nav2_controller_cancel_bt_node
|
||||
- nav2_path_longer_on_approach_bt_node
|
||||
- nav2_wait_cancel_bt_node
|
||||
- nav2_spin_cancel_bt_node
|
||||
- nav2_back_up_cancel_bt_node
|
||||
- nav2_drive_on_heading_cancel_bt_node
|
||||
|
||||
bt_navigator_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
controller_frequency: 30.0
|
||||
FollowPath:
|
||||
plugin: "nav2_mppi_controller::MPPIController"
|
||||
time_steps: 56
|
||||
model_dt: 0.05
|
||||
batch_size: 2000
|
||||
vx_std: 0.2
|
||||
vy_std: 0.0
|
||||
wz_std: 0.4
|
||||
vx_max: 0.5
|
||||
vx_min: -0.35
|
||||
vy_max: 0.0
|
||||
wz_max: 1.9
|
||||
iteration_count: 1
|
||||
temperature: 0.3
|
||||
gamma: 0.015
|
||||
motion_model: "Ackermann"
|
||||
visualize: false
|
||||
TrajectoryVisualizer:
|
||||
trajectory_step: 5
|
||||
time_step: 3
|
||||
AckermannConstraints:
|
||||
min_turning_r: 0.4
|
||||
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
|
||||
# Option to replace Cost and use Obstacles instead
|
||||
# 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.1
|
||||
# near_goal_distance: 0.5
|
||||
CostCritic:
|
||||
enabled: true
|
||||
cost_power: 1
|
||||
cost_weight: 3.81
|
||||
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
|
||||
forward_preference: true
|
||||
# VelocityDeadbandCritic:
|
||||
# enabled: true
|
||||
# cost_power: 1
|
||||
# cost_weight: 35.0
|
||||
# deadband_velocities: [0.05, 0.05, 0.05]
|
||||
# TwirlingCritic:
|
||||
# enabled: true
|
||||
# twirling_cost_power: 1
|
||||
# twirling_cost_weight: 10.0
|
||||
|
||||
controller_server_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 5.0
|
||||
publish_frequency: 2.0
|
||||
transform_tolerance: 0.5
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: True
|
||||
rolling_window: true
|
||||
width: 3
|
||||
height: 3
|
||||
resolution: 0.05
|
||||
footprint: "[[0.14, 0.085],
|
||||
[0.14, -0.085],
|
||||
[-0.14, -0.085],
|
||||
[-0.14, 0.085]]"
|
||||
footprint_padding: 0.02
|
||||
plugins: ["voxel_layer", "inflation_layer"]
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.55
|
||||
voxel_layer:
|
||||
plugin: "nav2_costmap_2d::VoxelLayer"
|
||||
enabled: True
|
||||
publish_voxel_map: True
|
||||
origin_z: 0.0
|
||||
z_resolution: 0.05
|
||||
z_voxels: 16
|
||||
max_obstacle_height: 2.0
|
||||
mark_threshold: 0
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
static_layer:
|
||||
map_subscribe_transient_local: True
|
||||
always_send_full_costmap: True
|
||||
local_costmap_client:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
local_costmap_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
transform_tolerance: 0.5
|
||||
update_frequency: 1.0
|
||||
publish_frequency: 1.0
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: True
|
||||
footprint: "[[0.14, 0.085],
|
||||
[0.14, -0.085],
|
||||
[-0.14, -0.085],
|
||||
[-0.14, 0.085]]"
|
||||
footprint_padding: 0.02
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
|
||||
obstacle_layer:
|
||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
||||
enabled: True
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.55
|
||||
always_send_full_costmap: True
|
||||
global_costmap_client:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
global_costmap_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
map_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
yaml_filename: "turtlebot3_world.yaml"
|
||||
|
||||
map_saver:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
save_map_timeout: 5.0
|
||||
free_thresh_default: 0.25
|
||||
occupied_thresh_default: 0.65
|
||||
map_subscribe_transient_local: True
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
planner_plugins: ["GridBased"]
|
||||
use_sim_time: True
|
||||
|
||||
GridBased:
|
||||
plugin: "nav2_smac_planner/SmacPlannerHybrid"
|
||||
downsample_costmap: false # whether or not to downsample the map
|
||||
downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm)
|
||||
tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found.
|
||||
allow_unknown: true # allow traveling in unknown space
|
||||
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
|
||||
motion_model_for_search: "REEDS_SHEPP" # or REEDS_SHEPP 可以倒车
|
||||
angle_quantization_bins: 72 # Number of angle bins for search
|
||||
analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach.
|
||||
analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting
|
||||
minimum_turning_radius: 0.40 # minimum turning radius in m of path / vehicle
|
||||
reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1
|
||||
change_penalty: 0.0 # 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.0 # 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.
|
||||
retrospective_penalty: 0.015
|
||||
lookup_table_size: 20.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 subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static.
|
||||
viz_expansions: false # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning). WARNING: heavy to compute and to display, for debug only as it degrades the performance.
|
||||
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
|
||||
|
||||
planner_server_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
smoother_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
smoother_plugins: ["simple_smoother"]
|
||||
simple_smoother:
|
||||
plugin: "nav2_smoother::SimpleSmoother"
|
||||
tolerance: 1.0e-10
|
||||
max_its: 1000
|
||||
do_refinement: True
|
||||
|
||||
behavior_server:
|
||||
ros__parameters:
|
||||
costmap_topic: local_costmap/costmap_raw
|
||||
footprint_topic: local_costmap/published_footprint
|
||||
cycle_frequency: 10.0
|
||||
behavior_plugins: ["spin", "backup", "wait"]
|
||||
spin:
|
||||
plugin: "nav2_behaviors/Spin"
|
||||
backup:
|
||||
plugin: "nav2_behaviors/BackUp"
|
||||
# drive_on_heading:
|
||||
# plugin: "nav2_behaviors/DriveOnHeading"
|
||||
wait:
|
||||
plugin: "nav2_behaviors/Wait"
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
transform_tolerance: 0.5
|
||||
use_sim_time: True
|
||||
simulate_ahead_time: 2.0
|
||||
max_rotational_vel: 1.0
|
||||
min_rotational_vel: 0.4
|
||||
rotational_acc_lim: 3.2
|
||||
|
||||
robot_state_publisher:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
waypoint_follower:
|
||||
ros__parameters:
|
||||
loop_rate: 20
|
||||
use_sim_time: True
|
||||
stop_on_failure: false
|
||||
waypoint_task_executor_plugin: "wait_at_waypoint"
|
||||
wait_at_waypoint:
|
||||
plugin: "nav2_waypoint_follower::WaitAtWaypoint"
|
||||
enabled: True
|
||||
waypoint_pause_duration: 200
|
||||
@@ -0,0 +1,276 @@
|
||||
slam_toolbox:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
global_frame: map
|
||||
robot_base_frame: base_footprint
|
||||
odom_topic: /odom
|
||||
bt_loop_duration: 50
|
||||
default_server_timeout: 20
|
||||
default_nav_to_pose_bt_xml: /home/sunrise/yiliao_ws/install/gc_navigation2_slamtoolbox/share/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml
|
||||
default_nav_through_poses_bt_xml: /home/sunrise/yiliao_ws/install/gc_navigation2_slamtoolbox/share/gc_navigation2_slamtoolbox/config/nav_through_poses_ackermann.xml
|
||||
plugin_lib_names:
|
||||
- nav2_compute_path_to_pose_action_bt_node
|
||||
- nav2_compute_path_through_poses_action_bt_node
|
||||
- nav2_smooth_path_action_bt_node
|
||||
- nav2_follow_path_action_bt_node
|
||||
- nav2_spin_action_bt_node
|
||||
- nav2_wait_action_bt_node
|
||||
- nav2_back_up_action_bt_node
|
||||
- nav2_drive_on_heading_bt_node
|
||||
- nav2_clear_costmap_service_bt_node
|
||||
- nav2_is_stuck_condition_bt_node
|
||||
- nav2_goal_reached_condition_bt_node
|
||||
- nav2_goal_updated_condition_bt_node
|
||||
- nav2_globally_updated_goal_condition_bt_node
|
||||
- nav2_is_path_valid_condition_bt_node
|
||||
- nav2_initial_pose_received_condition_bt_node
|
||||
- nav2_reinitialize_global_localization_service_bt_node
|
||||
- nav2_rate_controller_bt_node
|
||||
- nav2_distance_controller_bt_node
|
||||
- nav2_speed_controller_bt_node
|
||||
- nav2_truncate_path_action_bt_node
|
||||
- nav2_truncate_path_local_action_bt_node
|
||||
- nav2_goal_updater_node_bt_node
|
||||
- nav2_recovery_node_bt_node
|
||||
- nav2_pipeline_sequence_bt_node
|
||||
- nav2_round_robin_node_bt_node
|
||||
- nav2_transform_available_condition_bt_node
|
||||
- nav2_time_expired_condition_bt_node
|
||||
- nav2_path_expiring_timer_condition
|
||||
- nav2_distance_traveled_condition_bt_node
|
||||
- nav2_single_trigger_bt_node
|
||||
- nav2_is_battery_low_condition_bt_node
|
||||
- nav2_navigate_through_poses_action_bt_node
|
||||
- nav2_navigate_to_pose_action_bt_node
|
||||
- nav2_remove_passed_goals_action_bt_node
|
||||
- nav2_planner_selector_bt_node
|
||||
- nav2_controller_selector_bt_node
|
||||
- nav2_goal_checker_selector_bt_node
|
||||
- nav2_controller_cancel_bt_node
|
||||
- nav2_path_longer_on_approach_bt_node
|
||||
- nav2_wait_cancel_bt_node
|
||||
- nav2_spin_cancel_bt_node
|
||||
- nav2_back_up_cancel_bt_node
|
||||
- nav2_drive_on_heading_cancel_bt_node
|
||||
bt_navigator_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
controller_frequency: 20.0
|
||||
FollowPath:
|
||||
plugin: nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController
|
||||
desired_linear_vel: 0.3
|
||||
max_angular_vel: 1.9
|
||||
min_angular_vel: 0.2
|
||||
lookahead_dist: 0.5
|
||||
min_lookahead_dist: 0.3
|
||||
max_lookahead_dist: 0.8
|
||||
lookahead_time: 1.5
|
||||
rotate_to_heading_angular_vel: 0.8
|
||||
transform_tolerance: 0.5
|
||||
use_velocity_scaled_lookahead_dist: True
|
||||
min_approach_linear_vel: 0.1
|
||||
approach_velocity_scaling_dist: 0.5
|
||||
use_collision_detection: True
|
||||
max_allowed_time_to_collision: 1.0
|
||||
use_cost_regulated_linear_velocity_scaling: True
|
||||
cost_scaling_dist: 0.6
|
||||
cost_scaling_gain: 1.0
|
||||
regulated_linear_scaling_min_radius: 0.35
|
||||
regulated_linear_scaling_min_speed: 0.1
|
||||
use_rotate_to_heading: False
|
||||
rotate_to_heading_min_angle: 0.5
|
||||
max_robot_pose_search_dist: 10.0
|
||||
use_interpolation: False
|
||||
allow_reversing: True
|
||||
controller_server_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 1.0
|
||||
publish_frequency: 2.0
|
||||
transform_tolerance: 2.0
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: False
|
||||
rolling_window: True
|
||||
width: 3
|
||||
height: 3
|
||||
resolution: 0.05
|
||||
footprint: '[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]'
|
||||
footprint_padding: 0.02
|
||||
plugins:
|
||||
- voxel_layer
|
||||
- inflation_layer
|
||||
inflation_layer:
|
||||
plugin: nav2_costmap_2d::InflationLayer
|
||||
cost_scaling_factor: 5.0
|
||||
inflation_radius: 0.3
|
||||
voxel_layer:
|
||||
plugin: nav2_costmap_2d::VoxelLayer
|
||||
enabled: True
|
||||
publish_voxel_map: True
|
||||
origin_z: 0.0
|
||||
z_resolution: 0.05
|
||||
z_voxels: 16
|
||||
max_obstacle_height: 2.0
|
||||
mark_threshold: 0
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: LaserScan
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
static_layer:
|
||||
map_subscribe_transient_local: True
|
||||
always_send_full_costmap: True
|
||||
local_costmap_client:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
local_costmap_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
transform_tolerance: 2.0
|
||||
update_frequency: 1.0
|
||||
publish_frequency: 1.0
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
footprint: '[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]'
|
||||
footprint_padding: 0.02
|
||||
resolution: 0.05
|
||||
track_unknown_space: True
|
||||
plugins:
|
||||
- static_layer
|
||||
- obstacle_layer
|
||||
- inflation_layer
|
||||
obstacle_layer:
|
||||
plugin: nav2_costmap_2d::ObstacleLayer
|
||||
enabled: True
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: LaserScan
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
static_layer:
|
||||
plugin: nav2_costmap_2d::StaticLayer
|
||||
map_subscribe_transient_local: True
|
||||
inflation_layer:
|
||||
plugin: nav2_costmap_2d::InflationLayer
|
||||
cost_scaling_factor: 5.0
|
||||
inflation_radius: 0.3
|
||||
always_send_full_costmap: True
|
||||
global_costmap_client:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
global_costmap_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
map_saver:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
save_map_timeout: 5.0
|
||||
free_thresh_default: 0.25
|
||||
occupied_thresh_default: 0.65
|
||||
map_subscribe_transient_local: True
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
planner_plugins:
|
||||
- GridBased
|
||||
use_sim_time: False
|
||||
GridBased:
|
||||
plugin: nav2_smac_planner/SmacPlannerHybrid
|
||||
downsample_costmap: False
|
||||
downsampling_factor: 1
|
||||
tolerance: 0.25
|
||||
allow_unknown: True
|
||||
max_iterations: 10000
|
||||
max_on_approach_iterations: 1000
|
||||
max_planning_time: 5.0
|
||||
motion_model_for_search: REEDS_SHEPP
|
||||
angle_quantization_bins: 72
|
||||
analytic_expansion_ratio: 2.0
|
||||
analytic_expansion_max_length: 3.0
|
||||
minimum_turning_radius: 0.5
|
||||
reverse_penalty: 1.3
|
||||
change_penalty: 0.0
|
||||
non_straight_penalty: 0.0
|
||||
cost_penalty: 5.0
|
||||
retrospective_penalty: 0.015
|
||||
lookup_table_size: 5.0
|
||||
cache_obstacle_heuristic: False
|
||||
viz_expansions: False
|
||||
smooth_path: True
|
||||
smoother:
|
||||
max_iterations: 1000
|
||||
w_smooth: 0.4
|
||||
w_data: 0.2
|
||||
tolerance: 1.0e-10
|
||||
do_refinement: True
|
||||
refinement_num: 4
|
||||
planner_server_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
smoother_server:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
smoother_plugins:
|
||||
- simple_smoother
|
||||
simple_smoother:
|
||||
plugin: nav2_smoother::SimpleSmoother
|
||||
tolerance: 1.0e-10
|
||||
max_its: 1000
|
||||
do_refinement: True
|
||||
behavior_server:
|
||||
ros__parameters:
|
||||
costmap_topic: local_costmap/costmap_raw
|
||||
footprint_topic: local_costmap/published_footprint
|
||||
cycle_frequency: 10.0
|
||||
behavior_plugins:
|
||||
- backup
|
||||
backup:
|
||||
plugin: nav2_behaviors/BackUp
|
||||
backup_dist: 0.8
|
||||
backup_speed: 0.18
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
transform_tolerance: 2.0
|
||||
use_sim_time: False
|
||||
simulate_ahead_time: 2.0
|
||||
max_rotational_vel: 1.0
|
||||
min_rotational_vel: 0.4
|
||||
rotational_acc_lim: 3.2
|
||||
robot_state_publisher:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
waypoint_follower:
|
||||
ros__parameters:
|
||||
loop_rate: 20
|
||||
use_sim_time: False
|
||||
stop_on_failure: False
|
||||
waypoint_task_executor_plugin: wait_at_waypoint
|
||||
wait_at_waypoint:
|
||||
plugin: nav2_waypoint_follower::WaitAtWaypoint
|
||||
enabled: True
|
||||
waypoint_pause_duration: 200
|
||||
@@ -0,0 +1,343 @@
|
||||
slam_toolbox:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
global_frame: map
|
||||
robot_base_frame: base_footprint
|
||||
odom_topic: /odom
|
||||
bt_loop_duration: 50
|
||||
default_server_timeout: 20
|
||||
# 阿克曼底盘专用 BT：移除 Spin，恢复行为 = 清代价地图 → 后退 → 等待
|
||||
default_nav_to_pose_bt_xml: /home/sunrise/yiliao_ws/install/gc_navigation2_slamtoolbox/share/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml
|
||||
plugin_lib_names:
|
||||
- nav2_compute_path_to_pose_action_bt_node
|
||||
- nav2_compute_path_through_poses_action_bt_node
|
||||
- nav2_smooth_path_action_bt_node
|
||||
- nav2_follow_path_action_bt_node
|
||||
- nav2_spin_action_bt_node
|
||||
- nav2_wait_action_bt_node
|
||||
- nav2_back_up_action_bt_node
|
||||
- nav2_drive_on_heading_bt_node
|
||||
- nav2_clear_costmap_service_bt_node
|
||||
- nav2_is_stuck_condition_bt_node
|
||||
- nav2_goal_reached_condition_bt_node
|
||||
- nav2_goal_updated_condition_bt_node
|
||||
- nav2_globally_updated_goal_condition_bt_node
|
||||
- nav2_is_path_valid_condition_bt_node
|
||||
- nav2_initial_pose_received_condition_bt_node
|
||||
- nav2_reinitialize_global_localization_service_bt_node
|
||||
- nav2_rate_controller_bt_node
|
||||
- nav2_distance_controller_bt_node
|
||||
- nav2_speed_controller_bt_node
|
||||
- nav2_truncate_path_action_bt_node
|
||||
- nav2_truncate_path_local_action_bt_node
|
||||
- nav2_goal_updater_node_bt_node
|
||||
- nav2_recovery_node_bt_node
|
||||
- nav2_pipeline_sequence_bt_node
|
||||
- nav2_round_robin_node_bt_node
|
||||
- nav2_transform_available_condition_bt_node
|
||||
- nav2_time_expired_condition_bt_node
|
||||
- nav2_path_expiring_timer_condition
|
||||
- nav2_distance_traveled_condition_bt_node
|
||||
- nav2_single_trigger_bt_node
|
||||
- nav2_is_battery_low_condition_bt_node
|
||||
- nav2_navigate_through_poses_action_bt_node
|
||||
- nav2_navigate_to_pose_action_bt_node
|
||||
- nav2_remove_passed_goals_action_bt_node
|
||||
- nav2_planner_selector_bt_node
|
||||
- nav2_controller_selector_bt_node
|
||||
- nav2_goal_checker_selector_bt_node
|
||||
- nav2_controller_cancel_bt_node
|
||||
- nav2_path_longer_on_approach_bt_node
|
||||
- nav2_wait_cancel_bt_node
|
||||
- nav2_spin_cancel_bt_node
|
||||
- nav2_back_up_cancel_bt_node
|
||||
- nav2_drive_on_heading_cancel_bt_node
|
||||
|
||||
bt_navigator_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
controller_frequency: 20.0
|
||||
FollowPath:
|
||||
plugin: "nav2_mppi_controller::MPPIController"
|
||||
time_steps: 20
|
||||
model_dt: 0.05
|
||||
batch_size: 200
|
||||
vx_std: 0.2
|
||||
vy_std: 0.0
|
||||
wz_std: 0.4
|
||||
vx_max: 0.5
|
||||
vx_min: -0.35
|
||||
vy_max: 0.0
|
||||
wz_max: 1.9
|
||||
iteration_count: 1
|
||||
temperature: 0.3
|
||||
gamma: 0.015
|
||||
motion_model: "Ackermann"
|
||||
visualize: false
|
||||
TrajectoryVisualizer:
|
||||
trajectory_step: 5
|
||||
time_step: 3
|
||||
AckermannConstraints:
|
||||
min_turning_r: 0.4
|
||||
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: 2.0
|
||||
threshold_to_consider: 0.5
|
||||
CostCritic:
|
||||
enabled: true
|
||||
cost_power: 1
|
||||
cost_weight: 3.81
|
||||
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: 7.0 # PathAlignCritic
|
||||
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
|
||||
forward_preference: true
|
||||
|
||||
controller_server_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 1.0
|
||||
publish_frequency: 2.0
|
||||
transform_tolerance: 0.5
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: False
|
||||
rolling_window: true
|
||||
width: 3
|
||||
height: 3
|
||||
resolution: 0.05
|
||||
footprint: "[[0.14, 0.085],
|
||||
[0.14, -0.085],
|
||||
[-0.14, -0.085],
|
||||
[-0.14, 0.085]]"
|
||||
footprint_padding: 0.02
|
||||
plugins: ["voxel_layer", "inflation_layer"]
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.2
|
||||
voxel_layer:
|
||||
plugin: "nav2_costmap_2d::VoxelLayer"
|
||||
enabled: True
|
||||
publish_voxel_map: True
|
||||
origin_z: 0.0
|
||||
z_resolution: 0.05
|
||||
z_voxels: 16
|
||||
max_obstacle_height: 2.0
|
||||
mark_threshold: 0
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
static_layer:
|
||||
map_subscribe_transient_local: True
|
||||
always_send_full_costmap: True
|
||||
local_costmap_client:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
local_costmap_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
transform_tolerance: 0.5
|
||||
update_frequency: 1.0
|
||||
publish_frequency: 1.0
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: False
|
||||
footprint: "[[0.14, 0.085],
|
||||
[0.14, -0.085],
|
||||
[-0.14, -0.085],
|
||||
[-0.14, 0.085]]"
|
||||
footprint_padding: 0.02
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
|
||||
obstacle_layer:
|
||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
||||
enabled: True
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.2
|
||||
always_send_full_costmap: True
|
||||
global_costmap_client:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
global_costmap_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
map_saver:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
save_map_timeout: 5.0
|
||||
free_thresh_default: 0.25
|
||||
occupied_thresh_default: 0.65
|
||||
map_subscribe_transient_local: True
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
planner_plugins: ["GridBased"]
|
||||
use_sim_time: False
|
||||
|
||||
GridBased:
|
||||
plugin: "nav2_smac_planner/SmacPlannerHybrid"
|
||||
downsample_costmap: false
|
||||
downsampling_factor: 1
|
||||
tolerance: 0.25
|
||||
allow_unknown: true
|
||||
max_iterations: 100000
|
||||
max_on_approach_iterations: 1000
|
||||
max_planning_time: 5.0
|
||||
motion_model_for_search: "REEDS_SHEPP"
|
||||
angle_quantization_bins: 72
|
||||
analytic_expansion_ratio: 3.5
|
||||
analytic_expansion_max_length: 3.0
|
||||
minimum_turning_radius: 0.40
|
||||
reverse_penalty: 1.3 # 后退惩罚，越小越愿意后退（默认 2.0，设为 1.3 允许灵活倒车）
|
||||
change_penalty: 0.0
|
||||
non_straight_penalty: 0.5
|
||||
cost_penalty: 5.0
|
||||
retrospective_penalty: 0.015
|
||||
lookup_table_size: 5.0
|
||||
cache_obstacle_heuristic: false
|
||||
viz_expansions: false
|
||||
smooth_path: True
|
||||
|
||||
smoother:
|
||||
max_iterations: 1000
|
||||
w_smooth: 0.3
|
||||
w_data: 0.2
|
||||
tolerance: 1.0e-10
|
||||
do_refinement: true
|
||||
refinement_num: 2
|
||||
|
||||
planner_server_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
smoother_server:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
smoother_plugins: ["simple_smoother"]
|
||||
simple_smoother:
|
||||
plugin: "nav2_smoother::SimpleSmoother"
|
||||
tolerance: 1.0e-10
|
||||
max_its: 1000
|
||||
do_refinement: True
|
||||
|
||||
behavior_server:
|
||||
ros__parameters:
|
||||
costmap_topic: local_costmap/costmap_raw
|
||||
footprint_topic: local_costmap/published_footprint
|
||||
cycle_frequency: 10.0
|
||||
behavior_plugins: ["spin", "backup", "wait"]
|
||||
spin:
|
||||
plugin: "nav2_behaviors/Spin" # 需保留以匹配默认 BT XML
|
||||
backup:
|
||||
plugin: "nav2_behaviors/BackUp"
|
||||
backup_dist: 0.8
|
||||
backup_speed: 0.18
|
||||
wait:
|
||||
plugin: "nav2_behaviors/Wait"
|
||||
wait_duration: 0.5
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
transform_tolerance: 0.5
|
||||
use_sim_time: False
|
||||
simulate_ahead_time: 2.0
|
||||
max_rotational_vel: 1.0
|
||||
min_rotational_vel: 0.4
|
||||
rotational_acc_lim: 3.2
|
||||
|
||||
robot_state_publisher:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
waypoint_follower:
|
||||
ros__parameters:
|
||||
loop_rate: 20
|
||||
use_sim_time: False
|
||||
stop_on_failure: false
|
||||
waypoint_task_executor_plugin: "wait_at_waypoint"
|
||||
wait_at_waypoint:
|
||||
plugin: "nav2_waypoint_follower::WaitAtWaypoint"
|
||||
enabled: True
|
||||
waypoint_pause_duration: 200
|
||||
@@ -0,0 +1,343 @@
|
||||
slam_toolbox:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
global_frame: map
|
||||
robot_base_frame: base_footprint
|
||||
odom_topic: /odom
|
||||
bt_loop_duration: 50
|
||||
default_server_timeout: 20
|
||||
# 阿克曼底盘专用 BT：移除 Spin，恢复行为 = 清代价地图 → 后退 → 等待
|
||||
default_nav_to_pose_bt_xml: /home/sunrise/yiliao_ws/install/gc_navigation2_slamtoolbox/share/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml
|
||||
plugin_lib_names:
|
||||
- nav2_compute_path_to_pose_action_bt_node
|
||||
- nav2_compute_path_through_poses_action_bt_node
|
||||
- nav2_smooth_path_action_bt_node
|
||||
- nav2_follow_path_action_bt_node
|
||||
- nav2_spin_action_bt_node
|
||||
- nav2_wait_action_bt_node
|
||||
- nav2_back_up_action_bt_node
|
||||
- nav2_drive_on_heading_bt_node
|
||||
- nav2_clear_costmap_service_bt_node
|
||||
- nav2_is_stuck_condition_bt_node
|
||||
- nav2_goal_reached_condition_bt_node
|
||||
- nav2_goal_updated_condition_bt_node
|
||||
- nav2_globally_updated_goal_condition_bt_node
|
||||
- nav2_is_path_valid_condition_bt_node
|
||||
- nav2_initial_pose_received_condition_bt_node
|
||||
- nav2_reinitialize_global_localization_service_bt_node
|
||||
- nav2_rate_controller_bt_node
|
||||
- nav2_distance_controller_bt_node
|
||||
- nav2_speed_controller_bt_node
|
||||
- nav2_truncate_path_action_bt_node
|
||||
- nav2_truncate_path_local_action_bt_node
|
||||
- nav2_goal_updater_node_bt_node
|
||||
- nav2_recovery_node_bt_node
|
||||
- nav2_pipeline_sequence_bt_node
|
||||
- nav2_round_robin_node_bt_node
|
||||
- nav2_transform_available_condition_bt_node
|
||||
- nav2_time_expired_condition_bt_node
|
||||
- nav2_path_expiring_timer_condition
|
||||
- nav2_distance_traveled_condition_bt_node
|
||||
- nav2_single_trigger_bt_node
|
||||
- nav2_is_battery_low_condition_bt_node
|
||||
- nav2_navigate_through_poses_action_bt_node
|
||||
- nav2_navigate_to_pose_action_bt_node
|
||||
- nav2_remove_passed_goals_action_bt_node
|
||||
- nav2_planner_selector_bt_node
|
||||
- nav2_controller_selector_bt_node
|
||||
- nav2_goal_checker_selector_bt_node
|
||||
- nav2_controller_cancel_bt_node
|
||||
- nav2_path_longer_on_approach_bt_node
|
||||
- nav2_wait_cancel_bt_node
|
||||
- nav2_spin_cancel_bt_node
|
||||
- nav2_back_up_cancel_bt_node
|
||||
- nav2_drive_on_heading_cancel_bt_node
|
||||
|
||||
bt_navigator_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
controller_frequency: 20.0
|
||||
FollowPath:
|
||||
plugin: "nav2_mppi_controller::MPPIController"
|
||||
time_steps: 20
|
||||
model_dt: 0.05
|
||||
batch_size: 200
|
||||
vx_std: 0.2
|
||||
vy_std: 0.0
|
||||
wz_std: 0.8
|
||||
vx_max: 0.5
|
||||
vx_min: -0.35
|
||||
vy_max: 0.0
|
||||
wz_max: 1.9
|
||||
iteration_count: 1
|
||||
temperature: 0.3
|
||||
gamma: 0.015
|
||||
motion_model: "Ackermann"
|
||||
visualize: false
|
||||
TrajectoryVisualizer:
|
||||
trajectory_step: 5
|
||||
time_step: 3
|
||||
AckermannConstraints:
|
||||
min_turning_r: 0.4
|
||||
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: 2.0
|
||||
GoalAngleCritic:
|
||||
enabled: true
|
||||
cost_power: 1
|
||||
cost_weight: 3.0
|
||||
threshold_to_consider: 0.5
|
||||
PreferForwardCritic:
|
||||
enabled: true
|
||||
cost_power: 1
|
||||
cost_weight: 2.0
|
||||
threshold_to_consider: 0.5
|
||||
CostCritic:
|
||||
enabled: true
|
||||
cost_power: 1
|
||||
cost_weight: 8.0
|
||||
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: 5.0 # PathAlignCritic
|
||||
max_path_occupancy_ratio: 0.05
|
||||
trajectory_point_step: 4
|
||||
threshold_to_consider: 0.5
|
||||
offset_from_furthest: 10
|
||||
use_path_orientations: false
|
||||
PathFollowCritic:
|
||||
enabled: true
|
||||
cost_power: 1
|
||||
cost_weight: 3.0
|
||||
offset_from_furthest: 5
|
||||
threshold_to_consider: 1.4
|
||||
PathAngleCritic:
|
||||
enabled: true
|
||||
cost_power: 1
|
||||
cost_weight: 1.0
|
||||
offset_from_furthest: 4
|
||||
threshold_to_consider: 0.5
|
||||
max_angle_to_furthest: 1.0
|
||||
forward_preference: true
|
||||
|
||||
controller_server_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 1.0
|
||||
publish_frequency: 2.0
|
||||
transform_tolerance: 2.0
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: False
|
||||
rolling_window: true
|
||||
width: 3
|
||||
height: 3
|
||||
resolution: 0.05
|
||||
footprint: "[[0.14, 0.085],
|
||||
[0.14, -0.085],
|
||||
[-0.14, -0.085],
|
||||
[-0.14, 0.085]]"
|
||||
footprint_padding: 0.02
|
||||
plugins: ["voxel_layer", "inflation_layer"]
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.3
|
||||
voxel_layer:
|
||||
plugin: "nav2_costmap_2d::VoxelLayer"
|
||||
enabled: True
|
||||
publish_voxel_map: True
|
||||
origin_z: 0.0
|
||||
z_resolution: 0.05
|
||||
z_voxels: 16
|
||||
max_obstacle_height: 2.0
|
||||
mark_threshold: 0
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
static_layer:
|
||||
map_subscribe_transient_local: True
|
||||
always_send_full_costmap: True
|
||||
local_costmap_client:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
local_costmap_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
transform_tolerance: 2.0
|
||||
update_frequency: 1.0
|
||||
publish_frequency: 1.0
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: False
|
||||
footprint: "[[0.14, 0.085],
|
||||
[0.14, -0.085],
|
||||
[-0.14, -0.085],
|
||||
[-0.14, 0.085]]"
|
||||
footprint_padding: 0.02
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
|
||||
obstacle_layer:
|
||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
||||
enabled: True
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.3
|
||||
always_send_full_costmap: True
|
||||
global_costmap_client:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
global_costmap_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
map_saver:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
save_map_timeout: 5.0
|
||||
free_thresh_default: 0.25
|
||||
occupied_thresh_default: 0.65
|
||||
map_subscribe_transient_local: True
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
planner_plugins: ["GridBased"]
|
||||
use_sim_time: False
|
||||
|
||||
GridBased:
|
||||
plugin: "nav2_smac_planner/SmacPlannerHybrid"
|
||||
downsample_costmap: false
|
||||
downsampling_factor: 1
|
||||
tolerance: 0.25
|
||||
allow_unknown: true
|
||||
max_iterations: 100000
|
||||
max_on_approach_iterations: 1000
|
||||
max_planning_time: 5.0
|
||||
motion_model_for_search: "REEDS_SHEPP"
|
||||
angle_quantization_bins: 72
|
||||
analytic_expansion_ratio: 2.0
|
||||
analytic_expansion_max_length: 3.0
|
||||
minimum_turning_radius: 0.40
|
||||
reverse_penalty: 1.3 # 后退惩罚，越小越愿意后退（默认 2.0，设为 1.3 允许灵活倒车）
|
||||
change_penalty: 0.0
|
||||
non_straight_penalty: 0.0
|
||||
cost_penalty: 5.0
|
||||
retrospective_penalty: 0.015
|
||||
lookup_table_size: 5.0
|
||||
cache_obstacle_heuristic: false
|
||||
viz_expansions: false
|
||||
smooth_path: True
|
||||
|
||||
smoother:
|
||||
max_iterations: 1000
|
||||
w_smooth: 0.4
|
||||
w_data: 0.2
|
||||
tolerance: 1.0e-10
|
||||
do_refinement: true
|
||||
refinement_num: 4
|
||||
|
||||
planner_server_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
smoother_server:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
smoother_plugins: ["simple_smoother"]
|
||||
simple_smoother:
|
||||
plugin: "nav2_smoother::SimpleSmoother"
|
||||
tolerance: 1.0e-10
|
||||
max_its: 1000
|
||||
do_refinement: True
|
||||
|
||||
behavior_server:
|
||||
ros__parameters:
|
||||
costmap_topic: local_costmap/costmap_raw
|
||||
footprint_topic: local_costmap/published_footprint
|
||||
cycle_frequency: 10.0
|
||||
behavior_plugins: ["spin", "backup", "wait"]
|
||||
spin:
|
||||
plugin: "nav2_behaviors/Spin" # 需保留以匹配默认 BT XML
|
||||
backup:
|
||||
plugin: "nav2_behaviors/BackUp"
|
||||
backup_dist: 0.8
|
||||
backup_speed: 0.18
|
||||
wait:
|
||||
plugin: "nav2_behaviors/Wait"
|
||||
wait_duration: 0.5
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
transform_tolerance: 2.0
|
||||
use_sim_time: False
|
||||
simulate_ahead_time: 2.0
|
||||
max_rotational_vel: 1.0
|
||||
min_rotational_vel: 0.4
|
||||
rotational_acc_lim: 3.2
|
||||
|
||||
robot_state_publisher:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
waypoint_follower:
|
||||
ros__parameters:
|
||||
loop_rate: 20
|
||||
use_sim_time: False
|
||||
stop_on_failure: false
|
||||
waypoint_task_executor_plugin: "wait_at_waypoint"
|
||||
wait_at_waypoint:
|
||||
plugin: "nav2_waypoint_follower::WaitAtWaypoint"
|
||||
enabled: True
|
||||
waypoint_pause_duration: 200
|
||||
@@ -0,0 +1,11 @@
|
||||
<?xml version="1.0" ?>
|
||||
<model>
|
||||
<name>zhihui</name>
|
||||
<version>1.0</version>
|
||||
<sdf version="1.7">model.sdf</sdf>
|
||||
<author>
|
||||
<name></name>
|
||||
<email></email>
|
||||
</author>
|
||||
<description></description>
|
||||
</model>
|
||||
471
src/navigation/gc_navigation2_slamtoolbox/world/zhihui/model.sdf
Normal file
471
src/navigation/gc_navigation2_slamtoolbox/world/zhihui/model.sdf
Normal file
@@ -0,0 +1,471 @@
|
||||
<?xml version='1.0'?>
|
||||
<sdf version='1.7'>
|
||||
<model name='zhihui'>
|
||||
<pose>-2.11635 -1.73906 0 0 -0 0</pose>
|
||||
<link name='Wall_12'>
|
||||
<collision name='Wall_12_Collision'>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>1.75 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
</collision>
|
||||
<visual name='Wall_12_Visual'>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>1.75 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
<ambient>1 1 1 1</ambient>
|
||||
</material>
|
||||
<meta>
|
||||
<layer>0</layer>
|
||||
</meta>
|
||||
</visual>
|
||||
<pose>-1.61999 -0.744014 0 0 -0 0</pose>
|
||||
</link>
|
||||
<link name='Wall_15'>
|
||||
<collision name='Wall_15_Collision'>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>2.75 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
</collision>
|
||||
<visual name='Wall_15_Visual'>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>2.75 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
<ambient>1 1 1 1</ambient>
|
||||
</material>
|
||||
<meta>
|
||||
<layer>0</layer>
|
||||
</meta>
|
||||
</visual>
|
||||
<pose>1.62754 -0.744014 0 0 -0 0</pose>
|
||||
</link>
|
||||
<link name='Wall_17'>
|
||||
<collision name='Wall_17_Collision'>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>1 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
</collision>
|
||||
<visual name='Wall_17_Visual'>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>1 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
<ambient>1 1 1 1</ambient>
|
||||
</material>
|
||||
<meta>
|
||||
<layer>0</layer>
|
||||
</meta>
|
||||
</visual>
|
||||
<pose>-0.827543 -0.3078 0 0 -0 1.5708</pose>
|
||||
</link>
|
||||
<link name='Wall_19'>
|
||||
<collision name='Wall_19_Collision'>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>1 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
</collision>
|
||||
<visual name='Wall_19_Visual'>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>1 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
<ambient>1 1 1 1</ambient>
|
||||
</material>
|
||||
<meta>
|
||||
<layer>0</layer>
|
||||
</meta>
|
||||
</visual>
|
||||
<pose>0.327543 -0.319014 0 0 -0 1.5708</pose>
|
||||
</link>
|
||||
<link name='Wall_2'>
|
||||
<collision name='Wall_2_Collision'>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>5 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
</collision>
|
||||
<visual name='Wall_2_Visual'>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>5 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
<ambient>1 1 1 1</ambient>
|
||||
</material>
|
||||
<meta>
|
||||
<layer>0</layer>
|
||||
</meta>
|
||||
</visual>
|
||||
<pose>0.062177 -2.89677 0 0 -0 0</pose>
|
||||
</link>
|
||||
<link name='Wall_29'>
|
||||
<collision name='Wall_29_Collision'>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>3.75 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
</collision>
|
||||
<visual name='Wall_29_Visual'>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>3.75 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
<ambient>1 1 1 1</ambient>
|
||||
</material>
|
||||
<meta>
|
||||
<layer>0</layer>
|
||||
</meta>
|
||||
</visual>
|
||||
<pose>-0.305988 0.758719 0 0 -0 0</pose>
|
||||
</link>
|
||||
<link name='Wall_31'>
|
||||
<collision name='Wall_31_Collision'>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>3.75 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
</collision>
|
||||
<visual name='Wall_31_Visual'>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>3.75 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
<ambient>1 1 1 1</ambient>
|
||||
</material>
|
||||
<meta>
|
||||
<layer>0</layer>
|
||||
</meta>
|
||||
</visual>
|
||||
<pose>-0.300519 2.09188 0 0 -0 0</pose>
|
||||
</link>
|
||||
<link name='Wall_33'>
|
||||
<collision name='Wall_33_Collision'>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>1.5 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
</collision>
|
||||
<visual name='Wall_33_Visual'>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>1.5 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
<ambient>1 1 1 1</ambient>
|
||||
</material>
|
||||
<meta>
|
||||
<layer>0</layer>
|
||||
</meta>
|
||||
</visual>
|
||||
<pose>1.65085 1.41824 0 0 -0 -1.5708</pose>
|
||||
</link>
|
||||
<link name='Wall_36'>
|
||||
<collision name='Wall_36_Collision'>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>2.75 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
</collision>
|
||||
<visual name='Wall_36_Visual'>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>2.75 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
<ambient>1 1 1 1</ambient>
|
||||
</material>
|
||||
<meta>
|
||||
<layer>0</layer>
|
||||
</meta>
|
||||
</visual>
|
||||
<pose>2.3125 1.40828 0 0 -0 1.5708</pose>
|
||||
</link>
|
||||
<link name='Wall_38'>
|
||||
<collision name='Wall_38_Collision'>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>5.25 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
</collision>
|
||||
<visual name='Wall_38_Visual'>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>5.25 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
<ambient>1 1 1 1</ambient>
|
||||
</material>
|
||||
<meta>
|
||||
<layer>0</layer>
|
||||
</meta>
|
||||
</visual>
|
||||
<pose>-0.19189 2.74413 0 0 -0 3.14159</pose>
|
||||
</link>
|
||||
<link name='Wall_4'>
|
||||
<collision name='Wall_4_Collision'>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>2 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
</collision>
|
||||
<visual name='Wall_4_Visual'>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>2 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
<ambient>1 1 1 1</ambient>
|
||||
</material>
|
||||
<meta>
|
||||
<layer>0</layer>
|
||||
</meta>
|
||||
</visual>
|
||||
<pose>2.6265 -1.8264 0 0 -0 1.5708</pose>
|
||||
</link>
|
||||
<link name='Wall_40'>
|
||||
<collision name='Wall_40_Collision'>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>1.5 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
</collision>
|
||||
<visual name='Wall_40_Visual'>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>1.5 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
<ambient>1 1 1 1</ambient>
|
||||
</material>
|
||||
<meta>
|
||||
<layer>0</layer>
|
||||
</meta>
|
||||
</visual>
|
||||
<pose>-2.25486 1.43754 0 0 -0 -1.5708</pose>
|
||||
</link>
|
||||
<link name='Wall_44'>
|
||||
<collision name='Wall_44_Collision'>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>3 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
</collision>
|
||||
<visual name='Wall_44_Visual'>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>3 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
<ambient>1 1 1 1</ambient>
|
||||
</material>
|
||||
<meta>
|
||||
<layer>0</layer>
|
||||
</meta>
|
||||
</visual>
|
||||
<pose>-2.89932 1.47176 0 0 -0 -1.5708</pose>
|
||||
</link>
|
||||
<link name='Wall_46'>
|
||||
<collision name='Wall_46_Collision'>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>2.25 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
</collision>
|
||||
<visual name='Wall_46_Visual'>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>2.25 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
<ambient>1 1 1 1</ambient>
|
||||
</material>
|
||||
<meta>
|
||||
<layer>0</layer>
|
||||
</meta>
|
||||
</visual>
|
||||
<pose>-1.87754 0.1172 0 0 -0 3.14159</pose>
|
||||
</link>
|
||||
<link name='Wall_48'>
|
||||
<collision name='Wall_48_Collision'>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>2.13496 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
</collision>
|
||||
<visual name='Wall_48_Visual'>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>2.13496 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
<ambient>1 1 1 1</ambient>
|
||||
</material>
|
||||
<meta>
|
||||
<layer>0</layer>
|
||||
</meta>
|
||||
</visual>
|
||||
<pose>1.32002 0.107134 0 0 -0 0.001157</pose>
|
||||
</link>
|
||||
<link name='Wall_6'>
|
||||
<collision name='Wall_6_Collision'>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>2 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
</collision>
|
||||
<visual name='Wall_6_Visual'>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>2 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
<ambient>1 1 1 1</ambient>
|
||||
</material>
|
||||
<meta>
|
||||
<layer>0</layer>
|
||||
</meta>
|
||||
</visual>
|
||||
<pose>-2.50971 -1.81518 0 0 -0 1.5708</pose>
|
||||
</link>
|
||||
<static>1</static>
|
||||
</model>
|
||||
</sdf>
|
||||
29
src/navigation/gc_navigation_fish/CMakeLists.txt
Normal file
29
src/navigation/gc_navigation_fish/CMakeLists.txt
Normal file
@@ -0,0 +1,29 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(gc_navigation_fish)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
# uncomment the following section in order to fill in
|
||||
# further dependencies manually.
|
||||
# find_package(<dependency> REQUIRED)
|
||||
install(
|
||||
DIRECTORY launch param maps
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
@@ -0,0 +1,34 @@
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import IncludeLaunchDescription
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
# =============================1.定位到包的地址=============================================================
|
||||
gc_navigation_fish_dir = get_package_share_directory(
|
||||
'gc_navigation_fish')
|
||||
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
|
||||
|
||||
# =============================2.声明参数，获取配置文件路径===================================================
|
||||
# use_sim_time 这里要设置成true,因为gazebo是仿真环境，其时间是通过/clock话题获取，而不是系统时间
|
||||
use_sim_time = LaunchConfiguration('use_sim_time', default='false')
|
||||
map_yaml_path = LaunchConfiguration('map', default=os.path.join(
|
||||
gc_navigation_fish_dir, 'maps', 'test_map.yaml'))
|
||||
nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
|
||||
gc_navigation_fish_dir, 'param', 'gc_navigation.yaml'))
|
||||
# =============================3.声明启动launch文件，传入：地图路径、是否使用仿真时间以及nav2参数文件==============
|
||||
nav2_bringup_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
[nav2_bringup_dir, '/launch', '/bringup_launch.py']),
|
||||
launch_arguments={
|
||||
'map': map_yaml_path,
|
||||
'use_sim_time': use_sim_time,
|
||||
'params_file': nav2_param_path}.items(),
|
||||
)
|
||||
|
||||
|
||||
return LaunchDescription([nav2_bringup_launch])
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user