templates/lms-nav2-gazebo-control
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: add NAV2 Gazebo control module

Browse Source
This commit is contained in:
chyoo
2026-07-29 11:26:20 +09:00
commit f3e91c5bc9
20 changed files with 1422 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+6
View File
@@ -0,0 +1,6 @@
build/
install/
log/
__pycache__/
**/__pycache__/
*.py[cod]
build.sh
Executable
+6
View File
@@ -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}"
run.sh
Executable
+10
View File
@@ -0,0 +1,10 @@
#!/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/\]/-}"
ROS_PARAM='sim_mode:=true robot:=turtlebot3 level:=2d'
read -r -a ROS_ARGS <<< "$ROS_PARAM"
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}" "${ROS_ARGS[@]}" "$@"
src/control_pkg/config/control_2d_real_mkmini.yaml
+119
View File
@@ -0,0 +1,119 @@
# File: real/mkmini/control_2d.yaml
# Description: Control parameters for MK-Mini (2D LiDAR) — real vehicle
controller_server:
ros__parameters:
use_sim_time: True
controller_frequency: 10.0
min_x_velocity_threshold: 0.001
min_y_velocity_threshold: 0.0
min_theta_velocity_threshold: 0.001
failure_tolerance: 1.0
progress_checker_plugin: "progress_checker"
goal_checker_plugins: ["general_goal_checker"]
controller_plugins: ["FollowPath"]
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.2
movement_time_allowance: 20.0
general_goal_checker:
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.5
yaw_goal_tolerance: 2.0
stateful: True
FollowPath:
plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
allow_reversing: false
use_dynamic_window: false
use_velocity_scaled_lookahead_dist: false
use_rotate_to_heading: false
use_collision_detection: true
use_regulated_linear_velocity_scaling: true
use_cost_regulated_linear_velocity_scaling: false
desired_linear_vel: 0.5
min_approach_linear_velocity: 0.3
lookahead_dist: 0.5
cost_scaling_dist: 0.8
cost_scaling_gain: 1.0
approach_velocity_scaling_dist: 2.0
regulated_linear_scaling_min_radius: 1.1
regulated_linear_scaling_min_speed: 0.3
max_allowed_time_to_collision_up_to_carrot: 2.0
local_costmap:
local_costmap:
ros__parameters:
update_frequency: 10.0
publish_frequency: 5.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.35, 0.28], [0.35, -0.28], [-0.35, -0.28], [-0.35, 0.28] ]"
plugins: ["obstacle_layer", "inflation_layer"]
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
inflation_radius: 0.01
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: 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: 2.0
raytrace_min_range: 0.0
obstacle_max_range: 1.0
obstacle_min_range: 0.0
always_send_full_costmap: 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", "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: 1.0
use_sim_time: true
simulate_ahead_time: 2.0
max_rotational_vel: 1.0
min_rotational_vel: 0.4
rotational_acc_lim: 3.2
velocity_smoother:
ros__parameters:
use_sim_time: True
smoothing_frequency: 10.0
scale_velocities: False
feedback: "OPEN_LOOP"
max_velocity: [0.3, 0.0, 0.3]
max_accel: [0.4, 0.0, 0.4]
max_decel: [-0.5, 0.0, -0.5]
odom_topic: "/odom_custom"
odom_duration: 0.1
deadband_velocity: [0.0, 0.0, 0.0]
velocity_timeout: 1.0
src/control_pkg/config/control_2d_sim_fr09.yaml
+119
View File
@@ -0,0 +1,119 @@
# File: fr09/control_2d.yaml
# Description: Control parameters for FR-09 (2D LiDAR)
controller_server:
ros__parameters:
use_sim_time: True
controller_frequency: 10.0
min_x_velocity_threshold: 0.001
min_y_velocity_threshold: 0.0
min_theta_velocity_threshold: 0.001
failure_tolerance: 1.0
progress_checker_plugin: "progress_checker"
goal_checker_plugins: ["general_goal_checker"]
controller_plugins: ["FollowPath"]
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.2
movement_time_allowance: 20.0
general_goal_checker:
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.5
yaw_goal_tolerance: 2.0
stateful: True
FollowPath:
plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
allow_reversing: false
use_dynamic_window: false
use_velocity_scaled_lookahead_dist: false
use_rotate_to_heading: false
use_collision_detection: true
use_regulated_linear_velocity_scaling: true
use_cost_regulated_linear_velocity_scaling: true
desired_linear_vel: 0.5
min_approach_linear_velocity: 0.05
lookahead_dist: 1.5
cost_scaling_dist: 1.0
cost_scaling_gain: 2.0
approach_velocity_scaling_dist: 2.0
regulated_linear_scaling_min_radius: 1.71
regulated_linear_scaling_min_speed: 0.3
max_allowed_time_to_collision_up_to_carrot: 2.0
local_costmap:
local_costmap:
ros__parameters:
update_frequency: 10.0
publish_frequency: 5.0
global_frame: odom
robot_base_frame: base_link
use_sim_time: True
rolling_window: true
width: 4
height: 4
resolution: 0.05
footprint: "[ [0.79, 0.37], [0.79, -0.37], [-0.79, -0.37], [-0.79, 0.37] ]"
plugins: ["obstacle_layer", "inflation_layer"]
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 2.0
inflation_radius: 0.7
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: 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: 4.5
raytrace_min_range: 0.0
obstacle_max_range: 3.5
obstacle_min_range: 0.0
always_send_full_costmap: 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", "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: 1.0
use_sim_time: true
simulate_ahead_time: 2.0
max_rotational_vel: 1.0
min_rotational_vel: 0.4
rotational_acc_lim: 3.2
velocity_smoother:
ros__parameters:
use_sim_time: True
smoothing_frequency: 10.0
scale_velocities: False
feedback: "OPEN_LOOP"
max_velocity: [0.5, 0.0, 0.5]
max_accel: [0.5, 0.0, 0.5]
max_decel: [-1.5, 0.0, -1.0]
odom_topic: "/odom_custom"
odom_duration: 0.1
deadband_velocity: [0.0, 0.0, 0.0]
velocity_timeout: 1.0
src/control_pkg/config/control_2d_sim_mkmini.yaml
+119
View File
@@ -0,0 +1,119 @@
# File: mkmini/control_2d.yaml
# Description: Control parameters for MK-Mini (2D LiDAR)
controller_server:
ros__parameters:
use_sim_time: True
controller_frequency: 10.0
min_x_velocity_threshold: 0.001
min_y_velocity_threshold: 0.0
min_theta_velocity_threshold: 0.001
failure_tolerance: 1.0
progress_checker_plugin: "progress_checker"
goal_checker_plugins: ["general_goal_checker"]
controller_plugins: ["FollowPath"]
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.2
movement_time_allowance: 20.0
general_goal_checker:
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.5
yaw_goal_tolerance: 2.0
stateful: True
FollowPath:
plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
allow_reversing: false
use_dynamic_window: false
use_velocity_scaled_lookahead_dist: false
use_rotate_to_heading: false
use_collision_detection: true
use_regulated_linear_velocity_scaling: true
use_cost_regulated_linear_velocity_scaling: true
desired_linear_vel: 0.5
min_approach_linear_velocity: 0.3
lookahead_dist: 1.0
cost_scaling_dist: 0.6
cost_scaling_gain: 1.0
approach_velocity_scaling_dist: 1.5
regulated_linear_scaling_min_radius: 1.1
regulated_linear_scaling_min_speed: 0.3
max_allowed_time_to_collision_up_to_carrot: 2.0
local_costmap:
local_costmap:
ros__parameters:
update_frequency: 10.0
publish_frequency: 5.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.42, 0.305], [0.42, -0.305], [-0.42, -0.305], [-0.42, 0.305] ]"
plugins: ["obstacle_layer", "inflation_layer"]
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 4.0
inflation_radius: 0.5
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: 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: 4.0
raytrace_min_range: 0.0
obstacle_max_range: 3.0
obstacle_min_range: 0.0
always_send_full_costmap: 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", "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: 1.0
use_sim_time: true
simulate_ahead_time: 2.0
max_rotational_vel: 1.0
min_rotational_vel: 0.4
rotational_acc_lim: 3.2
velocity_smoother:
ros__parameters:
use_sim_time: True
smoothing_frequency: 10.0
scale_velocities: False
feedback: "OPEN_LOOP"
max_velocity: [0.5, 0.0, 0.5]
max_accel: [0.5, 0.0, 0.5]
max_decel: [-0.5, 0.0, -0.5]
odom_topic: "/odom_custom"
odom_duration: 0.1
deadband_velocity: [0.0, 0.0, 0.0]
velocity_timeout: 1.0
src/control_pkg/config/control_2d_sim_turtlebot3.yaml
+113
View File
@@ -0,0 +1,113 @@
# TurtleBot3 Burger Gazebo control profile. Planning owns the global costmap
# and BT navigator; this module owns the local controller and /cmd_vel output.
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
failure_tolerance: 0.5
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
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
behavior_server:
ros__parameters:
use_sim_time: true
costmap_topic: local_costmap/costmap_raw
footprint_topic: local_costmap/published_footprint
cycle_frequency: 10.0
behavior_plugins: [backup, drive_on_heading, wait]
backup:
plugin: nav2_behaviors/BackUp
drive_on_heading:
plugin: nav2_behaviors/DriveOnHeading
wait:
plugin: nav2_behaviors/Wait
global_frame: odom
robot_base_frame: base_footprint
transform_tolerance: 0.2
velocity_smoother:
ros__parameters:
use_sim_time: true
smoothing_frequency: 20.0
scale_velocities: false
feedback: OPEN_LOOP
max_velocity: [0.22, 0.0, 2.5]
min_velocity: [-0.05, 0.0, -2.5]
max_accel: [1.5, 0.0, 3.2]
max_decel: [-1.5, 0.0, -3.2]
odom_topic: /odom
odom_duration: 0.1
deadband_velocity: [0.0, 0.0, 0.0]
velocity_timeout: 1.0
src/control_pkg/config/control_3d_fr09.yaml
+154
View File
@@ -0,0 +1,154 @@
controller_server:
ros__parameters:
use_sim_time: True
controller_frequency: 8.0
min_x_velocity_threshold: 0.001
# min_x_velocity_threshold: 0.01
min_y_velocity_threshold: 0.0
min_theta_velocity_threshold: 0.001
failure_tolerance: 1.0
progress_checker_plugin: "progress_checker"
goal_checker_plugins: ["general_goal_checker"]
controller_plugins: ["FollowPath"]
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.2
movement_time_allowance: 15.0
general_goal_checker:
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 1.0
yaw_goal_tolerance: 2.0
stateful: True
FollowPath:
plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
use_dynamic_window: false
use_velocity_scaled_lookahead_dist: true
use_rotate_to_heading: false
use_collision_detection: false
use_regulated_linear_velocity_scaling: true
use_cost_regulated_linear_velocity_scaling: false
use_fixed_curvature_lookahead: false
allow_reversing: True
desired_linear_vel: 1.5
min_approach_linear_velocity: 0.15
lookahead_dist: 3.0
min_lookahead_dist: 1.0
max_lookahead_dist: 4.0
lookahead_time: 2.0
curvature_lookahead_dist: 1.5
approach_velocity_scaling_dist: 1.2
max_allowed_time_to_collision_up_to_carrot: 2.0
regulated_linear_scaling_min_radius: 1.1
regulated_linear_scaling_min_speed: 0.35
transform_tolerance: 1.2
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: 4
height: 4
resolution: 0.05
footprint: "[[1.220, 0.3715], [1.220, -0.3715], [-0.369, -0.3715], [-0.369, 0.3715]]"
plugins: ["static_layer", "voxel_layer", "inflation_layer"]
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 4.0
inflation_radius: 0.5
# obstacle_layer:
# plugin: "nav2_costmap_2d::ObstacleLayer"
# enabled: 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: 2.5
# raytrace_min_range: 0.0
# obstacle_max_range: 2.0
# obstacle_min_range: 0.0
voxel_layer:
plugin: "nav2_costmap_2d::VoxelLayer"
enabled: True
publish_voxel_map: True
origin_z: 0.0
z_resolution: 0.2
z_voxels: 16
max_obstacle_height: 3.0
mark_threshold: 0
observation_sources: lidar3d
lidar3d:
topic: /lidar_filtered
data_type: "PointCloud2"
marking: True
clearing: True
min_obstacle_height: 0.1
max_obstacle_height: 2.5
obstacle_min_range: 0.0
obstacle_max_range: 12.0
raytrace_min_range: 0.0
raytrace_max_range: 12.0
static_layer:
plugin: "nav2_costmap_2d::StaticLayer"
map_subscribe_transient_local: True
always_send_full_costmap: 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", "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: 1.0
use_sim_time: true
simulate_ahead_time: 2.0
max_rotational_vel: 1.0
min_rotational_vel: 0.4
rotational_acc_lim: 3.2
velocity_smoother:
ros__parameters:
use_sim_time: True
smoothing_frequency: 10.0
scale_velocities: False
feedback: "OPEN_LOOP"
max_velocity: [1.5, 0.0, 1.0]
min_velocity: [-1.0, 0.0, -1.0]
max_accel: [2.5, 0.0, 3.2]
max_decel: [-2.5, 0.0, -3.2]
odom_topic: "/wheel_odom"
odom_duration: 0.1
deadband_velocity: [0.0, 0.0, 0.0]
velocity_timeout: 1.0
src/control_pkg/config/control_3d_real_mkmini.yaml
+160
View File
@@ -0,0 +1,160 @@
controller_server:
ros__parameters:
use_sim_time: True
controller_frequency: 10.0
min_x_velocity_threshold: 0.001
# min_x_velocity_threshold: 0.01
min_y_velocity_threshold: 0.0
min_theta_velocity_threshold: 0.001
failure_tolerance: 1.0
progress_checker_plugin: "progress_checker"
goal_checker_plugins: ["general_goal_checker"]
controller_plugins: ["FollowPath"]
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.2
movement_time_allowance: 15.0
general_goal_checker:
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.8
yaw_goal_tolerance: 1.8
stateful: True
FollowPath:
plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
# use_dynamic_window: false
# use_velocity_scaled_lookahead_dist: true
# use_rotate_to_heading: false
# use_collision_detection: false
# use_regulated_linear_velocity_scaling: true
# use_cost_regulated_linear_velocity_scaling: false
# use_fixed_curvature_lookahead: false
# allow_reversing: True
# desired_linear_vel: 0.5
# min_approach_linear_velocity: 0.15
# lookahead_dist: 2.0
# min_lookahead_dist: 1.0
# max_lookahead_dist: 2.0
# lookahead_time: 2.0
# curvature_lookahead_dist: 1.0
# approach_velocity_scaling_dist: 1.2
# max_allowed_time_to_collision_up_to_carrot: 2.0
# regulated_linear_scaling_min_radius: 1.1
# regulated_linear_scaling_min_speed: 0.35
allow_reversing: true
use_dynamic_window: false
use_velocity_scaled_lookahead_dist: false
use_rotate_to_heading: false
use_collision_detection: true
use_regulated_linear_velocity_scaling: true
use_cost_regulated_linear_velocity_scaling: false
desired_linear_vel: 0.2
min_approach_linear_velocity: 0.15
# lookahead_dist: 1.2
lookahead_dist: 0.5
cost_scaling_dist: 0.8
cost_scaling_gain: 1.0
approach_velocity_scaling_dist: 2.0
regulated_linear_scaling_min_radius: 1.1
regulated_linear_scaling_min_speed: 0.3
max_allowed_time_to_collision_up_to_carrot: 2.0
transform_tolerance: 1.2
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: 2
height: 2
resolution: 0.05
footprint: "[ [0.35, 0.28], [0.35, -0.28], [-0.35, -0.28], [-0.35, 0.28] ]"
plugins: ["static_layer", "voxel_layer", "inflation_layer"]
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
inflation_radius: 0.01
voxel_layer:
plugin: "nav2_costmap_2d::VoxelLayer"
enabled: True
publish_voxel_map: True
origin_z: 0.0
z_resolution: 0.2
z_voxels: 16
max_obstacle_height: 3.0
mark_threshold: 0
observation_sources: lidar3d
lidar3d:
topic: /carla/hero/lidar_filtered
data_type: "PointCloud2"
marking: True
clearing: True
min_obstacle_height: 0.1
max_obstacle_height: 2.5
obstacle_min_range: 0.0
obstacle_max_range: 12.0
raytrace_min_range: 0.0
raytrace_max_range: 12.0
static_layer:
plugin: "nav2_costmap_2d::StaticLayer"
map_subscribe_transient_local: True
always_send_full_costmap: 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", "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: 1.0
use_sim_time: true
simulate_ahead_time: 2.0
max_rotational_vel: 1.0
min_rotational_vel: 0.4
rotational_acc_lim: 3.2
velocity_smoother:
ros__parameters:
use_sim_time: True
smoothing_frequency: 10.0
scale_velocities: False
feedback: "OPEN_LOOP"
# max_velocity: [0.5, 0.0, 0.5]
min_velocity: [-0.3, 0.0, -0.3]
# max_accel: [2.5, 0.0, 3.2]
# max_decel: [-2.5, 0.0, -3.2]
max_velocity: [0.3, 0.0, 0.3]
max_accel: [0.4, 0.0, 0.4]
max_decel: [-0.5, 0.0, -0.5]
odom_topic: "/odom_custom"
odom_duration: 0.1
deadband_velocity: [0.0, 0.0, 0.0]
velocity_timeout: 1.0
src/control_pkg/config/control_3d_sim_fr09.yaml
+137
View File
@@ -0,0 +1,137 @@
controller_server:
ros__parameters:
use_sim_time: True
controller_frequency: 8.0
min_x_velocity_threshold: 0.001
# min_x_velocity_threshold: 0.01
min_y_velocity_threshold: 0.0
min_theta_velocity_threshold: 0.001
failure_tolerance: 1.0
progress_checker_plugin: "progress_checker"
goal_checker_plugins: ["general_goal_checker"]
controller_plugins: ["FollowPath"]
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.2
movement_time_allowance: 15.0
general_goal_checker:
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 1.0
yaw_goal_tolerance: 2.0
stateful: True
FollowPath:
plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
use_dynamic_window: false
use_velocity_scaled_lookahead_dist: true
use_rotate_to_heading: false
use_collision_detection: false
use_regulated_linear_velocity_scaling: true
use_cost_regulated_linear_velocity_scaling: false
use_fixed_curvature_lookahead: false
allow_reversing: True
# desired_linear_vel: 1.5
desired_linear_vel: 0.5
min_approach_linear_velocity: 0.15
# lookahead_dist: 3.0
lookahead_dist: 1.0
min_lookahead_dist: 1.0
max_lookahead_dist: 4.0
lookahead_time: 2.0
curvature_lookahead_dist: 1.5
approach_velocity_scaling_dist: 1.2
max_allowed_time_to_collision_up_to_carrot: 2.0
regulated_linear_scaling_min_radius: 1.1
regulated_linear_scaling_min_speed: 0.35
transform_tolerance: 1.2
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: 4
height: 4
resolution: 0.05
footprint: "[[1.220, 0.3715], [1.220, -0.3715], [-0.369, -0.3715], [-0.369, 0.3715]]"
plugins: ["static_layer", "voxel_layer", "inflation_layer"]
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 4.0
inflation_radius: 0.5
voxel_layer:
plugin: "nav2_costmap_2d::VoxelLayer"
enabled: True
publish_voxel_map: True
origin_z: 0.0
z_resolution: 0.2
z_voxels: 16
max_obstacle_height: 3.0
mark_threshold: 0
observation_sources: lidar3d
lidar3d:
topic: /lidar_filtered
data_type: "PointCloud2"
marking: True
clearing: True
min_obstacle_height: 0.1
max_obstacle_height: 2.5
obstacle_min_range: 0.0
obstacle_max_range: 12.0
raytrace_min_range: 0.0
raytrace_max_range: 12.0
static_layer:
plugin: "nav2_costmap_2d::StaticLayer"
map_subscribe_transient_local: True
always_send_full_costmap: 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", "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: 1.0
use_sim_time: true
simulate_ahead_time: 2.0
max_rotational_vel: 1.0
min_rotational_vel: 0.4
rotational_acc_lim: 3.2
velocity_smoother:
ros__parameters:
use_sim_time: True
smoothing_frequency: 10.0
scale_velocities: False
feedback: "OPEN_LOOP"
max_velocity: [1.5, 0.0, 1.0]
min_velocity: [-1.0, 0.0, -1.0]
max_accel: [2.5, 0.0, 3.2]
max_decel: [-2.5, 0.0, -3.2]
odom_topic: "/wheel_odom"
odom_duration: 0.1
deadband_velocity: [0.0, 0.0, 0.0]
velocity_timeout: 1.0
src/control_pkg/config/control_3d_sim_mkmini.yaml
+154
View File
@@ -0,0 +1,154 @@
controller_server:
ros__parameters:
use_sim_time: True
controller_frequency: 10.0
min_x_velocity_threshold: 0.001
# min_x_velocity_threshold: 0.01
min_y_velocity_threshold: 0.0
min_theta_velocity_threshold: 0.001
failure_tolerance: 1.0
progress_checker_plugin: "progress_checker"
goal_checker_plugins: ["general_goal_checker"]
controller_plugins: ["FollowPath"]
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.2
movement_time_allowance: 15.0
general_goal_checker:
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 1.0
yaw_goal_tolerance: 2.0
stateful: True
FollowPath:
plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
use_dynamic_window: false
use_velocity_scaled_lookahead_dist: true
use_rotate_to_heading: false
use_collision_detection: false
use_regulated_linear_velocity_scaling: true
use_cost_regulated_linear_velocity_scaling: false
use_fixed_curvature_lookahead: false
allow_reversing: True
desired_linear_vel: 0.5
min_approach_linear_velocity: 0.15
lookahead_dist: 2.0
min_lookahead_dist: 1.0
max_lookahead_dist: 2.0
lookahead_time: 2.0
curvature_lookahead_dist: 1.0
approach_velocity_scaling_dist: 1.2
max_allowed_time_to_collision_up_to_carrot: 2.0
regulated_linear_scaling_min_radius: 1.1
regulated_linear_scaling_min_speed: 0.35
transform_tolerance: 1.2
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: 2
height: 2
resolution: 0.05
footprint: "[ [0.42, 0.305], [0.42, -0.305], [-0.42, -0.305], [-0.42, 0.305] ]"
plugins: ["static_layer", "voxel_layer", "inflation_layer"]
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 4.0
inflation_radius: 0.5
# obstacle_layer:
# plugin: "nav2_costmap_2d::ObstacleLayer"
# enabled: 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: 2.5
# raytrace_min_range: 0.0
# obstacle_max_range: 2.0
# obstacle_min_range: 0.0
voxel_layer:
plugin: "nav2_costmap_2d::VoxelLayer"
enabled: True
publish_voxel_map: True
origin_z: 0.0
z_resolution: 0.2
z_voxels: 16
max_obstacle_height: 3.0
mark_threshold: 0
observation_sources: lidar3d
lidar3d:
topic: /carla/hero/lidar_filtered
data_type: "PointCloud2"
marking: True
clearing: True
min_obstacle_height: 0.1
max_obstacle_height: 2.5
obstacle_min_range: 0.0
obstacle_max_range: 12.0
raytrace_min_range: 0.0
raytrace_max_range: 12.0
static_layer:
plugin: "nav2_costmap_2d::StaticLayer"
map_subscribe_transient_local: True
always_send_full_costmap: 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", "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: 1.0
use_sim_time: true
simulate_ahead_time: 2.0
max_rotational_vel: 1.0
min_rotational_vel: 0.4
rotational_acc_lim: 3.2
velocity_smoother:
ros__parameters:
use_sim_time: True
smoothing_frequency: 10.0
scale_velocities: False
feedback: "OPEN_LOOP"
max_velocity: [0.5, 0.0, 0.5]
min_velocity: [-1.0, 0.0, -1.0]
max_accel: [2.5, 0.0, 3.2]
max_decel: [-2.5, 0.0, -3.2]
odom_topic: "/odom_custom"
odom_duration: 0.1
deadband_velocity: [0.0, 0.0, 0.0]
velocity_timeout: 1.0
src/control_pkg/control_pkg/__init__.py
View File
src/control_pkg/control_pkg/control_manager.py
+57
View File
@@ -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 ControlManager(Node):
def __init__(self):
super().__init__('control_manager')
self.get_logger().info('Control 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"Control Received config: {msg.data}")
config_data = json.loads(msg.data)
self.planning_process = subprocess.Popen(['ros2', 'launch',
'control_pkg',
'nav2_control.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 = ControlManager()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
src/control_pkg/control_pkg/waypoint_sender.py
+91
View File
@@ -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()
src/control_pkg/launch/control.launch.py
+15
View File
@@ -0,0 +1,15 @@
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
node = Node(
package='control_pkg',
executable='control_manager',
name='control_manager',
output='screen',
)
ld = LaunchDescription([])
ld.add_action(node)
return ld
src/control_pkg/launch/nav2_control.launch.py
+104
View File
@@ -0,0 +1,104 @@
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='turtlebot3', choices=['mkmini', 'fr09', 'turtlebot3']),
DeclareLaunchArgument('level', default_value='2d'),
]
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',
'behavior_server',
'velocity_smoother']
# Directories
pkg_nav2 = get_package_share_directory(
'control_pkg')
mode = 'sim' if sim_mode == 'true' else 'real'
params_file = PathJoinSubstitution([
pkg_nav2,
'config',
PythonExpression(["'control_", level, "_", mode, "_", robot, ".yaml'"])])
configured_params = ParameterFile(
RewrittenYaml(
source_file=params_file,
param_rewrites={'use_sim_time': sim_mode},
convert_types=True),
allow_substs=True)
load_nodes = GroupAction(
actions=[
# PushRosNamespace(namespace),
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],
remappings=[('/cmd_vel', 'cmd_vel_nav')]
),
Node(
package='nav2_velocity_smoother',
executable='velocity_smoother',
name='velocity_smoother',
output='screen',
respawn=False,
respawn_delay=2.0,
parameters=[configured_params],
arguments=['--ros-args', '--log-level', log_level],
remappings=[('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]
),
Node(
package='nav2_behaviors',
executable='behavior_server',
name='behavior_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_control',
output='screen',
arguments=['--ros-args', '--log-level', log_level],
parameters=[{'use_sim_time': sim_mode == '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
src/control_pkg/package.xml
+24
View File
@@ -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>control_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_controller</exec_depend>
<exec_depend>nav2_behaviors</exec_depend>
<exec_depend>nav2_velocity_smoother</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>
src/control_pkg/resource/control_pkg
View File
src/control_pkg/setup.cfg
+4
View File
@@ -0,0 +1,4 @@
[develop]
script_dir=$base/lib/control_pkg
[install]
install_scripts=$base/lib/control_pkg
src/control_pkg/setup.py
+30
View File
@@ -0,0 +1,30 @@
import os
from glob import glob
from setuptools import find_packages, setup
package_name = 'control_pkg'
setup(
name=package_name,
version='0.0.1',
packages=find_packages(exclude=['test']),
data_files=[
('share/ament_index/resource_index/packages', ['resource/control_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')),
],
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 = control_pkg.waypoint_sender:main',
'control_manager = control_pkg.control_manager:main',
],
},
)