R1
This commit is contained in:
parent
06b1e7522b
commit
dcef535b33
44
.vscode/c_cpp_properties.json
vendored
Normal file
44
.vscode/c_cpp_properties.json
vendored
Normal 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
21
.vscode/settings.json
vendored
Normal 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
221
README.md
@ -1,221 +0,0 @@
|
|||||||
# 🎯 RC2025 自动定位瞄准代码
|
|
||||||
|
|
||||||
> 基于ROS2的机器人自动定位与瞄准系统，支持激光雷达建图和导航功能
|
|
||||||
|
|
||||||
[](https://docs.ros.org/en/humble/)
|
|
||||||
[](https://ubuntu.com/)
|
|
||||||
[](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>
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
</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
4
mapping.sh
Normal file → Executable file
@ -2,11 +2,11 @@ source install/setup.bash
|
|||||||
|
|
||||||
commands=(
|
commands=(
|
||||||
"ros2 launch rm_nav_bringup bringup_real.launch.py \
|
"ros2 launch rm_nav_bringup bringup_real.launch.py \
|
||||||
world:=RC2026 \
|
world:=test \
|
||||||
mode:=mapping \
|
mode:=mapping \
|
||||||
lio:=fastlio \
|
lio:=fastlio \
|
||||||
localization:=icp \
|
localization:=icp \
|
||||||
lio_rviz:=false \
|
lio_rviz:=true \
|
||||||
nav_rviz:=true"
|
nav_rviz:=true"
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|||||||
8
nav.sh
Normal file → Executable file
8
nav.sh
Normal file → Executable file
@ -1,7 +1,7 @@
|
|||||||
source install/setup.bash
|
source install/setup.bash
|
||||||
|
|
||||||
commands=(
|
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 \
|
"ros2 launch rm_nav_bringup bringup_real.launch.py \
|
||||||
world:=RC2026 \
|
world:=RC2026 \
|
||||||
mode:=nav \
|
mode:=nav \
|
||||||
@ -10,7 +10,11 @@ commands=(
|
|||||||
lio_rviz:=false \
|
lio_rviz:=false \
|
||||||
nav_rviz:=true"
|
nav_rviz:=true"
|
||||||
"ros2 launch rm_simpal_move simple_move.launch.py"
|
"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
|
for cmd in "${commands[@]}"; do
|
||||||
|
|||||||
7
src/rm_driver/livox_ros_driver2/.gitignore
vendored
7
src/rm_driver/livox_ros_driver2/.gitignore
vendored
@ -1,10 +1,7 @@
|
|||||||
devel/
|
@~!@
|
||||||
build/
|
build/
|
||||||
install/
|
install/
|
||||||
log/
|
log/
|
||||||
|
|
||||||
.vscode/
|
.vscode/
|
||||||
__pycache__/
|
|
||||||
.catkin_workspace
|
|
||||||
*.gv
|
|
||||||
*.pdf
|
|
||||||
|
|||||||
@ -25,7 +25,7 @@
|
|||||||
},
|
},
|
||||||
"lidar_configs" : [
|
"lidar_configs" : [
|
||||||
{
|
{
|
||||||
"ip" : "192.168.1.137",
|
"ip" : "192.168.1.176",
|
||||||
"pcl_data_type" : 1,
|
"pcl_data_type" : 1,
|
||||||
"pattern_mode" : 0,
|
"pattern_mode" : 0,
|
||||||
"extrinsic_parameter" : {
|
"extrinsic_parameter" : {
|
||||||
|
|||||||
@ -10,7 +10,7 @@ class AimDataSerial(Node):
|
|||||||
super().__init__('aim_data_serial')
|
super().__init__('aim_data_serial')
|
||||||
|
|
||||||
# 串口配置
|
# 串口配置
|
||||||
self.serial_port = '/dev/ttyUSB0' # 根据实际串口设备调整
|
self.serial_port = '/dev/upper' # 根据实际串口设备调整
|
||||||
self.baud_rate = 115200
|
self.baud_rate = 115200
|
||||||
|
|
||||||
try:
|
try:
|
||||||
@ -38,17 +38,20 @@ class AimDataSerial(Node):
|
|||||||
try:
|
try:
|
||||||
# 提取yaw和distance数据
|
# 提取yaw和distance数据
|
||||||
# 根据实际消息结构调整，这里假设使用Point32的x和y字段
|
# 根据实际消息结构调整，这里假设使用Point32的x和y字段
|
||||||
yaw = msg.yaw
|
# yaw = msg.yaw
|
||||||
|
distance_r2 = msg.yaw
|
||||||
distance = msg.distance
|
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)
|
# 格式: 包头(0xFF) + yaw(4字节float) + distance(4字节float) + 包尾(0xFE)
|
||||||
packet = bytearray()
|
packet = bytearray()
|
||||||
packet.append(0xFF) # 包头
|
packet.append(0xFF) # 包头
|
||||||
packet.extend(struct.pack('<f', yaw)) # yaw (小端序float)
|
packet.extend(struct.pack('<f', distance)) # yaw (小端序float)
|
||||||
packet.extend(struct.pack('<f', distance)) # distance (小端序float)
|
packet.extend(struct.pack('<f', distance_r2))
|
||||||
|
packet.extend(struct.pack('<f', z)) # distance (小端序float)
|
||||||
packet.append(0xFE) # 包尾
|
packet.append(0xFE) # 包尾
|
||||||
|
|
||||||
# 发送数据
|
# 发送数据
|
||||||
|
|||||||
@ -10,7 +10,7 @@ class AimDataSerial(Node):
|
|||||||
super().__init__('aim_data_serial')
|
super().__init__('aim_data_serial')
|
||||||
|
|
||||||
# 串口配置
|
# 串口配置
|
||||||
self.serial_port = '/dev/ttyUSB0' # 根据实际串口设备调整
|
self.serial_port = '/dev/underpan' # 根据实际串口设备调整
|
||||||
self.baud_rate = 115200
|
self.baud_rate = 115200
|
||||||
|
|
||||||
try:
|
try:
|
||||||
@ -39,16 +39,17 @@ class AimDataSerial(Node):
|
|||||||
# 提取yaw和distance数据
|
# 提取yaw和distance数据
|
||||||
# 根据实际消息结构调整，这里假设使用Point32的x和y字段
|
# 根据实际消息结构调整，这里假设使用Point32的x和y字段
|
||||||
yaw = msg.yaw
|
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)
|
# 格式: 包头(0xFF) + yaw(4字节float) + distance(4字节float) + 包尾(0xFE)
|
||||||
packet = bytearray()
|
packet = bytearray()
|
||||||
packet.append(0xFF) # 包头
|
packet.append(0xFF) # 包头
|
||||||
packet.extend(struct.pack('<f', yaw)) # yaw (小端序float)
|
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) # 包尾
|
packet.append(0xFE) # 包尾
|
||||||
|
|
||||||
# 发送数据
|
# 发送数据
|
||||||
|
|||||||
@ -15,7 +15,7 @@ class SelfPositionSerial(Node):
|
|||||||
super().__init__('self_position_serial')
|
super().__init__('self_position_serial')
|
||||||
|
|
||||||
# 串口配置
|
# 串口配置
|
||||||
self.serial_port = '/dev/ttyUSB1' # 根据实际串口设备调整
|
self.serial_port = '/dev/ttyUSB2' # 根据实际串口设备调整
|
||||||
self.baud_rate = 115200
|
self.baud_rate = 115200
|
||||||
|
|
||||||
try:
|
try:
|
||||||
@ -65,11 +65,7 @@ class SelfPositionSerial(Node):
|
|||||||
packet.extend(struct.pack('<f', z)) # z坐标
|
packet.extend(struct.pack('<f', z)) # z坐标
|
||||||
packet.extend(struct.pack('<f', yaw)) # yaw角
|
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) # 包尾
|
packet.append(0xFE) # 包尾
|
||||||
|
|
||||||
|
|||||||
323
src/rm_driver/rm_serial_driver/script/r1_serial.py
Normal file
323
src/rm_driver/rm_serial_driver/script/r1_serial.py
Normal 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
274
src/rm_driver/rm_serial_driver/script/receive_and_pub copy.py
Normal 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()
|
||||||
@ -1,5 +1,3 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
|
|
||||||
import rclpy
|
import rclpy
|
||||||
from rclpy.node import Node
|
from rclpy.node import Node
|
||||||
import serial
|
import serial
|
||||||
@ -16,8 +14,9 @@ class ReceiveAndPubNode(Node):
|
|||||||
super().__init__('receive_and_pub')
|
super().__init__('receive_and_pub')
|
||||||
|
|
||||||
# 串口配置
|
# 串口配置
|
||||||
self.receive_port = '/dev/ttyUSB2' # 接收串口
|
self.receive_port = '/dev/r2' # 接收串口
|
||||||
self.send_port = '/dev/ttyUSB3' # 发送串口
|
self.send_port = '/dev/underpan'
|
||||||
|
self.send_port_upper = '/dev/upper' # 发送串口
|
||||||
self.baud_rate = 115200
|
self.baud_rate = 115200
|
||||||
|
|
||||||
try:
|
try:
|
||||||
@ -33,7 +32,12 @@ class ReceiveAndPubNode(Node):
|
|||||||
baudrate=self.baud_rate,
|
baudrate=self.baud_rate,
|
||||||
timeout=1
|
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:
|
except Exception as e:
|
||||||
self.get_logger().error(f'Failed to open serial ports: {e}')
|
self.get_logger().error(f'Failed to open serial ports: {e}')
|
||||||
return
|
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.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.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 = threading.Thread(target=self.receive_position_thread)
|
||||||
self.receive_thread.daemon = True
|
self.receive_thread.daemon = True
|
||||||
@ -66,7 +74,6 @@ class ReceiveAndPubNode(Node):
|
|||||||
self.get_logger().info('Receive and pub node started')
|
self.get_logger().info('Receive and pub node started')
|
||||||
|
|
||||||
def receive_position_thread(self):
|
def receive_position_thread(self):
|
||||||
"""接收位置信息的线程"""
|
|
||||||
buffer = bytearray()
|
buffer = bytearray()
|
||||||
while rclpy.ok():
|
while rclpy.ok():
|
||||||
try:
|
try:
|
||||||
@ -75,7 +82,7 @@ class ReceiveAndPubNode(Node):
|
|||||||
buffer.extend(data)
|
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)
|
start_idx = buffer.find(0xFF)
|
||||||
if start_idx == -1:
|
if start_idx == -1:
|
||||||
@ -86,18 +93,13 @@ class ReceiveAndPubNode(Node):
|
|||||||
buffer = buffer[start_idx:]
|
buffer = buffer[start_idx:]
|
||||||
|
|
||||||
# 检查包长度
|
# 检查包长度
|
||||||
if len(buffer) < 22:
|
if len(buffer) < 18:
|
||||||
break
|
break
|
||||||
|
|
||||||
# 查找包尾
|
# 查找包尾
|
||||||
if buffer[21] == 0xFE:
|
if buffer[17] == 0xFE:
|
||||||
# 校验数据
|
# 解析数据
|
||||||
checksum = 0
|
try:
|
||||||
for i in range(1, 20):
|
|
||||||
checksum ^= buffer[i]
|
|
||||||
|
|
||||||
if checksum == buffer[20]:
|
|
||||||
# 解析数据
|
|
||||||
x = struct.unpack('<f', buffer[1:5])[0]
|
x = struct.unpack('<f', buffer[1:5])[0]
|
||||||
y = struct.unpack('<f', buffer[5:9])[0]
|
y = struct.unpack('<f', buffer[5:9])[0]
|
||||||
z = struct.unpack('<f', buffer[9:13])[0]
|
z = struct.unpack('<f', buffer[9:13])[0]
|
||||||
@ -110,18 +112,23 @@ class ReceiveAndPubNode(Node):
|
|||||||
# 发布tf变换
|
# 发布tf变换
|
||||||
self.publish_r2_transform(x, y, z, yaw)
|
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}')
|
# self.get_logger().info(f' Received R2 position: x={x:.3f}, y={y:.3f}, z={z:.3f}, yaw={yaw:.3f}')
|
||||||
else:
|
|
||||||
self.get_logger().warn('Checksum error')
|
except struct.error as e:
|
||||||
|
self.get_logger().warn(f'⚠️ Data parsing error: {e}')
|
||||||
|
|
||||||
# 移除已处理的数据包
|
# 移除已处理的数据包
|
||||||
buffer = buffer[22:]
|
buffer = buffer[18:]
|
||||||
else:
|
else:
|
||||||
# 包尾不匹配，移除包头继续查找
|
# 包尾不匹配，移除包头继续查找
|
||||||
buffer = buffer[1:]
|
buffer = buffer[1:]
|
||||||
|
else:
|
||||||
|
# 避免忙等待
|
||||||
|
import time
|
||||||
|
time.sleep(0.001)
|
||||||
|
|
||||||
except Exception as e:
|
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):
|
def publish_r2_transform(self, x, y, z, yaw):
|
||||||
"""发布R2到map的tf变换"""
|
"""发布R2到map的tf变换"""
|
||||||
@ -166,10 +173,14 @@ class ReceiveAndPubNode(Node):
|
|||||||
|
|
||||||
# 这里可以根据R2的位置计算目标数据
|
# 这里可以根据R2的位置计算目标数据
|
||||||
# 简化处理：返回R2的yaw角和到原点的距离
|
# 简化处理：返回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(
|
r2_distance = math.sqrt(
|
||||||
self.received_position['x']**2 +
|
(self.transform_position['x'])**2 +
|
||||||
self.received_position['y']**2
|
self.transform_position['y']**2
|
||||||
)
|
)
|
||||||
|
|
||||||
return r2_yaw, r2_distance
|
return r2_yaw, r2_distance
|
||||||
@ -197,30 +208,52 @@ class ReceiveAndPubNode(Node):
|
|||||||
packet = bytearray()
|
packet = bytearray()
|
||||||
packet.append(0xFF) # 包头
|
packet.append(0xFF) # 包头
|
||||||
packet.extend(struct.pack('<f', original_yaw)) # 原始yaw
|
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_yaw)) # R2 yaw
|
||||||
packet.extend(struct.pack('<f', r2_distance)) # R2 distance
|
# 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) # 包尾
|
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.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:
|
except Exception as e:
|
||||||
self.get_logger().error(f'Error sending target data: {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):
|
def __del__(self):
|
||||||
if hasattr(self, 'receive_serial') and self.receive_serial.is_open:
|
if hasattr(self, 'receive_serial') and self.receive_serial.is_open:
|
||||||
self.receive_serial.close()
|
self.receive_serial.close()
|
||||||
if hasattr(self, 'send_serial') and self.send_serial.is_open:
|
if hasattr(self, 'send_serial') and self.send_serial.is_open:
|
||||||
self.send_serial.close()
|
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')
|
self.get_logger().info('Serial ports closed')
|
||||||
|
|
||||||
def main(args=None):
|
def main(args=None):
|
||||||
|
|||||||
294
src/rm_driver/rm_serial_driver/script/siyuanshu.py
Normal file
294
src/rm_driver/rm_serial_driver/script/siyuanshu.py
Normal 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
44
src/rm_driver/rm_serial_driver/script/slect_aim.py
Normal 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()
|
||||||
@ -4,3 +4,6 @@ Some ROS 2 custom messages for Robotaster
|
|||||||
Usage
|
Usage
|
||||||
Modify or add files in the /msg directory as needed
|
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"
|
||||||
BIN
src/rm_nav_bringup/PCD/1/rc_map1.data
Normal file
BIN
src/rm_nav_bringup/PCD/1/rc_map1.data
Normal file
Binary file not shown.
BIN
src/rm_nav_bringup/PCD/1/rc_map1.pcd
Normal file
BIN
src/rm_nav_bringup/PCD/1/rc_map1.pcd
Normal file
Binary file not shown.
4
src/rm_nav_bringup/PCD/1/rc_map1.pgm
Normal file
4
src/rm_nav_bringup/PCD/1/rc_map1.pgm
Normal file
File diff suppressed because one or more lines are too long
BIN
src/rm_nav_bringup/PCD/1/rc_map1.posegraph
Normal file
BIN
src/rm_nav_bringup/PCD/1/rc_map1.posegraph
Normal file
Binary file not shown.
7
src/rm_nav_bringup/PCD/1/rc_map1.yaml
Normal file
7
src/rm_nav_bringup/PCD/1/rc_map1.yaml
Normal 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
BIN
src/rm_nav_bringup/PCD/2/rc_map1.data
Normal file
Binary file not shown.
BIN
src/rm_nav_bringup/PCD/2/rc_map1.pcd
Normal file
BIN
src/rm_nav_bringup/PCD/2/rc_map1.pcd
Normal file
Binary file not shown.
BIN
src/rm_nav_bringup/PCD/2/rc_map1.pgm
Normal file
BIN
src/rm_nav_bringup/PCD/2/rc_map1.pgm
Normal file
Binary file not shown.
BIN
src/rm_nav_bringup/PCD/2/rc_map1.posegraph
Normal file
BIN
src/rm_nav_bringup/PCD/2/rc_map1.posegraph
Normal file
Binary file not shown.
7
src/rm_nav_bringup/PCD/2/rc_map1.yaml
Normal file
7
src/rm_nav_bringup/PCD/2/rc_map1.yaml
Normal 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
BIN
src/rm_nav_bringup/PCD/RC2025.pcd
Normal file
Binary file not shown.
BIN
src/rm_nav_bringup/PCD/rc_map1.pcd
Normal file
BIN
src/rm_nav_bringup/PCD/rc_map1.pcd
Normal file
Binary file not shown.
BIN
src/rm_nav_bringup/PCD/test.pcd
Normal file
BIN
src/rm_nav_bringup/PCD/test.pcd
Normal file
Binary file not shown.
BIN
src/rm_nav_bringup/PCD/test2.pcd
Normal file
BIN
src/rm_nav_bringup/PCD/test2.pcd
Normal file
Binary file not shown.
@ -25,7 +25,7 @@
|
|||||||
},
|
},
|
||||||
"lidar_configs" : [
|
"lidar_configs" : [
|
||||||
{
|
{
|
||||||
"ip" : "192.168.1.176",
|
"ip" : "192.168.1.137",
|
||||||
"pcl_data_type" : 1,
|
"pcl_data_type" : 1,
|
||||||
"pattern_mode" : 0,
|
"pattern_mode" : 0,
|
||||||
"extrinsic_parameter" : {
|
"extrinsic_parameter" : {
|
||||||
|
|||||||
@ -7,7 +7,7 @@
|
|||||||
filter_size_map: 0.5
|
filter_size_map: 0.5
|
||||||
cube_side_length: 1000.0
|
cube_side_length: 1000.0
|
||||||
runtime_pos_log_enable: false
|
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:
|
common:
|
||||||
lid_topic: "/livox/lidar"
|
lid_topic: "/livox/lidar"
|
||||||
|
|||||||
@ -3,7 +3,7 @@
|
|||||||
use_sim_time: false
|
use_sim_time: false
|
||||||
rough_leaf_size: 0.4
|
rough_leaf_size: 0.4
|
||||||
refine_leaf_size: 0.1
|
refine_leaf_size: 0.1
|
||||||
pcd_path: ""
|
pcd_path: "RC2026"
|
||||||
map_frame_id: "map"
|
map_frame_id: "map"
|
||||||
odom_frame_id: "odom"
|
odom_frame_id: "odom"
|
||||||
range_odom_frame_id: "lidar_odom"
|
range_odom_frame_id: "lidar_odom"
|
||||||
|
|||||||
@ -1,3 +1,3 @@
|
|||||||
base_link2livox_frame:
|
base_link2livox_frame:
|
||||||
xyz: "\"0.037 -0.354 0.41\""
|
xyz: "\"0.246 -0.143 0.397\""
|
||||||
rpy: "\"0.0 0.0 0.0\""
|
rpy: "\"0.0 0.0 0.0\""
|
||||||
BIN
src/rm_nav_bringup/map/rc_map1.data
Normal file
BIN
src/rm_nav_bringup/map/rc_map1.data
Normal file
Binary file not shown.
BIN
src/rm_nav_bringup/map/rc_map1.pgm
Normal file
BIN
src/rm_nav_bringup/map/rc_map1.pgm
Normal file
Binary file not shown.
BIN
src/rm_nav_bringup/map/rc_map1.posegraph
Normal file
BIN
src/rm_nav_bringup/map/rc_map1.posegraph
Normal file
Binary file not shown.
7
src/rm_nav_bringup/map/rc_map1.yaml
Normal file
7
src/rm_nav_bringup/map/rc_map1.yaml
Normal 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
BIN
src/rm_nav_bringup/map/test.data
Normal file
Binary file not shown.
BIN
src/rm_nav_bringup/map/test.pgm
Normal file
BIN
src/rm_nav_bringup/map/test.pgm
Normal file
Binary file not shown.
BIN
src/rm_nav_bringup/map/test.posegraph
Normal file
BIN
src/rm_nav_bringup/map/test.posegraph
Normal file
Binary file not shown.
7
src/rm_nav_bringup/map/test.yaml
Normal file
7
src/rm_nav_bringup/map/test.yaml
Normal 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
BIN
src/rm_nav_bringup/map/test2.data
Normal file
Binary file not shown.
BIN
src/rm_nav_bringup/map/test2.pgm
Normal file
BIN
src/rm_nav_bringup/map/test2.pgm
Normal file
Binary file not shown.
BIN
src/rm_nav_bringup/map/test2.posegraph
Normal file
BIN
src/rm_nav_bringup/map/test2.posegraph
Normal file
Binary file not shown.
7
src/rm_nav_bringup/map/test2.yaml
Normal file
7
src/rm_nav_bringup/map/test2.yaml
Normal 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
|
||||||
@ -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)
|
: 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);
|
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));
|
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_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);
|
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的坐标变换
|
// 计算当前位置到goal_pose和base_link的坐标变换
|
||||||
geometry_msgs::msg::TransformStamped goal_in_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_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_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_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_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_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 消息
|
// 发布 DataAim 消息
|
||||||
auto data_aim_msg = rm_msgs::msg::DataAim();
|
auto data_aim_msg = rm_msgs::msg::DataAim();
|
||||||
data_aim_msg.yaw = aim_yaw; // x方向瞄准目标的偏差角度
|
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);
|
data_aim_pub_->publish(data_aim_msg);
|
||||||
|
|
||||||
// 发布 DataNav 消息
|
// 发布 DataNav 消息
|
||||||
|
|||||||
BIN
test.posegraph
Normal file
BIN
test.posegraph
Normal file
Binary file not shown.
Loading…
Reference in New Issue
Block a user