From 75104e84fcb8d01abcde387b2e11feb04a931a7a Mon Sep 17 00:00:00 2001 From: Renan Salles Date: Fri, 1 Aug 2025 16:29:59 +0900 Subject: [PATCH 1/9] add cavalier padding --- .gitmodules | 3 +++ costmap_2d/CMakeLists.txt | 6 +++++ costmap_2d/external/CavalierContours | 1 + costmap_2d/src/footprint.cpp | 33 +++++++++++++++++++++++----- 4 files changed, 37 insertions(+), 6 deletions(-) create mode 100644 .gitmodules create mode 160000 costmap_2d/external/CavalierContours diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000000..ad645633be --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "costmap_2d/external/CavalierContours"] + path = costmap_2d/external/CavalierContours + url = https://github.com/jbuckmccready/CavalierContours.git diff --git a/costmap_2d/CMakeLists.txt b/costmap_2d/CMakeLists.txt index 39ec3360ff..11f67ad487 100644 --- a/costmap_2d/CMakeLists.txt +++ b/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}) diff --git a/costmap_2d/external/CavalierContours b/costmap_2d/external/CavalierContours new file mode 160000 index 0000000000..31a012947a --- /dev/null +++ b/costmap_2d/external/CavalierContours @@ -0,0 +1 @@ +Subproject commit 31a012947aa2e7e9474e2ec90502825afe8b99a4 diff --git a/costmap_2d/src/footprint.cpp b/costmap_2d/src/footprint.cpp index e830cd8e66..75e7274307 100644 --- a/costmap_2d/src/footprint.cpp +++ b/costmap_2d/src/footprint.cpp @@ -33,7 +33,9 @@ #include #include #include -#include +#include +#include +#include namespace costmap_2d { @@ -137,12 +139,31 @@ void transformFootprint(double x, double y, double theta, const std::vector& footprint, double padding) { - // pad footprint in place - for (unsigned int i = 0; i < footprint.size(); i++) + if (footprint.size() < 3) + { + return; + } + + cavc::Polyline 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); + } } } From 502215916c575b8d316e7c40a17c982d80ff2965 Mon Sep 17 00:00:00 2001 From: Renan Salles Date: Mon, 4 Aug 2025 16:29:44 +0900 Subject: [PATCH 2/9] add submodule --- .github/workflows/industrial_ci_action.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml index 83195a4d85..66fed0f487 100644 --- a/.github/workflows/industrial_ci_action.yml +++ b/.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'} From de4b9e2ab852eb1aef54075096b9b59327fd40dc Mon Sep 17 00:00:00 2001 From: Renan Salles Date: Tue, 5 Aug 2025 10:27:17 +0900 Subject: [PATCH 3/9] use boost --- .gitmodules | 3 -- costmap_2d/external/CavalierContours | 1 - costmap_2d/src/footprint.cpp | 69 ++++++++++++++++++++-------- 3 files changed, 51 insertions(+), 22 deletions(-) delete mode 160000 costmap_2d/external/CavalierContours diff --git a/.gitmodules b/.gitmodules index ad645633be..e69de29bb2 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +0,0 @@ -[submodule "costmap_2d/external/CavalierContours"] - path = costmap_2d/external/CavalierContours - url = https://github.com/jbuckmccready/CavalierContours.git diff --git a/costmap_2d/external/CavalierContours b/costmap_2d/external/CavalierContours deleted file mode 160000 index 31a012947a..0000000000 --- a/costmap_2d/external/CavalierContours +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 31a012947aa2e7e9474e2ec90502825afe8b99a4 diff --git a/costmap_2d/src/footprint.cpp b/costmap_2d/src/footprint.cpp index 75e7274307..d61c130e28 100644 --- a/costmap_2d/src/footprint.cpp +++ b/costmap_2d/src/footprint.cpp @@ -34,8 +34,10 @@ #include #include #include -#include -#include +#include +#include +#include +#include namespace costmap_2d { @@ -139,34 +141,65 @@ void transformFootprint(double x, double y, double theta, const std::vector& footprint, double padding) { + namespace bg = boost::geometry; + using BoostPoint = bg::model::d2::point_xy; + using BoostPolygon = bg::model::polygon; + if (footprint.size() < 3) { + ROS_WARN_NAMED("costmap_2d", "Footprint has fewer than 3 points. Skipping padding."); return; } - cavc::Polyline polyline; - polyline.isClosed() = true; - + BoostPolygon input_poly; for (const auto& pt : footprint) { - polyline.addVertex(pt.x, pt.y, 0); + bg::append(input_poly.outer(), BoostPoint(pt.x, pt.y)); } - const auto results = cavc::parallelOffset(polyline, -padding); - if (!results.empty()) + // ensure closure + if (footprint.front().x != footprint.back().x || footprint.front().y != footprint.back().y) { - 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); - } + bg::append(input_poly.outer(), BoostPoint(footprint.front().x, footprint.front().y)); + } + + // correct polygon before validation + bg::correct(input_poly); + + std::string reason; + if (!bg::is_valid(input_poly, reason)) + { + ROS_WARN_STREAM_NAMED("costmap_2d", + "Input polygon is STILL invalid after correction. Skipping padding. Reason: " << reason); + return; } -} + std::vector buffered_result; + bg::strategy::buffer::distance_symmetric distance_strategy(padding); + bg::strategy::buffer::join_miter join_strategy(5.0); // <-- Sharp edges + bg::strategy::buffer::end_flat end_strategy; // Use flat ends for open polygons + bg::strategy::buffer::point_square circle_strategy; // Square buffer around points + bg::strategy::buffer::side_straight side_strategy; + + bg::buffer(input_poly, buffered_result, distance_strategy, side_strategy, join_strategy, end_strategy, + circle_strategy); + + if (buffered_result.empty()) + { + ROS_WARN_NAMED("costmap_2d", "Buffer operation produced no results. Skipping padding."); + return; + } + + footprint.clear(); + for (const auto& pt : buffered_result.front().outer()) + { + geometry_msgs::Point p; + p.x = pt.x(); + p.y = pt.y(); + p.z = 0.0; + footprint.push_back(p); + } +} std::vector makeFootprintFromRadius(double radius) { From 0dd5bc57f3f5776b0ba03d094a0a2c0c342a2f40 Mon Sep 17 00:00:00 2001 From: Renan Salles Date: Tue, 5 Aug 2025 10:28:38 +0900 Subject: [PATCH 4/9] rm --- .github/workflows/industrial_ci_action.yml | 2 -- costmap_2d/CMakeLists.txt | 5 ----- 2 files changed, 7 deletions(-) diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml index 66fed0f487..83195a4d85 100644 --- a/.github/workflows/industrial_ci_action.yml +++ b/.github/workflows/industrial_ci_action.yml @@ -19,7 +19,5 @@ 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'} diff --git a/costmap_2d/CMakeLists.txt b/costmap_2d/CMakeLists.txt index 11f67ad487..2ebf33ef5f 100644 --- a/costmap_2d/CMakeLists.txt +++ b/costmap_2d/CMakeLists.txt @@ -26,16 +26,11 @@ 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}) From 7ce3c858be3536ddcb2dff8da0a5c0495322877f Mon Sep 17 00:00:00 2001 From: Renan Salles Date: Tue, 5 Aug 2025 10:30:21 +0900 Subject: [PATCH 5/9] rm --- .gitmodules | 0 costmap_2d/CMakeLists.txt | 1 - costmap_2d/src/footprint.cpp | 2 +- 3 files changed, 1 insertion(+), 2 deletions(-) delete mode 100644 .gitmodules diff --git a/.gitmodules b/.gitmodules deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/costmap_2d/CMakeLists.txt b/costmap_2d/CMakeLists.txt index 2ebf33ef5f..39ec3360ff 100644 --- a/costmap_2d/CMakeLists.txt +++ b/costmap_2d/CMakeLists.txt @@ -25,7 +25,6 @@ find_package(catkin REQUIRED find_package(Eigen3 REQUIRED) find_package(Boost REQUIRED COMPONENTS system thread) - include_directories( include ${catkin_INCLUDE_DIRS} diff --git a/costmap_2d/src/footprint.cpp b/costmap_2d/src/footprint.cpp index d61c130e28..fb7f3c2932 100644 --- a/costmap_2d/src/footprint.cpp +++ b/costmap_2d/src/footprint.cpp @@ -27,7 +27,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include +#include #include #include #include From f19c3fe2a1d62bef4db0874878bbb3e13dc2f28a Mon Sep 17 00:00:00 2001 From: Renan Salles Date: Fri, 8 Aug 2025 15:34:44 +0900 Subject: [PATCH 6/9] fix test --- costmap_2d/src/footprint.cpp | 12 +- costmap_2d/test/footprint_tests.cpp | 198 +++++++++++++++++----------- 2 files changed, 132 insertions(+), 78 deletions(-) diff --git a/costmap_2d/src/footprint.cpp b/costmap_2d/src/footprint.cpp index fb7f3c2932..1351a8d0a6 100644 --- a/costmap_2d/src/footprint.cpp +++ b/costmap_2d/src/footprint.cpp @@ -190,8 +190,12 @@ void padFootprint(std::vector& footprint, double padding) return; } + // simplify to remove collinear points + BoostPolygon simplified_poly; + bg::simplify(buffered_result.front(), simplified_poly, 1e-6); + footprint.clear(); - for (const auto& pt : buffered_result.front().outer()) + for (const auto& pt : simplified_poly.outer()) { geometry_msgs::Point p; p.x = pt.x(); @@ -199,6 +203,12 @@ void padFootprint(std::vector& footprint, double padding) p.z = 0.0; footprint.push_back(p); } + + // Remove closing point if same as first + if (footprint.size() > 1 && footprint.front().x == footprint.back().x && footprint.front().y == footprint.back().y) + { + footprint.pop_back(); + } } std::vector makeFootprintFromRadius(double radius) diff --git a/costmap_2d/test/footprint_tests.cpp b/costmap_2d/test/footprint_tests.cpp index 356b9b671b..2224a0db1b 100644 --- a/costmap_2d/test/footprint_tests.cpp +++ b/costmap_2d/test/footprint_tests.cpp @@ -45,126 +45,170 @@ using namespace costmap_2d; tf2_ros::TransformListener* tfl_; tf2_ros::Buffer* tf_; -TEST( Costmap2DROS, unpadded_footprint_from_string_param ) +bool pointEqual(const geometry_msgs::Point& a, const geometry_msgs::Point& b, double eps = 1e-6) { - Costmap2DROS cm( "unpadded/string", *tf_ ); - std::vector footprint = cm.getRobotFootprint(); - EXPECT_EQ( 3, footprint.size() ); + return std::fabs(a.x - b.x) < eps && std::fabs(a.y - b.y) < eps && std::fabs(a.z - b.z) < eps; +} - EXPECT_EQ( 1.0f, footprint[ 0 ].x ); - EXPECT_EQ( 1.0f, footprint[ 0 ].y ); - EXPECT_EQ( 0.0f, footprint[ 0 ].z ); +bool pointLess(const geometry_msgs::Point& a, const geometry_msgs::Point& b) +{ + if (a.x != b.x) + { + return a.x < b.x; + } + if (a.y != b.y) + { + return a.y < b.y; + } + return a.z < b.z; +} - EXPECT_EQ( -1.0f, footprint[ 1 ].x ); - EXPECT_EQ( 1.0f, footprint[ 1 ].y ); - EXPECT_EQ( 0.0f, footprint[ 1 ].z ); +bool compareFootprint(std::vector& expected_footprint, + std::vector& footprint) +{ + std::string expected_log; + std::string footprint_log; + for (const auto& point : expected_footprint) + { + expected_log += + "Point(" + std::to_string(point.x) + ", " + std::to_string(point.y) + ", " + std::to_string(point.z) + ")\n"; + } + for (const auto& point : footprint) + { + footprint_log += + "Point(" + std::to_string(point.x) + ", " + std::to_string(point.y) + ", " + std::to_string(point.z) + ")\n"; + } + ROS_ERROR_STREAM("Expected footprint points: \n" << expected_log); + ROS_ERROR_STREAM("Actual footprint points: \n" << footprint_log); + + if (footprint.size() != expected_footprint.size()) + { + ROS_ERROR("Footprint size mismatch: expected %zu points, got %zu points.", expected_footprint.size(), + footprint.size()); + return false; + } + + std::sort(footprint.begin(), footprint.end(), pointLess); + std::sort(expected_footprint.begin(), expected_footprint.end(), pointLess); + + for (size_t i = 0; i < footprint.size(); ++i) + { + if (!pointEqual(footprint[i], expected_footprint[i])) + { + ROS_ERROR("Footprint point mismatch at index %zu: expected (%f, %f, %f), got (%f, %f, %f).", i, + expected_footprint[i].x, expected_footprint[i].y, expected_footprint[i].z, footprint[i].x, + footprint[i].y, footprint[i].z); + return false; + } + } + return true; +} - EXPECT_EQ( -1.0f, footprint[ 2 ].x ); - EXPECT_EQ( -1.0f, footprint[ 2 ].y ); - EXPECT_EQ( 0.0f, footprint[ 2 ].z ); +geometry_msgs::Point createPoint(double x, double y, double z = 0.0) +{ + geometry_msgs::Point point; + point.x = x; + point.y = y; + point.z = z; + return point; } -TEST( Costmap2DROS, padded_footprint_from_string_param ) +TEST(Costmap2DROS, unpadded_footprint_from_string_param) { - Costmap2DROS cm( "padded/string", *tf_ ); + Costmap2DROS cm("unpadded/string", *tf_); std::vector footprint = cm.getRobotFootprint(); - EXPECT_EQ( 3, footprint.size() ); - - EXPECT_EQ( 1.5f, footprint[ 0 ].x ); - EXPECT_EQ( 1.5f, footprint[ 0 ].y ); - EXPECT_EQ( 0.0f, footprint[ 0 ].z ); - - EXPECT_EQ( -1.5f, footprint[ 1 ].x ); - EXPECT_EQ( 1.5f, footprint[ 1 ].y ); - EXPECT_EQ( 0.0f, footprint[ 1 ].z ); + std::vector expected_footprint = { + createPoint(1.0, 1.0, 0.0), + createPoint(-1.0, -1.0, 0.0), + createPoint(-1.0, 1.0, 0.0), + }; + EXPECT_TRUE(compareFootprint(expected_footprint, footprint)); +} - EXPECT_EQ( -1.5f, footprint[ 2 ].x ); - EXPECT_EQ( -1.5f, footprint[ 2 ].y ); - EXPECT_EQ( 0.0f, footprint[ 2 ].z ); +TEST(Costmap2DROS, padded_footprint_from_string_param) +{ + Costmap2DROS cm("padded/string", *tf_); + std::vector footprint = cm.getRobotFootprint(); + std::vector expected_footprint = { + createPoint(2.207107, 1.5, 0.0), + createPoint(-1.5, 1.5, 0.0), + createPoint(-1.5, -2.207107, 0.0), + }; + EXPECT_TRUE(compareFootprint(expected_footprint, footprint)); } -TEST( Costmap2DROS, radius_param ) +TEST(Costmap2DROS, radius_param) { - Costmap2DROS cm( "radius/sub", *tf_ ); + Costmap2DROS cm("radius/sub", *tf_); std::vector footprint = cm.getRobotFootprint(); // Circular robot has 16-point footprint auto-generated. - EXPECT_EQ( 16, footprint.size() ); + EXPECT_EQ(16, footprint.size()); // Check the first point - EXPECT_EQ( 10.0f, footprint[ 0 ].x ); - EXPECT_EQ( 0.0f, footprint[ 0 ].y ); - EXPECT_EQ( 0.0f, footprint[ 0 ].z ); + EXPECT_NEAR(-10.0f, footprint[0].x, 0.0001); + EXPECT_NEAR(0.0f, footprint[0].y, 0.0001); + EXPECT_EQ(0.0f, footprint[0].z); // Check the 4th point, which should be 90 degrees around the circle from the first. - EXPECT_NEAR( 0.0f, footprint[ 4 ].x, 0.0001 ); - EXPECT_NEAR( 10.0f, footprint[ 4 ].y, 0.0001 ); - EXPECT_EQ( 0.0f, footprint[ 4 ].z ); + EXPECT_NEAR(0.0f, footprint[4].x, 0.0001); + EXPECT_NEAR(10.0f, footprint[4].y, 0.0001); + EXPECT_EQ(0.0f, footprint[4].z); } -TEST( Costmap2DROS, footprint_from_xmlrpc_param ) +TEST(Costmap2DROS, footprint_from_xmlrpc_param) { - Costmap2DROS cm( "xmlrpc", *tf_ ); + Costmap2DROS cm("xmlrpc", *tf_); std::vector footprint = cm.getRobotFootprint(); - EXPECT_EQ( 4, footprint.size() ); - - EXPECT_EQ( 0.1f, footprint[ 0 ].x ); - EXPECT_EQ( 0.1f, footprint[ 0 ].y ); - EXPECT_EQ( 0.0f, footprint[ 0 ].z ); - - EXPECT_EQ( -0.1f, footprint[ 1 ].x ); - EXPECT_EQ( 0.1f, footprint[ 1 ].y ); - EXPECT_EQ( 0.0f, footprint[ 1 ].z ); - - EXPECT_EQ( -0.1f, footprint[ 2 ].x ); - EXPECT_EQ( -0.1f, footprint[ 2 ].y ); - EXPECT_EQ( 0.0f, footprint[ 2 ].z ); - - EXPECT_EQ( 0.1f, footprint[ 3 ].x ); - EXPECT_EQ( -0.1f, footprint[ 3 ].y ); - EXPECT_EQ( 0.0f, footprint[ 3 ].z ); + std::vector expected_footprint = { + createPoint(0.1, 0.1, 0.0), + createPoint(-0.1, 0.1, 0.0), + createPoint(-0.1, -0.1, 0.0), + createPoint(0.1, -0.1, 0.0), + }; + EXPECT_TRUE(compareFootprint(expected_footprint, footprint)); } -TEST( Costmap2DROS, footprint_from_same_level_param ) +TEST(Costmap2DROS, footprint_from_same_level_param) { - Costmap2DROS cm( "same_level", *tf_ ); + Costmap2DROS cm("same_level", *tf_); std::vector footprint = cm.getRobotFootprint(); - EXPECT_EQ( 3, footprint.size() ); + EXPECT_EQ(3, footprint.size()); - EXPECT_EQ( 1.0f, footprint[ 0 ].x ); - EXPECT_EQ( 2.0f, footprint[ 0 ].y ); - EXPECT_EQ( 0.0f, footprint[ 0 ].z ); + EXPECT_EQ(1.0f, footprint[0].x); + EXPECT_EQ(2.0f, footprint[0].y); + EXPECT_EQ(0.0f, footprint[0].z); - EXPECT_EQ( 3.0f, footprint[ 1 ].x ); - EXPECT_EQ( 4.0f, footprint[ 1 ].y ); - EXPECT_EQ( 0.0f, footprint[ 1 ].z ); + EXPECT_EQ(3.0f, footprint[1].x); + EXPECT_EQ(4.0f, footprint[1].y); + EXPECT_EQ(0.0f, footprint[1].z); - EXPECT_EQ( 5.0f, footprint[ 2 ].x ); - EXPECT_EQ( 6.0f, footprint[ 2 ].y ); - EXPECT_EQ( 0.0f, footprint[ 2 ].z ); + EXPECT_EQ(5.0f, footprint[2].x); + EXPECT_EQ(6.0f, footprint[2].y); + EXPECT_EQ(0.0f, footprint[2].z); } -TEST( Costmap2DROS, footprint_from_xmlrpc_param_failure ) +TEST(Costmap2DROS, footprint_from_xmlrpc_param_failure) { - ASSERT_ANY_THROW( Costmap2DROS cm( "xmlrpc_fail", *tf_ )); + ASSERT_ANY_THROW(Costmap2DROS cm("xmlrpc_fail", *tf_)); } -TEST( Costmap2DROS, footprint_empty ) +TEST(Costmap2DROS, footprint_empty) { - Costmap2DROS cm( "empty", *tf_ ); + Costmap2DROS cm("empty", *tf_); std::vector footprint = cm.getRobotFootprint(); // With no specification of footprint or radius, defaults to 0.46 meter radius plus 0.01 meter padding. - EXPECT_EQ( 16, footprint.size() ); + EXPECT_EQ(16, footprint.size()); - EXPECT_NEAR( 0.47f, footprint[ 0 ].x, 0.0001 ); - EXPECT_NEAR( 0.0f, footprint[ 0 ].y, 0.0001 ); - EXPECT_EQ( 0.0f, footprint[ 0 ].z ); + EXPECT_NEAR(-0.470196f, footprint[0].x, 0.0001); + EXPECT_NEAR(0.0f, footprint[0].y, 0.0001); + EXPECT_EQ(0.0f, footprint[0].z); } int main(int argc, char** argv) { ros::init(argc, argv, "footprint_tests_node"); - tf_ = new tf2_ros::Buffer( ros::Duration( 10 )); + tf_ = new tf2_ros::Buffer(ros::Duration(10)); tfl_ = new tf2_ros::TransformListener(*tf_); // This empty transform is added to satisfy the constructor of @@ -175,7 +219,7 @@ int main(int argc, char** argv) base_rel_map.child_frame_id = "base_link"; base_rel_map.header.frame_id = "map"; base_rel_map.header.stamp = ros::Time::now(); - tf_->setTransform( base_rel_map, "footprint_tests" ); + tf_->setTransform(base_rel_map, "footprint_tests"); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); From 765e1ec960d0f7f4ca095ddaaceb648c7b64e0b7 Mon Sep 17 00:00:00 2001 From: Renan Salles Date: Fri, 8 Aug 2025 15:47:10 +0900 Subject: [PATCH 7/9] remove logging --- costmap_2d/test/footprint_tests.cpp | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/costmap_2d/test/footprint_tests.cpp b/costmap_2d/test/footprint_tests.cpp index 2224a0db1b..88ff8f2a3f 100644 --- a/costmap_2d/test/footprint_tests.cpp +++ b/costmap_2d/test/footprint_tests.cpp @@ -66,21 +66,6 @@ bool pointLess(const geometry_msgs::Point& a, const geometry_msgs::Point& b) bool compareFootprint(std::vector& expected_footprint, std::vector& footprint) { - std::string expected_log; - std::string footprint_log; - for (const auto& point : expected_footprint) - { - expected_log += - "Point(" + std::to_string(point.x) + ", " + std::to_string(point.y) + ", " + std::to_string(point.z) + ")\n"; - } - for (const auto& point : footprint) - { - footprint_log += - "Point(" + std::to_string(point.x) + ", " + std::to_string(point.y) + ", " + std::to_string(point.z) + ")\n"; - } - ROS_ERROR_STREAM("Expected footprint points: \n" << expected_log); - ROS_ERROR_STREAM("Actual footprint points: \n" << footprint_log); - if (footprint.size() != expected_footprint.size()) { ROS_ERROR("Footprint size mismatch: expected %zu points, got %zu points.", expected_footprint.size(), From b4c07e05512edb8833789ad8dfabc16a6e14d4f0 Mon Sep 17 00:00:00 2001 From: Renan Salles Date: Mon, 18 Aug 2025 17:11:46 +0900 Subject: [PATCH 8/9] add helper --- costmap_2d/include/costmap_2d/footprint.h | 13 +++++ costmap_2d/src/footprint.cpp | 69 +++++++++++++++-------- 2 files changed, 58 insertions(+), 24 deletions(-) diff --git a/costmap_2d/include/costmap_2d/footprint.h b/costmap_2d/include/costmap_2d/footprint.h index 9beda1f6d2..1c297f67b4 100644 --- a/costmap_2d/include/costmap_2d/footprint.h +++ b/costmap_2d/include/costmap_2d/footprint.h @@ -43,6 +43,9 @@ #include #include #include +#include +#include +#include namespace costmap_2d { @@ -77,6 +80,16 @@ geometry_msgs::Polygon toPolygon(std::vector pt */ std::vector toPointVector(geometry_msgs::Polygon polygon); +/** + * @brief Convert std::vector to BoostPolygon. + */ +boost::geometry::model::polygon> toBoostPolygon(const std::vector& polygon); + +/** + * @brief Convert BoostPolygon to std::vector + */ +std::vector fromBoostPolygon(const boost::geometry::model::polygon>& polygon); + /** * @brief Given a pose and base footprint, build the oriented footprint of the robot (list of Points) * @param x The x position of the robot diff --git a/costmap_2d/src/footprint.cpp b/costmap_2d/src/footprint.cpp index 1351a8d0a6..0111a12425 100644 --- a/costmap_2d/src/footprint.cpp +++ b/costmap_2d/src/footprint.cpp @@ -139,32 +139,67 @@ void transformFootprint(double x, double y, double theta, const std::vector& footprint, double padding) +boost::geometry::model::polygon> toBoostPolygon(const std::vector& polygon) { namespace bg = boost::geometry; using BoostPoint = bg::model::d2::point_xy; using BoostPolygon = bg::model::polygon; - if (footprint.size() < 3) + if (polygon.size() < 3) { ROS_WARN_NAMED("costmap_2d", "Footprint has fewer than 3 points. Skipping padding."); - return; + return BoostPolygon(); } - BoostPolygon input_poly; - for (const auto& pt : footprint) + BoostPolygon boost_poly; + for (const auto& pt : polygon) { - bg::append(input_poly.outer(), BoostPoint(pt.x, pt.y)); + bg::append(boost_poly.outer(), BoostPoint(pt.x, pt.y)); } // ensure closure - if (footprint.front().x != footprint.back().x || footprint.front().y != footprint.back().y) + if (polygon.front().x != polygon.back().x || polygon.front().y != polygon.back().y) { - bg::append(input_poly.outer(), BoostPoint(footprint.front().x, footprint.front().y)); + bg::append(boost_poly.outer(), BoostPoint(polygon.front().x, polygon.front().y)); } // correct polygon before validation - bg::correct(input_poly); + bg::correct(boost_poly); + return boost_poly; +} + +std::vector fromBoostPolygon(const boost::geometry::model::polygon>& polygon) +{ + std::vector footprint; + for (const auto& pt : polygon.outer()) + { + geometry_msgs::Point p; + p.x = pt.x(); + p.y = pt.y(); + p.z = 0.0; + footprint.push_back(p); + } + + // Remove closing point if same as first + if (footprint.size() > 1 && footprint.front().x == footprint.back().x && footprint.front().y == footprint.back().y) + { + footprint.pop_back(); + } + return footprint; +} + + +void padFootprint(std::vector& footprint, double padding) +{ + namespace bg = boost::geometry; + using BoostPoint = bg::model::d2::point_xy; + using BoostPolygon = bg::model::polygon; + + const auto input_poly = toBoostPolygon(footprint); + if (input_poly.outer().size() < 3) + { + return; + } std::string reason; if (!bg::is_valid(input_poly, reason)) @@ -194,21 +229,7 @@ void padFootprint(std::vector& footprint, double padding) BoostPolygon simplified_poly; bg::simplify(buffered_result.front(), simplified_poly, 1e-6); - footprint.clear(); - for (const auto& pt : simplified_poly.outer()) - { - geometry_msgs::Point p; - p.x = pt.x(); - p.y = pt.y(); - p.z = 0.0; - footprint.push_back(p); - } - - // Remove closing point if same as first - if (footprint.size() > 1 && footprint.front().x == footprint.back().x && footprint.front().y == footprint.back().y) - { - footprint.pop_back(); - } + footprint = fromBoostPolygon(simplified_poly); } std::vector makeFootprintFromRadius(double radius) From c27800f2138de904d6027d00fe4459a683ef5d75 Mon Sep 17 00:00:00 2001 From: Renan Salles Date: Mon, 18 Aug 2025 17:22:00 +0900 Subject: [PATCH 9/9] comment --- costmap_2d/src/footprint.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/costmap_2d/src/footprint.cpp b/costmap_2d/src/footprint.cpp index 0111a12425..36f386eca0 100644 --- a/costmap_2d/src/footprint.cpp +++ b/costmap_2d/src/footprint.cpp @@ -147,7 +147,7 @@ boost::geometry::model::polygon> to if (polygon.size() < 3) { - ROS_WARN_NAMED("costmap_2d", "Footprint has fewer than 3 points. Skipping padding."); + ROS_WARN_NAMED("costmap_2d", "Footprint has fewer than 3 points. Skipping..."); return BoostPolygon(); }