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

0 专栏介绍

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

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

1 基于多边形的碰撞检测

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

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

在这里插入图片描述

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

在这里插入图片描述

2 碰撞检测仿真平台搭建

2.1 多边形实例

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

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

在这里插入图片描述

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

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

2.2 外部服务接口

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

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

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

# PolygonStamped.msg
std_msgs/Header header

Polygon 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 points
    if (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>