JYC/RC2025_r1
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

R1

Browse Source
This commit is contained in:
Robofish 2025-07-10 13:21:45 +08:00
parent 06b1e7522b
commit dcef535b33
53 changed files with 1159 additions and 289 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
.vscode/c_cpp_properties.json vendored Normal file
View File

@ -0,0 +1,44 @@
{
"configurations": [
{
"browse": {
"databaseFilename": "${default}",
"limitSymbolsToIncludedHeaders": false
},
"includePath": [
"/home/robofish/RC2025/install/teb_local_planner/include/**",
"/home/robofish/RC2025/install/teb_msgs/include/**",
"/home/robofish/RC2025/install/ros2_livox_simulation/include/**",
"/home/robofish/RC2025/install/rm_msgs/include/**",
"/home/robofish/RC2025/install/pointcloud_to_laserscan/include/**",
"/home/robofish/RC2025/install/icp_registration/include/**",
"/home/robofish/RC2025/install/fast_lio/include/**",
"/home/robofish/RC2025/install/livox_ros_driver2/include/**",
"/home/robofish/RC2025/install/linefit_ground_segmentation/include/**",
"/home/robofish/RC2025/install/imu_complementary_filter/include/**",
"/home/robofish/RC2025/install/fake_vel_transform/include/**",
"/home/robofish/RC2025/install/costmap_converter_msgs/include/**",
"/opt/ros/humble/include/**",
"/home/robofish/RC2025/src/rm_driver/rm_serial_driver/include/**",
"/home/robofish/RC2025/src/rm_localization/FAST_LIO/include/**",
"/home/robofish/RC2025/src/rm_localization/icp_registration/include/**",
"/home/robofish/RC2025/src/rm_localization/point_lio/include/**",
"/home/robofish/RC2025/src/rm_navigation/costmap_converter/costmap_converter/include/**",
"/home/robofish/RC2025/src/rm_navigation/fake_vel_transform/include/**",
"/home/robofish/RC2025/src/rm_navigation/teb_local_planner/teb_local_planner/include/**",
"/home/robofish/RC2025/src/rm_perception/imu_complementary_filter/include/**",
"/home/robofish/RC2025/src/rm_perception/linefit_ground_segementation_ros2/linefit_ground_segmentation/include/**",
"/home/robofish/RC2025/src/rm_perception/pointcloud_to_laserscan/include/**",
"/home/robofish/RC2025/src/rm_simpal_move/include/**",
"/home/robofish/RC2025/src/rm_simulation/livox_laser_simulation_RO2/include/**",
"/usr/include/**"
],
"name": "ROS",
"intelliSenseMode": "gcc-x64",
"compilerPath": "/usr/bin/gcc",
"cStandard": "gnu11",
"cppStandard": "c++14"
}
],
"version": 4
}

21
.vscode/settings.json vendored Normal file
View File

@ -0,0 +1,21 @@
{
"cmake.sourceDirectory": "/home/robofish/RC2025/src/rm_msg",
"python.autoComplete.extraPaths": [
"/home/robofish/RC2025/install/teb_msgs/local/lib/python3.10/dist-packages",
"/home/robofish/RC2025/install/rm_msgs/local/lib/python3.10/dist-packages",
"/home/robofish/RC2025/install/fast_lio/local/lib/python3.10/dist-packages",
"/home/robofish/RC2025/install/livox_ros_driver2/local/lib/python3.10/dist-packages",
"/home/robofish/RC2025/install/costmap_converter_msgs/local/lib/python3.10/dist-packages",
"/opt/ros/humble/lib/python3.10/site-packages",
"/opt/ros/humble/local/lib/python3.10/dist-packages"
],
"python.analysis.extraPaths": [
"/home/robofish/RC2025/install/teb_msgs/local/lib/python3.10/dist-packages",
"/home/robofish/RC2025/install/rm_msgs/local/lib/python3.10/dist-packages",
"/home/robofish/RC2025/install/fast_lio/local/lib/python3.10/dist-packages",
"/home/robofish/RC2025/install/livox_ros_driver2/local/lib/python3.10/dist-packages",
"/home/robofish/RC2025/install/costmap_converter_msgs/local/lib/python3.10/dist-packages",
"/opt/ros/humble/lib/python3.10/site-packages",
"/opt/ros/humble/local/lib/python3.10/dist-packages"
]
}

221
README.md
View File

@ -1,221 +0,0 @@
# 🎯 RC2025 自动定位瞄准代码
> 基于ROS2的机器人自动定位与瞄准系统，支持激光雷达建图和导航功能
[![ROS2](https://img.shields.io/badge/ROS2-Humble-blue.svg)](https://docs.ros.org/en/humble/)
[![Ubuntu](https://img.shields.io/badge/Ubuntu-22.04-orange.svg)](https://ubuntu.com/)
[![License](https://img.shields.io/badge/License-MIT-green.svg)](LICENSE)
## 📋 目录
- [系统要求](#系统要求)
- [环境配置](#环境配置)
- [快速开始](#快速开始)
- [重要参数配置](#重要参数配置)
- [故障排除](#故障排除)
- [贡献指南](#贡献指南)
## 🔧 系统要求
| 组件 | 版本/型号 |
|------|-----------|
| 操作系统 | Ubuntu 22.04 LTS |
| ROS版本 | ROS2 Humble |
| 激光雷达 | Livox MID360 |
| 处理器 | x86_64 (推荐) |
| 内存 | 8GB+ (推荐) |
## 🚀 环境配置
### 1. 安装 Livox SDK2
```bash
# 安装依赖
sudo apt update
sudo apt install cmake build-essential
# 克隆并编译 Livox SDK2
git clone https://github.com/Livox-SDK/Livox-SDK2.git
cd ./Livox-SDK2/
mkdir build && cd build
cmake .. && make -j$(nproc)
sudo make install
```
### 2. 安装串口驱动
```bash
pip install pyserial
```
### 3. 安装ROS2依赖
```bash
# 进入工作区
cd /Users/lvzucheng/Documents/R/RC2025
# 安装依赖包
rosdep install -r --from-paths src --ignore-src --rosdistro $ROS_DISTRO -y
```
## 🎯 快速开始
### 1. 编译项目
```bash
. build.sh
```
### 2. 🗺️ 建图模式
用于创建环境地图和点云数据：
```bash
. mapping.sh
```
#### 建图前配置
1. **修改地图保存文件名**
```bash
# 编辑 mapping.sh
nano mapping.sh
# 将 'RC2025' 改为您的项目名
```
2. **同步修改点云文件配置**
```bash
# 编辑 FAST-LIO 配置文件
nano src/rm_nav_bringup/config/reality/fastlio_mid360_real.yaml
# 确保 pcd 文件名与 mapping.sh 中一致
```
#### 建图操作步骤
1. **启动建图程序**
```bash
./mapping.sh
```
2. **控制机器人移动**
- 使用遥控器或键盘控制机器人
- 确保覆盖所有需要建图的区域
3. **保存点云文件**
```bash
ros2 service call /map_save std_srvs/srv/Trigger
```
4. **保存地图文件**
- 在RViz中使用地图保存功能
- 确保地图名称保持一致
#### 建图效果展示
<details>
<summary>点击查看建图效果图</summary>
![建图效果1](doc/img/07b3c725_11812035.png)
![建图效果2](doc/img/5032aa1d_11812035.png)
![建图效果3](doc/img/bea7dae2_11812035.jpeg)
</details>
### 3. 🧭 导航模式
使用已建立的地图进行导航：
```bash
chmod +x nav.sh
./nav.sh
```
#### 导航操作说明
1. **启动导航程序**
2. **在RViz中设置初始位置**
3. **设置目标点进行导航**
4. **监控导航状态**
## ⚙️ 重要参数配置
### 📍 激光雷达安装位置
#### 位置参数配置
```yaml
# 文件：src/rm_nav_bringup/config/reality/measurement_params_real.yaml
translation:
x: 0.0 # 前后位置 (m)
y: 0.0 # 左右位置 (m)
z: 0.0 # 上下位置 (m)
```
> ⚠️ **注意**：不要修改 `rpy` 参数
#### 姿态参数配置
```json
// 文件：src/rm_nav_bringup/config/reality/MID360_config.json
{
"yaw": 0.0, // 偏航角
"pitch": 0.0, // 俯仰角
"roll": 0.0 // 翻滚角
}
```
> ⚠️ **注意**：不要修改 `xyz` 参数
### 🌍 地面点云分割
```yaml
# 文件：src/rm_nav_bringup/config/reality/segmentation_real.yaml
sensor_height: 0.3 # 激光雷达距离地面的高度 (m)
max_dist_to_line: 0.05 # 地面点云分割的最低高度 (m)
```
### 🎯 目标点设定
```bash
# 文件：nav.sh
# 篮筐目标点坐标
TARGET_X=1.0 # X坐标
TARGET_Y=0.0 # Y坐标
```
## 🔧 故障排除
### 常见问题
<details>
<summary>激光雷达无法连接</summary>
1. 检查网络连接
2. 确认IP地址配置
3. 检查防火墙设置
4. 验证SDK安装
</details>
<details>
<summary>建图效果不佳</summary>
1. 检查激光雷达安装位置
2. 调整地面分割参数
3. 确保移动速度适中
4. 检查环境光照条件
</details>
<details>
<summary>导航精度不够</summary>
1. 重新标定雷达参数
2. 优化地图质量
3. 调整导航参数
4. 检查里程计数据
</details>
## 📝 使用技巧
- **建图时**：保持稳定的移动速度，避免急转急停
- **导航时**：确保地图与实际环境一致
- **调试时**：使用RViz可视化工具监控状态
- **维护时**：定期更新地图数据

4
mapping.sh Normal file → Executable file
View File

@ -2,11 +2,11 @@ source install/setup.bash
commands=(
"ros2 launch rm_nav_bringup bringup_real.launch.py \
world:=RC2026 \
world:=test \
mode:=mapping \
lio:=fastlio \
localization:=icp \
lio_rviz:=false \
lio_rviz:=true \
nav_rviz:=true"
)

8
nav.sh Normal file → Executable file
View File

@ -1,7 +1,7 @@
source install/setup.bash
commands=(
"/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/pub_aim.py"
# "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/pub_aim.py"
"ros2 launch rm_nav_bringup bringup_real.launch.py \
world:=RC2026 \
mode:=nav \
@ -10,7 +10,11 @@ commands=(
lio_rviz:=false \
nav_rviz:=true"
"ros2 launch rm_simpal_move simple_move.launch.py"
"ros2 topic pub /move_goal rm_msgs/msg/MoveGoal '{x: 0.60, y: 3.995, angle: 0.0, max_speed: 10.0, tolerance: 0.1, rotor: false}' --once"
"ros2 topic pub /move_goal rm_msgs/msg/MoveGoal '{x: 0.596, y: 4.01, angle: 0.0, max_speed: 10.0, tolerance: 0.1, rotor: false}' --once"
#"ros2 topic pub /move_goal rm_msgs/msg/MoveGoal '{x: 13.58, y: -3.35, angle: 0.0, max_speed: 10.0, tolerance: 0.1, rotor: false}' --once"
"ros2 topic pub /move_goal rm_msgs/msg/MoveGoal '{x: 0.54, y: -3.30, angle: 0.0, max_speed: 10.0, tolerance: 0.1, rotor: false}' --once"
"/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/siyuanshu.py"
"/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/pub_self.py"
)
for cmd in "${commands[@]}"; do

BIN
src.zip Executable file
View File

Binary file not shown.

7
src/rm_driver/livox_ros_driver2/.gitignore vendored
View File

@ -1,10 +1,7 @@
devel/
@~!@
build/
install/
log/
.vscode/
__pycache__/
.catkin_workspace
*.gv
*.pdf

2
src/rm_driver/livox_ros_driver2/src/config/MID360_config.json
View File

@ -25,7 +25,7 @@
},
"lidar_configs" : [
{
"ip" : "192.168.1.137",
"ip" : "192.168.1.176",
"pcl_data_type" : 1,
"pattern_mode" : 0,
"extrinsic_parameter" : {

13
src/rm_driver/rm_serial_driver/script/pub_OnlyAim.py
View File

@ -10,7 +10,7 @@ class AimDataSerial(Node):
super().__init__('aim_data_serial')
# 串口配置
self.serial_port = '/dev/ttyUSB0' # 根据实际串口设备调整
self.serial_port = '/dev/upper' # 根据实际串口设备调整
self.baud_rate = 115200
try:
@ -38,17 +38,20 @@ class AimDataSerial(Node):
try:
# 提取yaw和distance数据
# 根据实际消息结构调整，这里假设使用Point32的x和y字段
yaw = msg.yaw
# yaw = msg.yaw
distance_r2 = msg.yaw
distance = msg.distance
z = 0.0
self.get_logger().info(f'Received - yaw: {yaw}, distance: {distance}')
self.get_logger().info(f'Received - distance: {distance}, distance_r2: {distance_r2}')
# 构建发送数据包
# 格式: 包头(0xFF) + yaw(4字节float) + distance(4字节float) + 包尾(0xFE)
packet = bytearray()
packet.append(0xFF) # 包头
packet.extend(struct.pack('<f', yaw)) # yaw (小端序float)
packet.extend(struct.pack('<f', distance)) # distance (小端序float)
packet.extend(struct.pack('<f', distance)) # yaw (小端序float)
packet.extend(struct.pack('<f', distance_r2))
packet.extend(struct.pack('<f', z)) # distance (小端序float)
packet.append(0xFE) # 包尾
# 发送数据

9
src/rm_driver/rm_serial_driver/script/pub_aim.py
View File

@ -10,7 +10,7 @@ class AimDataSerial(Node):
super().__init__('aim_data_serial')
# 串口配置
self.serial_port = '/dev/ttyUSB0' # 根据实际串口设备调整
self.serial_port = '/dev/underpan' # 根据实际串口设备调整
self.baud_rate = 115200
try:
@ -39,16 +39,17 @@ class AimDataSerial(Node):
# 提取yaw和distance数据
# 根据实际消息结构调整，这里假设使用Point32的x和y字段
yaw = msg.yaw
distance = msg.distance
yaw_r2 =msg.distance
# distance = msg.distance
self.get_logger().info(f'Received - yaw: {yaw}, distance: {distance}')
self.get_logger().info(f'Received - yaw: {yaw},yaw_r2: {yaw_r2}')
# 构建发送数据包
# 格式: 包头(0xFF) + yaw(4字节float) + distance(4字节float) + 包尾(0xFE)
packet = bytearray()
packet.append(0xFF) # 包头
packet.extend(struct.pack('<f', yaw)) # yaw (小端序float)
packet.extend(struct.pack('<f', distance)) # distance (小端序float)
packet.extend(struct.pack('<f', yaw_r2)) # distance (小端序float)
packet.append(0xFE) # 包尾
# 发送数据

8
src/rm_driver/rm_serial_driver/script/pub_self.py
View File

@ -15,7 +15,7 @@ class SelfPositionSerial(Node):
super().__init__('self_position_serial')
# 串口配置
self.serial_port = '/dev/ttyUSB1' # 根据实际串口设备调整
self.serial_port = '/dev/ttyUSB2' # 根据实际串口设备调整
self.baud_rate = 115200
try:
@ -65,11 +65,7 @@ class SelfPositionSerial(Node):
packet.extend(struct.pack('<f', z)) # z坐标
packet.extend(struct.pack('<f', yaw)) # yaw角
# 计算校验和（数据部分的异或校验）
checksum = 0
for i in range(1, len(packet)):
checksum ^= packet[i]
packet.append(checksum)
packet.append(0xFE) # 包尾

323
src/rm_driver/rm_serial_driver/script/r1_serial.py Normal file
View File

@ -0,0 +1,323 @@
import rclpy
from rclpy.node import Node
import serial
import struct
import tf2_ros
from geometry_msgs.msg import TransformStamped
from sensor_msgs.msg import Imu
from rm_msgs.msg import DataAim
import math
import threading
from rclpy.executors import MultiThreadedExecutor
class ReceiveAndPubNode(Node):
def __init__(self):
super().__init__('receive_and_pub')
# 串口配置
self.receive_port = '/dev/r2' # 接收串口
self.send_port = '/dev/underpan'
self.send_port_upper = '/dev/upper' # 发送串口
self.baud_rate = 115200
self.imu_angular_z = 0.0
# 角度平滑滤波器参数
self.smooth_alpha = 0.7 # 平滑系数，0-1之间，越小越平滑
self.filtered_original_yaw = 0.0
self.filtered_r2_yaw = 0.0
self.filter_initialized = False
self.imu_subscription = self.create_subscription(
Imu,
'/livox/imu',
self.imu_callback,
10
)
try:
# 接收串口
self.receive_serial = serial.Serial(
port=self.receive_port,
baudrate=self.baud_rate,
timeout=0.1
)
# 发送串口
self.send_serial = serial.Serial(
port=self.send_port,
baudrate=self.baud_rate,
timeout=1
)
self.send_serial_upper = serial.Serial(
port=self.send_port_upper,
baudrate=self.baud_rate,
timeout=1
)
self.get_logger().info(f'Serial ports opened: {self.receive_port}, {self.send_port},{self.send_port_upper}')
except Exception as e:
self.get_logger().error(f'Failed to open serial ports: {e}')
return
# TF广播器和监听器
self.tf_broadcaster = tf2_ros.TransformBroadcaster(self)
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
# 订阅瞄准数据
self.subscription = self.create_subscription(
DataAim,
'/chassis/data_aim',
self.aim_callback,
10
)
# 存储接收到的位置信息和瞄准数据
self.received_position = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'yaw': 0.0, 'valid': False}
self.aim_data = {'yaw': 0.0, 'distance': 0.0, 'valid': False}
# R1->base_link坐标监听，结果存储在self.transform_position
self.transform_position = {'x': 0.0, 'y': 0.0}
self.r1_tf_timer = self.create_timer(0.05, self._update_r1_position)
# 启动接收线程
self.receive_thread = threading.Thread(target=self.receive_position_thread)
self.receive_thread.daemon = True
self.receive_thread.start()
# 定时发送数据
self.send_timer = self.create_timer(0.005, self.send_target_data) # 10Hz
self.get_logger().info('Receive and pub node started')
def angle_smooth_filter(self, new_angle, filtered_angle):
"""角度平滑滤波器，处理角度跳跃问题"""
if not self.filter_initialized:
return new_angle
# 处理角度跳跃问题（-π到π的边界）
diff = new_angle - filtered_angle
if diff > math.pi:
diff -= 2 * math.pi
elif diff < -math.pi:
diff += 2 * math.pi
# 指数移动平均滤波
return filtered_angle + self.smooth_alpha * diff
def imu_callback(self, msg):
"""接收IMU数据，提取angular.z"""
try:
# self.imu_angular_z = msg.angular_velocity.z
self.imu_angular_z = 0.0
except Exception as e:
self.get_logger().error(f'Error in imu callback: {e}')
def receive_position_thread(self):
buffer = bytearray()
while rclpy.ok():
try:
if self.receive_serial.in_waiting > 0:
data = self.receive_serial.read(self.receive_serial.in_waiting)
buffer.extend(data)
# 查找完整的数据包
while len(buffer) >= 18: # 包头(1) + 4个float(16) + 包尾(1) = 18字节
# 查找包头
start_idx = buffer.find(0xFF)
if start_idx == -1:
break
# 移除包头前的数据
if start_idx > 0:
buffer = buffer[start_idx:]
# 检查包长度
if len(buffer) < 18:
break
# 查找包尾
if buffer[17] == 0xFE:
# 解析数据
try:
x = struct.unpack('<f', buffer[1:5])[0]
y = struct.unpack('<f', buffer[5:9])[0]
z = struct.unpack('<f', buffer[9:13])[0]
yaw = struct.unpack('<f', buffer[13:17])[0]
self.received_position = {
'x': x, 'y': y, 'z': z, 'yaw': yaw, 'valid': True
}
# 发布tf变换
self.publish_r2_transform(x, y, z, yaw)
# self.get_logger().info(f' Received R2 position: x={x:.3f}, y={y:.3f}, z={z:.3f}, yaw={yaw:.3f}')
except struct.error as e:
self.get_logger().warn(f'⚠️ Data parsing error: {e}')
# 移除已处理的数据包
buffer = buffer[18:]
else:
# 包尾不匹配，移除包头继续查找
buffer = buffer[1:]
else:
# 避免忙等待
import time
time.sleep(0.001)
except Exception as e:
self.get_logger().error(f' Error in receive thread: {e}')
def publish_r2_transform(self, x, y, z, yaw):
"""发布R2到map的tf变换"""
try:
t = TransformStamped()
t.header.stamp = self.get_clock().now().to_msg()
t.header.frame_id = 'map'
t.child_frame_id = 'R2'
t.transform.translation.x = x
t.transform.translation.y = y
t.transform.translation.z = z
# 将yaw角转换为四元数
t.transform.rotation.x = 0.0
t.transform.rotation.y = 0.0
t.transform.rotation.z = math.sin(yaw / 2.0)
t.transform.rotation.w = math.cos(yaw / 2.0)
self.tf_broadcaster.sendTransform(t)
except Exception as e:
self.get_logger().error(f'Error publishing tf: {e}')
def aim_callback(self, msg):
"""接收瞄准数据"""
try:
self.aim_data = {
'yaw': msg.yaw,
'distance': msg.distance,
'valid': True
}
self.get_logger().debug(f'Received aim data: yaw={msg.yaw:.3f}, distance={msg.distance:.3f}')
except Exception as e:
self.get_logger().error(f'Error in aim callback: {e}')
def get_r2_target_data(self):
"""获取R2的瞄准数据"""
try:
if not self.received_position['valid']:
return 0.0, 0.0
# 这里可以根据R2的位置计算目标数据
# 简化处理：返回R2的yaw角和到原点的距离
r2_yaw = math.atan2(self.transform_position['y'], self.transform_position['x'])
if r2_yaw > 1.57:
r2_yaw = r2_yaw - 3.14
if r2_yaw < -1.57:
r2_yaw = r2_yaw + 3.14
r2_distance = math.sqrt(
(self.transform_position['x'])**2 +
self.transform_position['y']**2
)
return r2_yaw, r2_distance
except Exception as e:
self.get_logger().error(f'Error getting R2 target data: {e}')
return 0.0, 0.0
def send_target_data(self):
"""发送目标数据"""
try:
# 获取原始目标数据
if self.aim_data['valid']:
raw_original_yaw = self.aim_data['yaw']
original_distance = self.aim_data['distance']
else:
raw_original_yaw = 0.0
original_distance = 0.0
# 获取R2目标数据
raw_r2_yaw, r2_distance = self.get_r2_target_data()
# 初始化滤波器
if not self.filter_initialized:
self.filtered_original_yaw = raw_original_yaw
self.filtered_r2_yaw = raw_r2_yaw
self.filter_initialized = True
# 应用平滑滤波
self.filtered_original_yaw = self.angle_smooth_filter(raw_original_yaw, self.filtered_original_yaw)
self.filtered_r2_yaw = self.angle_smooth_filter(raw_r2_yaw, self.filtered_r2_yaw)
# 构建发送数据包
# 格式: 包头(0xFF) + original_yaw(4字节) + original_distance(4字节) + r2_yaw(4字节) + r2_distance(4字节) + 校验和(1字节) + 包尾(0xFE)
packet = bytearray()
packet.append(0xFF) # 包头
packet.extend(struct.pack('<f', self.filtered_original_yaw)) # 滤波后的原始yaw
# packet.extend(struct.pack('<f', original_distance)) # 原始distance
packet.extend(struct.pack('<f', self.filtered_r2_yaw)) # 滤波后的R2 yaw
# packet.extend(struct.pack('<f', r2_distance)) # R2 distance
# 添加IMU angular.z 到 underpan 的第三个数据
packet.extend(struct.pack('<f', self.imu_angular_z)) # 新增：IMU angular.z
packet.append(0xFE) # 包尾
packet_upper = bytearray()
packet_upper.append(0xFF) # 包头
# packet.extend(struct.pack('<f', original_yaw)) # 原始yaw
packet_upper.extend(struct.pack('<f', original_distance)) # 原始distance
# packet.extend(struct.pack('<f', r2_yaw)) # R2 yaw
packet_upper.extend(struct.pack('<f', r2_distance)) # R2 distance
# 计算校验和
checksum = 0
packet_upper.append(0xFE) # 包尾
# 发送数据
self.send_serial.write(packet)
self.send_serial_upper.write(packet_upper)
self.get_logger().info(f'Sent filtered angles - original({self.filtered_original_yaw:.3f}→{raw_original_yaw:.3f}) R2({self.filtered_r2_yaw:.3f}→{raw_r2_yaw:.3f}) dist({original_distance:.3f},{r2_distance:.3f}) IMU_z({self.imu_angular_z:.3f})')
except Exception as e:
self.get_logger().error(f'Error sending target data: {e}')
def _update_r1_position(self):
try:
trans = self.tf_buffer.lookup_transform(
'R2', # 目标坐标系（修正）
'base_link', # 源坐标系（修正）
rclpy.time.Time()
)
self.transform_position['x'] = trans.transform.translation.x
self.transform_position['y'] = trans.transform.translation.y
except Exception:
pass
def __del__(self):
if hasattr(self, 'receive_serial') and self.receive_serial.is_open:
self.receive_serial.close()
if hasattr(self, 'send_serial') and self.send_serial.is_open:
self.send_serial.close()
if hasattr(self, 'send_serial_upper') and self.send_serial_upper.is_open:
self.send_serial_upper.close()
self.get_logger().info('Serial ports closed')
def main(args=None):
rclpy.init(args=args)
try:
node = ReceiveAndPubNode()
executor = MultiThreadedExecutor()
executor.add_node(node)
executor.spin()
except KeyboardInterrupt:
pass
finally:
if rclpy.ok():
rclpy.shutdown()
if __name__ == '__main__':
main()

274
src/rm_driver/rm_serial_driver/script/receive_and_pub copy.py Normal file
View File

@ -0,0 +1,274 @@
import rclpy
from rclpy.node import Node
import serial
import struct
import tf2_ros
from geometry_msgs.msg import TransformStamped
from rm_msgs.msg import DataAim
import math
import threading
from rclpy.executors import MultiThreadedExecutor
class ReceiveAndPubNode(Node):
def __init__(self):
super().__init__('receive_and_pub')
# 串口配置
self.receive_port = '/dev/r2' # 接收串口
self.send_port = '/dev/underpan'
self.send_port_upper = '/dev/upper' # 发送串口
self.baud_rate = 115200
try:
# 接收串口
self.receive_serial = serial.Serial(
port=self.receive_port,
baudrate=self.baud_rate,
timeout=0.1
)
# 发送串口
self.send_serial = serial.Serial(
port=self.send_port,
baudrate=self.baud_rate,
timeout=1
)
self.send_serial_upper = serial.Serial(
port=self.send_port_upper,
baudrate=self.baud_rate,
timeout=1
)
self.get_logger().info(f'Serial ports opened: {self.receive_port}, {self.send_port},{self.send_port_upper}')
except Exception as e:
self.get_logger().error(f'Failed to open serial ports: {e}')
return
# TF广播器和监听器
self.tf_broadcaster = tf2_ros.TransformBroadcaster(self)
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
# 订阅瞄准数据
self.subscription = self.create_subscription(
DataAim,
'/chassis/data_aim',
self.aim_callback,
10
)
# 存储接收到的位置信息和瞄准数据
self.received_position = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'yaw': 0.0, 'valid': False}
self.aim_data = {'yaw': 0.0, 'distance': 0.0, 'valid': False}
# R1->base_link坐标监听，结果存储在self.transform_position
self.transform_position = {'x': 0.0, 'y': 0.0}
self.r1_tf_timer = self.create_timer(0.05, self._update_r1_position)
# 启动接收线程
self.receive_thread = threading.Thread(target=self.receive_position_thread)
self.receive_thread.daemon = True
self.receive_thread.start()
# 定时发送数据
self.send_timer = self.create_timer(0.005, self.send_target_data) # 10Hz
self.get_logger().info('Receive and pub node started')
def receive_position_thread(self):
buffer = bytearray()
while rclpy.ok():
try:
if self.receive_serial.in_waiting > 0:
data = self.receive_serial.read(self.receive_serial.in_waiting)
buffer.extend(data)
# 查找完整的数据包
while len(buffer) >= 18: # 包头(1) + 4个float(16) + 包尾(1) = 18字节
# 查找包头
start_idx = buffer.find(0xFF)
if start_idx == -1:
break
# 移除包头前的数据
if start_idx > 0:
buffer = buffer[start_idx:]
# 检查包长度
if len(buffer) < 18:
break
# 查找包尾
if buffer[17] == 0xFE:
# 解析数据
try:
x = struct.unpack('<f', buffer[1:5])[0]
y = struct.unpack('<f', buffer[5:9])[0]
z = struct.unpack('<f', buffer[9:13])[0]
yaw = struct.unpack('<f', buffer[13:17])[0]
self.received_position = {
'x': x, 'y': y, 'z': z, 'yaw': yaw, 'valid': True
}
# 发布tf变换
self.publish_r2_transform(x, y, z, yaw)
# self.get_logger().info(f' Received R2 position: x={x:.3f}, y={y:.3f}, z={z:.3f}, yaw={yaw:.3f}')
except struct.error as e:
self.get_logger().warn(f'⚠️ Data parsing error: {e}')
# 移除已处理的数据包
buffer = buffer[18:]
else:
# 包尾不匹配，移除包头继续查找
buffer = buffer[1:]
else:
# 避免忙等待
import time
time.sleep(0.001)
except Exception as e:
self.get_logger().error(f' Error in receive thread: {e}')
def publish_r2_transform(self, x, y, z, yaw):
"""发布R2到map的tf变换"""
try:
t = TransformStamped()
t.header.stamp = self.get_clock().now().to_msg()
t.header.frame_id = 'map'
t.child_frame_id = 'R2'
t.transform.translation.x = x
t.transform.translation.y = y
t.transform.translation.z = z
# 将yaw角转换为四元数
t.transform.rotation.x = 0.0
t.transform.rotation.y = 0.0
t.transform.rotation.z = math.sin(yaw / 2.0)
t.transform.rotation.w = math.cos(yaw / 2.0)
self.tf_broadcaster.sendTransform(t)
except Exception as e:
self.get_logger().error(f'Error publishing tf: {e}')
def aim_callback(self, msg):
"""接收瞄准数据"""
try:
self.aim_data = {
'yaw': msg.yaw,
'distance': msg.distance,
'valid': True
}
self.get_logger().debug(f'Received aim data: yaw={msg.yaw:.3f}, distance={msg.distance:.3f}')
except Exception as e:
self.get_logger().error(f'Error in aim callback: {e}')
def get_r2_target_data(self):
"""获取R2的瞄准数据"""
try:
if not self.received_position['valid']:
return 0.0, 0.0
# 这里可以根据R2的位置计算目标数据
# 简化处理：返回R2的yaw角和到原点的距离
r2_yaw = math.atan2(self.transform_position['y'], self.transform_position['x'])
if r2_yaw > 1.57:
r2_yaw = r2_yaw - 3.14
if r2_yaw < -1.57:
r2_yaw = r2_yaw + 3.14
r2_distance = math.sqrt(
(self.transform_position['x'])**2 +
self.transform_position['y']**2
)
return r2_yaw, r2_distance
except Exception as e:
self.get_logger().error(f'Error getting R2 target data: {e}')
return 0.0, 0.0
def send_target_data(self):
"""发送目标数据"""
try:
# 获取原始目标数据
if self.aim_data['valid']:
original_yaw = self.aim_data['yaw']
original_distance = self.aim_data['distance']
else:
original_yaw = 0.0
original_distance = 0.0
# 获取R2目标数据
r2_yaw, r2_distance = self.get_r2_target_data()
# 构建发送数据包
# 格式: 包头(0xFF) + original_yaw(4字节) + original_distance(4字节) + r2_yaw(4字节) + r2_distance(4字节) + 校验和(1字节) + 包尾(0xFE)
packet = bytearray()
packet.append(0xFF) # 包头
packet.extend(struct.pack('<f', original_yaw)) # 原始yaw
# packet.extend(struct.pack('<f', original_distance)) # 原始distance
packet.extend(struct.pack('<f', r2_yaw)) # R2 yaw
# packet.extend(struct.pack('<f', r2_distance)) # R2 distance
packet.append(0xFE) # 包尾
packet_upper = bytearray()
packet_upper.append(0xFF) # 包头
# packet.extend(struct.pack('<f', original_yaw)) # 原始yaw
packet_upper.extend(struct.pack('<f', original_distance)) # 原始distance
# packet.extend(struct.pack('<f', r2_yaw)) # R2 yaw
packet_upper.extend(struct.pack('<f', r2_distance)) # R2 distance
# 计算校验和
checksum = 0
packet_upper.append(0xFE) # 包尾
# 发送数据
self.send_serial.write(packet)
self.send_serial_upper.write(packet_upper)
self.get_logger().info(f'Sent target packet: original({original_yaw:.3f},{original_distance:.3f}) R2({r2_yaw:.3f},{r2_distance:.3f})')
except Exception as e:
self.get_logger().error(f'Error sending target data: {e}')
def _update_r1_position(self):
try:
trans = self.tf_buffer.lookup_transform(
'R2', # 目标坐标系（修正）
'base_link', # 源坐标系（修正）
rclpy.time.Time()
)
self.transform_position['x'] = trans.transform.translation.x
self.transform_position['y'] = trans.transform.translation.y
except Exception:
pass
def __del__(self):
if hasattr(self, 'receive_serial') and self.receive_serial.is_open:
self.receive_serial.close()
if hasattr(self, 'send_serial') and self.send_serial.is_open:
self.send_serial.close()
if hasattr(self, 'send_serial_upper') and self.send_serial_upper.is_open:
self.send_serial_upper.close()
self.get_logger().info('Serial ports closed')
def main(args=None):
rclpy.init(args=args)
try:
node = ReceiveAndPubNode()
executor = MultiThreadedExecutor()
executor.add_node(node)
executor.spin()
except KeyboardInterrupt:
pass
finally:
if rclpy.ok():
rclpy.shutdown()
if __name__ == '__main__':
main()

99
src/rm_driver/rm_serial_driver/script/receive_and_pub.py
View File

@ -1,5 +1,3 @@
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
import serial
@ -16,8 +14,9 @@ class ReceiveAndPubNode(Node):
super().__init__('receive_and_pub')
# 串口配置
self.receive_port = '/dev/ttyUSB2' # 接收串口
self.send_port = '/dev/ttyUSB3' # 发送串口
self.receive_port = '/dev/r2' # 接收串口
self.send_port = '/dev/underpan'
self.send_port_upper = '/dev/upper' # 发送串口
self.baud_rate = 115200
try:
@ -33,7 +32,12 @@ class ReceiveAndPubNode(Node):
baudrate=self.baud_rate,
timeout=1
)
self.get_logger().info(f'Serial ports opened: {self.receive_port}, {self.send_port}')
self.send_serial_upper = serial.Serial(
port=self.send_port_upper,
baudrate=self.baud_rate,
timeout=1
)
self.get_logger().info(f'Serial ports opened: {self.receive_port}, {self.send_port},{self.send_port_upper}')
except Exception as e:
self.get_logger().error(f'Failed to open serial ports: {e}')
return
@ -55,6 +59,10 @@ class ReceiveAndPubNode(Node):
self.received_position = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'yaw': 0.0, 'valid': False}
self.aim_data = {'yaw': 0.0, 'distance': 0.0, 'valid': False}
# R1->base_link坐标监听，结果存储在self.transform_position
self.transform_position = {'x': 0.0, 'y': 0.0}
self.r1_tf_timer = self.create_timer(0.05, self._update_r1_position)
# 启动接收线程
self.receive_thread = threading.Thread(target=self.receive_position_thread)
self.receive_thread.daemon = True
@ -66,7 +74,6 @@ class ReceiveAndPubNode(Node):
self.get_logger().info('Receive and pub node started')
def receive_position_thread(self):
"""接收位置信息的线程"""
buffer = bytearray()
while rclpy.ok():
try:
@ -75,7 +82,7 @@ class ReceiveAndPubNode(Node):
buffer.extend(data)
# 查找完整的数据包
while len(buffer) >= 22: # 最小包长度：包头(1) + 数据(16) + 校验(1) + 包尾(1) = 19
while len(buffer) >= 18: # 包头(1) + 4个float(16) + 包尾(1) = 18字节
# 查找包头
start_idx = buffer.find(0xFF)
if start_idx == -1:
@ -86,18 +93,13 @@ class ReceiveAndPubNode(Node):
buffer = buffer[start_idx:]
# 检查包长度
if len(buffer) < 22:
if len(buffer) < 18:
break
# 查找包尾
if buffer[21] == 0xFE:
# 校验数据
checksum = 0
for i in range(1, 20):
checksum ^= buffer[i]
if checksum == buffer[20]:
# 解析数据
if buffer[17] == 0xFE:
# 解析数据
try:
x = struct.unpack('<f', buffer[1:5])[0]
y = struct.unpack('<f', buffer[5:9])[0]
z = struct.unpack('<f', buffer[9:13])[0]
@ -110,19 +112,24 @@ class ReceiveAndPubNode(Node):
# 发布tf变换
self.publish_r2_transform(x, y, z, yaw)
self.get_logger().debug(f'Received position: x={x:.3f}, y={y:.3f}, z={z:.3f}, yaw={yaw:.3f}')
else:
self.get_logger().warn('Checksum error')
# self.get_logger().info(f' Received R2 position: x={x:.3f}, y={y:.3f}, z={z:.3f}, yaw={yaw:.3f}')
except struct.error as e:
self.get_logger().warn(f'⚠️ Data parsing error: {e}')
# 移除已处理的数据包
buffer = buffer[22:]
buffer = buffer[18:]
else:
# 包尾不匹配，移除包头继续查找
buffer = buffer[1:]
else:
# 避免忙等待
import time
time.sleep(0.001)
except Exception as e:
self.get_logger().error(f'Error in receive thread: {e}')
self.get_logger().error(f' Error in receive thread: {e}')
def publish_r2_transform(self, x, y, z, yaw):
"""发布R2到map的tf变换"""
try:
@ -166,10 +173,14 @@ class ReceiveAndPubNode(Node):
# 这里可以根据R2的位置计算目标数据
# 简化处理：返回R2的yaw角和到原点的距离
r2_yaw = self.received_position['yaw']
r2_yaw = math.atan2(self.transform_position['y'], self.transform_position['x'])
if r2_yaw > 1.57:
r2_yaw = r2_yaw - 3.14
if r2_yaw < -1.57:
r2_yaw = r2_yaw + 3.14
r2_distance = math.sqrt(
self.received_position['x']**2 +
self.received_position['y']**2
(self.transform_position['x'])**2 +
self.transform_position['y']**2
)
return r2_yaw, r2_distance
@ -197,30 +208,52 @@ class ReceiveAndPubNode(Node):
packet = bytearray()
packet.append(0xFF) # 包头
packet.extend(struct.pack('<f', original_yaw)) # 原始yaw
packet.extend(struct.pack('<f', original_distance)) # 原始distance
# packet.extend(struct.pack('<f', original_distance)) # 原始distance
packet.extend(struct.pack('<f', r2_yaw)) # R2 yaw
packet.extend(struct.pack('<f', r2_distance)) # R2 distance
# packet.extend(struct.pack('<f', r2_distance)) # R2 distance
packet.append(0xFE) # 包尾
packet_upper = bytearray()
packet_upper.append(0xFF) # 包头
# packet.extend(struct.pack('<f', original_yaw)) # 原始yaw
packet_upper.extend(struct.pack('<f', original_distance)) # 原始distance
# packet.extend(struct.pack('<f', r2_yaw)) # R2 yaw
packet_upper.extend(struct.pack('<f', r2_distance)) # R2 distance
# 计算校验和
checksum = 0
for i in range(1, len(packet)):
checksum ^= packet[i]
packet.append(checksum)
packet.append(0xFE) # 包尾
packet_upper.append(0xFE) # 包尾
# 发送数据
self.send_serial.write(packet)
self.get_logger().debug(f'Sent target packet: original({original_yaw:.3f},{original_distance:.3f}) R2({r2_yaw:.3f},{r2_distance:.3f})')
self.send_serial_upper.write(packet_upper)
self.get_logger().info(f'Sent target packet: original({original_yaw:.3f},{original_distance:.3f}) R2({r2_yaw:.3f},{r2_distance:.3f})')
except Exception as e:
self.get_logger().error(f'Error sending target data: {e}')
def _update_r1_position(self):
try:
trans = self.tf_buffer.lookup_transform(
'R2', # 目标坐标系（修正）
'base_link', # 源坐标系（修正）
rclpy.time.Time()
)
self.transform_position['x'] = trans.transform.translation.x
self.transform_position['y'] = trans.transform.translation.y
except Exception:
pass
def __del__(self):
if hasattr(self, 'receive_serial') and self.receive_serial.is_open:
self.receive_serial.close()
if hasattr(self, 'send_serial') and self.send_serial.is_open:
self.send_serial.close()
if hasattr(self, 'send_serial_upper') and self.send_serial_upper.is_open:
self.send_serial_upper.close()
self.get_logger().info('Serial ports closed')
def main(args=None):

294
src/rm_driver/rm_serial_driver/script/siyuanshu.py Normal file
View File

@ -0,0 +1,294 @@
import rclpy
from rclpy.node import Node
import serial
import struct
import tf2_ros
from geometry_msgs.msg import TransformStamped
from sensor_msgs.msg import Imu
from rm_msgs.msg import DataAim
import math
import threading
from rclpy.executors import MultiThreadedExecutor
class ReceiveAndPubNode(Node):
def __init__(self):
super().__init__('receive_and_pub')
# 串口配置
self.receive_port = '/dev/ttyUSB2' # 接收串口
self.send_port = '/dev/ttyUSB0'
self.send_port_upper = '/dev/ttyUSB1' # 发送串口
self.baud_rate = 115200
self.imu_angular_z = 0.0
self.imu_subscription = self.create_subscription(
Imu,
'/livox/imu',
self.imu_callback,
10
)
try:
# 接收串口
self.receive_serial = serial.Serial(
port=self.receive_port,
baudrate=self.baud_rate,
timeout=0.1
)
# 发送串口
self.send_serial = serial.Serial(
port=self.send_port,
baudrate=self.baud_rate,
timeout=1
)
self.send_serial_upper = serial.Serial(
port=self.send_port_upper,
baudrate=self.baud_rate,
timeout=1
)
self.get_logger().info(f'Serial ports opened: {self.receive_port}, {self.send_port},{self.send_port_upper}')
except Exception as e:
self.get_logger().error(f'Failed to open serial ports: {e}')
return
# TF广播器和监听器
self.tf_broadcaster = tf2_ros.TransformBroadcaster(self)
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
# 订阅瞄准数据
self.subscription = self.create_subscription(
DataAim,
'/chassis/data_aim',
self.aim_callback,
10
)
# 存储接收到的位置信息和瞄准数据
self.received_position = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'yaw': 0.0, 'valid': False}
self.aim_data = {'yaw': 0.0, 'distance': 0.0, 'valid': False}
# R1->base_link坐标监听，结果存储在self.transform_position
self.transform_position = {'x': 0.0, 'y': 0.0}
self.r1_tf_timer = self.create_timer(0.01, self._update_r1_position)
# 启动接收线程
self.receive_thread = threading.Thread(target=self.receive_position_thread)
self.receive_thread.daemon = True
self.receive_thread.start()
# 定时发送数据
self.send_timer = self.create_timer(0.1, self.send_target_data) # 10Hz
self.get_logger().info('Receive and pub node started')
def imu_callback(self, msg):
"""接收IMU数据，提取angular.z"""
try:
# self.imu_angular_z = msg.angular_velocity.z
# self.imu_angular_z = -self.imu_angular_z # 取反
# self.get_logger().debug(f'IMU angular.z: {self.imu_angular_z:.3f}')
self.imu_angular_z = 0.0
except Exception as e:
self.get_logger().error(f'Error in imu callback: {e}')
def receive_position_thread(self):
buffer = bytearray()
while rclpy.ok():
try:
if self.receive_serial.in_waiting > 0:
data = self.receive_serial.read(self.receive_serial.in_waiting)
buffer.extend(data)
# 查找完整的数据包
while len(buffer) >= 18: # 包头(1) + 4个float(16) + 包尾(1) = 18字节
# 查找包头
start_idx = buffer.find(0xFF)
if start_idx == -1:
break
# 移除包头前的数据
if start_idx > 0:
buffer = buffer[start_idx:]
# 检查包长度
if len(buffer) < 18:
break
# 查找包尾
if buffer[17] == 0xFE:
# 解析数据
try:
x = struct.unpack('<f', buffer[1:5])[0]
y = struct.unpack('<f', buffer[5:9])[0]
z = struct.unpack('<f', buffer[9:13])[0]
yaw = struct.unpack('<f', buffer[13:17])[0]
self.received_position = {
'x': x, 'y': y, 'z': z, 'yaw': yaw, 'valid': True
}
# 发布tf变换
self.publish_r2_transform(x, y, z, yaw)
# self.get_logger().info(f' Received R2 position: x={x:.3f}, y={y:.3f}, z={z:.3f}, yaw={yaw:.3f}')
except struct.error as e:
self.get_logger().warn(f'⚠️ Data parsing error: {e}')
# 移除已处理的数据包
buffer = buffer[18:]
else:
# 包尾不匹配，移除包头继续查找
buffer = buffer[1:]
else:
# 避免忙等待
import time
time.sleep(0.001)
except Exception as e:
self.get_logger().error(f' Error in receive thread: {e}')
def publish_r2_transform(self, x, y, z, yaw):
"""发布R2到map的tf变换"""
try:
t = TransformStamped()
t.header.stamp = self.get_clock().now().to_msg()
t.header.frame_id = 'map'
t.child_frame_id = 'R2'
t.transform.translation.x = x
t.transform.translation.y = y
t.transform.translation.z = z
# 将yaw角转换为四元数
t.transform.rotation.x = 0.0
t.transform.rotation.y = 0.0
t.transform.rotation.z = math.sin(yaw / 2.0)
t.transform.rotation.w = math.cos(yaw / 2.0)
self.tf_broadcaster.sendTransform(t)
except Exception as e:
self.get_logger().error(f'Error publishing tf: {e}')
def aim_callback(self, msg):
"""接收瞄准数据"""
try:
self.aim_data = {
'yaw': msg.yaw,
'distance': msg.distance,
'valid': True
}
self.get_logger().debug(f'Received aim data: yaw={msg.yaw:.3f}, distance={msg.distance:.3f}')
except Exception as e:
self.get_logger().error(f'Error in aim callback: {e}')
def get_r2_target_data(self):
"""获取R2的瞄准数据"""
try:
if not self.received_position['valid']:
return 0.0, 0.0
# 这里可以根据R2的位置计算目标数据
# 简化处理：返回R2的yaw角和到原点的距离
r2_yaw = math.atan2(self.transform_position['y'], self.transform_position['x'])
if r2_yaw > 1.57:
r2_yaw = r2_yaw - 3.14
if r2_yaw < -1.57:
r2_yaw = r2_yaw + 3.14
r2_distance = math.sqrt(
(self.transform_position['x'])**2 +
self.transform_position['y']**2
)
return r2_yaw, r2_distance
except Exception as e:
self.get_logger().error(f'Error getting R2 target data: {e}')
return 0.0, 0.0
def send_target_data(self):
"""发送目标数据"""
try:
# 获取原始目标数据
if self.aim_data['valid']:
original_yaw = self.aim_data['yaw']
original_distance = self.aim_data['distance']
else:
original_yaw = 0.0
original_distance = 0.0
# 获取R2目标数据
r2_yaw, r2_distance = self.get_r2_target_data()
# 构建发送数据包
# 格式: 包头(0xFF) + original_yaw(4字节) + original_distance(4字节) + r2_yaw(4字节) + r2_distance(4字节) + 校验和(1字节) + 包尾(0xFE)
packet = bytearray()
packet.append(0xFF) # 包头
packet.extend(struct.pack('<f', original_yaw)) # 原始yaw
# packet.extend(struct.pack('<f', original_distance)) # 原始distance
packet.extend(struct.pack('<f', r2_yaw)) # R2 yaw
# packet.extend(struct.pack('<f', r2_distance)) # R2 distance
# 添加IMU angular.z 到 underpan 的第三个数据
packet.extend(struct.pack('<f', self.imu_angular_z)) # 新增：IMU angular.z
packet.append(0xFE) # 包尾
packet_upper = bytearray()
packet_upper.append(0xFF) # 包头
# packet.extend(struct.pack('<f', original_yaw)) # 原始yaw
packet_upper.extend(struct.pack('<f', original_distance)) # 原始distance
# packet.extend(struct.pack('<f', r2_yaw)) # R2 yaw
packet_upper.extend(struct.pack('<f', r2_distance)) # R2 distance
# 计算校验和
checksum = 0
packet_upper.append(0xFE) # 包尾
# 发送数据
self.send_serial.write(packet)
self.send_serial_upper.write(packet_upper)
self.get_logger().info(f'Sent target packet: original({original_yaw:.3f},{original_distance:.3f}) R2({r2_yaw:.3f},{r2_distance:.3f}) IMU_z({self.imu_angular_z:.3f})')
except Exception as e:
self.get_logger().error(f'Error sending target data: {e}')
def _update_r1_position(self):
try:
trans = self.tf_buffer.lookup_transform(
'base_link',
'R2',
rclpy.time.Time()
)
self.transform_position['x'] = trans.transform.translation.x
self.transform_position['y'] = trans.transform.translation.y
except Exception:
pass
def __del__(self):
if hasattr(self, 'receive_serial') and self.receive_serial.is_open:
self.receive_serial.close()
if hasattr(self, 'send_serial') and self.send_serial.is_open:
self.send_serial.close()
if hasattr(self, 'send_serial_upper') and self.send_serial_upper.is_open:
self.send_serial_upper.close()
self.get_logger().info('Serial ports closed')
def main(args=None):
rclpy.init(args=args)
try:
node = ReceiveAndPubNode()
executor = MultiThreadedExecutor()
executor.add_node(node)
executor.spin()
except KeyboardInterrupt:
pass
finally:
if rclpy.ok():
rclpy.shutdown()
if __name__ == '__main__':
main()

44
src/rm_driver/rm_serial_driver/script/slect_aim.py Normal file
View File

@ -0,0 +1,44 @@
import rclpy
from rclpy.node import Node
from rm_msgs.msg import MoveGoal
from geometry_msgs.msg import TransformStamped
import tf2_ros
class AimSelector(Node):
def __init__(self):
super().__init__('aim_selector')
self.publisher = self.create_publisher(MoveGoal, '/move_goal', 10)
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
self.timer = self.create_timer(0.05, self.timer_callback)
def timer_callback(self):
try:
trans: TransformStamped = self.tf_buffer.lookup_transform(
'map', 'base_link', rclpy.time.Time())
x = trans.transform.translation.x
self.get_logger().info(f'base_link x in map: {x:.2f}')
msg = MoveGoal()
if x < 7:
msg.x = 0.61
msg.y = 3.995
else:
msg.x = 13.66
msg.y = 3.73
msg.angle = 0.0
msg.max_speed = 0.0
msg.tolerance = 0.1
msg.rotor = False
self.publisher.publish(msg)
except Exception as e:
self.get_logger().warn(f'Failed to get transform: {e}')
def main(args=None):
rclpy.init(args=args)
node = AimSelector()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

5
src/rm_msg/README.md
View File

@ -3,4 +3,7 @@ Some ROS 2 custom messages for Robotaster
Usage
Modify or add files in the /msg directory as needed
colcon build
colcon build
KERNELS=="3-2:1.0", SUBSYSTEMS=="USB", MODE:="0666",SYMLINK+="underpan"
KERNELS=="3-1:1.0", SUBSYSTEMS=="USB", MODE:="0666",SYMLINK+="upper"
KERNELS=="3-3:1.0", SUBSYSTEMS=="USB", MODE:="0666",SYMLINK+="r2"

2
src/rm_msg/msg/DataAim.msg
View File

@ -1,2 +1,2 @@
float32 yaw
float32 distance
float32 distance

BIN
src/rm_nav_bringup/PCD/1/rc_map1.data Normal file
View File

Binary file not shown.

BIN
src/rm_nav_bringup/PCD/1/rc_map1.pcd Normal file
View File

Binary file not shown.

4
src/rm_nav_bringup/PCD/1/rc_map1.pgm Normal file
View File

File diff suppressed because one or more lines are too long

BIN
src/rm_nav_bringup/PCD/1/rc_map1.posegraph Normal file
View File

Binary file not shown.

7
src/rm_nav_bringup/PCD/1/rc_map1.yaml Normal file
View File

@ -0,0 +1,7 @@
image: rc_map1.pgm
mode: trinary
resolution: 0.05
origin: [-6.19, -7.3, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25

BIN
src/rm_nav_bringup/PCD/2/rc_map1.data Normal file
View File

Binary file not shown.

BIN
src/rm_nav_bringup/PCD/2/rc_map1.pcd Normal file
View File

Binary file not shown.

BIN
src/rm_nav_bringup/PCD/2/rc_map1.pgm Normal file
View File

Binary file not shown.

BIN
src/rm_nav_bringup/PCD/2/rc_map1.posegraph Normal file
View File

Binary file not shown.

7
src/rm_nav_bringup/PCD/2/rc_map1.yaml Normal file
View File

@ -0,0 +1,7 @@
image: rc_map1.pgm
mode: trinary
resolution: 0.05
origin: [-7.9, -8.67, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25

BIN
src/rm_nav_bringup/PCD/RC2025.pcd Normal file
View File

Binary file not shown.

BIN
src/rm_nav_bringup/PCD/rc_map1.pcd Normal file
View File

Binary file not shown.

BIN
src/rm_nav_bringup/PCD/test.pcd Normal file
View File

Binary file not shown.

BIN
src/rm_nav_bringup/PCD/test2.pcd Normal file
View File

Binary file not shown.

2
src/rm_nav_bringup/config/reality/MID360_config.json
View File

@ -25,7 +25,7 @@
},
"lidar_configs" : [
{
"ip" : "192.168.1.176",
"ip" : "192.168.1.137",
"pcl_data_type" : 1,
"pattern_mode" : 0,
"extrinsic_parameter" : {

2
src/rm_nav_bringup/config/reality/fastlio_mid360_real.yaml
View File

@ -7,7 +7,7 @@
filter_size_map: 0.5
cube_side_length: 1000.0
runtime_pos_log_enable: false
map_file_path: "src/rm_nav_bringup/PCD/RC2025.pcd"
map_file_path: "src/rm_nav_bringup/PCD/test2.pcd"
common:
lid_topic: "/livox/lidar"

2
src/rm_nav_bringup/config/reality/icp_registration_real.yaml
View File

@ -3,7 +3,7 @@
use_sim_time: false
rough_leaf_size: 0.4
refine_leaf_size: 0.1
pcd_path: ""
pcd_path: "RC2026"
map_frame_id: "map"
odom_frame_id: "odom"
range_odom_frame_id: "lidar_odom"

2
src/rm_nav_bringup/config/reality/measurement_params_real.yaml
View File

@ -1,3 +1,3 @@
base_link2livox_frame:
xyz: "\"0.037 -0.354 0.41\""
xyz: "\"0.246 -0.143 0.397\""
rpy: "\"0.0 0.0 0.0\""

BIN
src/rm_nav_bringup/map/rc_map1.data Normal file
View File

Binary file not shown.

BIN
src/rm_nav_bringup/map/rc_map1.pgm Normal file
View File

Binary file not shown.

BIN
src/rm_nav_bringup/map/rc_map1.posegraph Normal file
View File

Binary file not shown.

7
src/rm_nav_bringup/map/rc_map1.yaml Normal file
View File

@ -0,0 +1,7 @@
image: rc_map1.pgm
mode: trinary
resolution: 0.05
origin: [-7.9, -8.67, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25

BIN
src/rm_nav_bringup/map/test.data Normal file
View File

Binary file not shown.

BIN
src/rm_nav_bringup/map/test.pgm Normal file
View File

Binary file not shown.

BIN
src/rm_nav_bringup/map/test.posegraph Normal file
View File

Binary file not shown.

7
src/rm_nav_bringup/map/test.yaml Normal file
View File

@ -0,0 +1,7 @@
image: test.pgm
mode: trinary
resolution: 0.05
origin: [-9.04, -13.8, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25

BIN
src/rm_nav_bringup/map/test2.data Normal file
View File

Binary file not shown.

BIN
src/rm_nav_bringup/map/test2.pgm Normal file
View File

Binary file not shown.

BIN
src/rm_nav_bringup/map/test2.posegraph Normal file
View File

Binary file not shown.

7
src/rm_nav_bringup/map/test2.yaml Normal file
View File

@ -0,0 +1,7 @@
image: test2.pgm
mode: trinary
resolution: 0.05
origin: [-5.88, -6.46, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25

16
src/rm_simpal_move/src/simple_move.cpp
View File

@ -35,7 +35,7 @@ namespace rm_simpal_move
: Node("global_position_listener", options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_), running_(true), goal_reached_(false)
{
tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
timer_ = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&RMSimpleMove::timer_callback, this));
timer_ = this->create_wall_timer(std::chrono::milliseconds(10), std::bind(&RMSimpleMove::timer_callback, this));
goal_pose_sub_ = this->create_subscription<rm_msgs::msg::MoveGoal>("/move_goal", 10, std::bind(&RMSimpleMove::goal_pose_callback, this, std::placeholders::_1));
data_ai_pub_ = this->create_publisher<rm_msgs::msg::DataAI>("/chassis/data_ai", 10);
data_nav_pub_ = this->create_publisher<rm_msgs::msg::DataNav>("/chassis/data_nav", 10);
@ -85,19 +85,27 @@ namespace rm_simpal_move
// 计算当前位置到goal_pose和base_link的坐标变换
geometry_msgs::msg::TransformStamped goal_in_base_link;
geometry_msgs::msg::TransformStamped goal_in_R2;
goal_in_base_link = tf_buffer_.lookupTransform("base_link", "goal_pose", tf2::TimePointZero);
// goal_in_R2 = tf_buffer_.lookupTransform("base_link", "R2", tf2::TimePointZero);
float goal_x_in_base_link = goal_in_base_link.transform.translation.x;
float goal_y_in_base_link = goal_in_base_link.transform.translation.y;
// float goal_x_in_R2 = goal_in_R2.transform.translation.x;
// float goal_y_in_R2 = goal_in_R2.transform.translation.y;
// 计算瞄准目标的角度差和距离
float aim_yaw = atan2f(goal_y_in_base_link, goal_x_in_base_link);
float aim_distance = sqrtf(goal_x_in_base_link * goal_x_in_base_link + goal_y_in_base_link * goal_y_in_base_link);
// float aim_yaw_r2 = atan2f(goal_y_in_R2, goal_x_in_R2);
// float aim_distance_r2 = sqrtf(goal_x_in_R2 * goal_x_in_R2 + goal_y_in_R2 * goal_y_in_R2);
// 发布 DataAim 消息
auto data_aim_msg = rm_msgs::msg::DataAim();
data_aim_msg.yaw = aim_yaw; // x方向瞄准目标的偏差角度
data_aim_msg.distance = aim_distance; // 到目标的距离
data_aim_msg.distance = aim_distance; // 到目标的距离
data_aim_pub_->publish(data_aim_msg);
// 发布 DataNav 消息

BIN
test.data Normal file
View File

Binary file not shown.

BIN
test.pgm Normal file
View File

Binary file not shown.

BIN
test.posegraph Normal file
View File

Binary file not shown.

7
test.yaml Normal file
View File

@ -0,0 +1,7 @@
image: test.pgm
mode: trinary
resolution: 0.05
origin: [-7.08, -15.4, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25