diff --git a/src/map/nav2_costmap_binary 01.png b/src/map/nav2_costmap_binary 01.png new file mode 100644 index 0000000..c414579 Binary files /dev/null and b/src/map/nav2_costmap_binary 01.png differ diff --git a/src/map/nav2_costmap_binary 02.png b/src/map/nav2_costmap_binary 02.png new file mode 100644 index 0000000..71a638f Binary files /dev/null and b/src/map/nav2_costmap_binary 02.png differ diff --git a/src/map/nav2_costmap_binary.png b/src/map/nav2_costmap_binary.png index c414579..711934e 100644 Binary files a/src/map/nav2_costmap_binary.png and b/src/map/nav2_costmap_binary.png differ diff --git a/src/navigation/obstacle_nav2/config/nav2_profile_10.yaml b/src/navigation/obstacle_nav2/config/nav2_profile_10.yaml index d905983..3015acc 100644 --- a/src/navigation/obstacle_nav2/config/nav2_profile_10.yaml +++ b/src/navigation/obstacle_nav2/config/nav2_profile_10.yaml @@ -112,7 +112,7 @@ controller_server: PreferForwardCritic: enabled: false cost_power: 1 - cost_weight: 7.0 + cost_weight: 9.0 threshold_to_consider: 0.5 CostCritic: enabled: true @@ -225,7 +225,7 @@ global_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 - inflation_radius: 0.30 + inflation_radius: 0.35 always_send_full_costmap: True global_costmap_client: ros__parameters: @@ -251,10 +251,10 @@ planner_server: angle_quantization_bins: 72 analytic_expansion_ratio: 3.5 analytic_expansion_max_length: 3.0 - minimum_turning_radius: 0.40 - reverse_penalty: 2.0 + minimum_turning_radius: 0.45 + reverse_penalty: 4.0 change_penalty: 1.0 - non_straight_penalty: 1.2 + non_straight_penalty: 1.0 cost_penalty: 3.0 retrospective_penalty: 0.015 # 5 m covers the rolling planning horizon without the startup and memory diff --git a/src/racing_control/config/racing_control.yaml b/src/racing_control/config/racing_control.yaml index cc2648b..f8e1422 100644 --- a/src/racing_control/config/racing_control.yaml +++ b/src/racing_control/config/racing_control.yaml @@ -4,6 +4,7 @@ racing_control: auto_start: false frame_id: odom use_post_qr_pose: false + split_qr_to_vlm_segment: true enable_vlm_image_relay: false enable_dynamic_replanning: false enable_recovery: true @@ -42,7 +43,7 @@ racing_control: recovery_backup_distance: 0.04 recovery_backup_timeout_sec: 0.2 max_recovery_attempts: 2 - circle_goal_tolerance: 0.30 + circle_goal_tolerance: 0.50 # VLM capture: # stop - stop at the VLM waypoint, trigger capture, then continue immediately. diff --git a/src/racing_control/include/racing_control/racing_control.hpp b/src/racing_control/include/racing_control/racing_control.hpp index 2a5c4ff..f523724 100644 --- a/src/racing_control/include/racing_control/racing_control.hpp +++ b/src/racing_control/include/racing_control/racing_control.hpp @@ -186,6 +186,37 @@ inline std::vector remainingWaypoints( return {waypoints.begin() + static_cast(next_waypoint_index), waypoints.end()}; } +inline std::vector remainingWaypointsAfterProgress( + const std::vector & waypoints, + const geometry_msgs::msg::PoseStamped & current, + const std::size_t next_waypoint_index, + const double tolerance) +{ + return remainingWaypoints( + waypoints, + advanceReachedWaypointIndex(waypoints, current, next_waypoint_index, tolerance)); +} + +inline std::vector routeWaypointsAfterQr( + const geometry_msgs::msg::PoseStamped & entry_pose, + const std::vector & route_waypoints, + const std::size_t vlm_waypoint_index, + const bool split_at_vlm) +{ + std::vector segment; + segment.reserve(route_waypoints.size() + 1); + segment.push_back(entry_pose); + + std::size_t route_end = route_waypoints.size(); + if (split_at_vlm && vlm_waypoint_index + 1 < route_end) { + route_end = vlm_waypoint_index + 1; + } + segment.insert( + segment.end(), route_waypoints.begin(), route_waypoints.begin() + + static_cast(route_end)); + return segment; +} + inline std::vector posesFromFlatDoubles( const std::vector & values, const std::string & frame_id) { diff --git a/src/racing_control/launch/racing_control.launch.py b/src/racing_control/launch/racing_control.launch.py index 9a8677e..3c945f3 100644 --- a/src/racing_control/launch/racing_control.launch.py +++ b/src/racing_control/launch/racing_control.launch.py @@ -16,6 +16,7 @@ def generate_launch_description(): params_file = LaunchConfiguration("params_file") auto_start = LaunchConfiguration("auto_start") + split_qr_to_vlm_segment = LaunchConfiguration("split_qr_to_vlm_segment") enable_vlm_image_relay = LaunchConfiguration("enable_vlm_image_relay") vlm_image_input_topic = LaunchConfiguration("vlm_image_input_topic") vlm_image_output_topic = LaunchConfiguration("vlm_image_output_topic") @@ -29,6 +30,7 @@ def generate_launch_description(): params_file, { "auto_start": auto_start, + "split_qr_to_vlm_segment": split_qr_to_vlm_segment, "enable_vlm_image_relay": enable_vlm_image_relay, "vlm_image_input_topic": vlm_image_input_topic, "vlm_image_output_topic": vlm_image_output_topic, @@ -48,6 +50,11 @@ def generate_launch_description(): default_value="false", description="Start the race immediately instead of waiting for SPACE", ), + DeclareLaunchArgument( + "split_qr_to_vlm_segment", + default_value="true", + description="Stop at the VLM waypoint before planning the final home segment", + ), DeclareLaunchArgument( "enable_vlm_image_relay", default_value="false", diff --git a/src/racing_control/src/racing_control.cpp b/src/racing_control/src/racing_control.cpp index 7838047..e2cdf1f 100644 --- a/src/racing_control/src/racing_control.cpp +++ b/src/racing_control/src/racing_control.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include #include @@ -58,6 +59,7 @@ enum class Stage enum class RouteSegment { None, + ToQr, ToVlm, AfterVlm, FullRoute @@ -212,6 +214,7 @@ private: 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); @@ -235,7 +238,7 @@ private: declare_parameter("recovery_backup_timeout_sec", 0.2); pass_through_vlm_trigger_radius_ = declare_parameter("pass_through_vlm_trigger_radius", 0.35); - circle_goal_tolerance_ = declare_parameter("circle_goal_tolerance", 0.30); + 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); @@ -303,40 +306,55 @@ private: void startKeyboardThread() { - if (!isatty(STDIN_FILENO)) { + 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(), - "stdin is not a TTY; use auto_start:=true to start without keyboard"); + "no usable TTY for SPACE start; run from an interactive ssh tty or set auto_start:=true"); return; } keyboard_thread_ = std::thread( - [this]() { + [this, keyboard_fd, close_keyboard_fd]() { termios old_termios {}; - if (tcgetattr(STDIN_FILENO, &old_termios) != 0) { + 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(STDIN_FILENO, TCSANOW, &raw); + tcsetattr(keyboard_fd, TCSANOW, &raw); while (!stop_keyboard_.load()) { fd_set read_set; FD_ZERO(&read_set); - FD_SET(STDIN_FILENO, &read_set); + FD_SET(keyboard_fd, &read_set); timeval timeout {}; timeout.tv_sec = 0; timeout.tv_usec = 200000; - const int ready = select(STDIN_FILENO + 1, &read_set, nullptr, nullptr, &timeout); - if (ready > 0 && FD_ISSET(STDIN_FILENO, &read_set)) { + 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(STDIN_FILENO, &c, 1) == 1 && c == ' ') { + if (read(keyboard_fd, &c, 1) == 1 && c == ' ') { start_requested_.store(true); } } } - tcsetattr(STDIN_FILENO, TCSANOW, &old_termios); + tcsetattr(keyboard_fd, TCSANOW, &old_termios); + if (close_keyboard_fd) { + close(keyboard_fd); + } }); } @@ -404,12 +422,14 @@ private: if (stage_ == Stage::ExecuteCirclePath && use_trajectory_guard_ && routeSegmentReached()) { finishStage("route segment final pose reached"); - if (active_segment_ == RouteSegment::ToVlm) { + if (active_segment_ == RouteSegment::ToQr) { + runQrWait(); + } else if (active_segment_ == RouteSegment::ToVlm) { runVlmWait(); } else if (active_segment_ == RouteSegment::AfterVlm || active_segment_ == RouteSegment::FullRoute) { - runSwitchToNormalProfile(); + finishRace(); } } } @@ -520,22 +540,13 @@ private: latest_qr_result_.clear(); selected_direction_ = RouteDirection::Unknown; qr_detection_disabled_ = false; + recovery_attempts_ = 0; + active_segment_ = RouteSegment::ToQr; + active_segment_waypoints_ = {qr_pose_}; + active_segment_next_waypoint_index_ = 0; publishSign(sign_profile_normal_); publishSign(sign_qr_enable_); - startStage(Stage::NavigateToQr, navigation_timeout_sec_); - sendNavigateGoal( - qr_pose_, [this](const bool ok) { - if (stage_ != Stage::NavigateToQr) { - RCLCPP_DEBUG(get_logger(), "stale QR navigation result ignored"); - return; - } - finishStage(ok ? "reached " + poseSummary(qr_pose_) : "navigation failed"); - if (!ok) { - failRace("failed to reach QR pose"); - return; - } - runQrWait(); - }); + runRouteSegmentPlanning("start to QR pose"); } void runQrWait() @@ -551,11 +562,9 @@ private: void runQrTransitNavigation() { - if (qrTransitTargetAfterRecognition(use_post_qr_pose_) == QrTransitTarget::PostQr) { - runPostQrNavigation(); - } else { - runEntryNavigation(); - } + disableQrDetectionOnce(); + cancelActiveFollowGoal(); + runSwitchToTask2Profile(); } void runPostQrNavigation() @@ -613,37 +622,37 @@ private: latest_vlm_result_.clear(); vlm_capture_triggered_ = false; recovery_attempts_ = 0; - if (vlm_capture_mode_ == VlmCaptureMode::PassThrough) { - active_segment_ = RouteSegment::FullRoute; - active_segment_waypoints_ = route.waypoints; - active_segment_next_waypoint_index_ = 0; - runRouteSegmentPlanning("full route with pass-through VLM capture"); - return; + 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_); } - active_segment_ = RouteSegment::ToVlm; - active_segment_waypoints_.assign( - route.waypoints.begin(), - route.waypoints.begin() + vlm_index + 1); + const auto qr_segment = routeWaypointsAfterQr( + 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("to VLM waypoint"); + 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); - if (vlm_index + 1 >= route.waypoints.size()) { - runSwitchToNormalProfile(); - return; - } recovery_attempts_ = 0; active_segment_ = RouteSegment::AfterVlm; - active_segment_waypoints_.assign( - route.waypoints.begin() + vlm_index + 1, - route.waypoints.end()); + active_segment_waypoints_ = remainingWaypoints(route.waypoints, vlm_index + 1); + active_segment_waypoints_.push_back(route.home_pose); active_segment_next_waypoint_index_ = 0; - runRouteSegmentPlanning("after VLM waypoint"); + publishSign(sign_profile_normal_); + runRouteSegmentPlanning("VLM to home"); } void runRouteSegmentPlanning(const std::string & label) @@ -719,12 +728,14 @@ private: handleRouteExecutionFailure("route segment FollowPath failed"); return; } - if (active_segment_ == RouteSegment::ToVlm) { + if (active_segment_ == RouteSegment::ToQr) { + runQrWait(); + } else if (active_segment_ == RouteSegment::ToVlm) { runVlmWait(); } else if (active_segment_ == RouteSegment::AfterVlm || active_segment_ == RouteSegment::FullRoute) { - runSwitchToNormalProfile(); + finishRace(); } }); } @@ -780,9 +791,7 @@ private: void maybeTriggerPassThroughVlmCapture() { - if (vlm_capture_mode_ != VlmCaptureMode::PassThrough || - active_segment_ != RouteSegment::FullRoute || vlm_capture_triggered_) - { + if (active_segment_ != RouteSegment::FullRoute || vlm_capture_triggered_) { return; } @@ -827,18 +836,19 @@ private: last_dynamic_replan_time_ = now(); dynamic_replan_in_flight_ = true; - runDynamicRouteReplanning(); + runDynamicRouteReplanning(*current); } - void runDynamicRouteReplanning() + 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 = remainingWaypoints( - active_segment_waypoints_, active_segment_next_waypoint_index_); + 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; @@ -914,6 +924,16 @@ private: 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"); }); } @@ -1239,7 +1259,10 @@ private: 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) { + 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(); } @@ -1260,6 +1283,7 @@ private: 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}; diff --git a/src/racing_control/test/test_racing_control_helpers.cpp b/src/racing_control/test/test_racing_control_helpers.cpp index fca25c2..9972c94 100644 --- a/src/racing_control/test/test_racing_control_helpers.cpp +++ b/src/racing_control/test/test_racing_control_helpers.cpp @@ -140,11 +140,11 @@ TEST(RacingControlHelpers, AdvancesNextWaypointOnlyAfterItIsReached) EXPECT_EQ( racing_control::advanceReachedWaypointIndex( - waypoints, racing_control::poseFromXYYaw(0.65, 0.0, 0.0, "map"), 1, 0.30), + 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.30), + waypoints, racing_control::poseFromXYYaw(1.05, 0.0, 0.0, "map"), 1, 0.50), 2U); } @@ -163,6 +163,52 @@ TEST(RacingControlHelpers, DynamicReplanningKeepsRemainingUnreachedWaypoints) 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)); diff --git a/src/vlm_detect/config/vlm_detect.yaml b/src/vlm_detect/config/vlm_detect.yaml index 33ff4ed..3274702 100644 --- a/src/vlm_detect/config/vlm_detect.yaml +++ b/src/vlm_detect/config/vlm_detect.yaml @@ -9,10 +9,10 @@ vlm_detect: image_max_dim: 128 image_topic: /image max_tokens: 30 - prompt_text: 图中是一个2D动漫插画风格的医院病房，有一个病人。请描述这个病人的状态。不要描述边框、背景、环境。20字以内。 + prompt_text: 请描述这个病人的状态。不要描述边框、背景。15字以内。 result_topic: /vlm_result temperature: 0.1 trigger_sign: 9 trigger_topic: /sign4return - vlm_host: http://192.168.175.111:8000 + vlm_host: http://192.168.175.64:8000 vlm_model: /home/wisdom/models/gguf/Qwen2-VL-2B-Instruct-Q4_K_M.gguf diff --git a/src/vlm_detect/launch/vlm_detect.launch.py b/src/vlm_detect/launch/vlm_detect.launch.py index c7b023f..34ae249 100644 --- a/src/vlm_detect/launch/vlm_detect.launch.py +++ b/src/vlm_detect/launch/vlm_detect.launch.py @@ -35,15 +35,16 @@ def generate_launch_description(): default_value=PathJoinSubstitution([ get_package_share_directory('vlm_detect'), 'config', 'vlm_detect.yaml'])) - declare_vlm_host = DeclareLaunchArgument('vlm_host', default_value='http://192.168.175.111:8000') - declare_vlm_model = DeclareLaunchArgument('vlm_model', default_value='/home/wisdom/models/gguf/Qwen2-VL-2B-Instruct-Q4_K_M.gguf') + # RDK X5 BPU server (default). For WSL server, use: vlm_host:=http://192.168.175.111:8000 + declare_vlm_host = DeclareLaunchArgument('vlm_host', default_value='http://192.168.175.64:8000') + declare_vlm_model = DeclareLaunchArgument('vlm_model', default_value='internvl2.5-qwen2.5-0.5b') declare_image_topic = DeclareLaunchArgument('image_topic', default_value='/image') declare_trigger_topic = DeclareLaunchArgument('trigger_topic', default_value='/sign4return') declare_trigger_sign = DeclareLaunchArgument('trigger_sign', default_value='9') declare_result_topic = DeclareLaunchArgument('result_topic', default_value='/vlm_result') - declare_prompt_text = DeclareLaunchArgument('prompt_text', default_value='图中是一个2D动漫插画风格的医院病房，有一个病人。请描述这个病人的状态。不要描述边框、背景、环境。20字以内。') - declare_max_tokens = DeclareLaunchArgument('max_tokens', default_value='100') - declare_image_max_dim = DeclareLaunchArgument('image_max_dim', default_value='128') + 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_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') diff --git a/src/vlm_detect/launch/vlm_detect.launch.py.wsl_bak b/src/vlm_detect/launch/vlm_detect.launch.py.wsl_bak new file mode 100644 index 0000000..c7b023f --- /dev/null +++ b/src/vlm_detect/launch/vlm_detect.launch.py.wsl_bak @@ -0,0 +1,113 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, LogInfo +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node + + +def generate_launch_description(): + use_vlm = LaunchConfiguration('use_vlm') + use_tts = LaunchConfiguration('use_tts') + use_qr_tts = LaunchConfiguration('use_qr_tts') + config_file = LaunchConfiguration('config_file') + + vlm_host = LaunchConfiguration('vlm_host') + vlm_model = LaunchConfiguration('vlm_model') + image_topic = LaunchConfiguration('image_topic') + trigger_topic = LaunchConfiguration('trigger_topic') + trigger_sign = LaunchConfiguration('trigger_sign') + result_topic = LaunchConfiguration('result_topic') + prompt_text = LaunchConfiguration('prompt_text') + max_tokens = LaunchConfiguration('max_tokens') + image_max_dim = LaunchConfiguration('image_max_dim') + + audio_sink = LaunchConfiguration('audio_sink') + tts_speed = LaunchConfiguration('tts_speed') + + declare_use_vlm = DeclareLaunchArgument('use_vlm', default_value='true') + declare_use_tts = DeclareLaunchArgument('use_tts', default_value='true') + declare_use_qr_tts = DeclareLaunchArgument('use_qr_tts', default_value='false') + declare_config_file = DeclareLaunchArgument('config_file', + default_value=PathJoinSubstitution([ + get_package_share_directory('vlm_detect'), 'config', 'vlm_detect.yaml'])) + + declare_vlm_host = DeclareLaunchArgument('vlm_host', default_value='http://192.168.175.111:8000') + declare_vlm_model = DeclareLaunchArgument('vlm_model', default_value='/home/wisdom/models/gguf/Qwen2-VL-2B-Instruct-Q4_K_M.gguf') + declare_image_topic = DeclareLaunchArgument('image_topic', default_value='/image') + declare_trigger_topic = DeclareLaunchArgument('trigger_topic', default_value='/sign4return') + declare_trigger_sign = DeclareLaunchArgument('trigger_sign', default_value='9') + declare_result_topic = DeclareLaunchArgument('result_topic', default_value='/vlm_result') + declare_prompt_text = DeclareLaunchArgument('prompt_text', default_value='图中是一个2D动漫插画风格的医院病房，有一个病人。请描述这个病人的状态。不要描述边框、背景、环境。20字以内。') + declare_max_tokens = DeclareLaunchArgument('max_tokens', default_value='100') + declare_image_max_dim = DeclareLaunchArgument('image_max_dim', default_value='128') + 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') + + vlm_node = Node( + package='vlm_detect', + executable='vlm_node', + name='vlm_detect', + output='screen', + condition=IfCondition(use_vlm), + parameters=[config_file, + { + 'vlm_host': vlm_host, + 'vlm_model': vlm_model, + 'image_topic': image_topic, + 'trigger_topic': trigger_topic, + 'trigger_sign': trigger_sign, + 'result_topic': result_topic, + 'prompt_text': prompt_text, + 'max_tokens': max_tokens, + 'image_max_dim': image_max_dim, + }], + ) + + tts_server = Node( + package='vlm_detect', + executable='tts_server', + name='tts_server', + output='screen', + condition=IfCondition(use_tts), + parameters=[config_file, + { + 'audio_sink': audio_sink, + 'tts_speed': tts_speed, + }], + ) + + qr_tts_bridge = Node( + package='vlm_detect', + executable='qr_tts_bridge', + name='qr_tts_bridge', + output='screen', + condition=IfCondition(use_qr_tts), + ) + + return LaunchDescription([ + declare_use_vlm, + declare_use_tts, + declare_use_qr_tts, + declare_config_file, + declare_vlm_host, + declare_vlm_model, + declare_image_topic, + declare_trigger_topic, + declare_trigger_sign, + declare_result_topic, + declare_prompt_text, + declare_max_tokens, + declare_image_max_dim, + declare_audio_sink, + declare_tts_speed, + LogInfo(msg=['Config: ', config_file]), + LogInfo(msg=['VLM Host: ', vlm_host]), + vlm_node, + tts_server, + qr_tts_bridge, + ])