Merge branch 'main' of ssh://gitea.qutrobot.top:222/robofish/RC2025
This commit is contained in:
commit
06b1e7522b
222
README.md
222
README.md
@ -1,14 +1,64 @@
|
||||
# RC2025 自动定位瞄准代码
|
||||
# 🎯 RC2025 自动定位瞄准代码
|
||||
|
||||
基于ROS2的机器人自动定位与瞄准系统，支持激光雷达建图和导航功能。
|
||||
> 基于ROS2的机器人自动定位与瞄准系统，支持激光雷达建图和导航功能
|
||||
|
||||
## 系统要求
|
||||
[](https://docs.ros.org/en/humble/)
|
||||
[](https://ubuntu.com/)
|
||||
[](LICENSE)
|
||||
|
||||
- Ubuntu 22.04
|
||||
- ROS2 Humble
|
||||
- 激光雷达：MID360
|
||||
## 📋 目录
|
||||
|
||||
## 快速开始
|
||||
- [系统要求](#系统要求)
|
||||
- [环境配置](#环境配置)
|
||||
- [快速开始](#快速开始)
|
||||
- [重要参数配置](#重要参数配置)
|
||||
- [故障排除](#故障排除)
|
||||
- [贡献指南](#贡献指南)
|
||||
|
||||
## 🔧 系统要求
|
||||
|
||||
| 组件 | 版本/型号 |
|
||||
|------|-----------|
|
||||
| 操作系统 | 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. 编译项目
|
||||
|
||||
@ -16,7 +66,7 @@
|
||||
. build.sh
|
||||
```
|
||||
|
||||
### 2. 建图模式
|
||||
### 2. 🗺️ 建图模式
|
||||
|
||||
用于创建环境地图和点云数据：
|
||||
|
||||
@ -24,46 +74,148 @@
|
||||
. mapping.sh
|
||||
```
|
||||
|
||||
**建图前配置：**
|
||||
1. 修改 `mapping.sh` 中的地图保存文件名（将 `RC2025` 改为您的项目名）
|
||||
2. 同步修改 `src/rm_nav_bringup/config/reality/fastlio_mid360_real.yaml` 中的 pcd 文件名
|
||||
#### 建图前配置
|
||||
|
||||
**建图操作：**
|
||||
- 保存点云文件：`ros2 service call /map_save std_srvs/srv/Trigger`
|
||||
- 保存地图：确保地图名称保持一致
|
||||
1. **修改地图保存文件名**
|
||||
```bash
|
||||
# 编辑 mapping.sh
|
||||
nano mapping.sh
|
||||
# 将 'RC2025' 改为您的项目名
|
||||
```
|
||||
|
||||
### 3. 导航模式
|
||||
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>
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
</details>
|
||||
|
||||
### 3. 🧭 导航模式
|
||||
|
||||
使用已建立的地图进行导航：
|
||||
|
||||
```bash
|
||||
. nav.sh
|
||||
chmod +x nav.sh
|
||||
./nav.sh
|
||||
```
|
||||
|
||||
#### 导航操作说明
|
||||
|
||||
## 重要参数配置
|
||||
1. **启动导航程序**
|
||||
2. **在RViz中设置初始位置**
|
||||
3. **设置目标点进行导航**
|
||||
4. **监控导航状态**
|
||||
|
||||
### 激光雷达安装位置
|
||||
## ⚙️ 重要参数配置
|
||||
|
||||
**位置参数配置：**
|
||||
- 文件：`src/rm_nav_bringup/config/reality/measurement_params_real.yaml`
|
||||
- 修改：`x`, `y`, `z` 坐标
|
||||
- 注意：不要修改 `rpy` 参数
|
||||
### 📍 激光雷达安装位置
|
||||
|
||||
**姿态参数配置：**
|
||||
- 文件：`src/rm_nav_bringup/config/reality/MID360_config.json`
|
||||
- 修改：`yaw`, `pitch`, `roll` 角度
|
||||
- 注意：不要修改 `xyz` 参数
|
||||
#### 位置参数配置
|
||||
```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` 参数
|
||||
|
||||
- 文件：`src/rm_nav_bringup/config/reality/segmentation_real.yaml`
|
||||
- 参数：`sensor_height`（激光雷达距离地面的高度）
|
||||
- 参数：`max_dist_to_line`（地面点云分割的最低高度）
|
||||
- 说明：此参数影响地面点云的正确分割
|
||||
### 🌍 地面点云分割
|
||||
|
||||
### 目标点设定
|
||||
```yaml
|
||||
# 文件：src/rm_nav_bringup/config/reality/segmentation_real.yaml
|
||||
sensor_height: 0.3 # 激光雷达距离地面的高度 (m)
|
||||
max_dist_to_line: 0.05 # 地面点云分割的最低高度 (m)
|
||||
```
|
||||
|
||||
- 文件：`nav.sh`
|
||||
- 参数：篮筐目标点的 `x` 和 `y` 坐标
|
||||
- 用途：设定机器人瞄准的目标点
|
||||
### 🎯 目标点设定
|
||||
|
||||
```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可视化工具监控状态
|
||||
- **维护时**：定期更新地图数据
|
||||
|
||||
19
r1.sh
Normal file
19
r1.sh
Normal file
@ -0,0 +1,19 @@
|
||||
source install/setup.bash
|
||||
|
||||
commands=(
|
||||
"/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:=RC2025 \
|
||||
mode:=nav \
|
||||
lio:=fastlio \
|
||||
localization:=icp \
|
||||
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.56, y: 3.960, angle: 0.0, max_speed: 10.0, tolerance: 0.1, rotor: false}' --once"
|
||||
)
|
||||
|
||||
for cmd in "${commands[@]}"; do
|
||||
gnome-terminal -- bash -c "source install/setup.bash; $cmd; exec bash"
|
||||
sleep 0.5
|
||||
done
|
||||
79
src/rm_driver/rm_serial_driver/script/pub_OnlyAim.py
Normal file
79
src/rm_driver/rm_serial_driver/script/pub_OnlyAim.py
Normal file
@ -0,0 +1,79 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
import serial
|
||||
import struct
|
||||
from rm_msgs.msg import DataAim # 假设使用DataAim消息类型，您可以根据实际消息类型调整
|
||||
class AimDataSerial(Node):
|
||||
def __init__(self):
|
||||
super().__init__('aim_data_serial')
|
||||
|
||||
# 串口配置
|
||||
self.serial_port = '/dev/ttyUSB0' # 根据实际串口设备调整
|
||||
self.baud_rate = 115200
|
||||
|
||||
try:
|
||||
self.serial_conn = serial.Serial(
|
||||
port=self.serial_port,
|
||||
baudrate=self.baud_rate,
|
||||
timeout=1
|
||||
)
|
||||
self.get_logger().info(f'Serial port {self.serial_port} opened successfully')
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'Failed to open serial port: {e}')
|
||||
return
|
||||
|
||||
# 订阅话题
|
||||
self.subscription = self.create_subscription(
|
||||
DataAim, # 根据实际消息类型调整
|
||||
'/chassis/data_aim',
|
||||
self.aim_callback,
|
||||
10
|
||||
)
|
||||
|
||||
self.get_logger().info('Aim data serial node started')
|
||||
|
||||
def aim_callback(self, msg):
|
||||
try:
|
||||
# 提取yaw和distance数据
|
||||
# 根据实际消息结构调整，这里假设使用Point32的x和y字段
|
||||
yaw = msg.yaw
|
||||
distance = msg.distance
|
||||
|
||||
self.get_logger().info(f'Received - yaw: {yaw}, distance: {distance}')
|
||||
|
||||
# 构建发送数据包
|
||||
# 格式: 包头(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.append(0xFE) # 包尾
|
||||
|
||||
# 发送数据
|
||||
self.serial_conn.write(packet)
|
||||
self.get_logger().debug(f'Sent packet: {packet.hex()}')
|
||||
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'Error in aim_callback: {e}')
|
||||
|
||||
def __del__(self):
|
||||
if hasattr(self, 'serial_conn') and self.serial_conn.is_open:
|
||||
self.serial_conn.close()
|
||||
self.get_logger().info('Serial port closed')
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
|
||||
try:
|
||||
node = AimDataSerial()
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
101
src/rm_driver/rm_serial_driver/script/pub_self.py
Normal file
101
src/rm_driver/rm_serial_driver/script/pub_self.py
Normal file
@ -0,0 +1,101 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
import serial
|
||||
import struct
|
||||
import tf2_ros
|
||||
from geometry_msgs.msg import TransformStamped
|
||||
from tf2_geometry_msgs import do_transform_pose
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
import math
|
||||
|
||||
class SelfPositionSerial(Node):
|
||||
def __init__(self):
|
||||
super().__init__('self_position_serial')
|
||||
|
||||
# 串口配置
|
||||
self.serial_port = '/dev/ttyUSB1' # 根据实际串口设备调整
|
||||
self.baud_rate = 115200
|
||||
|
||||
try:
|
||||
self.serial_conn = serial.Serial(
|
||||
port=self.serial_port,
|
||||
baudrate=self.baud_rate,
|
||||
timeout=1
|
||||
)
|
||||
self.get_logger().info(f'Serial port {self.serial_port} opened successfully')
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'Failed to open serial port: {e}')
|
||||
return
|
||||
|
||||
# TF监听器
|
||||
self.tf_buffer = tf2_ros.Buffer()
|
||||
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
|
||||
|
||||
# 定时器，定期发送位置信息
|
||||
self.timer = self.create_timer(0.1, self.send_position) # 10Hz
|
||||
|
||||
self.get_logger().info('Self position serial node started')
|
||||
|
||||
def send_position(self):
|
||||
try:
|
||||
# 获取base_link在map下的位置
|
||||
transform = self.tf_buffer.lookup_transform(
|
||||
'map', 'base_link', rclpy.time.Time())
|
||||
|
||||
# 提取位置和朝向
|
||||
x = transform.transform.translation.x
|
||||
y = transform.transform.translation.y
|
||||
z = transform.transform.translation.z
|
||||
|
||||
# 计算yaw角（从四元数）
|
||||
quat = transform.transform.rotation
|
||||
yaw = math.atan2(2.0 * (quat.w * quat.z + quat.x * quat.y),
|
||||
1.0 - 2.0 * (quat.y * quat.y + quat.z * quat.z))
|
||||
|
||||
self.get_logger().debug(f'Position - x: {x:.3f}, y: {y:.3f}, z: {z:.3f}, yaw: {yaw:.3f}')
|
||||
|
||||
# 构建发送数据包
|
||||
# 格式: 包头(0xFF) + x(4字节) + y(4字节) + z(4字节) + yaw(4字节) + 校验和(1字节) + 包尾(0xFE)
|
||||
packet = bytearray()
|
||||
packet.append(0xFF) # 包头
|
||||
packet.extend(struct.pack('<f', x)) # x坐标
|
||||
packet.extend(struct.pack('<f', y)) # y坐标
|
||||
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) # 包尾
|
||||
|
||||
# 发送数据
|
||||
self.serial_conn.write(packet)
|
||||
self.get_logger().debug(f'Sent position packet: {packet.hex()}')
|
||||
|
||||
except Exception as e:
|
||||
self.get_logger().warn(f'Failed to get transform or send data: {e}')
|
||||
|
||||
def __del__(self):
|
||||
if hasattr(self, 'serial_conn') and self.serial_conn.is_open:
|
||||
self.serial_conn.close()
|
||||
self.get_logger().info('Serial port closed')
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
|
||||
try:
|
||||
node = SelfPositionSerial()
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
241
src/rm_driver/rm_serial_driver/script/receive_and_pub.py
Normal file
241
src/rm_driver/rm_serial_driver/script/receive_and_pub.py
Normal file
@ -0,0 +1,241 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
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/ttyUSB2' # 接收串口
|
||||
self.send_port = '/dev/ttyUSB3' # 发送串口
|
||||
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.get_logger().info(f'Serial ports opened: {self.receive_port}, {self.send_port}')
|
||||
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}
|
||||
|
||||
# 启动接收线程
|
||||
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 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) >= 22: # 最小包长度：包头(1) + 数据(16) + 校验(1) + 包尾(1) = 19
|
||||
# 查找包头
|
||||
start_idx = buffer.find(0xFF)
|
||||
if start_idx == -1:
|
||||
break
|
||||
|
||||
# 移除包头前的数据
|
||||
if start_idx > 0:
|
||||
buffer = buffer[start_idx:]
|
||||
|
||||
# 检查包长度
|
||||
if len(buffer) < 22:
|
||||
break
|
||||
|
||||
# 查找包尾
|
||||
if buffer[21] == 0xFE:
|
||||
# 校验数据
|
||||
checksum = 0
|
||||
for i in range(1, 20):
|
||||
checksum ^= buffer[i]
|
||||
|
||||
if checksum == buffer[20]:
|
||||
# 解析数据
|
||||
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().debug(f'Received position: x={x:.3f}, y={y:.3f}, z={z:.3f}, yaw={yaw:.3f}')
|
||||
else:
|
||||
self.get_logger().warn('Checksum error')
|
||||
|
||||
# 移除已处理的数据包
|
||||
buffer = buffer[22:]
|
||||
else:
|
||||
# 包尾不匹配，移除包头继续查找
|
||||
buffer = buffer[1:]
|
||||
|
||||
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 = self.received_position['yaw']
|
||||
r2_distance = math.sqrt(
|
||||
self.received_position['x']**2 +
|
||||
self.received_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
|
||||
|
||||
# 计算校验和
|
||||
checksum = 0
|
||||
for i in range(1, len(packet)):
|
||||
checksum ^= packet[i]
|
||||
packet.append(checksum)
|
||||
|
||||
packet.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})')
|
||||
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'Error sending target data: {e}')
|
||||
|
||||
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()
|
||||
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()
|
||||
@ -1,184 +0,0 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rm_msgs.msg import MoveGoal
|
||||
|
||||
class SingleGoalPublisher(Node):
|
||||
def __init__(self):
|
||||
super().__init__('single_goal_publisher')
|
||||
self.publisher_ = self.create_publisher(MoveGoal, 'move_goal', 10)
|
||||
|
||||
def publish_goal(self):
|
||||
goal_pose = MoveGoal()
|
||||
goal_pose.x = 0.5377
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
goal_pose.y = 3.96
|
||||
goal_pose.angle = 0.0
|
||||
goal_pose.max_speed = 10.0
|
||||
goal_pose.tolerance = 0.1
|
||||
goal_pose.rotor = False
|
||||
|
||||
self.publisher_.publish(goal_pose)
|
||||
self.get_logger().info(f'Published goal pose: x={goal_pose.x}, y={goal_pose.y}')
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = SingleGoalPublisher()
|
||||
|
||||
# 发布一次目标点
|
||||
node.publish_goal()
|
||||
|
||||
# 等待一秒确保消息发送
|
||||
rclpy.spin_once(node, timeout_sec=1.0)
|
||||
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
Loading…
Reference in New Issue
Block a user