将src目录作为普通文件而非子模块添加
This commit is contained in:
1
src
1
src
Submodule src deleted from ec1c1e29cd
1
src/CMakeLists.txt
Symbolic link
1
src/CMakeLists.txt
Symbolic link
@@ -0,0 +1 @@
|
|||||||
|
/opt/ros/noetic/share/catkin/cmake/toplevel.cmake
|
||||||
146
src/README.md
Normal file
146
src/README.md
Normal file
@@ -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
|
||||||
41
src/build.sh
Executable file
41
src/build.sh
Executable file
@@ -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"
|
||||||
64
src/tr360_ros/CMakeLists.txt
Normal file
64
src/tr360_ros/CMakeLists.txt
Normal file
@@ -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
|
||||||
|
)
|
||||||
146
src/tr360_ros/README.md
Normal file
146
src/tr360_ros/README.md
Normal file
@@ -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
|
||||||
41
src/tr360_ros/build.sh
Executable file
41
src/tr360_ros/build.sh
Executable file
@@ -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"
|
||||||
47
src/tr360_ros/cfg/TR360Config.cfg
Normal file
47
src/tr360_ros/cfg/TR360Config.cfg
Normal file
@@ -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"))
|
||||||
177
src/tr360_ros/config/tr360.rviz
Normal file
177
src/tr360_ros/config/tr360.rviz
Normal file
@@ -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
|
||||||
83
src/tr360_ros/include/tr360_ros/tr360_types.h
Normal file
83
src/tr360_ros/include/tr360_ros/tr360_types.h
Normal file
@@ -0,0 +1,83 @@
|
|||||||
|
#ifndef TR360_ROS_TR360_TYPES_H
|
||||||
|
#define TR360_ROS_TR360_TYPES_H
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
|
||||||
|
// 雷达帧头结构
|
||||||
|
#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
|
||||||
53
src/tr360_ros/launch/tr360.launch
Normal file
53
src/tr360_ros/launch/tr360.launch
Normal file
@@ -0,0 +1,53 @@
|
|||||||
|
<launch>
|
||||||
|
<!-- TR360-3-60 雷达节点 -->
|
||||||
|
<node name="tr360_3_60_node" pkg="tr360_ros" type="tr360_node" output="screen">
|
||||||
|
<!-- 雷达类型配置 -->
|
||||||
|
<param name="radar_type" type="string" value="TR360-3-60" />
|
||||||
|
|
||||||
|
<!-- 雷达设备IP和端口 -->
|
||||||
|
<param name="radar_ip" type="string" value="192.168.1.80" />
|
||||||
|
<param name="radar_port" type="int" value="10123" />
|
||||||
|
|
||||||
|
<!-- 本地接收IP和端口 -->
|
||||||
|
<param name="local_ip" type="string" value="192.168.1.193" />
|
||||||
|
<param name="local_port" type="int" value="10195" />
|
||||||
|
|
||||||
|
<!-- 雷达数量配置 -->
|
||||||
|
<param name="max_radars" type="int" value="2" />
|
||||||
|
|
||||||
|
<!-- 命令服务器配置 -->
|
||||||
|
<param name="enable_command_server" type="bool" value="true" />
|
||||||
|
<param name="max_history_size" type="int" value="100" />
|
||||||
|
</node>
|
||||||
|
|
||||||
|
<!-- TR360-4-60 雷达节点 -->
|
||||||
|
<node name="tr360_4_60_node" pkg="tr360_ros" type="tr360_node" output="screen">
|
||||||
|
<!-- 雷达类型配置 -->
|
||||||
|
<param name="radar_type" type="string" value="TR360-4-60" />
|
||||||
|
|
||||||
|
<!-- 雷达设备IP和端口 -->
|
||||||
|
<param name="radar_ip" type="string" value="192.168.1.81" />
|
||||||
|
<param name="radar_port" type="int" value="20223" />
|
||||||
|
|
||||||
|
<!-- 本地接收IP和端口 -->
|
||||||
|
<param name="local_ip" type="string" value="192.168.1.193" />
|
||||||
|
<param name="local_port" type="int" value="20295" />
|
||||||
|
|
||||||
|
<!-- 雷达数量配置 -->
|
||||||
|
<param name="max_radars" type="int" value="1" />
|
||||||
|
|
||||||
|
<!-- 命令服务器配置 -->
|
||||||
|
<param name="enable_command_server" type="bool" value="true" />
|
||||||
|
<param name="max_history_size" type="int" value="100" />
|
||||||
|
</node>
|
||||||
|
|
||||||
|
<!-- 点云过滤参数（全局参数） -->
|
||||||
|
<param name="min_range" type="double" value="0.0" />
|
||||||
|
<param name="max_range" type="double" value="100.0" />
|
||||||
|
<param name="min_intensity" type="double" value="0.0" />
|
||||||
|
<param name="max_intensity" type="double" value="100.0" />
|
||||||
|
|
||||||
|
<!-- 启动RViz进行点云可视化 -->
|
||||||
|
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find tr360_ros)/config/tr360.rviz" />
|
||||||
|
|
||||||
|
</launch>
|
||||||
25
src/tr360_ros/package.xml
Normal file
25
src/tr360_ros/package.xml
Normal file
@@ -0,0 +1,25 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<package format="2">
|
||||||
|
<name>tr360_ros</name>
|
||||||
|
<version>0.1.0</version>
|
||||||
|
<description>ROS package for TR360 radar point cloud processing</description>
|
||||||
|
<maintainer email="yemai@example.com">yemai</maintainer>
|
||||||
|
<license>BSD</license>
|
||||||
|
|
||||||
|
<buildtool_depend>catkin</buildtool_depend>
|
||||||
|
|
||||||
|
<build_depend>roscpp</build_depend>
|
||||||
|
<build_depend>sensor_msgs</build_depend>
|
||||||
|
<build_depend>pcl_ros</build_depend>
|
||||||
|
<build_depend>pcl_conversions</build_depend>
|
||||||
|
<build_depend>dynamic_reconfigure</build_depend>
|
||||||
|
|
||||||
|
<exec_depend>roscpp</exec_depend>
|
||||||
|
<exec_depend>sensor_msgs</exec_depend>
|
||||||
|
<exec_depend>pcl_ros</exec_depend>
|
||||||
|
<exec_depend>pcl_conversions</exec_depend>
|
||||||
|
<exec_depend>dynamic_reconfigure</exec_depend>
|
||||||
|
|
||||||
|
<export>
|
||||||
|
</export>
|
||||||
|
</package>
|
||||||
66
src/tr360_ros/src/test_publisher.cpp
Normal file
66
src/tr360_ros/src/test_publisher.cpp
Normal file
@@ -0,0 +1,66 @@
|
|||||||
|
#include <ros/ros.h>
|
||||||
|
#include <sensor_msgs/PointCloud2.h>
|
||||||
|
#include <pcl/point_types.h>
|
||||||
|
#include <pcl/point_cloud.h>
|
||||||
|
#include <pcl_conversions/pcl_conversions.h>
|
||||||
|
#include <cmath>
|
||||||
|
|
||||||
|
class TestPublisher
|
||||||
|
{
|
||||||
|
private:
|
||||||
|
ros::NodeHandle nh_;
|
||||||
|
ros::Publisher cloud_pub_;
|
||||||
|
ros::Timer timer_;
|
||||||
|
|
||||||
|
public:
|
||||||
|
TestPublisher()
|
||||||
|
{
|
||||||
|
cloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("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<pcl::PointXYZI> 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;
|
||||||
|
}
|
||||||
459
src/tr360_ros/src/tr360_node.cpp
Normal file
459
src/tr360_ros/src/tr360_node.cpp
Normal file
@@ -0,0 +1,459 @@
|
|||||||
|
#include <ros/ros.h>
|
||||||
|
#include <sensor_msgs/PointCloud2.h>
|
||||||
|
#include <pcl/point_types.h>
|
||||||
|
#include <pcl/point_cloud.h>
|
||||||
|
#include <pcl_conversions/pcl_conversions.h>
|
||||||
|
#include <dynamic_reconfigure/server.h>
|
||||||
|
#include "tr360_ros/tr360_types.h"
|
||||||
|
#include <sys/socket.h>
|
||||||
|
#include <netinet/in.h>
|
||||||
|
#include <arpa/inet.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <thread>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
class TR360RadarNode
|
||||||
|
{
|
||||||
|
private:
|
||||||
|
ros::NodeHandle nh_;
|
||||||
|
std::vector<ros::Publisher> 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<std::string> 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<std::string>("radar_type", radar_type_, "TR360-3-60");
|
||||||
|
private_nh.param<std::string>("radar_ip", radar_ip_, "192.168.1.80");
|
||||||
|
private_nh.param("radar_port", radar_port_, 10123);
|
||||||
|
private_nh.param<std::string>("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<sensor_msgs::PointCloud2>(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<pcl::PointXYZI> 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 <port_number>";
|
||||||
|
}
|
||||||
|
|
||||||
|
// 默认响应 - 模拟雷达命令响应
|
||||||
|
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;
|
||||||
|
}
|
||||||
Reference in New Issue
Block a user