cyy/yq_2026_duo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

bf

Browse Source
This commit is contained in:
cyy_mac
2026-03-25 12:37:08 +08:00
commit 5e1e355ffa
462 changed files with 92376 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
src/rm_bringup/CMakeLists.txt Normal file
View File

@@ -0,0 +1,16 @@
cmake_minimum_required(VERSION 3.8)
project(rm_bringup)
find_package(ament_cmake_auto REQUIRED)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
set(ament_cmake_copyright_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_auto_package(
INSTALL_TO_SHARE
launch
config
)

20
src/rm_bringup/config/camera_info.yaml Normal file
View File

@@ -0,0 +1,20 @@
image_width: 1440
image_height: 1080
camera_name: hik
camera_matrix:
rows: 3
cols: 3
data: [1762.08104, 0.000000, 713.40561, 0.000000, 1770.76313, 555.02238, 0.000000, 0.000000, 1.000000]
distortion_model: plumb_bob
distortion_coefficients:
rows: 1
cols: 5
data: [-0.062731, 0.093291, -0.003401, 0.001370, 0.000000]
rectification_matrix:
rows: 3
cols: 3
data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000]
projection_matrix:
rows: 3
cols: 4
data: [1747.58521, 0.000000, 714.12243, 0.000000, 0.000000, 1760.43799, 552.92865, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]

48
src/rm_bringup/config/launch_params.yaml Normal file
View File

@@ -0,0 +1,48 @@
odom2camera:
xyz: "\"0.1536 -0.061325 0.0097\""
rpy: "\"0.0 0.0 0.0\"" #p越大越低，，，y越大偏左
#pitch_diff为负，偏上，yaw_diff为负，偏左
# 0.0148,,,0.003
# 枪管相对于pitch_link的偏移 [x=向前, y=向左, z=向上] 单位:米
barrel:
xyz: "\"0.18025 0.0 0.0\""
rpy: "\"0.0 0.0 0.0\""
#176
# 云台轴间距偏移 [x, y, z] (单位: 米)
# pitch轴相对于yaw轴的偏移
yaw_to_pitch_offset: [0.0, 0.0, 0.0]
# yaw->pitch 半径 (单位: 米)，用于角度驱动的 x/z 偏移
yaw_to_pitch_radius: 0.128
# 是否使用电控下发的 yaw->pitch 夹角
use_yaw_to_pitch_angle: true
# use_yaw_to_pitch_angle 为 true 时接收 20 字节；为 false 时接收 16 字节
# 是否启用枪管坐标系进行更精确的弹道解算
use_barrel_frame: true
# 设置启动参数
# 命名空间 所有节点、话题都会以这个命名空间开头，如 some_namespace/image_raw
namespace: ''
# true: 读取视频 false: 读取相机
video_play: false
# 相机类型: hik(海康) - 仅当video_play为false时生效
camera_type: hik
# true: 虚拟下位机 false: 实车
virtual_serial: false
# true: 英雄解算 false: 传统解算
hero_solver: false
# 装甲板识别方法切换（仅改这里）
# detect: 传统cv装甲板
# yolo_detect: YOLO装甲板
armor_detector_method: yolo_detect
# 兼容保留项（由 bringup.launch.py 读取）。
# 当前重新拉取版 armor_yolo_detect 节点本身不使用该参数，保留 false 即可。
yolo_require_tf: false
# true: 开启打符 false: 关闭打符
rune: false
# true: 启动导航tf false: 不启动导航tf
navigation: false

36
src/rm_bringup/config/node_params/armor_detector_params.yaml Normal file
View File

@@ -0,0 +1,36 @@
/**:
ros__parameters:
debug: true
debug_log_interval_frames: 60
debug.enable_terminal_log: false
debug.enable_markers: false
debug.enable_lights_msg: false
debug.enable_armors_msg: false
debug.enable_binary_img: true
debug.enable_number_img: false
debug.enable_result_img: true
tf_error_log_interval_ms: 1000
max_queue_size: 1
process_every_n_frames: 2
target_frame: odom
detect_color: 1
binary_thres: 170
use_pca: true # 使用PCA算法矫正灯条的角点
use_ba: false # 使用BA优化算法求解装甲板的Yaw角
light.use_fit_line: true # 使用最小二乘直线拟合(cv::fitLine)矫正灯条上下端点
light.min_ratio: 0.0001
light.max_ratio: 1.0
light.max_angle: 40.0
light.color_diff_thresh: 20
armor.min_light_ratio: 0.8
armor.min_small_center_distance: 0.8
armor.max_small_center_distance: 3.5
armor.min_large_center_distance: 3.5
armor.max_large_center_distance: 8.0
armor.max_angle: 35.0
classifier_threshold: 0.7
ignore_classes: ["negative"]

58
src/rm_bringup/config/node_params/armor_solver_params.yaml Normal file
View File

@@ -0,0 +1,58 @@
/**:
ros__parameters:
debug: true
bullet_speed_debug: true #测试弹速
target_frame: odom
max_armor_distance: 10.0
ekf:
# Adaptive Q matrix parameters
# Q noise varies based on velocity: high angular vel -> lower position noise, high linear vel -> lower yaw noise
# Formula: s2q = exp(-vel) * (max - min) + min
sigma2_q_xyz_max: 0.50 # Position noise upper bound (when target moves slowly)
sigma2_q_xyz_min: 0.01 # Position noise lower bound (when target spins fast)
sigma2_q_yaw_max: 0.50 # Yaw noise upper bound (when target moves slowly)
sigma2_q_yaw_min: 0.01 # Yaw noise lower bound (when target moves fast)
sigma2_q_r: 4.0 # Radius noise (fixed)
sigma2_q_d_zc: 8.0 # Height offset noise (fixed)
# R matrix parameters (measurement noise, scaled by distance)
r_x: 2e-3
r_y: 2e-3
r_z: 50e-3
r_yaw: 2e-3
tracker:
max_match_distance: 0.3
max_match_yaw_diff: 0.3
tracking_thres: 1
lost_time_thres: 1.0
solver:
shoot_rate: 3
bullet_speed: 20.5
shooting_range_width: 0.10 #射击范围
shooting_range_height: 0.10 #射击范围
prediction_delay: 0.02 # 预测装甲板位置的延时，单位秒，+飞行时间
controller_delay: 0.01
max_tracking_v_yaw: 60.1 #转速(rad/s)大于这个值时瞄准机器人中心
side_angle: 15.0
compenstator_type: "resistance"
gravity: 9.792
resistance: 0.038
iteration_times: 20 # 补偿的迭代次数
# ["距离下限, 距离上限, 高度下限, 高度下限, pitch轴补偿值"]
# [dist_low, dist_high, height_low, height_high, pitch_offset_deg, yaw_offset_deg]
angle_offset: [
"2.5 3.5 -1.0 0.4 0.0 0.0",
"5.0 6.0 0.4 0.8 0.0 0.0",
"5.0 6.0 0.8 1.2 0.0 0.0",
"6.0 7.0 -1.0 0.4 0.0 0.0",
"6.0 7.0 0.4 0.8 0.0 0.0",
"6.0 7.0 0.8 1.2 0.0 0.0",
"7.0 8.0 -1.0 0.4 0.0 0.0",
"7.0 8.0 0.4 0.8 0.0 0.0",
"7.0 8.0 0.8 1.2 0.0 0.0",
]

44
src/rm_bringup/config/node_params/armor_yolo_detect_params.yaml Normal file
View File

@@ -0,0 +1,44 @@
/**:
ros__parameters:
# Detection mode: 0=HYBRID_SYNC every picture, 1=HYBRID_ROI_CACHE
detector_mode: 1
roi_update_interval: 3
# YOLO params
conf_threshold: 0.75
nms_threshold: 0.50
detect_color: 1
# ROI expansion (pixels)
roi_expand_pixel: 80
# Binary threshold for light detection
binary_thres: 100
# Light detection params
light.use_fit_line: true
light.min_ratio: 0.08
light.max_ratio: 0.4
light.max_angle: 40.0
light.color_diff_thresh: 25
# Armor matching params
armor.min_light_ratio: 0.6
armor.min_small_center_distance: 0.8
armor.max_small_center_distance: 3.2
armor.min_large_center_distance: 3.2
armor.max_large_center_distance: 5.0
armor.max_angle: 35.0
# Queue and processing
max_queue_size: 1
process_every_n_frames: 2
use_ba: false
target_frame: odom
# Debug
debug: true
debug_log_interval_frames: 60
debug.enable_terminal_log: true
debug.enable_markers: false
debug.enable_result_img: true

11
src/rm_bringup/config/node_params/camera_driver_params.yaml Normal file
View File

@@ -0,0 +1,11 @@
/**:
ros__parameters:
camera_info_url: package://rm_bringup/config/camera_info.yaml
exposure_time: 1000
gain: 18.3
resolution_width: 1440
resolution_height: 1080
offsetX: 0
offsetY: 0
camera_name: "hik"
recording: false #是否录制

10
src/rm_bringup/config/node_params/rune_detector_params.yaml Normal file
View File

@@ -0,0 +1,10 @@
/**:
ros__parameters:
debug: true
requests_limit: 5
detect_r_tag: true # 是否使用传统方法识别R标
min_lightness: 100 # 二值化亮度阈值 (R标识别)
detector:
model: "package://rune_detector/model/yolox_rune_3.6m.onnx" # GPU模式下请用xml文件
device_type: "CPU"

12
src/rm_bringup/config/node_params/rune_solver_params.yaml Normal file
View File

@@ -0,0 +1,12 @@
/**:
ros__parameters:
debug: true
auto_type_determined: true
predict_time: 0.0
compensator_type: resistance
gravity: 9.8
bullet_speet: 28.0
lost_time_thres: 0.5
ekf:
q: [4e-4, 1e-4, 1e-4, 1e-4]
r: [4e-2, 1e-2, 1e-2, 1e-2]

8
src/rm_bringup/config/node_params/serial_driver_params.yaml Normal file
View File

@@ -0,0 +1,8 @@
/**:
ros__parameters:
target_frame: odom
timestamp_offset: 0.001
port_name: "/dev/ttyACM0"
protocol: "air"
enable_data_print: false # Print parsed receive data when true
enable_send_print: false # Print parsed send data when true

8
src/rm_bringup/config/node_params/video_player_params.yaml Normal file
View File

@@ -0,0 +1,8 @@
/**:
ros__parameters:
camera_info_url: package://rm_bringup/config/camera_info.yaml
path: "/home/zcf/csurm/csurm-RMUC2022/red.avi"
frame_rate: 10
start_frame: 0
frame_id: "camera_optical_frame"
keep_looping: true #是否循环播放

7
src/rm_bringup/config/node_params/virtual_serial_params.yaml Normal file
View File

@@ -0,0 +1,7 @@
/**:
ros__parameters:
roll: 0.0
pitch: 0.0
yaw: 180.0
vision_mode: 1
bullet_speed: 25.0

BIN
src/rm_bringup/launch/__pycache__/bringup.launch.cpython-313.pyc Normal file
View File

Binary file not shown.

287
src/rm_bringup/launch/bringup.launch.py Normal file
View File

@@ -0,0 +1,287 @@
import os
import sys
import yaml
from ament_index_python.packages import get_package_share_directory
from launch.substitutions import Command
from launch_ros.parameter_descriptions import ParameterValue
sys.path.append(os.path.join(get_package_share_directory("rm_bringup"), "launch"))
def generate_launch_description():
from launch_ros.descriptions import ComposableNode
from launch_ros.actions import (
ComposableNodeContainer,
Node,
SetParameter,
PushRosNamespace,
)
from launch.actions import TimerAction, Shutdown
from launch import LaunchDescription
launch_params = yaml.safe_load(
open(
os.path.join(
get_package_share_directory("rm_bringup"),
"config",
"launch_params.yaml",
)
)
)
SetParameter(name="rune", value=launch_params["rune"]),
robot_gimbal_description = ParameterValue(
Command(
[
"xacro ",
os.path.join(
get_package_share_directory("rm_robot_description"),
"urdf",
"rm_gimbal.urdf.xacro",
),
" xyz:=",
launch_params["odom2camera"]["xyz"],
" rpy:=",
launch_params["odom2camera"]["rpy"],
" barrel_xyz:=",
launch_params["barrel"]["xyz"],
" barrel_rpy:=",
launch_params["barrel"]["rpy"],
]
),
value_type=str,
)
robot_navigation_description = ParameterValue(
Command(
[
"xacro ",
os.path.join(
get_package_share_directory("rm_robot_description"),
"urdf",
"sentry.urdf.xacro",
),
]
),
value_type=str,
)
robot_gimbal_publisher = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
parameters=[
{"robot_description": robot_gimbal_description, "publish_frequency": 1000.0}
],
)
robot_navigation_publisher = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
parameters=[
{
"robot_description": robot_navigation_description,
}
],
# 'publish_frequency': 1000.0}]
)
def get_params(name):
return os.path.join(
get_package_share_directory("rm_bringup"),
"config",
"node_params",
"{}_params.yaml".format(name),
)
# 图像
if launch_params["video_play"]:
image_node = ComposableNode(
package="hik_camera",
plugin="fyt::camera_driver::VideoPlayerNode",
name="video_player",
parameters=[get_params("video_player")],
extra_arguments=[{"use_intra_process_comms": True}],
)
else:
# 仅使用海康相机节点（替换原大恒相机节点）
image_node = ComposableNode(
package="hik_camera",
plugin="fyt::camera_driver::HikCameraNode",
name="hik_camera",
parameters=[get_params("camera_driver")],
extra_arguments=[{"use_intra_process_comms": True}],
)
# 串口
if launch_params["virtual_serial"]:
serial_driver_node = Node(
package="rm_serial_driver",
executable="virtual_serial_node",
name="virtual_serial",
output="both",
emulate_tty=True,
parameters=[
get_params("virtual_serial"),
{"yaw_to_pitch_offset": launch_params["yaw_to_pitch_offset"]},
{"yaw_to_pitch_radius": launch_params["yaw_to_pitch_radius"]},
{"use_yaw_to_pitch_angle": launch_params["use_yaw_to_pitch_angle"]},
],
ros_arguments=[
"--ros-args",
"-p",
"has_rune:=true" if launch_params["rune"] else "has_rune:=false",
],
)
else:
serial_driver_node = Node(
package="rm_serial_driver",
executable="rm_serial_driver_node",
name="serial_driver",
output="both",
emulate_tty=True,
parameters=[
get_params("serial_driver"),
{"yaw_to_pitch_offset": launch_params["yaw_to_pitch_offset"]},
{"yaw_to_pitch_radius": launch_params["yaw_to_pitch_radius"]},
{"use_yaw_to_pitch_angle": launch_params["use_yaw_to_pitch_angle"]},
],
ros_arguments=[
"--ros-args",
],
)
# 装甲板识别（支持传统检测 / YOLO 检测切换）
armor_detector_method = launch_params.get("armor_detector_method", "detect")
yolo_require_tf = launch_params.get("yolo_require_tf", False)
if armor_detector_method == "yolo_detect":
armor_detector_node = ComposableNode(
package="armor_yolo_detect",
plugin="armor_yolo_detect::ArmorYoloDetectorNode",
name="armor_detector",
parameters=[get_params("armor_yolo_detect"), {"require_tf": yolo_require_tf}],
extra_arguments=[{"use_intra_process_comms": True}],
)
else:
armor_detector_node = ComposableNode(
package="armor_detector",
plugin="fyt::auto_aim::ArmorDetectorNode",
name="armor_detector",
parameters=[get_params("armor_detector")],
extra_arguments=[{"use_intra_process_comms": True}],
)
# 装甲板解算
if launch_params["hero_solver"]:
armor_solver_node = Node(
package="hero_armor_solver",
executable="hero_armor_solver_node",
name="armor_solver",
output="both",
emulate_tty=True,
parameters=[
get_params("armor_solver"),
{"solver.use_barrel_frame": launch_params["use_barrel_frame"]},
],
ros_arguments=[],
)
else:
armor_solver_node = Node(
package="armor_solver",
executable="armor_solver_node",
name="armor_solver",
output="both",
emulate_tty=True,
parameters=[
get_params("armor_solver"),
{"solver.use_barrel_frame": launch_params["use_barrel_frame"]},
],
ros_arguments=[],
)
# 打符
rune_detector_node = ComposableNode(
package="rune_detector",
plugin="fyt::rune::RuneDetectorNode",
name="rune_detector",
parameters=[get_params("rune_detector")],
extra_arguments=[{"use_intra_process_comms": True}],
)
rune_solver_node = Node(
package="rune_solver",
executable="rune_solver_node",
name="rune_solver",
output="both",
emulate_tty=True,
parameters=[get_params("rune_solver")],
arguments=[
"--ros-args",
],
)
# 使用intra cmmunication提高图像的传输速度
def get_camera_detector_container(*detector_nodes):
nodes_list = list(detector_nodes)
nodes_list.append(image_node)
container = ComposableNodeContainer(
name="camera_detector_container",
namespace="",
package="rclcpp_components",
executable="component_container_mt",
composable_node_descriptions=nodes_list,
output="both",
emulate_tty=True,
ros_arguments=[
"--ros-args",
],
)
return TimerAction(
period=2.0,
actions=[container],
)
# 延迟启动
delay_serial_node = TimerAction(
period=1.5,
actions=[serial_driver_node],
)
delay_armor_solver_node = TimerAction(
period=2.0,
actions=[armor_solver_node],
)
delay_rune_solver_node = TimerAction(
period=2.0,
actions=[rune_solver_node],
)
if launch_params["rune"]:
cam_detector_node = get_camera_detector_container(
armor_detector_node, rune_detector_node
)
else:
cam_detector_node = get_camera_detector_container(armor_detector_node)
delay_cam_detector_node = TimerAction(
period=2.0,
actions=[cam_detector_node],
)
push_namespace = PushRosNamespace(launch_params["namespace"])
launch_description_list = [
robot_gimbal_publisher,
push_namespace,
delay_serial_node,
delay_cam_detector_node,
delay_armor_solver_node,
]
if launch_params["rune"]:
launch_description_list.append(delay_rune_solver_node)
if launch_params["navigation"]:
launch_description_list.append(robot_navigation_publisher)
return LaunchDescription(launch_description_list)

84
src/rm_bringup/launch/bringup_navigation.launch.py Normal file
View File

@@ -0,0 +1,84 @@
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, TimerAction
from launch.launch_description_sources import PythonLaunchDescriptionSource
def generate_launch_description():
livox_bringup_dir = get_package_share_directory('livox_ros_driver2')
segmentation_bringup_dir = get_package_share_directory('linefit_ground_segmentation_ros')
fast_lio_bringup_dir = get_package_share_directory('fast_lio')
laserscan_conversion_bringup_dir = get_package_share_directory('pointcloud_to_laserscan')
icp_bringup_dir = get_package_share_directory('icp_registration')
navigation_bringup_dir = get_package_share_directory('rm_navigation')
rm_decision_bringup_dir = get_package_share_directory('rm_decision')
livox_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(livox_bringup_dir, 'launch', 'msg_MID360_launch.py'))
)
segmentation_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(segmentation_bringup_dir, 'launch', 'segmentation.launch.py'))
)
fast_lio_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(fast_lio_bringup_dir, 'launch', 'mapping.launch.py'))
)
laserscan_conversion_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(laserscan_conversion_bringup_dir, 'launch', 'pointcloud_to_laserscan_launch.py'))
)
icp_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(icp_bringup_dir, 'launch', 'icp.launch.py'))
)
bringup_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(navigation_bringup_dir, 'launch', 'bringup_launch.py'))
)
decision_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(rm_decision_bringup_dir, 'launch', 'rm_decision_launch.py')))
delay_segmentation_launch = TimerAction(
period=3.5,
actions=[segmentation_launch]
)
delay_fast_lio_launch = TimerAction(
period=3.0,
actions=[fast_lio_launch]
)
delay_laserscan_conversion_launch = TimerAction(
period=4.0,
actions=[laserscan_conversion_launch]
)
delay_icp_launch = TimerAction(
period=4.5,
actions=[icp_launch]
)
delay_bringup_launch = TimerAction(
period=5.5,
actions=[bringup_launch]
)
delay_decision_launch = TimerAction(
period=7.0,
actions=[decision_launch]
)
return LaunchDescription([
livox_launch,
delay_segmentation_launch,
delay_fast_lio_launch,
delay_laserscan_conversion_launch,
delay_icp_launch,
delay_bringup_launch,
delay_decision_launch
])

20
src/rm_bringup/package.xml Normal file
View File

@@ -0,0 +1,20 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>rm_bringup</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="chen.junn@outlook.com">chenjun</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rm_auto_aim</depend>
<depend>armor_yolo_detect</depend>
<depend>rm_serial_driver</depend>
<depend>rm_rune</depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>