From 66acea5e87eed514d9f1374356e03f76aad00f55 Mon Sep 17 00:00:00 2001 From: Orange <2314753575@qq.com> Date: Wed, 5 Aug 2026 22:15:35 +0800 Subject: [PATCH] =?UTF-8?q?=E7=BB=99=E8=BD=A8=E8=BF=B9+MPPI=E7=89=88?= =?UTF-8?q?=E5=AF=BC=E8=88=AA=E5=8A=A0=E5=85=A5=E4=BA=86=E9=9A=9C=E7=A2=8D?= =?UTF-8?q?=E7=89=A9=E6=A3=80=E6=B5=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../obstacle_nav2/config/nav2_params.yaml | 30 +- .../obstacle_nav2/config/nav2_profile_10.yaml | 334 +++++++++++++ .../obstacle_nav2/config/nav2_profile_11.yaml | 358 ++++++++++++++ .../config/trajectory_guard.yaml | 27 ++ .../launch/obstacle_nav2.launch.py | 7 + .../obstacle_nav2_q_waypoints.launch.py | 265 ++++++++++ .../obstacle_nav2/src/trajectory_guard.cpp | 277 +++++++++++ .../src/trajectory_guard_node.cpp | 454 ++++++++++++++++++ .../test/test_trajectory_guard.cpp | 162 +++++++ 9 files changed, 1900 insertions(+), 14 deletions(-) create mode 100644 src/navigation/obstacle_nav2/config/nav2_profile_10.yaml create mode 100644 src/navigation/obstacle_nav2/config/nav2_profile_11.yaml create mode 100644 src/navigation/obstacle_nav2/config/trajectory_guard.yaml create mode 100644 src/navigation/obstacle_nav2/launch/obstacle_nav2_q_waypoints.launch.py create mode 100644 src/navigation/obstacle_nav2/src/trajectory_guard.cpp create mode 100644 src/navigation/obstacle_nav2/src/trajectory_guard_node.cpp create mode 100644 src/navigation/obstacle_nav2/test/test_trajectory_guard.cpp diff --git a/src/navigation/obstacle_nav2/config/nav2_params.yaml b/src/navigation/obstacle_nav2/config/nav2_params.yaml index 50f984e..bfd63a8 100755 --- a/src/navigation/obstacle_nav2/config/nav2_params.yaml +++ b/src/navigation/obstacle_nav2/config/nav2_params.yaml @@ -112,8 +112,8 @@ controller_server: CostCritic: enabled: true cost_power: 1 - cost_weight: 4.5 - critical_cost: 300.0 + cost_weight: 6.0 + critical_cost: 250.0 consider_footprint: true collision_cost: 1000000.0 near_goal_distance: 0.8 @@ -134,7 +134,7 @@ controller_server: PathAlignCritic: enabled: true cost_power: 1 - cost_weight: 16.0 + cost_weight: 8.0 max_path_occupancy_ratio: 0.07 trajectory_point_step: 4 threshold_to_consider: 0.45 @@ -144,14 +144,14 @@ controller_server: PathFollowCritic: enabled: true cost_power: 1 - cost_weight: 6.0 + cost_weight: 4.0 offset_from_furthest: 4 threshold_to_consider: 1.0 PathAngleCritic: enabled: true cost_power: 1 - cost_weight: 6.0 + cost_weight: 5.0 offset_from_furthest: 5 threshold_to_consider: 0.45 max_angle_to_furthest: 1.2 @@ -160,7 +160,7 @@ controller_server: PreferForwardCritic: enabled: true cost_power: 1 - cost_weight: 2.0 + cost_weight: 1.0 threshold_to_consider: 0.4 controller_server_rclcpp_node: @@ -188,12 +188,13 @@ local_costmap: plugin: "obstacle_nav2::ObstacleArrayLayer" enabled: true topic: /obstacles - obstacle_timeout: 0.5 + obstacle_timeout: 1.0 transform_tolerance: 0.2 default_obstacle_radius: 0.05 minimum_obstacle_radius: 0.02 maximum_obstacle_radius: 0.06 extra_inflation: 0.02 + retain_previous_on_empty_snapshot: true inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 @@ -209,8 +210,8 @@ local_costmap: global_costmap: global_costmap: ros__parameters: - update_frequency: 2.0 - publish_frequency: 1.0 + update_frequency: 5.0 + publish_frequency: 3.0 transform_tolerance: 0.5 global_frame: odom robot_base_frame: base_footprint @@ -227,12 +228,13 @@ global_costmap: plugin: "obstacle_nav2::ObstacleArrayLayer" enabled: true topic: /obstacles - obstacle_timeout: 0.5 + obstacle_timeout: 1.0 transform_tolerance: 0.2 default_obstacle_radius: 0.05 minimum_obstacle_radius: 0.02 maximum_obstacle_radius: 0.50 extra_inflation: 0.02 + retain_previous_on_empty_snapshot: true inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 @@ -264,10 +266,10 @@ planner_server: analytic_expansion_max_length: 2.0 minimum_turning_radius: 0.60 reverse_penalty: 2.2 - change_penalty: 0.5 - non_straight_penalty: 0.85 - cost_penalty: 4.5 - retrospective_penalty: 0.015 + change_penalty: 0.0 + non_straight_penalty: 0.8 + cost_penalty: 5.0 + retrospective_penalty: 0.01 lookup_table_size: 5.0 cache_obstacle_heuristic: false viz_expansions: false diff --git a/src/navigation/obstacle_nav2/config/nav2_profile_10.yaml b/src/navigation/obstacle_nav2/config/nav2_profile_10.yaml new file mode 100644 index 0000000..359391a --- /dev/null +++ b/src/navigation/obstacle_nav2/config/nav2_profile_10.yaml @@ -0,0 +1,334 @@ +# ============================================================================ +# nav2_params.yaml — Odometry-only obstacle navigation +# +# No static map, no AMCL, no SLAM. +# Both costmaps are rolling windows in odom. The launch file can rewrite every +# global_frame leaf when a different connected odometry frame is required. +# Global planner: Smac Hybrid A* (Reeds-Shepp) +# Local controller: MPPI (Ackermann) +# ============================================================================ + +bt_navigator: + ros__parameters: + use_sim_time: False + global_frame: odom + robot_base_frame: base_footprint + odom_topic: /odom_combined + bt_loop_duration: 50 + default_server_timeout: 20 + # Injected by obstacle_nav2.launch.py from this package's share directory. + default_nav_to_pose_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_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_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_drive_on_heading_cancel_bt_node + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: False + +controller_server: + ros__parameters: + use_sim_time: False + controller_frequency: 20.0 + FollowPath: + plugin: "nav2_mppi_controller::MPPIController" + time_steps: 36 + model_dt: 0.05 + batch_size: 1000 + vx_std: 0.75 + vy_std: 0.0 + wz_std: 0.4 + vx_max: 1.75 + vx_min: -0.75 + vy_max: 0.0 + wz_max: 1.5 + iteration_count: 1 + temperature: 0.3 + gamma: 0.015 + motion_model: "Ackermann" + visualize: false + TrajectoryVisualizer: + trajectory_step: 5 + time_step: 3 + AckermannConstraints: + min_turning_r: 0.4 + critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"] + ConstraintCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 + GoalCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 1.4 + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + threshold_to_consider: 0.5 + PreferForwardCritic: + enabled: false + cost_power: 1 + cost_weight: 0.0 + threshold_to_consider: 0.5 + CostCritic: + enabled: true + cost_power: 1 + cost_weight: 3.81 + critical_cost: 300.0 + consider_footprint: true + collision_cost: 1000000.0 + near_goal_distance: 1.0 + trajectory_point_step: 2 + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 14.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 4 + threshold_to_consider: 0.5 + offset_from_furthest: 20 + use_path_orientations: false + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + offset_from_furthest: 5 + threshold_to_consider: 1.4 + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + offset_from_furthest: 4 + threshold_to_consider: 0.5 + max_angle_to_furthest: 1.0 + forward_preference: false + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: False + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + transform_tolerance: 0.5 + global_frame: odom + robot_base_frame: base_footprint + use_sim_time: False + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]" + footprint_padding: 0.02 + track_unknown_space: false + plugins: ["obstacle_array_layer", "inflation_layer"] + obstacle_array_layer: + plugin: "obstacle_nav2::ObstacleArrayLayer" + enabled: true + topic: /obstacles + obstacle_timeout: 0.5 + transform_tolerance: 0.2 + default_obstacle_radius: 0.05 + minimum_obstacle_radius: 0.02 + maximum_obstacle_radius: 0.50 + extra_inflation: 0.02 + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + local_costmap_client: + ros__parameters: + use_sim_time: False + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: False + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + transform_tolerance: 0.5 + global_frame: odom + robot_base_frame: base_footprint + use_sim_time: False + rolling_window: true + width: 10 + height: 10 + resolution: 0.05 + footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]" + footprint_padding: 0.02 + track_unknown_space: false + plugins: ["obstacle_array_layer", "inflation_layer"] + obstacle_array_layer: + plugin: "obstacle_nav2::ObstacleArrayLayer" + enabled: true + topic: /obstacles + obstacle_timeout: 0.5 + transform_tolerance: 0.2 + default_obstacle_radius: 0.05 + minimum_obstacle_radius: 0.02 + maximum_obstacle_radius: 0.50 + extra_inflation: 0.02 + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + global_costmap_client: + ros__parameters: + use_sim_time: False + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: False + +planner_server: + ros__parameters: + planner_plugins: ["GridBased"] + use_sim_time: False + GridBased: + plugin: "nav2_smac_planner/SmacPlannerHybrid" + downsample_costmap: false + downsampling_factor: 1 + tolerance: 0.25 + allow_unknown: false + max_iterations: 1000000 + max_on_approach_iterations: 1000 + max_planning_time: 5.0 + motion_model_for_search: "REEDS_SHEPP" + angle_quantization_bins: 72 + analytic_expansion_ratio: 3.5 + analytic_expansion_max_length: 3.0 + minimum_turning_radius: 0.40 + reverse_penalty: 1.0 + change_penalty: 0.0 + non_straight_penalty: 1.2 + cost_penalty: 2.0 + retrospective_penalty: 0.015 + # 5 m covers the rolling planning horizon without the startup and memory + # cost of the previous 20 m (401-cell) Hybrid-A* lookup table. + lookup_table_size: 5.0 + cache_obstacle_heuristic: false + viz_expansions: false + smooth_path: True + smoother: + max_iterations: 1000 + w_smooth: 0.3 + w_data: 0.2 + tolerance: 1.0e-10 + do_refinement: true + refinement_num: 2 + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: False + +smoother_server: + ros__parameters: + use_sim_time: False + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + backup_dist: 0.8 + backup_speed: 0.18 + wait: + plugin: "nav2_behaviors/Wait" + wait_duration: 0.5 + global_frame: odom + robot_base_frame: base_footprint + transform_tolerance: 0.5 + use_sim_time: False + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: False + +waypoint_follower: + ros__parameters: + loop_rate: 20 + use_sim_time: False + 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 + +velocity_smoother: + ros__parameters: + use_sim_time: False + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [1.75, 0.0, 1.5] + min_velocity: [-0.75, 0.0, -1.5] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: /odom_combined + odom_duration: 0.1 + deadband_velocity: [0.03, 0.0, 0.03] + velocity_timeout: 1.0 diff --git a/src/navigation/obstacle_nav2/config/nav2_profile_11.yaml b/src/navigation/obstacle_nav2/config/nav2_profile_11.yaml new file mode 100644 index 0000000..e23fa71 --- /dev/null +++ b/src/navigation/obstacle_nav2/config/nav2_profile_11.yaml @@ -0,0 +1,358 @@ +# ============================================================================ +# nav2_params_basic_tracking.yaml +# +# Basic stable trajectory-following profile for obstacle_nav2. +# This file is intentionally conservative: low speed, longer MPPI horizon, +# smoother acceleration, moderate path tracking weights, and Ackermann limits. +# +# It does not replace nav2_params.yaml. To use it, launch Nav2 with this params +# file or add a launch argument later. +# ============================================================================ + +bt_navigator: + ros__parameters: + use_sim_time: False + global_frame: odom + robot_base_frame: base_footprint + odom_topic: /odom_combined + bt_loop_duration: 50 + default_server_timeout: 20 + default_nav_to_pose_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_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_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_drive_on_heading_cancel_bt_node + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: False + +controller_server: + ros__parameters: + use_sim_time: False + controller_frequency: 20.0 + + FollowPath: + plugin: "nav2_mppi_controller::MPPIController" + + # Longer horizon and moderate sampling make turns easier to discover. + time_steps: 48 + model_dt: 0.05 + batch_size: 1000 + iteration_count: 1 + + # Conservative velocity exploration. Speed is deliberately not optimized. + vx_std: 0.20 + vy_std: 0.0 + wz_std: 0.85 + vx_max: 0.50 + vx_min: -0.15 + vy_max: 0.0 + wz_max: 1.8 + + # Lower temperature makes selection steadier; gamma damps rough controls. + temperature: 0.25 + gamma: 0.02 + + motion_model: "Ackermann" + visualize: false + TrajectoryVisualizer: + trajectory_step: 5 + time_step: 3 + AckermannConstraints: + min_turning_r: 0.60 + + critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"] + + ConstraintCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + + CostCritic: + enabled: true + cost_power: 1 + cost_weight: 6.0 + critical_cost: 250.0 + consider_footprint: true + collision_cost: 1000000.0 + near_goal_distance: 0.8 + trajectory_point_step: 2 + + GoalCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + threshold_to_consider: 1.0 + + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + threshold_to_consider: 0.45 + + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 8.0 + max_path_occupancy_ratio: 0.07 + trajectory_point_step: 4 + threshold_to_consider: 0.45 + offset_from_furthest: 8 + use_path_orientations: false + + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 + offset_from_furthest: 4 + threshold_to_consider: 1.0 + + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + offset_from_furthest: 5 + threshold_to_consider: 0.45 + max_angle_to_furthest: 1.2 + forward_preference: false + + PreferForwardCritic: + enabled: true + cost_power: 1 + cost_weight: 1.0 + threshold_to_consider: 0.4 + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: False + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 8.0 + publish_frequency: 4.0 + transform_tolerance: 0.5 + global_frame: odom + robot_base_frame: base_footprint + use_sim_time: False + rolling_window: true + width: 4 + height: 4 + resolution: 0.05 + footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]" + footprint_padding: 0.02 + track_unknown_space: false + plugins: ["obstacle_array_layer", "inflation_layer"] + obstacle_array_layer: + plugin: "obstacle_nav2::ObstacleArrayLayer" + enabled: true + topic: /obstacles + obstacle_timeout: 1.0 + transform_tolerance: 0.2 + default_obstacle_radius: 0.05 + minimum_obstacle_radius: 0.02 + maximum_obstacle_radius: 0.06 + extra_inflation: 0.02 + retain_previous_on_empty_snapshot: true + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.25 + always_send_full_costmap: True + local_costmap_client: + ros__parameters: + use_sim_time: False + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: False + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 3.0 + transform_tolerance: 0.5 + global_frame: odom + robot_base_frame: base_footprint + use_sim_time: False + rolling_window: true + width: 10 + height: 10 + resolution: 0.05 + footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]" + footprint_padding: 0.02 + track_unknown_space: false + plugins: ["obstacle_array_layer", "inflation_layer"] + obstacle_array_layer: + plugin: "obstacle_nav2::ObstacleArrayLayer" + enabled: true + topic: /obstacles + obstacle_timeout: 1.0 + transform_tolerance: 0.2 + default_obstacle_radius: 0.05 + minimum_obstacle_radius: 0.02 + maximum_obstacle_radius: 0.50 + extra_inflation: 0.02 + retain_previous_on_empty_snapshot: true + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.45 + always_send_full_costmap: True + global_costmap_client: + ros__parameters: + use_sim_time: False + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: False + +planner_server: + ros__parameters: + planner_plugins: ["GridBased"] + use_sim_time: False + GridBased: + plugin: "nav2_smac_planner/SmacPlannerHybrid" + downsample_costmap: false + downsampling_factor: 1 + tolerance: 0.20 + allow_unknown: false + max_iterations: 1000000 + max_on_approach_iterations: 1000 + max_planning_time: 5.0 + motion_model_for_search: "REEDS_SHEPP" + angle_quantization_bins: 72 + analytic_expansion_ratio: 3.5 + analytic_expansion_max_length: 2.0 + minimum_turning_radius: 0.60 + reverse_penalty: 2.2 + change_penalty: 0.0 + non_straight_penalty: 0.8 + cost_penalty: 5.0 + retrospective_penalty: 0.01 + lookup_table_size: 5.0 + cache_obstacle_heuristic: false + viz_expansions: false + smooth_path: True + smoother: + max_iterations: 1000 + w_smooth: 0.35 + w_data: 0.25 + tolerance: 1.0e-10 + do_refinement: true + refinement_num: 2 + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: False + +smoother_server: + ros__parameters: + use_sim_time: False + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + backup_dist: 0.30 + backup_speed: 0.08 + wait: + plugin: "nav2_behaviors/Wait" + wait_duration: 0.5 + global_frame: odom + robot_base_frame: base_footprint + transform_tolerance: 0.5 + use_sim_time: False + simulate_ahead_time: 2.0 + max_rotational_vel: 0.8 + min_rotational_vel: 0.2 + rotational_acc_lim: 1.5 + +robot_state_publisher: + ros__parameters: + use_sim_time: False + +waypoint_follower: + ros__parameters: + loop_rate: 20 + use_sim_time: False + 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 + +velocity_smoother: + ros__parameters: + use_sim_time: False + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.50, 0.0, 1.8] + min_velocity: [-0.15, 0.0, -1.8] + max_accel: [0.50, 0.0, 1.50] + max_decel: [-0.60, 0.0, -1.80] + odom_topic: /odom_combined + odom_duration: 0.1 + deadband_velocity: [0.02, 0.0, 0.02] + velocity_timeout: 1.0 + +lifecycle_manager_navigation: + ros__parameters: + use_sim_time: False + autostart: True + service_timeout: 10.0 + bond_timeout: 8.0 diff --git a/src/navigation/obstacle_nav2/config/trajectory_guard.yaml b/src/navigation/obstacle_nav2/config/trajectory_guard.yaml new file mode 100644 index 0000000..c55beee --- /dev/null +++ b/src/navigation/obstacle_nav2/config/trajectory_guard.yaml @@ -0,0 +1,27 @@ +trajectory_guard_node: + ros__parameters: + input_path_topic: /trajectory_guard/input_path + patched_path_topic: /trajectory_guard/patched_path + costmap_topic: /local_costmap/costmap + odom_topic: /odom_combined + planner_action: /compute_path_to_pose + follow_action: /follow_path + planner_id: GridBased + controller_id: FollowPath + goal_checker_id: "" + check_period_sec: 0.5 + lookahead_distance: 2.0 + rejoin_min_distance: 1.5 + rejoin_max_distance: 5.0 + occupied_threshold: 50 + treat_unknown_as_occupied: true + footprint_half_length: 0.14 + footprint_half_width: 0.085 + footprint_padding: 0.04 + footprint_sample_step: 0.05 + repair_cooldown_sec: 0.5 + blocked_retry_wait_sec: 0.8 + wait_for_planner_costmap_update: true + min_repair_progress_indices: 5 + publish_zero_on_blocked: true + cmd_vel_topic: /cmd_vel diff --git a/src/navigation/obstacle_nav2/launch/obstacle_nav2.launch.py b/src/navigation/obstacle_nav2/launch/obstacle_nav2.launch.py index 0bec55e..daff3c7 100755 --- a/src/navigation/obstacle_nav2/launch/obstacle_nav2.launch.py +++ b/src/navigation/obstacle_nav2/launch/obstacle_nav2.launch.py @@ -24,6 +24,7 @@ from launch.actions import ( DeclareLaunchArgument, GroupAction, IncludeLaunchDescription, + SetEnvironmentVariable, ) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -49,6 +50,7 @@ def generate_launch_description(): start_obstacle_scanner = LaunchConfiguration('start_obstacle_scanner', default='true') nav2_param_path = os.path.join(pkg_dir, 'config', 'nav2_params.yaml') + fastdds_profile_path = os.path.join(pkg_dir, 'config', 'fastdds_udp_only.xml') nav_to_pose_bt_path = os.path.join( pkg_dir, 'behavior_tree', 'nav_to_pose_ackermann.xml') configured_params = RewrittenYaml( @@ -103,6 +105,7 @@ def generate_launch_description(): 'autostart': 'true', }.items(), ) + # ========================== Assembly ====================================== return LaunchDescription([ DeclareLaunchArgument( @@ -138,6 +141,10 @@ def generate_launch_description(): default_value='true', description='Also start the obstacle_scanner node'), + SetEnvironmentVariable( + name='FASTRTPS_DEFAULT_PROFILES_FILE', + value=fastdds_profile_path), + safe_base_bringup, lslidar_launch, obstacle_scanner_launch, diff --git a/src/navigation/obstacle_nav2/launch/obstacle_nav2_q_waypoints.launch.py b/src/navigation/obstacle_nav2/launch/obstacle_nav2_q_waypoints.launch.py new file mode 100644 index 0000000..e035c38 --- /dev/null +++ b/src/navigation/obstacle_nav2/launch/obstacle_nav2_q_waypoints.launch.py @@ -0,0 +1,265 @@ +#!/usr/bin/env python3 +""" +obstacle_nav2_q_waypoints.launch.py +==================================== +复合启动文件：同时启动 obstacle_nav2 导航系统 + Q 路点候选选择器节点。 + +架构概览 +-------- +本 launch 文件包含两个核心组件： + 1. obstacle_nav2 : 完整的障碍物导航栈（底盘驱动、激光雷达、Nav2 导航、障碍物扫描） + 2. q_waypoint_selector : 独立的 Python 节点，周期性从 costmap 中评估并选择 + 最优的 Q 路点候选目标，通过 NavigateToPose action 下发导航目标。 + +Q 路点选择器的设计意图 +---------------------- +在比赛中，我们需要依次访问一系列路点（Q₁, Q₂, ..., Qₙ），但每个路点可能有多个 +候选位置。选择器从 JSON 文件中加载有序路点及其候选坐标，结合全局 costmap 的 +占用栅格信息，过滤掉被障碍物占据的候选点，并向 Nav2 下发当前最优的导航目标。 + +启动方式 +-------- + ros2 launch obstacle_nav2 obstacle_nav2_q_waypoints.launch.py \ + use_sim_time:=false \ + global_frame:=odom \ + json_dir:=/path/to/json \ + goal_frame:=odom \ + costmap_topic:=/global_costmap/costmap +""" + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch.substitutions import FindExecutable + + +def generate_launch_description(): + # ========================================================================= + # 路径配置 —— 定位 obstacle_nav2 包内的关键文件 + # ========================================================================= + + # obstacle_nav2 包的共享目录（install 后的 share/obstacle_nav2） + pkg_dir = get_package_share_directory("obstacle_nav2") + + # 基础导航 launch 文件路径，将被 IncludeLaunchDescription 引入 + base_launch = os.path.join(pkg_dir, "launch", "obstacle_nav2.launch.py") + + # Q 路点选择器的 Python 脚本路径，将被 ExecuteProcess 作为独立进程运行 + selector_script = os.path.join(pkg_dir, "config", "q_waypoint_selector.py") + + # 默认的 JSON 路点文件目录 + default_json_dir = os.path.join(pkg_dir, "config", "json") + + # ========================================================================= + # LaunchConfiguration —— 运行时参数占位符 + # 这些变量在 launch 时被替换为实际的参数值（命令行传入或默认值）。 + # 使用 LaunchConfiguration 而非直接读取值，是为了支持 launch 系统的 + # 延迟求值（lazy evaluation）机制。 + # ========================================================================= + + # ---- obstacle_nav2 基础导航栈参数 ---- + + # 是否使用仿真时间（实车=false，Gazebo 仿真=true） + use_sim_time = LaunchConfiguration("use_sim_time") + + # 全局坐标系名称（仿真用 "odom"，实车用 "odom_combined"） + global_frame = LaunchConfiguration("global_frame") + + # 是否加载静态地图（已知地图导航=true，纯 SLAM 探索=false） + use_static_map = LaunchConfiguration("use_static_map") + + # 静态地图 YAML 文件路径（为空时不加载 map_server） + map_yaml = LaunchConfiguration("map_yaml") + + # 是否启用底盘运动控制（调试时可关闭以只运行感知） + enable_motion = LaunchConfiguration("enable_motion") + + # 是否启动底盘驱动节点（实车=true，纯仿真可视化=false） + start_base = LaunchConfiguration("start_base") + + # 是否启动激光雷达驱动节点 + start_lidar = LaunchConfiguration("start_lidar") + + # 是否启动障碍物扫描节点（obstacle_scanner） + start_obstacle_scanner = LaunchConfiguration("start_obstacle_scanner") + + # ---- Q 路点选择器专属参数 ---- + + # JSON 路点文件所在目录，包含如 Q1.json, Q2.json 等有序路点定义 + json_dir = LaunchConfiguration("json_dir") + + # 下发 NavigateToPose 导航目标时使用的坐标系 + goal_frame = LaunchConfiguration("goal_frame") + + # 全局 costmap 话题，用于判断候选路点是否被障碍物占据 + costmap_topic = LaunchConfiguration("costmap_topic") + + # Nav2 的 NavigateToPose action 名称 + navigate_action = LaunchConfiguration("navigate_action") + + # 占用阈值：costmap 栅格值 >= 此值的单元格视为被占用（范围 0-100） + occupied_threshold = LaunchConfiguration("occupied_threshold") + + # 是否将未知区域（costmap 值 == -1）视为占用 + treat_unknown_as_occupied = LaunchConfiguration("treat_unknown_as_occupied") + + # 单个候选目标的超时时间（秒），超时后取消当前目标并尝试下一个候选 + goal_timeout_sec = LaunchConfiguration("goal_timeout_sec") + + # 选择器的控制周期（秒），即多久重新评估一次最优候选路点 + selection_period_sec = LaunchConfiguration("selection_period_sec") + + # ========================================================================= + # 组件 1 —— obstacle_nav2 基础导航栈 + # 通过 IncludeLaunchDescription 引入 obstacle_nav2.launch.py， + # 将其作为一个子 launch 嵌入当前 launch 描述中。 + # 传入的参数会透传给子 launch 文件，实现参数的统一管理。 + # ========================================================================= + obstacle_nav2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource(base_launch), + launch_arguments={ + "use_sim_time": use_sim_time, + "global_frame": global_frame, + "use_static_map": use_static_map, + "map_yaml": map_yaml, + "enable_motion": enable_motion, + "start_base": start_base, + "start_lidar": start_lidar, + "start_obstacle_scanner": start_obstacle_scanner, + }.items(), + ) + + # ========================================================================= + # 组件 2 —— Q 路点候选选择器 + # 作为一个独立的 Python 进程运行（ExecuteProcess），而非 ROS 2 Node action。 + # 选择器内部会创建一个 rclpy 节点，订阅 costmap、发布导航目标。 + # 参数通过 ROS 2 标准的 --ros-args -p 方式传递。 + # ========================================================================= + q_waypoint_selector = ExecuteProcess( + cmd=[ + # 使用 FindExecutable 查找 python3 可执行文件，确保跨平台兼容 + FindExecutable(name="python3"), + selector_script, + # ROS 2 命令行参数传递标准格式 + "--ros-args", + "-p", ["json_dir:=", json_dir], + "-p", ["goal_frame:=", goal_frame], + "-p", ["costmap_topic:=", costmap_topic], + "-p", ["navigate_action:=", navigate_action], + "-p", ["occupied_threshold:=", occupied_threshold], + "-p", ["treat_unknown_as_occupied:=", treat_unknown_as_occupied], + "-p", ["goal_timeout_sec:=", goal_timeout_sec], + "-p", ["selection_period_sec:=", selection_period_sec], + "-p", ["use_sim_time:=", use_sim_time], + ], + # 将进程的标准输出打印到终端，便于调试 + output="screen", + emulate_tty=True, + ) + + # ========================================================================= + # LaunchDescription —— 组装并返回完整的 launch 描述 + # 包含三部分： + # 1. DeclareLaunchArgument —— 声明所有可配置参数及其默认值 + # 2. obstacle_nav2 —— 基础导航栈 + # 3. q_waypoint_selector —— Q 路点选择器 + # ========================================================================= + return LaunchDescription( + [ + # ================================================================= + # obstacle_nav2 基础导航栈参数声明 + # ================================================================= + + # 是否使用仿真时间（/clock 话题），实车场景应设为 false + DeclareLaunchArgument("use_sim_time", default_value="false"), + + # 全局坐标系：仿真用 "odom"，实车 EKF 融合后用 "odom_combined" + DeclareLaunchArgument("global_frame", default_value="odom"), + + # 是否启用静态地图层（map_server），在线 SLAM 建图时设为 false + DeclareLaunchArgument("use_static_map", default_value="false"), + + # 静态地图 yaml 文件路径，为空字符串时不加载地图 + DeclareLaunchArgument("map_yaml", default_value=""), + + # 是否启用底盘运动控制指令下发，调试时可关闭 + DeclareLaunchArgument("enable_motion", default_value="true"), + + # 是否启动底盘串口驱动节点（实车需要，仿真不需要） + DeclareLaunchArgument("start_base", default_value="true"), + + # 是否启动激光雷达驱动节点 + DeclareLaunchArgument("start_lidar", default_value="true"), + + # 是否启动障碍物扫描与避障节点 + DeclareLaunchArgument("start_obstacle_scanner", default_value="true"), + + # ================================================================= + # Q 路点选择器参数声明 + # ================================================================= + + # JSON 路点文件目录，每个 JSON 文件定义一个路点的候选坐标列表 + DeclareLaunchArgument( + "json_dir", + default_value=default_json_dir, + description="Directory containing ordered waypoint JSON files.", + ), + + # 导航目标使用的坐标帧，应与全局 costmap 的坐标系一致 + DeclareLaunchArgument( + "goal_frame", + default_value="odom", + description="Frame used for NavigateToPose goals.", + ), + + # 全局 costmap 话题，选择器订阅此话题来评估候选点的可通行性 + DeclareLaunchArgument( + "costmap_topic", + default_value="/global_costmap/costmap", + description="OccupancyGrid topic used to reject occupied candidates.", + ), + + # NavigateToPose action 的服务名，与 Nav2 的 bt_navigator 对应 + DeclareLaunchArgument( + "navigate_action", + default_value="/navigate_to_pose", + description="Nav2 NavigateToPose action name.", + ), + + # costmap 占用判定阈值（0-100），>= 此值的栅格视为障碍物 + DeclareLaunchArgument( + "occupied_threshold", + default_value="50", + description="Costmap cells >= this value are considered occupied.", + ), + + # 是否将未知区域（costmap 值 == -1）视为不可通行 + DeclareLaunchArgument( + "treat_unknown_as_occupied", + default_value="true", + description="Treat costmap value -1 as occupied.", + ), + + # 单个导航目标的超时时间，超时后自动取消并切换候选 + DeclareLaunchArgument( + "goal_timeout_sec", + default_value="60.0", + description="Cancel a candidate goal if it does not finish before this timeout.", + ), + + # 选择器主循环的控制周期（秒），值越小响应越快但 CPU 开销越大 + DeclareLaunchArgument( + "selection_period_sec", + default_value="0.5", + description="Selector control period.", + ), + + # ---- 组装已配置的 action ---- + obstacle_nav2, + q_waypoint_selector, + ] + ) diff --git a/src/navigation/obstacle_nav2/src/trajectory_guard.cpp b/src/navigation/obstacle_nav2/src/trajectory_guard.cpp new file mode 100644 index 0000000..ebadf80 --- /dev/null +++ b/src/navigation/obstacle_nav2/src/trajectory_guard.cpp @@ -0,0 +1,277 @@ +#include "obstacle_nav2/trajectory_guard.hpp" + +#include +#include +#include + +namespace obstacle_nav2 +{ +namespace +{ + +bool worldToMap( + const nav_msgs::msg::OccupancyGrid & grid, + double wx, + double wy, + int & mx, + int & my) +{ + const double resolution = grid.info.resolution; + if (resolution <= 0.0) { + return false; + } + + const double ox = grid.info.origin.position.x; + const double oy = grid.info.origin.position.y; + if (wx < ox || wy < oy) { + return false; + } + + mx = static_cast((wx - ox) / resolution); + my = static_cast((wy - oy) / resolution); + return mx >= 0 && my >= 0 && + mx < static_cast(grid.info.width) && + my < static_cast(grid.info.height); +} + +bool occupiedAt( + const nav_msgs::msg::OccupancyGrid & grid, + double wx, + double wy, + const GuardSettings & settings) +{ + int mx = 0; + int my = 0; + if (!worldToMap(grid, wx, wy, mx, my)) { + return settings.treat_unknown_as_occupied; + } + + const auto value = grid.data[my * grid.info.width + mx]; + if (value < 0) { + return settings.treat_unknown_as_occupied; + } + return value >= settings.occupied_threshold; +} + +bool footprintOccupied( + const nav_msgs::msg::OccupancyGrid & costmap, + double cx, + double cy, + double yaw, + const GuardSettings & settings) +{ + const double half_l = settings.footprint_half_length + settings.footprint_padding; + const double half_w = settings.footprint_half_width + settings.footprint_padding; + const double step = std::max(0.02, settings.footprint_sample_step); + const double c = std::cos(yaw); + const double s = std::sin(yaw); + + auto sample = [&](double x, double y) { + const double wx = cx + c * x - s * y; + const double wy = cy + s * x + c * y; + return occupiedAt(costmap, wx, wy, settings); + }; + + if (sample(0.0, 0.0)) { + return true; + } + for (double x = -half_l; x <= half_l + 1.0e-9; x += step) { + if (sample(x, -half_w) || sample(x, 0.0) || sample(x, half_w)) { + return true; + } + } + for (double y = -half_w; y <= half_w + 1.0e-9; y += step) { + if (sample(-half_l, y) || sample(0.0, y) || sample(half_l, y)) { + return true; + } + } + return false; +} + +} // namespace + +double yawFromPose(const geometry_msgs::msg::PoseStamped & pose) +{ + const auto & q = pose.pose.orientation; + return std::atan2( + 2.0 * (q.w * q.z + q.x * q.y), + 1.0 - 2.0 * (q.y * q.y + q.z * q.z)); +} + +double distance2d( + const geometry_msgs::msg::PoseStamped & a, + const geometry_msgs::msg::PoseStamped & b) +{ + const double dx = a.pose.position.x - b.pose.position.x; + const double dy = a.pose.position.y - b.pose.position.y; + return std::hypot(dx, dy); +} + +std::size_t nearestPathIndex( + const nav_msgs::msg::Path & path, + const geometry_msgs::msg::PoseStamped & pose) +{ + std::size_t best_index = 0; + double best_distance = std::numeric_limits::infinity(); + + for (std::size_t i = 0; i < path.poses.size(); ++i) { + const double d = distance2d(path.poses[i], pose); + if (d < best_distance) { + best_distance = d; + best_index = i; + } + } + return best_index; +} + +std::size_t advanceByDistance( + const nav_msgs::msg::Path & path, + std::size_t start_index, + double distance_m) +{ + if (path.poses.empty()) { + return 0; + } + + std::size_t index = std::min(start_index, path.poses.size() - 1); + double traveled = 0.0; + while (index + 1 < path.poses.size() && traveled < distance_m) { + traveled += distance2d(path.poses[index], path.poses[index + 1]); + ++index; + } + return index; +} + +CollisionCheckResult checkPathAhead( + const nav_msgs::msg::Path & path, + std::size_t start_index, + const nav_msgs::msg::OccupancyGrid & costmap, + const GuardSettings & settings) +{ + if (path.poses.empty()) { + return CollisionCheckResult{true, 0, "empty_path"}; + } + + const auto clamped_start = std::min(start_index, path.poses.size() - 1); + const auto end_index = + advanceByDistance(path, clamped_start, settings.lookahead_distance); + const double step = std::max(0.02, settings.footprint_sample_step); + + for (std::size_t i = clamped_start; i <= end_index && i < path.poses.size(); ++i) { + if (i + 1 >= path.poses.size() || i == end_index) { + const auto & pose = path.poses[i]; + if (footprintOccupied( + costmap, pose.pose.position.x, pose.pose.position.y, yawFromPose(pose), settings)) + { + return CollisionCheckResult{true, i, "occupied"}; + } + continue; + } + + const auto & a = path.poses[i]; + const auto & b = path.poses[i + 1]; + const double dx = b.pose.position.x - a.pose.position.x; + const double dy = b.pose.position.y - a.pose.position.y; + const double segment_length = std::hypot(dx, dy); + const double yaw = segment_length > 1.0e-6 ? std::atan2(dy, dx) : yawFromPose(a); + const int samples = std::max(1, static_cast(std::ceil(segment_length / step))); + + for (int sample_index = 0; sample_index <= samples; ++sample_index) { + const double t = static_cast(sample_index) / static_cast(samples); + const double x = a.pose.position.x + t * dx; + const double y = a.pose.position.y + t * dy; + if (footprintOccupied(costmap, x, y, yaw, settings)) { + return CollisionCheckResult{true, i, "occupied"}; + } + } + } + + return CollisionCheckResult{false, end_index, "clear"}; +} + +std::optional findClearRejoinIndex( + const nav_msgs::msg::Path & path, + std::size_t start_index, + const nav_msgs::msg::OccupancyGrid & costmap, + const GuardSettings & settings) +{ + if (path.poses.empty()) { + return std::nullopt; + } + + const auto first = advanceByDistance(path, start_index, settings.rejoin_min_distance); + const auto last = advanceByDistance(path, start_index, settings.rejoin_max_distance); + GuardSettings single_pose_settings = settings; + single_pose_settings.lookahead_distance = 0.0; + + for (std::size_t i = first; i <= last && i < path.poses.size(); ++i) { + const auto result = checkPathAhead(path, i, costmap, single_pose_settings); + if (!result.blocked) { + return i; + } + } + return std::nullopt; +} + +bool shouldRetryBlockedRepair( + const std::optional & last_repair_nearest_index, + std::size_t nearest_index, + int min_repair_progress_indices, + double seconds_since_last_repair, + double blocked_retry_wait_sec, + bool costmap_updated_since_repair) +{ + if (!last_repair_nearest_index.has_value()) { + return true; + } + + const auto min_progress = + static_cast(std::max(0, min_repair_progress_indices)); + if (nearest_index > *last_repair_nearest_index + min_progress) { + return true; + } + + return seconds_since_last_repair >= blocked_retry_wait_sec && + costmap_updated_since_repair; +} + +nav_msgs::msg::Path slicePath( + const nav_msgs::msg::Path & path, + std::size_t start_index, + std::size_t end_index_inclusive) +{ + nav_msgs::msg::Path out; + out.header = path.header; + if (path.poses.empty()) { + return out; + } + + const auto begin = std::min(start_index, path.poses.size() - 1); + const auto end = std::min(end_index_inclusive, path.poses.size() - 1); + for (std::size_t i = begin; i <= end; ++i) { + out.poses.push_back(path.poses[i]); + } + return out; +} + +nav_msgs::msg::Path stitchPaths( + const nav_msgs::msg::Path & bypass, + const nav_msgs::msg::Path & original, + std::size_t rejoin_index) +{ + nav_msgs::msg::Path out; + out.header = bypass.header.frame_id.empty() ? original.header : bypass.header; + out.poses = bypass.poses; + + if (original.poses.empty()) { + return out; + } + + const auto begin = std::min(rejoin_index, original.poses.size()); + for (std::size_t i = begin; i < original.poses.size(); ++i) { + out.poses.push_back(original.poses[i]); + } + return out; +} + +} // namespace obstacle_nav2 diff --git a/src/navigation/obstacle_nav2/src/trajectory_guard_node.cpp b/src/navigation/obstacle_nav2/src/trajectory_guard_node.cpp new file mode 100644 index 0000000..bfda08b --- /dev/null +++ b/src/navigation/obstacle_nav2/src/trajectory_guard_node.cpp @@ -0,0 +1,454 @@ +#include +#include +#include +#include +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "nav2_msgs/action/compute_path_to_pose.hpp" +#include "nav2_msgs/action/follow_path.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "nav_msgs/msg/path.hpp" +#include "obstacle_nav2/trajectory_guard.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +using namespace std::chrono_literals; + +namespace obstacle_nav2 +{ + +class TrajectoryGuardNode : public rclcpp::Node +{ +public: + using ComputePathToPose = nav2_msgs::action::ComputePathToPose; + using FollowPath = nav2_msgs::action::FollowPath; + using ComputeGoalHandle = rclcpp_action::ClientGoalHandle; + using FollowGoalHandle = rclcpp_action::ClientGoalHandle; + + TrajectoryGuardNode() + : Node("trajectory_guard_node") + { + declare_parameter("input_path_topic", "/trajectory_guard/input_path"); + declare_parameter("patched_path_topic", "/trajectory_guard/patched_path"); + declare_parameter("costmap_topic", "/local_costmap/costmap"); + declare_parameter("planner_costmap_topic", "/global_costmap/costmap"); + declare_parameter("odom_topic", "/odom_combined"); + declare_parameter("planner_action", "/compute_path_to_pose"); + declare_parameter("follow_action", "/follow_path"); + declare_parameter("planner_id", "GridBased"); + declare_parameter("controller_id", "FollowPath"); + declare_parameter("goal_checker_id", ""); + declare_parameter("check_period_sec", 0.5); + declare_parameter("lookahead_distance", 2.0); + declare_parameter("rejoin_min_distance", 1.0); + declare_parameter("rejoin_max_distance", 4.0); + declare_parameter("occupied_threshold", 50); + declare_parameter("treat_unknown_as_occupied", true); + declare_parameter("footprint_half_length", 0.14); + declare_parameter("footprint_half_width", 0.085); + declare_parameter("footprint_padding", 0.04); + declare_parameter("footprint_sample_step", 0.05); + declare_parameter("repair_cooldown_sec", 1.0); + declare_parameter("blocked_retry_wait_sec", 0.8); + declare_parameter("wait_for_planner_costmap_update", true); + declare_parameter("min_repair_progress_indices", 5); + declare_parameter("publish_zero_on_blocked", true); + declare_parameter("cmd_vel_topic", "/cmd_vel"); + declare_parameter("execute_follow_path", true); + + loadParameters(); + last_repair_request_time_ = + get_clock()->now() - rclcpp::Duration::from_seconds(repair_cooldown_sec_); + + path_sub_ = create_subscription( + get_parameter("input_path_topic").as_string(), 1, + [this](nav_msgs::msg::Path::SharedPtr msg) { onPath(msg); }); + costmap_sub_ = create_subscription( + get_parameter("costmap_topic").as_string(), 1, + [this](nav_msgs::msg::OccupancyGrid::SharedPtr msg) { + latest_costmap_ = msg; + ++local_costmap_generation_; + }); + planner_costmap_sub_ = create_subscription( + get_parameter("planner_costmap_topic").as_string(), 1, + [this](nav_msgs::msg::OccupancyGrid::SharedPtr msg) { + latest_planner_costmap_ = msg; + ++planner_costmap_generation_; + }); + odom_sub_ = create_subscription( + get_parameter("odom_topic").as_string(), 10, + [this](nav_msgs::msg::Odometry::SharedPtr msg) { latest_odom_ = msg; }); + + patched_path_pub_ = create_publisher( + get_parameter("patched_path_topic").as_string(), 1); + cmd_vel_pub_ = create_publisher( + get_parameter("cmd_vel_topic").as_string(), 1); + + planner_client_ = rclcpp_action::create_client( + this, get_parameter("planner_action").as_string()); + follow_client_ = rclcpp_action::create_client( + this, get_parameter("follow_action").as_string()); + + const auto period = std::chrono::duration( + std::max(0.1, get_parameter("check_period_sec").as_double())); + timer_ = create_wall_timer( + std::chrono::duration_cast(period), + [this]() { tick(); }); + + RCLCPP_INFO( + get_logger(), "trajectory guard ready check_period_sec=%.2f input=%s patched=%s", + get_parameter("check_period_sec").as_double(), + get_parameter("input_path_topic").as_string().c_str(), + get_parameter("patched_path_topic").as_string().c_str()); + } + +private: + void loadParameters() + { + settings_.lookahead_distance = get_parameter("lookahead_distance").as_double(); + settings_.rejoin_min_distance = get_parameter("rejoin_min_distance").as_double(); + settings_.rejoin_max_distance = get_parameter("rejoin_max_distance").as_double(); + settings_.occupied_threshold = get_parameter("occupied_threshold").as_int(); + settings_.treat_unknown_as_occupied = + get_parameter("treat_unknown_as_occupied").as_bool(); + settings_.footprint_half_length = get_parameter("footprint_half_length").as_double(); + settings_.footprint_half_width = get_parameter("footprint_half_width").as_double(); + settings_.footprint_padding = get_parameter("footprint_padding").as_double(); + settings_.footprint_sample_step = get_parameter("footprint_sample_step").as_double(); + repair_cooldown_sec_ = std::max(0.0, get_parameter("repair_cooldown_sec").as_double()); + blocked_retry_wait_sec_ = + std::max(0.0, get_parameter("blocked_retry_wait_sec").as_double()); + wait_for_planner_costmap_update_ = + get_parameter("wait_for_planner_costmap_update").as_bool(); + min_repair_progress_indices_ = + std::max(0, static_cast(get_parameter("min_repair_progress_indices").as_int())); + publish_zero_on_blocked_ = get_parameter("publish_zero_on_blocked").as_bool(); + execute_follow_path_ = get_parameter("execute_follow_path").as_bool(); + } + + void onPath(const nav_msgs::msg::Path::SharedPtr msg) + { + active_path_ = *msg; + sent_original_for_current_path_ = false; + last_repair_nearest_index_.reset(); + last_repair_rejoin_index_.reset(); + retry_without_planner_costmap_update_ = false; + waiting_for_planner_costmap_update_ = false; + RCLCPP_INFO( + get_logger(), "received replay path poses=%zu frame=%s", + active_path_.poses.size(), active_path_.header.frame_id.c_str()); + } + + std::optional currentPose() const + { + if (!latest_odom_) { + return std::nullopt; + } + geometry_msgs::msg::PoseStamped pose; + pose.header = latest_odom_->header; + pose.pose = latest_odom_->pose.pose; + return pose; + } + + void tick() + { + if (active_path_.poses.empty() || !latest_costmap_) { + return; + } + + const auto pose = currentPose(); + if (!pose.has_value()) { + return; + } + + const auto nearest = nearestPathIndex(active_path_, *pose); + const auto result = checkPathAhead(active_path_, nearest, *latest_costmap_, settings_); + if (!result.blocked) { + if (!sent_original_for_current_path_) { + publishAndMaybeFollow(active_path_, "original_clear"); + sent_original_for_current_path_ = true; + } + return; + } + + const auto now = get_clock()->now(); + if (repair_in_flight_ || + (now - last_repair_request_time_).seconds() < repair_cooldown_sec_) + { + return; + } + + if (!waiting_for_planner_costmap_update_) { + waiting_for_planner_costmap_update_ = true; + waiting_planner_costmap_generation_ = planner_costmap_generation_; + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 2000, + "blocked replay path; waiting for next planner costmap frame before requesting bypass"); + publishStop(); + return; + } + + if (planner_costmap_generation_ <= waiting_planner_costmap_generation_) { + if (retry_without_planner_costmap_update_ && + (now - last_repair_request_time_).seconds() >= blocked_retry_wait_sec_) + { + waiting_for_planner_costmap_update_ = false; + } else { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 2000, + "blocked replay path; planner costmap has not updated yet"); + publishStop(); + return; + } + } + + waiting_for_planner_costmap_update_ = false; + + const bool costmap_updated_since_repair = + !wait_for_planner_costmap_update_ || + planner_costmap_generation_ > last_repair_planner_costmap_generation_; + const bool timeout_retry_allowed = + retry_without_planner_costmap_update_ && + (now - last_repair_request_time_).seconds() >= blocked_retry_wait_sec_; + if (!shouldRetryBlockedRepair( + last_repair_nearest_index_, nearest, min_repair_progress_indices_, + (now - last_repair_request_time_).seconds(), blocked_retry_wait_sec_, + costmap_updated_since_repair) && !timeout_retry_allowed) + { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 2000, + "patched path still blocked near the last repair point; waiting for path progress or costmap update"); + publishStop(); + return; + } + + retry_without_planner_costmap_update_ = false; + + std::size_t search_start = nearest; + if (last_repair_rejoin_index_.has_value()) { + const auto bump = static_cast(std::max(1, min_repair_progress_indices_)); + const auto advanced = std::min( + active_path_.poses.size() - 1, *last_repair_rejoin_index_ + bump); + search_start = std::max(search_start, advanced); + } + + const auto rejoin = findClearRejoinIndex( + active_path_, search_start, *latest_costmap_, settings_); + if (!rejoin.has_value()) { + RCLCPP_WARN(get_logger(), "blocked replay path but no clear rejoin point found"); + publishStop(); + return; + } + + RCLCPP_WARN( + get_logger(), "blocked replay path index=%zu reason=%s rejoin_index=%zu", + result.path_index, result.reason.c_str(), *rejoin); + requestBypass(nearest, *rejoin); + } + + void publishAndMaybeFollow(const nav_msgs::msg::Path & path, const std::string & label) + { + if (path.poses.empty()) { + return; + } + + nav_msgs::msg::Path stamped_path = path; + stamped_path.header.stamp = now(); + for (auto & pose : stamped_path.poses) { + pose.header.stamp = stamped_path.header.stamp; + if (pose.header.frame_id.empty()) { + pose.header.frame_id = stamped_path.header.frame_id; + } + } + patched_path_pub_->publish(stamped_path); + + if (!execute_follow_path_) { + RCLCPP_INFO( + get_logger(), "published %s path poses=%zu without FollowPath execution", + label.c_str(), stamped_path.poses.size()); + return; + } + + sendFollowPath(stamped_path, label); + } + + void sendFollowPath(const nav_msgs::msg::Path & path, const std::string & label) + { + if (!follow_client_->wait_for_action_server(100ms)) { + RCLCPP_WARN(get_logger(), "FollowPath action server not ready"); + return; + } + + if (active_follow_goal_handle_) { + follow_client_->async_cancel_goal(active_follow_goal_handle_); + active_follow_goal_handle_.reset(); + } + + FollowPath::Goal goal; + goal.path = path; + goal.controller_id = get_parameter("controller_id").as_string(); + goal.goal_checker_id = get_parameter("goal_checker_id").as_string(); + + auto options = rclcpp_action::Client::SendGoalOptions(); + options.goal_response_callback = + [this, label](FollowGoalHandle::SharedPtr goal_handle) { + if (!goal_handle) { + RCLCPP_WARN(get_logger(), "FollowPath %s rejected", label.c_str()); + return; + } + active_follow_goal_handle_ = goal_handle; + RCLCPP_INFO(get_logger(), "FollowPath %s accepted", label.c_str()); + }; + options.result_callback = + [this, label](const FollowGoalHandle::WrappedResult & result) { + active_follow_goal_handle_.reset(); + RCLCPP_INFO( + get_logger(), "FollowPath %s finished code=%d", + label.c_str(), static_cast(result.code)); + }; + + follow_client_->async_send_goal(goal, options); + RCLCPP_INFO(get_logger(), "sent FollowPath %s poses=%zu", label.c_str(), path.poses.size()); + } + + void requestBypass(std::size_t nearest_index, std::size_t rejoin_index) + { + const auto start = currentPose(); + if (!start.has_value() || rejoin_index >= active_path_.poses.size()) { + return; + } + + if (!planner_client_->wait_for_action_server(100ms)) { + RCLCPP_WARN(get_logger(), "ComputePathToPose action server not ready"); + publishStop(); + return; + } + + repair_in_flight_ = true; + last_repair_request_time_ = get_clock()->now(); + last_repair_nearest_index_ = nearest_index; + last_repair_rejoin_index_ = rejoin_index; + last_repair_planner_costmap_generation_ = planner_costmap_generation_; + waiting_for_planner_costmap_update_ = false; + + ComputePathToPose::Goal goal; + goal.start = *start; + goal.goal = active_path_.poses[rejoin_index]; + goal.planner_id = get_parameter("planner_id").as_string(); + goal.use_start = true; + + auto options = rclcpp_action::Client::SendGoalOptions(); + options.goal_response_callback = + [this](ComputeGoalHandle::SharedPtr goal_handle) { + if (!goal_handle) { + repair_in_flight_ = false; + RCLCPP_WARN(get_logger(), "bypass planning goal rejected"); + } + }; + options.result_callback = + [this, rejoin_index](const ComputeGoalHandle::WrappedResult & result) { + repair_in_flight_ = false; + if (result.code != rclcpp_action::ResultCode::SUCCEEDED || + result.result->path.poses.empty()) + { + RCLCPP_WARN(get_logger(), "bypass planning failed code=%d", static_cast(result.code)); + publishStop(); + return; + } + + const auto patched = stitchPaths(result.result->path, active_path_, rejoin_index); + if (!patchedPathIsClear(patched)) { + retry_without_planner_costmap_update_ = true; + waiting_for_planner_costmap_update_ = false; + RCLCPP_WARN( + get_logger(), + "planned bypass still blocked in local costmap; stopping and retrying from a later rejoin point"); + publishStop(); + return; + } + + active_path_ = patched; + sent_original_for_current_path_ = true; + waiting_for_planner_costmap_update_ = false; + retry_without_planner_costmap_update_ = false; + publishAndMaybeFollow(patched, "patched_bypass"); + }; + + planner_client_->async_send_goal(goal, options); + RCLCPP_INFO(get_logger(), "requested bypass to rejoin_index=%zu", rejoin_index); + } + + bool patchedPathIsClear(const nav_msgs::msg::Path & patched) const + { + if (!latest_costmap_) { + return false; + } + + const auto pose = currentPose(); + if (!pose.has_value()) { + return false; + } + + const auto nearest = nearestPathIndex(patched, *pose); + const auto result = checkPathAhead(patched, nearest, *latest_costmap_, settings_); + return !result.blocked; + } + + void publishStop() + { + if (!publish_zero_on_blocked_) { + return; + } + geometry_msgs::msg::Twist stop; + cmd_vel_pub_->publish(stop); + } + + GuardSettings settings_; + double repair_cooldown_sec_{1.0}; + double blocked_retry_wait_sec_{0.8}; + int min_repair_progress_indices_{5}; + bool publish_zero_on_blocked_{true}; + bool execute_follow_path_{true}; + bool wait_for_planner_costmap_update_{true}; + bool retry_without_planner_costmap_update_{false}; + bool repair_in_flight_{false}; + bool sent_original_for_current_path_{false}; + bool waiting_for_planner_costmap_update_{false}; + rclcpp::Time last_repair_request_time_{0, 0, RCL_ROS_TIME}; + std::optional last_repair_nearest_index_; + std::optional last_repair_rejoin_index_; + std::uint64_t local_costmap_generation_{0}; + std::uint64_t planner_costmap_generation_{0}; + std::uint64_t last_repair_planner_costmap_generation_{0}; + std::uint64_t waiting_planner_costmap_generation_{0}; + + nav_msgs::msg::Path active_path_; + nav_msgs::msg::OccupancyGrid::SharedPtr latest_costmap_; + nav_msgs::msg::OccupancyGrid::SharedPtr latest_planner_costmap_; + nav_msgs::msg::Odometry::SharedPtr latest_odom_; + FollowGoalHandle::SharedPtr active_follow_goal_handle_; + + rclcpp::Subscription::SharedPtr path_sub_; + rclcpp::Subscription::SharedPtr costmap_sub_; + rclcpp::Subscription::SharedPtr planner_costmap_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Publisher::SharedPtr patched_path_pub_; + rclcpp::Publisher::SharedPtr cmd_vel_pub_; + rclcpp_action::Client::SharedPtr planner_client_; + rclcpp_action::Client::SharedPtr follow_client_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +} // namespace obstacle_nav2 + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/navigation/obstacle_nav2/test/test_trajectory_guard.cpp b/src/navigation/obstacle_nav2/test/test_trajectory_guard.cpp new file mode 100644 index 0000000..3f68b34 --- /dev/null +++ b/src/navigation/obstacle_nav2/test/test_trajectory_guard.cpp @@ -0,0 +1,162 @@ +#include + +#include "obstacle_nav2/trajectory_guard.hpp" + +namespace +{ + +geometry_msgs::msg::PoseStamped pose(double x, double y) +{ + geometry_msgs::msg::PoseStamped p; + p.header.frame_id = "odom"; + p.pose.position.x = x; + p.pose.position.y = y; + p.pose.orientation.w = 1.0; + return p; +} + +nav_msgs::msg::Path straightPath() +{ + nav_msgs::msg::Path path; + path.header.frame_id = "odom"; + for (int i = 0; i <= 10; ++i) { + path.poses.push_back(pose(0.2 * i, 0.0)); + } + return path; +} + +nav_msgs::msg::OccupancyGrid gridWithOrigin(double origin_x, double origin_y) +{ + nav_msgs::msg::OccupancyGrid grid; + grid.header.frame_id = "odom"; + grid.info.resolution = 0.1; + grid.info.width = 40; + grid.info.height = 30; + grid.info.origin.position.x = origin_x; + grid.info.origin.position.y = origin_y; + grid.data.assign(grid.info.width * grid.info.height, 0); + return grid; +} + +void markOccupied(nav_msgs::msg::OccupancyGrid & grid, int mx, int my) +{ + ASSERT_GE(mx, 0); + ASSERT_GE(my, 0); + ASSERT_LT(mx, static_cast(grid.info.width)); + ASSERT_LT(my, static_cast(grid.info.height)); + grid.data[my * grid.info.width + mx] = 100; +} + +} // namespace + +TEST(TrajectoryGuard, NearestPathIndexFindsClosestPose) +{ + const auto path = straightPath(); + EXPECT_EQ(obstacle_nav2::nearestPathIndex(path, pose(0.43, 0.02)), 2u); +} + +TEST(TrajectoryGuard, AdvanceByDistanceStopsAtRequestedArcLength) +{ + const auto path = straightPath(); + EXPECT_EQ(obstacle_nav2::advanceByDistance(path, 0, 0.55), 3u); +} + +TEST(TrajectoryGuard, SlicePathIncludesEndIndex) +{ + const auto path = straightPath(); + const auto sliced = obstacle_nav2::slicePath(path, 2, 4); + ASSERT_EQ(sliced.poses.size(), 3u); + EXPECT_DOUBLE_EQ(sliced.poses.front().pose.position.x, 0.4); + EXPECT_DOUBLE_EQ(sliced.poses.back().pose.position.x, 0.8); +} + +TEST(TrajectoryGuard, StitchPathsAppendsOriginalFromRejoinIndex) +{ + const auto original = straightPath(); + nav_msgs::msg::Path bypass; + bypass.header.frame_id = "odom"; + bypass.poses.push_back(pose(0.0, 0.0)); + bypass.poses.push_back(pose(0.5, 0.3)); + + const auto stitched = obstacle_nav2::stitchPaths(bypass, original, 4); + ASSERT_EQ(stitched.poses.size(), 9u); + EXPECT_DOUBLE_EQ(stitched.poses[0].pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(stitched.poses[1].pose.position.y, 0.3); + EXPECT_DOUBLE_EQ(stitched.poses[2].pose.position.x, 0.8); +} + +TEST(TrajectoryGuard, CheckPathAheadReportsOccupiedCell) +{ + auto path = straightPath(); + auto grid = gridWithOrigin(-1.0, -1.0); + markOccupied(grid, 15, 10); + + obstacle_nav2::GuardSettings settings; + settings.lookahead_distance = 2.0; + settings.occupied_threshold = 50; + settings.footprint_half_length = 0.01; + settings.footprint_half_width = 0.01; + settings.footprint_padding = 0.0; + + const auto result = obstacle_nav2::checkPathAhead(path, 0, grid, settings); + EXPECT_TRUE(result.blocked); + EXPECT_EQ(result.reason, "occupied"); +} + +TEST(TrajectoryGuard, CheckPathAheadKeepsClearPathUnblocked) +{ + auto path = straightPath(); + auto grid = gridWithOrigin(-1.0, -1.0); + + obstacle_nav2::GuardSettings settings; + settings.lookahead_distance = 2.0; + settings.occupied_threshold = 50; + settings.footprint_half_length = 0.01; + settings.footprint_half_width = 0.01; + settings.footprint_padding = 0.0; + + const auto result = obstacle_nav2::checkPathAhead(path, 0, grid, settings); + EXPECT_FALSE(result.blocked); + EXPECT_EQ(result.reason, "clear"); +} + +TEST(TrajectoryGuard, FindClearRejoinIndexSkipsBlockedArea) +{ + auto path = straightPath(); + auto grid = gridWithOrigin(-1.0, -1.0); + for (int mx = 13; mx <= 18; ++mx) { + markOccupied(grid, mx, 10); + } + + obstacle_nav2::GuardSettings settings; + settings.rejoin_min_distance = 0.5; + settings.rejoin_max_distance = 2.0; + settings.occupied_threshold = 50; + settings.footprint_half_length = 0.01; + settings.footprint_half_width = 0.01; + settings.footprint_padding = 0.0; + + const auto rejoin = obstacle_nav2::findClearRejoinIndex(path, 0, grid, settings); + ASSERT_TRUE(rejoin.has_value()); + EXPECT_GE(*rejoin, 5u); +} + +TEST(TrajectoryGuard, RetryBlockedRepairWaitsForCostmapUpdateAndRetryDelay) +{ + const std::optional last_repair_index = 10u; + + EXPECT_FALSE(obstacle_nav2::shouldRetryBlockedRepair( + last_repair_index, 11u, 5, 0.8, 1.0, true)); + EXPECT_FALSE(obstacle_nav2::shouldRetryBlockedRepair( + last_repair_index, 11u, 5, 1.2, 1.0, false)); + EXPECT_TRUE(obstacle_nav2::shouldRetryBlockedRepair( + last_repair_index, 11u, 5, 1.2, 1.0, true)); +} + +TEST(TrajectoryGuard, RetryBlockedRepairAllowsProgressedPathImmediately) +{ + const std::optional last_repair_index = 10u; + + EXPECT_TRUE(obstacle_nav2::shouldRetryBlockedRepair( + last_repair_index, 16u, 5, 0.1, 1.0, false)); +}