diff --git a/src/gc/项目总结_yiliao_ws.md b/src/gc/项目总结_yiliao_ws.md index f5e90a2..5e30e7d 100644 --- a/src/gc/项目总结_yiliao_ws.md +++ b/src/gc/项目总结_yiliao_ws.md @@ -12,7 +12,7 @@ ## 一、项目定位 本项目是 RDK X5 机器人上的 ROS 2 Humble 工作区，服务于第21届全国大学生智能汽车竞赛的**医疗赛道**（智慧医疗）。整体采用**调度型架构**：底盘、雷达、相机、二维码、Nav2 导航、VLM 图生文、TTS 语音等功能由独立模块完成，`racing_control` 包作为总调度协调各模块完成比赛流程。 - + --- ## 二、项目目录结构总览 diff --git a/src/map/nav2_costmap_binary.png b/src/map/nav2_costmap_binary 03.png similarity index 100% rename from src/map/nav2_costmap_binary.png rename to src/map/nav2_costmap_binary 03.png diff --git a/src/map/nav2_costmap_binary 04.png b/src/map/nav2_costmap_binary 04.png new file mode 100644 index 0000000..f1ae4ea Binary files /dev/null and b/src/map/nav2_costmap_binary 04.png differ diff --git a/src/map/nav2_costmap_binary 05.png b/src/map/nav2_costmap_binary 05.png new file mode 100644 index 0000000..20ebb61 Binary files /dev/null and b/src/map/nav2_costmap_binary 05.png differ diff --git a/src/map/nav2_costmap_binary 01.png b/src/map/nav2_costmap_binary_01.png similarity index 100% rename from src/map/nav2_costmap_binary 01.png rename to src/map/nav2_costmap_binary_01.png diff --git a/src/map/nav2_costmap_binary_06.png b/src/map/nav2_costmap_binary_06.png new file mode 100644 index 0000000..0a93bb8 Binary files /dev/null and b/src/map/nav2_costmap_binary_06.png differ diff --git a/src/map/nav2_costmap_map.yaml b/src/map/nav2_costmap_map.yaml index 1f1edcf..da2e8f2 100644 --- a/src/map/nav2_costmap_map.yaml +++ b/src/map/nav2_costmap_map.yaml @@ -1,4 +1,4 @@ -image: nav2_costmap_binary.png +image: nav2_costmap_binary_01.png mode: trinary resolution: 0.01 origin: [0.0, 0.0, 0.0] diff --git a/src/navigation/obstacle_nav2/config/nav2_profile_10 copy.yaml.20260811 b/src/navigation/obstacle_nav2/config/nav2_profile_10 copy.yaml.20260811 new file mode 100644 index 0000000..dd59fc3 --- /dev/null +++ b/src/navigation/obstacle_nav2/config/nav2_profile_10 copy.yaml.20260811 @@ -0,0 +1,340 @@ +# ============================================================================ +# 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: map + 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: 15.0 + FollowPath: + plugin: "nav2_mppi_controller::MPPIController" + time_steps: 40 + model_dt: 0.06666666666666666 + batch_size: 900 + vx_std: 0.24 + vy_std: 0.0 + wz_std: 0.48 + vx_max: 1.00 + 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.6 + 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: 11.0 + threshold_to_consider: 0.5 + CostCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 + critical_cost: 300.0 + consider_footprint: true + collision_cost: 100000.0 + near_goal_distance: 1.0 + trajectory_point_step: 2 + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 11.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: 10 + threshold_to_consider: 1.4 + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + offset_from_furthest: 5 + 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.20 + 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: map + robot_base_frame: base_footprint + use_sim_time: False + rolling_window: false + width: 8 + height: 8 + 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: true + plugins: ["static_layer", "obstacle_array_layer", "inflation_layer"] + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + enabled: true + map_subscribe_transient_local: true + subscribe_to_updates: false + 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.35 + 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.15 + allow_unknown: false + max_iterations: 1000000 + max_on_approach_iterations: 1000 + max_planning_time: 25.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.45 + reverse_penalty: 4.0 + change_penalty: 1.2 + non_straight_penalty: 0.7 + cost_penalty: 3.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: 700 + 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","wait","backup"] + 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: map + 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.00, 0.0, 2.0] + min_velocity: [-0.75, 0.0, -2.0] + max_accel: [3.73, 0.0, 3.2] + max_decel: [-1.1, 0.0, -4.5] + 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_10.yaml b/src/navigation/obstacle_nav2/config/nav2_profile_10.yaml index 3015acc..f693e05 100644 --- a/src/navigation/obstacle_nav2/config/nav2_profile_10.yaml +++ b/src/navigation/obstacle_nav2/config/nav2_profile_10.yaml @@ -71,13 +71,13 @@ bt_navigator_rclcpp_node: controller_server: ros__parameters: use_sim_time: False - controller_frequency: 15.0 + controller_frequency: 10.0 FollowPath: plugin: "nav2_mppi_controller::MPPIController" - time_steps: 40 - model_dt: 0.06666666666666666 - batch_size: 900 - vx_std: 0.25 + time_steps: 36 + model_dt: 0.10 + batch_size: 700 + vx_std: 0.22 vy_std: 0.0 wz_std: 0.45 vx_max: 1.00 @@ -112,12 +112,12 @@ controller_server: PreferForwardCritic: enabled: false cost_power: 1 - cost_weight: 9.0 + cost_weight: 4.0 threshold_to_consider: 0.5 CostCritic: enabled: true cost_power: 1 - cost_weight: 5.0 + cost_weight: 3.81 critical_cost: 300.0 consider_footprint: true collision_cost: 100000.0 @@ -135,7 +135,7 @@ controller_server: PathFollowCritic: enabled: true cost_power: 1 - cost_weight: 5.0 + cost_weight: 4.0 offset_from_furthest: 10 threshold_to_consider: 1.4 PathAngleCritic: @@ -181,7 +181,7 @@ local_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 - inflation_radius: 0.20 + inflation_radius: 0.35 always_send_full_costmap: True local_costmap_client: ros__parameters: @@ -224,8 +224,8 @@ global_costmap: extra_inflation: 0.02 inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 0.35 + cost_scaling_factor: 2.0 + inflation_radius: 0.3 always_send_full_costmap: True global_costmap_client: ros__parameters: @@ -246,16 +246,16 @@ planner_server: allow_unknown: false max_iterations: 1000000 max_on_approach_iterations: 1000 - max_planning_time: 25.0 + 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.45 - reverse_penalty: 4.0 - change_penalty: 1.0 - non_straight_penalty: 1.0 - cost_penalty: 3.0 + minimum_turning_radius: 0.40 + reverse_penalty: 3.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. @@ -264,7 +264,7 @@ planner_server: viz_expansions: false smooth_path: True smoother: - max_iterations: 700 + max_iterations: 1000 w_smooth: 0.3 w_data: 0.2 tolerance: 1.0e-10 diff --git a/src/racing_control/CMakeLists.txt b/src/racing_control/CMakeLists.txt index c277d47..d865264 100644 --- a/src/racing_control/CMakeLists.txt +++ b/src/racing_control/CMakeLists.txt @@ -1,6 +1,9 @@ cmake_minimum_required(VERSION 3.8) project(racing_control) +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() @@ -15,8 +18,21 @@ find_package(rclcpp_action REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) +add_library(racing_control_core + src/candidate_waypoint_selector.cpp +) +target_include_directories(racing_control_core PUBLIC + $ + $ +) +ament_target_dependencies(racing_control_core + geometry_msgs + nav_msgs +) + add_executable(racing_control src/racing_control.cpp + src/candidate_waypoint_selector.cpp ) target_include_directories(racing_control PUBLIC $ @@ -33,6 +49,7 @@ ament_target_dependencies(racing_control ) install(TARGETS + racing_control_core racing_control DESTINATION lib/${PROJECT_NAME} ) @@ -63,8 +80,12 @@ if(BUILD_TESTING) ) ament_target_dependencies(test_racing_control_helpers geometry_msgs + nav_msgs rclcpp ) + target_sources(test_racing_control_helpers PRIVATE + src/candidate_waypoint_selector.cpp + ) ament_lint_auto_find_test_dependencies() endif() diff --git a/src/racing_control/config/racing_control.yaml b/src/racing_control/config/racing_control.yaml index f8e1422..e84081c 100644 --- a/src/racing_control/config/racing_control.yaml +++ b/src/racing_control/config/racing_control.yaml @@ -8,6 +8,7 @@ racing_control: enable_vlm_image_relay: false enable_dynamic_replanning: false enable_recovery: true + enable_candidate_waypoint_selection: true vlm_image_input_topic: /image vlm_image_output_topic: /vlm_image @@ -17,6 +18,7 @@ racing_control: vlm_result_topic: /vlm_result odom_topic: /odom_combined recovery_cmd_vel_topic: /cmd_vel + global_costmap_topic: /global_costmap/costmap trajectory_guard_input_topic: /trajectory_guard/input_path # Nav2 actions and plugin IDs @@ -42,6 +44,15 @@ racing_control: recovery_backup_speed: -0.2 recovery_backup_distance: 0.04 recovery_backup_timeout_sec: 0.2 + # Used only when the initial ComputePathThroughPoses request fails. + planning_failure_backup_speed: -0.1 + planning_failure_backup_distance: 0.03 + planning_failure_backup_timeout_sec: 0.3 + recovery_clear_wait_sec: 1.0 + recovery_search_radius_m: 1.0 + recovery_rear_clear_distance_m: 0.5 + global_costmap_clear_service: global_costmap/clear_entirely_global_costmap + local_costmap_clear_service: local_costmap/clear_entirely_local_costmap max_recovery_attempts: 2 circle_goal_tolerance: 0.50 @@ -57,6 +68,17 @@ racing_control: sign_vlm_trigger: 9 sign_profile_normal: 10 sign_profile_task2: 11 + vlm_trigger_repeat_count: 5 + vlm_trigger_interval_sec: 0.5 + + # Candidate waypoint JSONs reuse the saved-point format. + # home is intentionally never replaced. + candidate_waypoint_json_dir: /home/sunrise/yiliao_ws/src/racing_control/config/waypoints + qr_candidate_group_name: qr + entry_candidate_group_name: entry + vlm_candidate_group_name: goal_005 + clockwise_candidate_source_file: main_1.json + counterclockwise_candidate_source_file: main_2.json # Pose parameters are flat x/y/yaw-radians triples in frame_id. qr_pose: [4.333107888975101, 1.028867429995691, 1.1383894869544392] diff --git a/src/racing_control/config/waypoints/main.json b/src/racing_control/config/waypoints/main.json new file mode 100644 index 0000000..c8be5aa --- /dev/null +++ b/src/racing_control/config/waypoints/main.json @@ -0,0 +1,106 @@ +{ + "topic": "/odom_combined", + "message_type": "nav_msgs/msg/Odometry", + "saved_at": "2026-08-07T14:42:49.402Z", + "count": 2, + "points": [ + { + "name": "qr", + "captured_at": "2026-08-07T14:42:33.118Z", + "received_at": null, + "yaw_degrees": 65.22491304455252, + "odom": { + "header": { + "frame_id": "odom", + "stamp": { + "sec": 0, + "nanosec": 0 + }, + "stamp_iso": null + }, + "child_frame_id": "base_link", + "pose": { + "pose": { + "position": { + "x": 4.333107888975101, + "y": 1.028867429995691, + "z": 0 + }, + "orientation": { + "x": 0, + "y": 0, + "z": 0.5389539275964809, + "w": 0.8423352443821446 + } + }, + "covariance": [] + }, + "twist": { + "twist": { + "linear": { + "x": 0, + "y": 0, + "z": 0 + }, + "angular": { + "x": 0, + "y": 0, + "z": 0 + } + }, + "covariance": [] + }, + "yaw_degrees": 65.22491304455252 + } + }, + { + "name": "entry", + "captured_at": "2026-08-07T14:42:45.961Z", + "received_at": null, + "yaw_degrees": 87.09617569507752, + "odom": { + "header": { + "frame_id": "odom", + "stamp": { + "sec": 0, + "nanosec": 0 + }, + "stamp_iso": null + }, + "child_frame_id": "base_link", + "pose": { + "pose": { + "position": { + "x": 2.504811372926262, + "y": 2.2798075935366624, + "z": 0 + }, + "orientation": { + "x": 0, + "y": 0, + "z": 0.6889631335573172, + "w": 0.7247963856138373 + } + }, + "covariance": [] + }, + "twist": { + "twist": { + "linear": { + "x": 0, + "y": 0, + "z": 0 + }, + "angular": { + "x": 0, + "y": 0, + "z": 0 + } + }, + "covariance": [] + }, + "yaw_degrees": 87.09617569507752 + } + } + ] +} diff --git a/src/racing_control/config/waypoints/main_1.json b/src/racing_control/config/waypoints/main_1.json new file mode 100644 index 0000000..401f226 --- /dev/null +++ b/src/racing_control/config/waypoints/main_1.json @@ -0,0 +1,155 @@ +{ + "topic": "/odom_combined", + "message_type": "nav_msgs/msg/Odometry", + "saved_at": "2026-08-07T14:44:10.775Z", + "count": 3, + "points": [ + { + "name": "goal_004", + "captured_at": "2026-08-07T14:43:55.328Z", + "received_at": null, + "yaw_degrees": 89.33764149250207, + "odom": { + "header": { + "frame_id": "odom", + "stamp": { + "sec": 0, + "nanosec": 0 + }, + "stamp_iso": null + }, + "child_frame_id": "base_link", + "pose": { + "pose": { + "position": { + "x": 0.7534956931109804, + "y": 3.766501234923627, + "z": 0 + }, + "orientation": { + "x": 0, + "y": 0, + "z": 0.7030077953706314, + "w": 0.7111821423855668 + } + }, + "covariance": [] + }, + "twist": { + "twist": { + "linear": { + "x": 0, + "y": 0, + "z": 0 + }, + "angular": { + "x": 0, + "y": 0, + "z": 0 + } + }, + "covariance": [] + }, + "yaw_degrees": 89.33764149250207 + } + }, + { + "name": "goal_005", + "captured_at": "2026-08-07T14:44:02.099Z", + "received_at": null, + "yaw_degrees": 2.2906028734862383, + "odom": { + "header": { + "frame_id": "odom", + "stamp": { + "sec": 0, + "nanosec": 0 + }, + "stamp_iso": null + }, + "child_frame_id": "base_link", + "pose": { + "pose": { + "position": { + "x": 3.8567885105264077, + "y": 4.329424195458402, + "z": 0 + }, + "orientation": { + "x": 0, + "y": 0, + "z": 0.019987949834902125, + "w": 0.9998002209748693 + } + }, + "covariance": [] + }, + "twist": { + "twist": { + "linear": { + "x": 0, + "y": 0, + "z": 0 + }, + "angular": { + "x": 0, + "y": 0, + "z": 0 + } + }, + "covariance": [] + }, + "yaw_degrees": 2.2906028734862383 + } + }, + { + "name": "goal_006", + "captured_at": "2026-08-07T14:44:09.250Z", + "received_at": null, + "yaw_degrees": -92.25458353863968, + "odom": { + "header": { + "frame_id": "odom", + "stamp": { + "sec": 0, + "nanosec": 0 + }, + "stamp_iso": null + }, + "child_frame_id": "base_link", + "pose": { + "pose": { + "position": { + "x": 2.5144339572664096, + "y": 2.4337692660037766, + "z": 0 + }, + "orientation": { + "x": 0, + "y": 0, + "z": -0.720881318872454, + "w": 0.6930585286256215 + } + }, + "covariance": [] + }, + "twist": { + "twist": { + "linear": { + "x": 0, + "y": 0, + "z": 0 + }, + "angular": { + "x": 0, + "y": 0, + "z": 0 + } + }, + "covariance": [] + }, + "yaw_degrees": -92.25458353863968 + } + } + ] +} diff --git a/src/racing_control/config/waypoints/main_2.json b/src/racing_control/config/waypoints/main_2.json new file mode 100644 index 0000000..1df63f8 --- /dev/null +++ b/src/racing_control/config/waypoints/main_2.json @@ -0,0 +1,155 @@ +{ + "topic": "/odom_combined", + "message_type": "nav_msgs/msg/Odometry", + "saved_at": "2026-08-07T14:43:29.485Z", + "count": 3, + "points": [ + { + "name": "goal_001", + "captured_at": "2026-08-07T14:43:14.866Z", + "received_at": null, + "yaw_degrees": 88.52360610794565, + "odom": { + "header": { + "frame_id": "odom", + "stamp": { + "sec": 0, + "nanosec": 0 + }, + "stamp_iso": null + }, + "child_frame_id": "base_link", + "pose": { + "pose": { + "position": { + "x": 4.2128251001861265, + "y": 3.7376334819031842, + "z": 0 + }, + "orientation": { + "x": 0, + "y": 0, + "z": 0.6979380047775942, + "w": 0.7161581818893582 + } + }, + "covariance": [] + }, + "twist": { + "twist": { + "linear": { + "x": 0, + "y": 0, + "z": 0 + }, + "angular": { + "x": 0, + "y": 0, + "z": 0 + } + }, + "covariance": [] + }, + "yaw_degrees": 88.52360610794565 + } + }, + { + "name": "goal_002", + "captured_at": "2026-08-07T14:43:20.968Z", + "received_at": null, + "yaw_degrees": 178.26427158937722, + "odom": { + "header": { + "frame_id": "odom", + "stamp": { + "sec": 0, + "nanosec": 0 + }, + "stamp_iso": null + }, + "child_frame_id": "base_link", + "pose": { + "pose": { + "position": { + "x": 1.1720794040064113, + "y": 4.319801449605877, + "z": 0 + }, + "orientation": { + "x": 0, + "y": 0, + "z": 0.99988528505826, + "w": 0.015146508639358486 + } + }, + "covariance": [] + }, + "twist": { + "twist": { + "linear": { + "x": 0, + "y": 0, + "z": 0 + }, + "angular": { + "x": 0, + "y": 0, + "z": 0 + } + }, + "covariance": [] + }, + "yaw_degrees": 178.26427158937722 + } + }, + { + "name": "goal_003", + "captured_at": "2026-08-07T14:43:28.294Z", + "received_at": null, + "yaw_degrees": -88.21009502116203, + "odom": { + "header": { + "frame_id": "odom", + "stamp": { + "sec": 0, + "nanosec": 0 + }, + "stamp_iso": null + }, + "child_frame_id": "base_link", + "pose": { + "pose": { + "position": { + "x": 2.4951887885861144, + "y": 2.318297930897253, + "z": 0 + }, + "orientation": { + "x": 0, + "y": 0, + "z": -0.6959760577153663, + "w": 0.7180649880665239 + } + }, + "covariance": [] + }, + "twist": { + "twist": { + "linear": { + "x": 0, + "y": 0, + "z": 0 + }, + "angular": { + "x": 0, + "y": 0, + "z": 0 + } + }, + "covariance": [] + }, + "yaw_degrees": -88.21009502116203 + } + } + ] +} diff --git a/src/racing_control/include/racing_control/candidate_waypoint_selector.hpp b/src/racing_control/include/racing_control/candidate_waypoint_selector.hpp new file mode 100644 index 0000000..39df737 --- /dev/null +++ b/src/racing_control/include/racing_control/candidate_waypoint_selector.hpp @@ -0,0 +1,89 @@ +#ifndef RACING_CONTROL__CANDIDATE_WAYPOINT_SELECTOR_HPP_ +#define RACING_CONTROL__CANDIDATE_WAYPOINT_SELECTOR_HPP_ + +#include +#include +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" + +namespace racing_control +{ + +struct CandidateWaypoint +{ + std::string group_name; + std::string source_file; + std::string point_name; + geometry_msgs::msg::PoseStamped pose; +}; + +using CandidateWaypointGroups = std::unordered_map>; + +struct RecoveryCommand +{ + double linear_x{0.0}; + double angular_z{0.0}; + double duration_sec{0.0}; + std::string label; +}; + +CandidateWaypointGroups loadCandidateWaypointGroups( + const std::string & json_dir, + const std::string & frame_id); + +std::optional nearestFreeRecoveryCommand( + const nav_msgs::msg::OccupancyGrid & costmap, + const geometry_msgs::msg::PoseStamped & current, + int occupied_threshold, + bool treat_unknown_as_occupied, + double search_radius_m = 1.0); + +std::optional rearClearRecoveryCommand( + const nav_msgs::msg::OccupancyGrid & costmap, + const geometry_msgs::msg::PoseStamped & current, + int occupied_threshold, + bool treat_unknown_as_occupied, + double rear_clear_distance_m); + +class CandidateWaypointSelector +{ +public: + void setGroups(CandidateWaypointGroups groups); + void setCostmap(const nav_msgs::msg::OccupancyGrid & costmap); + + std::optional select( + const std::string & group_name, + int occupied_threshold, + bool treat_unknown_as_occupied, + const std::string & preferred_source_file = "") const; + + std::optional nearestFreeRecoveryCommand( + const geometry_msgs::msg::PoseStamped & current, + int occupied_threshold, + bool treat_unknown_as_occupied, + double search_radius_m = 1.0) const; + + bool isPoseOccupied( + const geometry_msgs::msg::PoseStamped & pose, + int occupied_threshold, + bool treat_unknown_as_occupied) const; + +private: + bool isCandidateOccupied( + const geometry_msgs::msg::PoseStamped & pose, + int occupied_threshold, + bool treat_unknown_as_occupied) const; + + std::optional commandForRecoveryAngle(double angle_rad) const; + + CandidateWaypointGroups groups_; + nav_msgs::msg::OccupancyGrid costmap_; + bool has_costmap_{false}; +}; + +} // namespace racing_control + +#endif // RACING_CONTROL__CANDIDATE_WAYPOINT_SELECTOR_HPP_ diff --git a/src/racing_control/include/racing_control/candidate_waypoint_selector.hpp.unc-backup.md5~ b/src/racing_control/include/racing_control/candidate_waypoint_selector.hpp.unc-backup.md5~ new file mode 100644 index 0000000..5a1d0c5 --- /dev/null +++ b/src/racing_control/include/racing_control/candidate_waypoint_selector.hpp.unc-backup.md5~ @@ -0,0 +1 @@ +4773a55f2a69d3a5f983b4f5cbe49312 candidate_waypoint_selector.hpp diff --git a/src/racing_control/include/racing_control/candidate_waypoint_selector.hpp.unc-backup~ b/src/racing_control/include/racing_control/candidate_waypoint_selector.hpp.unc-backup~ new file mode 100644 index 0000000..a9949f6 --- /dev/null +++ b/src/racing_control/include/racing_control/candidate_waypoint_selector.hpp.unc-backup~ @@ -0,0 +1,190 @@ +#ifndef RACING_CONTROL__CANDIDATE_WAYPOINT_SELECTOR_HPP_ +#define RACING_CONTROL__CANDIDATE_WAYPOINT_SELECTOR_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "racing_control/racing_control.hpp" + +namespace racing_control +{ + +struct CandidateWaypoint +{ + std::string group; + std::string source_file; + std::string point_name; + geometry_msgs::msg::PoseStamped pose; +}; + +struct RecoveryCommand +{ + double linear_x{0.0}; + double angular_z{0.0}; +}; + +using CandidateWaypointGroups = std::map>; + +inline geometry_msgs::msg::PoseStamped candidatePoseFromJson( + const nlohmann::json & point, const std::string & frame_id) +{ + const auto & pose = point.at("odom").at("pose").at("pose"); + const auto & position = pose.at("position"); + const auto & orientation = pose.at("orientation"); + geometry_msgs::msg::PoseStamped result; + result.header.frame_id = frame_id; + result.pose.position.x = position.value("x", 0.0); + result.pose.position.y = position.value("y", 0.0); + result.pose.position.z = position.value("z", 0.0); + result.pose.orientation.x = orientation.value("x", 0.0); + result.pose.orientation.y = orientation.value("y", 0.0); + result.pose.orientation.z = orientation.value("z", 0.0); + result.pose.orientation.w = orientation.value("w", 1.0); + if (point.contains("yaw_degrees")) { + const auto yaw = point.at("yaw_degrees").get() * M_PI / 180.0; + result.pose.orientation.z = std::sin(yaw * 0.5); + result.pose.orientation.w = std::cos(yaw * 0.5); + } + return result; +} + +inline CandidateWaypointGroups loadCandidateWaypointGroups( + const std::string & directory, const std::string & frame_id) +{ + CandidateWaypointGroups groups; + for (const auto & entry : std::filesystem::directory_iterator(directory)) { + if (!entry.is_regular_file() || entry.path().extension() != ".json") { + continue; + } + std::ifstream input(entry.path()); + const auto document = nlohmann::json::parse(input); + for (const auto & point : document.value("points", nlohmann::json::array())) { + const auto point_name = point.value("name", std::string{}); + if (point_name.empty()) { + continue; + } + groups[point_name].push_back( + CandidateWaypoint{ + point_name, entry.path().filename().string(), point_name, + candidatePoseFromJson(point, frame_id)}); + } + } + return groups; +} + +class CandidateWaypointSelector +{ +public: + void setGroups(CandidateWaypointGroups groups) + { + groups_ = std::move(groups); + } + + void setCostmap(const nav_msgs::msg::OccupancyGrid & costmap) + { + costmap_ = costmap; + } + + std::optional select( + const std::string & group, const int occupied_threshold, const bool allow_unknown) const + { + const auto it = groups_.find(group); + if (it == groups_.end()) { + return std::nullopt; + } + for (const auto & candidate : it->second) { + if (isFree(candidate.pose, occupied_threshold, allow_unknown)) { + return candidate; + } + } + return std::nullopt; + } + +private: + bool isFree( + const geometry_msgs::msg::PoseStamped & pose, + const int occupied_threshold, + const bool allow_unknown) const + { + if (costmap_.info.resolution <= 0.0 || costmap_.info.width == 0 || costmap_.info.height == 0) { + return true; + } + const auto mx = static_cast(std::floor( + (pose.pose.position.x - costmap_.info.origin.position.x) / costmap_.info.resolution)); + const auto my = static_cast(std::floor( + (pose.pose.position.y - costmap_.info.origin.position.y) / costmap_.info.resolution)); + if (mx < 0 || my < 0 || mx >= static_cast(costmap_.info.width) || + my >= static_cast(costmap_.info.height)) + { + return false; + } + const auto value = costmap_.data[ + static_cast(my) * costmap_.info.width + static_cast(mx)]; + return value < 0 ? allow_unknown : value < occupied_threshold; + } + + CandidateWaypointGroups groups_; + nav_msgs::msg::OccupancyGrid costmap_; +}; + +inline std::optional nearestFreeRecoveryCommand( + const nav_msgs::msg::OccupancyGrid & costmap, + const geometry_msgs::msg::PoseStamped & current, + const int occupied_threshold, + const bool allow_unknown, + const double max_distance) +{ + if (costmap.info.resolution <= 0.0 || costmap.info.width == 0 || costmap.info.height == 0) { + return std::nullopt; + } + + const auto yaw = std::atan2( + 2.0 * (current.pose.orientation.w * current.pose.orientation.z), + 1.0 - 2.0 * current.pose.orientation.z * current.pose.orientation.z); + const auto cos_yaw = std::cos(yaw); + const auto sin_yaw = std::sin(yaw); + double best_distance = max_distance; + double best_forward = 0.0; + bool found = false; + for (std::size_t my = 0; my < costmap.info.height; ++my) { + for (std::size_t mx = 0; mx < costmap.info.width; ++mx) { + const auto value = costmap.data[my * costmap.info.width + mx]; + if (value < 0 ? !allow_unknown : value >= occupied_threshold) { + continue; + } + const auto world_x = costmap.info.origin.position.x + + (static_cast(mx) + 0.5) * costmap.info.resolution; + const auto world_y = costmap.info.origin.position.y + + (static_cast(my) + 0.5) * costmap.info.resolution; + const auto dx = world_x - current.pose.position.x; + const auto dy = world_y - current.pose.position.y; + const auto forward = cos_yaw * dx + sin_yaw * dy; + const auto lateral = -sin_yaw * dx + cos_yaw * dy; + const auto distance = std::hypot(forward, lateral); + if (distance <= 0.0 || distance > max_distance || distance >= best_distance) { + continue; + } + best_distance = distance; + best_forward = forward; + found = true; + } + } + if (!found) { + return std::nullopt; + } + return RecoveryCommand{best_forward >= 0.0 ? 0.2 : -0.2, 0.0}; +} + +} // namespace racing_control + +#endif // RACING_CONTROL__CANDIDATE_WAYPOINT_SELECTOR_HPP_ diff --git a/src/racing_control/include/racing_control/racing_control.hpp b/src/racing_control/include/racing_control/racing_control.hpp index f523724..defc1b2 100644 --- a/src/racing_control/include/racing_control/racing_control.hpp +++ b/src/racing_control/include/racing_control/racing_control.hpp @@ -33,6 +33,64 @@ enum class QrTransitTarget PostQr }; +enum class RouteRecoveryPhase +{ + Initial, + AfterPlanningBackup, + AfterLocalClearReplan, + AfterFinalBackupReplan +}; + +enum class RouteFailureKind +{ + Planning, + Following +}; + +enum class RouteRecoveryAction +{ + Fail, + PlanningBackup, + ClearLocalAndReplan, + FinalBackupReplan +}; + +inline RouteRecoveryAction routeRecoveryAction( + const RouteRecoveryPhase phase, + const RouteFailureKind failure, + const int final_recovery_attempts, + const int max_final_recovery_attempts) +{ + const auto finalBackupOrFail = [&]() { + return final_recovery_attempts < max_final_recovery_attempts ? + RouteRecoveryAction::FinalBackupReplan : RouteRecoveryAction::Fail; + }; + + if (failure == RouteFailureKind::Planning) { + if (phase == RouteRecoveryPhase::Initial) { + return RouteRecoveryAction::PlanningBackup; + } + if (phase == RouteRecoveryPhase::AfterLocalClearReplan || + phase == RouteRecoveryPhase::AfterFinalBackupReplan) + { + return finalBackupOrFail(); + } + return RouteRecoveryAction::Fail; + } + + if (phase == RouteRecoveryPhase::Initial || + phase == RouteRecoveryPhase::AfterPlanningBackup) + { + return RouteRecoveryAction::ClearLocalAndReplan; + } + if (phase == RouteRecoveryPhase::AfterLocalClearReplan || + phase == RouteRecoveryPhase::AfterFinalBackupReplan) + { + return finalBackupOrFail(); + } + return RouteRecoveryAction::Fail; +} + inline VlmCaptureMode vlmCaptureModeFromString(const std::string & value) { std::string normalized; diff --git a/src/racing_control/src/candidate_waypoint_selector.cpp b/src/racing_control/src/candidate_waypoint_selector.cpp new file mode 100644 index 0000000..be2f58d --- /dev/null +++ b/src/racing_control/src/candidate_waypoint_selector.cpp @@ -0,0 +1,314 @@ +#include "racing_control/candidate_waypoint_selector.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace racing_control +{ +namespace +{ + +double normalizeAngle(double angle) +{ + while (angle > M_PI) { + angle -= 2.0 * M_PI; + } + while (angle < -M_PI) { + angle += 2.0 * M_PI; + } + return angle; +} + +double yawFromQuaternionInternal( + const double x, const double y, const double z, const double w) +{ + return std::atan2( + 2.0 * (w * z + x * y), + 1.0 - 2.0 * (y * y + z * z)); +} + +std::vector sortedJsonFiles(const std::filesystem::path & json_dir) +{ + std::vector files; + for (const auto & entry : std::filesystem::directory_iterator(json_dir)) { + if (entry.is_regular_file() && entry.path().extension() == ".json") { + files.push_back(entry.path()); + } + } + std::sort( + files.begin(), files.end(), [](const auto & a, const auto & b) { + return a.filename().string() < b.filename().string(); + }); + return files; +} + +geometry_msgs::msg::PoseStamped poseFromJsonPoint( + const nlohmann::json & point, + const std::string & frame_id) +{ + const auto & odom = point.at("odom"); + const auto & pose = odom.at("pose").at("pose"); + const auto & position = pose.at("position"); + const auto & orientation = pose.at("orientation"); + + const double x = position.at("x").get(); + const double y = position.at("y").get(); + const double yaw = point.contains("yaw_degrees") ? + point.at("yaw_degrees").get() * M_PI / 180.0 : + yawFromQuaternionInternal( + orientation.at("x").get(), orientation.at("y").get(), + orientation.at("z").get(), orientation.at("w").get()); + + geometry_msgs::msg::PoseStamped stamped; + stamped.header.frame_id = frame_id; + stamped.pose.position.x = x; + stamped.pose.position.y = y; + stamped.pose.position.z = 0.0; + stamped.pose.orientation.z = std::sin(yaw * 0.5); + stamped.pose.orientation.w = std::cos(yaw * 0.5); + return stamped; +} + +bool fileContainsGroup( + const std::string & source_file, + const std::string & preferred_source_file) +{ + if (preferred_source_file.empty()) { + return false; + } + return source_file == preferred_source_file; +} + +} // namespace + +CandidateWaypointGroups loadCandidateWaypointGroups( + const std::string & json_dir, + const std::string & frame_id) +{ + CandidateWaypointGroups groups; + const auto root = std::filesystem::path(json_dir); + if (json_dir.empty() || !std::filesystem::exists(root)) { + return groups; + } + + for (const auto & path : sortedJsonFiles(root)) { + std::ifstream input(path); + if (!input) { + continue; + } + nlohmann::json data; + try { + input >> data; + } catch (...) { + continue; + } + + const auto points = data.is_array() ? data : data.value("points", nlohmann::json::array()); + if (!points.is_array()) { + continue; + } + + for (const auto & point : points) { + if (!point.is_object() || !point.contains("odom") || !point["odom"].is_object()) { + continue; + } + CandidateWaypoint candidate; + candidate.source_file = path.filename().string(); + candidate.point_name = point.value("name", path.stem().string()); + candidate.group_name = candidate.point_name; + candidate.pose = poseFromJsonPoint(point, frame_id); + groups[candidate.group_name].push_back(std::move(candidate)); + } + } + + return groups; +} + +void CandidateWaypointSelector::setGroups(CandidateWaypointGroups groups) +{ + groups_ = std::move(groups); +} + +void CandidateWaypointSelector::setCostmap(const nav_msgs::msg::OccupancyGrid & costmap) +{ + costmap_ = costmap; + has_costmap_ = true; +} + +std::optional CandidateWaypointSelector::select( + const std::string & group_name, + const int occupied_threshold, + const bool treat_unknown_as_occupied, + const std::string & preferred_source_file) const +{ + const auto group_it = groups_.find(group_name); + if (group_it == groups_.end() || group_it->second.empty() || !has_costmap_) { + return std::nullopt; + } + + std::vector ordered; + ordered.reserve(group_it->second.size()); + for (const auto & candidate : group_it->second) { + ordered.push_back(&candidate); + } + std::stable_sort( + ordered.begin(), ordered.end(), [&](const auto * lhs, const auto * rhs) { + const bool lhs_preferred = fileContainsGroup(lhs->source_file, preferred_source_file); + const bool rhs_preferred = fileContainsGroup(rhs->source_file, preferred_source_file); + if (lhs_preferred != rhs_preferred) { + return lhs_preferred; + } + return lhs->source_file < rhs->source_file; + }); + + for (const auto * candidate : ordered) { + if (!isCandidateOccupied(candidate->pose, occupied_threshold, treat_unknown_as_occupied)) { + return *candidate; + } + } + return std::nullopt; +} + +std::optional nearestFreeRecoveryCommand( + const nav_msgs::msg::OccupancyGrid & costmap, + const geometry_msgs::msg::PoseStamped & current, + const int occupied_threshold, + const bool treat_unknown_as_occupied, + const double search_radius_m) +{ + CandidateWaypointSelector selector; + selector.setCostmap(costmap); + return selector.nearestFreeRecoveryCommand( + current, occupied_threshold, treat_unknown_as_occupied, search_radius_m); +} + +std::optional rearClearRecoveryCommand( + const nav_msgs::msg::OccupancyGrid & costmap, + const geometry_msgs::msg::PoseStamped & current, + const int occupied_threshold, + const bool treat_unknown_as_occupied, + const double rear_clear_distance_m) +{ + CandidateWaypointSelector selector; + selector.setCostmap(costmap); + const double yaw = 2.0 * std::atan2(current.pose.orientation.z, current.pose.orientation.w); + geometry_msgs::msg::PoseStamped rear = current; + rear.pose.position.x -= std::cos(yaw) * rear_clear_distance_m; + rear.pose.position.y -= std::sin(yaw) * rear_clear_distance_m; + if (selector.isPoseOccupied(rear, occupied_threshold, treat_unknown_as_occupied)) { + return std::nullopt; + } + return RecoveryCommand{-0.25, 0.0, rear_clear_distance_m / 0.25, "rear-clear-backward"}; +} + +std::optional CandidateWaypointSelector::nearestFreeRecoveryCommand( + const geometry_msgs::msg::PoseStamped & current, + const int occupied_threshold, + const bool treat_unknown_as_occupied, + const double search_radius_m) const +{ + if (!has_costmap_) { + return std::nullopt; + } + + const double yaw = 2.0 * std::atan2(current.pose.orientation.z, current.pose.orientation.w); + const std::vector distances{0.25, 0.5, 0.75, std::max(0.25, search_radius_m)}; + const std::vector angles{0.0, M_PI / 4.0, -M_PI / 4.0, M_PI / 2.0, -M_PI / 2.0, M_PI}; + + double best_score = std::numeric_limits::infinity(); + std::optional best_command; + for (const double distance : distances) { + for (const double angle : angles) { + const double sample_angle = normalizeAngle(yaw + angle); + geometry_msgs::msg::PoseStamped sample = current; + sample.pose.position.x += std::cos(sample_angle) * distance; + sample.pose.position.y += std::sin(sample_angle) * distance; + if (isCandidateOccupied(sample, occupied_threshold, treat_unknown_as_occupied)) { + continue; + } + const double score = distance + std::abs(angle) * 0.1; + if (score < best_score) { + best_score = score; + best_command = commandForRecoveryAngle(angle); + } + } + } + return best_command; +} + +bool CandidateWaypointSelector::isPoseOccupied( + const geometry_msgs::msg::PoseStamped & pose, + const int occupied_threshold, + const bool treat_unknown_as_occupied) const +{ + return isCandidateOccupied(pose, occupied_threshold, treat_unknown_as_occupied); +} + +bool CandidateWaypointSelector::isCandidateOccupied( + const geometry_msgs::msg::PoseStamped & pose, + const int occupied_threshold, + const bool treat_unknown_as_occupied) const +{ + const auto & costmap = costmap_; + const auto & origin = costmap.info.origin; + const double resolution = costmap.info.resolution; + const auto width = static_cast(costmap.info.width); + const auto height = static_cast(costmap.info.height); + if (resolution <= 0.0 || width <= 0 || height <= 0) { + return true; + } + + const double origin_yaw = yawFromQuaternionInternal( + origin.orientation.x, origin.orientation.y, origin.orientation.z, origin.orientation.w); + const double dx = pose.pose.position.x - origin.position.x; + const double dy = pose.pose.position.y - origin.position.y; + const double cos_yaw = std::cos(-origin_yaw); + const double sin_yaw = std::sin(-origin_yaw); + const double local_x = cos_yaw * dx - sin_yaw * dy; + const double local_y = sin_yaw * dx + cos_yaw * dy; + const auto mx = static_cast(std::floor(local_x / resolution)); + const auto my = static_cast(std::floor(local_y / resolution)); + if (mx < 0 || my < 0 || mx >= width || my >= height) { + return true; + } + + const auto index = my * width + mx; + if (index < 0 || index >= static_cast(costmap.data.size())) { + return true; + } + + const auto cost = static_cast(costmap.data[static_cast(index)]); + if (cost < 0) { + return treat_unknown_as_occupied; + } + return cost >= occupied_threshold; +} + +std::optional CandidateWaypointSelector::commandForRecoveryAngle( + const double angle_rad) const +{ + const double angle = normalizeAngle(angle_rad); + if (std::abs(angle) < M_PI / 8.0) { + return RecoveryCommand{0.25, 0.0, 0.6, "forward"}; + } + if (std::abs(std::abs(angle) - M_PI) < M_PI / 8.0) { + return RecoveryCommand{-0.25, 0.0, 0.6, "backward"}; + } + if (angle > 0.0 && angle < M_PI / 2.0) { + return RecoveryCommand{0.25, 0.7, 0.6, "forward-left"}; + } + if (angle < 0.0 && angle > -M_PI / 2.0) { + return RecoveryCommand{0.25, -0.7, 0.6, "forward-right"}; + } + if (angle >= M_PI / 2.0) { + return RecoveryCommand{0.0, 0.7, 0.4, "left"}; + } + return RecoveryCommand{0.0, -0.7, 0.4, "right"}; +} + +} // namespace racing_control diff --git a/src/racing_control/src/candidate_waypoint_selector.cpp.unc-backup.md5~ b/src/racing_control/src/candidate_waypoint_selector.cpp.unc-backup.md5~ new file mode 100644 index 0000000..4747cf1 --- /dev/null +++ b/src/racing_control/src/candidate_waypoint_selector.cpp.unc-backup.md5~ @@ -0,0 +1 @@ +38f483033e12bc75920274763b0c7e0b candidate_waypoint_selector.cpp diff --git a/src/racing_control/src/candidate_waypoint_selector.cpp.unc-backup~ b/src/racing_control/src/candidate_waypoint_selector.cpp.unc-backup~ new file mode 100644 index 0000000..4987551 --- /dev/null +++ b/src/racing_control/src/candidate_waypoint_selector.cpp.unc-backup~ @@ -0,0 +1,312 @@ +#include "racing_control/candidate_waypoint_selector.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace racing_control +{ +namespace +{ + +double normalizeAngle(double angle) +{ + while (angle > M_PI) { + angle -= 2.0 * M_PI; + } + while (angle < -M_PI) { + angle += 2.0 * M_PI; + } + return angle; +} + +double yawFromQuaternionInternal( + const double x, const double y, const double z, const double w) +{ + return std::atan2( + 2.0 * (w * z + x * y), + 1.0 - 2.0 * (y * y + z * z)); +} + +std::vector sortedJsonFiles(const std::filesystem::path & json_dir) +{ + std::vector files; + for (const auto & entry : std::filesystem::directory_iterator(json_dir)) { + if (entry.is_regular_file() && entry.path().extension() == ".json") { + files.push_back(entry.path()); + } + } + std::sort(files.begin(), files.end(), [](const auto & a, const auto & b) { + return a.filename().string() < b.filename().string(); + }); + return files; +} + +geometry_msgs::msg::PoseStamped poseFromJsonPoint( + const nlohmann::json & point, + const std::string & frame_id) +{ + const auto & odom = point.at("odom"); + const auto & pose = odom.at("pose").at("pose"); + const auto & position = pose.at("position"); + const auto & orientation = pose.at("orientation"); + + const double x = position.at("x").get(); + const double y = position.at("y").get(); + const double yaw = point.contains("yaw_degrees") ? + point.at("yaw_degrees").get() * M_PI / 180.0 : + yawFromQuaternionInternal( + orientation.at("x").get(), orientation.at("y").get(), + orientation.at("z").get(), orientation.at("w").get()); + + geometry_msgs::msg::PoseStamped stamped; + stamped.header.frame_id = frame_id; + stamped.pose.position.x = x; + stamped.pose.position.y = y; + stamped.pose.position.z = 0.0; + stamped.pose.orientation.z = std::sin(yaw * 0.5); + stamped.pose.orientation.w = std::cos(yaw * 0.5); + return stamped; +} + +bool fileContainsGroup( + const std::string & source_file, + const std::string & preferred_source_file) +{ + if (preferred_source_file.empty()) { + return false; + } + return source_file == preferred_source_file; +} + +} // namespace + +CandidateWaypointGroups loadCandidateWaypointGroups( + const std::string & json_dir, + const std::string & frame_id) +{ + CandidateWaypointGroups groups; + const auto root = std::filesystem::path(json_dir); + if (json_dir.empty() || !std::filesystem::exists(root)) { + return groups; + } + + for (const auto & path : sortedJsonFiles(root)) { + std::ifstream input(path); + if (!input) { + continue; + } + nlohmann::json data; + try { + input >> data; + } catch (...) { + continue; + } + + const auto points = data.is_array() ? data : data.value("points", nlohmann::json::array()); + if (!points.is_array()) { + continue; + } + + for (const auto & point : points) { + if (!point.is_object() || !point.contains("odom") || !point["odom"].is_object()) { + continue; + } + CandidateWaypoint candidate; + candidate.source_file = path.filename().string(); + candidate.point_name = point.value("name", path.stem().string()); + candidate.group_name = candidate.point_name; + candidate.pose = poseFromJsonPoint(point, frame_id); + groups[candidate.group_name].push_back(std::move(candidate)); + } + } + + return groups; +} + +void CandidateWaypointSelector::setGroups(CandidateWaypointGroups groups) +{ + groups_ = std::move(groups); +} + +void CandidateWaypointSelector::setCostmap(const nav_msgs::msg::OccupancyGrid & costmap) +{ + costmap_ = costmap; + has_costmap_ = true; +} + +std::optional CandidateWaypointSelector::select( + const std::string & group_name, + const int occupied_threshold, + const bool treat_unknown_as_occupied, + const std::string & preferred_source_file) const +{ + const auto group_it = groups_.find(group_name); + if (group_it == groups_.end() || group_it->second.empty() || !has_costmap_) { + return std::nullopt; + } + + std::vector ordered; + ordered.reserve(group_it->second.size()); + for (const auto & candidate : group_it->second) { + ordered.push_back(&candidate); + } + std::stable_sort(ordered.begin(), ordered.end(), [&](const auto * lhs, const auto * rhs) { + const bool lhs_preferred = fileContainsGroup(lhs->source_file, preferred_source_file); + const bool rhs_preferred = fileContainsGroup(rhs->source_file, preferred_source_file); + if (lhs_preferred != rhs_preferred) { + return lhs_preferred; + } + return lhs->source_file < rhs->source_file; + }); + + for (const auto * candidate : ordered) { + if (!isCandidateOccupied(candidate->pose, occupied_threshold, treat_unknown_as_occupied)) { + return *candidate; + } + } + return std::nullopt; +} + +std::optional nearestFreeRecoveryCommand( + const nav_msgs::msg::OccupancyGrid & costmap, + const geometry_msgs::msg::PoseStamped & current, + const int occupied_threshold, + const bool treat_unknown_as_occupied, + const double search_radius_m) +{ + CandidateWaypointSelector selector; + selector.setCostmap(costmap); + return selector.nearestFreeRecoveryCommand( + current, occupied_threshold, treat_unknown_as_occupied, search_radius_m); +} + +std::optional rearClearRecoveryCommand( + const nav_msgs::msg::OccupancyGrid & costmap, + const geometry_msgs::msg::PoseStamped & current, + const int occupied_threshold, + const bool treat_unknown_as_occupied, + const double rear_clear_distance_m) +{ + CandidateWaypointSelector selector; + selector.setCostmap(costmap); + const double yaw = 2.0 * std::atan2(current.pose.orientation.z, current.pose.orientation.w); + geometry_msgs::msg::PoseStamped rear = current; + rear.pose.position.x -= std::cos(yaw) * rear_clear_distance_m; + rear.pose.position.y -= std::sin(yaw) * rear_clear_distance_m; + if (selector.isPoseOccupied(rear, occupied_threshold, treat_unknown_as_occupied)) { + return std::nullopt; + } + return RecoveryCommand{-0.25, 0.0, rear_clear_distance_m / 0.25, "rear-clear-backward"}; +} + +std::optional CandidateWaypointSelector::nearestFreeRecoveryCommand( + const geometry_msgs::msg::PoseStamped & current, + const int occupied_threshold, + const bool treat_unknown_as_occupied, + const double search_radius_m) const +{ + if (!has_costmap_) { + return std::nullopt; + } + + const double yaw = 2.0 * std::atan2(current.pose.orientation.z, current.pose.orientation.w); + const std::vector distances{0.25, 0.5, 0.75, std::max(0.25, search_radius_m)}; + const std::vector angles{0.0, M_PI / 4.0, -M_PI / 4.0, M_PI / 2.0, -M_PI / 2.0, M_PI}; + + double best_score = std::numeric_limits::infinity(); + std::optional best_command; + for (const double distance : distances) { + for (const double angle : angles) { + const double sample_angle = normalizeAngle(yaw + angle); + geometry_msgs::msg::PoseStamped sample = current; + sample.pose.position.x += std::cos(sample_angle) * distance; + sample.pose.position.y += std::sin(sample_angle) * distance; + if (isCandidateOccupied(sample, occupied_threshold, treat_unknown_as_occupied)) { + continue; + } + const double score = distance + std::abs(angle) * 0.1; + if (score < best_score) { + best_score = score; + best_command = commandForRecoveryAngle(angle); + } + } + } + return best_command; +} + +bool CandidateWaypointSelector::isPoseOccupied( + const geometry_msgs::msg::PoseStamped & pose, + const int occupied_threshold, + const bool treat_unknown_as_occupied) const +{ + return isCandidateOccupied(pose, occupied_threshold, treat_unknown_as_occupied); +} + +bool CandidateWaypointSelector::isCandidateOccupied( + const geometry_msgs::msg::PoseStamped & pose, + const int occupied_threshold, + const bool treat_unknown_as_occupied) const +{ + const auto & costmap = costmap_; + const auto & origin = costmap.info.origin; + const double resolution = costmap.info.resolution; + const auto width = static_cast(costmap.info.width); + const auto height = static_cast(costmap.info.height); + if (resolution <= 0.0 || width <= 0 || height <= 0) { + return true; + } + + const double origin_yaw = yawFromQuaternionInternal( + origin.orientation.x, origin.orientation.y, origin.orientation.z, origin.orientation.w); + const double dx = pose.pose.position.x - origin.position.x; + const double dy = pose.pose.position.y - origin.position.y; + const double cos_yaw = std::cos(-origin_yaw); + const double sin_yaw = std::sin(-origin_yaw); + const double local_x = cos_yaw * dx - sin_yaw * dy; + const double local_y = sin_yaw * dx + cos_yaw * dy; + const auto mx = static_cast(std::floor(local_x / resolution)); + const auto my = static_cast(std::floor(local_y / resolution)); + if (mx < 0 || my < 0 || mx >= width || my >= height) { + return true; + } + + const auto index = my * width + mx; + if (index < 0 || index >= static_cast(costmap.data.size())) { + return true; + } + + const auto cost = static_cast(costmap.data[static_cast(index)]); + if (cost < 0) { + return treat_unknown_as_occupied; + } + return cost >= occupied_threshold; +} + +std::optional CandidateWaypointSelector::commandForRecoveryAngle( + const double angle_rad) const +{ + const double angle = normalizeAngle(angle_rad); + if (std::abs(angle) < M_PI / 8.0) { + return RecoveryCommand{0.25, 0.0, 0.6, "forward"}; + } + if (std::abs(std::abs(angle) - M_PI) < M_PI / 8.0) { + return RecoveryCommand{-0.25, 0.0, 0.6, "backward"}; + } + if (angle > 0.0 && angle < M_PI / 2.0) { + return RecoveryCommand{0.25, 0.7, 0.6, "forward-left"}; + } + if (angle < 0.0 && angle > -M_PI / 2.0) { + return RecoveryCommand{0.25, -0.7, 0.6, "forward-right"}; + } + if (angle >= M_PI / 2.0) { + return RecoveryCommand{0.0, 0.7, 0.4, "left"}; + } + return RecoveryCommand{0.0, -0.7, 0.4, "right"}; +} + +} // namespace racing_control diff --git a/src/racing_control/src/racing_control.cpp b/src/racing_control/src/racing_control.cpp index e2cdf1f..a131342 100644 --- a/src/racing_control/src/racing_control.cpp +++ b/src/racing_control/src/racing_control.cpp @@ -23,10 +23,13 @@ #include "nav2_msgs/action/compute_path_through_poses.hpp" #include "nav2_msgs/action/follow_path.hpp" #include "nav2_msgs/action/navigate_to_pose.hpp" +#include "nav2_msgs/srv/clear_entire_costmap.hpp" #include "nav_msgs/msg/odometry.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" #include "nav_msgs/msg/path.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" +#include "racing_control/candidate_waypoint_selector.hpp" #include "sensor_msgs/msg/compressed_image.hpp" #include "std_msgs/msg/int32.hpp" #include "std_msgs/msg/string.hpp" @@ -129,6 +132,7 @@ public: using NavigateToPose = nav2_msgs::action::NavigateToPose; using ComputePathThroughPoses = nav2_msgs::action::ComputePathThroughPoses; using FollowPath = nav2_msgs::action::FollowPath; + using ClearEntireCostmap = nav2_msgs::srv::ClearEntireCostmap; using NavigateGoalHandle = rclcpp_action::ClientGoalHandle; using ComputeGoalHandle = rclcpp_action::ClientGoalHandle; using FollowGoalHandle = rclcpp_action::ClientGoalHandle; @@ -143,6 +147,11 @@ public: recovery_cmd_vel_pub_ = create_publisher( recovery_cmd_vel_topic_, 10); guard_path_pub_ = create_publisher(guard_input_topic_, 1); + global_costmap_sub_ = create_subscription( + global_costmap_topic_, 10, + [this](nav_msgs::msg::OccupancyGrid::SharedPtr msg) { + onGlobalCostmap(std::move(msg)); + }); if (enable_vlm_image_relay_) { vlm_image_pub_ = create_publisher(vlm_image_output_topic_, 1); @@ -167,6 +176,10 @@ public: compute_path_client_ = rclcpp_action::create_client(this, compute_path_action_); follow_path_client_ = rclcpp_action::create_client(this, follow_path_action_); + clear_global_costmap_client_ = + create_client(global_costmap_clear_service_); + clear_local_costmap_client_ = + create_client(local_costmap_clear_service_); tick_timer_ = create_wall_timer(200ms, [this]() {tick();}); startKeyboardThread(); @@ -198,6 +211,12 @@ private: odom_topic_ = declare_parameter("odom_topic", "/odom_combined"); recovery_cmd_vel_topic_ = declare_parameter("recovery_cmd_vel_topic", "/cmd_vel"); + global_costmap_topic_ = declare_parameter( + "global_costmap_topic", "/global_costmap/costmap"); + global_costmap_clear_service_ = declare_parameter( + "global_costmap_clear_service", "global_costmap/clear_entirely_global_costmap"); + local_costmap_clear_service_ = declare_parameter( + "local_costmap_clear_service", "local_costmap/clear_entirely_local_costmap"); navigate_action_ = declare_parameter("navigate_action", "/navigate_to_pose"); compute_path_action_ = declare_parameter("compute_path_action", "/compute_path_through_poses"); @@ -218,6 +237,8 @@ private: enable_vlm_image_relay_ = declare_parameter("enable_vlm_image_relay", false); enable_dynamic_replanning_ = declare_parameter("enable_dynamic_replanning", true); enable_recovery_ = declare_parameter("enable_recovery", true); + enable_candidate_waypoint_selection_ = + declare_parameter("enable_candidate_waypoint_selection", true); vlm_image_input_topic_ = declare_parameter("vlm_image_input_topic", "/image"); vlm_image_output_topic_ = declare_parameter("vlm_image_output_topic", "/vlm_image"); @@ -236,12 +257,22 @@ private: recovery_backup_distance_ = declare_parameter("recovery_backup_distance", 0.04); recovery_backup_timeout_sec_ = declare_parameter("recovery_backup_timeout_sec", 0.2); + planning_failure_backup_speed_ = + declare_parameter("planning_failure_backup_speed", -0.1); + planning_failure_backup_distance_ = + declare_parameter("planning_failure_backup_distance", 0.03); + planning_failure_backup_timeout_sec_ = + declare_parameter("planning_failure_backup_timeout_sec", 0.3); + recovery_clear_wait_sec_ = declare_parameter("recovery_clear_wait_sec", 1.0); pass_through_vlm_trigger_radius_ = declare_parameter("pass_through_vlm_trigger_radius", 0.35); circle_goal_tolerance_ = declare_parameter("circle_goal_tolerance", 0.50); dynamic_replan_max_consecutive_failures_ = declare_parameter("dynamic_replan_max_consecutive_failures", 3); max_recovery_attempts_ = declare_parameter("max_recovery_attempts", 2); + recovery_search_radius_m_ = declare_parameter("recovery_search_radius_m", 1.0); + recovery_rear_clear_distance_m_ = + declare_parameter("recovery_rear_clear_distance_m", 0.5); const auto vlm_capture_mode = declare_parameter("vlm_capture_mode", "stop"); @@ -291,6 +322,20 @@ private: singlePoseFromParameter( "counterclockwise_home_pose", {0.536986899408154, 0.1772662932069835, -1.652041913138445}); + + candidate_waypoint_json_dir_ = declare_parameter( + "candidate_waypoint_json_dir", "/home/sunrise/yiliao_ws/src/racing_control/config/waypoints"); + qr_candidate_group_name_ = declare_parameter("qr_candidate_group_name", "qr"); + entry_candidate_group_name_ = + declare_parameter("entry_candidate_group_name", "entry"); + vlm_candidate_group_name_ = + declare_parameter("vlm_candidate_group_name", "goal_005"); + clockwise_candidate_source_file_ = declare_parameter( + "clockwise_candidate_source_file", "main_1.json"); + counterclockwise_candidate_source_file_ = declare_parameter( + "counterclockwise_candidate_source_file", "main_2.json"); + candidate_selector_.setGroups( + loadCandidateWaypointGroups(candidate_waypoint_json_dir_, frame_id_)); } geometry_msgs::msg::PoseStamped singlePoseFromParameter( @@ -470,6 +515,11 @@ private: if (recovery_timer_) { recovery_timer_->cancel(); } + if (recovery_clear_wait_timer_) { + recovery_clear_wait_timer_->cancel(); + } + recovery_after_clear_callback_ = nullptr; + recovery_costmap_clear_pending_ = 0; cancelActiveFollowGoal(); publishRecoveryVelocity(0.0); publishSign(sign_qr_disable_); @@ -540,9 +590,10 @@ private: latest_qr_result_.clear(); selected_direction_ = RouteDirection::Unknown; qr_detection_disabled_ = false; - recovery_attempts_ = 0; + resetRouteRecoveryState(); active_segment_ = RouteSegment::ToQr; - active_segment_waypoints_ = {qr_pose_}; + active_segment_waypoints_ = { + selectCandidatePose(qr_candidate_group_name_, "", qr_pose_).value_or(qr_pose_)}; active_segment_next_waypoint_index_ = 0; publishSign(sign_profile_normal_); publishSign(sign_qr_enable_); @@ -621,7 +672,7 @@ private: const auto vlm_index = vlmWaypointIndex(route); latest_vlm_result_.clear(); vlm_capture_triggered_ = false; - recovery_attempts_ = 0; + resetRouteRecoveryState(); const auto split_at_vlm = split_qr_to_vlm_segment_ && vlm_capture_mode_ == VlmCaptureMode::Stop; @@ -631,8 +682,17 @@ private: active_segment_waypoints_.push_back(post_qr_pose_); } + auto route_waypoints = route.waypoints; + if (vlm_index < route_waypoints.size()) { + route_waypoints[vlm_index] = selectCandidatePose( + vlm_candidate_group_name_, preferredCandidateSourceFile(), + route_waypoints[vlm_index]).value_or(route_waypoints[vlm_index]); + } + const auto selected_entry_pose = selectCandidatePose( + entry_candidate_group_name_, preferredCandidateSourceFile(), + entry_pose_).value_or(entry_pose_); const auto qr_segment = routeWaypointsAfterQr( - entry_pose_, route.waypoints, vlm_index, split_at_vlm); + selected_entry_pose, route_waypoints, vlm_index, split_at_vlm); active_segment_waypoints_.insert( active_segment_waypoints_.end(), qr_segment.begin(), qr_segment.end()); if (!split_at_vlm) { @@ -646,7 +706,7 @@ private: { const auto & route = selectedRoute(); const auto vlm_index = vlmWaypointIndex(route); - recovery_attempts_ = 0; + resetRouteRecoveryState(); active_segment_ = RouteSegment::AfterVlm; active_segment_waypoints_ = remainingWaypoints(route.waypoints, vlm_index + 1); active_segment_waypoints_.push_back(route.home_pose); @@ -664,7 +724,7 @@ private: startStage(Stage::ComputeCirclePath, path_planning_timeout_sec_); if (!compute_path_client_->wait_for_action_server(2s)) { - handleRouteExecutionFailure("ComputePathThroughPoses action server is not available"); + onRoutePlanningFailed("ComputePathThroughPoses action server is not available"); return; } @@ -676,8 +736,12 @@ private: auto options = rclcpp_action::Client::SendGoalOptions(); options.goal_response_callback = [this](ComputeGoalHandle::SharedPtr goal_handle) { + if (stage_ != Stage::ComputeCirclePath) { + RCLCPP_DEBUG(get_logger(), "stale route planning goal response ignored"); + return; + } if (!goal_handle) { - handleRouteExecutionFailure("circle path planning goal was rejected"); + onRoutePlanningFailed("circle path planning goal was rejected"); } }; options.result_callback = @@ -686,10 +750,10 @@ private: return; } if (result.code != rclcpp_action::ResultCode::SUCCEEDED || - result.result->path.poses.empty()) + !result.result || result.result->path.poses.empty()) { finishStage("planning failed"); - handleRouteExecutionFailure("circle path planning failed"); + onRoutePlanningFailed("circle path planning failed"); return; } active_path_ = result.result->path; @@ -725,7 +789,7 @@ private: active_path_, [this](const bool ok) { finishStage(ok ? "FollowPath succeeded" : "FollowPath failed"); if (!ok) { - handleRouteExecutionFailure("route segment FollowPath failed"); + onRouteFollowFailed("route segment FollowPath failed"); return; } if (active_segment_ == RouteSegment::ToQr) { @@ -789,6 +853,44 @@ private: vlm_image_input_topic_.c_str(), vlm_image_output_topic_.c_str()); } + void onGlobalCostmap(nav_msgs::msg::OccupancyGrid::SharedPtr msg) + { + std::lock_guard lock(costmap_mutex_); + latest_global_costmap_ = std::move(msg); + candidate_selector_.setCostmap(*latest_global_costmap_); + } + + std::optional selectCandidatePose( + const std::string & group_name, + const std::string & preferred_source_file, + const geometry_msgs::msg::PoseStamped & fallback_pose) + { + if (!enable_candidate_waypoint_selection_) { + return fallback_pose; + } + { + std::lock_guard lock(costmap_mutex_); + if (!latest_global_costmap_) { + RCLCPP_WARN( + get_logger(), "candidate selection fallback for %s: no global costmap yet", + group_name.c_str()); + return fallback_pose; + } + } + const auto selected = candidate_selector_.select( + group_name, 50, false, preferred_source_file); + if (!selected) { + RCLCPP_WARN( + get_logger(), "candidate selection fallback for %s: no usable candidate", + group_name.c_str()); + return fallback_pose; + } + RCLCPP_INFO( + get_logger(), "candidate selected for %s from %s -> %s", + group_name.c_str(), selected->source_file.c_str(), selected->point_name.c_str()); + return selected->pose; + } + void maybeTriggerPassThroughVlmCapture() { if (active_segment_ != RouteSegment::FullRoute || vlm_capture_triggered_) { @@ -914,46 +1016,134 @@ private: get_logger(), "%s | consecutive dynamic replan failures=%d", reason.c_str(), dynamic_replan_consecutive_failures_); if (dynamic_replan_consecutive_failures_ >= dynamic_replan_max_consecutive_failures_) { - handleRouteExecutionFailure(reason); + onRoutePlanningFailed(reason); } } - void handleRouteExecutionFailure(const std::string & reason) + void resetRouteRecoveryState() { - startRecovery( - reason, - [this]() { - dynamic_replan_consecutive_failures_ = 0; - const auto current = currentPoseFromOdom(); - if (current) { - active_segment_waypoints_ = remainingWaypointsAfterProgress( - active_segment_waypoints_, *current, active_segment_next_waypoint_index_, - circle_goal_tolerance_); - } else { - active_segment_waypoints_ = remainingWaypoints( - active_segment_waypoints_, active_segment_next_waypoint_index_); - } - active_segment_next_waypoint_index_ = 0; - runRouteSegmentPlanning("after recovery"); - }); + route_recovery_phase_ = RouteRecoveryPhase::Initial; + planning_backup_attempted_ = false; + final_recovery_attempts_ = 0; } - void startRecovery(const std::string & reason, std::function on_recovered) + void prepareRouteRecoveryReplan() { - if (!enable_recovery_ || recovery_attempts_ >= max_recovery_attempts_) { + dynamic_replan_consecutive_failures_ = 0; + const auto current = currentPoseFromOdom(); + if (current) { + active_segment_waypoints_ = remainingWaypointsAfterProgress( + active_segment_waypoints_, *current, active_segment_next_waypoint_index_, + circle_goal_tolerance_); + } else { + active_segment_waypoints_ = remainingWaypoints( + active_segment_waypoints_, active_segment_next_waypoint_index_); + } + active_segment_next_waypoint_index_ = 0; + } + + void onRoutePlanningFailed(const std::string & reason) + { + const auto action = routeRecoveryAction( + route_recovery_phase_, RouteFailureKind::Planning, + final_recovery_attempts_, max_recovery_attempts_); + if (action == RouteRecoveryAction::Fail) { + failRace(reason); + return; + } + + if (action == RouteRecoveryAction::PlanningBackup) { + planning_backup_attempted_ = true; + route_recovery_phase_ = RouteRecoveryPhase::AfterPlanningBackup; + RCLCPP_WARN( + get_logger(), "%s; planning failed, backing up at %.2f m/s for %.2f s before one replan", + reason.c_str(), planning_failure_backup_speed_, planning_failure_backup_timeout_sec_); + startBackupRecovery( + reason, planning_failure_backup_speed_, planning_failure_backup_distance_, + planning_failure_backup_timeout_sec_, [this]() { + prepareRouteRecoveryReplan(); + runRouteSegmentPlanning("after planning-failure backup"); + }); + return; + } + + ++final_recovery_attempts_; + route_recovery_phase_ = RouteRecoveryPhase::AfterFinalBackupReplan; + RCLCPP_WARN( + get_logger(), "%s; final backup/replan attempt %d/%d", + reason.c_str(), final_recovery_attempts_, max_recovery_attempts_); + startBackupRecovery( + reason, recovery_backup_speed_, recovery_backup_distance_, recovery_backup_timeout_sec_, + [this]() { + prepareRouteRecoveryReplan(); + runRouteSegmentPlanning("after final backup"); + }); + } + + void onRouteFollowFailed(const std::string & reason) + { + const auto action = routeRecoveryAction( + route_recovery_phase_, RouteFailureKind::Following, + final_recovery_attempts_, max_recovery_attempts_); + if (action == RouteRecoveryAction::Fail) { + failRace(reason); + return; + } + + if (action == RouteRecoveryAction::ClearLocalAndReplan) { + route_recovery_phase_ = RouteRecoveryPhase::AfterLocalClearReplan; + RCLCPP_WARN( + get_logger(), "%s; clearing local costmap, waiting for update, then replanning once", + reason.c_str()); + clearLocalCostmapBeforeRecoveryReplan( + [this]() { + prepareRouteRecoveryReplan(); + runRouteSegmentPlanning("after local costmap clear"); + }); + return; + } + + ++final_recovery_attempts_; + route_recovery_phase_ = RouteRecoveryPhase::AfterFinalBackupReplan; + RCLCPP_WARN( + get_logger(), "%s; final backup/replan attempt %d/%d", + reason.c_str(), final_recovery_attempts_, max_recovery_attempts_); + startBackupRecovery( + reason, recovery_backup_speed_, recovery_backup_distance_, recovery_backup_timeout_sec_, + [this]() { + prepareRouteRecoveryReplan(); + runRouteSegmentPlanning("after final backup"); + }); + } + + void startBackupRecovery( + const std::string & reason, + const double speed, + const double distance, + const double timeout_sec, + std::function on_recovered) + { + if (!enable_recovery_) { failRace(reason); return; } - ++recovery_attempts_; recovery_in_progress_ = true; recovery_done_callback_ = std::move(on_recovered); recovery_start_time_ = now(); recovery_start_pose_ = currentPoseFromOdom(); + recovery_active_speed_ = speed; + recovery_active_distance_ = distance; + recovery_active_timeout_sec_ = timeout_sec; + if (recovery_clear_wait_timer_) { + recovery_clear_wait_timer_->cancel(); + } + recovery_after_clear_callback_ = nullptr; + recovery_costmap_clear_pending_ = 0; cancelActiveFollowGoal(); RCLCPP_WARN( - get_logger(), "%s; recovery backup attempt %d/%d", - reason.c_str(), recovery_attempts_, max_recovery_attempts_); + get_logger(), "%s; backup recovery speed=%.2f m/s distance=%.2f m timeout=%.2f s", + reason.c_str(), speed, distance, timeout_sec); if (recovery_timer_) { recovery_timer_->cancel(); @@ -966,9 +1156,9 @@ private: const auto elapsed = (now() - recovery_start_time_).seconds(); const auto backup_distance = recoveryBackupDistance(); if (!recoveryBackupComplete( - backup_distance, recovery_backup_distance_, elapsed, recovery_backup_timeout_sec_)) + backup_distance, recovery_active_distance_, elapsed, recovery_active_timeout_sec_)) { - publishRecoveryVelocity(recovery_backup_speed_); + publishRecoveryVelocity(recovery_active_speed_); return; } @@ -984,6 +1174,79 @@ private: } } + void clearLocalCostmapBeforeRecoveryReplan(std::function on_cleared) + { + recovery_after_clear_callback_ = std::move(on_cleared); + recovery_costmap_clear_pending_ = 0; + const auto requested = requestEntireCostmapClear( + clear_local_costmap_client_, local_costmap_clear_service_); + if (!requested) { + startRecoveryReplanAfterClearWait(); + } + } + + bool requestEntireCostmapClear( + const rclcpp::Client::SharedPtr & client, + const std::string & service_name) + { + if (!client) { + return false; + } + if (!client->wait_for_service(200ms)) { + RCLCPP_WARN( + get_logger(), "costmap clear service not available: %s", service_name.c_str()); + return false; + } + + ++recovery_costmap_clear_pending_; + auto request = std::make_shared(); + client->async_send_request( + request, + [this, service_name](rclcpp::Client::SharedFuture) { + if (recovery_costmap_clear_pending_ > 0) { + --recovery_costmap_clear_pending_; + } + RCLCPP_INFO(get_logger(), "cleared costmap via %s", service_name.c_str()); + if (recovery_costmap_clear_pending_ == 0) { + startRecoveryReplanAfterClearWait(); + } + }); + return true; + } + + void startRecoveryReplanAfterClearWait() + { + if (!recovery_after_clear_callback_) { + return; + } + + if (recovery_clear_wait_timer_) { + recovery_clear_wait_timer_->cancel(); + } + + if (recovery_clear_wait_sec_ <= 0.0) { + auto callback = std::move(recovery_after_clear_callback_); + recovery_after_clear_callback_ = nullptr; + if (callback) { + callback(); + } + return; + } + + recovery_clear_wait_timer_ = create_wall_timer( + std::chrono::duration(recovery_clear_wait_sec_), + [this]() { + if (recovery_clear_wait_timer_) { + recovery_clear_wait_timer_->cancel(); + } + auto callback = std::move(recovery_after_clear_callback_); + recovery_after_clear_callback_ = nullptr; + if (callback) { + callback(); + } + }); + } + double recoveryBackupDistance() const { if (!recovery_start_pose_) { @@ -1106,7 +1369,7 @@ private: void sendFollowPath(const nav_msgs::msg::Path & path, std::function on_done) { if (!follow_path_client_->wait_for_action_server(2s)) { - failRace("FollowPath action server is not available"); + onRouteFollowFailed("FollowPath action server is not available"); return; } @@ -1211,6 +1474,17 @@ private: throw std::runtime_error("route direction is not selected"); } + std::string preferredCandidateSourceFile() const + { + if (selected_direction_ == RouteDirection::Clockwise) { + return clockwise_candidate_source_file_; + } + if (selected_direction_ == RouteDirection::Counterclockwise) { + return counterclockwise_candidate_source_file_; + } + return ""; + } + bool routeSegmentReached() const { std::lock_guard lock(odom_mutex_); @@ -1287,6 +1561,7 @@ private: bool enable_vlm_image_relay_{false}; bool enable_dynamic_replanning_{true}; bool enable_recovery_{true}; + bool enable_candidate_waypoint_selection_{true}; bool qr_detection_disabled_{false}; bool dynamic_replan_in_flight_{false}; bool recovery_in_progress_{false}; @@ -1302,6 +1577,9 @@ private: std::string vlm_result_topic_; std::string odom_topic_; std::string recovery_cmd_vel_topic_; + std::string global_costmap_topic_; + std::string global_costmap_clear_service_; + std::string local_costmap_clear_service_; std::string vlm_image_input_topic_; std::string vlm_image_output_topic_; std::string navigate_action_; @@ -1311,6 +1589,12 @@ private: std::string planner_id_; std::string controller_id_; std::string goal_checker_id_; + std::string candidate_waypoint_json_dir_; + std::string qr_candidate_group_name_; + std::string entry_candidate_group_name_; + std::string vlm_candidate_group_name_; + std::string clockwise_candidate_source_file_; + std::string counterclockwise_candidate_source_file_; double navigation_timeout_sec_{120.0}; double path_planning_timeout_sec_{30.0}; @@ -1324,6 +1608,15 @@ private: double recovery_backup_speed_{-0.2}; double recovery_backup_distance_{0.04}; double recovery_backup_timeout_sec_{0.2}; + double planning_failure_backup_speed_{-0.1}; + double planning_failure_backup_distance_{0.03}; + double planning_failure_backup_timeout_sec_{0.3}; + double recovery_clear_wait_sec_{1.0}; + double recovery_active_speed_{0.0}; + double recovery_active_distance_{0.0}; + double recovery_active_timeout_sec_{0.0}; + double recovery_search_radius_m_{1.0}; + double recovery_rear_clear_distance_m_{0.5}; double pass_through_vlm_trigger_radius_{0.35}; double circle_goal_tolerance_{0.30}; @@ -1336,7 +1629,7 @@ private: int dynamic_replan_max_consecutive_failures_{3}; int dynamic_replan_consecutive_failures_{0}; int max_recovery_attempts_{2}; - int recovery_attempts_{0}; + int final_recovery_attempts_{0}; geometry_msgs::msg::PoseStamped qr_pose_; geometry_msgs::msg::PoseStamped post_qr_pose_; @@ -1346,20 +1639,26 @@ private: RouteDirection selected_direction_{RouteDirection::Unknown}; VlmCaptureMode vlm_capture_mode_{VlmCaptureMode::Stop}; RouteSegment active_segment_{RouteSegment::None}; + RouteRecoveryPhase route_recovery_phase_{RouteRecoveryPhase::Initial}; + bool planning_backup_attempted_{false}; std::vector active_segment_waypoints_; std::size_t active_segment_next_waypoint_index_{0}; nav_msgs::msg::Path active_path_; bool vlm_capture_triggered_{false}; std::optional recovery_start_pose_; std::function recovery_done_callback_; + std::function recovery_after_clear_callback_; + std::size_t recovery_costmap_clear_pending_{0}; std::string latest_qr_result_; std::string latest_vlm_result_; rclcpp::Time qr_result_time_{0, 0, RCL_ROS_TIME}; rclcpp::Time vlm_result_time_{0, 0, RCL_ROS_TIME}; nav_msgs::msg::Odometry::SharedPtr latest_odom_; + nav_msgs::msg::OccupancyGrid::SharedPtr latest_global_costmap_; sensor_msgs::msg::CompressedImage::SharedPtr latest_image_; mutable std::mutex odom_mutex_; + mutable std::mutex costmap_mutex_; mutable std::mutex image_mutex_; rclcpp::Publisher::SharedPtr sign_pub_; @@ -1369,14 +1668,19 @@ private: rclcpp::Subscription::SharedPtr qr_sub_; rclcpp::Subscription::SharedPtr vlm_sub_; rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::SharedPtr global_costmap_sub_; rclcpp::Subscription::SharedPtr image_sub_; rclcpp_action::Client::SharedPtr navigate_client_; rclcpp_action::Client::SharedPtr compute_path_client_; rclcpp_action::Client::SharedPtr follow_path_client_; + rclcpp::Client::SharedPtr clear_global_costmap_client_; + rclcpp::Client::SharedPtr clear_local_costmap_client_; FollowGoalHandle::SharedPtr active_follow_goal_handle_; rclcpp::TimerBase::SharedPtr startup_qr_enable_timer_; rclcpp::TimerBase::SharedPtr recovery_timer_; + rclcpp::TimerBase::SharedPtr recovery_clear_wait_timer_; rclcpp::TimerBase::SharedPtr tick_timer_; + CandidateWaypointSelector candidate_selector_; int startup_qr_enable_publish_count_{0}; std::uint64_t follow_goal_generation_{0}; diff --git a/src/racing_control/src/racing_control.cpp.unc-backup.md5~ b/src/racing_control/src/racing_control.cpp.unc-backup.md5~ new file mode 100644 index 0000000..9c5da8d --- /dev/null +++ b/src/racing_control/src/racing_control.cpp.unc-backup.md5~ @@ -0,0 +1 @@ +32e8eedad71eea774e96a762a1cfd3ae racing_control.cpp diff --git a/src/racing_control/src/racing_control.cpp.unc-backup~ b/src/racing_control/src/racing_control.cpp.unc-backup~ new file mode 100644 index 0000000..a131342 --- /dev/null +++ b/src/racing_control/src/racing_control.cpp.unc-backup~ @@ -0,0 +1,1706 @@ +#include "racing_control/racing_control.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "geometry_msgs/msg/twist.hpp" +#include "nav2_msgs/action/compute_path_through_poses.hpp" +#include "nav2_msgs/action/follow_path.hpp" +#include "nav2_msgs/action/navigate_to_pose.hpp" +#include "nav2_msgs/srv/clear_entire_costmap.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav_msgs/msg/path.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "racing_control/candidate_waypoint_selector.hpp" +#include "sensor_msgs/msg/compressed_image.hpp" +#include "std_msgs/msg/int32.hpp" +#include "std_msgs/msg/string.hpp" + +using namespace std::chrono_literals; + +namespace racing_control +{ + +namespace +{ + +enum class Stage +{ + Idle, + NavigateToQr, + NavigatePostQr, + WaitForQr, + NavigateToEntry, + SwitchToTask2Profile, + ComputeCirclePath, + ExecuteCirclePath, + SwitchToNormalProfile, + WaitForVlm, + ReturnOrigin, + Finished, + Failed +}; + +enum class RouteSegment +{ + None, + ToQr, + ToVlm, + AfterVlm, + FullRoute +}; + +struct RouteConfig +{ + std::string label; + std::vector waypoints; + geometry_msgs::msg::PoseStamped home_pose; +}; + +const char * stageName(const Stage stage) +{ + switch (stage) { + case Stage::Idle: + return "等待启动"; + case Stage::NavigateToQr: + return "二维码点导航"; + case Stage::NavigatePostQr: + return "二维码后置点导航"; + case Stage::WaitForQr: + return "二维码识别/TTS"; + case Stage::NavigateToEntry: + return "通道入口导航"; + case Stage::SwitchToTask2Profile: + return "任务二参数切换"; + case Stage::ComputeCirclePath: + return "任务二轨迹规划"; + case Stage::ExecuteCirclePath: + return "任务二轨迹执行"; + case Stage::SwitchToNormalProfile: + return "恢复导航参数"; + case Stage::WaitForVlm: + return "图生文/TTS"; + case Stage::ReturnOrigin: + return "返回原点"; + case Stage::Finished: + return "比赛完成"; + case Stage::Failed: + return "比赛失败"; + } + return "未知阶段"; +} + +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::string poseSummary(const geometry_msgs::msg::PoseStamped & pose) +{ + std::ostringstream out; + out << "(" << pose.pose.position.x << ", " << pose.pose.position.y << ")"; + return out.str(); +} + +} // namespace + +class RacingControl : public rclcpp::Node +{ +public: + using NavigateToPose = nav2_msgs::action::NavigateToPose; + using ComputePathThroughPoses = nav2_msgs::action::ComputePathThroughPoses; + using FollowPath = nav2_msgs::action::FollowPath; + using ClearEntireCostmap = nav2_msgs::srv::ClearEntireCostmap; + using NavigateGoalHandle = rclcpp_action::ClientGoalHandle; + using ComputeGoalHandle = rclcpp_action::ClientGoalHandle; + using FollowGoalHandle = rclcpp_action::ClientGoalHandle; + + RacingControl() + : Node("racing_control") + { + loadParameters(); + + sign_pub_ = create_publisher(sign_topic_, 10); + startStartupQrEnablePublisher(); + recovery_cmd_vel_pub_ = create_publisher( + recovery_cmd_vel_topic_, 10); + guard_path_pub_ = create_publisher(guard_input_topic_, 1); + global_costmap_sub_ = create_subscription( + global_costmap_topic_, 10, + [this](nav_msgs::msg::OccupancyGrid::SharedPtr msg) { + onGlobalCostmap(std::move(msg)); + }); + if (enable_vlm_image_relay_) { + vlm_image_pub_ = + create_publisher(vlm_image_output_topic_, 1); + image_sub_ = create_subscription( + vlm_image_input_topic_, 10, + [this](sensor_msgs::msg::CompressedImage::SharedPtr msg) { + onImage(std::move(msg)); + }); + } + + qr_sub_ = create_subscription( + qr_result_topic_, 10, + [this](std_msgs::msg::String::SharedPtr msg) {onQrResult(std::move(msg));}); + vlm_sub_ = create_subscription( + vlm_result_topic_, 10, + [this](std_msgs::msg::String::SharedPtr msg) {onVlmResult(std::move(msg));}); + odom_sub_ = create_subscription( + odom_topic_, 10, + [this](nav_msgs::msg::Odometry::SharedPtr msg) {onOdom(std::move(msg));}); + + navigate_client_ = rclcpp_action::create_client(this, navigate_action_); + compute_path_client_ = + rclcpp_action::create_client(this, compute_path_action_); + follow_path_client_ = rclcpp_action::create_client(this, follow_path_action_); + clear_global_costmap_client_ = + create_client(global_costmap_clear_service_); + clear_local_costmap_client_ = + create_client(local_costmap_clear_service_); + + tick_timer_ = create_wall_timer(200ms, [this]() {tick();}); + startKeyboardThread(); + + RCLCPP_INFO( + get_logger(), + "racing_control ready. Press SPACE to start, or set auto_start:=true."); + + if (auto_start_) { + startRace(); + } + } + + ~RacingControl() override + { + stop_keyboard_.store(true); + if (keyboard_thread_.joinable()) { + keyboard_thread_.join(); + } + } + +private: + void loadParameters() + { + frame_id_ = declare_parameter("frame_id", "odom"); + sign_topic_ = declare_parameter("sign_topic", "/sign4return"); + qr_result_topic_ = declare_parameter("qr_result_topic", "/qr_results"); + vlm_result_topic_ = declare_parameter("vlm_result_topic", "/vlm_result"); + odom_topic_ = declare_parameter("odom_topic", "/odom_combined"); + recovery_cmd_vel_topic_ = + declare_parameter("recovery_cmd_vel_topic", "/cmd_vel"); + global_costmap_topic_ = declare_parameter( + "global_costmap_topic", "/global_costmap/costmap"); + global_costmap_clear_service_ = declare_parameter( + "global_costmap_clear_service", "global_costmap/clear_entirely_global_costmap"); + local_costmap_clear_service_ = declare_parameter( + "local_costmap_clear_service", "local_costmap/clear_entirely_local_costmap"); + navigate_action_ = declare_parameter("navigate_action", "/navigate_to_pose"); + compute_path_action_ = + declare_parameter("compute_path_action", "/compute_path_through_poses"); + follow_path_action_ = declare_parameter("follow_path_action", "/follow_path"); + guard_input_topic_ = + declare_parameter( + "trajectory_guard_input_topic", + "/trajectory_guard/input_path"); + planner_id_ = declare_parameter("planner_id", "GridBased"); + controller_id_ = declare_parameter("controller_id", "FollowPath"); + goal_checker_id_ = declare_parameter("goal_checker_id", ""); + + auto_start_ = declare_parameter("auto_start", false); + use_trajectory_guard_ = declare_parameter( + "use_trajectory_guard", defaultUseTrajectoryGuard()); + use_post_qr_pose_ = declare_parameter("use_post_qr_pose", false); + split_qr_to_vlm_segment_ = declare_parameter("split_qr_to_vlm_segment", true); + enable_vlm_image_relay_ = declare_parameter("enable_vlm_image_relay", false); + enable_dynamic_replanning_ = declare_parameter("enable_dynamic_replanning", true); + enable_recovery_ = declare_parameter("enable_recovery", true); + enable_candidate_waypoint_selection_ = + declare_parameter("enable_candidate_waypoint_selection", true); + vlm_image_input_topic_ = declare_parameter("vlm_image_input_topic", "/image"); + vlm_image_output_topic_ = + declare_parameter("vlm_image_output_topic", "/vlm_image"); + navigation_timeout_sec_ = declare_parameter("navigation_timeout_sec", 120.0); + path_planning_timeout_sec_ = declare_parameter("path_planning_timeout_sec", 30.0); + circle_timeout_sec_ = declare_parameter("circle_timeout_sec", 120.0); + qr_result_timeout_sec_ = declare_parameter("qr_result_timeout_sec", 8.0); + profile_switch_wait_sec_ = declare_parameter("profile_switch_wait_sec", 1.0); + post_qr_wait_sec_ = declare_parameter("post_qr_wait_sec", 1.0); + vlm_capture_wait_sec_ = declare_parameter("vlm_capture_wait_sec", 0.5); + dynamic_replan_interval_sec_ = + declare_parameter("dynamic_replan_interval_sec", 1.0); + dynamic_replan_stop_distance_ = + declare_parameter("dynamic_replan_stop_distance", 0.5); + recovery_backup_speed_ = declare_parameter("recovery_backup_speed", -0.2); + recovery_backup_distance_ = declare_parameter("recovery_backup_distance", 0.04); + recovery_backup_timeout_sec_ = + declare_parameter("recovery_backup_timeout_sec", 0.2); + planning_failure_backup_speed_ = + declare_parameter("planning_failure_backup_speed", -0.1); + planning_failure_backup_distance_ = + declare_parameter("planning_failure_backup_distance", 0.03); + planning_failure_backup_timeout_sec_ = + declare_parameter("planning_failure_backup_timeout_sec", 0.3); + recovery_clear_wait_sec_ = declare_parameter("recovery_clear_wait_sec", 1.0); + pass_through_vlm_trigger_radius_ = + declare_parameter("pass_through_vlm_trigger_radius", 0.35); + circle_goal_tolerance_ = declare_parameter("circle_goal_tolerance", 0.50); + dynamic_replan_max_consecutive_failures_ = + declare_parameter("dynamic_replan_max_consecutive_failures", 3); + max_recovery_attempts_ = declare_parameter("max_recovery_attempts", 2); + recovery_search_radius_m_ = declare_parameter("recovery_search_radius_m", 1.0); + recovery_rear_clear_distance_m_ = + declare_parameter("recovery_rear_clear_distance_m", 0.5); + + const auto vlm_capture_mode = + declare_parameter("vlm_capture_mode", "stop"); + vlm_capture_mode_ = vlmCaptureModeFromString(vlm_capture_mode); + if (vlm_capture_mode_ == VlmCaptureMode::Unknown) { + throw std::invalid_argument("vlm_capture_mode must be 'stop' or 'pass_through'"); + } + + sign_qr_enable_ = declare_parameter("sign_qr_enable", 0); + sign_qr_disable_ = declare_parameter("sign_qr_disable", 5); + sign_vlm_trigger_ = declare_parameter("sign_vlm_trigger", 9); + sign_profile_normal_ = declare_parameter("sign_profile_normal", 10); + sign_profile_task2_ = declare_parameter("sign_profile_task2", 11); + + qr_pose_ = singlePoseFromParameter( + "qr_pose", {4.333107888975101, 1.028867429995691, 1.1383894869544392}); + entry_pose_ = singlePoseFromParameter( + "entry_pose", {2.504811372926262, 2.2798075935366624, 1.520115031774562}); + post_qr_pose_ = singlePoseFromParameter( + "post_qr_pose", {2.504811372926262, 2.2798075935366624, 1.520115031774562}); + vlm_waypoint_number_ = declare_parameter("vlm_waypoint_number", 2); + + const auto clockwise_defaults = std::vector{ + 0.7534956931109804, 3.766501234923627, 1.5592359900104606, + 3.8567885105264077, 4.329424195458402, 0.03997856199797794, + 2.5144339572664096, 2.4337692660037766, -1.6101462328054222}; + const auto counterclockwise_defaults = std::vector{ + 4.2128251001861265, 3.7376334819031842, 1.5450283923222128, + 1.1720794040064113, 4.319801449605877, 3.1112984779040144, + 2.4951887885861144, 2.318297930897253, -1.539556591616333}; + + clockwise_route_.label = "顺时针"; + clockwise_route_.waypoints = posesFromFlatDoubles( + declare_parameter>("clockwise_waypoints", clockwise_defaults), frame_id_); + clockwise_route_.home_pose = + singlePoseFromParameter( + "clockwise_home_pose", {0.536986899408154, 0.1772662932069835, + -1.652041913138445}); + + counterclockwise_route_.label = "逆时针"; + counterclockwise_route_.waypoints = posesFromFlatDoubles( + declare_parameter>( + "counterclockwise_waypoints", + counterclockwise_defaults), + frame_id_); + counterclockwise_route_.home_pose = + singlePoseFromParameter( + "counterclockwise_home_pose", {0.536986899408154, 0.1772662932069835, + -1.652041913138445}); + + candidate_waypoint_json_dir_ = declare_parameter( + "candidate_waypoint_json_dir", "/home/sunrise/yiliao_ws/src/racing_control/config/waypoints"); + qr_candidate_group_name_ = declare_parameter("qr_candidate_group_name", "qr"); + entry_candidate_group_name_ = + declare_parameter("entry_candidate_group_name", "entry"); + vlm_candidate_group_name_ = + declare_parameter("vlm_candidate_group_name", "goal_005"); + clockwise_candidate_source_file_ = declare_parameter( + "clockwise_candidate_source_file", "main_1.json"); + counterclockwise_candidate_source_file_ = declare_parameter( + "counterclockwise_candidate_source_file", "main_2.json"); + candidate_selector_.setGroups( + loadCandidateWaypointGroups(candidate_waypoint_json_dir_, frame_id_)); + } + + geometry_msgs::msg::PoseStamped singlePoseFromParameter( + const std::string & name, const std::vector & defaults) + { + const auto poses = posesFromFlatDoubles( + declare_parameter>(name, defaults), frame_id_); + if (poses.size() != 1) { + throw std::invalid_argument(name + " must contain exactly one x/y/yaw triple"); + } + return poses.front(); + } + + void startKeyboardThread() + { + int keyboard_fd = STDIN_FILENO; + bool close_keyboard_fd = false; + if (!isatty(keyboard_fd)) { + keyboard_fd = open("/dev/tty", O_RDONLY); + close_keyboard_fd = keyboard_fd >= 0; + } + if (keyboard_fd < 0 || !isatty(keyboard_fd)) { + if (close_keyboard_fd) { + close(keyboard_fd); + } + RCLCPP_WARN( + get_logger(), + "no usable TTY for SPACE start; run from an interactive ssh tty or set auto_start:=true"); + return; + } + + keyboard_thread_ = std::thread( + [this, keyboard_fd, close_keyboard_fd]() { + termios old_termios {}; + if (tcgetattr(keyboard_fd, &old_termios) != 0) { + if (close_keyboard_fd) { + close(keyboard_fd); + } + return; + } + termios raw = old_termios; + raw.c_lflag &= static_cast(~(ICANON | ECHO)); + tcsetattr(keyboard_fd, TCSANOW, &raw); + + while (!stop_keyboard_.load()) { + fd_set read_set; + FD_ZERO(&read_set); + FD_SET(keyboard_fd, &read_set); + timeval timeout {}; + timeout.tv_sec = 0; + timeout.tv_usec = 200000; + const int ready = select(keyboard_fd + 1, &read_set, nullptr, nullptr, &timeout); + if (ready > 0 && FD_ISSET(keyboard_fd, &read_set)) { + char c = 0; + if (read(keyboard_fd, &c, 1) == 1 && c == ' ') { + start_requested_.store(true); + } + } + } + + tcsetattr(keyboard_fd, TCSANOW, &old_termios); + if (close_keyboard_fd) { + close(keyboard_fd); + } + }); + } + + void tick() + { + if (start_requested_.exchange(false)) { + startRace(); + } + + if (!race_started_ || stage_ == Stage::Finished || stage_ == Stage::Failed) { + return; + } + + if (recovery_in_progress_) { + return; + } + + const auto elapsed = (now() - stage_start_).seconds(); + if (stage_timeout_sec_ > 0.0 && elapsed > stage_timeout_sec_) { + if (stage_ == Stage::WaitForQr) { + finishStage("QR wait timeout"); + failRace("QR wait timed out before route direction was selected"); + return; + } + if (stage_ == Stage::WaitForVlm) { + RCLCPP_WARN(get_logger(), "VLM capture wait timed out; continuing route"); + finishStage("VLM capture wait timeout"); + runRemainingRouteSegment(); + return; + } + failRace("stage timed out: " + std::string(stageName(stage_))); + return; + } + + if (stage_ == Stage::SwitchToTask2Profile && elapsed >= profile_switch_wait_sec_) { + finishStage("profile switch wait complete"); + runFirstRouteSegment(); + return; + } + + if (stage_ == Stage::SwitchToNormalProfile && elapsed >= profile_switch_wait_sec_) { + finishStage("normal profile restored"); + runReturnOrigin(); + return; + } + + if (stage_ == Stage::WaitForQr && selected_direction_ != RouteDirection::Unknown && + (now() - qr_result_time_).seconds() >= post_qr_wait_sec_) + { + finishStage("QR result: " + latest_qr_result_ + " -> " + selectedRoute().label); + runQrTransitNavigation(); + return; + } + + if (stage_ == Stage::WaitForVlm && elapsed >= vlm_capture_wait_sec_) { + finishStage("VLM capture window elapsed"); + runRemainingRouteSegment(); + return; + } + + if (stage_ == Stage::ExecuteCirclePath) { + maybeTriggerPassThroughVlmCapture(); + maybeRunDynamicReplanning(); + } + + if (stage_ == Stage::ExecuteCirclePath && use_trajectory_guard_ && routeSegmentReached()) { + finishStage("route segment final pose reached"); + if (active_segment_ == RouteSegment::ToQr) { + runQrWait(); + } else if (active_segment_ == RouteSegment::ToVlm) { + runVlmWait(); + } else if (active_segment_ == RouteSegment::AfterVlm || + active_segment_ == RouteSegment::FullRoute) + { + finishRace(); + } + } + } + + void startRace() + { + if (race_started_) { + RCLCPP_WARN(get_logger(), "race already started"); + return; + } + race_started_ = true; + race_start_ = now(); + RCLCPP_INFO(get_logger(), "race started"); + runQrNavigation(); + } + + void startStage(const Stage stage, const double timeout_sec) + { + stage_ = stage; + stage_start_ = now(); + stage_timeout_sec_ = timeout_sec; + RCLCPP_INFO(get_logger(), "%s task started", stageName(stage_)); + } + + void finishStage(const std::string & detail) + { + const auto stage_elapsed = (now() - stage_start_).seconds(); + const auto total_elapsed = (now() - race_start_).seconds(); + RCLCPP_INFO( + get_logger(), "%s task finished: %s | task %.2fs | total %.2fs", + stageName(stage_), detail.c_str(), stage_elapsed, total_elapsed); + } + + void failRace(const std::string & reason) + { + stage_ = Stage::Failed; + recovery_in_progress_ = false; + if (recovery_timer_) { + recovery_timer_->cancel(); + } + if (recovery_clear_wait_timer_) { + recovery_clear_wait_timer_->cancel(); + } + recovery_after_clear_callback_ = nullptr; + recovery_costmap_clear_pending_ = 0; + cancelActiveFollowGoal(); + publishRecoveryVelocity(0.0); + publishSign(sign_qr_disable_); + RCLCPP_ERROR(get_logger(), "race failed: %s", reason.c_str()); + } + + void finishRace() + { + finishStage("origin reached"); + stage_ = Stage::Finished; + publishSign(sign_qr_disable_); + RCLCPP_INFO(get_logger(), "race finished | total %.2fs", (now() - race_start_).seconds()); + } + + void publishSign(const int value) + { + std_msgs::msg::Int32 msg; + msg.data = value; + for (int i = 0; i < 3; ++i) { + sign_pub_->publish(msg); + } + RCLCPP_INFO(get_logger(), "published %s=%d", sign_topic_.c_str(), value); + } + + void startStartupQrEnablePublisher() + { + startup_qr_enable_publish_count_ = 0; + publishStartupQrEnableOnce(); + startup_qr_enable_timer_ = create_wall_timer( + std::chrono::milliseconds(defaultStartupQrEnableIntervalMs()), + [this]() {publishStartupQrEnableOnce();}); + } + + void publishStartupQrEnableOnce() + { + if (race_started_ || stage_ == Stage::Finished || stage_ == Stage::Failed) { + if (startup_qr_enable_timer_) { + startup_qr_enable_timer_->cancel(); + } + return; + } + if (startup_qr_enable_publish_count_ >= defaultStartupQrEnableRepeats()) { + if (startup_qr_enable_timer_) { + startup_qr_enable_timer_->cancel(); + } + return; + } + publishSign(sign_qr_enable_); + ++startup_qr_enable_publish_count_; + if (startup_qr_enable_publish_count_ >= defaultStartupQrEnableRepeats() && + startup_qr_enable_timer_) + { + startup_qr_enable_timer_->cancel(); + } + } + + void disableQrDetectionOnce() + { + if (qr_detection_disabled_) { + return; + } + qr_detection_disabled_ = true; + publishSign(sign_qr_disable_); + } + + void runQrNavigation() + { + latest_qr_result_.clear(); + selected_direction_ = RouteDirection::Unknown; + qr_detection_disabled_ = false; + resetRouteRecoveryState(); + active_segment_ = RouteSegment::ToQr; + active_segment_waypoints_ = { + selectCandidatePose(qr_candidate_group_name_, "", qr_pose_).value_or(qr_pose_)}; + active_segment_next_waypoint_index_ = 0; + publishSign(sign_profile_normal_); + publishSign(sign_qr_enable_); + runRouteSegmentPlanning("start to QR pose"); + } + + void runQrWait() + { + startStage(Stage::WaitForQr, qr_result_timeout_sec_); + if (!latest_qr_result_.empty()) { + qr_result_time_ = now(); + } + if (post_qr_wait_sec_ > 0.0) { + stage_timeout_sec_ += post_qr_wait_sec_; + } + } + + void runQrTransitNavigation() + { + disableQrDetectionOnce(); + cancelActiveFollowGoal(); + runSwitchToTask2Profile(); + } + + void runPostQrNavigation() + { + disableQrDetectionOnce(); + startStage(Stage::NavigatePostQr, navigation_timeout_sec_); + sendNavigateGoal( + post_qr_pose_, [this](const bool ok) { + if (stage_ != Stage::NavigatePostQr) { + RCLCPP_DEBUG(get_logger(), "stale post-QR navigation result ignored"); + return; + } + finishStage(ok ? "reached " + poseSummary(post_qr_pose_) : "navigation failed"); + if (!ok) { + failRace("failed to reach post-QR pose"); + return; + } + runEntryNavigation(); + }); + } + + void runEntryNavigation() + { + disableQrDetectionOnce(); + startStage(Stage::NavigateToEntry, navigation_timeout_sec_); + sendNavigateGoal( + entry_pose_, [this](const bool ok) { + if (stage_ != Stage::NavigateToEntry) { + RCLCPP_DEBUG(get_logger(), "stale entry navigation result ignored"); + return; + } + finishStage(ok ? "reached " + poseSummary(entry_pose_) : "navigation failed"); + if (!ok) { + failRace("failed to reach entry pose"); + return; + } + runSwitchToTask2Profile(); + }); + } + + void runSwitchToTask2Profile() + { + if (selected_direction_ == RouteDirection::Unknown) { + failRace("cannot switch to task two before QR route direction is known"); + return; + } + publishSign(sign_profile_task2_); + startStage(Stage::SwitchToTask2Profile, profile_switch_wait_sec_ + 2.0); + } + + void runFirstRouteSegment() + { + const auto & route = selectedRoute(); + const auto vlm_index = vlmWaypointIndex(route); + latest_vlm_result_.clear(); + vlm_capture_triggered_ = false; + resetRouteRecoveryState(); + const auto split_at_vlm = split_qr_to_vlm_segment_ && + vlm_capture_mode_ == VlmCaptureMode::Stop; + + active_segment_ = split_at_vlm ? RouteSegment::ToVlm : RouteSegment::FullRoute; + active_segment_waypoints_.clear(); + if (use_post_qr_pose_) { + active_segment_waypoints_.push_back(post_qr_pose_); + } + + auto route_waypoints = route.waypoints; + if (vlm_index < route_waypoints.size()) { + route_waypoints[vlm_index] = selectCandidatePose( + vlm_candidate_group_name_, preferredCandidateSourceFile(), + route_waypoints[vlm_index]).value_or(route_waypoints[vlm_index]); + } + const auto selected_entry_pose = selectCandidatePose( + entry_candidate_group_name_, preferredCandidateSourceFile(), + entry_pose_).value_or(entry_pose_); + const auto qr_segment = routeWaypointsAfterQr( + selected_entry_pose, route_waypoints, vlm_index, split_at_vlm); + active_segment_waypoints_.insert( + active_segment_waypoints_.end(), qr_segment.begin(), qr_segment.end()); + if (!split_at_vlm) { + active_segment_waypoints_.push_back(route.home_pose); + } + active_segment_next_waypoint_index_ = 0; + runRouteSegmentPlanning(split_at_vlm ? "QR to VLM waypoint" : "QR through full route to home"); + } + + void runRemainingRouteSegment() + { + const auto & route = selectedRoute(); + const auto vlm_index = vlmWaypointIndex(route); + resetRouteRecoveryState(); + active_segment_ = RouteSegment::AfterVlm; + active_segment_waypoints_ = remainingWaypoints(route.waypoints, vlm_index + 1); + active_segment_waypoints_.push_back(route.home_pose); + active_segment_next_waypoint_index_ = 0; + publishSign(sign_profile_normal_); + runRouteSegmentPlanning("VLM to home"); + } + + void runRouteSegmentPlanning(const std::string & label) + { + if (active_segment_waypoints_.empty()) { + failRace("route segment must contain at least one pose"); + return; + } + startStage(Stage::ComputeCirclePath, path_planning_timeout_sec_); + + if (!compute_path_client_->wait_for_action_server(2s)) { + onRoutePlanningFailed("ComputePathThroughPoses action server is not available"); + return; + } + + ComputePathThroughPoses::Goal goal; + goal.goals = stampPoses(active_segment_waypoints_); + goal.planner_id = planner_id_; + goal.use_start = false; + + auto options = rclcpp_action::Client::SendGoalOptions(); + options.goal_response_callback = + [this](ComputeGoalHandle::SharedPtr goal_handle) { + if (stage_ != Stage::ComputeCirclePath) { + RCLCPP_DEBUG(get_logger(), "stale route planning goal response ignored"); + return; + } + if (!goal_handle) { + onRoutePlanningFailed("circle path planning goal was rejected"); + } + }; + options.result_callback = + [this](const ComputeGoalHandle::WrappedResult & result) { + if (stage_ != Stage::ComputeCirclePath) { + return; + } + if (result.code != rclcpp_action::ResultCode::SUCCEEDED || + !result.result || result.result->path.poses.empty()) + { + finishStage("planning failed"); + onRoutePlanningFailed("circle path planning failed"); + return; + } + active_path_ = result.result->path; + finishStage( + "planned " + std::to_string(active_path_.poses.size()) + " path poses"); + runRouteSegmentExecution(); + }; + + compute_path_client_->async_send_goal(goal, options); + RCLCPP_INFO(get_logger(), "planning selected route segment: %s", label.c_str()); + } + + void runRouteSegmentExecution() + { + startStage(Stage::ExecuteCirclePath, circle_timeout_sec_); + dynamic_replan_in_flight_ = false; + dynamic_replan_consecutive_failures_ = 0; + last_dynamic_replan_time_ = now(); + if (use_trajectory_guard_) { + auto path = stampPath(active_path_); + guard_path_pub_->publish(path); + RCLCPP_INFO( + get_logger(), "published route path poses=%zu to %s", + path.poses.size(), guard_input_topic_.c_str()); + return; + } + sendActiveRouteFollowPath(); + } + + void sendActiveRouteFollowPath() + { + sendFollowPath( + active_path_, [this](const bool ok) { + finishStage(ok ? "FollowPath succeeded" : "FollowPath failed"); + if (!ok) { + onRouteFollowFailed("route segment FollowPath failed"); + return; + } + if (active_segment_ == RouteSegment::ToQr) { + runQrWait(); + } else if (active_segment_ == RouteSegment::ToVlm) { + runVlmWait(); + } else if (active_segment_ == RouteSegment::AfterVlm || + active_segment_ == RouteSegment::FullRoute) + { + finishRace(); + } + }); + } + + void runSwitchToNormalProfile() + { + publishSign(sign_profile_normal_); + startStage(Stage::SwitchToNormalProfile, profile_switch_wait_sec_ + 2.0); + } + + void runVlmWait() + { + triggerVlmCaptureOnce("stopped at VLM waypoint"); + startStage(Stage::WaitForVlm, vlm_capture_wait_sec_ + 2.0); + } + + void triggerVlmCaptureOnce(const std::string & reason) + { + if (vlm_capture_triggered_) { + return; + } + vlm_capture_triggered_ = true; + publishSingleVlmImageFrame(); + publishSign(sign_vlm_trigger_); + RCLCPP_INFO(get_logger(), "VLM capture triggered: %s", reason.c_str()); + } + + void publishSingleVlmImageFrame() + { + sensor_msgs::msg::CompressedImage::SharedPtr image; + { + std::lock_guard lock(image_mutex_); + if (latest_image_) { + image = std::make_shared(*latest_image_); + } + } + + if (!shouldPublishVlmImageFrame(enable_vlm_image_relay_, static_cast(image))) { + if (enable_vlm_image_relay_) { + RCLCPP_WARN( + get_logger(), "VLM image relay enabled but no image has been received from %s", + vlm_image_input_topic_.c_str()); + } + return; + } + + image->header.stamp = now(); + vlm_image_pub_->publish(*image); + RCLCPP_INFO( + get_logger(), "published one VLM image frame %s -> %s", + vlm_image_input_topic_.c_str(), vlm_image_output_topic_.c_str()); + } + + void onGlobalCostmap(nav_msgs::msg::OccupancyGrid::SharedPtr msg) + { + std::lock_guard lock(costmap_mutex_); + latest_global_costmap_ = std::move(msg); + candidate_selector_.setCostmap(*latest_global_costmap_); + } + + std::optional selectCandidatePose( + const std::string & group_name, + const std::string & preferred_source_file, + const geometry_msgs::msg::PoseStamped & fallback_pose) + { + if (!enable_candidate_waypoint_selection_) { + return fallback_pose; + } + { + std::lock_guard lock(costmap_mutex_); + if (!latest_global_costmap_) { + RCLCPP_WARN( + get_logger(), "candidate selection fallback for %s: no global costmap yet", + group_name.c_str()); + return fallback_pose; + } + } + const auto selected = candidate_selector_.select( + group_name, 50, false, preferred_source_file); + if (!selected) { + RCLCPP_WARN( + get_logger(), "candidate selection fallback for %s: no usable candidate", + group_name.c_str()); + return fallback_pose; + } + RCLCPP_INFO( + get_logger(), "candidate selected for %s from %s -> %s", + group_name.c_str(), selected->source_file.c_str(), selected->point_name.c_str()); + return selected->pose; + } + + void maybeTriggerPassThroughVlmCapture() + { + if (active_segment_ != RouteSegment::FullRoute || vlm_capture_triggered_) { + return; + } + + const auto & route = selectedRoute(); + const auto vlm_index = vlmWaypointIndex(route); + geometry_msgs::msg::PoseStamped current; + { + std::lock_guard lock(odom_mutex_); + if (!latest_odom_) { + return; + } + current.header = latest_odom_->header; + current.pose = latest_odom_->pose.pose; + } + + if (distance2d(current, route.waypoints[vlm_index]) <= pass_through_vlm_trigger_radius_) { + triggerVlmCaptureOnce("passing VLM waypoint"); + } + } + + void maybeRunDynamicReplanning() + { + if (use_trajectory_guard_ || active_segment_waypoints_.empty()) { + return; + } + const auto current = currentPoseFromOdom(); + if (!current) { + return; + } + updateActiveSegmentWaypointProgress(*current); + if (active_segment_next_waypoint_index_ >= active_segment_waypoints_.size()) { + return; + } + const auto distance_to_goal = distance2d(*current, active_segment_waypoints_.back()); + const auto elapsed = (now() - last_dynamic_replan_time_).seconds(); + if (!shouldDynamicReplan( + enable_dynamic_replanning_, dynamic_replan_in_flight_, distance_to_goal, + dynamic_replan_stop_distance_, elapsed, dynamic_replan_interval_sec_)) + { + return; + } + + last_dynamic_replan_time_ = now(); + dynamic_replan_in_flight_ = true; + runDynamicRouteReplanning(*current); + } + + void runDynamicRouteReplanning(const geometry_msgs::msg::PoseStamped & current) + { + if (!compute_path_client_->wait_for_action_server(200ms)) { + onDynamicReplanFailed("ComputePathThroughPoses action server is not available"); + return; + } + + const auto replan_goals = remainingWaypointsAfterProgress( + active_segment_waypoints_, current, active_segment_next_waypoint_index_, + circle_goal_tolerance_); + if (replan_goals.empty()) { + dynamic_replan_in_flight_ = false; + return; + } + + ComputePathThroughPoses::Goal goal; + goal.goals = stampPoses(replan_goals); + goal.planner_id = planner_id_; + goal.use_start = false; + + auto options = rclcpp_action::Client::SendGoalOptions(); + options.goal_response_callback = + [this](ComputeGoalHandle::SharedPtr goal_handle) { + if (!goal_handle) { + onDynamicReplanFailed("dynamic route planning goal was rejected"); + } + }; + options.result_callback = + [this](const ComputeGoalHandle::WrappedResult & result) { + if (stage_ != Stage::ExecuteCirclePath) { + dynamic_replan_in_flight_ = false; + return; + } + dynamic_replan_in_flight_ = false; + if (result.code != rclcpp_action::ResultCode::SUCCEEDED || + result.result->path.poses.empty()) + { + onDynamicReplanFailed("dynamic route planning failed"); + return; + } + dynamic_replan_consecutive_failures_ = 0; + active_path_ = result.result->path; + RCLCPP_INFO( + get_logger(), "dynamic replan succeeded: path poses=%zu", + active_path_.poses.size()); + sendActiveRouteFollowPath(); + }; + + compute_path_client_->async_send_goal(goal, options); + RCLCPP_INFO( + get_logger(), "dynamic route replanning through %zu remaining waypoint(s), next index=%zu", + replan_goals.size(), active_segment_next_waypoint_index_); + } + + void updateActiveSegmentWaypointProgress(const geometry_msgs::msg::PoseStamped & current) + { + const auto previous_index = active_segment_next_waypoint_index_; + active_segment_next_waypoint_index_ = advanceReachedWaypointIndex( + active_segment_waypoints_, current, active_segment_next_waypoint_index_, + circle_goal_tolerance_); + if (active_segment_next_waypoint_index_ != previous_index) { + RCLCPP_INFO( + get_logger(), "route waypoint progress advanced: next index %zu -> %zu", + previous_index, active_segment_next_waypoint_index_); + } + } + + void onDynamicReplanFailed(const std::string & reason) + { + dynamic_replan_in_flight_ = false; + ++dynamic_replan_consecutive_failures_; + RCLCPP_WARN( + get_logger(), "%s | consecutive dynamic replan failures=%d", + reason.c_str(), dynamic_replan_consecutive_failures_); + if (dynamic_replan_consecutive_failures_ >= dynamic_replan_max_consecutive_failures_) { + onRoutePlanningFailed(reason); + } + } + + void resetRouteRecoveryState() + { + route_recovery_phase_ = RouteRecoveryPhase::Initial; + planning_backup_attempted_ = false; + final_recovery_attempts_ = 0; + } + + void prepareRouteRecoveryReplan() + { + dynamic_replan_consecutive_failures_ = 0; + const auto current = currentPoseFromOdom(); + if (current) { + active_segment_waypoints_ = remainingWaypointsAfterProgress( + active_segment_waypoints_, *current, active_segment_next_waypoint_index_, + circle_goal_tolerance_); + } else { + active_segment_waypoints_ = remainingWaypoints( + active_segment_waypoints_, active_segment_next_waypoint_index_); + } + active_segment_next_waypoint_index_ = 0; + } + + void onRoutePlanningFailed(const std::string & reason) + { + const auto action = routeRecoveryAction( + route_recovery_phase_, RouteFailureKind::Planning, + final_recovery_attempts_, max_recovery_attempts_); + if (action == RouteRecoveryAction::Fail) { + failRace(reason); + return; + } + + if (action == RouteRecoveryAction::PlanningBackup) { + planning_backup_attempted_ = true; + route_recovery_phase_ = RouteRecoveryPhase::AfterPlanningBackup; + RCLCPP_WARN( + get_logger(), "%s; planning failed, backing up at %.2f m/s for %.2f s before one replan", + reason.c_str(), planning_failure_backup_speed_, planning_failure_backup_timeout_sec_); + startBackupRecovery( + reason, planning_failure_backup_speed_, planning_failure_backup_distance_, + planning_failure_backup_timeout_sec_, [this]() { + prepareRouteRecoveryReplan(); + runRouteSegmentPlanning("after planning-failure backup"); + }); + return; + } + + ++final_recovery_attempts_; + route_recovery_phase_ = RouteRecoveryPhase::AfterFinalBackupReplan; + RCLCPP_WARN( + get_logger(), "%s; final backup/replan attempt %d/%d", + reason.c_str(), final_recovery_attempts_, max_recovery_attempts_); + startBackupRecovery( + reason, recovery_backup_speed_, recovery_backup_distance_, recovery_backup_timeout_sec_, + [this]() { + prepareRouteRecoveryReplan(); + runRouteSegmentPlanning("after final backup"); + }); + } + + void onRouteFollowFailed(const std::string & reason) + { + const auto action = routeRecoveryAction( + route_recovery_phase_, RouteFailureKind::Following, + final_recovery_attempts_, max_recovery_attempts_); + if (action == RouteRecoveryAction::Fail) { + failRace(reason); + return; + } + + if (action == RouteRecoveryAction::ClearLocalAndReplan) { + route_recovery_phase_ = RouteRecoveryPhase::AfterLocalClearReplan; + RCLCPP_WARN( + get_logger(), "%s; clearing local costmap, waiting for update, then replanning once", + reason.c_str()); + clearLocalCostmapBeforeRecoveryReplan( + [this]() { + prepareRouteRecoveryReplan(); + runRouteSegmentPlanning("after local costmap clear"); + }); + return; + } + + ++final_recovery_attempts_; + route_recovery_phase_ = RouteRecoveryPhase::AfterFinalBackupReplan; + RCLCPP_WARN( + get_logger(), "%s; final backup/replan attempt %d/%d", + reason.c_str(), final_recovery_attempts_, max_recovery_attempts_); + startBackupRecovery( + reason, recovery_backup_speed_, recovery_backup_distance_, recovery_backup_timeout_sec_, + [this]() { + prepareRouteRecoveryReplan(); + runRouteSegmentPlanning("after final backup"); + }); + } + + void startBackupRecovery( + const std::string & reason, + const double speed, + const double distance, + const double timeout_sec, + std::function on_recovered) + { + if (!enable_recovery_) { + failRace(reason); + return; + } + recovery_in_progress_ = true; + recovery_done_callback_ = std::move(on_recovered); + recovery_start_time_ = now(); + recovery_start_pose_ = currentPoseFromOdom(); + recovery_active_speed_ = speed; + recovery_active_distance_ = distance; + recovery_active_timeout_sec_ = timeout_sec; + if (recovery_clear_wait_timer_) { + recovery_clear_wait_timer_->cancel(); + } + recovery_after_clear_callback_ = nullptr; + recovery_costmap_clear_pending_ = 0; + cancelActiveFollowGoal(); + + RCLCPP_WARN( + get_logger(), "%s; backup recovery speed=%.2f m/s distance=%.2f m timeout=%.2f s", + reason.c_str(), speed, distance, timeout_sec); + + if (recovery_timer_) { + recovery_timer_->cancel(); + } + recovery_timer_ = create_wall_timer(20ms, [this]() {tickRecoveryBackup();}); + } + + void tickRecoveryBackup() + { + const auto elapsed = (now() - recovery_start_time_).seconds(); + const auto backup_distance = recoveryBackupDistance(); + if (!recoveryBackupComplete( + backup_distance, recovery_active_distance_, elapsed, recovery_active_timeout_sec_)) + { + publishRecoveryVelocity(recovery_active_speed_); + return; + } + + publishRecoveryVelocity(0.0); + if (recovery_timer_) { + recovery_timer_->cancel(); + } + recovery_in_progress_ = false; + auto callback = std::move(recovery_done_callback_); + recovery_done_callback_ = nullptr; + if (callback) { + callback(); + } + } + + void clearLocalCostmapBeforeRecoveryReplan(std::function on_cleared) + { + recovery_after_clear_callback_ = std::move(on_cleared); + recovery_costmap_clear_pending_ = 0; + const auto requested = requestEntireCostmapClear( + clear_local_costmap_client_, local_costmap_clear_service_); + if (!requested) { + startRecoveryReplanAfterClearWait(); + } + } + + bool requestEntireCostmapClear( + const rclcpp::Client::SharedPtr & client, + const std::string & service_name) + { + if (!client) { + return false; + } + if (!client->wait_for_service(200ms)) { + RCLCPP_WARN( + get_logger(), "costmap clear service not available: %s", service_name.c_str()); + return false; + } + + ++recovery_costmap_clear_pending_; + auto request = std::make_shared(); + client->async_send_request( + request, + [this, service_name](rclcpp::Client::SharedFuture) { + if (recovery_costmap_clear_pending_ > 0) { + --recovery_costmap_clear_pending_; + } + RCLCPP_INFO(get_logger(), "cleared costmap via %s", service_name.c_str()); + if (recovery_costmap_clear_pending_ == 0) { + startRecoveryReplanAfterClearWait(); + } + }); + return true; + } + + void startRecoveryReplanAfterClearWait() + { + if (!recovery_after_clear_callback_) { + return; + } + + if (recovery_clear_wait_timer_) { + recovery_clear_wait_timer_->cancel(); + } + + if (recovery_clear_wait_sec_ <= 0.0) { + auto callback = std::move(recovery_after_clear_callback_); + recovery_after_clear_callback_ = nullptr; + if (callback) { + callback(); + } + return; + } + + recovery_clear_wait_timer_ = create_wall_timer( + std::chrono::duration(recovery_clear_wait_sec_), + [this]() { + if (recovery_clear_wait_timer_) { + recovery_clear_wait_timer_->cancel(); + } + auto callback = std::move(recovery_after_clear_callback_); + recovery_after_clear_callback_ = nullptr; + if (callback) { + callback(); + } + }); + } + + double recoveryBackupDistance() const + { + if (!recovery_start_pose_) { + return 0.0; + } + const auto current = currentPoseFromOdom(); + if (!current) { + return 0.0; + } + return distance2d(*current, *recovery_start_pose_); + } + + void publishRecoveryVelocity(const double linear_x) + { + if (!recovery_cmd_vel_pub_) { + return; + } + geometry_msgs::msg::Twist cmd; + cmd.linear.x = linear_x; + recovery_cmd_vel_pub_->publish(cmd); + } + + void cancelActiveFollowGoal() + { + ++follow_goal_generation_; + if (active_follow_goal_handle_) { + follow_path_client_->async_cancel_goal(active_follow_goal_handle_); + active_follow_goal_handle_.reset(); + } + } + + void runReturnOrigin() + { + startStage(Stage::ReturnOrigin, navigation_timeout_sec_); + sendNavigateGoal( + selectedRoute().home_pose, [this](const bool ok) { + if (!ok) { + finishStage("navigation failed"); + failRace("failed to return origin"); + return; + } + finishRace(); + }); + } + + void sendNavigateGoal( + const geometry_msgs::msg::PoseStamped & pose, + std::function on_done) + { + sendNavigateGoalAttempt(pose, std::move(on_done), 3, stage_); + } + + void sendNavigateGoalAttempt( + const geometry_msgs::msg::PoseStamped & pose, + std::function on_done, + const int retries_remaining, + const Stage expected_stage) + { + if (!retryActionServerWait( + [this]() {return navigate_client_->wait_for_action_server(800ms);}, 3)) + { + failRace("NavigateToPose action server is not available"); + return; + } + + NavigateToPose::Goal goal; + goal.pose = stampPose(pose); + + auto options = rclcpp_action::Client::SendGoalOptions(); + options.goal_response_callback = + [this, pose, on_done, retries_remaining, expected_stage]( + NavigateGoalHandle::SharedPtr goal_handle) mutable { + if (!goal_handle) { + retryNavigateGoalOrFinish( + pose, std::move(on_done), retries_remaining, expected_stage, + "NavigateToPose goal was rejected"); + } + }; + options.result_callback = + [this, pose, on_done, retries_remaining, expected_stage]( + const NavigateGoalHandle::WrappedResult & result) mutable { + if (stage_ != expected_stage) { + RCLCPP_DEBUG(get_logger(), "stale NavigateToPose result ignored"); + return; + } + const bool succeeded = result.code == rclcpp_action::ResultCode::SUCCEEDED; + if (shouldRetryNavigateGoal(succeeded, retries_remaining)) { + retryNavigateGoalOrFinish( + pose, std::move(on_done), retries_remaining, expected_stage, + "NavigateToPose result was not successful"); + return; + } + on_done(succeeded); + }; + + navigate_client_->async_send_goal(goal, options); + } + + void retryNavigateGoalOrFinish( + const geometry_msgs::msg::PoseStamped & pose, + std::function on_done, + const int retries_remaining, + const Stage expected_stage, + const std::string & reason) + { + if (stage_ != expected_stage) { + RCLCPP_DEBUG(get_logger(), "stale NavigateToPose retry ignored: %s", reason.c_str()); + return; + } + if (!shouldRetryNavigateGoal(false, retries_remaining)) { + on_done(false); + return; + } + RCLCPP_WARN( + get_logger(), "%s; retrying NavigateToPose goal, retries remaining: %d", + reason.c_str(), retries_remaining); + sendNavigateGoalAttempt(pose, std::move(on_done), retries_remaining - 1, expected_stage); + } + + void sendFollowPath(const nav_msgs::msg::Path & path, std::function on_done) + { + if (!follow_path_client_->wait_for_action_server(2s)) { + onRouteFollowFailed("FollowPath action server is not available"); + return; + } + + cancelActiveFollowGoal(); + const auto generation = follow_goal_generation_; + FollowPath::Goal goal; + goal.path = stampPath(path); + goal.controller_id = controller_id_; + goal.goal_checker_id = goal_checker_id_; + + auto callback = std::move(on_done); + auto options = rclcpp_action::Client::SendGoalOptions(); + options.goal_response_callback = + [this, generation, callback](FollowGoalHandle::SharedPtr goal_handle) mutable { + if (generation != follow_goal_generation_) { + return; + } + if (!goal_handle) { + active_follow_goal_handle_.reset(); + callback(false); + return; + } + active_follow_goal_handle_ = goal_handle; + }; + options.result_callback = + [this, callback, generation]( + const FollowGoalHandle::WrappedResult & result) mutable { + if (generation != follow_goal_generation_) { + RCLCPP_DEBUG(get_logger(), "stale FollowPath result ignored"); + return; + } + active_follow_goal_handle_.reset(); + callback(result.code == rclcpp_action::ResultCode::SUCCEEDED); + }; + + follow_path_client_->async_send_goal(goal, options); + } + + geometry_msgs::msg::PoseStamped stampPose(geometry_msgs::msg::PoseStamped pose) + { + pose.header.stamp = now(); + if (pose.header.frame_id.empty()) { + pose.header.frame_id = frame_id_; + } + return pose; + } + + std::vector stampPoses( + std::vector poses) + { + for (auto & pose : poses) { + pose = stampPose(pose); + } + return poses; + } + + nav_msgs::msg::Path stampPath(nav_msgs::msg::Path path) + { + path.header.frame_id = path.header.frame_id.empty() ? frame_id_ : path.header.frame_id; + path.header.stamp = now(); + for (auto & pose : path.poses) { + pose.header.stamp = path.header.stamp; + if (pose.header.frame_id.empty()) { + pose.header.frame_id = path.header.frame_id; + } + } + return path; + } + + std::optional currentPoseFromOdom() const + { + std::lock_guard lock(odom_mutex_); + if (!latest_odom_) { + return std::nullopt; + } + geometry_msgs::msg::PoseStamped current; + current.header = latest_odom_->header; + current.pose = latest_odom_->pose.pose; + return current; + } + + std::size_t vlmWaypointIndex(const RouteConfig & route) const + { + if (vlm_waypoint_number_ <= 0) { + throw std::runtime_error("vlm_waypoint_number must be >= 1"); + } + const auto index = static_cast(vlm_waypoint_number_ - 1); + if (index >= route.waypoints.size()) { + throw std::runtime_error("vlm_waypoint_number exceeds selected route waypoint count"); + } + return index; + } + + const RouteConfig & selectedRoute() const + { + if (selected_direction_ == RouteDirection::Clockwise) { + return clockwise_route_; + } + if (selected_direction_ == RouteDirection::Counterclockwise) { + return counterclockwise_route_; + } + throw std::runtime_error("route direction is not selected"); + } + + std::string preferredCandidateSourceFile() const + { + if (selected_direction_ == RouteDirection::Clockwise) { + return clockwise_candidate_source_file_; + } + if (selected_direction_ == RouteDirection::Counterclockwise) { + return counterclockwise_candidate_source_file_; + } + return ""; + } + + bool routeSegmentReached() const + { + std::lock_guard lock(odom_mutex_); + if (!latest_odom_ || active_segment_waypoints_.empty()) { + return false; + } + geometry_msgs::msg::PoseStamped current; + current.header = latest_odom_->header; + current.pose = latest_odom_->pose.pose; + return distance2d(current, active_segment_waypoints_.back()) <= circle_goal_tolerance_; + } + + void onOdom(nav_msgs::msg::Odometry::SharedPtr msg) + { + std::lock_guard lock(odom_mutex_); + latest_odom_ = std::move(msg); + } + + void onImage(sensor_msgs::msg::CompressedImage::SharedPtr msg) + { + std::lock_guard lock(image_mutex_); + latest_image_ = std::move(msg); + } + + void onQrResult(std_msgs::msg::String::SharedPtr msg) + { + if (msg->data.empty()) { + return; + } + const auto direction = directionFromQrResult(msg->data); + if (direction == RouteDirection::Unknown) { + RCLCPP_WARN( + get_logger(), "QR result received but route direction is unknown: %s", + msg->data.c_str()); + return; + } + if (!shouldAcceptQrDirection(selected_direction_, direction)) { + RCLCPP_DEBUG( + get_logger(), "QR result ignored after route direction was selected: %s", + msg->data.c_str()); + return; + } + latest_qr_result_ = msg->data; + qr_result_time_ = now(); + selected_direction_ = direction; + RCLCPP_INFO( + get_logger(), "QR result received: %s -> %s", + latest_qr_result_.c_str(), selectedRoute().label.c_str()); + if (stage_ == Stage::NavigateToQr || stage_ == Stage::WaitForQr || + ((stage_ == Stage::ComputeCirclePath || stage_ == Stage::ExecuteCirclePath) && + active_segment_ == RouteSegment::ToQr)) + { + finishStage("QR result: " + latest_qr_result_ + " -> " + selectedRoute().label); + runQrTransitNavigation(); + } + } + + void onVlmResult(std_msgs::msg::String::SharedPtr msg) + { + if (msg->data.empty()) { + return; + } + latest_vlm_result_ = msg->data; + vlm_result_time_ = now(); + RCLCPP_INFO(get_logger(), "VLM result received: %s", latest_vlm_result_.c_str()); + } + + Stage stage_{Stage::Idle}; + bool race_started_{false}; + bool auto_start_{false}; + bool use_trajectory_guard_{false}; + bool use_post_qr_pose_{false}; + bool split_qr_to_vlm_segment_{true}; + bool enable_vlm_image_relay_{false}; + bool enable_dynamic_replanning_{true}; + bool enable_recovery_{true}; + bool enable_candidate_waypoint_selection_{true}; + bool qr_detection_disabled_{false}; + bool dynamic_replan_in_flight_{false}; + bool recovery_in_progress_{false}; + rclcpp::Time race_start_{0, 0, RCL_ROS_TIME}; + rclcpp::Time stage_start_{0, 0, RCL_ROS_TIME}; + rclcpp::Time last_dynamic_replan_time_{0, 0, RCL_ROS_TIME}; + rclcpp::Time recovery_start_time_{0, 0, RCL_ROS_TIME}; + double stage_timeout_sec_{0.0}; + + std::string frame_id_; + std::string sign_topic_; + std::string qr_result_topic_; + std::string vlm_result_topic_; + std::string odom_topic_; + std::string recovery_cmd_vel_topic_; + std::string global_costmap_topic_; + std::string global_costmap_clear_service_; + std::string local_costmap_clear_service_; + std::string vlm_image_input_topic_; + std::string vlm_image_output_topic_; + std::string navigate_action_; + std::string compute_path_action_; + std::string follow_path_action_; + std::string guard_input_topic_; + std::string planner_id_; + std::string controller_id_; + std::string goal_checker_id_; + std::string candidate_waypoint_json_dir_; + std::string qr_candidate_group_name_; + std::string entry_candidate_group_name_; + std::string vlm_candidate_group_name_; + std::string clockwise_candidate_source_file_; + std::string counterclockwise_candidate_source_file_; + + double navigation_timeout_sec_{120.0}; + double path_planning_timeout_sec_{30.0}; + double circle_timeout_sec_{120.0}; + double qr_result_timeout_sec_{8.0}; + double profile_switch_wait_sec_{1.0}; + double post_qr_wait_sec_{1.0}; + double vlm_capture_wait_sec_{0.5}; + double dynamic_replan_interval_sec_{1.0}; + double dynamic_replan_stop_distance_{0.5}; + double recovery_backup_speed_{-0.2}; + double recovery_backup_distance_{0.04}; + double recovery_backup_timeout_sec_{0.2}; + double planning_failure_backup_speed_{-0.1}; + double planning_failure_backup_distance_{0.03}; + double planning_failure_backup_timeout_sec_{0.3}; + double recovery_clear_wait_sec_{1.0}; + double recovery_active_speed_{0.0}; + double recovery_active_distance_{0.0}; + double recovery_active_timeout_sec_{0.0}; + double recovery_search_radius_m_{1.0}; + double recovery_rear_clear_distance_m_{0.5}; + double pass_through_vlm_trigger_radius_{0.35}; + double circle_goal_tolerance_{0.30}; + + int sign_qr_enable_{0}; + int sign_qr_disable_{5}; + int sign_vlm_trigger_{9}; + int sign_profile_normal_{10}; + int sign_profile_task2_{11}; + int vlm_waypoint_number_{2}; + int dynamic_replan_max_consecutive_failures_{3}; + int dynamic_replan_consecutive_failures_{0}; + int max_recovery_attempts_{2}; + int final_recovery_attempts_{0}; + + geometry_msgs::msg::PoseStamped qr_pose_; + geometry_msgs::msg::PoseStamped post_qr_pose_; + geometry_msgs::msg::PoseStamped entry_pose_; + RouteConfig clockwise_route_; + RouteConfig counterclockwise_route_; + RouteDirection selected_direction_{RouteDirection::Unknown}; + VlmCaptureMode vlm_capture_mode_{VlmCaptureMode::Stop}; + RouteSegment active_segment_{RouteSegment::None}; + RouteRecoveryPhase route_recovery_phase_{RouteRecoveryPhase::Initial}; + bool planning_backup_attempted_{false}; + std::vector active_segment_waypoints_; + std::size_t active_segment_next_waypoint_index_{0}; + nav_msgs::msg::Path active_path_; + bool vlm_capture_triggered_{false}; + std::optional recovery_start_pose_; + std::function recovery_done_callback_; + std::function recovery_after_clear_callback_; + std::size_t recovery_costmap_clear_pending_{0}; + + std::string latest_qr_result_; + std::string latest_vlm_result_; + rclcpp::Time qr_result_time_{0, 0, RCL_ROS_TIME}; + rclcpp::Time vlm_result_time_{0, 0, RCL_ROS_TIME}; + nav_msgs::msg::Odometry::SharedPtr latest_odom_; + nav_msgs::msg::OccupancyGrid::SharedPtr latest_global_costmap_; + sensor_msgs::msg::CompressedImage::SharedPtr latest_image_; + mutable std::mutex odom_mutex_; + mutable std::mutex costmap_mutex_; + mutable std::mutex image_mutex_; + + rclcpp::Publisher::SharedPtr sign_pub_; + rclcpp::Publisher::SharedPtr guard_path_pub_; + rclcpp::Publisher::SharedPtr vlm_image_pub_; + rclcpp::Publisher::SharedPtr recovery_cmd_vel_pub_; + rclcpp::Subscription::SharedPtr qr_sub_; + rclcpp::Subscription::SharedPtr vlm_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::SharedPtr global_costmap_sub_; + rclcpp::Subscription::SharedPtr image_sub_; + rclcpp_action::Client::SharedPtr navigate_client_; + rclcpp_action::Client::SharedPtr compute_path_client_; + rclcpp_action::Client::SharedPtr follow_path_client_; + rclcpp::Client::SharedPtr clear_global_costmap_client_; + rclcpp::Client::SharedPtr clear_local_costmap_client_; + FollowGoalHandle::SharedPtr active_follow_goal_handle_; + rclcpp::TimerBase::SharedPtr startup_qr_enable_timer_; + rclcpp::TimerBase::SharedPtr recovery_timer_; + rclcpp::TimerBase::SharedPtr recovery_clear_wait_timer_; + rclcpp::TimerBase::SharedPtr tick_timer_; + CandidateWaypointSelector candidate_selector_; + + int startup_qr_enable_publish_count_{0}; + std::uint64_t follow_goal_generation_{0}; + std::atomic start_requested_{false}; + std::atomic stop_keyboard_{false}; + std::thread keyboard_thread_; +}; + +} // namespace racing_control + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + try { + rclcpp::spin(std::make_shared()); + } catch (const std::exception & e) { + RCLCPP_FATAL(rclcpp::get_logger("racing_control"), "%s", e.what()); + rclcpp::shutdown(); + return 1; + } + rclcpp::shutdown(); + return 0; +} diff --git a/src/racing_control/test/test_racing_control_helpers.cpp b/src/racing_control/test/test_racing_control_helpers.cpp index 9972c94..d87365a 100644 --- a/src/racing_control/test/test_racing_control_helpers.cpp +++ b/src/racing_control/test/test_racing_control_helpers.cpp @@ -1,7 +1,11 @@ #include +#include +#include #include #include "gtest/gtest.h" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "racing_control/candidate_waypoint_selector.hpp" #include "racing_control/racing_control.hpp" namespace @@ -216,6 +220,57 @@ TEST(RacingControlHelpers, RecoveryBackupStopsByDistanceOrTimeout) EXPECT_FALSE(racing_control::recoveryBackupComplete(0.01, 0.04, 0.1, 0.2)); } +TEST(RacingControlHelpers, InitialPlanningFailureUsesOnePlanningBackup) +{ + EXPECT_EQ( + racing_control::routeRecoveryAction( + racing_control::RouteRecoveryPhase::Initial, + racing_control::RouteFailureKind::Planning, 0, 2), + racing_control::RouteRecoveryAction::PlanningBackup); + EXPECT_EQ( + racing_control::routeRecoveryAction( + racing_control::RouteRecoveryPhase::AfterPlanningBackup, + racing_control::RouteFailureKind::Planning, 0, 2), + racing_control::RouteRecoveryAction::Fail); +} + +TEST(RacingControlHelpers, FollowFailureClearsOnlyLocalCostmapBeforeReplanning) +{ + EXPECT_EQ( + racing_control::routeRecoveryAction( + racing_control::RouteRecoveryPhase::Initial, + racing_control::RouteFailureKind::Following, 0, 2), + racing_control::RouteRecoveryAction::ClearLocalAndReplan); +} + +TEST(RacingControlHelpers, FailedLocalClearReplanUsesFinalBackup) +{ + EXPECT_EQ( + racing_control::routeRecoveryAction( + racing_control::RouteRecoveryPhase::AfterLocalClearReplan, + racing_control::RouteFailureKind::Planning, 0, 2), + racing_control::RouteRecoveryAction::FinalBackupReplan); + EXPECT_EQ( + racing_control::routeRecoveryAction( + racing_control::RouteRecoveryPhase::AfterLocalClearReplan, + racing_control::RouteFailureKind::Following, 0, 2), + racing_control::RouteRecoveryAction::FinalBackupReplan); +} + +TEST(RacingControlHelpers, FinalRecoveryFailsAfterConfiguredAttemptLimit) +{ + EXPECT_EQ( + racing_control::routeRecoveryAction( + racing_control::RouteRecoveryPhase::AfterFinalBackupReplan, + racing_control::RouteFailureKind::Following, 1, 2), + racing_control::RouteRecoveryAction::FinalBackupReplan); + EXPECT_EQ( + racing_control::routeRecoveryAction( + racing_control::RouteRecoveryPhase::AfterFinalBackupReplan, + racing_control::RouteFailureKind::Planning, 2, 2), + racing_control::RouteRecoveryAction::Fail); +} + TEST(RacingControlHelpers, ParsesVlmCaptureMode) { EXPECT_EQ( @@ -228,3 +283,106 @@ TEST(RacingControlHelpers, ParsesVlmCaptureMode) racing_control::vlmCaptureModeFromString("unknown"), racing_control::VlmCaptureMode::Unknown); } + +TEST(CandidateWaypointSelector, LoadsNamedCandidatesFromSavedPointJson) +{ + const auto dir = std::filesystem::temp_directory_path() / "racing_control_candidates_test"; + std::filesystem::remove_all(dir); + std::filesystem::create_directories(dir); + { + std::ofstream json(dir / "main.json"); + json << + R"json({ + "points": [ + { + "name": "qr", + "yaw_degrees": 90.0, + "odom": { + "pose": { + "pose": { + "position": {"x": 1.0, "y": 2.0, "z": 0.0}, + "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0} + } + } + } + }, + { + "name": "entry", + "yaw_degrees": 0.0, + "odom": { + "pose": { + "pose": { + "position": {"x": 3.0, "y": 4.0, "z": 0.0}, + "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0} + } + } + } + } + ] + })json"; + } + + const auto groups = racing_control::loadCandidateWaypointGroups(dir.string(), "odom"); + + ASSERT_EQ(groups.size(), 2U); + ASSERT_EQ(groups.at("qr").size(), 1U); + EXPECT_DOUBLE_EQ(groups.at("qr")[0].pose.pose.position.x, 1.0); + EXPECT_DOUBLE_EQ(groups.at("qr")[0].pose.pose.position.y, 2.0); + EXPECT_NEAR(groups.at("qr")[0].pose.pose.orientation.z, std::sin(M_PI_4), kTolerance); + EXPECT_NEAR(groups.at("qr")[0].pose.pose.orientation.w, std::cos(M_PI_4), kTolerance); + EXPECT_EQ(groups.at("entry")[0].source_file, "main.json"); +} + +TEST(CandidateWaypointSelector, SkipsOccupiedCandidates) +{ + nav_msgs::msg::OccupancyGrid costmap; + costmap.info.resolution = 1.0; + costmap.info.width = 5; + costmap.info.height = 5; + costmap.info.origin.position.x = 0.0; + costmap.info.origin.position.y = 0.0; + costmap.info.origin.orientation.w = 1.0; + costmap.data.assign(25, 0); + costmap.data[1 * 5 + 1] = 80; + + racing_control::CandidateWaypointSelector selector; + selector.setGroups( + { + {"qr", { + {"qr", "blocked.json", "qr_blocked", racing_control::poseFromXYYaw(1.2, 1.2, 0.0, "odom")}, + {"qr", "free.json", "qr_free", racing_control::poseFromXYYaw(3.2, 1.2, 0.0, "odom")}, + }}, + }); + selector.setCostmap(costmap); + + const auto selected = selector.select("qr", 50, false); + + ASSERT_TRUE(selected.has_value()); + EXPECT_EQ(selected->source_file, "free.json"); + EXPECT_EQ(selected->point_name, "qr_free"); +} + +TEST(CandidateWaypointSelector, FindsNearestFreeRecoveryCommand) +{ + nav_msgs::msg::OccupancyGrid costmap; + costmap.info.resolution = 0.25; + costmap.info.width = 9; + costmap.info.height = 9; + costmap.info.origin.position.x = -1.0; + costmap.info.origin.position.y = -1.0; + costmap.info.origin.orientation.w = 1.0; + costmap.data.assign(81, 100); + const auto set_free = [&](const int mx, const int my) { + costmap.data[my * static_cast(costmap.info.width) + mx] = 0; + }; + set_free(6, 4); + set_free(7, 4); + + const auto current = racing_control::poseFromXYYaw(0.0, 0.0, 0.0, "odom"); + const auto command = racing_control::nearestFreeRecoveryCommand( + costmap, current, 50, false, 1.0); + + ASSERT_TRUE(command.has_value()); + EXPECT_GT(command->linear_x, 0.0); + EXPECT_DOUBLE_EQ(command->angular_z, 0.0); +} diff --git a/src/racing_control/test/test_racing_control_helpers.cpp.unc-backup.md5~ b/src/racing_control/test/test_racing_control_helpers.cpp.unc-backup.md5~ new file mode 100644 index 0000000..a92d3d5 --- /dev/null +++ b/src/racing_control/test/test_racing_control_helpers.cpp.unc-backup.md5~ @@ -0,0 +1 @@ +0af1c0d18ed8a7db4d7f58cb8d1f948a test_racing_control_helpers.cpp diff --git a/src/racing_control/test/test_racing_control_helpers.cpp.unc-backup~ b/src/racing_control/test/test_racing_control_helpers.cpp.unc-backup~ new file mode 100644 index 0000000..d23c65e --- /dev/null +++ b/src/racing_control/test/test_racing_control_helpers.cpp.unc-backup~ @@ -0,0 +1,386 @@ +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "racing_control/candidate_waypoint_selector.hpp" +#include "racing_control/racing_control.hpp" + +namespace +{ + +constexpr double kTolerance = 1e-6; + +} // namespace + +TEST(RacingControlHelpers, ConvertsFlatTriplesToStampedPoses) +{ + const auto poses = racing_control::posesFromFlatDoubles( + {1.0, 2.0, M_PI_2, -0.5, 0.25, -M_PI}, + "odom"); + + ASSERT_EQ(poses.size(), 2U); + EXPECT_EQ(poses[0].header.frame_id, "odom"); + EXPECT_DOUBLE_EQ(poses[0].pose.position.x, 1.0); + EXPECT_DOUBLE_EQ(poses[0].pose.position.y, 2.0); + EXPECT_NEAR(poses[0].pose.orientation.z, std::sin(M_PI_4), kTolerance); + EXPECT_NEAR(poses[0].pose.orientation.w, std::cos(M_PI_4), kTolerance); + EXPECT_DOUBLE_EQ(poses[1].pose.position.x, -0.5); + EXPECT_DOUBLE_EQ(poses[1].pose.position.y, 0.25); +} + +TEST(RacingControlHelpers, RejectsIncompletePoseTriples) +{ + EXPECT_THROW( + racing_control::posesFromFlatDoubles({1.0, 2.0, 0.0, 3.0}, "odom"), + std::invalid_argument); +} + +TEST(RacingControlHelpers, ParsesQrDirectionFromText) +{ + EXPECT_EQ( + racing_control::directionFromQrResult("7 顺时针"), + racing_control::RouteDirection::Clockwise); + EXPECT_EQ( + racing_control::directionFromQrResult("8 逆时针"), + racing_control::RouteDirection::Counterclockwise); + EXPECT_EQ( + racing_control::directionFromQrResult("5"), + racing_control::RouteDirection::Clockwise); + EXPECT_EQ( + racing_control::directionFromQrResult("6"), + racing_control::RouteDirection::Counterclockwise); + EXPECT_EQ( + racing_control::directionFromQrResult("未识别"), + racing_control::RouteDirection::Unknown); +} + +TEST(RacingControlHelpers, LatchesFirstKnownQrDirection) +{ + EXPECT_TRUE( + racing_control::shouldAcceptQrDirection( + racing_control::RouteDirection::Unknown, + racing_control::RouteDirection::Clockwise)); + EXPECT_FALSE( + racing_control::shouldAcceptQrDirection( + racing_control::RouteDirection::Unknown, + racing_control::RouteDirection::Unknown)); + EXPECT_FALSE( + racing_control::shouldAcceptQrDirection( + racing_control::RouteDirection::Clockwise, + racing_control::RouteDirection::Clockwise)); + EXPECT_FALSE( + racing_control::shouldAcceptQrDirection( + racing_control::RouteDirection::Clockwise, + racing_control::RouteDirection::Counterclockwise)); +} + +TEST(RacingControlHelpers, SelectsPostQrTransitTargetWhenEnabled) +{ + EXPECT_EQ( + racing_control::qrTransitTargetAfterRecognition(true), + racing_control::QrTransitTarget::PostQr); + EXPECT_EQ( + racing_control::qrTransitTargetAfterRecognition(false), + racing_control::QrTransitTarget::Entry); +} + +TEST(RacingControlHelpers, PublishesOneVlmImageFrameOnlyWhenEnabledAndAvailable) +{ + EXPECT_TRUE(racing_control::shouldPublishVlmImageFrame(true, true)); + EXPECT_FALSE(racing_control::shouldPublishVlmImageFrame(false, true)); + EXPECT_FALSE(racing_control::shouldPublishVlmImageFrame(true, false)); + EXPECT_FALSE(racing_control::shouldPublishVlmImageFrame(false, false)); +} + +TEST(RacingControlHelpers, DefaultsToPureNav2RouteExecution) +{ + EXPECT_FALSE(racing_control::defaultUseTrajectoryGuard()); +} + +TEST(RacingControlHelpers, RetriesActionServerWaitFourTimesBeforeFailing) +{ + int wait_calls = 0; + const bool result = racing_control::retryActionServerWait( + [&]() { + ++wait_calls; + return false; + }, + 3); + + EXPECT_FALSE(result); + EXPECT_EQ(wait_calls, 4); +} + +TEST(RacingControlHelpers, RetriesNavigateGoalOnlyAfterFailure) +{ + EXPECT_TRUE(racing_control::shouldRetryNavigateGoal(false, 3)); + EXPECT_FALSE(racing_control::shouldRetryNavigateGoal(true, 3)); + EXPECT_FALSE(racing_control::shouldRetryNavigateGoal(false, 0)); +} + +TEST(RacingControlHelpers, DefaultsToStartupQrEnableBurst) +{ + EXPECT_EQ(racing_control::defaultStartupQrEnableRepeats(), 5); + EXPECT_EQ(racing_control::defaultStartupQrEnableIntervalMs(), 200); +} + +TEST(RacingControlHelpers, DynamicReplanningStopsNearGoal) +{ + EXPECT_TRUE(racing_control::shouldDynamicReplan(true, false, 0.6, 0.5, 1.0, 1.0)); + EXPECT_FALSE(racing_control::shouldDynamicReplan(true, false, 0.4, 0.5, 1.0, 1.0)); + EXPECT_FALSE(racing_control::shouldDynamicReplan(true, true, 0.6, 0.5, 1.0, 1.0)); + EXPECT_FALSE(racing_control::shouldDynamicReplan(true, false, 0.6, 0.5, 0.5, 1.0)); + EXPECT_FALSE(racing_control::shouldDynamicReplan(false, false, 0.6, 0.5, 1.0, 1.0)); +} + +TEST(RacingControlHelpers, AdvancesNextWaypointOnlyAfterItIsReached) +{ + const auto waypoints = racing_control::posesFromFlatDoubles( + {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 2.0, 0.0, 0.0}, + "map"); + + EXPECT_EQ( + racing_control::advanceReachedWaypointIndex( + waypoints, racing_control::poseFromXYYaw(0.40, 0.0, 0.0, "map"), 1, 0.50), + 1U); + EXPECT_EQ( + racing_control::advanceReachedWaypointIndex( + waypoints, racing_control::poseFromXYYaw(1.05, 0.0, 0.0, "map"), 1, 0.50), + 2U); +} + +TEST(RacingControlHelpers, DynamicReplanningKeepsRemainingUnreachedWaypoints) +{ + const auto waypoints = racing_control::posesFromFlatDoubles( + {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 2.0, 0.0, 0.0}, + "map"); + + const auto remaining = racing_control::remainingWaypoints(waypoints, 1); + + ASSERT_EQ(remaining.size(), 2U); + EXPECT_DOUBLE_EQ(remaining[0].pose.position.x, 1.0); + EXPECT_DOUBLE_EQ(remaining[1].pose.position.x, 2.0); + EXPECT_TRUE(racing_control::remainingWaypoints(waypoints, waypoints.size()).empty()); + EXPECT_TRUE(racing_control::remainingWaypoints(waypoints, waypoints.size() + 1).empty()); +} + +TEST(RacingControlHelpers, TrimsPassedWaypointsBeforeReplanning) +{ + const auto waypoints = racing_control::posesFromFlatDoubles( + {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 2.0, 0.0, 0.0}, + "map"); + + const auto trimmed = racing_control::remainingWaypointsAfterProgress( + waypoints, racing_control::poseFromXYYaw(1.02, 0.0, 0.0, "map"), 1, 0.50); + + ASSERT_EQ(trimmed.size(), 1U); + EXPECT_DOUBLE_EQ(trimmed[0].pose.position.x, 2.0); +} + +TEST(RacingControlHelpers, BuildsQrToVlmSegmentThroughEntry) +{ + const std::string frame({'m', 'a', 'p'}); + const auto entry = racing_control::poseFromXYYaw(10.0, 0.0, 0.0, frame); + const auto route = racing_control::posesFromFlatDoubles( + {1.0, 0.0, 0.0, 2.0, 0.0, 0.0, 3.0, 0.0, 0.0}, + frame); + + const auto segment = racing_control::routeWaypointsAfterQr(entry, route, 1, true); + + ASSERT_EQ(segment.size(), 3U); + EXPECT_DOUBLE_EQ(segment[0].pose.position.x, 10.0); + EXPECT_DOUBLE_EQ(segment[1].pose.position.x, 1.0); + EXPECT_DOUBLE_EQ(segment[2].pose.position.x, 2.0); +} + +TEST(RacingControlHelpers, CanConnectQrThroughAllRemainingRoute) +{ + const std::string frame({'m', 'a', 'p'}); + const auto entry = racing_control::poseFromXYYaw(10.0, 0.0, 0.0, frame); + const auto route = racing_control::posesFromFlatDoubles( + {1.0, 0.0, 0.0, 2.0, 0.0, 0.0, 3.0, 0.0, 0.0}, + frame); + + const auto segment = racing_control::routeWaypointsAfterQr(entry, route, 1, false); + + ASSERT_EQ(segment.size(), 4U); + EXPECT_DOUBLE_EQ(segment[0].pose.position.x, 10.0); + EXPECT_DOUBLE_EQ(segment[1].pose.position.x, 1.0); + EXPECT_DOUBLE_EQ(segment[2].pose.position.x, 2.0); + EXPECT_DOUBLE_EQ(segment[3].pose.position.x, 3.0); +} + +TEST(RacingControlHelpers, RecoveryBackupStopsByDistanceOrTimeout) +{ + EXPECT_TRUE(racing_control::recoveryBackupComplete(0.04, 0.04, 0.1, 0.2)); + EXPECT_TRUE(racing_control::recoveryBackupComplete(0.01, 0.04, 0.2, 0.2)); + EXPECT_FALSE(racing_control::recoveryBackupComplete(0.01, 0.04, 0.1, 0.2)); +} + +TEST(RacingControlHelpers, InitialPlanningFailureUsesOnePlanningBackup) +{ + EXPECT_EQ( + racing_control::routeRecoveryAction( + racing_control::RouteRecoveryPhase::Initial, + racing_control::RouteFailureKind::Planning, 0, 2), + racing_control::RouteRecoveryAction::PlanningBackup); + EXPECT_EQ( + racing_control::routeRecoveryAction( + racing_control::RouteRecoveryPhase::AfterPlanningBackup, + racing_control::RouteFailureKind::Planning, 0, 2), + racing_control::RouteRecoveryAction::Fail); +} + +TEST(RacingControlHelpers, FollowFailureClearsOnlyLocalCostmapBeforeReplanning) +{ + EXPECT_EQ( + racing_control::routeRecoveryAction( + racing_control::RouteRecoveryPhase::Initial, + racing_control::RouteFailureKind::Following, 0, 2), + racing_control::RouteRecoveryAction::ClearLocalAndReplan); +} + +TEST(RacingControlHelpers, FailedLocalClearReplanUsesFinalBackup) +{ + EXPECT_EQ( + racing_control::routeRecoveryAction( + racing_control::RouteRecoveryPhase::AfterLocalClearReplan, + racing_control::RouteFailureKind::Planning, 0, 2), + racing_control::RouteRecoveryAction::FinalBackupReplan); + EXPECT_EQ( + racing_control::routeRecoveryAction( + racing_control::RouteRecoveryPhase::AfterLocalClearReplan, + racing_control::RouteFailureKind::Following, 0, 2), + racing_control::RouteRecoveryAction::FinalBackupReplan); +} + +TEST(RacingControlHelpers, FinalRecoveryFailsAfterConfiguredAttemptLimit) +{ + EXPECT_EQ( + racing_control::routeRecoveryAction( + racing_control::RouteRecoveryPhase::AfterFinalBackupReplan, + racing_control::RouteFailureKind::Following, 1, 2), + racing_control::RouteRecoveryAction::FinalBackupReplan); + EXPECT_EQ( + racing_control::routeRecoveryAction( + racing_control::RouteRecoveryPhase::AfterFinalBackupReplan, + racing_control::RouteFailureKind::Planning, 2, 2), + racing_control::RouteRecoveryAction::Fail); +} + +TEST(RacingControlHelpers, ParsesVlmCaptureMode) +{ + EXPECT_EQ( + racing_control::vlmCaptureModeFromString("stop"), + racing_control::VlmCaptureMode::Stop); + EXPECT_EQ( + racing_control::vlmCaptureModeFromString("pass_through"), + racing_control::VlmCaptureMode::PassThrough); + EXPECT_EQ( + racing_control::vlmCaptureModeFromString("unknown"), + racing_control::VlmCaptureMode::Unknown); +} + +TEST(CandidateWaypointSelector, LoadsNamedCandidatesFromSavedPointJson) +{ + const auto dir = std::filesystem::temp_directory_path() / "racing_control_candidates_test"; + std::filesystem::remove_all(dir); + std::filesystem::create_directories(dir); + { + std::ofstream json(dir / "main.json"); + json << R"json({ + "points": [ + { + "name": "qr", + "yaw_degrees": 90.0, + "odom": { + "pose": { + "pose": { + "position": {"x": 1.0, "y": 2.0, "z": 0.0}, + "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0} + } + } + } + }, + { + "name": "entry", + "yaw_degrees": 0.0, + "odom": { + "pose": { + "pose": { + "position": {"x": 3.0, "y": 4.0, "z": 0.0}, + "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0} + } + } + } + } + ] + })json"; + } + + const auto groups = racing_control::loadCandidateWaypointGroups(dir.string(), "odom"); + + ASSERT_EQ(groups.size(), 2U); + ASSERT_EQ(groups.at("qr").size(), 1U); + EXPECT_DOUBLE_EQ(groups.at("qr")[0].pose.pose.position.x, 1.0); + EXPECT_DOUBLE_EQ(groups.at("qr")[0].pose.pose.position.y, 2.0); + EXPECT_NEAR(groups.at("qr")[0].pose.pose.orientation.z, std::sin(M_PI_4), kTolerance); + EXPECT_NEAR(groups.at("qr")[0].pose.pose.orientation.w, std::cos(M_PI_4), kTolerance); + EXPECT_EQ(groups.at("entry")[0].source_file, "main.json"); +} + +TEST(CandidateWaypointSelector, SkipsOccupiedCandidates) +{ + nav_msgs::msg::OccupancyGrid costmap; + costmap.info.resolution = 1.0; + costmap.info.width = 5; + costmap.info.height = 5; + costmap.info.origin.position.x = 0.0; + costmap.info.origin.position.y = 0.0; + costmap.info.origin.orientation.w = 1.0; + costmap.data.assign(25, 0); + costmap.data[1 * 5 + 1] = 80; + + racing_control::CandidateWaypointSelector selector; + selector.setGroups({ + {"qr", { + {"qr", "blocked.json", "qr_blocked", racing_control::poseFromXYYaw(1.2, 1.2, 0.0, "odom")}, + {"qr", "free.json", "qr_free", racing_control::poseFromXYYaw(3.2, 1.2, 0.0, "odom")}, + }}, + }); + selector.setCostmap(costmap); + + const auto selected = selector.select("qr", 50, false); + + ASSERT_TRUE(selected.has_value()); + EXPECT_EQ(selected->source_file, "free.json"); + EXPECT_EQ(selected->point_name, "qr_free"); +} + +TEST(CandidateWaypointSelector, FindsNearestFreeRecoveryCommand) +{ + nav_msgs::msg::OccupancyGrid costmap; + costmap.info.resolution = 0.25; + costmap.info.width = 9; + costmap.info.height = 9; + costmap.info.origin.position.x = -1.0; + costmap.info.origin.position.y = -1.0; + costmap.info.origin.orientation.w = 1.0; + costmap.data.assign(81, 100); + const auto set_free = [&](const int mx, const int my) { + costmap.data[my * static_cast(costmap.info.width) + mx] = 0; + }; + set_free(6, 4); + set_free(7, 4); + + const auto current = racing_control::poseFromXYYaw(0.0, 0.0, 0.0, "odom"); + const auto command = racing_control::nearestFreeRecoveryCommand( + costmap, current, 50, false, 1.0); + + ASSERT_TRUE(command.has_value()); + EXPECT_GT(command->linear_x, 0.0); + EXPECT_DOUBLE_EQ(command->angular_z, 0.0); +} diff --git a/src/vlm_detect/launch/vlm_detect.launch.py b/src/vlm_detect/launch/vlm_detect.launch.py index 34ae249..e743efb 100644 --- a/src/vlm_detect/launch/vlm_detect.launch.py +++ b/src/vlm_detect/launch/vlm_detect.launch.py @@ -24,6 +24,12 @@ def generate_launch_description(): prompt_text = LaunchConfiguration('prompt_text') max_tokens = LaunchConfiguration('max_tokens') image_max_dim = LaunchConfiguration('image_max_dim') + use_tcp = LaunchConfiguration('use_tcp') + use_serial = LaunchConfiguration('use_serial') + tcp_host = LaunchConfiguration('tcp_host') + tcp_port = LaunchConfiguration('tcp_port') + serial_port = LaunchConfiguration('serial_port') + serial_baud = LaunchConfiguration('serial_baud') audio_sink = LaunchConfiguration('audio_sink') tts_speed = LaunchConfiguration('tts_speed') @@ -45,6 +51,12 @@ def generate_launch_description(): declare_prompt_text = DeclareLaunchArgument('prompt_text', default_value='请描述这个病人的状态。不要描述边框、背景。15字以内。') declare_max_tokens = DeclareLaunchArgument('max_tokens', default_value='30') declare_image_max_dim = DeclareLaunchArgument('image_max_dim', default_value='448') + declare_use_tcp = DeclareLaunchArgument('use_tcp', default_value='true') + declare_use_serial = DeclareLaunchArgument('use_serial', default_value='false') + declare_tcp_host = DeclareLaunchArgument('tcp_host', default_value='192.168.127.1') + declare_tcp_port = DeclareLaunchArgument('tcp_port', default_value='9216') + declare_serial_port = DeclareLaunchArgument('serial_port', default_value='/dev/ttyUSB0') + declare_serial_baud = DeclareLaunchArgument('serial_baud', default_value='921600') declare_audio_sink = DeclareLaunchArgument('audio_sink', default_value='alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo') declare_tts_speed = DeclareLaunchArgument('tts_speed', default_value='1.5') @@ -66,6 +78,12 @@ def generate_launch_description(): 'prompt_text': prompt_text, 'max_tokens': max_tokens, 'image_max_dim': image_max_dim, + 'use_tcp': use_tcp, + 'use_serial': use_serial, + 'tcp_host': tcp_host, + 'tcp_port': tcp_port, + 'serial_port': serial_port, + 'serial_baud': serial_baud, }], ) @@ -104,6 +122,12 @@ def generate_launch_description(): declare_prompt_text, declare_max_tokens, declare_image_max_dim, + declare_use_tcp, + declare_use_serial, + declare_tcp_host, + declare_tcp_port, + declare_serial_port, + declare_serial_baud, declare_audio_sink, declare_tts_speed, LogInfo(msg=['Config: ', config_file]), diff --git a/src/vlm_detect/vlm_detect/__pycache__/tcp_vlm_client.cpython-310.pyc b/src/vlm_detect/vlm_detect/__pycache__/tcp_vlm_client.cpython-310.pyc new file mode 100644 index 0000000..b30e22c Binary files /dev/null and b/src/vlm_detect/vlm_detect/__pycache__/tcp_vlm_client.cpython-310.pyc differ diff --git a/src/vlm_detect/vlm_detect/serial_vlm_client.py b/src/vlm_detect/vlm_detect/serial_vlm_client.py new file mode 100644 index 0000000..3585175 --- /dev/null +++ b/src/vlm_detect/vlm_detect/serial_vlm_client.py @@ -0,0 +1,157 @@ +#!/usr/bin/env python3 +"""Serial VLM Client — 通过串口调用 VLM 推理。 +用在客户端 (192.168.175.65)，模拟 OpenAI client.chat.completions.create 接口。 +""" + +import json, struct, time, serial + +MAGIC = b'\xAB\xCD' +FLAG_CMD = ord('C') +FLAG_IMG = ord('I') +FLAG_RESP = ord('R') + + +class SerialVLMError(Exception): + pass + + +class SerialVLMClient: + """串口 VLM 客户端，兼容 OpenAI client.chat.completions.create 调用模式。""" + + def __init__(self, port='/dev/ttyUSB0', baud=921600, timeout=95): + self.port = port + self.baud = baud + self.timeout = timeout + self._ser = None + + def _connect(self): + if self._ser is None or not self._ser.is_open: + self._ser = serial.Serial(self.port, self.baud, timeout=0.1) + self._ser.reset_input_buffer() + self._ser.reset_output_buffer() + + def _send_packet(self, flag, data): + if isinstance(data, str): + data = data.encode('utf-8') + self._ser.write(MAGIC) + self._ser.write(bytes([flag])) + self._ser.write(struct.pack('>I', len(data))) + self._ser.write(data) + self._ser.flush() + + def _recv_exact(self, n, timeout=15): + deadline = time.time() + timeout + buf = b'' + while len(buf) < n: + remain = n - len(buf) + chunk = self._ser.read(remain) + if chunk: + buf += chunk + if time.time() > deadline: + raise SerialVLMError(f"recv timeout: got {len(buf)}/{n}") + if not chunk: + time.sleep(0.005) + return buf + + def _recv_packet(self): + while True: + b = self._ser.read(1) + if not b: + continue + if b == b'\xAB': + b2 = self._ser.read(1) + if b2 == b'\xCD': + break + flag = self._recv_exact(1)[0] + length = struct.unpack('>I', self._recv_exact(4))[0] + data = self._recv_exact(length) + return flag, data + + def infer(self, image_bytes: bytes, prompt: str) -> dict: + """发送图片+提示词，返回 {"success": bool, "answer": str, "elapsed_sec": float}""" + self._connect() + try: + # Reset buffers + self._ser.reset_input_buffer() + self._ser.reset_output_buffer() + + # Send command + cmd = json.dumps({"prompt": prompt, "image_len": len(image_bytes)}) + self._send_packet(FLAG_CMD, cmd) + + # Send image + self._send_packet(FLAG_IMG, image_bytes) + + # Receive response + t0 = time.time() + flag, data = self._recv_packet() + elapsed = time.time() - t0 + + if flag != FLAG_RESP: + return {"success": False, "error": f"unexpected flag: {flag}"} + + result = json.loads(data.decode('utf-8')) + return result + + except SerialVLMError: + return {"success": False, "error": "serial timeout"} + except Exception as e: + return {"success": False, "error": str(e)} + + def close(self): + if self._ser and self._ser.is_open: + self._ser.close() + self._ser = None + + # ---- OpenAI-compatible interface ---- + class _FakeChoices: + class _Msg: + def __init__(self, content): self.content = content + def __init__(self, content): + self.message = self._Msg(content) + + class _FakeResponse: + def __init__(self, content): + self.choices = [SerialVLMClient._FakeChoices(content)] + + class _FakeCompletions: + def __init__(self, client): self._client = client + def create(self, *, model=None, messages=None, max_tokens=None, temperature=None, timeout=None): + # Extract image and text from OpenAI-format messages + import base64 + image_bytes = None + prompt = "" + for msg in (messages or []): + content = msg.get("content", "") + if isinstance(content, list): + for item in content: + t = item.get("type", "") + if t == "text": + prompt = item.get("text", "") + elif t == "image_url": + url = item.get("image_url", {}).get("url", "") + if url.startswith("data:") and "," in url: + image_bytes = base64.b64decode(url.split(",", 1)[1]) + if image_bytes is None: + raise SerialVLMError("no image in messages") + + result = self._client.infer(image_bytes, prompt) + if not result.get("success"): + raise SerialVLMError(result.get("error", "unknown")) + return SerialVLMClient._FakeResponse(result["answer"]) + + class _FakeChat: + def __init__(self, client): self.completions = SerialVLMClient._FakeCompletions(client) + + @property + def chat(self): + return self._FakeChat(self) + + +# Convenience function +def serial_infer(image_bytes, prompt, port='/dev/ttyUSB0', baud=921600): + client = SerialVLMClient(port=port, baud=baud) + try: + return client.infer(image_bytes, prompt) + finally: + client.close() diff --git a/src/vlm_detect/vlm_detect/tcp_vlm_client.py b/src/vlm_detect/vlm_detect/tcp_vlm_client.py new file mode 100644 index 0000000..30ebd0a --- /dev/null +++ b/src/vlm_detect/vlm_detect/tcp_vlm_client.py @@ -0,0 +1,146 @@ +#!/usr/bin/env python3 +"""TCP VLM Client — 通过 TCP 套接字调用 VLM 推理。 +用在客户端 (192.168.175.65)，模拟 OpenAI client.chat.completions.create 接口。 +""" + +import json, struct, time, socket + + +MAGIC = b'\xAB\xCD' +FLAG_CMD = ord('C') +FLAG_IMG = ord('I') +FLAG_RESP = ord('R') + + +class TCPVLMError(Exception): + pass + + +class TCPVLMClient: + """TCP VLM 客户端，兼容 OpenAI client.chat.completions.create 调用模式。""" + + def __init__(self, host='192.168.127.1', port=9216, timeout=95): + self.host = host + self.port = port + self.timeout = timeout + + def _recv_exact(self, sock, n, timeout=15): + deadline = time.time() + timeout + buf = b'' + while len(buf) < n: + remain = n - len(buf) + sock.settimeout(max(0.1, deadline - time.time())) + try: + chunk = sock.recv(remain) + except socket.timeout: + if time.time() > deadline: + raise TCPVLMError(f"recv timeout: got {len(buf)}/{n}") + continue + if not chunk: + raise TCPVLMError("connection closed by server") + buf += chunk + return buf + + def _send_packet(self, sock, flag, data): + if isinstance(data, str): + data = data.encode('utf-8') + sock.sendall(MAGIC) + sock.sendall(bytes([flag])) + sock.sendall(struct.pack('>I', len(data))) + sock.sendall(data) + + def _recv_packet(self, sock): + while True: + b = self._recv_exact(sock, 1, timeout=60) + if b == b'\xAB': + b2 = self._recv_exact(sock, 1, timeout=5) + if b2 == b'\xCD': + break + flag = self._recv_exact(sock, 1)[0] + length = struct.unpack('>I', self._recv_exact(sock, 4))[0] + if length > 1024 * 1024: + raise TCPVLMError(f"packet too large: {length}") + data = self._recv_exact(sock, length, timeout=30) + return flag, data + + def infer(self, image_bytes: bytes, prompt: str) -> dict: + """发送图片+提示词，返回 {"success": bool, "answer": str, "elapsed_sec": float}""" + sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + sock.settimeout(self.timeout) + try: + sock.connect((self.host, self.port)) + + # Send command + cmd = json.dumps({"prompt": prompt, "image_len": len(image_bytes)}) + self._send_packet(sock, FLAG_CMD, cmd) + + # Send image + self._send_packet(sock, FLAG_IMG, image_bytes) + + # Receive response + t0 = time.time() + flag, data = self._recv_packet(sock) + elapsed = time.time() - t0 + + if flag != FLAG_RESP: + return {"success": False, "error": f"unexpected flag: {flag}"} + + result = json.loads(data.decode('utf-8')) + return result + + except TCPVLMError: + return {"success": False, "error": "TCP timeout"} + except Exception as e: + return {"success": False, "error": str(e)} + finally: + try: + sock.close() + except Exception: + pass + + # ---- OpenAI-compatible interface ---- + class _FakeChoices: + class _Msg: + def __init__(self, content): self.content = content + def __init__(self, content): + self.message = self._Msg(content) + + class _FakeResponse: + def __init__(self, content): + self.choices = [TCPVLMClient._FakeChoices(content)] + + class _FakeCompletions: + def __init__(self, client): self._client = client + def create(self, *, model=None, messages=None, max_tokens=None, temperature=None, timeout=None): + import base64 + image_bytes = None + prompt = "" + for msg in (messages or []): + content = msg.get("content", "") + if isinstance(content, list): + for item in content: + t = item.get("type", "") + if t == "text": + prompt = item.get("text", "") + elif t == "image_url": + url = item.get("image_url", {}).get("url", "") + if url.startswith("data:") and "," in url: + image_bytes = base64.b64decode(url.split(",", 1)[1]) + if image_bytes is None: + raise TCPVLMError("no image in messages") + result = self._client.infer(image_bytes, prompt) + if not result.get("success"): + raise TCPVLMError(result.get("error", "unknown")) + return TCPVLMClient._FakeResponse(result["answer"]) + + class _FakeChat: + def __init__(self, client): self.completions = TCPVLMClient._FakeCompletions(client) + + @property + def chat(self): + return self._FakeChat(self) + + +def tcp_infer(image_bytes, prompt, host='192.168.127.1', port=9216): + client = TCPVLMClient(host=host, port=port) + return client.infer(image_bytes, prompt) diff --git a/src/vlm_detect/vlm_detect/vlm_node.py b/src/vlm_detect/vlm_detect/vlm_node.py index da61829..f1b8571 100644 --- a/src/vlm_detect/vlm_detect/vlm_node.py +++ b/src/vlm_detect/vlm_detect/vlm_node.py @@ -12,11 +12,25 @@ import time import numpy as np from origincar_msg.srv import Speak +# TCP VLM client (optional) +try: + from .tcp_vlm_client import TCPVLMClient, TCPVLMError + HAS_TCP = True +except ImportError: + HAS_TCP = False + +# Serial VLM client (optional) +try: + from .serial_vlm_client import SerialVLMClient, SerialVLMError + HAS_SERIAL = True +except ImportError: + HAS_SERIAL = False + class VLMProcessor(Node): def __init__(self): super().__init__('vlm_detect') - self.declare_parameter('vlm_host', 'http://192.168.10.189:8000') + self.declare_parameter('vlm_host', 'http://192.168.175.64:8000') self.declare_parameter('vlm_model', '/home/wisdom/models/gguf/Qwen2-VL-2B-Instruct-Q4_K_M.gguf') self.declare_parameter('image_topic', '/image_mjpeg') self.declare_parameter('trigger_topic', '/sign4return') @@ -28,6 +42,12 @@ class VLMProcessor(Node): self.declare_parameter('temperature', 0.1) self.declare_parameter('crop_ratio', 0.45) self.declare_parameter('auto_crop', False) + self.declare_parameter('use_tcp', False) + self.declare_parameter('use_serial', False) + self.declare_parameter('tcp_host', '192.168.127.1') + self.declare_parameter('tcp_port', 9216) + self.declare_parameter('serial_port', '/dev/ttyUSB0') + self.declare_parameter('serial_baud', 921600) vlm_host = self.get_parameter('vlm_host').value vlm_model = self.get_parameter('vlm_model').value @@ -41,12 +61,39 @@ class VLMProcessor(Node): self.temperature = self.get_parameter('temperature').value self.crop_ratio = self.get_parameter('crop_ratio').value self.auto_crop = self.get_parameter('auto_crop').value + self.use_tcp = self.get_parameter('use_tcp').value + self.use_serial = self.get_parameter('use_serial').value + self.tcp_host = self.get_parameter('tcp_host').value + self.tcp_port = self.get_parameter('tcp_port').value + self.serial_port = self.get_parameter('serial_port').value + self.serial_baud = self.get_parameter('serial_baud').value + + # Init client: TCP > Serial > HTTP + self.tcp_client = None + self.serial_client = None + self.client = None + + if self.use_tcp: + if not HAS_TCP: + self.get_logger().fatal("tcp_vlm_client not found") + raise RuntimeError("tcp_vlm_client module required for use_tcp=True") + self.tcp_client = TCPVLMClient(host=self.tcp_host, port=self.tcp_port) + self.get_logger().info(f"VLM using TCP | {self.tcp_host}:{self.tcp_port}") + elif self.use_serial: + if not HAS_SERIAL: + self.get_logger().fatal("serial_vlm_client not found") + raise RuntimeError("serial_vlm_client module required for use_serial=True") + self.serial_client = SerialVLMClient(port=self.serial_port, baud=self.serial_baud) + self.get_logger().info(f"VLM using SERIAL | port={self.serial_port} baud={self.serial_baud}") + else: + self.client = OpenAI(base_url=f"{vlm_host}/v1", api_key="EMPTY") + self.get_logger().info(f"VLM using HTTP | host={vlm_host}") - self.client = OpenAI(base_url=f"{vlm_host}/v1", api_key="EMPTY") self.vlm_model = vlm_model self.latest_image = None self.image_lock = threading.Lock() self._busy = False + self._vlm_done = False # one-shot: ignore triggers after first self.image_sub = self.create_subscription(CompressedImage, image_topic, self.image_callback, 10) self.sign_sub = self.create_subscription(Int32, trigger_topic, self.sign_callback, 10) @@ -56,7 +103,8 @@ class VLMProcessor(Node): while not self.tts_client.wait_for_service(timeout_sec=5.0): self.get_logger().info('Waiting for TTS service...') - self.get_logger().info(f"VLM ready | host={vlm_host} | dim={self.image_max_dim} | crop={self.auto_crop}") + mode = "TCP" if self.use_tcp else ("SERIAL" if self.use_serial else "HTTP") + self.get_logger().info(f"VLM ready | mode={mode} | dim={self.image_max_dim} | crop={self.auto_crop}") def image_callback(self, msg): with self.image_lock: @@ -69,10 +117,14 @@ class VLMProcessor(Node): def sign_callback(self, msg): if msg.data != self.trigger_sign: return + if self._vlm_done: + self.get_logger().warn("VLM already triggered, skip") + return if self._busy: self.get_logger().warn("Busy, skip") return - self.get_logger().info(f"Trigger {msg.data}") + self._vlm_done = True + self.get_logger().info(f"Trigger {msg.data} (first and only)") with self.image_lock: if self.latest_image is None: self.get_logger().warning("No image, using fallback") @@ -115,17 +167,36 @@ class VLMProcessor(Node): else: new_w, new_h = w, h _, jpeg = cv2.imencode('.jpg', img, [cv2.IMWRITE_JPEG_QUALITY, 60]) - b64 = base64.b64encode(jpeg.tobytes()).decode('utf-8') + jpeg_bytes = jpeg.tobytes() t0 = time.time() - resp = self.client.chat.completions.create( - model=self.vlm_model, - messages=[{"role": "user", "content": [ - {"type": "text", "text": self.prompt_text}, - {"type": "image_url", "image_url": {"url": f"data:image/jpeg;base64,{b64}"}} - ]}], - max_tokens=self.max_tokens, temperature=self.temperature, timeout=60) - self.get_logger().info(f"API {time.time()-t0:.1f}s {new_w}x{new_h} {len(jpeg)//1024}KB") - return resp.choices[0].message.content + + if self.use_tcp: + result = self.tcp_client.infer(jpeg_bytes, self.prompt_text) + if not result.get("success"): + raise RuntimeError(f"TCP inference failed: {result.get('error', 'unknown')}") + elapsed = result.get("elapsed_sec", time.time() - t0) + self.get_logger().info(f"TCP {elapsed:.1f}s {new_w}x{new_h} {len(jpeg_bytes)//1024}KB") + return result["answer"] + + elif self.use_serial: + result = self.serial_client.infer(jpeg_bytes, self.prompt_text) + if not result.get("success"): + raise RuntimeError(f"Serial inference failed: {result.get('error', 'unknown')}") + elapsed = result.get("elapsed_sec", time.time() - t0) + self.get_logger().info(f"Serial {elapsed:.1f}s {new_w}x{new_h} {len(jpeg_bytes)//1024}KB") + return result["answer"] + + else: + b64 = base64.b64encode(jpeg_bytes).decode('utf-8') + resp = self.client.chat.completions.create( + model=self.vlm_model, + messages=[{"role": "user", "content": [ + {"type": "text", "text": self.prompt_text}, + {"type": "image_url", "image_url": {"url": f"data:image/jpeg;base64,{b64}"}} + ]}], + max_tokens=self.max_tokens, temperature=self.temperature, timeout=60) + self.get_logger().info(f"HTTP {time.time()-t0:.1f}s {new_w}x{new_h} {len(jpeg_bytes)//1024}KB") + return resp.choices[0].message.content def _detect_and_crop(self, img): h, w = img.shape[:2] diff --git a/src/vlm_detect/vlm_detect/vlm_node_http.py.bak b/src/vlm_detect/vlm_detect/vlm_node_http.py.bak new file mode 100644 index 0000000..da61829 --- /dev/null +++ b/src/vlm_detect/vlm_detect/vlm_node_http.py.bak @@ -0,0 +1,165 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +import rclpy +from rclpy.node import Node +from std_msgs.msg import Int32, String +from sensor_msgs.msg import CompressedImage +import cv2 +import base64 +import threading +from openai import OpenAI +import time +import numpy as np +from origincar_msg.srv import Speak + + +class VLMProcessor(Node): + def __init__(self): + super().__init__('vlm_detect') + self.declare_parameter('vlm_host', 'http://192.168.10.189:8000') + self.declare_parameter('vlm_model', '/home/wisdom/models/gguf/Qwen2-VL-2B-Instruct-Q4_K_M.gguf') + self.declare_parameter('image_topic', '/image_mjpeg') + self.declare_parameter('trigger_topic', '/sign4return') + self.declare_parameter('trigger_sign', 9) + self.declare_parameter('result_topic', '/vlm_result') + self.declare_parameter('prompt_text', '') + self.declare_parameter('max_tokens', 30) + self.declare_parameter('image_max_dim', 96) + self.declare_parameter('temperature', 0.1) + self.declare_parameter('crop_ratio', 0.45) + self.declare_parameter('auto_crop', False) + + vlm_host = self.get_parameter('vlm_host').value + vlm_model = self.get_parameter('vlm_model').value + image_topic = self.get_parameter('image_topic').value + trigger_topic = self.get_parameter('trigger_topic').value + self.trigger_sign = self.get_parameter('trigger_sign').value + result_topic = self.get_parameter('result_topic').value + self.prompt_text = self.get_parameter('prompt_text').value + self.max_tokens = self.get_parameter('max_tokens').value + self.image_max_dim = self.get_parameter('image_max_dim').value + self.temperature = self.get_parameter('temperature').value + self.crop_ratio = self.get_parameter('crop_ratio').value + self.auto_crop = self.get_parameter('auto_crop').value + + self.client = OpenAI(base_url=f"{vlm_host}/v1", api_key="EMPTY") + self.vlm_model = vlm_model + self.latest_image = None + self.image_lock = threading.Lock() + self._busy = False + + self.image_sub = self.create_subscription(CompressedImage, image_topic, self.image_callback, 10) + self.sign_sub = self.create_subscription(Int32, trigger_topic, self.sign_callback, 10) + self.result_pub = self.create_publisher(String, result_topic, 10) + + self.tts_client = self.create_client(Speak, '/tts/speak') + while not self.tts_client.wait_for_service(timeout_sec=5.0): + self.get_logger().info('Waiting for TTS service...') + + self.get_logger().info(f"VLM ready | host={vlm_host} | dim={self.image_max_dim} | crop={self.auto_crop}") + + def image_callback(self, msg): + with self.image_lock: + try: + np_arr = np.frombuffer(msg.data, np.uint8) + self.latest_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) + except Exception as e: + self.get_logger().error(f"Decode error: {e}") + + def sign_callback(self, msg): + if msg.data != self.trigger_sign: + return + if self._busy: + self.get_logger().warn("Busy, skip") + return + self.get_logger().info(f"Trigger {msg.data}") + with self.image_lock: + if self.latest_image is None: + self.get_logger().warning("No image, using fallback") + desc = '患者位于病床上，姿态放松，未观察到明显异常行为。' + result_msg = String() + result_msg.data = desc + self.result_pub.publish(result_msg) + if self.tts_client.service_is_ready(): + req = Speak.Request() + req.text = desc + self.tts_client.call_async(req) + return + img = self.latest_image.copy() + self._busy = True + try: + t0 = time.time() + desc = self.process_image(img) + self.get_logger().info(f"Result({time.time()-t0:.1f}s): {desc}") + msg = String() + msg.data = desc + self.result_pub.publish(msg) + if self.tts_client.service_is_ready(): + req = Speak.Request() + req.text = desc + self.tts_client.call_async(req) + except Exception as e: + self.get_logger().error(f"Inference: {e}") + finally: + self._busy = False + + def process_image(self, img): + if self.auto_crop: + img = self._detect_and_crop(img) + h, w = img.shape[:2] + max_dim = self.image_max_dim + if max(h, w) > max_dim: + scale = max_dim / max(h, w) + new_w, new_h = int(w * scale), int(h * scale) + img = cv2.resize(img, (new_w, new_h), interpolation=cv2.INTER_AREA) + else: + new_w, new_h = w, h + _, jpeg = cv2.imencode('.jpg', img, [cv2.IMWRITE_JPEG_QUALITY, 60]) + b64 = base64.b64encode(jpeg.tobytes()).decode('utf-8') + t0 = time.time() + resp = self.client.chat.completions.create( + model=self.vlm_model, + messages=[{"role": "user", "content": [ + {"type": "text", "text": self.prompt_text}, + {"type": "image_url", "image_url": {"url": f"data:image/jpeg;base64,{b64}"}} + ]}], + max_tokens=self.max_tokens, temperature=self.temperature, timeout=60) + self.get_logger().info(f"API {time.time()-t0:.1f}s {new_w}x{new_h} {len(jpeg)//1024}KB") + return resp.choices[0].message.content + + def _detect_and_crop(self, img): + h, w = img.shape[:2] + gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + blurred = cv2.GaussianBlur(gray, (5, 5), 0) + edges = cv2.Canny(blurred, 50, 150) + kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3)) + edges = cv2.dilate(edges, kernel, iterations=1) + contours, _ = cv2.findContours(edges, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + best_rect, best_area = None, 0 + for cnt in contours: + peri = cv2.arcLength(cnt, True) + approx = cv2.approxPolyDP(cnt, 0.02 * peri, True) + area = cv2.contourArea(approx) + if len(approx) == 4 and area > w * h * 0.02 and area > best_area: + best_rect, best_area = approx, area + if best_rect is not None: + rx, ry, rw, rh = cv2.boundingRect(best_rect) + self.get_logger().info(f'Crop: {rw}x{rh}') + return img[ry:ry + rh, rx:rx + rw] + return img + + +def main(args=None): + rclpy.init(args=args) + node = VLMProcessor() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/vlm_detect/vlm_detect/vlm_node_serial.py b/src/vlm_detect/vlm_detect/vlm_node_serial.py new file mode 100644 index 0000000..fbc86a3 --- /dev/null +++ b/src/vlm_detect/vlm_detect/vlm_node_serial.py @@ -0,0 +1,202 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +import rclpy +from rclpy.node import Node +from std_msgs.msg import Int32, String +from sensor_msgs.msg import CompressedImage +import cv2 +import base64 +import threading +from openai import OpenAI +import time +import numpy as np +from origincar_msg.srv import Speak + +# Serial VLM client (optional) +try: + from serial_vlm_client import SerialVLMClient, SerialVLMError + HAS_SERIAL = True +except ImportError: + HAS_SERIAL = False + + +class VLMProcessor(Node): + def __init__(self): + super().__init__('vlm_detect') + self.declare_parameter('vlm_host', 'http://192.168.10.189:8000') + self.declare_parameter('vlm_model', '/home/wisdom/models/gguf/Qwen2-VL-2B-Instruct-Q4_K_M.gguf') + self.declare_parameter('image_topic', '/image_mjpeg') + self.declare_parameter('trigger_topic', '/sign4return') + self.declare_parameter('trigger_sign', 9) + self.declare_parameter('result_topic', '/vlm_result') + self.declare_parameter('prompt_text', '') + self.declare_parameter('max_tokens', 30) + self.declare_parameter('image_max_dim', 96) + self.declare_parameter('temperature', 0.1) + self.declare_parameter('crop_ratio', 0.45) + self.declare_parameter('auto_crop', False) + self.declare_parameter('use_serial', False) + self.declare_parameter('serial_port', '/dev/ttyUSB0') + self.declare_parameter('serial_baud', 921600) + + vlm_host = self.get_parameter('vlm_host').value + vlm_model = self.get_parameter('vlm_model').value + image_topic = self.get_parameter('image_topic').value + trigger_topic = self.get_parameter('trigger_topic').value + self.trigger_sign = self.get_parameter('trigger_sign').value + result_topic = self.get_parameter('result_topic').value + self.prompt_text = self.get_parameter('prompt_text').value + self.max_tokens = self.get_parameter('max_tokens').value + self.image_max_dim = self.get_parameter('image_max_dim').value + self.temperature = self.get_parameter('temperature').value + self.crop_ratio = self.get_parameter('crop_ratio').value + self.auto_crop = self.get_parameter('auto_crop').value + self.use_serial = self.get_parameter('use_serial').value + self.serial_port = self.get_parameter('serial_port').value + self.serial_baud = self.get_parameter('serial_baud').value + + # Init client: serial or HTTP + if self.use_serial: + if not HAS_SERIAL: + self.get_logger().fatal("serial_vlm_client not found, falling back to serial") + raise RuntimeError("serial_vlm_client module required for use_serial=True") + self.serial_client = SerialVLMClient(port=self.serial_port, baud=self.serial_baud) + self.client = None # not needed for serial mode + self.get_logger().info(f"VLM using SERIAL | port={self.serial_port} baud={self.serial_baud}") + else: + self.client = OpenAI(base_url=f"{vlm_host}/v1", api_key="EMPTY") + self.serial_client = None + self.get_logger().info(f"VLM using HTTP | host={vlm_host}") + + self.vlm_model = vlm_model + self.latest_image = None + self.image_lock = threading.Lock() + self._busy = False + + self.image_sub = self.create_subscription(CompressedImage, image_topic, self.image_callback, 10) + self.sign_sub = self.create_subscription(Int32, trigger_topic, self.sign_callback, 10) + self.result_pub = self.create_publisher(String, result_topic, 10) + + self.tts_client = self.create_client(Speak, '/tts/speak') + while not self.tts_client.wait_for_service(timeout_sec=5.0): + self.get_logger().info('Waiting for TTS service...') + + self.get_logger().info(f"VLM ready | dim={self.image_max_dim} | crop={self.auto_crop}") + + def image_callback(self, msg): + with self.image_lock: + try: + np_arr = np.frombuffer(msg.data, np.uint8) + self.latest_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) + except Exception as e: + self.get_logger().error(f"Decode error: {e}") + + def sign_callback(self, msg): + if msg.data != self.trigger_sign: + return + if self._busy: + self.get_logger().warn("Busy, skip") + return + self.get_logger().info(f"Trigger {msg.data}") + with self.image_lock: + if self.latest_image is None: + self.get_logger().warning("No image, using fallback") + desc = '患者位于病床上，姿态放松，未观察到明显异常行为。' + result_msg = String() + result_msg.data = desc + self.result_pub.publish(result_msg) + if self.tts_client.service_is_ready(): + req = Speak.Request() + req.text = desc + self.tts_client.call_async(req) + return + img = self.latest_image.copy() + self._busy = True + try: + t0 = time.time() + desc = self.process_image(img) + self.get_logger().info(f"Result({time.time()-t0:.1f}s): {desc}") + msg = String() + msg.data = desc + self.result_pub.publish(msg) + if self.tts_client.service_is_ready(): + req = Speak.Request() + req.text = desc + self.tts_client.call_async(req) + except Exception as e: + self.get_logger().error(f"Inference: {e}") + finally: + self._busy = False + + def process_image(self, img): + if self.auto_crop: + img = self._detect_and_crop(img) + h, w = img.shape[:2] + max_dim = self.image_max_dim + if max(h, w) > max_dim: + scale = max_dim / max(h, w) + new_w, new_h = int(w * scale), int(h * scale) + img = cv2.resize(img, (new_w, new_h), interpolation=cv2.INTER_AREA) + else: + new_w, new_h = w, h + _, jpeg = cv2.imencode('.jpg', img, [cv2.IMWRITE_JPEG_QUALITY, 60]) + jpeg_bytes = jpeg.tobytes() + t0 = time.time() + + if self.use_serial: + # Serial mode + result = self.serial_client.infer(jpeg_bytes, self.prompt_text) + if not result.get("success"): + raise RuntimeError(f"Serial inference failed: {result.get('error', 'unknown')}") + elapsed = result.get("elapsed_sec", time.time() - t0) + self.get_logger().info(f"Serial {elapsed:.1f}s {new_w}x{new_h} {len(jpeg_bytes)//1024}KB") + return result["answer"] + else: + # HTTP mode (unchanged) + b64 = base64.b64encode(jpeg_bytes).decode('utf-8') + resp = self.client.chat.completions.create( + model=self.vlm_model, + messages=[{"role": "user", "content": [ + {"type": "text", "text": self.prompt_text}, + {"type": "image_url", "image_url": {"url": f"data:image/jpeg;base64,{b64}"}} + ]}], + max_tokens=self.max_tokens, temperature=self.temperature, timeout=60) + self.get_logger().info(f"API {time.time()-t0:.1f}s {new_w}x{new_h} {len(jpeg_bytes)//1024}KB") + return resp.choices[0].message.content + + def _detect_and_crop(self, img): + h, w = img.shape[:2] + gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + blurred = cv2.GaussianBlur(gray, (5, 5), 0) + edges = cv2.Canny(blurred, 50, 150) + kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3)) + edges = cv2.dilate(edges, kernel, iterations=1) + contours, _ = cv2.findContours(edges, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + best_rect, best_area = None, 0 + for cnt in contours: + peri = cv2.arcLength(cnt, True) + approx = cv2.approxPolyDP(cnt, 0.02 * peri, True) + area = cv2.contourArea(approx) + if len(approx) == 4 and area > w * h * 0.02 and area > best_area: + best_rect, best_area = approx, area + if best_rect is not None: + rx, ry, rw, rh = cv2.boundingRect(best_rect) + self.get_logger().info(f'Crop: {rw}x{rh}') + return img[ry:ry + rh, rx:rx + rw] + return img + + +def main(args=None): + rclpy.init(args=args) + node = VLMProcessor() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/vlm_detect/vlm_detect/vlm_node_serial.py.bak b/src/vlm_detect/vlm_detect/vlm_node_serial.py.bak new file mode 100644 index 0000000..fbc86a3 --- /dev/null +++ b/src/vlm_detect/vlm_detect/vlm_node_serial.py.bak @@ -0,0 +1,202 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +import rclpy +from rclpy.node import Node +from std_msgs.msg import Int32, String +from sensor_msgs.msg import CompressedImage +import cv2 +import base64 +import threading +from openai import OpenAI +import time +import numpy as np +from origincar_msg.srv import Speak + +# Serial VLM client (optional) +try: + from serial_vlm_client import SerialVLMClient, SerialVLMError + HAS_SERIAL = True +except ImportError: + HAS_SERIAL = False + + +class VLMProcessor(Node): + def __init__(self): + super().__init__('vlm_detect') + self.declare_parameter('vlm_host', 'http://192.168.10.189:8000') + self.declare_parameter('vlm_model', '/home/wisdom/models/gguf/Qwen2-VL-2B-Instruct-Q4_K_M.gguf') + self.declare_parameter('image_topic', '/image_mjpeg') + self.declare_parameter('trigger_topic', '/sign4return') + self.declare_parameter('trigger_sign', 9) + self.declare_parameter('result_topic', '/vlm_result') + self.declare_parameter('prompt_text', '') + self.declare_parameter('max_tokens', 30) + self.declare_parameter('image_max_dim', 96) + self.declare_parameter('temperature', 0.1) + self.declare_parameter('crop_ratio', 0.45) + self.declare_parameter('auto_crop', False) + self.declare_parameter('use_serial', False) + self.declare_parameter('serial_port', '/dev/ttyUSB0') + self.declare_parameter('serial_baud', 921600) + + vlm_host = self.get_parameter('vlm_host').value + vlm_model = self.get_parameter('vlm_model').value + image_topic = self.get_parameter('image_topic').value + trigger_topic = self.get_parameter('trigger_topic').value + self.trigger_sign = self.get_parameter('trigger_sign').value + result_topic = self.get_parameter('result_topic').value + self.prompt_text = self.get_parameter('prompt_text').value + self.max_tokens = self.get_parameter('max_tokens').value + self.image_max_dim = self.get_parameter('image_max_dim').value + self.temperature = self.get_parameter('temperature').value + self.crop_ratio = self.get_parameter('crop_ratio').value + self.auto_crop = self.get_parameter('auto_crop').value + self.use_serial = self.get_parameter('use_serial').value + self.serial_port = self.get_parameter('serial_port').value + self.serial_baud = self.get_parameter('serial_baud').value + + # Init client: serial or HTTP + if self.use_serial: + if not HAS_SERIAL: + self.get_logger().fatal("serial_vlm_client not found, falling back to serial") + raise RuntimeError("serial_vlm_client module required for use_serial=True") + self.serial_client = SerialVLMClient(port=self.serial_port, baud=self.serial_baud) + self.client = None # not needed for serial mode + self.get_logger().info(f"VLM using SERIAL | port={self.serial_port} baud={self.serial_baud}") + else: + self.client = OpenAI(base_url=f"{vlm_host}/v1", api_key="EMPTY") + self.serial_client = None + self.get_logger().info(f"VLM using HTTP | host={vlm_host}") + + self.vlm_model = vlm_model + self.latest_image = None + self.image_lock = threading.Lock() + self._busy = False + + self.image_sub = self.create_subscription(CompressedImage, image_topic, self.image_callback, 10) + self.sign_sub = self.create_subscription(Int32, trigger_topic, self.sign_callback, 10) + self.result_pub = self.create_publisher(String, result_topic, 10) + + self.tts_client = self.create_client(Speak, '/tts/speak') + while not self.tts_client.wait_for_service(timeout_sec=5.0): + self.get_logger().info('Waiting for TTS service...') + + self.get_logger().info(f"VLM ready | dim={self.image_max_dim} | crop={self.auto_crop}") + + def image_callback(self, msg): + with self.image_lock: + try: + np_arr = np.frombuffer(msg.data, np.uint8) + self.latest_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) + except Exception as e: + self.get_logger().error(f"Decode error: {e}") + + def sign_callback(self, msg): + if msg.data != self.trigger_sign: + return + if self._busy: + self.get_logger().warn("Busy, skip") + return + self.get_logger().info(f"Trigger {msg.data}") + with self.image_lock: + if self.latest_image is None: + self.get_logger().warning("No image, using fallback") + desc = '患者位于病床上，姿态放松，未观察到明显异常行为。' + result_msg = String() + result_msg.data = desc + self.result_pub.publish(result_msg) + if self.tts_client.service_is_ready(): + req = Speak.Request() + req.text = desc + self.tts_client.call_async(req) + return + img = self.latest_image.copy() + self._busy = True + try: + t0 = time.time() + desc = self.process_image(img) + self.get_logger().info(f"Result({time.time()-t0:.1f}s): {desc}") + msg = String() + msg.data = desc + self.result_pub.publish(msg) + if self.tts_client.service_is_ready(): + req = Speak.Request() + req.text = desc + self.tts_client.call_async(req) + except Exception as e: + self.get_logger().error(f"Inference: {e}") + finally: + self._busy = False + + def process_image(self, img): + if self.auto_crop: + img = self._detect_and_crop(img) + h, w = img.shape[:2] + max_dim = self.image_max_dim + if max(h, w) > max_dim: + scale = max_dim / max(h, w) + new_w, new_h = int(w * scale), int(h * scale) + img = cv2.resize(img, (new_w, new_h), interpolation=cv2.INTER_AREA) + else: + new_w, new_h = w, h + _, jpeg = cv2.imencode('.jpg', img, [cv2.IMWRITE_JPEG_QUALITY, 60]) + jpeg_bytes = jpeg.tobytes() + t0 = time.time() + + if self.use_serial: + # Serial mode + result = self.serial_client.infer(jpeg_bytes, self.prompt_text) + if not result.get("success"): + raise RuntimeError(f"Serial inference failed: {result.get('error', 'unknown')}") + elapsed = result.get("elapsed_sec", time.time() - t0) + self.get_logger().info(f"Serial {elapsed:.1f}s {new_w}x{new_h} {len(jpeg_bytes)//1024}KB") + return result["answer"] + else: + # HTTP mode (unchanged) + b64 = base64.b64encode(jpeg_bytes).decode('utf-8') + resp = self.client.chat.completions.create( + model=self.vlm_model, + messages=[{"role": "user", "content": [ + {"type": "text", "text": self.prompt_text}, + {"type": "image_url", "image_url": {"url": f"data:image/jpeg;base64,{b64}"}} + ]}], + max_tokens=self.max_tokens, temperature=self.temperature, timeout=60) + self.get_logger().info(f"API {time.time()-t0:.1f}s {new_w}x{new_h} {len(jpeg_bytes)//1024}KB") + return resp.choices[0].message.content + + def _detect_and_crop(self, img): + h, w = img.shape[:2] + gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + blurred = cv2.GaussianBlur(gray, (5, 5), 0) + edges = cv2.Canny(blurred, 50, 150) + kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3)) + edges = cv2.dilate(edges, kernel, iterations=1) + contours, _ = cv2.findContours(edges, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + best_rect, best_area = None, 0 + for cnt in contours: + peri = cv2.arcLength(cnt, True) + approx = cv2.approxPolyDP(cnt, 0.02 * peri, True) + area = cv2.contourArea(approx) + if len(approx) == 4 and area > w * h * 0.02 and area > best_area: + best_rect, best_area = approx, area + if best_rect is not None: + rx, ry, rw, rh = cv2.boundingRect(best_rect) + self.get_logger().info(f'Crop: {rw}x{rh}') + return img[ry:ry + rh, rx:rx + rw] + return img + + +def main(args=None): + rclpy.init(args=args) + node = VLMProcessor() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/vlm_detect/vlm_detect/vlm_node_tcp.py b/src/vlm_detect/vlm_detect/vlm_node_tcp.py new file mode 100644 index 0000000..10e76de --- /dev/null +++ b/src/vlm_detect/vlm_detect/vlm_node_tcp.py @@ -0,0 +1,231 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +import rclpy +from rclpy.node import Node +from std_msgs.msg import Int32, String +from sensor_msgs.msg import CompressedImage +import cv2 +import base64 +import threading +from openai import OpenAI +import time +import numpy as np +from origincar_msg.srv import Speak + +# TCP VLM client (optional) +try: + from tcp_vlm_client import TCPVLMClient, TCPVLMError + HAS_TCP = True +except ImportError: + HAS_TCP = False + +# Serial VLM client (optional) +try: + from serial_vlm_client import SerialVLMClient, SerialVLMError + HAS_SERIAL = True +except ImportError: + HAS_SERIAL = False + + +class VLMProcessor(Node): + def __init__(self): + super().__init__('vlm_detect') + self.declare_parameter('vlm_host', 'http://192.168.175.64:8000') + self.declare_parameter('vlm_model', '/home/wisdom/models/gguf/Qwen2-VL-2B-Instruct-Q4_K_M.gguf') + self.declare_parameter('image_topic', '/image_mjpeg') + self.declare_parameter('trigger_topic', '/sign4return') + self.declare_parameter('trigger_sign', 9) + self.declare_parameter('result_topic', '/vlm_result') + self.declare_parameter('prompt_text', '') + self.declare_parameter('max_tokens', 30) + self.declare_parameter('image_max_dim', 96) + self.declare_parameter('temperature', 0.1) + self.declare_parameter('crop_ratio', 0.45) + self.declare_parameter('auto_crop', False) + self.declare_parameter('use_tcp', False) + self.declare_parameter('use_serial', False) + self.declare_parameter('tcp_host', '192.168.127.1') + self.declare_parameter('tcp_port', 9216) + self.declare_parameter('serial_port', '/dev/ttyUSB0') + self.declare_parameter('serial_baud', 921600) + + vlm_host = self.get_parameter('vlm_host').value + vlm_model = self.get_parameter('vlm_model').value + image_topic = self.get_parameter('image_topic').value + trigger_topic = self.get_parameter('trigger_topic').value + self.trigger_sign = self.get_parameter('trigger_sign').value + result_topic = self.get_parameter('result_topic').value + self.prompt_text = self.get_parameter('prompt_text').value + self.max_tokens = self.get_parameter('max_tokens').value + self.image_max_dim = self.get_parameter('image_max_dim').value + self.temperature = self.get_parameter('temperature').value + self.crop_ratio = self.get_parameter('crop_ratio').value + self.auto_crop = self.get_parameter('auto_crop').value + self.use_tcp = self.get_parameter('use_tcp').value + self.use_serial = self.get_parameter('use_serial').value + self.tcp_host = self.get_parameter('tcp_host').value + self.tcp_port = self.get_parameter('tcp_port').value + self.serial_port = self.get_parameter('serial_port').value + self.serial_baud = self.get_parameter('serial_baud').value + + # Init client: TCP > Serial > HTTP + self.tcp_client = None + self.serial_client = None + self.client = None + + if self.use_tcp: + if not HAS_TCP: + self.get_logger().fatal("tcp_vlm_client not found") + raise RuntimeError("tcp_vlm_client module required for use_tcp=True") + self.tcp_client = TCPVLMClient(host=self.tcp_host, port=self.tcp_port) + self.get_logger().info(f"VLM using TCP | {self.tcp_host}:{self.tcp_port}") + elif self.use_serial: + if not HAS_SERIAL: + self.get_logger().fatal("serial_vlm_client not found") + raise RuntimeError("serial_vlm_client module required for use_serial=True") + self.serial_client = SerialVLMClient(port=self.serial_port, baud=self.serial_baud) + self.get_logger().info(f"VLM using SERIAL | port={self.serial_port} baud={self.serial_baud}") + else: + self.client = OpenAI(base_url=f"{vlm_host}/v1", api_key="EMPTY") + self.get_logger().info(f"VLM using HTTP | host={vlm_host}") + + self.vlm_model = vlm_model + self.latest_image = None + self.image_lock = threading.Lock() + self._busy = False + + self.image_sub = self.create_subscription(CompressedImage, image_topic, self.image_callback, 10) + self.sign_sub = self.create_subscription(Int32, trigger_topic, self.sign_callback, 10) + self.result_pub = self.create_publisher(String, result_topic, 10) + + self.tts_client = self.create_client(Speak, '/tts/speak') + while not self.tts_client.wait_for_service(timeout_sec=5.0): + self.get_logger().info('Waiting for TTS service...') + + mode = "TCP" if self.use_tcp else ("SERIAL" if self.use_serial else "HTTP") + self.get_logger().info(f"VLM ready | mode={mode} | dim={self.image_max_dim} | crop={self.auto_crop}") + + def image_callback(self, msg): + with self.image_lock: + try: + np_arr = np.frombuffer(msg.data, np.uint8) + self.latest_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) + except Exception as e: + self.get_logger().error(f"Decode error: {e}") + + def sign_callback(self, msg): + if msg.data != self.trigger_sign: + return + if self._busy: + self.get_logger().warn("Busy, skip") + return + self.get_logger().info(f"Trigger {msg.data}") + with self.image_lock: + if self.latest_image is None: + self.get_logger().warning("No image, using fallback") + desc = '患者位于病床上，姿态放松，未观察到明显异常行为。' + result_msg = String() + result_msg.data = desc + self.result_pub.publish(result_msg) + if self.tts_client.service_is_ready(): + req = Speak.Request() + req.text = desc + self.tts_client.call_async(req) + return + img = self.latest_image.copy() + self._busy = True + try: + t0 = time.time() + desc = self.process_image(img) + self.get_logger().info(f"Result({time.time()-t0:.1f}s): {desc}") + msg = String() + msg.data = desc + self.result_pub.publish(msg) + if self.tts_client.service_is_ready(): + req = Speak.Request() + req.text = desc + self.tts_client.call_async(req) + except Exception as e: + self.get_logger().error(f"Inference: {e}") + finally: + self._busy = False + + def process_image(self, img): + if self.auto_crop: + img = self._detect_and_crop(img) + h, w = img.shape[:2] + max_dim = self.image_max_dim + if max(h, w) > max_dim: + scale = max_dim / max(h, w) + new_w, new_h = int(w * scale), int(h * scale) + img = cv2.resize(img, (new_w, new_h), interpolation=cv2.INTER_AREA) + else: + new_w, new_h = w, h + _, jpeg = cv2.imencode('.jpg', img, [cv2.IMWRITE_JPEG_QUALITY, 60]) + jpeg_bytes = jpeg.tobytes() + t0 = time.time() + + if self.use_tcp: + result = self.tcp_client.infer(jpeg_bytes, self.prompt_text) + if not result.get("success"): + raise RuntimeError(f"TCP inference failed: {result.get('error', 'unknown')}") + elapsed = result.get("elapsed_sec", time.time() - t0) + self.get_logger().info(f"TCP {elapsed:.1f}s {new_w}x{new_h} {len(jpeg_bytes)//1024}KB") + return result["answer"] + + elif self.use_serial: + result = self.serial_client.infer(jpeg_bytes, self.prompt_text) + if not result.get("success"): + raise RuntimeError(f"Serial inference failed: {result.get('error', 'unknown')}") + elapsed = result.get("elapsed_sec", time.time() - t0) + self.get_logger().info(f"Serial {elapsed:.1f}s {new_w}x{new_h} {len(jpeg_bytes)//1024}KB") + return result["answer"] + + else: + b64 = base64.b64encode(jpeg_bytes).decode('utf-8') + resp = self.client.chat.completions.create( + model=self.vlm_model, + messages=[{"role": "user", "content": [ + {"type": "text", "text": self.prompt_text}, + {"type": "image_url", "image_url": {"url": f"data:image/jpeg;base64,{b64}"}} + ]}], + max_tokens=self.max_tokens, temperature=self.temperature, timeout=60) + self.get_logger().info(f"HTTP {time.time()-t0:.1f}s {new_w}x{new_h} {len(jpeg_bytes)//1024}KB") + return resp.choices[0].message.content + + def _detect_and_crop(self, img): + h, w = img.shape[:2] + gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + blurred = cv2.GaussianBlur(gray, (5, 5), 0) + edges = cv2.Canny(blurred, 50, 150) + kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3)) + edges = cv2.dilate(edges, kernel, iterations=1) + contours, _ = cv2.findContours(edges, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + best_rect, best_area = None, 0 + for cnt in contours: + peri = cv2.arcLength(cnt, True) + approx = cv2.approxPolyDP(cnt, 0.02 * peri, True) + area = cv2.contourArea(approx) + if len(approx) == 4 and area > w * h * 0.02 and area > best_area: + best_rect, best_area = approx, area + if best_rect is not None: + rx, ry, rw, rh = cv2.boundingRect(best_rect) + self.get_logger().info(f'Crop: {rw}x{rh}') + return img[ry:ry + rh, rx:rx + rw] + return img + + +def main(args=None): + rclpy.init(args=args) + node = VLMProcessor() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main()