改了下地图和部分规划参数

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

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.

src/racing_control/include/racing_control/racing_control.hpp

@@ -186,6 +186,37 @@ inline std::vector<geometry_msgs::msg::PoseStamped> remainingWaypoints(
   return {waypoints.begin() + static_cast<std::ptrdiff_t>(next_waypoint_index), waypoints.end()};
 }
 
+inline std::vector<geometry_msgs::msg::PoseStamped> remainingWaypointsAfterProgress(
+  const std::vector<geometry_msgs::msg::PoseStamped> & 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<geometry_msgs::msg::PoseStamped> routeWaypointsAfterQr(
+  const geometry_msgs::msg::PoseStamped & entry_pose,
+  const std::vector<geometry_msgs::msg::PoseStamped> & route_waypoints,
+  const std::size_t vlm_waypoint_index,
+  const bool split_at_vlm)
+{
+  std::vector<geometry_msgs::msg::PoseStamped> 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<std::ptrdiff_t>(route_end));
+  return segment;
+}
+
 inline std::vector<geometry_msgs::msg::PoseStamped> posesFromFlatDoubles(
   const std::vector<double> & values, const std::string & frame_id)
 {

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",

src/racing_control/src/racing_control.cpp

@@ -4,6 +4,7 @@
 #include <chrono>
 #include <cstdint>
 #include <cmath>
+#include <fcntl.h>
 #include <functional>
 #include <memory>
 #include <mutex>
@@ -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<bool>(
       "use_trajectory_guard", defaultUseTrajectoryGuard());
     use_post_qr_pose_ = declare_parameter<bool>("use_post_qr_pose", false);
+    split_qr_to_vlm_segment_ = declare_parameter<bool>("split_qr_to_vlm_segment", true);
     enable_vlm_image_relay_ = declare_parameter<bool>("enable_vlm_image_relay", false);
     enable_dynamic_replanning_ = declare_parameter<bool>("enable_dynamic_replanning", true);
     enable_recovery_ = declare_parameter<bool>("enable_recovery", true);
@@ -235,7 +238,7 @@ private:
       declare_parameter<double>("recovery_backup_timeout_sec", 0.2);
     pass_through_vlm_trigger_radius_ =
       declare_parameter<double>("pass_through_vlm_trigger_radius", 0.35);
-    circle_goal_tolerance_ = declare_parameter<double>("circle_goal_tolerance", 0.30);
+    circle_goal_tolerance_ = declare_parameter<double>("circle_goal_tolerance", 0.50);
     dynamic_replan_max_consecutive_failures_ =
       declare_parameter<int>("dynamic_replan_max_consecutive_failures", 3);
     max_recovery_attempts_ = declare_parameter<int>("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<unsigned int>(~(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};

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));

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

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')

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,
+    ])