Compare commits
5 Commits
06b1e7522b
...
096e317353
| Author | SHA1 | Date | |
|---|---|---|---|
| 096e317353 | |||
| 500f6bd1d8 | |||
| 2f3e6caa8d | |||
| ccb4a91e64 | |||
| 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"
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|||||||
10
nav.sh
10
nav.sh
@ -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"
|
|
||||||
"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,13 @@ 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"
|
"/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/R2_Serial.py"
|
||||||
|
"/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/receive.py"
|
||||||
|
# "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/slect.py map"
|
||||||
|
"/bin/python3 /home/robofish/RC2025/src/rc_lidar/pcd2pgm.py"
|
||||||
|
"/bin/python3 /home/robofish/RC2025/src/rc_lidar/caijian.py"
|
||||||
|
"/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/pub_goal.py"
|
||||||
|
|
||||||
)
|
)
|
||||||
|
|
||||||
for cmd in "${commands[@]}"; do
|
for cmd in "${commands[@]}"; do
|
||||||
|
|||||||
25
nav1.sh
Normal file
25
nav1.sh
Normal file
@ -0,0 +1,25 @@
|
|||||||
|
# 备场代码
|
||||||
|
source install/setup.bash
|
||||||
|
|
||||||
|
commands=(
|
||||||
|
"ros2 launch rm_nav_bringup bringup_real.launch.py \
|
||||||
|
world:=map1 \
|
||||||
|
mode:=nav \
|
||||||
|
lio:=fastlio \
|
||||||
|
localization:=icp \
|
||||||
|
lio_rviz:=false \
|
||||||
|
nav_rviz:=true"
|
||||||
|
"ros2 launch rm_simpal_move simple_move.launch.py"
|
||||||
|
"/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/R2_Serial.py"
|
||||||
|
"/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/receive.py"
|
||||||
|
# "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/slect.py map1"
|
||||||
|
"/bin/python3 /home/robofish/RC2025/src/rc_lidar/pcd2pgm.py"
|
||||||
|
"/bin/python3 /home/robofish/RC2025/src/rc_lidar/caijian.py"
|
||||||
|
"/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/pub_goal.py"
|
||||||
|
|
||||||
|
)
|
||||||
|
|
||||||
|
for cmd in "${commands[@]}"; do
|
||||||
|
gnome-terminal -- bash -c "source install/setup.bash; $cmd; exec bash"
|
||||||
|
sleep 0.5
|
||||||
|
done
|
||||||
25
nav2.sh
Normal file
25
nav2.sh
Normal file
@ -0,0 +1,25 @@
|
|||||||
|
# 备场代码
|
||||||
|
source install/setup.bash
|
||||||
|
|
||||||
|
commands=(
|
||||||
|
"ros2 launch rm_nav_bringup bringup_real.launch.py \
|
||||||
|
world:=map2 \
|
||||||
|
mode:=nav \
|
||||||
|
lio:=fastlio \
|
||||||
|
localization:=icp \
|
||||||
|
lio_rviz:=false \
|
||||||
|
nav_rviz:=true"
|
||||||
|
"ros2 launch rm_simpal_move simple_move.launch.py"
|
||||||
|
"/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/R2_Serial.py"
|
||||||
|
"/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/receive.py"
|
||||||
|
# "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/slect.py map2"
|
||||||
|
"/bin/python3 /home/robofish/RC2025/src/rc_lidar/pcd2pgm.py"
|
||||||
|
"/bin/python3 /home/robofish/RC2025/src/rc_lidar/caijian.py"
|
||||||
|
"/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/pub_goal.py"
|
||||||
|
|
||||||
|
)
|
||||||
|
|
||||||
|
for cmd in "${commands[@]}"; do
|
||||||
|
gnome-terminal -- bash -c "source install/setup.bash; $cmd; exec bash"
|
||||||
|
sleep 0.5
|
||||||
|
done
|
||||||
19
r1.sh
19
r1.sh
@ -1,19 +0,0 @@
|
|||||||
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
|
|
||||||
63
src/rc_lidar/caijian.py
Normal file
63
src/rc_lidar/caijian.py
Normal file
@ -0,0 +1,63 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
import rclpy
|
||||||
|
from rclpy.node import Node
|
||||||
|
from sensor_msgs.msg import PointCloud2, PointField
|
||||||
|
import numpy as np
|
||||||
|
import struct
|
||||||
|
from sklearn.cluster import DBSCAN
|
||||||
|
|
||||||
|
class LidarFilterNode(Node):
|
||||||
|
def __init__(self):
|
||||||
|
super().__init__('caijian_node')
|
||||||
|
self.publisher_ = self.create_publisher(PointCloud2, '/livox/lidar_filtered', 10)
|
||||||
|
self.subscription = self.create_subscription(
|
||||||
|
PointCloud2,
|
||||||
|
'/livox/lidar/pointcloud',
|
||||||
|
self.filter_callback,
|
||||||
|
10)
|
||||||
|
self.get_logger().info('caijian_node started, numpy filtering z in [1.5,3]m, distance<=12m, remove isolated points')
|
||||||
|
|
||||||
|
def filter_callback(self, msg):
|
||||||
|
num_points = msg.width * msg.height
|
||||||
|
data = np.frombuffer(msg.data, dtype=np.uint8)
|
||||||
|
points = np.zeros((num_points, 4), dtype=np.float32) # x, y, z, intensity
|
||||||
|
|
||||||
|
for i in range(num_points):
|
||||||
|
offset = i * msg.point_step
|
||||||
|
x = struct.unpack_from('f', data, offset)[0]
|
||||||
|
y = struct.unpack_from('f', data, offset + 4)[0]
|
||||||
|
z = struct.unpack_from('f', data, offset + 8)[0]
|
||||||
|
intensity = struct.unpack_from('f', data, offset + 12)[0]
|
||||||
|
points[i] = [x, y, z, intensity]
|
||||||
|
|
||||||
|
z_mask = (points[:,2] >= 1.5) & (points[:,2] <= 3.0)
|
||||||
|
dist_mask = np.linalg.norm(points[:,:3], axis=1) <= 16.0
|
||||||
|
mask = z_mask & dist_mask
|
||||||
|
filtered_points = points[mask]
|
||||||
|
|
||||||
|
# 使用DBSCAN去除孤立点
|
||||||
|
if filtered_points.shape[0] > 0:
|
||||||
|
clustering = DBSCAN(eps=0.3, min_samples=5).fit(filtered_points[:,:3])
|
||||||
|
core_mask = clustering.labels_ != -1
|
||||||
|
filtered_points = filtered_points[core_mask]
|
||||||
|
|
||||||
|
fields = [
|
||||||
|
PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
|
||||||
|
PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
|
||||||
|
PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
|
||||||
|
PointField(name='intensity', offset=12, datatype=PointField.FLOAT32, count=1),
|
||||||
|
]
|
||||||
|
filtered_points_list = filtered_points.tolist()
|
||||||
|
import sensor_msgs_py.point_cloud2 as pc2
|
||||||
|
filtered_msg = pc2.create_cloud(msg.header, fields, filtered_points_list)
|
||||||
|
self.publisher_.publish(filtered_msg)
|
||||||
|
|
||||||
|
def main(args=None):
|
||||||
|
rclpy.init(args=args)
|
||||||
|
node = LidarFilterNode()
|
||||||
|
rclpy.spin(node)
|
||||||
|
node.destroy_node()
|
||||||
|
rclpy.shutdown()
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
main()
|
||||||
90
src/rc_lidar/circlr.py
Normal file
90
src/rc_lidar/circlr.py
Normal file
@ -0,0 +1,90 @@
|
|||||||
|
import rclpy
|
||||||
|
from rclpy.node import Node
|
||||||
|
from sensor_msgs.msg import PointCloud2
|
||||||
|
from geometry_msgs.msg import PointStamped
|
||||||
|
from sensor_msgs_py import point_cloud2 as pc2
|
||||||
|
import numpy as np
|
||||||
|
from sklearn.cluster import DBSCAN
|
||||||
|
import time
|
||||||
|
|
||||||
|
def statistical_outlier_removal(points, k=20, std_ratio=2.0):
|
||||||
|
from sklearn.neighbors import NearestNeighbors
|
||||||
|
nbrs = NearestNeighbors(n_neighbors=k+1).fit(points)
|
||||||
|
distances, _ = nbrs.kneighbors(points)
|
||||||
|
mean_dist = np.mean(distances[:, 1:], axis=1)
|
||||||
|
threshold = np.mean(mean_dist) + std_ratio * np.std(mean_dist)
|
||||||
|
mask = mean_dist < threshold
|
||||||
|
return points[mask]
|
||||||
|
|
||||||
|
class HoopFinder(Node):
|
||||||
|
def __init__(self):
|
||||||
|
super().__init__('find_hoop')
|
||||||
|
self.sub = self.create_subscription(
|
||||||
|
PointCloud2,
|
||||||
|
'/livox/lidar_filtered',
|
||||||
|
self.callback,
|
||||||
|
10)
|
||||||
|
self.pub = self.create_publisher(PointStamped, '/hoop_position', 10)
|
||||||
|
self.buffer = []
|
||||||
|
self.start_time = None
|
||||||
|
self.hoop_history = []
|
||||||
|
|
||||||
|
def callback(self, msg):
|
||||||
|
# 采集0.4秒内的点云
|
||||||
|
if self.start_time is None:
|
||||||
|
self.start_time = time.time()
|
||||||
|
for p in pc2.read_points(msg, field_names=("x", "y", "z", "intensity"), skip_nans=True):
|
||||||
|
self.buffer.append([p[0], p[1], p[2], p[3]])
|
||||||
|
if time.time() - self.start_time < 0.4:
|
||||||
|
return
|
||||||
|
|
||||||
|
points = np.array(self.buffer)
|
||||||
|
self.buffer = []
|
||||||
|
self.start_time = None
|
||||||
|
|
||||||
|
# 高度滤波
|
||||||
|
filtered = points[(points[:,2] > 1.0) & (points[:,2] < 3.0)]
|
||||||
|
if len(filtered) == 0:
|
||||||
|
return
|
||||||
|
|
||||||
|
# 统计离群点滤波
|
||||||
|
filtered = statistical_outlier_removal(filtered[:,:3], k=20, std_ratio=2.0)
|
||||||
|
|
||||||
|
# DBSCAN聚类
|
||||||
|
clustering = DBSCAN(eps=0.3, min_samples=10).fit(filtered)
|
||||||
|
labels = clustering.labels_
|
||||||
|
unique_labels = set(labels)
|
||||||
|
hoop_pos = None
|
||||||
|
max_cluster_size = 0
|
||||||
|
|
||||||
|
for label in unique_labels:
|
||||||
|
if label == -1:
|
||||||
|
continue
|
||||||
|
cluster = filtered[labels == label]
|
||||||
|
if len(cluster) > max_cluster_size:
|
||||||
|
max_cluster_size = len(cluster)
|
||||||
|
hoop_pos = np.mean(cluster, axis=0)
|
||||||
|
|
||||||
|
# 均值滤波输出
|
||||||
|
if hoop_pos is not None:
|
||||||
|
self.hoop_history.append(hoop_pos)
|
||||||
|
if len(self.hoop_history) > 5:
|
||||||
|
self.hoop_history.pop(0)
|
||||||
|
smooth_pos = np.mean(self.hoop_history, axis=0)
|
||||||
|
pt = PointStamped()
|
||||||
|
pt.header = msg.header
|
||||||
|
pt.point.x = float(smooth_pos[0])
|
||||||
|
pt.point.y = float(smooth_pos[1])
|
||||||
|
pt.point.z = float(smooth_pos[2])
|
||||||
|
self.pub.publish(pt)
|
||||||
|
self.get_logger().info(f"Hoop position (smoothed): {smooth_pos}")
|
||||||
|
|
||||||
|
def main(args=None):
|
||||||
|
rclpy.init(args=args)
|
||||||
|
node = HoopFinder()
|
||||||
|
rclpy.spin(node)
|
||||||
|
node.destroy_node()
|
||||||
|
rclpy.shutdown()
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
main()
|
||||||
59
src/rc_lidar/find.py
Normal file
59
src/rc_lidar/find.py
Normal file
@ -0,0 +1,59 @@
|
|||||||
|
import rclpy
|
||||||
|
from rclpy.node import Node
|
||||||
|
from sensor_msgs.msg import PointCloud2
|
||||||
|
from geometry_msgs.msg import PointStamped
|
||||||
|
from sensor_msgs_py import point_cloud2 as pc2
|
||||||
|
import numpy as np
|
||||||
|
from sklearn.cluster import DBSCAN
|
||||||
|
|
||||||
|
class HoopFinder(Node):
|
||||||
|
def __init__(self):
|
||||||
|
super().__init__('find_hoop')
|
||||||
|
self.sub = self.create_subscription(
|
||||||
|
PointCloud2,
|
||||||
|
'/livox/lidar',
|
||||||
|
self.callback,
|
||||||
|
10)
|
||||||
|
self.pub = self.create_publisher(PointStamped, '/hoop_position', 10)
|
||||||
|
|
||||||
|
def callback(self, msg):
|
||||||
|
points = []
|
||||||
|
for p in pc2.read_points(msg, field_names=("x", "y", "z", "intensity"), skip_nans=True):
|
||||||
|
points.append([p[0], p[1], p[2], p[3]])
|
||||||
|
points = np.array(points)
|
||||||
|
filtered = points[(points[:,2] > 1.0) & (points[:,2] < 3.0)]
|
||||||
|
if len(filtered) == 0:
|
||||||
|
return
|
||||||
|
|
||||||
|
clustering = DBSCAN(eps=0.3, min_samples=10).fit(filtered[:,:3])
|
||||||
|
labels = clustering.labels_
|
||||||
|
unique_labels = set(labels)
|
||||||
|
hoop_pos = None
|
||||||
|
max_cluster_size = 0
|
||||||
|
|
||||||
|
for label in unique_labels:
|
||||||
|
if label == -1:
|
||||||
|
continue
|
||||||
|
cluster = filtered[labels == label]
|
||||||
|
if len(cluster) > max_cluster_size:
|
||||||
|
max_cluster_size = len(cluster)
|
||||||
|
hoop_pos = np.mean(cluster[:,:3], axis=0)
|
||||||
|
|
||||||
|
if hoop_pos is not None:
|
||||||
|
pt = PointStamped()
|
||||||
|
pt.header = msg.header
|
||||||
|
pt.point.x = float(hoop_pos[0])
|
||||||
|
pt.point.y = float(hoop_pos[1])
|
||||||
|
pt.point.z = float(hoop_pos[2])
|
||||||
|
self.pub.publish(pt)
|
||||||
|
self.get_logger().info(f"Hoop position: {hoop_pos}")
|
||||||
|
|
||||||
|
def main(args=None):
|
||||||
|
rclpy.init(args=args)
|
||||||
|
node = HoopFinder()
|
||||||
|
rclpy.spin(node)
|
||||||
|
node.destroy_node()
|
||||||
|
rclpy.shutdown()
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
main()
|
||||||
63
src/rc_lidar/fliter.py
Normal file
63
src/rc_lidar/fliter.py
Normal file
@ -0,0 +1,63 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
import rclpy
|
||||||
|
from rclpy.node import Node
|
||||||
|
from sensor_msgs.msg import PointCloud2, PointField
|
||||||
|
import numpy as np
|
||||||
|
import struct
|
||||||
|
from sklearn.cluster import DBSCAN
|
||||||
|
|
||||||
|
class LidarFilterNode(Node):
|
||||||
|
def __init__(self):
|
||||||
|
super().__init__('caijian_node')
|
||||||
|
self.publisher_ = self.create_publisher(PointCloud2, '/livox/lidar_filtered', 10)
|
||||||
|
self.subscription = self.create_subscription(
|
||||||
|
PointCloud2,
|
||||||
|
'/livox/lidar/pointcloud',
|
||||||
|
self.filter_callback,
|
||||||
|
10)
|
||||||
|
self.get_logger().info('caijian_node started, numpy filtering z in [1.5,3]m, distance<=12m, remove isolated points')
|
||||||
|
|
||||||
|
def filter_callback(self, msg):
|
||||||
|
num_points = msg.width * msg.height
|
||||||
|
data = np.frombuffer(msg.data, dtype=np.uint8)
|
||||||
|
points = np.zeros((num_points, 4), dtype=np.float32) # x, y, z, intensity
|
||||||
|
|
||||||
|
for i in range(num_points):
|
||||||
|
offset = i * msg.point_step
|
||||||
|
x = struct.unpack_from('f', data, offset)[0]
|
||||||
|
y = struct.unpack_from('f', data, offset + 4)[0]
|
||||||
|
z = struct.unpack_from('f', data, offset + 8)[0]
|
||||||
|
intensity = struct.unpack_from('f', data, offset + 12)[0]
|
||||||
|
points[i] = [x, y, z, intensity]
|
||||||
|
|
||||||
|
z_mask = (points[:,2] >= 1.5) & (points[:,2] <= 3.0)
|
||||||
|
dist_mask = np.linalg.norm(points[:,:3], axis=1) <= 12.0
|
||||||
|
mask = z_mask & dist_mask
|
||||||
|
filtered_points = points[mask]
|
||||||
|
|
||||||
|
# 使用DBSCAN去除孤立点
|
||||||
|
if filtered_points.shape[0] > 0:
|
||||||
|
clustering = DBSCAN(eps=0.3, min_samples=5).fit(filtered_points[:,:3])
|
||||||
|
core_mask = clustering.labels_ != -1
|
||||||
|
filtered_points = filtered_points[core_mask]
|
||||||
|
|
||||||
|
fields = [
|
||||||
|
PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
|
||||||
|
PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
|
||||||
|
PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
|
||||||
|
PointField(name='intensity', offset=12, datatype=PointField.FLOAT32, count=1),
|
||||||
|
]
|
||||||
|
filtered_points_list = filtered_points.tolist()
|
||||||
|
import sensor_msgs_py.point_cloud2 as pc2
|
||||||
|
filtered_msg = pc2.create_cloud(msg.header, fields, filtered_points_list)
|
||||||
|
self.publisher_.publish(filtered_msg)
|
||||||
|
|
||||||
|
def main(args=None):
|
||||||
|
rclpy.init(args=args)
|
||||||
|
node = LidarFilterNode()
|
||||||
|
rclpy.spin(node)
|
||||||
|
node.destroy_node()
|
||||||
|
rclpy.shutdown()
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
main()
|
||||||
340
src/rc_lidar/juxing.py
Normal file
340
src/rc_lidar/juxing.py
Normal file
@ -0,0 +1,340 @@
|
|||||||
|
import rclpy
|
||||||
|
from rclpy.node import Node
|
||||||
|
from sensor_msgs.msg import PointCloud2, PointField
|
||||||
|
import numpy as np
|
||||||
|
import struct
|
||||||
|
from sklearn.cluster import DBSCAN
|
||||||
|
import cv2
|
||||||
|
from visualization_msgs.msg import Marker
|
||||||
|
from sklearn.linear_model import RANSACRegressor
|
||||||
|
|
||||||
|
|
||||||
|
def ransac_line_3d(points, threshold=0.05, min_inliers=20):
|
||||||
|
best_inliers = []
|
||||||
|
best_line = None
|
||||||
|
N = len(points)
|
||||||
|
if N < min_inliers:
|
||||||
|
return None, []
|
||||||
|
for _ in range(100):
|
||||||
|
idx = np.random.choice(N, 2, replace=False)
|
||||||
|
p1, p2 = points[idx]
|
||||||
|
v = p2 - p1
|
||||||
|
v = v / np.linalg.norm(v)
|
||||||
|
dists = np.linalg.norm(np.cross(points - p1, v), axis=1)
|
||||||
|
inliers = np.where(dists < threshold)[0]
|
||||||
|
if len(inliers) > len(best_inliers):
|
||||||
|
best_inliers = inliers
|
||||||
|
best_line = (p1, p2)
|
||||||
|
if len(best_inliers) > N * 0.5:
|
||||||
|
break
|
||||||
|
return best_line, best_inliers
|
||||||
|
|
||||||
|
def fit_rectangle_pca(cluster):
|
||||||
|
# 用PCA找主方向和边界点
|
||||||
|
pts = cluster[:, :3]
|
||||||
|
mean = np.mean(pts, axis=0)
|
||||||
|
cov = np.cov(pts.T)
|
||||||
|
eigvals, eigvecs = np.linalg.eigh(cov)
|
||||||
|
order = np.argsort(eigvals)[::-1]
|
||||||
|
main_dir = eigvecs[:, order[0]]
|
||||||
|
second_dir = eigvecs[:, order[1]]
|
||||||
|
# 投影到主方向和次方向
|
||||||
|
proj_main = np.dot(pts - mean, main_dir)
|
||||||
|
proj_second = np.dot(pts - mean, second_dir)
|
||||||
|
# 找四个角点
|
||||||
|
corners = []
|
||||||
|
for xm in [np.min(proj_main), np.max(proj_main)]:
|
||||||
|
for xs in [np.min(proj_second), np.max(proj_second)]:
|
||||||
|
idx = np.argmin((proj_main - xm)**2 + (proj_second - xs)**2)
|
||||||
|
corners.append(pts[idx])
|
||||||
|
return corners
|
||||||
|
|
||||||
|
def rectangle_score(pts):
|
||||||
|
# 评估4个点是否接近矩形
|
||||||
|
d = [np.linalg.norm(pts[i] - pts[(i+1)%4]) for i in range(4)]
|
||||||
|
diag1 = np.linalg.norm(pts[0] - pts[2])
|
||||||
|
diag2 = np.linalg.norm(pts[1] - pts[3])
|
||||||
|
w = max(d)
|
||||||
|
h = min(d)
|
||||||
|
ratio = w / h if h > 0 else 0
|
||||||
|
ideal_ratio = 1.8 / 1.05
|
||||||
|
score = abs(ratio - ideal_ratio) + abs(diag1 - diag2) / max(diag1, diag2)
|
||||||
|
return score
|
||||||
|
|
||||||
|
def classify_lines(lines):
|
||||||
|
# lines: 每条线是 [x, y, z, intensity]
|
||||||
|
# 假设 lines 是8个端点，两两为一条线
|
||||||
|
vertical_lines = []
|
||||||
|
horizontal_lines = []
|
||||||
|
for i in range(0, len(lines), 2):
|
||||||
|
p1 = np.array(lines[i][:3])
|
||||||
|
p2 = np.array(lines[i+1][:3])
|
||||||
|
vec = p2 - p1
|
||||||
|
length = np.linalg.norm(vec)
|
||||||
|
# 计算与地面的夹角（假设地面为z轴为0，垂直为z方向）
|
||||||
|
dz = abs(vec[2])
|
||||||
|
dxy = np.linalg.norm(vec[:2])
|
||||||
|
# 垂直线：z方向分量大，长度约1.05m
|
||||||
|
if dz > dxy and 0.95 < length < 1.15:
|
||||||
|
vertical_lines.append((i, i+1, length))
|
||||||
|
# 水平线：z方向分量小，长度约1.8m
|
||||||
|
elif dz < dxy and 1.7 < length < 1.9:
|
||||||
|
horizontal_lines.append((i, i+1, length))
|
||||||
|
return vertical_lines, horizontal_lines
|
||||||
|
|
||||||
|
def find_best_rectangle_from_lines(lines):
|
||||||
|
vertical_lines, horizontal_lines = classify_lines(lines)
|
||||||
|
# 只选出2条垂直线和2条水平线
|
||||||
|
if len(vertical_lines) < 2 or len(horizontal_lines) < 2:
|
||||||
|
return None
|
||||||
|
# 取长度最接近目标的两条
|
||||||
|
vertical_lines = sorted(vertical_lines, key=lambda x: abs(x[2]-1.05))[:2]
|
||||||
|
horizontal_lines = sorted(horizontal_lines, key=lambda x: abs(x[2]-1.8))[:2]
|
||||||
|
# 组合4个端点
|
||||||
|
indices = [vertical_lines[0][0], vertical_lines[0][1],
|
||||||
|
vertical_lines[1][0], vertical_lines[1][1],
|
||||||
|
horizontal_lines[0][0], horizontal_lines[0][1],
|
||||||
|
horizontal_lines[1][0], horizontal_lines[1][1]]
|
||||||
|
# 去重，只保留4个顶点
|
||||||
|
unique_indices = list(set(indices))
|
||||||
|
if len(unique_indices) < 4:
|
||||||
|
return None
|
||||||
|
pts = [lines[idx] for idx in unique_indices[:4]]
|
||||||
|
# 按矩形评分排序
|
||||||
|
from itertools import permutations
|
||||||
|
best_score = float('inf')
|
||||||
|
best_rect = None
|
||||||
|
for order in permutations(range(4)):
|
||||||
|
ordered = [pts[i] for i in order]
|
||||||
|
score = rectangle_score(np.array([p[:3] for p in ordered]))
|
||||||
|
if score < best_score:
|
||||||
|
best_score = score
|
||||||
|
best_rect = ordered
|
||||||
|
return best_rect
|
||||||
|
|
||||||
|
|
||||||
|
class BasketballFrameDetector(Node):
|
||||||
|
def __init__(self):
|
||||||
|
super().__init__('basketball_frame_detector')
|
||||||
|
self.subscription = self.create_subscription(
|
||||||
|
PointCloud2,
|
||||||
|
'/livox/lidar_filtered',
|
||||||
|
self.pointcloud_callback,
|
||||||
|
10
|
||||||
|
)
|
||||||
|
self.publisher = self.create_publisher(
|
||||||
|
PointCloud2,
|
||||||
|
'/basketball_frame_cloud',
|
||||||
|
10
|
||||||
|
)
|
||||||
|
self.marker_pub = self.create_publisher(
|
||||||
|
Marker,
|
||||||
|
'/basketball_frame_lines',
|
||||||
|
10
|
||||||
|
)
|
||||||
|
self.pointcloud_buffer = []
|
||||||
|
self.max_buffer_size = 10 # 减少缓冲帧数，加快响应
|
||||||
|
self.center_buffer = []
|
||||||
|
self.center_buffer_size = 5
|
||||||
|
|
||||||
|
def pointcloud_callback(self, msg):
|
||||||
|
points = self.pointcloud2_to_xyz(msg)
|
||||||
|
if points.shape[0] == 0:
|
||||||
|
return # 跳过空点云
|
||||||
|
self.pointcloud_buffer.append(points)
|
||||||
|
# 只保留非空点云
|
||||||
|
self.pointcloud_buffer = [arr for arr in self.pointcloud_buffer if arr.shape[0] > 0]
|
||||||
|
if len(self.pointcloud_buffer) > self.max_buffer_size:
|
||||||
|
self.pointcloud_buffer.pop(0)
|
||||||
|
all_points = np.vstack(self.pointcloud_buffer)
|
||||||
|
xy_points = all_points[:, :2]
|
||||||
|
|
||||||
|
if len(xy_points) < 10:
|
||||||
|
self.get_logger().info('点数太少，跳过')
|
||||||
|
return
|
||||||
|
clustering = DBSCAN(eps=0.3, min_samples=10).fit(xy_points)
|
||||||
|
labels = clustering.labels_
|
||||||
|
unique_labels = set(labels)
|
||||||
|
found = False
|
||||||
|
for label in unique_labels:
|
||||||
|
if label == -1:
|
||||||
|
continue
|
||||||
|
cluster = all_points[labels == label]
|
||||||
|
if len(cluster) < 30:
|
||||||
|
continue
|
||||||
|
min_x, min_y = np.min(cluster[:, :2], axis=0)
|
||||||
|
max_x, max_y = np.max(cluster[:, :2], axis=0)
|
||||||
|
width = abs(max_x - min_x)
|
||||||
|
height = abs(max_y - min_y)
|
||||||
|
if 1.5 < width < 2.1 and 0.7 < height < 1.3:
|
||||||
|
cloud_msg = self.xyz_array_to_pointcloud2(cluster, msg.header)
|
||||||
|
self.publisher.publish(cloud_msg)
|
||||||
|
|
||||||
|
# 用PCA直接找矩形四角
|
||||||
|
corners = fit_rectangle_pca(cluster)
|
||||||
|
from itertools import permutations
|
||||||
|
best_score = float('inf')
|
||||||
|
best_rect = None
|
||||||
|
for order in permutations(range(4)):
|
||||||
|
ordered = [corners[i] for i in order]
|
||||||
|
score = rectangle_score(np.array(ordered))
|
||||||
|
if score < best_score:
|
||||||
|
best_score = score
|
||||||
|
best_rect = ordered
|
||||||
|
rect_lines = best_rect
|
||||||
|
if rect_lines is None or len(rect_lines) < 4:
|
||||||
|
self.get_logger().info('未找到合适矩形')
|
||||||
|
continue
|
||||||
|
|
||||||
|
# 发布最新的矩形
|
||||||
|
marker = Marker()
|
||||||
|
marker.header = msg.header
|
||||||
|
marker.ns = "basketball_frame"
|
||||||
|
marker.id = 0
|
||||||
|
marker.type = Marker.LINE_LIST
|
||||||
|
marker.action = Marker.ADD
|
||||||
|
marker.scale.x = 0.08
|
||||||
|
marker.color.r = 0.0
|
||||||
|
marker.color.g = 1.0
|
||||||
|
marker.color.b = 0.0
|
||||||
|
marker.color.a = 1.0
|
||||||
|
marker.points = []
|
||||||
|
from geometry_msgs.msg import Point
|
||||||
|
for i in range(4):
|
||||||
|
p1 = rect_lines[i]
|
||||||
|
p2 = rect_lines[(i+1)%4]
|
||||||
|
pt1 = Point(x=float(p1[0]), y=float(p1[1]), z=float(p1[2]))
|
||||||
|
pt2 = Point(x=float(p2[0]), y=float(p2[1]), z=float(p2[2]))
|
||||||
|
marker.points.append(pt1)
|
||||||
|
marker.points.append(pt2)
|
||||||
|
self.marker_pub.publish(marker)
|
||||||
|
|
||||||
|
# 分别发布4条最优边线
|
||||||
|
for i in range(4):
|
||||||
|
edge_marker = Marker()
|
||||||
|
edge_marker.header = msg.header
|
||||||
|
edge_marker.ns = "basketball_frame_edges"
|
||||||
|
edge_marker.id = i
|
||||||
|
edge_marker.type = Marker.LINE_STRIP
|
||||||
|
edge_marker.action = Marker.ADD
|
||||||
|
edge_marker.scale.x = 0.12
|
||||||
|
colors = [
|
||||||
|
(1.0, 0.0, 0.0),
|
||||||
|
(0.0, 1.0, 0.0),
|
||||||
|
(0.0, 0.0, 1.0),
|
||||||
|
(1.0, 1.0, 0.0)
|
||||||
|
]
|
||||||
|
edge_marker.color.r = colors[i][0]
|
||||||
|
edge_marker.color.g = colors[i][1]
|
||||||
|
edge_marker.color.b = colors[i][2]
|
||||||
|
edge_marker.color.a = 1.0
|
||||||
|
pt1 = Point(x=float(rect_lines[i][0]), y=float(rect_lines[i][1]), z=float(rect_lines[i][2]))
|
||||||
|
pt2 = Point(x=float(rect_lines[(i+1)%4][0]), y=float(rect_lines[(i+1)%4][1]), z=float(rect_lines[(i+1)%4][2]))
|
||||||
|
edge_marker.points = [pt1, pt2]
|
||||||
|
self.marker_pub.publish(edge_marker)
|
||||||
|
|
||||||
|
# 中心点滑动平均
|
||||||
|
center = np.mean(np.array(rect_lines), axis=0)
|
||||||
|
self.center_buffer.append(center)
|
||||||
|
if len(self.center_buffer) > self.center_buffer_size:
|
||||||
|
self.center_buffer.pop(0)
|
||||||
|
stable_center = np.mean(self.center_buffer, axis=0)
|
||||||
|
|
||||||
|
# 发布中心点Marker
|
||||||
|
center_marker = Marker()
|
||||||
|
center_marker.header = msg.header
|
||||||
|
center_marker.ns = "basketball_frame"
|
||||||
|
center_marker.id = 1
|
||||||
|
center_marker.type = Marker.SPHERE
|
||||||
|
center_marker.action = Marker.ADD
|
||||||
|
center_marker.scale.x = 0.15
|
||||||
|
center_marker.scale.y = 0.15
|
||||||
|
center_marker.scale.z = 0.15
|
||||||
|
center_marker.color.r = 1.0
|
||||||
|
center_marker.color.g = 0.0
|
||||||
|
center_marker.color.b = 0.0
|
||||||
|
center_marker.color.a = 1.0
|
||||||
|
center_marker.pose.position.x = float(stable_center[0])
|
||||||
|
center_marker.pose.position.y = float(stable_center[1])
|
||||||
|
center_marker.pose.position.z = float(stable_center[2])
|
||||||
|
center_marker.pose.orientation.x = 0.0
|
||||||
|
center_marker.pose.orientation.y = 0.0
|
||||||
|
center_marker.pose.orientation.z = 0.0
|
||||||
|
center_marker.pose.orientation.w = 1.0
|
||||||
|
self.marker_pub.publish(center_marker)
|
||||||
|
|
||||||
|
# 计算法向量（篮板主方向，PCA最小特征值方向）
|
||||||
|
pts = np.array(rect_lines)
|
||||||
|
mean = np.mean(pts, axis=0)
|
||||||
|
cov = np.cov(pts.T)
|
||||||
|
eigvals, eigvecs = np.linalg.eigh(cov)
|
||||||
|
order = np.argsort(eigvals)[::-1]
|
||||||
|
normal_vec = eigvecs[:, order[2]]
|
||||||
|
normal_vec = normal_vec / np.linalg.norm(normal_vec)
|
||||||
|
# 向内侧偏移30cm
|
||||||
|
offset_point = stable_center + 0.3 * normal_vec
|
||||||
|
|
||||||
|
# 发布偏移点Marker
|
||||||
|
offset_marker = Marker()
|
||||||
|
offset_marker.header = msg.header
|
||||||
|
offset_marker.ns = "basketball_frame"
|
||||||
|
offset_marker.id = 2
|
||||||
|
offset_marker.type = Marker.SPHERE
|
||||||
|
offset_marker.action = Marker.ADD
|
||||||
|
offset_marker.scale.x = 0.12
|
||||||
|
offset_marker.scale.y = 0.12
|
||||||
|
offset_marker.scale.z = 0.12
|
||||||
|
offset_marker.color.r = 0.0
|
||||||
|
offset_marker.color.g = 0.0
|
||||||
|
offset_marker.color.b = 1.0
|
||||||
|
offset_marker.color.a = 1.0
|
||||||
|
offset_marker.pose.position.x = float(offset_point[0])
|
||||||
|
offset_marker.pose.position.y = float(offset_point[1])
|
||||||
|
offset_marker.pose.position.z = float(offset_point[2])
|
||||||
|
offset_marker.pose.orientation.x = 0.0
|
||||||
|
offset_marker.pose.orientation.y = 0.0
|
||||||
|
offset_marker.pose.orientation.z = 0.0
|
||||||
|
offset_marker.pose.orientation.w = 1.0
|
||||||
|
self.marker_pub.publish(offset_marker)
|
||||||
|
|
||||||
|
found = True
|
||||||
|
break
|
||||||
|
if not found:
|
||||||
|
self.get_logger().info('本帧未找到矩形')
|
||||||
|
|
||||||
|
def pointcloud2_to_xyz(self, cloud_msg):
|
||||||
|
fmt = 'ffff'
|
||||||
|
points = []
|
||||||
|
for i in range(cloud_msg.width):
|
||||||
|
offset = i * cloud_msg.point_step
|
||||||
|
x, y, z, intensity = struct.unpack_from(fmt, cloud_msg.data, offset)
|
||||||
|
points.append([x, y, z, intensity])
|
||||||
|
return np.array(points)
|
||||||
|
|
||||||
|
def xyz_array_to_pointcloud2(self, points, header):
|
||||||
|
msg = PointCloud2()
|
||||||
|
msg.header = header
|
||||||
|
msg.height = 1
|
||||||
|
msg.width = len(points)
|
||||||
|
msg.is_dense = True
|
||||||
|
msg.is_bigendian = False
|
||||||
|
msg.point_step = 16
|
||||||
|
msg.row_step = msg.point_step * msg.width
|
||||||
|
msg.fields = [
|
||||||
|
PointField(name='x', offset=0, datatype=7, count=1),
|
||||||
|
PointField(name='y', offset=4, datatype=7, count=1),
|
||||||
|
PointField(name='z', offset=8, datatype=7, count=1),
|
||||||
|
PointField(name='intensity', offset=12, datatype=7, count=1),
|
||||||
|
]
|
||||||
|
msg.data = b''.join([struct.pack('ffff', *p) for p in points])
|
||||||
|
return msg
|
||||||
|
|
||||||
|
def main(args=None):
|
||||||
|
rclpy.init(args=args)
|
||||||
|
node = BasketballFrameDetector()
|
||||||
|
rclpy.spin(node)
|
||||||
|
node.destroy_node()
|
||||||
|
rclpy.shutdown()
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
main()
|
||||||
0
src/rc_lidar/line.py
Normal file
0
src/rc_lidar/line.py
Normal file
75
src/rc_lidar/pcd2pgm.py
Normal file
75
src/rc_lidar/pcd2pgm.py
Normal file
@ -0,0 +1,75 @@
|
|||||||
|
import rclpy
|
||||||
|
from rclpy.node import Node
|
||||||
|
from sensor_msgs.msg import PointCloud2
|
||||||
|
from nav_msgs.msg import OccupancyGrid
|
||||||
|
import numpy as np
|
||||||
|
import struct
|
||||||
|
import time
|
||||||
|
|
||||||
|
class PointCloudToGrid(Node):
|
||||||
|
def __init__(self):
|
||||||
|
super().__init__('pointcloud_to_grid')
|
||||||
|
self.subscription = self.create_subscription(
|
||||||
|
PointCloud2,
|
||||||
|
'/livox/lidar_filtered',
|
||||||
|
self.pointcloud_callback,
|
||||||
|
10)
|
||||||
|
self.publisher = self.create_publisher(OccupancyGrid, '/lidar_grid', 10)
|
||||||
|
self.grid_size = 2000
|
||||||
|
self.resolution = 0.02
|
||||||
|
self.origin_x = -20.0
|
||||||
|
self.origin_y = -20.0
|
||||||
|
self.points_buffer = []
|
||||||
|
self.last_header = None
|
||||||
|
# 定时器每0.5秒触发一次
|
||||||
|
self.timer = self.create_timer(0.5, self.publish_grid)
|
||||||
|
|
||||||
|
def pointcloud_callback(self, msg):
|
||||||
|
points = self.pointcloud2_to_xyz_array(msg)
|
||||||
|
self.points_buffer.append(points)
|
||||||
|
self.last_header = msg.header # 保存最新header用于地图消息
|
||||||
|
|
||||||
|
def publish_grid(self):
|
||||||
|
if not self.points_buffer:
|
||||||
|
return
|
||||||
|
# 合并0.5秒内所有点
|
||||||
|
all_points = np.concatenate(self.points_buffer, axis=0)
|
||||||
|
grid = np.zeros((self.grid_size, self.grid_size), dtype=np.int8)
|
||||||
|
for x, y, z in all_points:
|
||||||
|
if z < 2.0:
|
||||||
|
ix = int((x - self.origin_x) / self.resolution)
|
||||||
|
iy = int((y - self.origin_y) / self.resolution)
|
||||||
|
if 0 <= ix < self.grid_size and 0 <= iy < self.grid_size:
|
||||||
|
grid[iy, ix] = 100
|
||||||
|
grid_msg = OccupancyGrid()
|
||||||
|
if self.last_header:
|
||||||
|
grid_msg.header = self.last_header
|
||||||
|
grid_msg.info.resolution = self.resolution
|
||||||
|
grid_msg.info.width = self.grid_size
|
||||||
|
grid_msg.info.height = self.grid_size
|
||||||
|
grid_msg.info.origin.position.x = self.origin_x
|
||||||
|
grid_msg.info.origin.position.y = self.origin_y
|
||||||
|
grid_msg.data = grid.flatten().tolist()
|
||||||
|
self.publisher.publish(grid_msg)
|
||||||
|
self.points_buffer.clear() # 清空缓存
|
||||||
|
|
||||||
|
def pointcloud2_to_xyz_array(self, cloud_msg):
|
||||||
|
# 解析 PointCloud2 数据为 numpy 数组
|
||||||
|
fmt = 'fff' # x, y, z
|
||||||
|
point_step = cloud_msg.point_step
|
||||||
|
data = cloud_msg.data
|
||||||
|
points = []
|
||||||
|
for i in range(0, len(data), point_step):
|
||||||
|
x, y, z = struct.unpack_from(fmt, data, i)
|
||||||
|
points.append([x, y, z])
|
||||||
|
return np.array(points)
|
||||||
|
|
||||||
|
def main(args=None):
|
||||||
|
rclpy.init(args=args)
|
||||||
|
node = PointCloudToGrid()
|
||||||
|
rclpy.spin(node)
|
||||||
|
node.destroy_node()
|
||||||
|
rclpy.shutdown()
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
main()
|
||||||
55
src/rc_lidar/save_pcd.py
Normal file
55
src/rc_lidar/save_pcd.py
Normal file
@ -0,0 +1,55 @@
|
|||||||
|
import rclpy
|
||||||
|
from rclpy.node import Node
|
||||||
|
from sensor_msgs.msg import PointCloud2
|
||||||
|
import numpy as np
|
||||||
|
import open3d as o3d
|
||||||
|
import time
|
||||||
|
|
||||||
|
class PointCloudSaver(Node):
|
||||||
|
def __init__(self):
|
||||||
|
super().__init__('pcd_saver')
|
||||||
|
self.subscription = self.create_subscription(
|
||||||
|
PointCloud2,
|
||||||
|
'/livox/lidar_filtered',
|
||||||
|
self.listener_callback,
|
||||||
|
10)
|
||||||
|
self.point_clouds = []
|
||||||
|
self.start_time = time.time()
|
||||||
|
self.timer = self.create_timer(3.0, self.save_and_exit)
|
||||||
|
self.saving = False
|
||||||
|
|
||||||
|
def listener_callback(self, msg):
|
||||||
|
if not self.saving:
|
||||||
|
pc = self.pointcloud2_to_xyz_array(msg)
|
||||||
|
if pc is not None:
|
||||||
|
self.point_clouds.append(pc)
|
||||||
|
|
||||||
|
def pointcloud2_to_xyz_array(self, cloud_msg):
|
||||||
|
# 仅支持 x, y, z 均为 float32 且无 padding 的点云
|
||||||
|
import struct
|
||||||
|
points = []
|
||||||
|
point_step = cloud_msg.point_step
|
||||||
|
for i in range(cloud_msg.width * cloud_msg.height):
|
||||||
|
offset = i * point_step
|
||||||
|
x, y, z = struct.unpack_from('fff', cloud_msg.data, offset)
|
||||||
|
points.append([x, y, z])
|
||||||
|
return np.array(points)
|
||||||
|
|
||||||
|
def save_and_exit(self):
|
||||||
|
if not self.saving:
|
||||||
|
self.saving = True
|
||||||
|
if self.point_clouds:
|
||||||
|
all_points = np.vstack(self.point_clouds)
|
||||||
|
pcd = o3d.geometry.PointCloud()
|
||||||
|
pcd.points = o3d.utility.Vector3dVector(all_points)
|
||||||
|
o3d.io.write_point_cloud("output.pcd", pcd)
|
||||||
|
self.get_logger().info("Saved output.pcd")
|
||||||
|
rclpy.shutdown()
|
||||||
|
|
||||||
|
def main(args=None):
|
||||||
|
rclpy.init(args=args)
|
||||||
|
saver = PointCloudSaver()
|
||||||
|
rclpy.spin(saver)
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
main()
|
||||||
102
src/rc_lidar/simple.py
Normal file
102
src/rc_lidar/simple.py
Normal file
@ -0,0 +1,102 @@
|
|||||||
|
import rclpy
|
||||||
|
from rclpy.node import Node
|
||||||
|
from sensor_msgs.msg import PointCloud2, PointField
|
||||||
|
import numpy as np
|
||||||
|
import struct
|
||||||
|
from sklearn.cluster import DBSCAN
|
||||||
|
|
||||||
|
class BasketballFrameDetector(Node):
|
||||||
|
def __init__(self):
|
||||||
|
super().__init__('basketball_frame_detector')
|
||||||
|
self.subscription = self.create_subscription(
|
||||||
|
PointCloud2,
|
||||||
|
'/livox/lidar_filtered',
|
||||||
|
self.pointcloud_callback,
|
||||||
|
10
|
||||||
|
)
|
||||||
|
self.publisher = self.create_publisher(
|
||||||
|
PointCloud2,
|
||||||
|
'/basketball_frame_cloud',
|
||||||
|
10
|
||||||
|
)
|
||||||
|
self.pointcloud_buffer = []
|
||||||
|
|
||||||
|
def pointcloud_callback(self, msg):
|
||||||
|
points = self.pointcloud2_to_xyz(msg)
|
||||||
|
self.pointcloud_buffer.append(points)
|
||||||
|
self.get_logger().info(f'已保存点云组数: {len(self.pointcloud_buffer)}')
|
||||||
|
if len(self.pointcloud_buffer) < 10:
|
||||||
|
return
|
||||||
|
|
||||||
|
# 合并10组点云
|
||||||
|
all_points = np.vstack(self.pointcloud_buffer)
|
||||||
|
xy_points = all_points[:, :2]
|
||||||
|
self.get_logger().info(f'合并后点数: {xy_points.shape[0]}')
|
||||||
|
|
||||||
|
# 清空缓存，准备下一批
|
||||||
|
self.pointcloud_buffer = []
|
||||||
|
|
||||||
|
# 聚类识别
|
||||||
|
if len(xy_points) < 10:
|
||||||
|
return
|
||||||
|
clustering = DBSCAN(eps=0.3, min_samples=10).fit(xy_points)
|
||||||
|
labels = clustering.labels_
|
||||||
|
unique_labels = set(labels)
|
||||||
|
for label in unique_labels:
|
||||||
|
if label == -1:
|
||||||
|
continue
|
||||||
|
cluster = all_points[labels == label]
|
||||||
|
if len(cluster) < 30:
|
||||||
|
continue
|
||||||
|
min_x, min_y = np.min(cluster[:, :2], axis=0)
|
||||||
|
max_x, max_y = np.max(cluster[:, :2], axis=0)
|
||||||
|
width = abs(max_x - min_x)
|
||||||
|
height = abs(max_y - min_y)
|
||||||
|
self.get_logger().info(
|
||||||
|
f'聚类: label={label}, width={width:.2f}, height={height:.2f}, 点数={len(cluster)}'
|
||||||
|
)
|
||||||
|
if 1.6 < width < 2.0 and 0.8 < height < 1.2:
|
||||||
|
self.get_logger().info(
|
||||||
|
f'可能是篮球框: label={label}, width={width:.2f}, height={height:.2f}, 点数={len(cluster)}'
|
||||||
|
)
|
||||||
|
# 发布识别到的篮球框点云
|
||||||
|
cloud_msg = self.xyz_array_to_pointcloud2(cluster, msg.header)
|
||||||
|
self.publisher.publish(cloud_msg)
|
||||||
|
|
||||||
|
def pointcloud2_to_xyz(self, cloud_msg):
|
||||||
|
fmt = 'ffff' # x, y, z, intensity
|
||||||
|
points = []
|
||||||
|
for i in range(cloud_msg.width):
|
||||||
|
offset = i * cloud_msg.point_step
|
||||||
|
x, y, z, intensity = struct.unpack_from(fmt, cloud_msg.data, offset)
|
||||||
|
points.append([x, y, z, intensity])
|
||||||
|
return np.array(points)
|
||||||
|
|
||||||
|
def xyz_array_to_pointcloud2(self, points, header):
|
||||||
|
# 构造 PointCloud2 消息
|
||||||
|
msg = PointCloud2()
|
||||||
|
msg.header = header
|
||||||
|
msg.height = 1
|
||||||
|
msg.width = len(points)
|
||||||
|
msg.is_dense = True
|
||||||
|
msg.is_bigendian = False
|
||||||
|
msg.point_step = 16
|
||||||
|
msg.row_step = msg.point_step * msg.width
|
||||||
|
msg.fields = [
|
||||||
|
PointField(name='x', offset=0, datatype=7, count=1),
|
||||||
|
PointField(name='y', offset=4, datatype=7, count=1),
|
||||||
|
PointField(name='z', offset=8, datatype=7, count=1),
|
||||||
|
PointField(name='intensity', offset=12, datatype=7, count=1),
|
||||||
|
]
|
||||||
|
msg.data = b''.join([struct.pack('ffff', *p) for p in points])
|
||||||
|
return msg
|
||||||
|
|
||||||
|
def main(args=None):
|
||||||
|
rclpy.init(args=args)
|
||||||
|
node = BasketballFrameDetector()
|
||||||
|
rclpy.spin(node)
|
||||||
|
node.destroy_node()
|
||||||
|
rclpy.shutdown()
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
main()
|
||||||
126
src/rc_lidar/simple_icp.py
Normal file
126
src/rc_lidar/simple_icp.py
Normal file
@ -0,0 +1,126 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
import rclpy
|
||||||
|
from rclpy.node import Node
|
||||||
|
from sensor_msgs.msg import PointCloud2
|
||||||
|
from sensor_msgs_py import point_cloud2
|
||||||
|
from geometry_msgs.msg import PoseStamped
|
||||||
|
import open3d as o3d
|
||||||
|
import numpy as np
|
||||||
|
from transforms3d.quaternions import mat2quat
|
||||||
|
|
||||||
|
class PointCloudLocalization(Node):
|
||||||
|
def __init__(self):
|
||||||
|
super().__init__('point_cloud_localizer')
|
||||||
|
|
||||||
|
# 加载参考点云地图 (PCD文件)
|
||||||
|
self.reference_map = o3d.io.read_point_cloud("/home/robofish/RC2025/lankuang.pcd") # 替换为你的PCD文件路径
|
||||||
|
if not self.reference_map.has_points():
|
||||||
|
self.get_logger().error("Failed to load reference map!")
|
||||||
|
rclpy.shutdown()
|
||||||
|
|
||||||
|
# 预处理参考地图
|
||||||
|
self.reference_map = self.reference_map.voxel_down_sample(voxel_size=0.05)
|
||||||
|
self.reference_map.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0)[0]
|
||||||
|
|
||||||
|
# 创建ICP对象
|
||||||
|
self.icp = o3d.pipelines.registration.registration_icp
|
||||||
|
self.threshold = 0.5 # 匹配距离阈值 (米)
|
||||||
|
self.trans_init = np.identity(4) # 初始变换矩阵
|
||||||
|
|
||||||
|
# 订阅激光雷达点云
|
||||||
|
self.subscription = self.create_subscription(
|
||||||
|
PointCloud2,
|
||||||
|
'/livox/lidar_filtered',
|
||||||
|
self.lidar_callback,
|
||||||
|
10)
|
||||||
|
|
||||||
|
# 发布估计位置
|
||||||
|
self.pose_pub = self.create_publisher(PoseStamped, '/estimated_pose', 10)
|
||||||
|
|
||||||
|
self.get_logger().info("Point Cloud Localization Node Initialized!")
|
||||||
|
|
||||||
|
def ros_pc2_to_o3d(self, ros_cloud):
|
||||||
|
"""将ROS PointCloud2转换为Open3D点云"""
|
||||||
|
# 提取xyz坐标
|
||||||
|
points = point_cloud2.read_points(ros_cloud, field_names=("x", "y", "z"), skip_nans=True)
|
||||||
|
xyz = np.array([ [p[0], p[1], p[2]] for p in points ], dtype=np.float32)
|
||||||
|
|
||||||
|
if xyz.shape[0] == 0:
|
||||||
|
return None
|
||||||
|
|
||||||
|
# 创建Open3D点云
|
||||||
|
pcd = o3d.geometry.PointCloud()
|
||||||
|
pcd.points = o3d.utility.Vector3dVector(xyz)
|
||||||
|
return pcd
|
||||||
|
|
||||||
|
def preprocess_pointcloud(self, pcd):
|
||||||
|
"""点云预处理"""
|
||||||
|
# 降采样
|
||||||
|
pcd = pcd.voxel_down_sample(voxel_size=0.03)
|
||||||
|
|
||||||
|
# 移除离群点
|
||||||
|
pcd, _ = pcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=1.0)
|
||||||
|
|
||||||
|
# 移除地面 (可选)
|
||||||
|
# plane_model, inliers = pcd.segment_plane(distance_threshold=0.1, ransac_n=3, num_iterations=100)
|
||||||
|
# pcd = pcd.select_by_index(inliers, invert=True)
|
||||||
|
|
||||||
|
return pcd
|
||||||
|
|
||||||
|
def lidar_callback(self, msg):
|
||||||
|
"""处理新的激光雷达数据"""
|
||||||
|
# 转换为Open3D格式
|
||||||
|
current_pcd = self.ros_pc2_to_o3d(msg)
|
||||||
|
if current_pcd is None:
|
||||||
|
self.get_logger().warn("Received empty point cloud!")
|
||||||
|
return
|
||||||
|
|
||||||
|
# 预处理当前点云
|
||||||
|
current_pcd = self.preprocess_pointcloud(current_pcd)
|
||||||
|
|
||||||
|
# 执行ICP配准
|
||||||
|
reg_result = self.icp(
|
||||||
|
current_pcd, self.reference_map, self.threshold,
|
||||||
|
self.trans_init,
|
||||||
|
o3d.pipelines.registration.TransformationEstimationPointToPoint(),
|
||||||
|
o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=50)
|
||||||
|
)
|
||||||
|
|
||||||
|
# 更新变换矩阵
|
||||||
|
self.trans_init = reg_result.transformation
|
||||||
|
|
||||||
|
# 提取位置和方向
|
||||||
|
translation = reg_result.transformation[:3, 3]
|
||||||
|
rotation_matrix = reg_result.transformation[:3, :3]
|
||||||
|
|
||||||
|
# 转换为四元数
|
||||||
|
quaternion = mat2quat(reg_result.transformation[:3, :3]) # 注意返回顺序为 [w, x, y, z]
|
||||||
|
|
||||||
|
# 发布位姿
|
||||||
|
pose_msg = PoseStamped()
|
||||||
|
pose_msg.header.stamp = self.get_clock().now().to_msg()
|
||||||
|
pose_msg.header.frame_id = "livox_frame"
|
||||||
|
pose_msg.pose.position.x = translation[0]
|
||||||
|
pose_msg.pose.position.y = translation[1]
|
||||||
|
pose_msg.pose.position.z = translation[2]
|
||||||
|
pose_msg.pose.orientation.x = quaternion[1] # x
|
||||||
|
pose_msg.pose.orientation.y = quaternion[2] # y
|
||||||
|
pose_msg.pose.orientation.z = quaternion[3] # z
|
||||||
|
pose_msg.pose.orientation.w = quaternion[0] # w
|
||||||
|
|
||||||
|
self.pose_pub.publish(pose_msg)
|
||||||
|
self.get_logger().info(f"Estimated Position: x={translation[0]:.2f}, y={translation[1]:.2f}, z={translation[2]:.2f}")
|
||||||
|
|
||||||
|
def main(args=None):
|
||||||
|
rclpy.init(args=args)
|
||||||
|
node = PointCloudLocalization()
|
||||||
|
try:
|
||||||
|
rclpy.spin(node)
|
||||||
|
except KeyboardInterrupt:
|
||||||
|
pass
|
||||||
|
finally:
|
||||||
|
node.destroy_node()
|
||||||
|
rclpy.shutdown()
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
main()
|
||||||
75
src/rc_lidar/xiamian.py
Normal file
75
src/rc_lidar/xiamian.py
Normal file
@ -0,0 +1,75 @@
|
|||||||
|
import rclpy
|
||||||
|
from rclpy.node import Node
|
||||||
|
from sensor_msgs.msg import PointCloud2
|
||||||
|
from nav_msgs.msg import OccupancyGrid
|
||||||
|
import numpy as np
|
||||||
|
import struct
|
||||||
|
import time
|
||||||
|
|
||||||
|
class PointCloudToGrid(Node):
|
||||||
|
def __init__(self):
|
||||||
|
super().__init__('pointcloud_to_grid')
|
||||||
|
self.subscription = self.create_subscription(
|
||||||
|
PointCloud2,
|
||||||
|
'/livox/lidar_filtered',
|
||||||
|
self.pointcloud_callback,
|
||||||
|
10)
|
||||||
|
self.publisher = self.create_publisher(OccupancyGrid, '/lidar_grid', 10)
|
||||||
|
self.grid_size = 2000
|
||||||
|
self.resolution = 0.02
|
||||||
|
self.origin_x = -20.0
|
||||||
|
self.origin_y = -20.0
|
||||||
|
self.points_buffer = []
|
||||||
|
self.last_header = None
|
||||||
|
# 定时器每0.5秒触发一次
|
||||||
|
self.timer = self.create_timer(0.5, self.publish_grid)
|
||||||
|
|
||||||
|
def pointcloud_callback(self, msg):
|
||||||
|
points = self.pointcloud2_to_xyz_array(msg)
|
||||||
|
self.points_buffer.append(points)
|
||||||
|
self.last_header = msg.header # 保存最新header用于地图消息
|
||||||
|
|
||||||
|
def publish_grid(self):
|
||||||
|
if not self.points_buffer:
|
||||||
|
return
|
||||||
|
# 合并0.5秒内所有点
|
||||||
|
all_points = np.concatenate(self.points_buffer, axis=0)
|
||||||
|
grid = np.zeros((self.grid_size, self.grid_size), dtype=np.int8)
|
||||||
|
for x, y, z in all_points:
|
||||||
|
if z < 2.0:
|
||||||
|
ix = int((x - self.origin_x) / self.resolution)
|
||||||
|
iy = int((y - self.origin_y) / self.resolution)
|
||||||
|
if 0 <= ix < self.grid_size and 0 <= iy < self.grid_size:
|
||||||
|
grid[iy, ix] = 100
|
||||||
|
grid_msg = OccupancyGrid()
|
||||||
|
if self.last_header:
|
||||||
|
grid_msg.header = self.last_header
|
||||||
|
grid_msg.info.resolution = self.resolution
|
||||||
|
grid_msg.info.width = self.grid_size
|
||||||
|
grid_msg.info.height = self.grid_size
|
||||||
|
grid_msg.info.origin.position.x = self.origin_x
|
||||||
|
grid_msg.info.origin.position.y = self.origin_y
|
||||||
|
grid_msg.data = grid.flatten().tolist()
|
||||||
|
self.publisher.publish(grid_msg)
|
||||||
|
self.points_buffer.clear() # 清空缓存
|
||||||
|
|
||||||
|
def pointcloud2_to_xyz_array(self, cloud_msg):
|
||||||
|
# 解析 PointCloud2 数据为 numpy 数组
|
||||||
|
fmt = 'fff' # x, y, z
|
||||||
|
point_step = cloud_msg.point_step
|
||||||
|
data = cloud_msg.data
|
||||||
|
points = []
|
||||||
|
for i in range(0, len(data), point_step):
|
||||||
|
x, y, z = struct.unpack_from(fmt, data, i)
|
||||||
|
points.append([x, y, z])
|
||||||
|
return np.array(points)
|
||||||
|
|
||||||
|
def main(args=None):
|
||||||
|
rclpy.init(args=args)
|
||||||
|
node = PointCloudToGrid()
|
||||||
|
rclpy.spin(node)
|
||||||
|
node.destroy_node()
|
||||||
|
rclpy.shutdown()
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
main()
|
||||||
51
src/rc_lidar/zhaoban.py
Normal file
51
src/rc_lidar/zhaoban.py
Normal file
@ -0,0 +1,51 @@
|
|||||||
|
import numpy as np
|
||||||
|
from sklearn.cluster import DBSCAN
|
||||||
|
from scipy.optimize import leastsq
|
||||||
|
import matplotlib.pyplot as plt
|
||||||
|
|
||||||
|
def fit_circle(x, y):
|
||||||
|
# 拟合圆的函数
|
||||||
|
def calc_R(xc, yc):
|
||||||
|
return np.sqrt((x - xc)**2 + (y - yc)**2)
|
||||||
|
def f(c):
|
||||||
|
Ri = calc_R(*c)
|
||||||
|
return Ri - Ri.mean()
|
||||||
|
center_estimate = np.mean(x), np.mean(y)
|
||||||
|
center, _ = leastsq(f, center_estimate)
|
||||||
|
radius = calc_R(*center).mean()
|
||||||
|
return center[0], center[1], radius
|
||||||
|
|
||||||
|
def find_circle(points, eps=0.5, min_samples=10):
|
||||||
|
# 聚类
|
||||||
|
clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(points)
|
||||||
|
labels = clustering.labels_
|
||||||
|
# 只取最大簇
|
||||||
|
unique, counts = np.unique(labels[labels != -1], return_counts=True)
|
||||||
|
if len(unique) == 0:
|
||||||
|
raise ValueError("未找到有效聚类")
|
||||||
|
main_cluster = unique[np.argmax(counts)]
|
||||||
|
cluster_points = points[labels == main_cluster]
|
||||||
|
x, y = cluster_points[:, 0], cluster_points[:, 1]
|
||||||
|
# 拟合圆
|
||||||
|
xc, yc, r = fit_circle(x, y)
|
||||||
|
return xc, yc, r, cluster_points
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
# 示例数据
|
||||||
|
np.random.seed(0)
|
||||||
|
angle = np.linspace(0, 2 * np.pi, 100)
|
||||||
|
x = 5 + 3 * np.cos(angle) + np.random.normal(0, 0.1, 100)
|
||||||
|
y = 2 + 3 * np.sin(angle) + np.random.normal(0, 0.1, 100)
|
||||||
|
points = np.vstack((x, y)).T
|
||||||
|
|
||||||
|
xc, yc, r, cluster_points = find_circle(points)
|
||||||
|
print(f"圆心: ({xc:.2f}, {yc:.2f}), 半径: {r:.2f}")
|
||||||
|
|
||||||
|
# 可视化
|
||||||
|
plt.scatter(points[:, 0], points[:, 1], label='所有点')
|
||||||
|
plt.scatter(cluster_points[:, 0], cluster_points[:, 1], label='聚类点')
|
||||||
|
circle = plt.Circle((xc, yc), r, color='r', fill=False, label='拟合圆')
|
||||||
|
plt.gca().add_patch(circle)
|
||||||
|
plt.legend()
|
||||||
|
plt.axis('equal')
|
||||||
|
plt.show()
|
||||||
0
src/rc_lidar/zhaoyuan.py
Normal file
0
src/rc_lidar/zhaoyuan.py
Normal file
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
|
|
||||||
|
|||||||
@ -327,3 +327,7 @@ Please add '/usr/local/lib' to the env LD_LIBRARY_PATH.
|
|||||||
export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:/usr/local/lib
|
export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:/usr/local/lib
|
||||||
source ~/.bashrc
|
source ~/.bashrc
|
||||||
```
|
```
|
||||||
|
|
||||||
|
KERNELS=="3-2:1.0", SUBSYSTEMS=="usb", MODE:="0666", SYMLINK+="underpan"
|
||||||
|
KERNELS=="3-1:1.0", SUBSYSTEMS=="usb", MODE:="0666", SYMLINK+="upper"
|
||||||
|
KERNELS=="3-2:1.0", SUBSYSTEMS=="usb", MODE:="0666", SYMLINK+="r2"
|
||||||
@ -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" : {
|
||||||
|
|||||||
121
src/rm_driver/rm_serial_driver/script/R2_Serial.py
Normal file
121
src/rm_driver/rm_serial_driver/script/R2_Serial.py
Normal file
@ -0,0 +1,121 @@
|
|||||||
|
import rclpy
|
||||||
|
from rclpy.node import Node
|
||||||
|
import serial
|
||||||
|
import struct
|
||||||
|
from rm_msgs.msg import DataAim
|
||||||
|
|
||||||
|
class R2SerialNode(Node):
|
||||||
|
def __init__(self):
|
||||||
|
super().__init__('r2_serial_node')
|
||||||
|
|
||||||
|
# 声明参数
|
||||||
|
self.declare_parameter('yaw_port', '/dev/ttyUSB0')
|
||||||
|
self.declare_parameter('distance_port', '/dev/ttyUSB1')
|
||||||
|
self.declare_parameter('baud_rate', 115200)
|
||||||
|
|
||||||
|
# 获取参数
|
||||||
|
self.yaw_port = self.get_parameter('yaw_port').get_parameter_value().string_value
|
||||||
|
self.distance_port = self.get_parameter('distance_port').get_parameter_value().string_value
|
||||||
|
self.baud_rate = self.get_parameter('baud_rate').get_parameter_value().integer_value
|
||||||
|
|
||||||
|
# 创建定时器
|
||||||
|
# self.timer = self.create_timer(0.01, self.send_data) # 每100ms发送一次数据
|
||||||
|
# 初始化串口
|
||||||
|
try:
|
||||||
|
self.yaw_serial = serial.Serial(self.yaw_port, self.baud_rate, timeout=1)
|
||||||
|
self.get_logger().info(f"Yaw serial port {self.yaw_port} opened successfully")
|
||||||
|
except Exception as e:
|
||||||
|
self.get_logger().error(f"Failed to open yaw serial port: {e}")
|
||||||
|
self.yaw_serial = None
|
||||||
|
|
||||||
|
try:
|
||||||
|
self.distance_serial = serial.Serial(self.distance_port, self.baud_rate, timeout=1)
|
||||||
|
self.get_logger().info(f"Distance serial port {self.distance_port} opened successfully")
|
||||||
|
except Exception as e:
|
||||||
|
self.get_logger().error(f"Failed to open distance serial port: {e}")
|
||||||
|
self.distance_serial = None
|
||||||
|
|
||||||
|
# 数据存储
|
||||||
|
self.yaw1 = 0.0
|
||||||
|
self.yaw2 = 0.0
|
||||||
|
self.distance1 = 0.0
|
||||||
|
self.distance2 = 0.0
|
||||||
|
|
||||||
|
# 订阅话题
|
||||||
|
self.sub1 = self.create_subscription(
|
||||||
|
DataAim,
|
||||||
|
'/chassis/data_aim',
|
||||||
|
self.data_aim_callback,
|
||||||
|
10
|
||||||
|
)
|
||||||
|
self.sub2 = self.create_subscription(
|
||||||
|
DataAim,
|
||||||
|
'/data_aim_r2',
|
||||||
|
self.data_aim_r2_callback,
|
||||||
|
10
|
||||||
|
)
|
||||||
|
|
||||||
|
self.get_logger().info("R2 Serial Node initialized")
|
||||||
|
|
||||||
|
def data_aim_callback(self, msg):
|
||||||
|
"""处理/data_aim话题的回调"""
|
||||||
|
self.yaw1 = msg.yaw
|
||||||
|
self.distance1 = msg.distance
|
||||||
|
self.get_logger().debug(f"Received data_aim: yaw={self.yaw1}, distance={self.distance1}")
|
||||||
|
self.send_data()
|
||||||
|
|
||||||
|
def data_aim_r2_callback(self, msg):
|
||||||
|
"""处理/data_aim_r2话题的回调"""
|
||||||
|
self.yaw2 = msg.yaw
|
||||||
|
self.distance2 = msg.distance
|
||||||
|
self.get_logger().debug(f"Received data_aim_r2: yaw={self.yaw2}, distance={self.distance2}")
|
||||||
|
self.send_data()
|
||||||
|
|
||||||
|
def send_data(self):
|
||||||
|
"""发送数据到串口"""
|
||||||
|
# 打包所有数据：包头FF + yaw1 + distance1 + yaw2 + distance2 + 包尾FE
|
||||||
|
try:
|
||||||
|
# 包头FF + 四个float32值 + 包尾FE
|
||||||
|
all_data = struct.pack('<ffff', self.yaw1, self.distance1, self.yaw2, self.distance2) # 小端序
|
||||||
|
data_packet = b'\xFF' + all_data + b'\xFE'
|
||||||
|
|
||||||
|
# 通过yaw串口发送
|
||||||
|
if self.yaw_serial and self.yaw_serial.is_open:
|
||||||
|
self.yaw_serial.write(data_packet)
|
||||||
|
print(f"Sent yaw data: {self.yaw1}, {self.yaw2}")
|
||||||
|
|
||||||
|
# 通过distance串口发送相同数据
|
||||||
|
if self.distance_serial and self.distance_serial.is_open:
|
||||||
|
self.distance_serial.write(data_packet)
|
||||||
|
print(f"Sent distance data: {self.distance1}, {self.distance2}")
|
||||||
|
|
||||||
|
except Exception as e:
|
||||||
|
self.get_logger().error(f"Failed to send data: {e}")
|
||||||
|
|
||||||
|
def __del__(self):
|
||||||
|
"""析构函数，关闭串口"""
|
||||||
|
if hasattr(self, 'yaw_serial') and self.yaw_serial and self.yaw_serial.is_open:
|
||||||
|
self.yaw_serial.close()
|
||||||
|
self.get_logger().info("Yaw serial port closed")
|
||||||
|
|
||||||
|
if hasattr(self, 'distance_serial') and self.distance_serial and self.distance_serial.is_open:
|
||||||
|
self.distance_serial.close()
|
||||||
|
self.get_logger().info("Distance serial port closed")
|
||||||
|
|
||||||
|
def main(args=None):
|
||||||
|
rclpy.init(args=args)
|
||||||
|
|
||||||
|
try:
|
||||||
|
node = R2SerialNode()
|
||||||
|
rclpy.spin(node)
|
||||||
|
except KeyboardInterrupt:
|
||||||
|
pass
|
||||||
|
except Exception as e:
|
||||||
|
print(f"Error in main: {e}")
|
||||||
|
finally:
|
||||||
|
if 'node' in locals():
|
||||||
|
node.destroy_node()
|
||||||
|
rclpy.shutdown()
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
main()
|
||||||
@ -1,79 +0,0 @@
|
|||||||
#!/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()
|
|
||||||
@ -1,79 +0,0 @@
|
|||||||
#!/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()
|
|
||||||
36
src/rm_driver/rm_serial_driver/script/pub_goal.py
Executable file
36
src/rm_driver/rm_serial_driver/script/pub_goal.py
Executable file
@ -0,0 +1,36 @@
|
|||||||
|
import rclpy
|
||||||
|
from rclpy.node import Node
|
||||||
|
from geometry_msgs.msg import PointStamped
|
||||||
|
from rm_msgs.msg import MoveGoal
|
||||||
|
|
||||||
|
class ClickedGoalPublisher(Node):
|
||||||
|
def __init__(self):
|
||||||
|
super().__init__('clicked_goal_publisher')
|
||||||
|
self.subscription = self.create_subscription(
|
||||||
|
PointStamped,
|
||||||
|
'/clicked_point',
|
||||||
|
self.clicked_callback,
|
||||||
|
10
|
||||||
|
)
|
||||||
|
self.publisher = self.create_publisher(MoveGoal, '/move_goal', 10)
|
||||||
|
|
||||||
|
def clicked_callback(self, msg):
|
||||||
|
goal = MoveGoal()
|
||||||
|
goal.x = msg.point.x
|
||||||
|
goal.y = msg.point.y
|
||||||
|
goal.angle = 0.0
|
||||||
|
goal.max_speed = 0.0
|
||||||
|
goal.tolerance = 0.1
|
||||||
|
goal.rotor = False
|
||||||
|
self.publisher.publish(goal)
|
||||||
|
self.get_logger().info(f'发布目标: x={goal.x:.2f}, y={goal.y:.2f}')
|
||||||
|
|
||||||
|
def main(args=None):
|
||||||
|
rclpy.init(args=args)
|
||||||
|
node = ClickedGoalPublisher()
|
||||||
|
rclpy.spin(node)
|
||||||
|
node.destroy_node()
|
||||||
|
rclpy.shutdown()
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
main()
|
||||||
@ -1,101 +0,0 @@
|
|||||||
#!/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()
|
|
||||||
169
src/rm_driver/rm_serial_driver/script/receive.py
Normal file
169
src/rm_driver/rm_serial_driver/script/receive.py
Normal file
@ -0,0 +1,169 @@
|
|||||||
|
import serial
|
||||||
|
import struct
|
||||||
|
import rclpy
|
||||||
|
from rclpy.node import Node
|
||||||
|
from geometry_msgs.msg import PointStamped, TransformStamped
|
||||||
|
from rm_msgs.msg import DataAim
|
||||||
|
import tf2_ros
|
||||||
|
import tf2_geometry_msgs
|
||||||
|
import math
|
||||||
|
|
||||||
|
# 串口参数
|
||||||
|
SERIAL_PORT = '/dev/ttyACM0' # 根据实际情况修改
|
||||||
|
BAUDRATE = 115200
|
||||||
|
|
||||||
|
# 数据包格式
|
||||||
|
PACKET_FORMAT = '<BfffB' # 1字节int8头，3个float，1字节int8尾
|
||||||
|
PACKET_SIZE = struct.calcsize(PACKET_FORMAT)
|
||||||
|
HEADER = 0xAA
|
||||||
|
TAIL = 0xBB
|
||||||
|
|
||||||
|
class SerialReceiver(Node):
|
||||||
|
def __init__(self):
|
||||||
|
super().__init__('serial_receiver')
|
||||||
|
|
||||||
|
# 创建发布者
|
||||||
|
self.point_publisher = self.create_publisher(PointStamped, 'r2_map_point', 10)
|
||||||
|
self.data_aim_publisher = self.create_publisher(DataAim, 'data_aim_r2', 10)
|
||||||
|
|
||||||
|
# 创建TF监听器和广播器
|
||||||
|
self.tf_buffer = tf2_ros.Buffer()
|
||||||
|
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
|
||||||
|
self.tf_broadcaster = tf2_ros.TransformBroadcaster(self)
|
||||||
|
|
||||||
|
# 串口初始化
|
||||||
|
self.ser = serial.Serial(SERIAL_PORT, BAUDRATE, timeout=1)
|
||||||
|
self.get_logger().info(f"打开串口 {SERIAL_PORT}，波特率 {BAUDRATE}")
|
||||||
|
|
||||||
|
# 创建定时器用于读取串口数据
|
||||||
|
self.timer = self.create_timer(0.001, self.read_serial_data) # 10ms
|
||||||
|
|
||||||
|
self.buffer = bytearray()
|
||||||
|
|
||||||
|
def read_serial_data(self):
|
||||||
|
try:
|
||||||
|
data = self.ser.read(1)
|
||||||
|
if data:
|
||||||
|
self.buffer += data
|
||||||
|
# 保证缓冲区长度不超过一个包
|
||||||
|
if len(self.buffer) > PACKET_SIZE:
|
||||||
|
self.buffer = self.buffer[-PACKET_SIZE:]
|
||||||
|
# 检查是否有完整包
|
||||||
|
if len(self.buffer) >= PACKET_SIZE:
|
||||||
|
# 检查包头和包尾
|
||||||
|
if self.buffer[0] == HEADER and self.buffer[PACKET_SIZE-1] == TAIL:
|
||||||
|
# 解析数据
|
||||||
|
_, x, y, z, _ = struct.unpack(PACKET_FORMAT, self.buffer[:PACKET_SIZE])
|
||||||
|
self.get_logger().info(f"接收到数据: x={x:.3f}, y={y:.3f}, z={z:.3f}")
|
||||||
|
|
||||||
|
# 发布map坐标点
|
||||||
|
self.publish_map_point(x, y, z)
|
||||||
|
|
||||||
|
# 发布R2的TF变换
|
||||||
|
self.publish_r2_tf(x, y, z)
|
||||||
|
|
||||||
|
# 计算相对于base_link的角度和距离
|
||||||
|
self.calculate_and_publish_data_aim(x, y, z)
|
||||||
|
|
||||||
|
self.buffer = self.buffer[PACKET_SIZE:] # 移除已处理数据
|
||||||
|
else:
|
||||||
|
# 移除第一个字节，继续查找包头
|
||||||
|
self.buffer = self.buffer[1:]
|
||||||
|
except Exception as e:
|
||||||
|
self.get_logger().error(f"读取串口数据错误: {e}")
|
||||||
|
|
||||||
|
def publish_map_point(self, x, y, z):
|
||||||
|
"""发布地图坐标点"""
|
||||||
|
point_msg = PointStamped()
|
||||||
|
point_msg.header.frame_id = 'map'
|
||||||
|
point_msg.header.stamp = self.get_clock().now().to_msg()
|
||||||
|
point_msg.point.x = x
|
||||||
|
point_msg.point.y = y
|
||||||
|
point_msg.point.z = z
|
||||||
|
|
||||||
|
self.point_publisher.publish(point_msg)
|
||||||
|
|
||||||
|
def publish_r2_tf(self, x, y, z):
|
||||||
|
"""发布R2的TF变换"""
|
||||||
|
transform = TransformStamped()
|
||||||
|
|
||||||
|
# 设置header
|
||||||
|
transform.header.stamp = self.get_clock().now().to_msg()
|
||||||
|
transform.header.frame_id = 'map'
|
||||||
|
transform.child_frame_id = 'r2_robot'
|
||||||
|
|
||||||
|
# 设置位置
|
||||||
|
transform.transform.translation.x = x
|
||||||
|
transform.transform.translation.y = y
|
||||||
|
transform.transform.translation.z = z
|
||||||
|
|
||||||
|
# 设置旋转（假设R2面向x轴正方向，无旋转）
|
||||||
|
transform.transform.rotation.x = 0.0
|
||||||
|
transform.transform.rotation.y = 0.0
|
||||||
|
transform.transform.rotation.z = 0.0
|
||||||
|
transform.transform.rotation.w = 1.0
|
||||||
|
|
||||||
|
# 发布TF变换
|
||||||
|
self.tf_broadcaster.sendTransform(transform)
|
||||||
|
|
||||||
|
def calculate_and_publish_data_aim(self, x, y, z):
|
||||||
|
"""计算相对于base_link的角度和距离，并发布DataAim消息"""
|
||||||
|
try:
|
||||||
|
# 创建map坐标系下的点
|
||||||
|
point_stamped = PointStamped()
|
||||||
|
point_stamped.header.frame_id = 'map'
|
||||||
|
point_stamped.header.stamp = self.get_clock().now().to_msg()
|
||||||
|
point_stamped.point.x = x
|
||||||
|
point_stamped.point.y = y
|
||||||
|
point_stamped.point.z = z
|
||||||
|
|
||||||
|
# 获取从map到base_link的变换
|
||||||
|
transform = self.tf_buffer.lookup_transform(
|
||||||
|
'base_link', 'map', rclpy.time.Time())
|
||||||
|
|
||||||
|
# 将点变换到base_link坐标系
|
||||||
|
point_base_link = tf2_geometry_msgs.do_transform_point(point_stamped, transform)
|
||||||
|
|
||||||
|
# 计算距离
|
||||||
|
distance = math.sqrt(
|
||||||
|
point_base_link.point.x**2 +
|
||||||
|
point_base_link.point.y**2 +
|
||||||
|
point_base_link.point.z**2
|
||||||
|
)
|
||||||
|
|
||||||
|
# 计算yaw角度（绕z轴旋转角度）
|
||||||
|
yaw = math.atan2(point_base_link.point.y, point_base_link.point.x)
|
||||||
|
|
||||||
|
# 发布DataAim消息
|
||||||
|
data_aim_msg = DataAim()
|
||||||
|
data_aim_msg.yaw = yaw
|
||||||
|
data_aim_msg.distance = distance
|
||||||
|
|
||||||
|
self.data_aim_publisher.publish(data_aim_msg)
|
||||||
|
|
||||||
|
self.get_logger().info(f"发布DataAim: yaw={yaw:.3f} rad, distance={distance:.3f} m")
|
||||||
|
|
||||||
|
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
|
||||||
|
self.get_logger().warn(f"TF变换失败: {e}")
|
||||||
|
|
||||||
|
def destroy_node(self):
|
||||||
|
"""节点销毁时关闭串口"""
|
||||||
|
if hasattr(self, 'ser') and self.ser.is_open:
|
||||||
|
self.ser.close()
|
||||||
|
super().destroy_node()
|
||||||
|
|
||||||
|
def main(args=None):
|
||||||
|
rclpy.init(args=args)
|
||||||
|
|
||||||
|
receiver = SerialReceiver()
|
||||||
|
|
||||||
|
try:
|
||||||
|
rclpy.spin(receiver)
|
||||||
|
except KeyboardInterrupt:
|
||||||
|
print("接收中断，退出程序。")
|
||||||
|
finally:
|
||||||
|
receiver.destroy_node()
|
||||||
|
rclpy.shutdown()
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
main()
|
||||||
@ -1,241 +0,0 @@
|
|||||||
#!/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()
|
|
||||||
65
src/rm_driver/rm_serial_driver/script/slect.py
Normal file
65
src/rm_driver/rm_serial_driver/script/slect.py
Normal file
File diff suppressed because one or more lines are too long
@ -50,7 +50,7 @@ private:
|
|||||||
initial_pose_sub_;
|
initial_pose_sub_;
|
||||||
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr
|
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr
|
||||||
pointcloud_sub_;
|
pointcloud_sub_;
|
||||||
// rclcpp::TimerBase::SharedPtr timer_;
|
rclcpp::TimerBase::SharedPtr timer_;
|
||||||
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
|
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
|
||||||
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
|
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
|
||||||
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
|
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
|
||||||
|
|||||||
@ -3,4 +3,7 @@ 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"
|
||||||
@ -1,2 +1,2 @@
|
|||||||
float32 yaw
|
float32 yaw
|
||||||
float32 distance
|
float32 distance
|
||||||
|
|||||||
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/map1.pcd
Executable file
BIN
src/rm_nav_bringup/PCD/map1.pcd
Executable file
Binary file not shown.
BIN
src/rm_nav_bringup/PCD/map2.pcd
Executable file
BIN
src/rm_nav_bringup/PCD/map2.pcd
Executable 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: "map2"
|
||||||
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.251 -0.1285 0.397\""
|
||||||
rpy: "\"0.0 0.0 0.0\""
|
rpy: "\"0.0 0.0 0.0\""
|
||||||
BIN
src/rm_nav_bringup/map/map1.data
Executable file
BIN
src/rm_nav_bringup/map/map1.data
Executable file
Binary file not shown.
BIN
src/rm_nav_bringup/map/map1.pgm
Executable file
BIN
src/rm_nav_bringup/map/map1.pgm
Executable file
Binary file not shown.
BIN
src/rm_nav_bringup/map/map1.posegraph
Executable file
BIN
src/rm_nav_bringup/map/map1.posegraph
Executable file
Binary file not shown.
7
src/rm_nav_bringup/map/map1.yaml
Executable file
7
src/rm_nav_bringup/map/map1.yaml
Executable file
@ -0,0 +1,7 @@
|
|||||||
|
image: map1.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/map2.data
Executable file
BIN
src/rm_nav_bringup/map/map2.data
Executable file
Binary file not shown.
BIN
src/rm_nav_bringup/map/map2.pgm
Executable file
BIN
src/rm_nav_bringup/map/map2.pgm
Executable file
Binary file not shown.
BIN
src/rm_nav_bringup/map/map2.posegraph
Executable file
BIN
src/rm_nav_bringup/map/map2.posegraph
Executable file
Binary file not shown.
7
src/rm_nav_bringup/map/map2.yaml
Executable file
7
src/rm_nav_bringup/map/map2.yaml
Executable file
@ -0,0 +1,7 @@
|
|||||||
|
image: map2.pgm
|
||||||
|
mode: trinary
|
||||||
|
resolution: 0.05
|
||||||
|
origin: [-7.08, -15.4, 0]
|
||||||
|
negate: 0
|
||||||
|
occupied_thresh: 0.65
|
||||||
|
free_thresh: 0.25
|
||||||
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
|
||||||
Loading…
Reference in New Issue
Block a user