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=(
|
||||
"ros2 launch rm_nav_bringup bringup_real.launch.py \
|
||||
world:=RC2026 \
|
||||
world:=test \
|
||||
mode:=mapping \
|
||||
lio:=fastlio \
|
||||
localization:=icp \
|
||||
lio_rviz:=false \
|
||||
lio_rviz:=true \
|
||||
nav_rviz:=true"
|
||||
)
|
||||
|
||||
|
||||
10
nav.sh
10
nav.sh
@ -1,7 +1,7 @@
|
||||
# 备场代码
|
||||
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:=RC2026 \
|
||||
mode:=nav \
|
||||
@ -10,7 +10,13 @@ commands=(
|
||||
lio_rviz:=false \
|
||||
nav_rviz:=true"
|
||||
"ros2 launch rm_simpal_move simple_move.launch.py"
|
||||
"ros2 topic pub /move_goal rm_msgs/msg/MoveGoal '{x: 0.60, y: 3.995, angle: 0.0, max_speed: 10.0, tolerance: 0.1, rotor: false}' --once"
|
||||
"/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
|
||||
|
||||
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/
|
||||
install/
|
||||
log/
|
||||
|
||||
.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
|
||||
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" : [
|
||||
{
|
||||
"ip" : "192.168.1.137",
|
||||
"ip" : "192.168.1.176",
|
||||
"pcl_data_type" : 1,
|
||||
"pattern_mode" : 0,
|
||||
"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_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr
|
||||
pointcloud_sub_;
|
||||
// rclcpp::TimerBase::SharedPtr timer_;
|
||||
rclcpp::TimerBase::SharedPtr timer_;
|
||||
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
|
||||
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
|
||||
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
|
||||
|
||||
@ -3,4 +3,7 @@ Some ROS 2 custom messages for Robotaster
|
||||
|
||||
Usage
|
||||
Modify or add files in the /msg directory as needed
|
||||
colcon build
|
||||
colcon build
|
||||
KERNELS=="3-2:1.0", SUBSYSTEMS=="USB", MODE:="0666",SYMLINK+="underpan"
|
||||
KERNELS=="3-1:1.0", SUBSYSTEMS=="USB", MODE:="0666",SYMLINK+="upper"
|
||||
KERNELS=="3-3:1.0", SUBSYSTEMS=="USB", MODE:="0666",SYMLINK+="r2"
|
||||
@ -1,2 +1,2 @@
|
||||
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" : [
|
||||
{
|
||||
"ip" : "192.168.1.176",
|
||||
"ip" : "192.168.1.137",
|
||||
"pcl_data_type" : 1,
|
||||
"pattern_mode" : 0,
|
||||
"extrinsic_parameter" : {
|
||||
|
||||
@ -7,7 +7,7 @@
|
||||
filter_size_map: 0.5
|
||||
cube_side_length: 1000.0
|
||||
runtime_pos_log_enable: false
|
||||
map_file_path: "src/rm_nav_bringup/PCD/RC2025.pcd"
|
||||
map_file_path: "src/rm_nav_bringup/PCD/test2.pcd"
|
||||
|
||||
common:
|
||||
lid_topic: "/livox/lidar"
|
||||
|
||||
@ -3,7 +3,7 @@
|
||||
use_sim_time: false
|
||||
rough_leaf_size: 0.4
|
||||
refine_leaf_size: 0.1
|
||||
pcd_path: ""
|
||||
pcd_path: "map2"
|
||||
map_frame_id: "map"
|
||||
odom_frame_id: "odom"
|
||||
range_odom_frame_id: "lidar_odom"
|
||||
|
||||
@ -1,3 +1,3 @@
|
||||
base_link2livox_frame:
|
||||
xyz: "\"0.037 -0.354 0.41\""
|
||||
xyz: "\"0.251 -0.1285 0.397\""
|
||||
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