From f3e91c5bc96bcbe218b246000eec02a9554c04b4 Mon Sep 17 00:00:00 2001 From: chyoo Date: Wed, 29 Jul 2026 11:26:20 +0900 Subject: [PATCH] feat: add NAV2 Gazebo control module --- .gitignore | 6 + build.sh | 6 + run.sh | 10 ++ .../config/control_2d_real_mkmini.yaml | 119 +++++++++++++ .../config/control_2d_sim_fr09.yaml | 119 +++++++++++++ .../config/control_2d_sim_mkmini.yaml | 119 +++++++++++++ .../config/control_2d_sim_turtlebot3.yaml | 113 +++++++++++++ src/control_pkg/config/control_3d_fr09.yaml | 154 +++++++++++++++++ .../config/control_3d_real_mkmini.yaml | 160 ++++++++++++++++++ .../config/control_3d_sim_fr09.yaml | 137 +++++++++++++++ .../config/control_3d_sim_mkmini.yaml | 154 +++++++++++++++++ src/control_pkg/control_pkg/__init__.py | 0 .../control_pkg/control_manager.py | 57 +++++++ .../control_pkg/waypoint_sender.py | 91 ++++++++++ src/control_pkg/launch/control.launch.py | 15 ++ src/control_pkg/launch/nav2_control.launch.py | 104 ++++++++++++ src/control_pkg/package.xml | 24 +++ src/control_pkg/resource/control_pkg | 0 src/control_pkg/setup.cfg | 4 + src/control_pkg/setup.py | 30 ++++ 20 files changed, 1422 insertions(+) create mode 100644 .gitignore create mode 100755 build.sh create mode 100755 run.sh create mode 100644 src/control_pkg/config/control_2d_real_mkmini.yaml create mode 100644 src/control_pkg/config/control_2d_sim_fr09.yaml create mode 100644 src/control_pkg/config/control_2d_sim_mkmini.yaml create mode 100644 src/control_pkg/config/control_2d_sim_turtlebot3.yaml create mode 100644 src/control_pkg/config/control_3d_fr09.yaml create mode 100644 src/control_pkg/config/control_3d_real_mkmini.yaml create mode 100644 src/control_pkg/config/control_3d_sim_fr09.yaml create mode 100644 src/control_pkg/config/control_3d_sim_mkmini.yaml create mode 100644 src/control_pkg/control_pkg/__init__.py create mode 100644 src/control_pkg/control_pkg/control_manager.py create mode 100644 src/control_pkg/control_pkg/waypoint_sender.py create mode 100644 src/control_pkg/launch/control.launch.py create mode 100644 src/control_pkg/launch/nav2_control.launch.py create mode 100644 src/control_pkg/package.xml create mode 100644 src/control_pkg/resource/control_pkg create mode 100644 src/control_pkg/setup.cfg create mode 100644 src/control_pkg/setup.py diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..fa310fd --- /dev/null +++ b/.gitignore @@ -0,0 +1,6 @@ +build/ +install/ +log/ +__pycache__/ +**/__pycache__/ +*.py[cod] diff --git a/build.sh b/build.sh new file mode 100755 index 0000000..0c1c5bc --- /dev/null +++ b/build.sh @@ -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}" diff --git a/run.sh b/run.sh new file mode 100755 index 0000000..9726985 --- /dev/null +++ b/run.sh @@ -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[@]}" "$@" diff --git a/src/control_pkg/config/control_2d_real_mkmini.yaml b/src/control_pkg/config/control_2d_real_mkmini.yaml new file mode 100644 index 0000000..63fa5a2 --- /dev/null +++ b/src/control_pkg/config/control_2d_real_mkmini.yaml @@ -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 diff --git a/src/control_pkg/config/control_2d_sim_fr09.yaml b/src/control_pkg/config/control_2d_sim_fr09.yaml new file mode 100644 index 0000000..8a4492c --- /dev/null +++ b/src/control_pkg/config/control_2d_sim_fr09.yaml @@ -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 diff --git a/src/control_pkg/config/control_2d_sim_mkmini.yaml b/src/control_pkg/config/control_2d_sim_mkmini.yaml new file mode 100644 index 0000000..7b48d0f --- /dev/null +++ b/src/control_pkg/config/control_2d_sim_mkmini.yaml @@ -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 diff --git a/src/control_pkg/config/control_2d_sim_turtlebot3.yaml b/src/control_pkg/config/control_2d_sim_turtlebot3.yaml new file mode 100644 index 0000000..9760e3b --- /dev/null +++ b/src/control_pkg/config/control_2d_sim_turtlebot3.yaml @@ -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 diff --git a/src/control_pkg/config/control_3d_fr09.yaml b/src/control_pkg/config/control_3d_fr09.yaml new file mode 100644 index 0000000..5a6120e --- /dev/null +++ b/src/control_pkg/config/control_3d_fr09.yaml @@ -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 diff --git a/src/control_pkg/config/control_3d_real_mkmini.yaml b/src/control_pkg/config/control_3d_real_mkmini.yaml new file mode 100644 index 0000000..ced04d6 --- /dev/null +++ b/src/control_pkg/config/control_3d_real_mkmini.yaml @@ -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 diff --git a/src/control_pkg/config/control_3d_sim_fr09.yaml b/src/control_pkg/config/control_3d_sim_fr09.yaml new file mode 100644 index 0000000..9a95ed7 --- /dev/null +++ b/src/control_pkg/config/control_3d_sim_fr09.yaml @@ -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 diff --git a/src/control_pkg/config/control_3d_sim_mkmini.yaml b/src/control_pkg/config/control_3d_sim_mkmini.yaml new file mode 100644 index 0000000..5a8c8a6 --- /dev/null +++ b/src/control_pkg/config/control_3d_sim_mkmini.yaml @@ -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 diff --git a/src/control_pkg/control_pkg/__init__.py b/src/control_pkg/control_pkg/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/control_pkg/control_pkg/control_manager.py b/src/control_pkg/control_pkg/control_manager.py new file mode 100644 index 0000000..1e0996a --- /dev/null +++ b/src/control_pkg/control_pkg/control_manager.py @@ -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() + \ No newline at end of file diff --git a/src/control_pkg/control_pkg/waypoint_sender.py b/src/control_pkg/control_pkg/waypoint_sender.py new file mode 100644 index 0000000..bc0104e --- /dev/null +++ b/src/control_pkg/control_pkg/waypoint_sender.py @@ -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() diff --git a/src/control_pkg/launch/control.launch.py b/src/control_pkg/launch/control.launch.py new file mode 100644 index 0000000..2c5c270 --- /dev/null +++ b/src/control_pkg/launch/control.launch.py @@ -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 diff --git a/src/control_pkg/launch/nav2_control.launch.py b/src/control_pkg/launch/nav2_control.launch.py new file mode 100644 index 0000000..bfd727f --- /dev/null +++ b/src/control_pkg/launch/nav2_control.launch.py @@ -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 diff --git a/src/control_pkg/package.xml b/src/control_pkg/package.xml new file mode 100644 index 0000000..db11e62 --- /dev/null +++ b/src/control_pkg/package.xml @@ -0,0 +1,24 @@ + + + + control_pkg + 0.0.1 + Modular Nav2 launch package for FR-09/MK-Mini autonomous driving + FutureDrive + Apache-2.0 + + nav2_controller + nav2_behaviors + nav2_velocity_smoother + nav2_lifecycle_manager + nav2_waypoint_follower + + ament_python + ament_copyright + ament_flake8 + ament_pep257 + + + ament_python + + diff --git a/src/control_pkg/resource/control_pkg b/src/control_pkg/resource/control_pkg new file mode 100644 index 0000000..e69de29 diff --git a/src/control_pkg/setup.cfg b/src/control_pkg/setup.cfg new file mode 100644 index 0000000..8d37cd6 --- /dev/null +++ b/src/control_pkg/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/control_pkg +[install] +install_scripts=$base/lib/control_pkg diff --git a/src/control_pkg/setup.py b/src/control_pkg/setup.py new file mode 100644 index 0000000..1f57f3f --- /dev/null +++ b/src/control_pkg/setup.py @@ -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', + ], + }, +)