From 52ae8d29c8f00321897e09fe79919005eb770b84 Mon Sep 17 00:00:00 2001 From: yemai <1551079531@qq.com> Date: Thu, 11 Sep 2025 15:44:41 +0800 Subject: [PATCH] =?UTF-8?q?=E5=B0=86src=E7=9B=AE=E5=BD=95=E4=BD=9C?= =?UTF-8?q?=E4=B8=BA=E6=99=AE=E9=80=9A=E6=96=87=E4=BB=B6=E8=80=8C=E9=9D=9E?= =?UTF-8?q?=E5=AD=90=E6=A8=A1=E5=9D=97=E6=B7=BB=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src | 1 - src/CMakeLists.txt | 1 + src/README.md | 146 ++++++ src/build.sh | 41 ++ src/tr360_ros/CMakeLists.txt | 64 +++ src/tr360_ros/README.md | 146 ++++++ src/tr360_ros/build.sh | 41 ++ src/tr360_ros/cfg/TR360Config.cfg | 47 ++ src/tr360_ros/config/tr360.rviz | 177 +++++++ src/tr360_ros/include/tr360_ros/tr360_types.h | 83 ++++ src/tr360_ros/launch/tr360.launch | 53 ++ src/tr360_ros/package.xml | 25 + src/tr360_ros/src/test_publisher.cpp | 66 +++ src/tr360_ros/src/tr360_node.cpp | 459 ++++++++++++++++++ 14 files changed, 1349 insertions(+), 1 deletion(-) delete mode 160000 src create mode 120000 src/CMakeLists.txt create mode 100644 src/README.md create mode 100755 src/build.sh create mode 100644 src/tr360_ros/CMakeLists.txt create mode 100644 src/tr360_ros/README.md create mode 100755 src/tr360_ros/build.sh create mode 100644 src/tr360_ros/cfg/TR360Config.cfg create mode 100644 src/tr360_ros/config/tr360.rviz create mode 100644 src/tr360_ros/include/tr360_ros/tr360_types.h create mode 100644 src/tr360_ros/launch/tr360.launch create mode 100644 src/tr360_ros/package.xml create mode 100644 src/tr360_ros/src/test_publisher.cpp create mode 100644 src/tr360_ros/src/tr360_node.cpp diff --git a/src b/src deleted file mode 160000 index ec1c1e2..0000000 --- a/src +++ /dev/null @@ -1 +0,0 @@ -Subproject commit ec1c1e29cd6ab536cda3ab3c051b955c656006d8 diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt new file mode 120000 index 0000000..2016816 --- /dev/null +++ b/src/CMakeLists.txt @@ -0,0 +1 @@ +/opt/ros/noetic/share/catkin/cmake/toplevel.cmake \ No newline at end of file diff --git a/src/README.md b/src/README.md new file mode 100644 index 0000000..fc65cbb --- /dev/null +++ b/src/README.md @@ -0,0 +1,146 @@ +# TR360 ROS 包 + +基于ROS Noetic的TR360雷达点云处理包，支持TR360-3-60避障雷达和TR360-4-60地形雷达。 + +## 功能特性 + +- ✅ UDP数据接收和解析 +- ✅ 点云数据转换（极坐标→直角坐标） +- ✅ PointCloud2话题发布 +- ✅ RViz可视化支持 +- ✅ 动态参数配置 +- ✅ 多雷达类型支持 +- ✅ 网络参数可配置 +- ✅ 测试数据模拟 + +## 系统要求 + +- Ubuntu 20.04 +- ROS Noetic +- PCL 1.10 + +## 安装和编译 + +### 自动编译 +```bash +cd tr360_ros +./build.sh +``` + +### 手动编译 +```bash +# 设置ROS环境 +source /opt/ros/noetic/setup.bash + +# 创建工作空间（如果不存在） +mkdir -p ~/catkin_ws/src +cd ~/catkin_ws +catkin_make + +# 复制包到工作空间 +cp -r /path/to/tr360_ros ~/catkin_ws/src/ + +# 编译 +cd ~/catkin_ws +catkin_make + +# 设置环境 +source devel/setup.bash +``` + +## 使用方法 + +### 1. 启动主节点（需要真实雷达） +```bash +roslaunch tr360_ros tr360.launch +``` + +### 2. 启动测试节点（无雷达时使用） +```bash +rosrun tr360_ros test_publisher +``` + +### 3. 可视化 +```bash +rviz -d $(rospack find tr360_ros)/config/tr360.rviz +``` + +## 参数配置 + +### 启动参数 +在`launch/tr360.launch`中配置： +- `radar_type`: 雷达类型 (`TR360-3-60`/`TR360-4-60`) +- `radar_ip`: 雷达设备IP +- `radar_port`: 雷达端口 (默认: 10123) +- `local_ip`: 本地接收IP +- `local_port`: 本地端口 (默认: 10193/20193) + +### 动态参数 +运行时可通过`rqt_reconfigure`调整： +- 距离范围限制 +- 角度范围限制 +- 强度范围限制 +- 坐标显示范围 +- 发布频率 + +## 网络配置 + +### TR360-3-60 (避障雷达) +- 雷达IP: `192.168.1.80` +- 雷达端口: `10123` +- 本地IP: `192.168.1.193` +- 本地端口: `10193` + +### TR360-4-60 (地形雷达) +- 雷达IP: `192.168.0.80` +- 雷达端口: `10123` +- 本地IP: `192.168.0.193` +- 本地端口: `20193` + +## 数据结构 + +### 点云格式 +每个点包含： +- 方位角 (9 bits) +- 距离 (12 bits) +- 俯仰角 (11 bits) +- RCS值 (8 bits) + +### 坐标转换 +极坐标 → 直角坐标： +``` +{x, y, z, v} = {r*cos(e)*cos(a), r*cos(e)*sin(a), r*sin(e), v} +``` + +## 话题 + +### 多雷达支持 +支持同时处理多个TR360雷达数据，根据雷达类型和数量自动生成话题名称： + +#### TR360-3-60 双雷达配置 (max_radars=2) +- `/TR360-3-30-up` (`sensor_msgs/PointCloud2`) - 上方雷达点云数据 +- `/TR360-3-60-front` (`sensor_msgs/PointCloud2`) - 前方雷达点云数据 + +#### TR360-3-60 单雷达配置 (max_radars=1) +- `/TR360-4-60-terrain` (`sensor_msgs/PointCloud2`) - 地形雷达点云数据 + +#### 默认配置 +- `/tr360_points_1` (`sensor_msgs/PointCloud2`) - 雷达1的点云数据 +- `/tr360_points_2` (`sensor_msgs/PointCloud2`) - 雷达2的点云数据 +- `/tr360_points_3` (`sensor_msgs/PointCloud2`) - 雷达3的点云数据 +- `/tr360_points_4` (`sensor_msgs/PointCloud2`) - 雷达4的点云数据 + +### 坐标系 +每个雷达使用独立的坐标系： +- `tr360_radar_1` - 雷达1坐标系 +- `tr360_radar_2` - 雷达2坐标系 +- `tr360_radar_3` - 雷达3坐标系 +- `tr360_radar_4` - 雷达4坐标系 + +## 版本控制 + +使用Git进行版本管理，采用分步测试开发模式。 + +## 许可证 + +BSD 3-Clause License \ No newline at end of file diff --git a/src/build.sh b/src/build.sh new file mode 100755 index 0000000..812e7c5 --- /dev/null +++ b/src/build.sh @@ -0,0 +1,41 @@ +#!/bin/bash + +# TR360 ROS包编译脚本 +echo "正在编译TR360 ROS包..." + +# 设置ROS环境 +source /opt/ros/noetic/setup.bash + +# 创建编译目录 +BUILD_DIR="$HOME/catkin_ws" +if [ ! -d "$BUILD_DIR" ]; then + echo "创建catkin工作空间..." + mkdir -p "$BUILD_DIR/src" + cd "$BUILD_DIR" + catkin_make +fi + +# 复制包到工作空间 +PACKAGE_DIR="$BUILD_DIR/src/tr360_ros" +if [ -d "$PACKAGE_DIR" ]; then + echo "删除旧版包..." + rm -rf "$PACKAGE_DIR" +fi + +echo "复制包到工作空间..." +cp -r "$(dirname "$0")" "$BUILD_DIR/src/" + +# 编译包 +cd "$BUILD_DIR" +echo "开始编译..." +catkin_make + +# 设置环境变量 +echo "source $BUILD_DIR/devel/setup.bash" >> ~/.bashrc +source "$BUILD_DIR/devel/setup.bash" + +echo "编译完成！" +echo "使用方法：" +echo "1. 启动主节点: roslaunch tr360_ros tr360.launch" +echo "2. 启动测试节点: rosrun tr360_ros test_publisher" +echo "3. 可视化: rviz -d $(rospack find tr360_ros)/config/tr360.rviz" \ No newline at end of file diff --git a/src/tr360_ros/CMakeLists.txt b/src/tr360_ros/CMakeLists.txt new file mode 100644 index 0000000..82a7fe7 --- /dev/null +++ b/src/tr360_ros/CMakeLists.txt @@ -0,0 +1,64 @@ +cmake_minimum_required(VERSION 3.0.2) +project(tr360_ros) + +find_package(catkin REQUIRED COMPONENTS + roscpp + sensor_msgs + pcl_ros + pcl_conversions + dynamic_reconfigure +) + +# 添加动态参数配置生成 +find_package(catkin REQUIRED) +generate_dynamic_reconfigure_options( + cfg/TR360Config.cfg +) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES tr360_ros + CATKIN_DEPENDS roscpp sensor_msgs pcl_ros pcl_conversions +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} +) + +add_executable(tr360_node src/tr360_node.cpp) +target_link_libraries(tr360_node + ${catkin_LIBRARIES} + ${PCL_LIBRARIES} +) + +# 添加网络库链接 +target_link_libraries(tr360_node ${Boost_LIBRARIES}) + +# 测试发布器 +add_executable(test_publisher src/test_publisher.cpp) +target_link_libraries(test_publisher + ${catkin_LIBRARIES} + ${PCL_LIBRARIES} +) + +install(TARGETS test_publisher + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(TARGETS tr360_node + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch +) + +install(DIRECTORY config/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config +) \ No newline at end of file diff --git a/src/tr360_ros/README.md b/src/tr360_ros/README.md new file mode 100644 index 0000000..fc65cbb --- /dev/null +++ b/src/tr360_ros/README.md @@ -0,0 +1,146 @@ +# TR360 ROS 包 + +基于ROS Noetic的TR360雷达点云处理包，支持TR360-3-60避障雷达和TR360-4-60地形雷达。 + +## 功能特性 + +- ✅ UDP数据接收和解析 +- ✅ 点云数据转换（极坐标→直角坐标） +- ✅ PointCloud2话题发布 +- ✅ RViz可视化支持 +- ✅ 动态参数配置 +- ✅ 多雷达类型支持 +- ✅ 网络参数可配置 +- ✅ 测试数据模拟 + +## 系统要求 + +- Ubuntu 20.04 +- ROS Noetic +- PCL 1.10 + +## 安装和编译 + +### 自动编译 +```bash +cd tr360_ros +./build.sh +``` + +### 手动编译 +```bash +# 设置ROS环境 +source /opt/ros/noetic/setup.bash + +# 创建工作空间（如果不存在） +mkdir -p ~/catkin_ws/src +cd ~/catkin_ws +catkin_make + +# 复制包到工作空间 +cp -r /path/to/tr360_ros ~/catkin_ws/src/ + +# 编译 +cd ~/catkin_ws +catkin_make + +# 设置环境 +source devel/setup.bash +``` + +## 使用方法 + +### 1. 启动主节点（需要真实雷达） +```bash +roslaunch tr360_ros tr360.launch +``` + +### 2. 启动测试节点（无雷达时使用） +```bash +rosrun tr360_ros test_publisher +``` + +### 3. 可视化 +```bash +rviz -d $(rospack find tr360_ros)/config/tr360.rviz +``` + +## 参数配置 + +### 启动参数 +在`launch/tr360.launch`中配置： +- `radar_type`: 雷达类型 (`TR360-3-60`/`TR360-4-60`) +- `radar_ip`: 雷达设备IP +- `radar_port`: 雷达端口 (默认: 10123) +- `local_ip`: 本地接收IP +- `local_port`: 本地端口 (默认: 10193/20193) + +### 动态参数 +运行时可通过`rqt_reconfigure`调整： +- 距离范围限制 +- 角度范围限制 +- 强度范围限制 +- 坐标显示范围 +- 发布频率 + +## 网络配置 + +### TR360-3-60 (避障雷达) +- 雷达IP: `192.168.1.80` +- 雷达端口: `10123` +- 本地IP: `192.168.1.193` +- 本地端口: `10193` + +### TR360-4-60 (地形雷达) +- 雷达IP: `192.168.0.80` +- 雷达端口: `10123` +- 本地IP: `192.168.0.193` +- 本地端口: `20193` + +## 数据结构 + +### 点云格式 +每个点包含： +- 方位角 (9 bits) +- 距离 (12 bits) +- 俯仰角 (11 bits) +- RCS值 (8 bits) + +### 坐标转换 +极坐标 → 直角坐标： +``` +{x, y, z, v} = {r*cos(e)*cos(a), r*cos(e)*sin(a), r*sin(e), v} +``` + +## 话题 + +### 多雷达支持 +支持同时处理多个TR360雷达数据，根据雷达类型和数量自动生成话题名称： + +#### TR360-3-60 双雷达配置 (max_radars=2) +- `/TR360-3-30-up` (`sensor_msgs/PointCloud2`) - 上方雷达点云数据 +- `/TR360-3-60-front` (`sensor_msgs/PointCloud2`) - 前方雷达点云数据 + +#### TR360-3-60 单雷达配置 (max_radars=1) +- `/TR360-4-60-terrain` (`sensor_msgs/PointCloud2`) - 地形雷达点云数据 + +#### 默认配置 +- `/tr360_points_1` (`sensor_msgs/PointCloud2`) - 雷达1的点云数据 +- `/tr360_points_2` (`sensor_msgs/PointCloud2`) - 雷达2的点云数据 +- `/tr360_points_3` (`sensor_msgs/PointCloud2`) - 雷达3的点云数据 +- `/tr360_points_4` (`sensor_msgs/PointCloud2`) - 雷达4的点云数据 + +### 坐标系 +每个雷达使用独立的坐标系： +- `tr360_radar_1` - 雷达1坐标系 +- `tr360_radar_2` - 雷达2坐标系 +- `tr360_radar_3` - 雷达3坐标系 +- `tr360_radar_4` - 雷达4坐标系 + +## 版本控制 + +使用Git进行版本管理，采用分步测试开发模式。 + +## 许可证 + +BSD 3-Clause License \ No newline at end of file diff --git a/src/tr360_ros/build.sh b/src/tr360_ros/build.sh new file mode 100755 index 0000000..812e7c5 --- /dev/null +++ b/src/tr360_ros/build.sh @@ -0,0 +1,41 @@ +#!/bin/bash + +# TR360 ROS包编译脚本 +echo "正在编译TR360 ROS包..." + +# 设置ROS环境 +source /opt/ros/noetic/setup.bash + +# 创建编译目录 +BUILD_DIR="$HOME/catkin_ws" +if [ ! -d "$BUILD_DIR" ]; then + echo "创建catkin工作空间..." + mkdir -p "$BUILD_DIR/src" + cd "$BUILD_DIR" + catkin_make +fi + +# 复制包到工作空间 +PACKAGE_DIR="$BUILD_DIR/src/tr360_ros" +if [ -d "$PACKAGE_DIR" ]; then + echo "删除旧版包..." + rm -rf "$PACKAGE_DIR" +fi + +echo "复制包到工作空间..." +cp -r "$(dirname "$0")" "$BUILD_DIR/src/" + +# 编译包 +cd "$BUILD_DIR" +echo "开始编译..." +catkin_make + +# 设置环境变量 +echo "source $BUILD_DIR/devel/setup.bash" >> ~/.bashrc +source "$BUILD_DIR/devel/setup.bash" + +echo "编译完成！" +echo "使用方法：" +echo "1. 启动主节点: roslaunch tr360_ros tr360.launch" +echo "2. 启动测试节点: rosrun tr360_ros test_publisher" +echo "3. 可视化: rviz -d $(rospack find tr360_ros)/config/tr360.rviz" \ No newline at end of file diff --git a/src/tr360_ros/cfg/TR360Config.cfg b/src/tr360_ros/cfg/TR360Config.cfg new file mode 100644 index 0000000..c54e8f4 --- /dev/null +++ b/src/tr360_ros/cfg/TR360Config.cfg @@ -0,0 +1,47 @@ +#!/usr/bin/env python + +PACKAGE = "tr360_ros" + +from dynamic_reconfigure.parameter_generator_catkin import * + +# 距离参数 +gen = ParameterGenerator() + +# 距离限制参数 +gen.add("r_lo", double_t, 0, "距离下限(m)", 0.0, 0.0, 100.0) +gen.add("r_hi", double_t, 0, "距离上限(m)", 100.0, 0.0, 100.0) + +# 方位角参数 +gen.add("az_lo", double_t, 0, "方位角下限(度)", -180.0, -180.0, 180.0) +gen.add("az_hi", double_t, 0, "方位角上限(度)", 180.0, -180.0, 180.0) + +# 俯仰角参数 +gen.add("el_lo", double_t, 0, "俯仰角下限(度)", -90.0, -90.0, 90.0) +gen.add("el_hi", double_t, 0, "俯仰角上限(度)", 90.0, -90.0, 90.0) + +# 强度参数 +gen.add("intensity_lo", double_t, 0, "强度下限", 0.0, 0.0, 100.0) +gen.add("intensity_hi", double_t, 0, "强度上限", 100.0, 0.0, 100.0) + +# 坐标显示范围 +gen.add("x_lo", double_t, 0, "X轴显示下限(m)", -50.0, -100.0, 100.0) +gen.add("x_hi", double_t, 0, "X轴显示上限(m)", 50.0, -100.0, 100.0) +gen.add("y_lo", double_t, 0, "Y轴显示下限(m)", -50.0, -100.0, 100.0) +gen.add("y_hi", double_t, 0, "Y轴显示上限(m)", 50.0, -100.0, 100.0) +gen.add("z_lo", double_t, 0, "Z轴显示下限(m)", -50.0, -100.0, 100.0) +gen.add("z_hi", double_t, 0, "Z轴显示上限(m)", 50.0, -100.0, 100.0) + +# 雷达类型选择 +gen.add("radar_type", str_t, 0, "雷达类型", "TR360-3-60", edit_method={"enum_description": ["TR360-3-60: 避障雷达", "TR360-4-60: 地形雷达"]}) + +# 网络配置 +gen.add("radar_ip", str_t, 0, "雷达IP地址", "192.168.1.80") +gen.add("radar_port", int_t, 0, "雷达端口", 10123, 1024, 65535) +gen.add("local_ip", str_t, 0, "本地IP地址", "192.168.1.193") +gen.add("local_port", int_t, 0, "本地端口", 10193, 1024, 65535) + +# 点云处理参数 +gen.add("max_points", int_t, 0, "最大点数", 20000, 1000, 100000) +gen.add("publish_rate", double_t, 0, "发布频率(Hz)", 10.0, 1.0, 100.0) + +exit(gen.generate(PACKAGE, "tr360_ros", "TR360Config")) \ No newline at end of file diff --git a/src/tr360_ros/config/tr360.rviz b/src/tr360_ros/config/tr360.rviz new file mode 100644 index 0000000..f0b31c3 --- /dev/null +++ b/src/tr360_ros/config/tr360.rviz @@ -0,0 +1,177 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 323 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class:极 rviz/Views + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + # 雷达1点云显示 + - Alpha: 0.5 + Class: rviz/PointCloud2 + Color: 1 0 0 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255 + Min Color: 0 + Name: TR360 Radar 1 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Style: Points + Topic: /tr360_points_1 + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + # 雷达2点云显示 + - Alpha: 0.5 + Class极: rviz/PointCloud2 + Color: 0 1 0 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255 + Min Color: 0 + Name: TR360 Radar 2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Style: Points + Topic: /tr360_points_2 + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + # 雷达3点云显示 + - Alpha: 0.5 + Class: rviz/PointCloud2 + Color: 0 0 1 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255 + Min Color: 0 + Name: TR360 Radar 3 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Style: Points + Topic: /tr360_points_3 + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + # 雷达4点云显示 + - Alpha: 0.5 + Class: rviz/PointCloud2 + Color: 1 1 0 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255 + Min Color: 0 + Name: TR360 Radar 4 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Style: Points + Topic: /tr360_points_4 + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Grid + Cell Size: 1 + Color: 0.9411764740943909 0.9411764740943909 0.9411764740943909 + Enabled: true + Line Style: Lines + Line Width: 0.03 + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: "" + Value: true + Enabled: true + Global Options: + Background Color: 48 48 48 + Fixed Frame: tr360_radar_1 + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: false + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/2D Pose Estimate + - Class: rviz/2D Nav Goal + - Class: rviz/Publish Point + Value: rviz/Interact + Views: + Current: + Class: rviz/Orbit + Distance: 10 + Enable Stereo Rendering: true + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.7853981852531433 + Target Frame: "" + Value: Orbit (rviz) + Yaw: 0.7853981852531433 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 768 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000000000000000001d6000003c4fcfd0000000bf000000000000000000000432000001590000000643ffffff0000001000000032000001d6fcfd00000002df0000020f00000001000000000000000000000000fc0200000002fb000000100064006900730070006c00610079007300000001df0000020f00000001000000000000000000000000fc0100000001fb0000000a00760069006500770073000000014300000159fcfd0000000a000000000000000000000432000001590000000000000000000000000000000000000200000001000000005f0000012dfc0200000003fb0000001e0074006f006f006c005f00700072006f007000650072007400690065007300000001df0000020f00000001000000000000000000000000fc0100000001fb0000000c00740069006d00650072007300000000000000000000000000000000020000010000000000000000000000000000000000000000000200000001000000005f0000012dfc0200000003fb0000001e0074006f006f006c005f00700072006f007000650072007400690065007300000001df0000020f00000001000000000000000000000000fc0100000001fb0000000c00740069006d00650072007300000000000000000000000000000000020000010000000000000000000000000000000000000000000200000001000000005f0000012dfc0200000003fb极0000001e0074006f006f006c005f00700072006f007000650072007400极690065007300000001df0000020f00000001000000000000000000000000fc0100000001fb0000000c00740069006d0065007200730000000000000000000000000000000002000001000000000000000000000000000000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1024 + X: 0 + Y: 0 \ No newline at end of file diff --git a/src/tr360_ros/include/tr360_ros/tr360_types.h b/src/tr360_ros/include/tr360_ros/tr360_types.h new file mode 100644 index 0000000..22a5c8a --- /dev/null +++ b/src/tr360_ros/include/tr360_ros/tr360_types.h @@ -0,0 +1,83 @@ +#ifndef TR360_ROS_TR360_TYPES_H +#define TR360_ROS_TR360_TYPES_H + +#include + +// 雷达帧头结构 +#pragma pack(push, 1) +typedef struct +{ + uint32_t a:9; // 方位角 + uint32_t r:12; // 距离 + int32_t e:11; // 俯仰角 + uint8_t v; // rcs +} points_5b; + +// 帧头对象新结构 +typedef struct +{ + unsigned char Outtype; //0 raev 1 xyz + unsigned char Version; //默认4 + unsigned char RadarType; // + unsigned char NFrame; + unsigned int Rev[3]; + unsigned int FrameCounter; // 帧计数 + float Res[4]; // 分辨率 + float Offset[4]; // 偏移量 + float Time; //雷达输出时间和开始0度采集时间差值 + int ObjNum; // 目标个数 + unsigned int CheckSum; // 和校验 +} FrameHeadObjNewS; + +// 雷达头信息结构 +typedef struct +{ + char header[12]; //帧头 避障"TR360-3-60"，地形"TR360-4-60" + uint32_t FrameCounter; // 帧计数 + uint32_t nRadar; // 传感器个数，避障默认2，地形默认1 + uint32_t ObjNum; // 总目标个数 + uint8_t Rev[36]; //保留 + unsigned int CheckSum; //总和校验 +} RadarHeaderInfoS; + +// 参数限制结构 +typedef struct +{ + int RLo100; // 距离下限cm + int RHi100; // 距离上限cm + int azLo100; // 方位角下限 + int azHi100; // 方位角上限 + int elLo100; // 俯仰角下限 + int elHi100; // 俯仰角上限 + int vLo; // 幅值下限 + int vHi; // 幅值上限 + int xLo100; + int xHi100; + int yLo100; + int yHi100; + int zLo100; + int zHi100; + int azComp100; // 方位角补偿 + int elComp100; // 俯仰角补偿 + int Roll100; // 安装滚转角补偿 + int Pitch100; // 安装俯仰角补偿 + int Yaw100; // 安装方位角补偿 + int type; + int xGLo100; // x显示下限 + int xGHi100; // x显示上限 + int yGLo100; // y显示下限 + int yGHi100; // y显示上限 + int zGLo100; // z显示下限 + int zGHi100; // z显示上限 + int vGLo; // 幅度显示下限 + int vGHi; // 幅度显示上限 + int nStep; // vGLo与vGHi之间显示nStep个色阶 + int MaxPoint; //最多显示多少个点 + int EleQ; + int ARQ; + int NegQ; +} ParaLim; + +#pragma pack(pop) + +#endif // TR360_ROS_TR360_TYPES_H \ No newline at end of file diff --git a/src/tr360_ros/launch/tr360.launch b/src/tr360_ros/launch/tr360.launch new file mode 100644 index 0000000..f2d862c --- /dev/null +++ b/src/tr360_ros/launch/tr360.launch @@ -0,0 +1,53 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/tr360_ros/package.xml b/src/tr360_ros/package.xml new file mode 100644 index 0000000..a17f27b --- /dev/null +++ b/src/tr360_ros/package.xml @@ -0,0 +1,25 @@ + + + tr360_ros + 0.1.0 + ROS package for TR360 radar point cloud processing + yemai + BSD + + catkin + + roscpp + sensor_msgs + pcl_ros + pcl_conversions + dynamic_reconfigure + + roscpp + sensor_msgs + pcl_ros + pcl_conversions + dynamic_reconfigure + + + + \ No newline at end of file diff --git a/src/tr360_ros/src/test_publisher.cpp b/src/tr360_ros/src/test_publisher.cpp new file mode 100644 index 0000000..9a45def --- /dev/null +++ b/src/tr360_ros/src/test_publisher.cpp @@ -0,0 +1,66 @@ +#include +#include +#include +#include +#include +#include + +class TestPublisher +{ +private: + ros::NodeHandle nh_; + ros::Publisher cloud_pub_; + ros::Timer timer_; + +public: + TestPublisher() + { + cloud_pub_ = nh_.advertise("tr360_points", 10); + timer_ = nh_.createTimer(ros::Duration(0.1), &TestPublisher::publishTestData, this); + ROS_INFO("Test publisher initialized"); + } + + void publishTestData(const ros::TimerEvent& event) + { + // 创建测试点云数据 + pcl::PointCloud cloud; + cloud.header.frame_id = "tr360_radar"; + cloud.is_dense = false; + + // 生成模拟雷达数据（圆锥形点云） + int num_points = 1000; + for (int i = 0; i < num_points; ++i) { + float angle = 2.0 * M_PI * i / num_points; + float distance = 5.0 + 2.0 * sin(angle * 5.0); + float height = 2.0 * cos(angle * 3.0); + + pcl::PointXYZI point; + point.x = distance * cos(angle); + point.y = distance * sin(angle); + point.z = height; + point.intensity = 50.0 + 30.0 * sin(angle * 2.0); + + cloud.points.push_back(point); + } + + // 发布点云 + sensor_msgs::PointCloud2 cloud_msg; + pcl::toROSMsg(cloud, cloud_msg); + cloud_msg.header.stamp = ros::Time::now(); + cloud_msg.header.frame_id = "tr360_radar"; + + cloud_pub_.publish(cloud_msg); + + ROS_INFO("Published %d test points", num_points); + } +}; + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "test_publisher"); + + TestPublisher publisher; + ros::spin(); + + return 0; +} \ No newline at end of file diff --git a/src/tr360_ros/src/tr360_node.cpp b/src/tr360_ros/src/tr360_node.cpp new file mode 100644 index 0000000..8b30a0f --- /dev/null +++ b/src/tr360_ros/src/tr360_node.cpp @@ -0,0 +1,459 @@ +#include +#include +#include +#include +#include +#include +#include "tr360_ros/tr360_types.h" +#include +#include +#include +#include +#include +#include +#include + +class TR360RadarNode +{ +private: + ros::NodeHandle nh_; + std::vector cloud_pubs_; + + int udp_socket_; + struct sockaddr_in server_addr_, client_addr_; + + // 配置参数 + std::string radar_type_; + std::string radar_ip_; + int radar_port_; + std::string local_ip_; + int local_port_; + int max_radars_; + + // 命令历史记录 + std::vector command_history_; + int max_history_size_; + + // TCP命令服务器 + int tcp_socket_; + bool enable_command_server_; + + // 点云处理状态 + uint32_t state_; + uint32_t total_len_; + uint32_t len1_; + int counter_; + uint8_t header_buf_[128]; + uint8_t out_data_[327680]; + + ParaLim para_obj_; + +public: + TR360RadarNode() : state_(0), total_len_(0), len1_(0), counter_(0) + { + // 初始化参数 + ros::NodeHandle private_nh("~"); + private_nh.param("radar_type", radar_type_, "TR360-3-60"); + private_nh.param("radar_ip", radar_ip_, "192.168.1.80"); + private_nh.param("radar_port", radar_port_, 10123); + private_nh.param("local_ip", local_ip_, "192.168.1.193"); + private_nh.param("local_port", local_port_, 10193); + private_nh.param("max_radars", max_radars_, 4); // 最大雷达数量 + private_nh.param("enable_command_server", enable_command_server_, true); + private_nh.param("max_history_size", max_history_size_, 100); + + // 创建点云发布者（支持多个雷达） + for (int i = 0; i < max_radars_; ++i) { + std::string topic_name; + if (radar_type_ == "TR360-3-60" && max_radars_ == 2) { + // TR360-3-60 双雷达配置 + if (i == 0) topic_name = "tr360_3_30_up"; + else if (i == 1) topic_name = "tr360_3_60_front"; + else topic_name = "tr360_points_" + std::to_string(i+1); + } else if (radar_type_ == "TR360-3-60" && max_radars_ == 1) { + // TR360-3-60 单雷达配置 + topic_name = "tr360_4_60_terrain"; + } else { + // 默认配置 + topic_name = "tr360_points_" + std::to_string(i+1); + } + cloud_pubs_.push_back(nh_.advertise(topic_name, 10)); + } + + // 初始化UDP socket + initUDP(); + + // 初始化TCP命令服务器 + if (enable_command_server_) { + initCommandServer(); + } + + // 初始化参数对象 + initParameters(); + } + + void initUDP() + { + udp_socket_ = socket(AF_INET, SOCK_DGRAM, 0); + if (udp_socket_ < 0) { + ROS_ERROR("Failed to create socket"); + return; + } + + // 设置本地地址 + memset(&server_addr_, 0, sizeof(server_addr_)); + server_addr_.sin_family = AF_INET; + server_addr_.sin_addr.s_addr = inet_addr(local_ip_.c_str()); + server_addr_.sin_port = htons(local_port_); + + // 绑定socket + if (bind(udp_socket_, (struct sockaddr*)&server_addr_, sizeof(server_addr_)) < 0) { + ROS_ERROR("Bind failed"); + close(udp_socket_); + return; + } + + ROS_INFO("UDP socket initialized on %s:%d", local_ip_.c_str(), local_port_); + + // 根据雷达类型设置默认端口 + if (radar_type_ == "TR360-3-60") { + local_port_ = 10195; + } else if (radar_type_ == "TR360-4-60") { + local_port_ = 20295; + } + } + + void initParameters() + { + // 初始化参数默认值 + para_obj_.RLo100 = 0; + para_obj_.RHi100 = 4000; + para_obj_.azLo100 = 0; + para_obj_.azHi100 = 36000; + para_obj_.elLo100 = -12000; + para_obj_.elHi100 = 12000; + para_obj_.vLo = 0; + para_obj_.vHi = 100; + para_obj_.xLo100 = -4000; + para_obj_.xHi100 = 4000; + para_obj_.yLo100 = -4000; + para_obj_.yHi100 = 4000; + para_obj_.zLo100 = -4000; + para_obj_.zHi100 = 4000; + para_obj_.azComp100 = 0; + para_obj_.elComp100 = 0; + para_obj_.Roll100 = 0; + para_obj_.Pitch100 = 0; + para_obj_.Yaw100 = 0; + para_obj_.xGLo100 = -4000; + para_obj_.xGHi100 = 4000; + para_obj_.yGLo100 = -4000; + para_obj_.yGHi100 = 4000; + para_obj_.zGLo100 = -4000; + para_obj_.zGHi100 = 4000; + para_obj_.vGLo = 20; + para_obj_.vGHi = 60; + para_obj_.nStep = 33; + para_obj_.MaxPoint = 20000; + para_obj_.EleQ = 0; + para_obj_.ARQ = 0; + para_obj_.NegQ = 0; + } + + int parseFrame(unsigned char *data, unsigned int len) + { + RadarHeaderInfoS *ptr = (RadarHeaderInfoS *)data; + FrameHeadObjNewS *frame_ptr; + + const char *header = radar_type_.c_str(); + + if (strncmp(header, (char*)ptr->header, strlen(header)) == 0) { + frame_ptr = (FrameHeadObjNewS *)(data + 64); + + if (state_ != 3) { + ROS_DEBUG("Counter:%d, len1:%d, total_len:%d, ObjNum:%d, nRadar:%d, RadarObj:%d", + counter_, len1_, total_len_, ptr->ObjNum, ptr->nRadar, frame_ptr->ObjNum); + } + + if (len >= 128) { + memcpy(header_buf_, data, 128); + } else { + memset(header_buf_, 0, 128); + } + + total_len_ = ptr->ObjNum * 5 + ptr->nRadar * 64 + 64; + len1_ = len; + state_ = 1; + } else if (state_ == 1) { + len1_ += len; + state_ = 2; + } else if (state_ == 2) { + len1_ += len; + state_ = 2; + } + + if (state_ == 1) { + counter_++; + if ((total_len_ < 0) || (total_len_ > 60000)) { + ROS_WARN("Invalid total length: %d", total_len_); + total_len_ = 0; + state_ = 0; + } + } + + if (state_) { + if (len1_ > total_len_) { + ptr = (RadarHeaderInfoS *)header_buf_; + frame_ptr = (FrameHeadObjNewS *)(header_buf_ + 64); + ROS_WARN("Data length exceeded: Counter:%d, len1:%d, total_len:%d", + counter_, len1_, total_len_); + total_len_ = 0; + state_ = 0; + } else if (len1_ == total_len_) { + total_len_ = 0; + state_ = 3; + return state_; + } + } + + return state_; + } + + void callback_arev5b(float *arve, void *ptr, float *factor4, float *offset4) + { + points_5b *point_ptr = (points_5b *)ptr; + arve[0] = point_ptr->a * factor4[0] + offset4[0]; + arve[1] = point_ptr->r * factor4[1] + offset4[1]; + arve[2] = point_ptr->e * factor4[2]; + arve[3] = point_ptr->v * factor4[3] + offset4[3]; + } + + void processPointCloud(unsigned char *data, unsigned int len) + { + RadarHeaderInfoS *radar_info_ptr = (RadarHeaderInfoS *)data; + if (radar_info_ptr->nRadar == 0) { + return; + } + + // 处理所有雷达数据 + for (int radar_idx = 0; radar_idx < radar_info_ptr->nRadar; ++radar_idx) { + if (radar_idx >= cloud_pubs_.size()) { + ROS_WARN("Radar index %d exceeds configured publisher count", radar_idx); + continue; + } + + FrameHeadObjNewS frame_obj = *(FrameHeadObjNewS *)(data + 64 * radar_idx + 64); + + // 创建PCL点云 + pcl::PointCloud cloud; + cloud.header.frame_id = "tr360_radar"; + cloud.is_dense = false; + + // 解析点云数据 + points_5b *points = (points_5b *)(data + len - 5 * frame_obj.ObjNum); + + for (int i = 0; i < frame_obj.ObjNum; ++i) { + points_5b point = points[i]; + + // 转换为直角坐标系 + float a_rad = point.a * frame_obj.Res[0] + frame_obj.Offset[0]; + float r = point.r * frame_obj.Res[1] + frame_obj.Offset[1]; + float e_rad = point.e * frame_obj.Res[2]; + float intensity = point.v * frame_obj.Res[3] + frame_obj.Offset[3]; + + // 坐标转换: {x,y,z,v} = {r*cos(e)*cos(a), r*cos(e)*sin(a), r*sin(e), v} + pcl::PointXYZI pcl_point; + pcl_point.x = r * cos(e_rad) * cos(a_rad); + pcl_point.y = r * cos(e_rad) * sin(a_rad); + pcl_point.z = r * sin(e_rad); + pcl_point.intensity = intensity; + + cloud.points.push_back(pcl_point); + } + + // 发布点云 + sensor_msgs::PointCloud2 cloud_msg; + pcl::toROSMsg(cloud, cloud_msg); + cloud_msg.header.stamp = ros::Time::now(); + cloud_msg.header.frame_id = "tr360_radar_" + std::to_string(radar_idx + 1); + + cloud_pubs_[radar_idx].publish(cloud_msg); + + ROS_INFO("Published %lu points from radar %d", cloud.points.size(), radar_idx + 1); + } + } + + void initCommandServer() + { + tcp_socket_ = socket(AF_INET, SOCK_STREAM, 0); + if (tcp_socket_ < 0) { + ROS_ERROR("Failed to create TCP socket"); + return; + } + + // 设置TCP服务器地址 + struct sockaddr_in tcp_addr; + memset(&tcp_addr, 0, sizeof(tcp_addr)); + tcp_addr.sin_family = AF_INET; + tcp_addr.sin_addr.s_addr = inet_addr(local_ip_.c_str()); + tcp_addr.sin_port = htons(local_port_ + 1000); // 命令端口为数据端口+1000 + + // 绑定并监听 + if (bind(tcp_socket_, (struct sockaddr*)&tcp_addr, sizeof(tcp_addr)) < 0) { + ROS_ERROR("TCP bind failed"); + close(tcp_socket_); + return; + } + + if (listen(tcp_socket_, 5) < 0) { + ROS_ERROR("TCP listen failed"); + close(tcp_socket_); + return; + } + + ROS_INFO("TCP command server started on %s:%d", local_ip_.c_str(), local_port_ + 1000); + } + + void handleCommand(int client_sock) + { + char buffer[1024]; + int recv_len = recv(client_sock, buffer, sizeof(buffer) - 1, 0); + + if (recv_len > 0) { + buffer[recv_len] = '\0'; + std::string command(buffer); + + // 去除回车换行 + size_t pos; + while ((pos = command.find("\r\n")) != std::string::npos) { + command.erase(pos, 2); + } + while ((pos = command.find('\n')) != std::string::npos) { + command.erase(pos, 1); + } + while ((pos = command.find('\r')) != std::string::npos) { + command.erase(pos, 1); + } + + // 添加到历史记录 + addToHistory(command); + + // 处理命令 + std::string response = processRadarCommand(command); + + // 发送响应（添加回车换行） + response += "\r\n"; + send(client_sock, response.c_str(), response.length(), 0); + + ROS_INFO("Command: %s -> Response: %s", command.c_str(), response.c_str()); + } + + close(client_sock); + } + + void addToHistory(const std::string& command) + { + command_history_.push_back(command); + if (command_history_.size() > max_history_size_) { + command_history_.erase(command_history_.begin()); + } + } + + std::string processRadarCommand(const std::string& command) + { + if (command == "history") { + std::string history; + for (size_t i = 0; i < command_history_.size(); ++i) { + history += std::to_string(i + 1) + ": " + command_history_[i] + "\n"; + } + return history.empty() ? "No command history" : history; + } + else if (command == "status") { + return "Radar type: " + radar_type_ + ", Port: " + std::to_string(local_port_); + } + else if (command.find("set_port") == 0) { + // 设置端口命令格式: set_port 10195 + size_t space_pos = command.find(' '); + if (space_pos != std::string::npos) { + try { + int new_port = std::stoi(command.substr(space_pos + 1)); + local_port_ = new_port; + return "Port set to: " + std::to_string(new_port); + } catch (...) { + return "Invalid port number"; + } + } + return "Usage: set_port "; + } + + // 默认响应 - 模拟雷达命令响应 + return "ACK: " + command; + } + + void run() + { + unsigned char buffer[1500]; + socklen_t client_len = sizeof(client_addr_); + fd_set read_fds; + int max_fd; + + while (ros::ok()) { + FD_ZERO(&read_fds); + FD_SET(udp_socket_, &read_fds); + max_fd = udp_socket_; + + if (enable_command_server_) { + FD_SET(tcp_socket_, &read_fds); + if (tcp_socket_ > max_fd) max_fd = tcp_socket_; + } + + struct timeval timeout = {0, 100000}; // 100ms timeout + + int activity = select(max_fd + 1, &read_fds, NULL, NULL, &timeout); + + if (activity > 0) { + // 处理UDP数据 + if (FD_ISSET(udp_socket_, &read_fds)) { + int recv_len = recvfrom(udp_socket_, buffer, sizeof(buffer), 0, + (struct sockaddr*)&client_addr_, &client_len); + + if (recv_len > 0) { + int result = parseFrame(buffer, recv_len); + if (result == 3) { + processPointCloud(buffer, recv_len); + } + } + } + + // 处理TCP命令连接 + if (enable_command_server_ && FD_ISSET(tcp_socket_, &read_fds)) { + struct sockaddr_in client_addr; + socklen_t client_len = sizeof(client_addr); + int client_sock = accept(tcp_socket_, (struct sockaddr*)&client_addr, &client_len); + + if (client_sock >= 0) { + // 在新线程中处理命令 + std::thread(&TR360RadarNode::handleCommand, this, client_sock).detach(); + } + } + } + + ros::spinOnce(); + } + + close(udp_socket_); + if (enable_command_server_) { + close(tcp_socket_); + } + } +}; + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "tr360_radar_node"); + + TR360RadarNode node; + node.run(); + + return 0; +} \ No newline at end of file