碰撞检测 | 基于ROS Rviz插件的多边形碰撞检测仿真平台

0 专栏介绍

🔥课设、毕设、创新竞赛必备！🔥本专栏涉及更高阶的运动规划算法轨迹优化实战，包括：曲线生成、碰撞检测、安全走廊、优化建模(QP、SQP、NMPC、iLQR等)、轨迹优化(梯度法、曲线法等)，每个算法都包含代码实现加深理解

🚀详情：运动规划实战进阶：轨迹优化篇

1 基于多边形的碰撞检测

基于多边形的碰撞检测是游戏开发、物理模拟以及许多其他领域中的一个重要研究方向。在计算机图形学中，多边形是构建三维模型和场景的基本单位，多边形碰撞引擎有利于渲染优化、营造光照阴影和沙尘等效果。

在自动驾驶和机器人领域，多边形碰撞检测在保证规划控制安全性方面发挥了极大的作用。以自动驾驶系统为例，自车使用传感器（如激光雷达、摄像头、雷达等）来检测道路上的障碍物，识别和建模道路、车道线、交通标志等，在这个过程中，通常基于多边形构建障碍物的几何模型，并判断是否与车辆发生碰撞。

在这里插入图片描述

本文的目标就是搭建一个多边形构造仿真平台，为后续基于多边形的碰撞检测、TTC等算法提供验证环境

在这里插入图片描述

2 碰撞检测仿真平台搭建

2.1 多边形实例

本平台目前设计的多边形实例有：

任意多边形
矩形(支持任意角度)

在这里插入图片描述

在这里插入图片描述
计划继续更新的实例包括：

$N$ 圆覆盖模型
动态矩形
动态圆形

2.2 外部服务接口

本文设计的仿真平台可以向外部程序提供多边形的具体信息，因此需要定义一个外部接口，这里采用自定义服务和消息的方式实现：

首先设计多边形请求服务
```
---
PolygonStamped[] selection
```

接着设计多边形数据消息，包含头部帧和点数据

# PolygonStamped.msg
std_msgs/Header headerPolygon polygon

# Polygon.msg
geometry_msgs/Point32[] points

通过上述接口串连其多边形数据，再设置服务器响应外部请求即可

bool PolygonSimulation::_callback(GetSelection::Request& /*req*/, GetSelection::Response& res)
{res.selection.reserve(polygons_.size());for (const auto& polygon : polygons_){// Skip selections with fewer than 3 pointsif (polygon->size() < 3)continue;PolygonStamped polygon_stamped;polygon_stamped.header.frame_id = context_->getFixedFrame().toStdString();for (const Ogre::Vector3& pt : polygon->points()){geometry_msgs::Point32 msg;msg.x = pt.x;msg.y = pt.y;msg.z = pt.z;polygon_stamped.polygon.points.push_back(msg);}res.selection.push_back(polygon_stamped);}return true;
}

2.3 Rviz插件化

Rviz插件开发的详细教程详见：

ROS从入门到精通2-4：Rviz插件制作案例(以多点导航插件为例)
ROS2从入门到精通2-4：Rviz2插件制作案例(以工具栏和多点导航插件为例)

本文列举主要的步骤：

首先是CMakeLists.txt文件，因为Rviz实际上是采用QT开发的，所以和一般的ROS工程相比，要增加一些QT库的链接配置

set(CMAKE_AUTOMOC ON)
SET(CMAKE_PREFIX_PATH "/usr/lib/x86_64-linux-gnu/cmake")
find_package(Qt5 COMPONENTS Core Gui Widgets UiTools REQUIRED)
QT5_WRAP_CPP(QT_HEADERS ${MOC_HEADERS})
set(QT_LIBRARIES Qt5::Widgets)....target_link_libraries(${PROJECT_NAME}${QT_LIBRARIES}${catkin_LIBRARIES}
)

在package.xml中配置插件导出

<?xml version="1.0"?>
<package format="2"><name>polygon_selection</name><version>0.0.0</version><description>Rviz polygon simulation tool</description><maintainer email="913982779@qq.com">Winter</maintainer><license>TODO</license><buildtool_depend>catkin</buildtool_depend><depend>roscpp</depend><depend>pluginlib</depend><depend>geometry_msgs</depend><depend>actionlib_msgs</depend><depend>rviz</depend><depend>std_msgs</depend><depend>message_generation</depend><depend>message_runtime</depend><export><rviz plugin="${prefix}/polygon_simulation_plugin.xml"/></export>
</package>

配置插件说明文件

<library path="lib/libpolygon_simulation"><class name="polygon_simulation/PolygonSimulation" type="polygon_simulation::PolygonSimulation" base_class_type="rviz::Tool"><description>Rviz tool for collecting a polygon selection boundary</description></class>
</library>