commit 5bd6aac99cb1d4644229ace8c46c3c3b5149d058 Author: 유창훈 Date: Tue Jul 28 10:35:43 2026 +0900 feat: add lms-nav2-gazebo-planning module diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..76b3365 --- /dev/null +++ b/.gitignore @@ -0,0 +1,5 @@ +build/ +install/ +log/ +.colcon/ +*.log 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..656b11d --- /dev/null +++ b/run.sh @@ -0,0 +1,8 @@ +#!/usr/bin/env bash +set -euo pipefail + +MODULE_PATH="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +RUNTIME_ROOT="${AUTONOMY_STUDIO_RUNTIME_ROOT:-$HOME/autonomy-studio/runtime/nav2}" +FOLDER_NAME="$(basename "$MODULE_PATH")" +CONTAINER_NAME="${FOLDER_NAME#\[}"; CONTAINER_NAME="${CONTAINER_NAME/\]/-}" +exec bash "$RUNTIME_ROOT/docker/docker_run_nav2.sh" "$CONTAINER_NAME" "$(dirname "$MODULE_PATH")" "$FOLDER_NAME" "${NAV2_DOCKER_IMAGE:-future-nav2-gazebo:humble}" "${ROS_DOMAIN_ID:-0}" "$@" diff --git a/src/planning_pkg/behavior/navigate_through_poses_no_spin.xml b/src/planning_pkg/behavior/navigate_through_poses_no_spin.xml new file mode 100755 index 0000000..44dd933 --- /dev/null +++ b/src/planning_pkg/behavior/navigate_through_poses_no_spin.xml @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/planning_pkg/behavior/navigate_to_pose_no_spin.xml b/src/planning_pkg/behavior/navigate_to_pose_no_spin.xml new file mode 100755 index 0000000..3537e32 --- /dev/null +++ b/src/planning_pkg/behavior/navigate_to_pose_no_spin.xml @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/planning_pkg/config/planning_2d_real_mkmini.yaml b/src/planning_pkg/config/planning_2d_real_mkmini.yaml new file mode 100755 index 0000000..3ee7501 --- /dev/null +++ b/src/planning_pkg/config/planning_2d_real_mkmini.yaml @@ -0,0 +1,153 @@ +# File: real/mkmini/planning_2d.yaml +# Description: Planning parameters for MK-Mini (2D LiDAR) — real vehicle + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom_custom + bt_loop_duration: 20 + default_server_timeout: 2000 + wait_for_service_timeout: 4000 + default_nav_to_pose_bt_xml: "" + default_nav_through_poses_bt_xml: "" + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 2.0 + publish_frequency: 2.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + footprint: "[ [0.35, 0.28], [0.35, -0.28], [-0.35, -0.28], [-0.35, 0.28] ]" + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 2.0 + raytrace_min_range: 0.0 + obstacle_max_range: 1.0 + obstacle_min_range: 0.0 + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.5 + always_send_full_costmap: True + +planner_server: + ros__parameters: + expected_planner_frequency: 2.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_smac_planner/SmacPlannerHybrid" + tolerance: 0.5 + allow_unknown: true + downsample_costmap: true + downsampling_factor: 2 + max_iterations: 3000000 + max_on_approach_iterations: 5000 + max_planning_time: 10.0 + motion_model_for_search: "DUBIN" + angle_quantization_bins: 144 + analytic_expansion_ratio: 3.5 + analytic_expansion_max_length: 5.0 + minimum_turning_radius: 1.1 + change_penalty: 0.0 + non_straight_penalty: 1.5 + cost_penalty: 5.0 + retrospective_penalty: 0.1 + lookup_table_size: 20.0 + cache_obstacle_heuristic: true + smooth_path: true + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 diff --git a/src/planning_pkg/config/planning_2d_sim_fr09.yaml b/src/planning_pkg/config/planning_2d_sim_fr09.yaml new file mode 100755 index 0000000..3dc18da --- /dev/null +++ b/src/planning_pkg/config/planning_2d_sim_fr09.yaml @@ -0,0 +1,153 @@ +# File: fr09/planning_2d.yaml +# Description: Planning parameters for FR-09 (2D LiDAR) + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom_custom + bt_loop_duration: 20 + default_server_timeout: 2000 + wait_for_service_timeout: 4000 + default_nav_to_pose_bt_xml: "" + default_nav_through_poses_bt_xml: "" + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 2.0 + publish_frequency: 2.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + footprint: "[ [0.79, 0.37], [0.79, -0.37], [-0.79, -0.37], [-0.79, 0.37] ]" + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 10.0 + raytrace_min_range: 0.0 + obstacle_max_range: 8.0 + obstacle_min_range: 0.0 + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 2.0 + inflation_radius: 1.5 + always_send_full_costmap: True + +planner_server: + ros__parameters: + expected_planner_frequency: 2.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_smac_planner/SmacPlannerHybrid" + tolerance: 0.5 + allow_unknown: true + downsample_costmap: true + downsampling_factor: 2 + max_iterations: 3000000 + max_on_approach_iterations: 5000 + max_planning_time: 10.0 + motion_model_for_search: "DUBIN" + angle_quantization_bins: 144 + analytic_expansion_ratio: 3.5 + analytic_expansion_max_length: 5.0 + minimum_turning_radius: 1.71 + change_penalty: 0.0 + non_straight_penalty: 1.5 + cost_penalty: 5.0 + retrospective_penalty: 0.1 + lookup_table_size: 20.0 + cache_obstacle_heuristic: true + smooth_path: true + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 diff --git a/src/planning_pkg/config/planning_2d_sim_mkmini.yaml b/src/planning_pkg/config/planning_2d_sim_mkmini.yaml new file mode 100755 index 0000000..d1d1b9a --- /dev/null +++ b/src/planning_pkg/config/planning_2d_sim_mkmini.yaml @@ -0,0 +1,153 @@ +# File: mkmini/planning_2d.yaml +# Description: Planning parameters for MK-Mini (2D LiDAR) + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom_custom + bt_loop_duration: 20 + default_server_timeout: 2000 + wait_for_service_timeout: 4000 + default_nav_to_pose_bt_xml: "" + default_nav_through_poses_bt_xml: "" + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 2.0 + publish_frequency: 2.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + footprint: "[ [0.42, 0.305], [0.42, -0.305], [-0.42, -0.305], [-0.42, 0.305] ]" + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 8.0 + raytrace_min_range: 0.0 + obstacle_max_range: 6.0 + obstacle_min_range: 0.0 + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 2.0 + inflation_radius: 1.5 + always_send_full_costmap: True + +planner_server: + ros__parameters: + expected_planner_frequency: 2.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_smac_planner/SmacPlannerHybrid" + tolerance: 0.5 + allow_unknown: true + downsample_costmap: true + downsampling_factor: 2 + max_iterations: 3000000 + max_on_approach_iterations: 5000 + max_planning_time: 10.0 + motion_model_for_search: "DUBIN" + angle_quantization_bins: 144 + analytic_expansion_ratio: 3.5 + analytic_expansion_max_length: 5.0 + minimum_turning_radius: 1.1 + change_penalty: 0.0 + non_straight_penalty: 1.5 + cost_penalty: 5.0 + retrospective_penalty: 0.1 + lookup_table_size: 20.0 + cache_obstacle_heuristic: true + smooth_path: true + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 diff --git a/src/planning_pkg/config/planning_2d_sim_turtlebot3.yaml b/src/planning_pkg/config/planning_2d_sim_turtlebot3.yaml new file mode 100644 index 0000000..fd51698 --- /dev/null +++ b/src/planning_pkg/config/planning_2d_sim_turtlebot3.yaml @@ -0,0 +1,151 @@ +# TurtleBot3 Burger Gazebo profile. The simulator provides /scan, /odom, TF, +# and consumes /cmd_vel directly. +bt_navigator: + ros__parameters: + use_sim_time: true + global_frame: map + robot_base_frame: base_footprint + odom_topic: /odom + bt_loop_duration: 20 + default_server_timeout: 2000 + wait_for_service_timeout: 4000 + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: true + +planner_server: + ros__parameters: + use_sim_time: true + expected_planner_frequency: 20.0 + planner_plugins: [GridBased] + GridBased: + plugin: nav2_navfn_planner/NavfnPlanner + tolerance: 0.5 + use_astar: false + allow_unknown: true + +controller_server: + ros__parameters: + use_sim_time: true + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.0 + min_theta_velocity_threshold: 0.001 + progress_checker_plugin: progress_checker + goal_checker_plugins: [goal_checker] + controller_plugins: [FollowPath] + progress_checker: + plugin: nav2_controller::SimpleProgressChecker + required_movement_radius: 0.08 + movement_time_allowance: 10.0 + goal_checker: + plugin: nav2_controller::SimpleGoalChecker + xy_goal_tolerance: 0.15 + yaw_goal_tolerance: 0.2 + stateful: true + FollowPath: + plugin: dwb_core::DWBLocalPlanner + debug_trajectory_details: false + min_vel_x: -0.05 + max_vel_x: 0.22 + max_vel_theta: 2.5 + min_speed_xy: 0.0 + max_speed_xy: 0.22 + acc_lim_x: 1.5 + decel_lim_x: -1.5 + acc_lim_theta: 3.2 + decel_lim_theta: -3.2 + vx_samples: 20 + vtheta_samples: 20 + sim_time: 1.7 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.15 + trans_stopped_velocity: 0.05 + short_circuit_trajectory_evaluation: true + stateful: true + critics: [RotateToGoal, Oscillation, BaseObstacle, GoalAlign, PathAlign, PathDist, GoalDist] + BaseObstacle.scale: 0.02 + PathAlign.scale: 24.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 18.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 24.0 + GoalDist.scale: 18.0 + RotateToGoal.scale: 24.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +global_costmap: + global_costmap: + ros__parameters: + use_sim_time: true + global_frame: map + robot_base_frame: base_footprint + update_frequency: 5.0 + publish_frequency: 2.0 + transform_tolerance: 0.5 + resolution: 0.05 + robot_radius: 0.12 + track_unknown_space: true + plugins: [static_layer, obstacle_layer, inflation_layer] + static_layer: + plugin: nav2_costmap_2d::StaticLayer + map_subscribe_transient_local: true + obstacle_layer: + plugin: nav2_costmap_2d::ObstacleLayer + observation_sources: scan + scan: + topic: /scan + data_type: LaserScan + marking: true + clearing: true + max_obstacle_height: 1.0 + inflation_layer: + plugin: nav2_costmap_2d::InflationLayer + inflation_radius: 0.30 + cost_scaling_factor: 3.0 + +local_costmap: + local_costmap: + ros__parameters: + use_sim_time: true + global_frame: odom + robot_base_frame: base_footprint + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.12 + update_frequency: 5.0 + publish_frequency: 2.0 + plugins: [obstacle_layer, inflation_layer] + obstacle_layer: + plugin: nav2_costmap_2d::ObstacleLayer + observation_sources: scan + scan: + topic: /scan + data_type: LaserScan + marking: true + clearing: true + max_obstacle_height: 1.0 + inflation_layer: + plugin: nav2_costmap_2d::InflationLayer + inflation_radius: 0.30 + cost_scaling_factor: 3.0 + +smoother_server: + ros__parameters: + use_sim_time: true + smoother_plugins: [simple_smoother] + simple_smoother: + plugin: nav2_smoother::SimpleSmoother + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: true + +waypoint_follower: + ros__parameters: + use_sim_time: true + loop_rate: 20 + stop_on_failure: false diff --git a/src/planning_pkg/config/planning_3d_real_mkmini.yaml b/src/planning_pkg/config/planning_3d_real_mkmini.yaml new file mode 100755 index 0000000..891f16f --- /dev/null +++ b/src/planning_pkg/config/planning_3d_real_mkmini.yaml @@ -0,0 +1,168 @@ + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom_custom + bt_loop_duration: 20 + default_server_timeout: 2000 + wait_for_service_timeout: 4000 + + default_nav_to_pose_bt_xml: "navigate_to_pose_w_replanning_and_recovery.xml" + default_nav_through_poses_bt_xml: "navigate_through_poses_w_replanning_and_recovery.xml" + + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +global_costmap: + global_costmap: + ros__parameters: + transform_tolerance: 1.0 + + update_frequency: 0.5 + publish_frequency: 0.5 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + footprint: "[ [0.35, 0.28], [0.35, -0.28], [-0.35, -0.28], [-0.35, 0.28] ]" + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + + obstacle_layer: + min_obstacle_height: 0.1 + max_obstacle_height: 2.5 + + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: lidar3d + lidar3d: + topic: /lidar_filtered + + min_obstacle_height: 0.1 + max_obstacle_height: 2.5 + + clearing: True + marking: True + data_type: "PointCloud2" + obstacle_min_range: 0.0 + obstacle_max_range: 15.0 + raytrace_min_range: 0.0 + raytrace_max_range: 15.0 + + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.02 + always_send_full_costmap: True + +planner_server: + ros__parameters: + expected_planner_frequency: 2.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_smac_planner/SmacPlannerHybrid" + tolerance: 0.5 + allow_unknown: true + downsample_costmap: true + downsampling_factor: 2 + max_iterations: 3000000 + max_on_approach_iterations: 5000 + max_planning_time: 10.0 + motion_model_for_search: "DUBIN" + angle_quantization_bins: 144 + analytic_expansion_ratio: 3.5 + analytic_expansion_max_length: 5.0 + minimum_turning_radius: 1.1 + reverse_penalty: 2.1 + change_penalty: 0.0 + non_straight_penalty: 1.5 + cost_penalty: 5.0 + retrospective_penalty: 0.1 + lookup_table_size: 20.0 + cache_obstacle_heuristic: true + smooth_path: true + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 diff --git a/src/planning_pkg/config/planning_3d_sim_fr09.yaml b/src/planning_pkg/config/planning_3d_sim_fr09.yaml new file mode 100755 index 0000000..d5898cb --- /dev/null +++ b/src/planning_pkg/config/planning_3d_sim_fr09.yaml @@ -0,0 +1,166 @@ + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: wheel_odom + bt_loop_duration: 20 + default_server_timeout: 2000 + wait_for_service_timeout: 4000 + + default_nav_to_pose_bt_xml: "navigate_to_pose_w_replanning_and_recovery.xml" + default_nav_through_poses_bt_xml: "navigate_through_poses_w_replanning_and_recovery.xml" + + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 0.5 + publish_frequency: 0.5 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + footprint: "[[1.220, 0.3715], [1.220, -0.3715], [-0.369, -0.3715], [-0.369, 0.3715]]" + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + + obstacle_layer: + min_obstacle_height: 0.1 + max_obstacle_height: 2.5 + + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: lidar3d + lidar3d: + topic: /lidar_filtered + + min_obstacle_height: 0.1 + max_obstacle_height: 2.5 + + clearing: True + marking: True + data_type: "PointCloud2" + obstacle_min_range: 0.0 + obstacle_max_range: 15.0 + raytrace_min_range: 0.0 + raytrace_max_range: 15.0 + + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 2.0 + inflation_radius: 1.0 + always_send_full_costmap: True + +planner_server: + ros__parameters: + expected_planner_frequency: 2.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_smac_planner/SmacPlannerHybrid" + tolerance: 0.5 + allow_unknown: true + downsample_costmap: true + downsampling_factor: 2 + max_iterations: 3000000 + max_on_approach_iterations: 5000 + max_planning_time: 10.0 + motion_model_for_search: "DUBIN" + angle_quantization_bins: 144 + analytic_expansion_ratio: 3.5 + analytic_expansion_max_length: 5.0 + minimum_turning_radius: 0.5 + reverse_penalty: 2.1 + change_penalty: 1.0 + non_straight_penalty: 1.5 + cost_penalty: 5.0 + retrospective_penalty: 0.1 + lookup_table_size: 20.0 + cache_obstacle_heuristic: true + smooth_path: true + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 diff --git a/src/planning_pkg/config/planning_3d_sim_mkmini.yaml b/src/planning_pkg/config/planning_3d_sim_mkmini.yaml new file mode 100755 index 0000000..68c5607 --- /dev/null +++ b/src/planning_pkg/config/planning_3d_sim_mkmini.yaml @@ -0,0 +1,166 @@ + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: wheel_odom + bt_loop_duration: 20 + default_server_timeout: 2000 + wait_for_service_timeout: 4000 + + default_nav_to_pose_bt_xml: "navigate_to_pose_w_replanning_and_recovery.xml" + default_nav_through_poses_bt_xml: "navigate_through_poses_w_replanning_and_recovery.xml" + + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 0.5 + publish_frequency: 0.5 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + footprint: "[ [0.42, 0.305], [0.42, -0.305], [-0.42, -0.305], [-0.42, 0.305] ]" + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + + obstacle_layer: + min_obstacle_height: 0.1 + max_obstacle_height: 2.5 + + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: lidar3d + lidar3d: + topic: /carla/hero/lidar_filtered + + min_obstacle_height: 0.1 + max_obstacle_height: 2.5 + + clearing: True + marking: True + data_type: "PointCloud2" + obstacle_min_range: 0.0 + obstacle_max_range: 15.0 + raytrace_min_range: 0.0 + raytrace_max_range: 15.0 + + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 2.0 + inflation_radius: 1.0 + always_send_full_costmap: True + +planner_server: + ros__parameters: + expected_planner_frequency: 2.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_smac_planner/SmacPlannerHybrid" + tolerance: 0.5 + allow_unknown: true + downsample_costmap: true + downsampling_factor: 2 + max_iterations: 3000000 + max_on_approach_iterations: 5000 + max_planning_time: 10.0 + motion_model_for_search: "DUBIN" + angle_quantization_bins: 144 + analytic_expansion_ratio: 3.5 + analytic_expansion_max_length: 5.0 + minimum_turning_radius: 1.1 + reverse_penalty: 2.1 + change_penalty: 0.0 + non_straight_penalty: 1.5 + cost_penalty: 5.0 + retrospective_penalty: 0.1 + lookup_table_size: 20.0 + cache_obstacle_heuristic: true + smooth_path: true + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 diff --git a/src/planning_pkg/launch/__pycache__/nav2_planning.launch.cpython-310.pyc b/src/planning_pkg/launch/__pycache__/nav2_planning.launch.cpython-310.pyc new file mode 100644 index 0000000..20e8a4c Binary files /dev/null and b/src/planning_pkg/launch/__pycache__/nav2_planning.launch.cpython-310.pyc differ diff --git a/src/planning_pkg/launch/nav2_planning.launch.py b/src/planning_pkg/launch/nav2_planning.launch.py new file mode 100755 index 0000000..5431691 --- /dev/null +++ b/src/planning_pkg/launch/nav2_planning.launch.py @@ -0,0 +1,144 @@ + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import GroupAction +from launch.actions import OpaqueFunction +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import PythonExpression +from launch_ros.actions import Node +from launch_ros.descriptions import ParameterFile +from nav2_common.launch import RewrittenYaml + + +ARGUMENTS = [ + DeclareLaunchArgument('sim_mode', default_value='true'), + DeclareLaunchArgument('log_level', default_value='info'), + DeclareLaunchArgument('robot', default_value='fr09', choices=['mkmini', 'fr09', 'turtlebot3']), + DeclareLaunchArgument('level', default_value='3d'), +] + +def launch_setup(context, *args, **kwargs): + sim_mode = LaunchConfiguration('sim_mode').perform(context).lower() + robot = LaunchConfiguration('robot').perform(context) + level = LaunchConfiguration('level').perform(context) + log_level = LaunchConfiguration('log_level').perform(context) + + lifecycle_nodes = ['controller_server', + 'smoother_server', + 'planner_server', + 'bt_navigator', + 'waypoint_follower'] + + pkg_planning = get_package_share_directory('planning_pkg') + + # Behavior Tree + default_nav_to_pose_bt_xml = PathJoinSubstitution([ + pkg_planning, + 'behavior', + 'navigate_to_pose_no_spin.xml' + ]) + default_nav_through_poses_bt_xml = PathJoinSubstitution([ + pkg_planning, + 'behavior', + 'navigate_through_poses_no_spin.xml' + ]) + + param_file_name = None + + if sim_mode == 'true': + param_file_name = f'planning_{level}_sim_{robot}.yaml' + else: + param_file_name = f'planning_{level}_real_{robot}.yaml' + + # Parameters + params_file = PathJoinSubstitution([ + pkg_planning, + 'config', param_file_name]) + param_substitutions = { + 'use_sim_time': 'true', + 'default_nav_to_pose_bt_xml': default_nav_to_pose_bt_xml, + 'default_nav_through_poses_bt_xml': default_nav_through_poses_bt_xml + } + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + param_rewrites=param_substitutions, + convert_types=True), + allow_substs=True) + + load_nodes = GroupAction( + actions=[ + Node( + package='nav2_controller', + executable='controller_server', + name='controller_server', + output='screen', + respawn=False, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + ), + Node( + package='nav2_planner', + executable='planner_server', + name='planner_server', + output='screen', + respawn=False, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + ), + Node( + package='nav2_bt_navigator', + executable='bt_navigator', + name='bt_navigator', + output='screen', + respawn=False, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + ), + Node( + package='nav2_waypoint_follower', + executable='waypoint_follower', + name='waypoint_follower', + output='screen', + respawn=False, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + ), + Node( + package='nav2_smoother', + executable='smoother_server', + name='smoother_server', + output='screen', + respawn=False, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + ), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_planning', + output='screen', + arguments=['--ros-args', '--log-level', log_level], + parameters=[{'use_sim_time': True}, + {'autostart': True}, + {'node_names': lifecycle_nodes}] + ), + ], + ) + + return [load_nodes] + +def generate_launch_description(): + + # Create launch description and add actions + ld = LaunchDescription(ARGUMENTS) + ld.add_action(OpaqueFunction(function=launch_setup)) + + return ld diff --git a/src/planning_pkg/launch/planning.launch.py b/src/planning_pkg/launch/planning.launch.py new file mode 100755 index 0000000..8d892f8 --- /dev/null +++ b/src/planning_pkg/launch/planning.launch.py @@ -0,0 +1,15 @@ + +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + node = Node( + package='planning_pkg', + executable='planning_manager', + name='planning_manager', + output='screen', + ) + + ld = LaunchDescription([]) + ld.add_action(node) + return ld diff --git a/src/planning_pkg/package.xml b/src/planning_pkg/package.xml new file mode 100755 index 0000000..ff1e120 --- /dev/null +++ b/src/planning_pkg/package.xml @@ -0,0 +1,24 @@ + + + + planning_pkg + 0.0.1 + Modular Nav2 launch package for FR-09/MK-Mini autonomous driving + FutureDrive + Apache-2.0 + + nav2_planner + nav2_smoother + nav2_bt_navigator + nav2_lifecycle_manager + nav2_waypoint_follower + + ament_python + ament_copyright + ament_flake8 + ament_pep257 + + + ament_python + + diff --git a/src/planning_pkg/planning_pkg/__init__.py b/src/planning_pkg/planning_pkg/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/src/planning_pkg/planning_pkg/__pycache__/__init__.cpython-310.pyc b/src/planning_pkg/planning_pkg/__pycache__/__init__.cpython-310.pyc new file mode 100755 index 0000000..bde908c Binary files /dev/null and b/src/planning_pkg/planning_pkg/__pycache__/__init__.cpython-310.pyc differ diff --git a/src/planning_pkg/planning_pkg/__pycache__/planning_manager.cpython-310.pyc b/src/planning_pkg/planning_pkg/__pycache__/planning_manager.cpython-310.pyc new file mode 100755 index 0000000..1b5b7de Binary files /dev/null and b/src/planning_pkg/planning_pkg/__pycache__/planning_manager.cpython-310.pyc differ diff --git a/src/planning_pkg/planning_pkg/planning_manager.py b/src/planning_pkg/planning_pkg/planning_manager.py new file mode 100755 index 0000000..12b4c75 --- /dev/null +++ b/src/planning_pkg/planning_pkg/planning_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 PlanningManager(Node): + def __init__(self): + super().__init__('planning_manager') + self.get_logger().info('Planning Manager node has been started.') + + qos = QoSProfile( + depth=1, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + reliability=ReliabilityPolicy.RELIABLE + ) + self.planning_process = None + + self.create_subscription(String, '/vehicle_config', self.handle_config, qos_profile=qos) + + def handle_config(self, msg:String): + self.get_logger().info(f"SLAM Received config: {msg.data}") + config_data = json.loads(msg.data) + + self.planning_process = subprocess.Popen(['ros2', 'launch', + 'planning_pkg', + 'nav2_planning.launch.py', + f'sim_mode:={config_data.get("sim_mode")}', + f'robot:={config_data.get("robot")}', + f'level:={config_data.get("level")}']) + pass + + def destroy_node(self): + if self.planning_process: + self.planning_process.terminate() + self.planning_process.wait() + + return super().destroy_node() + +def main(args=None): + rclpy.init(args=args) + node = PlanningManager() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() + \ No newline at end of file diff --git a/src/planning_pkg/planning_pkg/waypoint_sender.py b/src/planning_pkg/planning_pkg/waypoint_sender.py new file mode 100755 index 0000000..bc0104e --- /dev/null +++ b/src/planning_pkg/planning_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/planning_pkg/resource/planning_pkg b/src/planning_pkg/resource/planning_pkg new file mode 100755 index 0000000..e69de29 diff --git a/src/planning_pkg/setup.cfg b/src/planning_pkg/setup.cfg new file mode 100755 index 0000000..ba83b27 --- /dev/null +++ b/src/planning_pkg/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/planning_pkg +[install] +install_scripts=$base/lib/planning_pkg diff --git a/src/planning_pkg/setup.py b/src/planning_pkg/setup.py new file mode 100755 index 0000000..336b3c0 --- /dev/null +++ b/src/planning_pkg/setup.py @@ -0,0 +1,31 @@ + +import os +from glob import glob +from setuptools import find_packages, setup + +package_name = 'planning_pkg' + +setup( + name=package_name, + version='0.0.1', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/planning_pkg']), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), + (os.path.join('share', package_name, 'behavior'), glob('behavior/*.xml')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='FutureDrive', + maintainer_email='todo@todo.com', + description='Modular Nav2 launch package for FR-09/MK-Mini autonomous driving', + license='Apache-2.0', + entry_points={ + 'console_scripts': [ + 'waypoint_sender = planning_pkg.waypoint_sender:main', + 'planning_manager = planning_pkg.planning_manager:main', + ], + }, +)