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

0 专栏介绍

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

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

1 基于多边形的碰撞检测

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

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

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

2 碰撞检测仿真平台搭建

2.1 多边形实例

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

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

• 圆

计划继续更新的实例包括：

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

2.2 外部服务接口

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

1. 首先设计多边形请求服务

   ---
   PolygonStamped[] selection
   

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

   # 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插件制作案例(以工具栏和多点导航插件为例)

本文列举主要的步骤：

1. 首先是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}
   )
   

2. 在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>
   

3. 配置插件说明文件

   <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>
   

3 案例演示

3.1 功能介绍

通过Rviz2->Add New Tool添加Polygon Simulation插件

添加完毕后在Tool Properties面板可以看到Polygon Simulation插件的模板，其中Visualization是可视化属性，包括点的颜色大小、线的颜色、是否显示标记等；Polygon mode可以下拉选择采用哪种多边形实例

3.2 绘制多边形

以矩形为例，按住鼠标左键可以拉出一个矩形，在合适的位置松开，此时可以通过右侧面板的数值属性进行微调

比如我们调整一个45度的正方形出来

觉得满意后，点击鼠标右键确认绘图，产生标号

完整工程代码请联系下方博主名片获取

更多精彩专栏：

• 《ROS从入门到精通》
• 《Pytorch深度学习实战》
• 《机器学习强基计划》
• 《运动规划实战精讲》
• …