feat: add lms-nav2-gazebo-planning module
This commit is contained in:
@@ -0,0 +1,5 @@
|
||||
build/
|
||||
install/
|
||||
log/
|
||||
.colcon/
|
||||
*.log
|
||||
@@ -0,0 +1,6 @@
|
||||
#!/usr/bin/env bash
|
||||
set -euo pipefail
|
||||
|
||||
MODULE_PATH="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||
RUNTIME_ROOT="${AUTONOMY_STUDIO_RUNTIME_ROOT:-$HOME/autonomy-studio/runtime/nav2}"
|
||||
exec bash "$RUNTIME_ROOT/docker/docker_build_nav2.sh" "$(dirname "$MODULE_PATH")" "$(basename "$MODULE_PATH")" "${NAV2_DOCKER_IMAGE:-future-nav2-gazebo:humble}" "${ROS_DOMAIN_ID:-0}"
|
||||
@@ -0,0 +1,8 @@
|
||||
#!/usr/bin/env bash
|
||||
set -euo pipefail
|
||||
|
||||
MODULE_PATH="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||
RUNTIME_ROOT="${AUTONOMY_STUDIO_RUNTIME_ROOT:-$HOME/autonomy-studio/runtime/nav2}"
|
||||
FOLDER_NAME="$(basename "$MODULE_PATH")"
|
||||
CONTAINER_NAME="${FOLDER_NAME#\[}"; CONTAINER_NAME="${CONTAINER_NAME/\]/-}"
|
||||
exec bash "$RUNTIME_ROOT/docker/docker_run_nav2.sh" "$CONTAINER_NAME" "$(dirname "$MODULE_PATH")" "$FOLDER_NAME" "${NAV2_DOCKER_IMAGE:-future-nav2-gazebo:humble}" "${ROS_DOMAIN_ID:-0}" "$@"
|
||||
@@ -0,0 +1,36 @@
|
||||
|
||||
<!--
|
||||
navigate_through_poses_w_replanning_and_recovery.xml (Spin removed for Ackermann vehicle)
|
||||
-->
|
||||
<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>
|
||||
<Wait wait_duration="2"/>
|
||||
<BackUp backup_dist="0.30" backup_speed="0.05"/>
|
||||
</RoundRobin>
|
||||
</ReactiveFallback>
|
||||
</RecoveryNode>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
+34
@@ -0,0 +1,34 @@
|
||||
|
||||
<!--
|
||||
navigate_to_pose_w_replanning_and_recovery.xml (Spin removed for Ackermann vehicle)
|
||||
-->
|
||||
<root main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
<RecoveryNode number_of_retries="8" 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">
|
||||
<!-- <BackUp backup_dist="1.0" backup_speed="0.20"/> -->
|
||||
<BackUp backup_dist="0.3" backup_speed="0.20"/>
|
||||
<!-- <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> -->
|
||||
<Wait wait_duration="1"/>
|
||||
</RoundRobin>
|
||||
</ReactiveFallback>
|
||||
</RecoveryNode>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
+153
@@ -0,0 +1,153 @@
|
||||
# File: real/mkmini/planning_2d.yaml
|
||||
# Description: Planning parameters for MK-Mini (2D LiDAR) — real vehicle
|
||||
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
odom_topic: /odom_custom
|
||||
bt_loop_duration: 20
|
||||
default_server_timeout: 2000
|
||||
wait_for_service_timeout: 4000
|
||||
default_nav_to_pose_bt_xml: ""
|
||||
default_nav_through_poses_bt_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_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
|
||||
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 2.0
|
||||
publish_frequency: 2.0
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: True
|
||||
footprint: "[ [0.35, 0.28], [0.35, -0.28], [-0.35, -0.28], [-0.35, 0.28] ]"
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
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: 2.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 1.0
|
||||
obstacle_min_range: 0.0
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.5
|
||||
always_send_full_costmap: True
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
expected_planner_frequency: 2.0
|
||||
use_sim_time: True
|
||||
planner_plugins: ["GridBased"]
|
||||
GridBased:
|
||||
plugin: "nav2_smac_planner/SmacPlannerHybrid"
|
||||
tolerance: 0.5
|
||||
allow_unknown: true
|
||||
downsample_costmap: true
|
||||
downsampling_factor: 2
|
||||
max_iterations: 3000000
|
||||
max_on_approach_iterations: 5000
|
||||
max_planning_time: 10.0
|
||||
motion_model_for_search: "DUBIN"
|
||||
angle_quantization_bins: 144
|
||||
analytic_expansion_ratio: 3.5
|
||||
analytic_expansion_max_length: 5.0
|
||||
minimum_turning_radius: 1.1
|
||||
change_penalty: 0.0
|
||||
non_straight_penalty: 1.5
|
||||
cost_penalty: 5.0
|
||||
retrospective_penalty: 0.1
|
||||
lookup_table_size: 20.0
|
||||
cache_obstacle_heuristic: true
|
||||
smooth_path: 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
|
||||
|
||||
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
|
||||
+153
@@ -0,0 +1,153 @@
|
||||
# File: fr09/planning_2d.yaml
|
||||
# Description: Planning parameters for FR-09 (2D LiDAR)
|
||||
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
odom_topic: /odom_custom
|
||||
bt_loop_duration: 20
|
||||
default_server_timeout: 2000
|
||||
wait_for_service_timeout: 4000
|
||||
default_nav_to_pose_bt_xml: ""
|
||||
default_nav_through_poses_bt_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_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
|
||||
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 2.0
|
||||
publish_frequency: 2.0
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: True
|
||||
footprint: "[ [0.79, 0.37], [0.79, -0.37], [-0.79, -0.37], [-0.79, 0.37] ]"
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
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: 10.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 8.0
|
||||
obstacle_min_range: 0.0
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 2.0
|
||||
inflation_radius: 1.5
|
||||
always_send_full_costmap: True
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
expected_planner_frequency: 2.0
|
||||
use_sim_time: True
|
||||
planner_plugins: ["GridBased"]
|
||||
GridBased:
|
||||
plugin: "nav2_smac_planner/SmacPlannerHybrid"
|
||||
tolerance: 0.5
|
||||
allow_unknown: true
|
||||
downsample_costmap: true
|
||||
downsampling_factor: 2
|
||||
max_iterations: 3000000
|
||||
max_on_approach_iterations: 5000
|
||||
max_planning_time: 10.0
|
||||
motion_model_for_search: "DUBIN"
|
||||
angle_quantization_bins: 144
|
||||
analytic_expansion_ratio: 3.5
|
||||
analytic_expansion_max_length: 5.0
|
||||
minimum_turning_radius: 1.71
|
||||
change_penalty: 0.0
|
||||
non_straight_penalty: 1.5
|
||||
cost_penalty: 5.0
|
||||
retrospective_penalty: 0.1
|
||||
lookup_table_size: 20.0
|
||||
cache_obstacle_heuristic: true
|
||||
smooth_path: 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
|
||||
|
||||
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
|
||||
+153
@@ -0,0 +1,153 @@
|
||||
# File: mkmini/planning_2d.yaml
|
||||
# Description: Planning parameters for MK-Mini (2D LiDAR)
|
||||
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
odom_topic: /odom_custom
|
||||
bt_loop_duration: 20
|
||||
default_server_timeout: 2000
|
||||
wait_for_service_timeout: 4000
|
||||
default_nav_to_pose_bt_xml: ""
|
||||
default_nav_through_poses_bt_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_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
|
||||
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 2.0
|
||||
publish_frequency: 2.0
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: True
|
||||
footprint: "[ [0.42, 0.305], [0.42, -0.305], [-0.42, -0.305], [-0.42, 0.305] ]"
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
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: 8.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 6.0
|
||||
obstacle_min_range: 0.0
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 2.0
|
||||
inflation_radius: 1.5
|
||||
always_send_full_costmap: True
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
expected_planner_frequency: 2.0
|
||||
use_sim_time: True
|
||||
planner_plugins: ["GridBased"]
|
||||
GridBased:
|
||||
plugin: "nav2_smac_planner/SmacPlannerHybrid"
|
||||
tolerance: 0.5
|
||||
allow_unknown: true
|
||||
downsample_costmap: true
|
||||
downsampling_factor: 2
|
||||
max_iterations: 3000000
|
||||
max_on_approach_iterations: 5000
|
||||
max_planning_time: 10.0
|
||||
motion_model_for_search: "DUBIN"
|
||||
angle_quantization_bins: 144
|
||||
analytic_expansion_ratio: 3.5
|
||||
analytic_expansion_max_length: 5.0
|
||||
minimum_turning_radius: 1.1
|
||||
change_penalty: 0.0
|
||||
non_straight_penalty: 1.5
|
||||
cost_penalty: 5.0
|
||||
retrospective_penalty: 0.1
|
||||
lookup_table_size: 20.0
|
||||
cache_obstacle_heuristic: true
|
||||
smooth_path: 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
|
||||
|
||||
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
|
||||
@@ -0,0 +1,151 @@
|
||||
# TurtleBot3 Burger Gazebo profile. The simulator provides /scan, /odom, TF,
|
||||
# and consumes /cmd_vel directly.
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: true
|
||||
global_frame: map
|
||||
robot_base_frame: base_footprint
|
||||
odom_topic: /odom
|
||||
bt_loop_duration: 20
|
||||
default_server_timeout: 2000
|
||||
wait_for_service_timeout: 4000
|
||||
|
||||
bt_navigator_navigate_to_pose_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: true
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
use_sim_time: true
|
||||
expected_planner_frequency: 20.0
|
||||
planner_plugins: [GridBased]
|
||||
GridBased:
|
||||
plugin: nav2_navfn_planner/NavfnPlanner
|
||||
tolerance: 0.5
|
||||
use_astar: false
|
||||
allow_unknown: true
|
||||
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
use_sim_time: true
|
||||
controller_frequency: 20.0
|
||||
min_x_velocity_threshold: 0.001
|
||||
min_y_velocity_threshold: 0.0
|
||||
min_theta_velocity_threshold: 0.001
|
||||
progress_checker_plugin: progress_checker
|
||||
goal_checker_plugins: [goal_checker]
|
||||
controller_plugins: [FollowPath]
|
||||
progress_checker:
|
||||
plugin: nav2_controller::SimpleProgressChecker
|
||||
required_movement_radius: 0.08
|
||||
movement_time_allowance: 10.0
|
||||
goal_checker:
|
||||
plugin: nav2_controller::SimpleGoalChecker
|
||||
xy_goal_tolerance: 0.15
|
||||
yaw_goal_tolerance: 0.2
|
||||
stateful: true
|
||||
FollowPath:
|
||||
plugin: dwb_core::DWBLocalPlanner
|
||||
debug_trajectory_details: false
|
||||
min_vel_x: -0.05
|
||||
max_vel_x: 0.22
|
||||
max_vel_theta: 2.5
|
||||
min_speed_xy: 0.0
|
||||
max_speed_xy: 0.22
|
||||
acc_lim_x: 1.5
|
||||
decel_lim_x: -1.5
|
||||
acc_lim_theta: 3.2
|
||||
decel_lim_theta: -3.2
|
||||
vx_samples: 20
|
||||
vtheta_samples: 20
|
||||
sim_time: 1.7
|
||||
transform_tolerance: 0.2
|
||||
xy_goal_tolerance: 0.15
|
||||
trans_stopped_velocity: 0.05
|
||||
short_circuit_trajectory_evaluation: true
|
||||
stateful: true
|
||||
critics: [RotateToGoal, Oscillation, BaseObstacle, GoalAlign, PathAlign, PathDist, GoalDist]
|
||||
BaseObstacle.scale: 0.02
|
||||
PathAlign.scale: 24.0
|
||||
PathAlign.forward_point_distance: 0.1
|
||||
GoalAlign.scale: 18.0
|
||||
GoalAlign.forward_point_distance: 0.1
|
||||
PathDist.scale: 24.0
|
||||
GoalDist.scale: 18.0
|
||||
RotateToGoal.scale: 24.0
|
||||
RotateToGoal.slowing_factor: 5.0
|
||||
RotateToGoal.lookahead_time: -1.0
|
||||
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
use_sim_time: true
|
||||
global_frame: map
|
||||
robot_base_frame: base_footprint
|
||||
update_frequency: 5.0
|
||||
publish_frequency: 2.0
|
||||
transform_tolerance: 0.5
|
||||
resolution: 0.05
|
||||
robot_radius: 0.12
|
||||
track_unknown_space: true
|
||||
plugins: [static_layer, obstacle_layer, inflation_layer]
|
||||
static_layer:
|
||||
plugin: nav2_costmap_2d::StaticLayer
|
||||
map_subscribe_transient_local: true
|
||||
obstacle_layer:
|
||||
plugin: nav2_costmap_2d::ObstacleLayer
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
data_type: LaserScan
|
||||
marking: true
|
||||
clearing: true
|
||||
max_obstacle_height: 1.0
|
||||
inflation_layer:
|
||||
plugin: nav2_costmap_2d::InflationLayer
|
||||
inflation_radius: 0.30
|
||||
cost_scaling_factor: 3.0
|
||||
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
use_sim_time: true
|
||||
global_frame: odom
|
||||
robot_base_frame: base_footprint
|
||||
rolling_window: true
|
||||
width: 3
|
||||
height: 3
|
||||
resolution: 0.05
|
||||
robot_radius: 0.12
|
||||
update_frequency: 5.0
|
||||
publish_frequency: 2.0
|
||||
plugins: [obstacle_layer, inflation_layer]
|
||||
obstacle_layer:
|
||||
plugin: nav2_costmap_2d::ObstacleLayer
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
data_type: LaserScan
|
||||
marking: true
|
||||
clearing: true
|
||||
max_obstacle_height: 1.0
|
||||
inflation_layer:
|
||||
plugin: nav2_costmap_2d::InflationLayer
|
||||
inflation_radius: 0.30
|
||||
cost_scaling_factor: 3.0
|
||||
|
||||
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
|
||||
|
||||
waypoint_follower:
|
||||
ros__parameters:
|
||||
use_sim_time: true
|
||||
loop_rate: 20
|
||||
stop_on_failure: false
|
||||
+168
@@ -0,0 +1,168 @@
|
||||
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
odom_topic: /odom_custom
|
||||
bt_loop_duration: 20
|
||||
default_server_timeout: 2000
|
||||
wait_for_service_timeout: 4000
|
||||
|
||||
default_nav_to_pose_bt_xml: "navigate_to_pose_w_replanning_and_recovery.xml"
|
||||
default_nav_through_poses_bt_xml: "navigate_through_poses_w_replanning_and_recovery.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_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
|
||||
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
transform_tolerance: 1.0
|
||||
|
||||
update_frequency: 0.5
|
||||
publish_frequency: 0.5
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: True
|
||||
footprint: "[ [0.35, 0.28], [0.35, -0.28], [-0.35, -0.28], [-0.35, 0.28] ]"
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
|
||||
|
||||
obstacle_layer:
|
||||
min_obstacle_height: 0.1
|
||||
max_obstacle_height: 2.5
|
||||
|
||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
||||
enabled: True
|
||||
observation_sources: lidar3d
|
||||
lidar3d:
|
||||
topic: /lidar_filtered
|
||||
|
||||
min_obstacle_height: 0.1
|
||||
max_obstacle_height: 2.5
|
||||
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "PointCloud2"
|
||||
obstacle_min_range: 0.0
|
||||
obstacle_max_range: 15.0
|
||||
raytrace_min_range: 0.0
|
||||
raytrace_max_range: 15.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.02
|
||||
always_send_full_costmap: True
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
expected_planner_frequency: 2.0
|
||||
use_sim_time: True
|
||||
planner_plugins: ["GridBased"]
|
||||
GridBased:
|
||||
plugin: "nav2_smac_planner/SmacPlannerHybrid"
|
||||
tolerance: 0.5
|
||||
allow_unknown: true
|
||||
downsample_costmap: true
|
||||
downsampling_factor: 2
|
||||
max_iterations: 3000000
|
||||
max_on_approach_iterations: 5000
|
||||
max_planning_time: 10.0
|
||||
motion_model_for_search: "DUBIN"
|
||||
angle_quantization_bins: 144
|
||||
analytic_expansion_ratio: 3.5
|
||||
analytic_expansion_max_length: 5.0
|
||||
minimum_turning_radius: 1.1
|
||||
reverse_penalty: 2.1
|
||||
change_penalty: 0.0
|
||||
non_straight_penalty: 1.5
|
||||
cost_penalty: 5.0
|
||||
retrospective_penalty: 0.1
|
||||
lookup_table_size: 20.0
|
||||
cache_obstacle_heuristic: true
|
||||
smooth_path: 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
|
||||
|
||||
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
|
||||
+166
@@ -0,0 +1,166 @@
|
||||
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
odom_topic: wheel_odom
|
||||
bt_loop_duration: 20
|
||||
default_server_timeout: 2000
|
||||
wait_for_service_timeout: 4000
|
||||
|
||||
default_nav_to_pose_bt_xml: "navigate_to_pose_w_replanning_and_recovery.xml"
|
||||
default_nav_through_poses_bt_xml: "navigate_through_poses_w_replanning_and_recovery.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_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
|
||||
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 0.5
|
||||
publish_frequency: 0.5
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: True
|
||||
footprint: "[[1.220, 0.3715], [1.220, -0.3715], [-0.369, -0.3715], [-0.369, 0.3715]]"
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
|
||||
|
||||
obstacle_layer:
|
||||
min_obstacle_height: 0.1
|
||||
max_obstacle_height: 2.5
|
||||
|
||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
||||
enabled: True
|
||||
observation_sources: lidar3d
|
||||
lidar3d:
|
||||
topic: /lidar_filtered
|
||||
|
||||
min_obstacle_height: 0.1
|
||||
max_obstacle_height: 2.5
|
||||
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "PointCloud2"
|
||||
obstacle_min_range: 0.0
|
||||
obstacle_max_range: 15.0
|
||||
raytrace_min_range: 0.0
|
||||
raytrace_max_range: 15.0
|
||||
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 2.0
|
||||
inflation_radius: 1.0
|
||||
always_send_full_costmap: True
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
expected_planner_frequency: 2.0
|
||||
use_sim_time: True
|
||||
planner_plugins: ["GridBased"]
|
||||
GridBased:
|
||||
plugin: "nav2_smac_planner/SmacPlannerHybrid"
|
||||
tolerance: 0.5
|
||||
allow_unknown: true
|
||||
downsample_costmap: true
|
||||
downsampling_factor: 2
|
||||
max_iterations: 3000000
|
||||
max_on_approach_iterations: 5000
|
||||
max_planning_time: 10.0
|
||||
motion_model_for_search: "DUBIN"
|
||||
angle_quantization_bins: 144
|
||||
analytic_expansion_ratio: 3.5
|
||||
analytic_expansion_max_length: 5.0
|
||||
minimum_turning_radius: 0.5
|
||||
reverse_penalty: 2.1
|
||||
change_penalty: 1.0
|
||||
non_straight_penalty: 1.5
|
||||
cost_penalty: 5.0
|
||||
retrospective_penalty: 0.1
|
||||
lookup_table_size: 20.0
|
||||
cache_obstacle_heuristic: true
|
||||
smooth_path: 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
|
||||
|
||||
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
|
||||
+166
@@ -0,0 +1,166 @@
|
||||
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
odom_topic: wheel_odom
|
||||
bt_loop_duration: 20
|
||||
default_server_timeout: 2000
|
||||
wait_for_service_timeout: 4000
|
||||
|
||||
default_nav_to_pose_bt_xml: "navigate_to_pose_w_replanning_and_recovery.xml"
|
||||
default_nav_through_poses_bt_xml: "navigate_through_poses_w_replanning_and_recovery.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_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
|
||||
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 0.5
|
||||
publish_frequency: 0.5
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: True
|
||||
footprint: "[ [0.42, 0.305], [0.42, -0.305], [-0.42, -0.305], [-0.42, 0.305] ]"
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
|
||||
|
||||
obstacle_layer:
|
||||
min_obstacle_height: 0.1
|
||||
max_obstacle_height: 2.5
|
||||
|
||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
||||
enabled: True
|
||||
observation_sources: lidar3d
|
||||
lidar3d:
|
||||
topic: /carla/hero/lidar_filtered
|
||||
|
||||
min_obstacle_height: 0.1
|
||||
max_obstacle_height: 2.5
|
||||
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "PointCloud2"
|
||||
obstacle_min_range: 0.0
|
||||
obstacle_max_range: 15.0
|
||||
raytrace_min_range: 0.0
|
||||
raytrace_max_range: 15.0
|
||||
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 2.0
|
||||
inflation_radius: 1.0
|
||||
always_send_full_costmap: True
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
expected_planner_frequency: 2.0
|
||||
use_sim_time: True
|
||||
planner_plugins: ["GridBased"]
|
||||
GridBased:
|
||||
plugin: "nav2_smac_planner/SmacPlannerHybrid"
|
||||
tolerance: 0.5
|
||||
allow_unknown: true
|
||||
downsample_costmap: true
|
||||
downsampling_factor: 2
|
||||
max_iterations: 3000000
|
||||
max_on_approach_iterations: 5000
|
||||
max_planning_time: 10.0
|
||||
motion_model_for_search: "DUBIN"
|
||||
angle_quantization_bins: 144
|
||||
analytic_expansion_ratio: 3.5
|
||||
analytic_expansion_max_length: 5.0
|
||||
minimum_turning_radius: 1.1
|
||||
reverse_penalty: 2.1
|
||||
change_penalty: 0.0
|
||||
non_straight_penalty: 1.5
|
||||
cost_penalty: 5.0
|
||||
retrospective_penalty: 0.1
|
||||
lookup_table_size: 20.0
|
||||
cache_obstacle_heuristic: true
|
||||
smooth_path: 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
|
||||
|
||||
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
|
||||
Binary file not shown.
+144
@@ -0,0 +1,144 @@
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.actions import GroupAction
|
||||
from launch.actions import OpaqueFunction
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
from launch.substitutions import PythonExpression
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.descriptions import ParameterFile
|
||||
from nav2_common.launch import RewrittenYaml
|
||||
|
||||
|
||||
ARGUMENTS = [
|
||||
DeclareLaunchArgument('sim_mode', default_value='true'),
|
||||
DeclareLaunchArgument('log_level', default_value='info'),
|
||||
DeclareLaunchArgument('robot', default_value='fr09', choices=['mkmini', 'fr09', 'turtlebot3']),
|
||||
DeclareLaunchArgument('level', default_value='3d'),
|
||||
]
|
||||
|
||||
def launch_setup(context, *args, **kwargs):
|
||||
sim_mode = LaunchConfiguration('sim_mode').perform(context).lower()
|
||||
robot = LaunchConfiguration('robot').perform(context)
|
||||
level = LaunchConfiguration('level').perform(context)
|
||||
log_level = LaunchConfiguration('log_level').perform(context)
|
||||
|
||||
lifecycle_nodes = ['controller_server',
|
||||
'smoother_server',
|
||||
'planner_server',
|
||||
'bt_navigator',
|
||||
'waypoint_follower']
|
||||
|
||||
pkg_planning = get_package_share_directory('planning_pkg')
|
||||
|
||||
# Behavior Tree
|
||||
default_nav_to_pose_bt_xml = PathJoinSubstitution([
|
||||
pkg_planning,
|
||||
'behavior',
|
||||
'navigate_to_pose_no_spin.xml'
|
||||
])
|
||||
default_nav_through_poses_bt_xml = PathJoinSubstitution([
|
||||
pkg_planning,
|
||||
'behavior',
|
||||
'navigate_through_poses_no_spin.xml'
|
||||
])
|
||||
|
||||
param_file_name = None
|
||||
|
||||
if sim_mode == 'true':
|
||||
param_file_name = f'planning_{level}_sim_{robot}.yaml'
|
||||
else:
|
||||
param_file_name = f'planning_{level}_real_{robot}.yaml'
|
||||
|
||||
# Parameters
|
||||
params_file = PathJoinSubstitution([
|
||||
pkg_planning,
|
||||
'config', param_file_name])
|
||||
param_substitutions = {
|
||||
'use_sim_time': 'true',
|
||||
'default_nav_to_pose_bt_xml': default_nav_to_pose_bt_xml,
|
||||
'default_nav_through_poses_bt_xml': default_nav_through_poses_bt_xml
|
||||
}
|
||||
configured_params = ParameterFile(
|
||||
RewrittenYaml(
|
||||
source_file=params_file,
|
||||
param_rewrites=param_substitutions,
|
||||
convert_types=True),
|
||||
allow_substs=True)
|
||||
|
||||
load_nodes = GroupAction(
|
||||
actions=[
|
||||
Node(
|
||||
package='nav2_controller',
|
||||
executable='controller_server',
|
||||
name='controller_server',
|
||||
output='screen',
|
||||
respawn=False,
|
||||
respawn_delay=2.0,
|
||||
parameters=[configured_params],
|
||||
arguments=['--ros-args', '--log-level', log_level],
|
||||
),
|
||||
Node(
|
||||
package='nav2_planner',
|
||||
executable='planner_server',
|
||||
name='planner_server',
|
||||
output='screen',
|
||||
respawn=False,
|
||||
respawn_delay=2.0,
|
||||
parameters=[configured_params],
|
||||
arguments=['--ros-args', '--log-level', log_level],
|
||||
),
|
||||
Node(
|
||||
package='nav2_bt_navigator',
|
||||
executable='bt_navigator',
|
||||
name='bt_navigator',
|
||||
output='screen',
|
||||
respawn=False,
|
||||
respawn_delay=2.0,
|
||||
parameters=[configured_params],
|
||||
arguments=['--ros-args', '--log-level', log_level],
|
||||
),
|
||||
Node(
|
||||
package='nav2_waypoint_follower',
|
||||
executable='waypoint_follower',
|
||||
name='waypoint_follower',
|
||||
output='screen',
|
||||
respawn=False,
|
||||
respawn_delay=2.0,
|
||||
parameters=[configured_params],
|
||||
arguments=['--ros-args', '--log-level', log_level],
|
||||
),
|
||||
Node(
|
||||
package='nav2_smoother',
|
||||
executable='smoother_server',
|
||||
name='smoother_server',
|
||||
output='screen',
|
||||
respawn=False,
|
||||
respawn_delay=2.0,
|
||||
parameters=[configured_params],
|
||||
arguments=['--ros-args', '--log-level', log_level],
|
||||
),
|
||||
Node(
|
||||
package='nav2_lifecycle_manager',
|
||||
executable='lifecycle_manager',
|
||||
name='lifecycle_manager_planning',
|
||||
output='screen',
|
||||
arguments=['--ros-args', '--log-level', log_level],
|
||||
parameters=[{'use_sim_time': True},
|
||||
{'autostart': True},
|
||||
{'node_names': lifecycle_nodes}]
|
||||
),
|
||||
],
|
||||
)
|
||||
|
||||
return [load_nodes]
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
# Create launch description and add actions
|
||||
ld = LaunchDescription(ARGUMENTS)
|
||||
ld.add_action(OpaqueFunction(function=launch_setup))
|
||||
|
||||
return ld
|
||||
Executable
+15
@@ -0,0 +1,15 @@
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def generate_launch_description():
|
||||
node = Node(
|
||||
package='planning_pkg',
|
||||
executable='planning_manager',
|
||||
name='planning_manager',
|
||||
output='screen',
|
||||
)
|
||||
|
||||
ld = LaunchDescription([])
|
||||
ld.add_action(node)
|
||||
return ld
|
||||
Executable
+24
@@ -0,0 +1,24 @@
|
||||
<?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>planning_pkg</name>
|
||||
<version>0.0.1</version>
|
||||
<description>Modular Nav2 launch package for FR-09/MK-Mini autonomous driving</description>
|
||||
<maintainer email="todo@todo.com">FutureDrive</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<exec_depend>nav2_planner</exec_depend>
|
||||
<exec_depend>nav2_smoother</exec_depend>
|
||||
<exec_depend>nav2_bt_navigator</exec_depend>
|
||||
<exec_depend>nav2_lifecycle_manager</exec_depend>
|
||||
<exec_depend>nav2_waypoint_follower</exec_depend>
|
||||
|
||||
<buildtool_depend>ament_python</buildtool_depend>
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
Binary file not shown.
Binary file not shown.
+57
@@ -0,0 +1,57 @@
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import QoSProfile, DurabilityPolicy, ReliabilityPolicy
|
||||
from std_msgs.msg import String
|
||||
|
||||
import subprocess
|
||||
import json
|
||||
|
||||
class PlanningManager(Node):
|
||||
def __init__(self):
|
||||
super().__init__('planning_manager')
|
||||
self.get_logger().info('Planning Manager node has been started.')
|
||||
|
||||
qos = QoSProfile(
|
||||
depth=1,
|
||||
durability=DurabilityPolicy.TRANSIENT_LOCAL,
|
||||
reliability=ReliabilityPolicy.RELIABLE
|
||||
)
|
||||
self.planning_process = None
|
||||
|
||||
self.create_subscription(String, '/vehicle_config', self.handle_config, qos_profile=qos)
|
||||
|
||||
def handle_config(self, msg:String):
|
||||
self.get_logger().info(f"SLAM Received config: {msg.data}")
|
||||
config_data = json.loads(msg.data)
|
||||
|
||||
self.planning_process = subprocess.Popen(['ros2', 'launch',
|
||||
'planning_pkg',
|
||||
'nav2_planning.launch.py',
|
||||
f'sim_mode:={config_data.get("sim_mode")}',
|
||||
f'robot:={config_data.get("robot")}',
|
||||
f'level:={config_data.get("level")}'])
|
||||
pass
|
||||
|
||||
def destroy_node(self):
|
||||
if self.planning_process:
|
||||
self.planning_process.terminate()
|
||||
self.planning_process.wait()
|
||||
|
||||
return super().destroy_node()
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = PlanningManager()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
||||
+91
@@ -0,0 +1,91 @@
|
||||
#!/usr/bin/env python3
|
||||
# File: waypoint_sender.py
|
||||
# Description:
|
||||
# WAYPOINTS 리스트에 정의된 목표 지점을 순서대로 navigate_to_pose 액션으로 전송하는 유틸 노드.
|
||||
# 한 목표에 도달(결과 수신)하면 자동으로 다음 목표를 전송하고, 모두 완료되면 종료.
|
||||
# waypoint 좌표는 RViz에서 /amcl_pose를 확인하거나 ros2 topic echo /amcl_pose로 기록.
|
||||
|
||||
import math
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.action import ActionClient
|
||||
from nav2_msgs.action import NavigateToPose
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
|
||||
|
||||
# Waypoint 목록
|
||||
# 형식: (x, y, qz, qw) — map 프레임 기준 위치와 방향 (quaternion의 z, w 성분)
|
||||
# qx=0, qy=0 고정 (2D 평면 운동)
|
||||
WAYPOINTS = [
|
||||
(46.626, 0.0, -0.131, 0.991), # WP1
|
||||
(48.180, -16.008, -0.892, 0.452), # WP2
|
||||
(11.741, -17.762, 0.991, 0.131), # WP3
|
||||
(20.609, -0.391, -0.020, 1.000), # WP4
|
||||
]
|
||||
|
||||
|
||||
class WaypointSender(Node):
|
||||
def __init__(self):
|
||||
super().__init__('waypoint_sender')
|
||||
|
||||
# navigate_to_pose 액션 클라이언트 (bt_navigator가 서버)
|
||||
self._client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
|
||||
self._waypoints = WAYPOINTS
|
||||
self._index = 0 # 현재 전송 중인 waypoint 인덱스
|
||||
|
||||
def send_next(self):
|
||||
"""다음 waypoint 목표 전송. 모두 완료되면 노드 종료."""
|
||||
if self._index >= len(self._waypoints):
|
||||
self.get_logger().info('모든 waypoint 완료!')
|
||||
rclpy.shutdown()
|
||||
return
|
||||
|
||||
x, y, qz, qw = self._waypoints[self._index]
|
||||
qx, qy = 0.0, 0.0
|
||||
|
||||
# PoseStamped 목표 생성
|
||||
goal = NavigateToPose.Goal()
|
||||
goal.pose = PoseStamped()
|
||||
goal.pose.header.frame_id = 'map'
|
||||
goal.pose.header.stamp = self.get_clock().now().to_msg()
|
||||
goal.pose.pose.position.x = x
|
||||
goal.pose.pose.position.y = y
|
||||
goal.pose.pose.position.z = 0.0
|
||||
goal.pose.pose.orientation.x = qx
|
||||
goal.pose.pose.orientation.y = qy
|
||||
goal.pose.pose.orientation.z = qz
|
||||
goal.pose.pose.orientation.w = qw
|
||||
|
||||
self.get_logger().info(
|
||||
f'[{self._index + 1}/{len(self._waypoints)}] 목표 전송: ({x}, {y})'
|
||||
)
|
||||
|
||||
# 서버(bt_navigator) 준비 대기 후 goal 전송
|
||||
self._client.wait_for_server()
|
||||
future = self._client.send_goal_async(goal)
|
||||
future.add_done_callback(self._goal_response_cb)
|
||||
|
||||
def _goal_response_cb(self, future):
|
||||
"""goal 수락 여부 확인 후 result 콜백 등록"""
|
||||
handle = future.result()
|
||||
if not handle.accepted:
|
||||
self.get_logger().warn('Goal 거절됨')
|
||||
return
|
||||
handle.get_result_async().add_done_callback(self._result_cb)
|
||||
|
||||
def _result_cb(self, future):
|
||||
"""목표 도달 결과 수신 → 다음 waypoint 전송"""
|
||||
self._index += 1
|
||||
self.send_next()
|
||||
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
node = WaypointSender()
|
||||
node.get_logger().info('Waypoint sender 시작')
|
||||
node.send_next()
|
||||
rclpy.spin(node)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
Executable
Executable
+4
@@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/planning_pkg
|
||||
[install]
|
||||
install_scripts=$base/lib/planning_pkg
|
||||
Executable
+31
@@ -0,0 +1,31 @@
|
||||
|
||||
import os
|
||||
from glob import glob
|
||||
from setuptools import find_packages, setup
|
||||
|
||||
package_name = 'planning_pkg'
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version='0.0.1',
|
||||
packages=find_packages(exclude=['test']),
|
||||
data_files=[
|
||||
('share/ament_index/resource_index/packages', ['resource/planning_pkg']),
|
||||
('share/' + package_name, ['package.xml']),
|
||||
(os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
|
||||
(os.path.join('share', package_name, 'config'), glob('config/*.yaml')),
|
||||
(os.path.join('share', package_name, 'behavior'), glob('behavior/*.xml')),
|
||||
],
|
||||
install_requires=['setuptools'],
|
||||
zip_safe=True,
|
||||
maintainer='FutureDrive',
|
||||
maintainer_email='todo@todo.com',
|
||||
description='Modular Nav2 launch package for FR-09/MK-Mini autonomous driving',
|
||||
license='Apache-2.0',
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'waypoint_sender = planning_pkg.waypoint_sender:main',
|
||||
'planning_manager = planning_pkg.planning_manager:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
Reference in New Issue
Block a user