feat: add NAV2 Gazebo control module
This commit is contained in:
@@ -0,0 +1,6 @@
|
|||||||
|
build/
|
||||||
|
install/
|
||||||
|
log/
|
||||||
|
__pycache__/
|
||||||
|
**/__pycache__/
|
||||||
|
*.py[cod]
|
||||||
@@ -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,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[@]}" "$@"
|
||||||
@@ -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
|
||||||
@@ -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
|
||||||
@@ -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
|
||||||
@@ -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
|
||||||
@@ -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
|
||||||
@@ -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
|
||||||
@@ -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
|
||||||
@@ -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
|
||||||
@@ -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()
|
||||||
|
|
||||||
@@ -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()
|
||||||
@@ -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
|
||||||
@@ -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
|
||||||
@@ -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>
|
||||||
@@ -0,0 +1,4 @@
|
|||||||
|
[develop]
|
||||||
|
script_dir=$base/lib/control_pkg
|
||||||
|
[install]
|
||||||
|
install_scripts=$base/lib/control_pkg
|
||||||
@@ -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',
|
||||||
|
],
|
||||||
|
},
|
||||||
|
)
|
||||||
Reference in New Issue
Block a user