cyy/yiliao2026
1
0
Fork 0
forked from zbw/yiliao2026
Code Pull Requests Activity

vlm和tts

Browse Source
This commit is contained in:
Orange
2026-07-23 14:51:51 +08:00
parent 90761b40b2
commit a5cee5e142
35 changed files with 12694 additions and 168 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
src/my_robot_bringup/launch/master_launch.py
View File

@@ -130,21 +130,21 @@ def generate_launch_description():
# ================================================================
# 阶段 4: USB 摄像头 + 二维码识别 (t=4s)
# usb_cam: 驱动 USB 摄像头，发布 /image_raw
# car_usb_cam: 使用 hobot_usb_cam 驱动 USB 摄像头，发布 /image_raw
# qr_detect: 订阅 /image_raw，识别二维码，发布 qr_results
# ================================================================
usb_camera = Node(
package='usb_cam',
executable='usb_cam_node_exe',
name='usb_cam',
output='screen',
usb_camera = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
FindPackageShare('car_usb_cam'), '/launch', '/hobot_usb_cam.launch.py'
]),
condition=IfCondition(use_qr),
parameters=[{
'video_device': camera_device,
'image_size': [640, 480],
'pixel_format': 'YUYV',
'framerate': 30.0,
}],
launch_arguments={
'usb_video_device': camera_device,
'usb_image_width': '640',
'usb_image_height': '480',
'usb_framerate': '30',
'usb_pixel_format': 'yuyv2rgb',
}.items(),
)
qr_detect = Node(

4
src/origincar_description/CMakeLists.txt
View File

@@ -22,11 +22,7 @@ find_package(ament_cmake REQUIRED)
# find_package(<dependency> REQUIRED)
install(
<<<<<<< HEAD
DIRECTORY launch urdf rviz meshes world
=======
DIRECTORY launch urdf rviz meshes world
>>>>>>> mo_new
DESTINATION share/${PROJECT_NAME}
)

1
src/origincar_msg/CMakeLists.txt
View File

@@ -25,6 +25,7 @@ find_package(std_msgs REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/Data.msg"
"msg/Sign.msg"
"srv/Speak.srv"
DEPENDENCIES std_msgs
ADD_LINTER_TESTS
)

4
src/origincar_msg/srv/Speak.srv Normal file
View File

@@ -0,0 +1,4 @@
string text
---
bool success
string message

3
src/planner/config/odom_hybrid_astar.yaml
View File

@@ -46,9 +46,10 @@ grid_astar_theta_planner:
yaw_bins: 36
primitive_length: 0.12
collision_sample_step: 0.03
goal_xy_tolerance: 0.06
goal_xy_tolerance: 0.10
goal_yaw_tolerance: 0.174533
require_goal_yaw: false
snap_final_xy_to_goal: true
use_grid_heuristic: true
heuristic_weight: 1.35
goal_yaw_heuristic_weight: 0.0

24
src/planner/config/odom_pure_pursuit.yaml
View File

@@ -14,18 +14,28 @@ topology_pure_pursuit:
cancel_topic: /navigation_cancel
control_rate: 20.0
lookahead_distance: 0.40
lookahead_distance: 0.28
min_lookahead_distance: 0.10
lookahead_curvature_gain: 0.80
lookahead_error_gain: 2.00
heading_correction_gain: 0.80
cross_track_correction_gain: 1.20
cross_track_speed_gain: 1.80
min_tracking_speed_ratio: 0.35
path_reacquire_distance: 0.45
goal_tolerance: 0.06
goal_yaw_tolerance: 0.174533
align_goal_yaw: false
final_alignment_offset: 0.25
final_alignment_speed: 0.06
linear_speed: 0.60
min_linear_speed: 0.08
reverse_speed: 0.25
slowdown_distance: 0.35
curvature_slowdown_gain: 0.10
max_angular_speed: 2.80
linear_speed: 0.45
min_linear_speed: 0.06
reverse_speed: 0.18
slowdown_distance: 0.45
curvature_slowdown_gain: 0.06
min_curvature_speed_ratio: 0.70
min_goal_slowdown_ratio: 0.35
max_angular_speed: 3.20
min_turning_radius: 0.25
latency_compensation: true

1
src/planner/config/real_hybrid_astar.yaml
View File

@@ -47,6 +47,7 @@ grid_astar_theta_planner:
# Point navigation does not require the car to finish at a specific yaw.
# Set true when final docking orientation is required.
require_goal_yaw: false
snap_final_xy_to_goal: true
use_grid_heuristic: true
heuristic_weight: 1.25
goal_yaw_heuristic_weight: 0.0

20
src/planner/config/real_pure_pursuit.yaml
View File

@@ -14,16 +14,26 @@ topology_pure_pursuit:
cancel_topic: /navigation_cancel
control_rate: 20.0
lookahead_distance: 0.45
lookahead_distance: 0.30
min_lookahead_distance: 0.12
lookahead_curvature_gain: 0.70
lookahead_error_gain: 1.60
heading_correction_gain: 0.70
cross_track_correction_gain: 1.00
cross_track_speed_gain: 1.50
min_tracking_speed_ratio: 0.35
path_reacquire_distance: 0.45
goal_tolerance: 0.15
goal_yaw_tolerance: 0.174533
align_goal_yaw: false
linear_speed: 0.20
linear_speed: 0.18
min_linear_speed: 0.06
reverse_speed: 0.10
reverse_speed: 0.08
slowdown_distance: 0.70
curvature_slowdown_gain: 0.25
max_angular_speed: 1.20
curvature_slowdown_gain: 0.12
min_curvature_speed_ratio: 0.65
min_goal_slowdown_ratio: 0.35
max_angular_speed: 1.50
min_turning_radius: 0.40
latency_compensation: true

86
src/planner/launch/odom_hybrid_astar.launch.py
View File

@@ -41,11 +41,21 @@ def generate_launch_description():
planner_min_turning_radius = LaunchConfiguration("planner_min_turning_radius")
tracker_min_turning_radius = LaunchConfiguration("tracker_min_turning_radius")
lookahead_distance = LaunchConfiguration("lookahead_distance")
min_lookahead_distance = LaunchConfiguration("min_lookahead_distance")
lookahead_curvature_gain = LaunchConfiguration("lookahead_curvature_gain")
lookahead_error_gain = LaunchConfiguration("lookahead_error_gain")
heading_correction_gain = LaunchConfiguration("heading_correction_gain")
cross_track_correction_gain = LaunchConfiguration("cross_track_correction_gain")
cross_track_speed_gain = LaunchConfiguration("cross_track_speed_gain")
min_tracking_speed_ratio = LaunchConfiguration("min_tracking_speed_ratio")
path_reacquire_distance = LaunchConfiguration("path_reacquire_distance")
linear_speed = LaunchConfiguration("linear_speed")
min_linear_speed = LaunchConfiguration("min_linear_speed")
reverse_speed = LaunchConfiguration("reverse_speed")
slowdown_distance = LaunchConfiguration("slowdown_distance")
curvature_slowdown_gain = LaunchConfiguration("curvature_slowdown_gain")
min_curvature_speed_ratio = LaunchConfiguration("min_curvature_speed_ratio")
min_goal_slowdown_ratio = LaunchConfiguration("min_goal_slowdown_ratio")
max_angular_speed = LaunchConfiguration("max_angular_speed")
goal_tolerance = LaunchConfiguration("goal_tolerance")
goal_yaw_tolerance = LaunchConfiguration("goal_yaw_tolerance")
@@ -54,6 +64,7 @@ def generate_launch_description():
final_alignment_speed = LaunchConfiguration("final_alignment_speed")
require_goal_yaw = LaunchConfiguration("require_goal_yaw")
planner_goal_xy_tolerance = LaunchConfiguration("planner_goal_xy_tolerance")
snap_final_xy_to_goal = LaunchConfiguration("snap_final_xy_to_goal")
use_grid_heuristic = LaunchConfiguration("use_grid_heuristic")
heuristic_weight = LaunchConfiguration("heuristic_weight")
goal_yaw_heuristic_weight = LaunchConfiguration("goal_yaw_heuristic_weight")
@@ -83,6 +94,30 @@ def generate_launch_description():
tracker_min_turning_radius, value_type=float
)
lookahead_distance_value = ParameterValue(lookahead_distance, value_type=float)
min_lookahead_distance_value = ParameterValue(
min_lookahead_distance, value_type=float
)
lookahead_curvature_gain_value = ParameterValue(
lookahead_curvature_gain, value_type=float
)
lookahead_error_gain_value = ParameterValue(
lookahead_error_gain, value_type=float
)
heading_correction_gain_value = ParameterValue(
heading_correction_gain, value_type=float
)
cross_track_correction_gain_value = ParameterValue(
cross_track_correction_gain, value_type=float
)
cross_track_speed_gain_value = ParameterValue(
cross_track_speed_gain, value_type=float
)
min_tracking_speed_ratio_value = ParameterValue(
min_tracking_speed_ratio, value_type=float
)
path_reacquire_distance_value = ParameterValue(
path_reacquire_distance, value_type=float
)
linear_speed_value = ParameterValue(linear_speed, value_type=float)
min_linear_speed_value = ParameterValue(min_linear_speed, value_type=float)
reverse_speed_value = ParameterValue(reverse_speed, value_type=float)
@@ -90,6 +125,12 @@ def generate_launch_description():
curvature_slowdown_gain_value = ParameterValue(
curvature_slowdown_gain, value_type=float
)
min_curvature_speed_ratio_value = ParameterValue(
min_curvature_speed_ratio, value_type=float
)
min_goal_slowdown_ratio_value = ParameterValue(
min_goal_slowdown_ratio, value_type=float
)
max_angular_speed_value = ParameterValue(max_angular_speed, value_type=float)
goal_tolerance_value = ParameterValue(goal_tolerance, value_type=float)
goal_yaw_tolerance_value = ParameterValue(goal_yaw_tolerance, value_type=float)
@@ -104,6 +145,9 @@ def generate_launch_description():
planner_goal_xy_tolerance_value = ParameterValue(
planner_goal_xy_tolerance, value_type=float
)
snap_final_xy_to_goal_value = ParameterValue(
snap_final_xy_to_goal, value_type=bool
)
use_grid_heuristic_value = ParameterValue(use_grid_heuristic, value_type=bool)
heuristic_weight_value = ParameterValue(heuristic_weight, value_type=float)
goal_yaw_heuristic_weight_value = ParameterValue(
@@ -225,6 +269,7 @@ def generate_launch_description():
"min_turning_radius": planner_min_turning_radius_value,
"require_goal_yaw": require_goal_yaw_value,
"goal_xy_tolerance": planner_goal_xy_tolerance_value,
"snap_final_xy_to_goal": snap_final_xy_to_goal_value,
"use_grid_heuristic": use_grid_heuristic_value,
"heuristic_weight": heuristic_weight_value,
"goal_yaw_heuristic_weight": goal_yaw_heuristic_weight_value,
@@ -251,11 +296,21 @@ def generate_launch_description():
"cmd_vel_topic": "/planner_cmd_vel",
"min_turning_radius": tracker_min_turning_radius_value,
"lookahead_distance": lookahead_distance_value,
"min_lookahead_distance": min_lookahead_distance_value,
"lookahead_curvature_gain": lookahead_curvature_gain_value,
"lookahead_error_gain": lookahead_error_gain_value,
"heading_correction_gain": heading_correction_gain_value,
"cross_track_correction_gain": cross_track_correction_gain_value,
"cross_track_speed_gain": cross_track_speed_gain_value,
"min_tracking_speed_ratio": min_tracking_speed_ratio_value,
"path_reacquire_distance": path_reacquire_distance_value,
"linear_speed": linear_speed_value,
"min_linear_speed": min_linear_speed_value,
"reverse_speed": reverse_speed_value,
"slowdown_distance": slowdown_distance_value,
"curvature_slowdown_gain": curvature_slowdown_gain_value,
"min_curvature_speed_ratio": min_curvature_speed_ratio_value,
"min_goal_slowdown_ratio": min_goal_slowdown_ratio_value,
"max_angular_speed": max_angular_speed_value,
"goal_tolerance": goal_tolerance_value,
"goal_yaw_tolerance": goal_yaw_tolerance_value,
@@ -308,15 +363,25 @@ def generate_launch_description():
),
DeclareLaunchArgument(
"lookahead_distance",
default_value="0.40",
default_value="0.28",
description="Pure Pursuit lookahead in meters.",
),
DeclareLaunchArgument("linear_speed", default_value="0.60"),
DeclareLaunchArgument("min_linear_speed", default_value="0.08"),
DeclareLaunchArgument("reverse_speed", default_value="0.25"),
DeclareLaunchArgument("slowdown_distance", default_value="0.35"),
DeclareLaunchArgument("curvature_slowdown_gain", default_value="0.10"),
DeclareLaunchArgument("max_angular_speed", default_value="2.80"),
DeclareLaunchArgument("min_lookahead_distance", default_value="0.10"),
DeclareLaunchArgument("lookahead_curvature_gain", default_value="0.80"),
DeclareLaunchArgument("lookahead_error_gain", default_value="2.00"),
DeclareLaunchArgument("heading_correction_gain", default_value="0.80"),
DeclareLaunchArgument("cross_track_correction_gain", default_value="1.20"),
DeclareLaunchArgument("cross_track_speed_gain", default_value="1.80"),
DeclareLaunchArgument("min_tracking_speed_ratio", default_value="0.35"),
DeclareLaunchArgument("path_reacquire_distance", default_value="0.45"),
DeclareLaunchArgument("linear_speed", default_value="0.45"),
DeclareLaunchArgument("min_linear_speed", default_value="0.06"),
DeclareLaunchArgument("reverse_speed", default_value="0.18"),
DeclareLaunchArgument("slowdown_distance", default_value="0.45"),
DeclareLaunchArgument("curvature_slowdown_gain", default_value="0.06"),
DeclareLaunchArgument("min_curvature_speed_ratio", default_value="0.70"),
DeclareLaunchArgument("min_goal_slowdown_ratio", default_value="0.35"),
DeclareLaunchArgument("max_angular_speed", default_value="3.20"),
DeclareLaunchArgument("goal_tolerance", default_value="0.06"),
DeclareLaunchArgument("goal_yaw_tolerance", default_value="0.174533"),
DeclareLaunchArgument("align_goal_yaw", default_value="false"),
@@ -329,9 +394,14 @@ def generate_launch_description():
),
DeclareLaunchArgument(
"planner_goal_xy_tolerance",
default_value="0.06",
default_value="0.10",
description="Hybrid A* final XY tolerance in meters.",
),
DeclareLaunchArgument(
"snap_final_xy_to_goal",
default_value="true",
description="Publish the final path XY exactly at the requested goal XY without forcing yaw.",
),
DeclareLaunchArgument(
"use_grid_heuristic",
default_value="true",

100
src/planner/launch/real_hybrid_astar.launch.py
View File

@@ -32,9 +32,27 @@ def generate_launch_description():
enable_motion = LaunchConfiguration("enable_motion")
wheelbase = LaunchConfiguration("wheelbase")
max_steering_angle = LaunchConfiguration("max_steering_angle")
lookahead_distance = LaunchConfiguration("lookahead_distance")
min_lookahead_distance = LaunchConfiguration("min_lookahead_distance")
lookahead_curvature_gain = LaunchConfiguration("lookahead_curvature_gain")
lookahead_error_gain = LaunchConfiguration("lookahead_error_gain")
heading_correction_gain = LaunchConfiguration("heading_correction_gain")
cross_track_correction_gain = LaunchConfiguration("cross_track_correction_gain")
cross_track_speed_gain = LaunchConfiguration("cross_track_speed_gain")
min_tracking_speed_ratio = LaunchConfiguration("min_tracking_speed_ratio")
path_reacquire_distance = LaunchConfiguration("path_reacquire_distance")
linear_speed = LaunchConfiguration("linear_speed")
min_linear_speed = LaunchConfiguration("min_linear_speed")
reverse_speed = LaunchConfiguration("reverse_speed")
slowdown_distance = LaunchConfiguration("slowdown_distance")
curvature_slowdown_gain = LaunchConfiguration("curvature_slowdown_gain")
min_curvature_speed_ratio = LaunchConfiguration("min_curvature_speed_ratio")
min_goal_slowdown_ratio = LaunchConfiguration("min_goal_slowdown_ratio")
max_angular_speed = LaunchConfiguration("max_angular_speed")
use_grid_heuristic = LaunchConfiguration("use_grid_heuristic")
heuristic_weight = LaunchConfiguration("heuristic_weight")
goal_yaw_heuristic_weight = LaunchConfiguration("goal_yaw_heuristic_weight")
snap_final_xy_to_goal = LaunchConfiguration("snap_final_xy_to_goal")
state_log_period = LaunchConfiguration("state_log_period")
search_progress_log_period_ms = LaunchConfiguration("search_progress_log_period_ms")
use_sim_time = LaunchConfiguration("use_sim_time")
@@ -42,11 +60,53 @@ def generate_launch_description():
enable_motion_value = ParameterValue(enable_motion, value_type=bool)
wheelbase_value = ParameterValue(wheelbase, value_type=float)
max_steering_angle_value = ParameterValue(max_steering_angle, value_type=float)
lookahead_distance_value = ParameterValue(lookahead_distance, value_type=float)
min_lookahead_distance_value = ParameterValue(
min_lookahead_distance, value_type=float
)
lookahead_curvature_gain_value = ParameterValue(
lookahead_curvature_gain, value_type=float
)
lookahead_error_gain_value = ParameterValue(
lookahead_error_gain, value_type=float
)
heading_correction_gain_value = ParameterValue(
heading_correction_gain, value_type=float
)
cross_track_correction_gain_value = ParameterValue(
cross_track_correction_gain, value_type=float
)
cross_track_speed_gain_value = ParameterValue(
cross_track_speed_gain, value_type=float
)
min_tracking_speed_ratio_value = ParameterValue(
min_tracking_speed_ratio, value_type=float
)
path_reacquire_distance_value = ParameterValue(
path_reacquire_distance, value_type=float
)
linear_speed_value = ParameterValue(linear_speed, value_type=float)
min_linear_speed_value = ParameterValue(min_linear_speed, value_type=float)
reverse_speed_value = ParameterValue(reverse_speed, value_type=float)
slowdown_distance_value = ParameterValue(slowdown_distance, value_type=float)
curvature_slowdown_gain_value = ParameterValue(
curvature_slowdown_gain, value_type=float
)
min_curvature_speed_ratio_value = ParameterValue(
min_curvature_speed_ratio, value_type=float
)
min_goal_slowdown_ratio_value = ParameterValue(
min_goal_slowdown_ratio, value_type=float
)
max_angular_speed_value = ParameterValue(max_angular_speed, value_type=float)
use_grid_heuristic_value = ParameterValue(use_grid_heuristic, value_type=bool)
heuristic_weight_value = ParameterValue(heuristic_weight, value_type=float)
goal_yaw_heuristic_weight_value = ParameterValue(
goal_yaw_heuristic_weight, value_type=float
)
snap_final_xy_to_goal_value = ParameterValue(
snap_final_xy_to_goal, value_type=bool
)
state_log_period_value = ParameterValue(state_log_period, value_type=float)
search_progress_log_period_ms_value = ParameterValue(
search_progress_log_period_ms, value_type=float
@@ -168,6 +228,7 @@ def generate_launch_description():
"use_grid_heuristic": use_grid_heuristic_value,
"heuristic_weight": heuristic_weight_value,
"goal_yaw_heuristic_weight": goal_yaw_heuristic_weight_value,
"snap_final_xy_to_goal": snap_final_xy_to_goal_value,
"state_log_period": state_log_period_value,
"search_progress_log_period_ms": search_progress_log_period_ms_value,
}],
@@ -182,6 +243,23 @@ def generate_launch_description():
"use_sim_time": use_sim_time_value,
"publish_cmd_vel": enable_motion_value,
"cmd_vel_topic": "/planner_cmd_vel",
"lookahead_distance": lookahead_distance_value,
"min_lookahead_distance": min_lookahead_distance_value,
"lookahead_curvature_gain": lookahead_curvature_gain_value,
"lookahead_error_gain": lookahead_error_gain_value,
"heading_correction_gain": heading_correction_gain_value,
"cross_track_correction_gain": cross_track_correction_gain_value,
"cross_track_speed_gain": cross_track_speed_gain_value,
"min_tracking_speed_ratio": min_tracking_speed_ratio_value,
"path_reacquire_distance": path_reacquire_distance_value,
"linear_speed": linear_speed_value,
"min_linear_speed": min_linear_speed_value,
"reverse_speed": reverse_speed_value,
"slowdown_distance": slowdown_distance_value,
"curvature_slowdown_gain": curvature_slowdown_gain_value,
"min_curvature_speed_ratio": min_curvature_speed_ratio_value,
"min_goal_slowdown_ratio": min_goal_slowdown_ratio_value,
"max_angular_speed": max_angular_speed_value,
"state_log_period": state_log_period_value,
}],
)
@@ -211,6 +289,23 @@ def generate_launch_description():
),
DeclareLaunchArgument("wheelbase", default_value="0.143"),
DeclareLaunchArgument("max_steering_angle", default_value="0.60"),
DeclareLaunchArgument("lookahead_distance", default_value="0.30"),
DeclareLaunchArgument("min_lookahead_distance", default_value="0.12"),
DeclareLaunchArgument("lookahead_curvature_gain", default_value="0.70"),
DeclareLaunchArgument("lookahead_error_gain", default_value="1.60"),
DeclareLaunchArgument("heading_correction_gain", default_value="0.70"),
DeclareLaunchArgument("cross_track_correction_gain", default_value="1.00"),
DeclareLaunchArgument("cross_track_speed_gain", default_value="1.50"),
DeclareLaunchArgument("min_tracking_speed_ratio", default_value="0.35"),
DeclareLaunchArgument("path_reacquire_distance", default_value="0.45"),
DeclareLaunchArgument("linear_speed", default_value="0.18"),
DeclareLaunchArgument("min_linear_speed", default_value="0.06"),
DeclareLaunchArgument("reverse_speed", default_value="0.08"),
DeclareLaunchArgument("slowdown_distance", default_value="0.70"),
DeclareLaunchArgument("curvature_slowdown_gain", default_value="0.12"),
DeclareLaunchArgument("min_curvature_speed_ratio", default_value="0.65"),
DeclareLaunchArgument("min_goal_slowdown_ratio", default_value="0.35"),
DeclareLaunchArgument("max_angular_speed", default_value="1.50"),
DeclareLaunchArgument(
"use_grid_heuristic",
default_value="true",
@@ -226,6 +321,11 @@ def generate_launch_description():
default_value="0.0",
description="Weight for final yaw guidance in the Hybrid A* heuristic.",
),
DeclareLaunchArgument(
"snap_final_xy_to_goal",
default_value="true",
description="Publish the final path XY exactly at the requested goal XY without forcing yaw.",
),
DeclareLaunchArgument(
"state_log_period",
default_value="1.0",

BIN
src/planner/scripts/__pycache__/ackermann_cmd_bridge.cpython-313.pyc Normal file
View File

Binary file not shown.

BIN
src/planner/scripts/__pycache__/evaluate_navigation_test.cpython-313.pyc Normal file
View File

Binary file not shown.

BIN
src/planner/scripts/__pycache__/navigation_test_recorder.cpython-313.pyc Normal file
View File

Binary file not shown.

BIN
src/planner/scripts/__pycache__/odom_map_tf.cpython-313.pyc Normal file
View File

Binary file not shown.

BIN
src/planner/scripts/__pycache__/topology_pure_pursuit_node.cpython-313.pyc Normal file
View File

Binary file not shown.

36
src/planner/scripts/goal_reach_debug_collector.py
View File

@@ -142,6 +142,7 @@ class GoalReachDebugCollector(Node):
self.finished = False
self.latest_odom = None
self.latest_odom_received_ns = 0
self.latest_goal = None
self.latest_plan_points = []
self.latest_cmd = None
@@ -208,6 +209,7 @@ class GoalReachDebugCollector(Node):
def _odom_cb(self, msg):
self.latest_odom = msg
self.latest_odom_received_ns = self.get_clock().now().nanoseconds
def _goal_cb(self, msg):
goal = {
@@ -348,18 +350,21 @@ class GoalReachDebugCollector(Node):
return None
msg = self.latest_odom
stamp = Time.from_msg(msg.header.stamp)
now_ns = self.get_clock().now().nanoseconds
receive_age = None
if self.latest_odom_received_ns > 0:
receive_age = max(0.0, (now_ns - self.latest_odom_received_ns) * 1e-9)
if msg.header.stamp.sec == 0 and msg.header.stamp.nanosec == 0:
age = 0.0
stamp_age = None
else:
age = (self.get_clock().now() - stamp).nanoseconds * 1e-9
if age < 0.0:
age = 0.0
if age > self.max_odom_age:
stamp = Time.from_msg(msg.header.stamp)
stamp_age = max(0.0, (self.get_clock().now() - stamp).nanoseconds * 1e-9)
if receive_age is not None and receive_age > self.max_odom_age:
return {
"available": False,
"reason": "odom_too_old",
"age_sec": age,
"odom_receive_age_sec": receive_age,
"odom_stamp_age_sec": stamp_age,
}
source_frame = msg.header.frame_id or self.map_frame
@@ -381,7 +386,8 @@ class GoalReachDebugCollector(Node):
"x": x,
"y": y,
"yaw": yaw,
"odom_age_sec": age,
"odom_receive_age_sec": receive_age,
"odom_stamp_age_sec": stamp_age,
"odom_linear_x": msg.twist.twist.linear.x,
"odom_angular_z": msg.twist.twist.angular.z,
}
@@ -448,6 +454,7 @@ class GoalReachDebugCollector(Node):
"goal_xy_tolerance",
"goal_yaw_tolerance",
"require_goal_yaw",
"snap_final_xy_to_goal",
"resolution",
"yaw_bins",
"primitive_length",
@@ -467,11 +474,21 @@ class GoalReachDebugCollector(Node):
"publish_cmd_vel",
"cmd_vel_topic",
"lookahead_distance",
"min_lookahead_distance",
"lookahead_curvature_gain",
"lookahead_error_gain",
"heading_correction_gain",
"cross_track_correction_gain",
"cross_track_speed_gain",
"min_tracking_speed_ratio",
"path_reacquire_distance",
"linear_speed",
"min_linear_speed",
"reverse_speed",
"slowdown_distance",
"curvature_slowdown_gain",
"min_curvature_speed_ratio",
"min_goal_slowdown_ratio",
"min_turning_radius",
"max_angular_speed",
],
@@ -628,8 +645,9 @@ class GoalReachDebugCollector(Node):
planner_xy = planner.get("goal_xy_tolerance")
tracker_xy = tracker.get("goal_tolerance")
snap_xy = bool(planner.get("snap_final_xy_to_goal"))
if isinstance(planner_xy, (int, float)) and isinstance(tracker_xy, (int, float)):
if planner_xy > tracker_xy:
if planner_xy > tracker_xy and not snap_xy:
hints.append(
"planner 的 goal_xy_tolerance 大于 tracker 的 goal_tolerance；"
"planner 可能发布一个离原始目标较远的路径终点。"

2
src/planner/scripts/preflight_check.py
View File

@@ -32,7 +32,7 @@ class PreflightCheck(Node):
self.declare_parameter("map_topic", "/map")
self.declare_parameter("odom_topic", "/odom_combined")
self.declare_parameter("scan_topic", "/scan")
self.declare_parameter("cmd_vel_topic", "/cmd_vel")
self.declare_parameter("cmd_vel_topic", "/planner_cmd_vel")
self.declare_parameter("expected_scan_frame", "laser_link")
self.declare_parameter("tf_wait_sec", 3.0)
self.declare_parameter("drive_command_topic", "/ackermann_cmd")

4
src/planner/scripts/send_goal.py
View File

@@ -39,8 +39,8 @@ def main():
parser.add_argument("yaw", type=float, nargs="?", default=0.0)
parser.add_argument("--topic", default="/goal_pose")
parser.add_argument("--frame", default="map")
parser.add_argument("--timeout", type=float, default=5.0)
parser.add_argument("--repeat", type=int, default=1)
parser.add_argument("--timeout", type=float, default=8.0)
parser.add_argument("--repeat", type=int, default=3)
args = parser.parse_args()
rclpy.init()

338
src/planner/scripts/topology_pure_pursuit_node.py
View File

@@ -62,12 +62,22 @@ class TopologyPurePursuit(Node):
self.declare_parameter("path_resolution", 0.10)
self.declare_parameter("nearest_node_max_distance", 2.0)
self.declare_parameter("lookahead_distance", 0.55)
self.declare_parameter("min_lookahead_distance", 0.16)
self.declare_parameter("lookahead_curvature_gain", 0.50)
self.declare_parameter("lookahead_error_gain", 1.20)
self.declare_parameter("heading_correction_gain", 0.55)
self.declare_parameter("cross_track_correction_gain", 0.80)
self.declare_parameter("cross_track_speed_gain", 1.20)
self.declare_parameter("min_tracking_speed_ratio", 0.40)
self.declare_parameter("path_reacquire_distance", 0.50)
self.declare_parameter("goal_tolerance", 0.25)
self.declare_parameter("linear_speed", 0.30)
self.declare_parameter("min_linear_speed", 0.08)
self.declare_parameter("reverse_speed", 0.12)
self.declare_parameter("slowdown_distance", 0.80)
self.declare_parameter("curvature_slowdown_gain", 0.18)
self.declare_parameter("min_curvature_speed_ratio", 0.35)
self.declare_parameter("min_goal_slowdown_ratio", 0.25)
self.declare_parameter("max_angular_speed", 1.5)
self.declare_parameter("min_turning_radius", 0.35)
self.declare_parameter("goal_yaw_tolerance", 0.10)
@@ -99,6 +109,35 @@ class TopologyPurePursuit(Node):
self.lookahead_distance = float(
self.get_parameter("lookahead_distance").value
)
self.min_lookahead_distance = max(
0.05,
min(
self.lookahead_distance,
float(self.get_parameter("min_lookahead_distance").value),
),
)
self.lookahead_curvature_gain = max(
0.0, float(self.get_parameter("lookahead_curvature_gain").value)
)
self.lookahead_error_gain = max(
0.0, float(self.get_parameter("lookahead_error_gain").value)
)
self.heading_correction_gain = max(
0.0, float(self.get_parameter("heading_correction_gain").value)
)
self.cross_track_correction_gain = max(
0.0, float(self.get_parameter("cross_track_correction_gain").value)
)
self.cross_track_speed_gain = max(
0.0, float(self.get_parameter("cross_track_speed_gain").value)
)
self.min_tracking_speed_ratio = max(
0.0,
min(1.0, float(self.get_parameter("min_tracking_speed_ratio").value)),
)
self.path_reacquire_distance = max(
0.0, float(self.get_parameter("path_reacquire_distance").value)
)
self.goal_tolerance = float(self.get_parameter("goal_tolerance").value)
self.linear_speed = float(self.get_parameter("linear_speed").value)
self.min_linear_speed = float(self.get_parameter("min_linear_speed").value)
@@ -107,6 +146,14 @@ class TopologyPurePursuit(Node):
self.curvature_slowdown_gain = float(
self.get_parameter("curvature_slowdown_gain").value
)
self.min_curvature_speed_ratio = max(
0.0,
min(1.0, float(self.get_parameter("min_curvature_speed_ratio").value)),
)
self.min_goal_slowdown_ratio = max(
0.0,
min(1.0, float(self.get_parameter("min_goal_slowdown_ratio").value)),
)
self.max_angular_speed = float(
self.get_parameter("max_angular_speed").value
)
@@ -151,6 +198,7 @@ class TopologyPurePursuit(Node):
self.tf_listener = TransformListener(self.tf_buffer, self)
self.latest_odom = None
self.latest_odom_received_ns = 0
self.active_path = []
self.active_node_path = []
self.active_goal_node = ""
@@ -162,9 +210,11 @@ class TopologyPurePursuit(Node):
self.last_cmd_linear = 0.0
self.last_cmd_angular = 0.0
self.plan_pub = self.create_publisher(
NavPath, self.get_parameter("plan_topic").value, 10
)
self.plan_pub = None
if self.enable_topology_planning:
self.plan_pub = self.create_publisher(
NavPath, self.get_parameter("plan_topic").value, 10
)
self.cmd_pub = self.create_publisher(
Twist, self.get_parameter("cmd_vel_topic").value, 10
)
@@ -272,6 +322,7 @@ class TopologyPurePursuit(Node):
def _odom_callback(self, msg):
self.latest_odom = msg
self.latest_odom_received_ns = self.get_clock().now().nanoseconds
if (
self.enable_topology_planning
and self.autoplan_default_goal
@@ -462,6 +513,8 @@ class TopologyPurePursuit(Node):
return updated
def _publish_path(self):
if self.plan_pub is None:
return
path_msg = NavPath()
path_msg.header.frame_id = self.map_frame
path_msg.header.stamp = self.get_clock().now().to_msg()
@@ -474,6 +527,12 @@ class TopologyPurePursuit(Node):
def _current_pose(self, compensate=True):
if self.latest_odom is None:
return None
now_ns = self.get_clock().now().nanoseconds
if self.latest_odom_received_ns <= 0:
return None
age = max(0.0, (now_ns - self.latest_odom_received_ns) * 1e-9)
if age > self.max_odom_age:
return None
msg = self.latest_odom
source_frame = msg.header.frame_id or self.map_frame
@@ -483,16 +542,6 @@ class TopologyPurePursuit(Node):
vx = msg.twist.twist.linear.x
wz = msg.twist.twist.angular.z
stamp = Time.from_msg(msg.header.stamp)
if msg.header.stamp.sec == 0 and msg.header.stamp.nanosec == 0:
age = 0.0
else:
age = (self.get_clock().now() - stamp).nanoseconds * 1e-9
if age < 0.0:
age = 0.0
if age > self.max_odom_age:
return None
if compensate and self.latency_compensation:
horizon = min(age, self.max_latency_compensation)
if abs(wz) < 1e-4:
@@ -587,28 +636,53 @@ class TopologyPurePursuit(Node):
)
return
nearest_index = self._nearest_path_index(x, y)
projection = self._nearest_path_projection(x, y)
nearest_index = projection["index"]
self.current_path_index = nearest_index
target_index = self._lookahead_index(x, y, nearest_index)
target_x, target_y, target_path_yaw = self.active_path[target_index]
nearest_x, nearest_y, _ = self.active_path[nearest_index]
cross_track_error = math.hypot(nearest_x - x, nearest_y - y)
cross_track_signed = projection["signed_error"]
cross_track_error = abs(cross_track_signed)
path_curvature = self._path_curvature(nearest_index)
lookahead_threshold = self._adaptive_lookahead(
path_curvature, cross_track_error, goal_distance
)
target = self._path_target_from_projection(projection, lookahead_threshold)
target_index = target["index"]
target_x = target["x"]
target_y = target["y"]
target_path_yaw = target["yaw"]
dx = target_x - x
dy = target_y - y
path_heading = self._segment_heading(target_index, x, y)
path_heading = target["path_heading"]
reverse = math.cos(normalize_angle(target_path_yaw - path_heading)) < 0.0
control_yaw = normalize_angle(yaw + math.pi) if reverse else yaw
# Track the XY curve. The path pose yaw is still used to detect reverse
# segments, but heading correction should follow the segment tangent so
# a noisy or intentionally ignored goal yaw does not pull the vehicle
# away from the planned line.
heading_error = normalize_angle(path_heading - control_yaw)
local_x = math.cos(control_yaw) * dx + math.sin(control_yaw) * dy
local_y = -math.sin(control_yaw) * dx + math.cos(control_yaw) * dy
lookahead = max(0.05, math.hypot(local_x, local_y))
curvature = 2.0 * local_y / (lookahead * lookahead)
curvature += (
self.heading_correction_gain
* math.sin(heading_error)
/ max(self.min_lookahead_distance, lookahead)
)
curvature += (
-self.cross_track_correction_gain
* cross_track_signed
/ max(self.min_lookahead_distance * self.min_lookahead_distance, lookahead * lookahead)
)
if self.min_turning_radius > 1e-3:
max_curvature = 1.0 / self.min_turning_radius
curvature = max(-max_curvature, min(max_curvature, curvature))
speed = self._target_speed(abs(curvature), goal_distance, reverse)
speed = self._target_speed(
abs(curvature), goal_distance, reverse, cross_track_error
)
angular = speed * curvature
angular = max(-self.max_angular_speed, min(self.max_angular_speed, angular))
self._publish_cmd(speed, angular)
@@ -620,9 +694,13 @@ class TopologyPurePursuit(Node):
nearest_index=nearest_index,
target_index=target_index,
cross_track_error=cross_track_error,
cross_track_signed=cross_track_signed,
reverse=reverse,
curvature=curvature,
lookahead=lookahead,
lookahead_threshold=lookahead_threshold,
path_curvature=path_curvature,
heading_error=heading_error,
)
def _align_final_yaw(self, x, y, yaw, goal_x, goal_y, goal_yaw, goal_yaw_error):
@@ -670,15 +748,177 @@ class TopologyPurePursuit(Node):
best_dist = dist
return best_index
def _lookahead_index(self, x, y, start_index):
def _nearest_path_projection(self, x, y):
if len(self.active_path) < 2:
px, py, pyaw = self.active_path[0]
return {
"index": 0,
"t": 0.0,
"x": px,
"y": py,
"yaw": pyaw,
"path_heading": pyaw,
"signed_error": 0.0,
"distance": math.hypot(x - px, y - py),
}
if self.path_reacquire_distance > 0.0:
nearest_vertex = self._nearest_path_index(x, y)
vx, vy, _ = self.active_path[nearest_vertex]
vertex_error = math.hypot(vx - x, vy - y)
if vertex_error > self.path_reacquire_distance:
start = 0
else:
start = max(0, self.current_path_index - 8)
else:
start = max(0, self.current_path_index - 8)
best = None
best_dist_sq = float("inf")
for index in range(start, len(self.active_path) - 1):
x0, y0, yaw0 = self.active_path[index]
x1, y1, yaw1 = self.active_path[index + 1]
dx = x1 - x0
dy = y1 - y0
seg_len_sq = dx * dx + dy * dy
if seg_len_sq <= 1e-10:
continue
t = ((x - x0) * dx + (y - y0) * dy) / seg_len_sq
t = max(0.0, min(1.0, t))
px = x0 + t * dx
py = y0 + t * dy
err_x = x - px
err_y = y - py
dist_sq = err_x * err_x + err_y * err_y
if dist_sq >= best_dist_sq:
continue
seg_len = math.sqrt(seg_len_sq)
ux = dx / seg_len
uy = dy / seg_len
signed_error = ux * err_y - uy * err_x
heading = math.atan2(dy, dx)
yaw = yaw0 if t < 0.5 else yaw1
best = {
"index": index,
"t": t,
"x": px,
"y": py,
"yaw": yaw,
"path_heading": heading,
"signed_error": signed_error,
"distance": math.sqrt(dist_sq),
}
best_dist_sq = dist_sq
if best is not None:
return best
index = min(max(0, self.current_path_index), len(self.active_path) - 1)
px, py, pyaw = self.active_path[index]
return {
"index": index,
"t": 0.0,
"x": px,
"y": py,
"yaw": pyaw,
"path_heading": pyaw,
"signed_error": 0.0,
"distance": math.hypot(x - px, y - py),
}
def _path_target_from_projection(self, projection, lookahead_distance):
remaining = max(0.05, float(lookahead_distance))
index = min(projection["index"], len(self.active_path) - 2)
t = max(0.0, min(1.0, projection.get("t", 0.0)))
while index < len(self.active_path) - 1:
x0, y0, yaw0 = self.active_path[index]
x1, y1, yaw1 = self.active_path[index + 1]
dx = x1 - x0
dy = y1 - y0
seg_len = math.hypot(dx, dy)
if seg_len <= 1e-9:
index += 1
t = 0.0
continue
available = (1.0 - t) * seg_len
if remaining <= available:
ratio = t + remaining / seg_len
x = x0 + ratio * dx
y = y0 + ratio * dy
heading = math.atan2(dy, dx)
yaw = yaw0 if ratio < 0.5 else yaw1
return {
"index": index + 1,
"x": x,
"y": y,
"yaw": yaw,
"path_heading": heading,
}
remaining -= available
index += 1
t = 0.0
x, y, yaw = self.active_path[-1]
heading = self._segment_heading(len(self.active_path) - 1)
return {
"index": len(self.active_path) - 1,
"x": x,
"y": y,
"yaw": yaw,
"path_heading": heading,
}
def _lookahead_index(self, x, y, start_index, lookahead_distance=None):
threshold = (
self.lookahead_distance
if lookahead_distance is None
else max(0.05, float(lookahead_distance))
)
target_index = len(self.active_path) - 1
for index in range(start_index, len(self.active_path)):
px, py, _ = self.active_path[index]
if math.hypot(px - x, py - y) >= self.lookahead_distance:
if math.hypot(px - x, py - y) >= threshold:
target_index = index
break
return target_index
def _adaptive_lookahead(self, path_curvature, cross_track_error, goal_distance):
scale = (
1.0
+ self.lookahead_curvature_gain * abs(path_curvature)
+ self.lookahead_error_gain * max(0.0, cross_track_error)
)
lookahead = self.lookahead_distance / max(1.0, scale)
if goal_distance < self.lookahead_distance:
lookahead = min(lookahead, max(self.min_lookahead_distance, goal_distance))
return max(self.min_lookahead_distance, min(self.lookahead_distance, lookahead))
def _path_curvature(self, index):
if len(self.active_path) < 3:
return 0.0
i0 = max(0, index - 1)
i1 = max(0, min(index, len(self.active_path) - 1))
i2 = min(len(self.active_path) - 1, index + 1)
if i0 == i1:
i2 = min(len(self.active_path) - 1, i1 + 2)
if i1 == i2:
i0 = max(0, i1 - 2)
if i0 == i1 or i1 == i2:
return 0.0
x0, y0, _ = self.active_path[i0]
x1, y1, _ = self.active_path[i1]
x2, y2, _ = self.active_path[i2]
a = math.hypot(x1 - x0, y1 - y0)
b = math.hypot(x2 - x1, y2 - y1)
c = math.hypot(x2 - x0, y2 - y0)
denominator = a * b * c
if denominator <= 1e-6:
return 0.0
cross = (x1 - x0) * (y2 - y0) - (y1 - y0) * (x2 - x0)
return 2.0 * cross / denominator
def _segment_heading(self, index, current_x=None, current_y=None):
if len(self.active_path) < 2:
return 0.0
@@ -716,9 +956,13 @@ class TopologyPurePursuit(Node):
nearest_index=None,
target_index=None,
cross_track_error=None,
cross_track_signed=None,
reverse=False,
curvature=None,
lookahead=None,
lookahead_threshold=None,
path_curvature=None,
heading_error=None,
):
if self.state_log_period <= 0.0:
return
@@ -731,10 +975,34 @@ class TopologyPurePursuit(Node):
parts = [f"tracker_state mode={mode}"]
if pose is None:
parts.append("pose=unavailable")
if self.latest_odom is None:
parts.append("odom=none")
elif self.latest_odom_received_ns > 0:
receive_age = max(
0.0,
(now_ns - self.latest_odom_received_ns) * 1e-9,
)
parts.append(f"odom_receive_age={receive_age:.3f}")
else:
x, y, yaw, vx, wz = pose
parts.append(f"pose=({x:.3f},{y:.3f},{yaw:.3f})")
parts.append(f"odom_twist=({vx:.3f},{wz:.3f})")
if self.latest_odom_received_ns > 0:
receive_age = max(
0.0,
(now_ns - self.latest_odom_received_ns) * 1e-9,
)
parts.append(f"odom_receive_age={receive_age:.3f}")
if self.latest_odom is not None:
stamp = Time.from_msg(self.latest_odom.header.stamp)
if (
self.latest_odom.header.stamp.sec != 0
or self.latest_odom.header.stamp.nanosec != 0
):
stamp_age = max(
0.0, (self.get_clock().now() - stamp).nanoseconds * 1e-9
)
parts.append(f"odom_stamp_age={stamp_age:.3f}")
parts.append(f"path_points={len(self.active_path)}")
parts.append(f"completed={self.completed}")
@@ -748,21 +1016,41 @@ class TopologyPurePursuit(Node):
parts.append(f"target_index={target_index}")
if cross_track_error is not None:
parts.append(f"cross_track={cross_track_error:.3f}")
if cross_track_signed is not None:
parts.append(f"cross_track_signed={cross_track_signed:.3f}")
if curvature is not None:
parts.append(f"curvature={curvature:.3f}")
if lookahead is not None:
parts.append(f"lookahead={lookahead:.3f}")
if lookahead_threshold is not None:
parts.append(f"lookahead_threshold={lookahead_threshold:.3f}")
if path_curvature is not None:
parts.append(f"path_curvature={path_curvature:.3f}")
if heading_error is not None:
parts.append(f"heading_error={heading_error:.3f}")
parts.append(f"reverse={reverse}")
parts.append(f"cmd=({self.last_cmd_linear:.3f},{self.last_cmd_angular:.3f})")
self.get_logger().info(" ".join(parts))
def _target_speed(self, abs_curvature, goal_distance, reverse):
def _target_speed(self, abs_curvature, goal_distance, reverse, cross_track_error=0.0):
base_speed = self.reverse_speed if reverse else self.linear_speed
speed = base_speed
speed *= max(0.35, 1.0 - self.curvature_slowdown_gain * abs_curvature)
speed *= max(
self.min_curvature_speed_ratio,
1.0 - self.curvature_slowdown_gain * abs_curvature,
)
if goal_distance < self.slowdown_distance:
ratio = max(0.25, goal_distance / max(0.05, self.slowdown_distance))
ratio = max(
self.min_goal_slowdown_ratio,
goal_distance / max(0.05, self.slowdown_distance),
)
speed *= ratio
if cross_track_error > 0.0 and self.cross_track_speed_gain > 0.0:
tracking_ratio = max(
self.min_tracking_speed_ratio,
1.0 - self.cross_track_speed_gain * cross_track_error,
)
speed *= tracking_ratio
speed = max(self.min_linear_speed, min(base_speed, speed))
return -speed if reverse else speed

18
src/vlm_detect/config/vlm_detect.yaml
View File

@@ -1,36 +1,18 @@
# vlm_detect 参数配置
# 使用: ros2 launch vlm_detect vlm_detect.launch.py
vlm_node:
ros__parameters:
# VLM 推理服务地址
vlm_host: "http://192.168.10.189:8000"
# 模型名称 (OpenAI 格式)
vlm_model: "./OpenGVLab/InternVL3-1B/"
# 订阅的压缩图像话题
image_topic: "/image_mjpeg"
# 订阅的触发信号话题
trigger_topic: "/sign4return"
# 触发信号值
trigger_sign: 9
# 发布结果的话题
result_topic: "/vlm_result"
# 发送给 VLM 的提示词
prompt_text: "描述图片中有一个病人的特征，字数控制在20字以内。"
# 最大输出 token 数
max_tokens: 100
tts_node:
ros__parameters:
# VLM 推理服务地址 (需与 vlm_node 一致)
vlm_host: "http://192.168.10.189:8000"
# 音频输出设备 (PulseAudio sink)
audio_sink: "alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo"
# 订阅 VLM 结果的话题 (需与 vlm_node 一致)
result_topic: "/vlm_result"
# TTS 语音 (edge-tts 语音名)
tts_voice: "zh-CN-XiaoxiaoNeural"
# 临时 MP3 存储路径
tmp_mp3_path: "/tmp/tts_out.mp3"
# 播放速度 (ffplay atempo, 范围 0.5~2.0)
tts_speed: 1.5

80
src/vlm_detect/launch/vlm_detect.launch.py
View File

@@ -1,13 +1,15 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
vlm_detect 联合启动文件
同时启动 vlm_node (图生文) 和 tts_node (语音播报)
vlm_detect 启动文件
同时启动 vlm_node (图生文) + tts_server (语音播报服务) + qr_tts_bridge (二维码播报)
用法:
ros2 launch vlm_detect vlm_detect.launch.py # 默认配置
ros2 launch vlm_detect vlm_detect.launch.py vlm_host:=http://... # 覆盖 VLM 服务地址
ros2 launch vlm_detect vlm_detect.launch.py use_tts:=false # 只启动 vlm_node
ros2 launch vlm_detect vlm_detect.launch.py # 默认全部
ros2 launch vlm_detect vlm_detect.launch.py vlm_host:=http://... # 指定 VLM 服务地址
ros2 launch vlm_detect vlm_detect.launch.py use_tts:=false # 关闭语音播报
ros2 launch vlm_detect vlm_detect.launch.py use_qr_tts:=false # 关闭二维码播报桥接
ros2 launch vlm_detect vlm_detect.launch.py use_vlm:=false # 只启动语音服务
"""
import os
@@ -23,7 +25,9 @@ from launch_ros.actions import Node
def generate_launch_description():
# ==================== Launch 参数 ====================
use_vlm = LaunchConfiguration('use_vlm')
use_tts = LaunchConfiguration('use_tts')
use_qr_tts = LaunchConfiguration('use_qr_tts')
config_file = LaunchConfiguration('config_file')
@@ -37,15 +41,22 @@ def generate_launch_description():
prompt_text = LaunchConfiguration('prompt_text')
max_tokens = LaunchConfiguration('max_tokens')
# tts_node 可覆盖参数
# tts_server 可覆盖参数
audio_sink = LaunchConfiguration('audio_sink')
tts_voice = LaunchConfiguration('tts_voice')
tts_speed = LaunchConfiguration('tts_speed')
# ==================== 声明参数 ====================
# ==================== 参数声明 ====================
declare_use_vlm = DeclareLaunchArgument(
'use_vlm', default_value='true',
description='启动 VLM 图生文节点')
declare_use_tts = DeclareLaunchArgument(
'use_tts', default_value='true',
description='是否同时启动 TTS 语音播报节点')
description='启动 TTS 语音播报服务')
declare_use_qr_tts = DeclareLaunchArgument(
'use_qr_tts', default_value='true',
description='启动二维码 → TTS 桥接节点')
declare_config_file = DeclareLaunchArgument(
'config_file',
@@ -57,47 +68,46 @@ def generate_launch_description():
# vlm_node 参数
declare_vlm_host = DeclareLaunchArgument(
'vlm_host', default_value='http://192.168.10.189:8000',
description='VLM 推理服务地址')
description='VLM 服务器地址')
declare_vlm_model = DeclareLaunchArgument(
'vlm_model', default_value='./OpenGVLab/InternVL3-1B/',
description='VLM 模型名称')
declare_image_topic = DeclareLaunchArgument(
'image_topic', default_value='/image_mjpeg',
description='订阅的压缩图像话题')
description='输入的压缩图像话题')
declare_trigger_topic = DeclareLaunchArgument(
'trigger_topic', default_value='/sign4return',
description='订阅的触发信号话题')
description='输入的触发信号话题')
declare_trigger_sign = DeclareLaunchArgument(
'trigger_sign', default_value='9',
description='触发信号值 (Int32)')
declare_result_topic = DeclareLaunchArgument(
'result_topic', default_value='/vlm_result',
description='发布 VLM 结果的话题')
description='输出 VLM 结果的话题')
declare_prompt_text = DeclareLaunchArgument(
'prompt_text', default_value='描述图片中有一个病人的特征，字数控制在20字以内。',
'prompt_text', default_value='请描述这张图片的内容，用一句简短的话概括，不超过20个字。',
description='发送给 VLM 的提示词')
declare_max_tokens = DeclareLaunchArgument(
'max_tokens', default_value='100',
description='最大输出 token 数')
description='最大生成 token 数')
# tts_node 参数
# tts_server 参数
declare_audio_sink = DeclareLaunchArgument(
'audio_sink',
default_value='alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo',
description='音频输出设备 (PulseAudio sink)')
declare_tts_voice = DeclareLaunchArgument(
'tts_voice', default_value='zh-CN-XiaoxiaoNeural',
description='TTS 语音名称 (edge-tts)')
declare_tts_speed = DeclareLaunchArgument(
'tts_speed', default_value='1.5',
description='播放速度倍率 (0.5~2.0)')
description='语速倍率 (0.5~2.0)')
# ==================== 节点 ====================
# VLM 图生文节点 (内部自带 TTS 服务客户端)
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,
@@ -111,26 +121,35 @@ def generate_launch_description():
}],
)
tts_node = Node(
# TTS 语音播报服务端
tts_server = Node(
package='vlm_detect',
executable='tts_node',
name='tts_node',
executable='tts_server',
name='tts_server',
output='screen',
condition=IfCondition(use_tts),
parameters=[config_file,
{
'vlm_host': vlm_host,
'audio_sink': audio_sink,
'result_topic': result_topic,
'tts_voice': tts_voice,
'tts_speed': tts_speed,
}],
)
# ==================== 启动描述 ====================
# 二维码 → TTS 桥接 (订阅 qr_results，调用 /tts/speak)
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,
@@ -141,12 +160,13 @@ def generate_launch_description():
declare_prompt_text,
declare_max_tokens,
declare_audio_sink,
declare_tts_voice,
declare_tts_speed,
# 节点
LogInfo(msg=['配置文件: ', config_file]),
LogInfo(msg=['VLM 服务: ', vlm_host]),
LogInfo(msg=['TTS 播报: ', use_tts]),
LogInfo(msg=['TTS 服务: ', use_tts]),
LogInfo(msg=['QR-TTS 桥接: ', use_qr_tts]),
vlm_node,
tts_node,
tts_server,
qr_tts_bridge,
])

2
src/vlm_detect/setup.py
View File

@@ -27,6 +27,8 @@ setup(
'vlm_node = vlm_detect.vlm_node:main',
'test_publisher = vlm_detect.test_publisher:main',
'tts_node = vlm_detect.tts_node:main',
'tts_server = vlm_detect.tts_server:main',
'qr_tts_bridge = vlm_detect.qr_tts_bridge:main',
],
},
)

BIN
src/vlm_detect/vlm_detect/__pycache__/test_publisher.cpython-310.pyc
View File

Binary file not shown.

BIN
src/vlm_detect/vlm_detect/__pycache__/tts_node.cpython-310.pyc
View File

Binary file not shown.

BIN
src/vlm_detect/vlm_detect/__pycache__/vlm_node.cpython-310.pyc
View File

Binary file not shown.

66
src/vlm_detect/vlm_detect/qr_tts_bridge.py Normal file
View File

@@ -0,0 +1,66 @@
#!/usr/bin/env python3
"""
QR 识别 → TTS 语音播报桥接节点
订阅 qr_results，调用 /tts/speak 服务
"""
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
from origincar_msg.srv import Speak
class QrTtsBridge(Node):
def __init__(self):
super().__init__('qr_tts_bridge')
# TTS 服务客户端
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('TTS service connected')
# 订阅 QR 识别结果
self.sub = self.create_subscription(
String, 'qr_results', self.callback, 10)
self.get_logger().info('QR-TTS Bridge ready, listening on qr_results')
def callback(self, msg):
text = msg.data.strip()
if not text:
return
self.get_logger().info(f'QR result: {text}')
if not self.tts_client.service_is_ready():
self.get_logger().warning('TTS service not available')
return
req = Speak.Request()
req.text = text
future = self.tts_client.call_async(req)
future.add_done_callback(self._tts_done_callback)
def _tts_done_callback(self, future):
try:
resp = future.result()
if not resp.success:
self.get_logger().warning(f'TTS failed: {resp.message}')
except Exception as e:
self.get_logger().error(f'TTS call error: {e}')
def main(args=None):
rclpy.init(args=args)
node = QrTtsBridge()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

2
src/vlm_detect/vlm_detect/test_publisher.py
View File

@@ -2,7 +2,7 @@
# -*- coding: utf-8 -*-
"""
测试发布者：发送图片和触发信号给 VLM 节点
用法: ros2 run vlm_detect test_publisher --ros-args -p image_path:="/path/to/image.jpg"
用法: ros2 run vlm_detect test_publisher --ros-args -p image_path:="/home/sunrise/yiliao_ws/my_model/image.png"
"""
import rclpy

70
src/vlm_detect/vlm_detect/tts_node.py
View File

@@ -1,60 +1,50 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import rclpy, subprocess, requests, os
import rclpy, subprocess, os, wave
from rclpy.node import Node
from std_msgs.msg import String
from piper import PiperVoice
from piper.config import SynthesisConfig
MODEL_PATH = '/home/sunrise/tts_model/zh_CN-huayan-medium.onnx'
class TTSNode(Node):
def __init__(self):
super().__init__("tts_node")
self.declare_parameter('vlm_host', 'http://192.168.10.189:8000')
self.declare_parameter('audio_sink',
'alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo')
super().__init__('tts_node')
self.declare_parameter('audio_sink', 'alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo')
self.declare_parameter('result_topic', '/vlm_result')
self.declare_parameter('tts_voice', 'zh-CN-XiaoxiaoNeural')
self.declare_parameter('tmp_mp3_path', '/tmp/tts_out.mp3')
self.declare_parameter('tts_speed', 1.5)
self.vlm_host = self.get_parameter('vlm_host').value
audio_sink = self.get_parameter('audio_sink').value
self.audio_sink = self.get_parameter('audio_sink').value
result_topic = self.get_parameter('result_topic').value
self.tts_voice = self.get_parameter('tts_voice').value
self.tmp_mp3 = self.get_parameter('tmp_mp3_path').value
self.tts_speed = self.get_parameter('tts_speed').value
self.audio_env = {**os.environ, "PULSE_SINK": audio_sink}
tts_speed = self.get_parameter('tts_speed').value
self.length_scale = 1.0 / tts_speed
self.espeak_speed = int(175 * tts_speed) # espeak default=175wpm, scale with tts_speed
self.sub = self.create_subscription(String, result_topic, self.callback, 10)
self.get_logger().info(
f"TTS 节点启动 | host={self.vlm_host} | sink={audio_sink} | "
f"voice={self.tts_voice} | speed={self.tts_speed}x"
)
self.get_logger().info(f'Piper TTS started | voice=zh_CN-huayan | speed={tts_speed}x | sink={self.audio_sink}')
self.get_logger().info(f'Loading model: {MODEL_PATH}')
self.voice = PiperVoice.load(MODEL_PATH)
self.get_logger().info('Model loaded OK')
def callback(self, msg):
text = msg.data
self.get_logger().info(f"语音播报: {text}")
text = msg.data.strip()
if not text:
return
self.get_logger().info(f'TTS: {text}')
try:
resp = requests.post(
f"{self.vlm_host}/v1/tts",
json={"text": text, "voice": self.tts_voice},
timeout=60
)
resp.raise_for_status()
with open(self.tmp_mp3, "wb") as f:
f.write(resp.content)
speed_str = f"atempo={self.tts_speed}"
syn_config = SynthesisConfig(length_scale=self.length_scale)
wav_path = '/tmp/tts_out.wav'
with wave.open(wav_path, 'wb') as wf:
self.voice.synthesize_wav(text, wf, syn_config=syn_config)
subprocess.Popen(
["ffplay", "-nodisp", "-autoexit", "-af", speed_str, self.tmp_mp3],
stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL,
env=self.audio_env
)
['paplay', f'--device={self.audio_sink}', wav_path],
stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
except Exception as e:
self.get_logger().error(f"TTS 失败, 降级 espeak: {e}")
self.get_logger().error(f'Piper TTS failed, fallback espeak: {e}')
subprocess.Popen(
["espeak-ng", "-v", "zh", "-s", "150", text],
env=self.audio_env
)
['espeak-ng', '-v', 'cmn', '-s', str(self.espeak_speed), text],
env={**os.environ, 'PULSE_SINK': self.audio_sink},
stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
def main(args=None):
rclpy.init(args=args)
@@ -67,5 +57,5 @@ def main(args=None):
node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
if __name__ == '__main__':
main()

90
src/vlm_detect/vlm_detect/tts_server.py Normal file
View File

@@ -0,0 +1,90 @@
#!/usr/bin/env python3
"""
TTS 语音播报服务端 (ROS2 Service Server)
服务类型: origincar_msg/srv/Speak
主 TTS: Piper 离线模型 降级: espeak-ng
"""
import rclpy
import subprocess
import os
import wave
from rclpy.node import Node
from origincar_msg.srv import Speak
from piper import PiperVoice
from piper.config import SynthesisConfig
MODEL_PATH = '/home/sunrise/tts_model/zh_CN-huayan-medium.onnx'
class TTSServer(Node):
def __init__(self):
super().__init__('tts_server')
self.declare_parameter('audio_sink',
'alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo')
self.declare_parameter('tts_speed', 1.5)
self.audio_sink = self.get_parameter('audio_sink').value
tts_speed = self.get_parameter('tts_speed').value
self.length_scale = 1.0 / tts_speed
self.espeak_speed = int(175 * tts_speed)
self.srv = self.create_service(Speak, '/tts/speak', self.handle_speak)
self.get_logger().info(f'TTS Server ready | voice=zh_CN-huayan | speed={tts_speed}x | sink={self.audio_sink}')
self.get_logger().info(f'Loading model: {MODEL_PATH}')
self.voice = PiperVoice.load(MODEL_PATH)
self.get_logger().info('Model loaded OK')
def handle_speak(self, request, response):
text = request.text.strip()
if not text:
response.success = False
response.message = 'empty text'
return response
self.get_logger().info(f'TTS: {text}')
try:
syn_config = SynthesisConfig(length_scale=self.length_scale)
wav_path = '/tmp/tts_out.wav'
with wave.open(wav_path, 'wb') as wf:
self.voice.synthesize_wav(text, wf, syn_config=syn_config)
subprocess.Popen(
['paplay', f'--device={self.audio_sink}', wav_path],
stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
response.success = True
response.message = 'ok'
except Exception as e:
self.get_logger().error(f'Piper TTS failed, fallback espeak: {e}')
try:
subprocess.Popen(
['espeak-ng', '-v', 'cmn', '-s', str(self.espeak_speed), text],
env={**os.environ, 'PULSE_SINK': self.audio_sink},
stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
response.success = True
response.message = 'ok (espeak fallback)'
except Exception as e2:
self.get_logger().error(f'espeak also failed: {e2}')
response.success = False
response.message = str(e2)
return response
def main(args=None):
rclpy.init(args=args)
node = TTSServer()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

46
src/vlm_detect/vlm_detect/vlm_node.py
View File

@@ -1,5 +1,8 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
VLM 图生文节点 —— 收到触发信号后拍图发给 VLM 服务，结果调用 TTS 服务播报
"""
import rclpy
from rclpy.node import Node
from std_msgs.msg import Int32, String
@@ -13,6 +16,9 @@ import os
import time
import numpy as np
from origincar_msg.srv import Speak
class VLMProcessor(Node):
def __init__(self):
super().__init__('vlm_detect')
@@ -24,7 +30,7 @@ class VLMProcessor(Node):
self.declare_parameter('trigger_topic', '/sign4return')
self.declare_parameter('trigger_sign', 9)
self.declare_parameter('result_topic', '/vlm_result')
self.declare_parameter('prompt_text', '描述图片中有一个病人的特征，字数控制在20字以内。')
self.declare_parameter('prompt_text', '请描述这张图片的内容，用一句简短的话概括，不超过20个字。')
self.declare_parameter('max_tokens', 100)
vlm_host = self.get_parameter('vlm_host').value
@@ -43,7 +49,7 @@ class VLMProcessor(Node):
)
self.vlm_model = vlm_model
# ROS2 组件
# ROS2 通信
self.bridge = CvBridge()
self.latest_image = None
self.image_lock = threading.Lock()
@@ -56,8 +62,13 @@ class VLMProcessor(Node):
)
self.result_pub = self.create_publisher(String, result_topic, 10)
# TTS 服务客户端
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 Processor 启动 | host={vlm_host} | model={vlm_model} | "
f"VLM Processor 就绪 | host={vlm_host} | model={vlm_model} | "
f"image={image_topic} | trigger={trigger_topic}(sign={self.trigger_sign})"
)
@@ -73,7 +84,7 @@ class VLMProcessor(Node):
def sign_callback(self, msg):
if msg.data == self.trigger_sign:
self.get_logger().info(f"收到触发信号 ({msg.data}), 开始处理...")
self.get_logger().info(f"收到触发信号 ({msg.data}), 开始推理...")
with self.image_lock:
if self.latest_image is None:
self.get_logger().warning("无可用图片")
@@ -85,12 +96,34 @@ class VLMProcessor(Node):
try:
description = self.process_image(temp_path)
self.get_logger().info(f"图像描述: {description}")
# 发布结果到话题
result_msg = String()
result_msg.data = description
self.result_pub.publish(result_msg)
# 调用 TTS 服务播报
if self.tts_client.service_is_ready():
req = Speak.Request()
req.text = description
future = self.tts_client.call_async(req)
future.add_done_callback(self._tts_done_callback)
else:
self.get_logger().warning('TTS service not available')
os.remove(temp_path)
except Exception as e:
self.get_logger().error(f"图像处理出错: {e}")
self.get_logger().error(f"图像推理失败: {e}")
def _tts_done_callback(self, future):
try:
resp = future.result()
if resp.success:
self.get_logger().debug(f'TTS OK: {resp.message}')
else:
self.get_logger().warning(f'TTS failed: {resp.message}')
except Exception as e:
self.get_logger().error(f'TTS call error: {e}')
def process_image(self, image_path):
with open(image_path, "rb") as image_file:
@@ -109,10 +142,12 @@ class VLMProcessor(Node):
]
}],
max_tokens=self.max_tokens,
timeout=30,
)
self.get_logger().info(f"VLM 推理耗时 {time.time() - start_time:.1f}s")
return response.choices[0].message.content
def main(args=None):
rclpy.init(args=args)
node = VLMProcessor()
@@ -124,5 +159,6 @@ def main(args=None):
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()