diff --git a/costmap_2d/include/costmap_2d/footprint.h b/costmap_2d/include/costmap_2d/footprint.h index 9beda1f6d..1c297f67b 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 e830cd8e6..36f386eca 100644 --- a/costmap_2d/src/footprint.cpp +++ b/costmap_2d/src/footprint.cpp @@ -27,13 +27,17 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include +#include #include #include #include #include #include -#include +#include +#include +#include +#include +#include namespace costmap_2d { @@ -135,18 +139,99 @@ void transformFootprint(double x, double y, double theta, const std::vector& footprint, double padding) +boost::geometry::model::polygon> toBoostPolygon(const std::vector& polygon) { - // pad footprint in place - for (unsigned int i = 0; i < footprint.size(); i++) + namespace bg = boost::geometry; + using BoostPoint = bg::model::d2::point_xy; + using BoostPolygon = bg::model::polygon; + + if (polygon.size() < 3) + { + ROS_WARN_NAMED("costmap_2d", "Footprint has fewer than 3 points. Skipping..."); + return BoostPolygon(); + } + + BoostPolygon boost_poly; + for (const auto& pt : polygon) + { + bg::append(boost_poly.outer(), BoostPoint(pt.x, pt.y)); + } + + // ensure closure + if (polygon.front().x != polygon.back().x || polygon.front().y != polygon.back().y) + { + bg::append(boost_poly.outer(), BoostPoint(polygon.front().x, polygon.front().y)); + } + + // correct polygon before validation + 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& pt = footprint[ i ]; - pt.x += sign0(pt.x) * padding; - pt.y += sign0(pt.y) * padding; + 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)) + { + 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; + } + + // simplify to remove collinear points + BoostPolygon simplified_poly; + bg::simplify(buffered_result.front(), simplified_poly, 1e-6); + + footprint = fromBoostPolygon(simplified_poly); +} + std::vector makeFootprintFromRadius(double radius) { std::vector points; diff --git a/costmap_2d/test/footprint_tests.cpp b/costmap_2d/test/footprint_tests.cpp index 356b9b671..88ff8f2a3 100644 --- a/costmap_2d/test/footprint_tests.cpp +++ b/costmap_2d/test/footprint_tests.cpp @@ -45,126 +45,155 @@ 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) +{ + 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 +204,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();