Add boost padding

.github/workflows/industrial_ci_action.yml

@@ -19,5 +19,7 @@ jobs:
     runs-on: ubuntu-latest
     steps:
       - uses: actions/checkout@v3
+        with:
+          submodules: true
       - uses: 'ros-industrial/industrial_ci@master'
         env: {ROS_DISTRO: noetic, ROS_REPO: main, UPSTREAM_WORKSPACE: 'github:magazino/move_base_flex#master -move_base_flex/mbf_abstract_core -move_base_flex/mbf_abstract_nav -move_base_flex/mbf_costmap_core -move_base_flex/mbf_costmap_nav -move_base_flex/mbf_simple_nav'}

.gitmodules

@@ -0,0 +1,3 @@
+[submodule "costmap_2d/external/CavalierContours"]
+	path = costmap_2d/external/CavalierContours
+	url = https://github.com/jbuckmccready/CavalierContours.git

costmap_2d/CMakeLists.txt

@@ -25,11 +25,17 @@ find_package(catkin REQUIRED
 
 find_package(Eigen3 REQUIRED)
 find_package(Boost REQUIRED COMPONENTS system thread)
+
+# CavalierContours (header only)
+set(CAVALIER_CONTOURS_DIR ${CMAKE_CURRENT_SOURCE_DIR}/external/CavalierContours/include)
+add_definitions(-DCAVC_HEADER_ONLY)
+
 include_directories(
     include
     ${catkin_INCLUDE_DIRS}
     ${EIGEN3_INCLUDE_DIRS}
     ${Boost_INCLUDE_DIRS}
+    ${CAVALIER_CONTOURS_DIR}
 )
 
 add_definitions(${EIGEN3_DEFINITIONS})

costmap_2d/src/footprint.cpp

@@ -33,7 +33,9 @@
 #include <boost/algorithm/string.hpp>
 #include <costmap_2d/footprint.h>
 #include <costmap_2d/array_parser.h>
-#include<geometry_msgs/Point32.h>
+#include <geometry_msgs/Point32.h>
+#include <cavc/polylineoffset.hpp>
+#include <cavc/polyline.hpp>
 
 namespace costmap_2d
 {
@@ -137,12 +139,31 @@ void transformFootprint(double x, double y, double theta, const std::vector<geom
 
 void padFootprint(std::vector<geometry_msgs::Point>& footprint, double padding)
 {
-  // pad footprint in place
-  for (unsigned int i = 0; i < footprint.size(); i++)
+  if (footprint.size() < 3)
+  {
+    return;
+  }
+
+  cavc::Polyline<double> polyline;
+  polyline.isClosed() = true;
+
+  for (const auto& pt : footprint)
   {
-    geometry_msgs::Point& pt = footprint[ i ];
-    pt.x += sign0(pt.x) * padding;
-    pt.y += sign0(pt.y) * padding;
+    polyline.addVertex(pt.x, pt.y, 0);
+  }
+
+  const auto results = cavc::parallelOffset(polyline, -padding);
+  if (!results.empty())
+  {
+    footprint.clear();
+    for (const auto& vertex : results[0].vertexes())
+    {
+      geometry_msgs::Point p;
+      p.x = vertex.x();
+      p.y = vertex.y();
+      p.z = 0.0;
+      footprint.push_back(p);
+    }
   }
 }