diff --git a/.circleci/config.yml b/.circleci/config.yml index ab6312a368..190ba5d7f1 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -33,12 +33,12 @@ _commands: - restore_cache: name: Restore Cache << parameters.key >> keys: - - "<< parameters.key >>-v17\ + - "<< parameters.key >>-v18\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" - - "<< parameters.key >>-v17\ + - "<< parameters.key >>-v18\ -{{ arch }}\ -main\ -\ @@ -58,7 +58,7 @@ _commands: steps: - save_cache: name: Save Cache << parameters.key >> - key: "<< parameters.key >>-v17\ + key: "<< parameters.key >>-v18\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index 815694d83c..b15672807f 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -210,6 +210,14 @@ class Polygon */ bool isPointInside(const Point & point) const; + /** + * @brief Extracts Polygon points from a string with of the form [[x1,y1],[x2,y2],[x3,y3]...] + * @param poly_string Input String containing the verteceis of the polygon + * @param polygon Output Point vector with all the vertecies of the polygon + * @return True if all parameters were obtained or false in failure case + */ + bool getPolygonFromString(std::string & poly_string, std::vector & polygon); + // ----- Variables ----- /// @brief Collision Monitor node diff --git a/nav2_collision_monitor/params/collision_detector_params.yaml b/nav2_collision_monitor/params/collision_detector_params.yaml index 68b4cd7b50..eb3ee0a873 100644 --- a/nav2_collision_monitor/params/collision_detector_params.yaml +++ b/nav2_collision_monitor/params/collision_detector_params.yaml @@ -9,7 +9,7 @@ collision_detector: polygons: ["PolygonFront"] PolygonFront: type: "polygon" - points: [0.3, 0.3, 0.3, -0.3, 0.0, -0.3, 0.0, 0.3] + points: "[[0.3, 0.3], [0.3, -0.3], [0.0, -0.3], [0.0, 0.3]]" action_type: "none" min_points: 4 visualize: True diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml index fcb01f5482..ab5b08bbb9 100644 --- a/nav2_collision_monitor/params/collision_monitor_params.yaml +++ b/nav2_collision_monitor/params/collision_monitor_params.yaml @@ -17,7 +17,7 @@ collision_monitor: polygons: ["PolygonStop"] PolygonStop: type: "polygon" - points: [0.3, 0.3, 0.3, -0.3, 0.0, -0.3, 0.0, 0.3] + points: "[[0.3, 0.3], [0.3, -0.3], [0.0, -0.3], [0.0, 0.3]]" action_type: "stop" min_points: 4 visualize: True @@ -25,7 +25,7 @@ collision_monitor: enabled: True PolygonSlow: type: "polygon" - points: [0.4, 0.4, 0.4, -0.4, -0.4, -0.4, -0.4, 0.4] + points: "[[0.4, 0.4], [0.4, -0.4], [-0.4, -0.4], [-0.4, 0.4]]" action_type: "slowdown" min_points: 4 slowdown_ratio: 0.3 @@ -34,7 +34,7 @@ collision_monitor: enabled: True PolygonLimit: type: "polygon" - points: [0.5, 0.5, 0.5, -0.5, -0.5, -0.5, -0.5, 0.5] + points: "[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5], [-0.5, 0.5]]" action_type: "limit" min_points: 4 linear_limit: 0.4 diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 248bdcec6d..f78f9291ef 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -22,6 +22,7 @@ #include "nav2_util/node_utils.hpp" #include "nav2_util/robot_utils.hpp" +#include "nav2_util/array_parser.hpp" #include "nav2_collision_monitor/kinematics.hpp" @@ -401,34 +402,14 @@ bool Polygon::getParameters( try { // Leave it uninitialized: it will throw an inner exception if the parameter is not set nav2_util::declare_parameter_if_not_declared( - node, polygon_name_ + ".points", rclcpp::PARAMETER_DOUBLE_ARRAY); - std::vector poly_row = - node->get_parameter(polygon_name_ + ".points").as_double_array(); - // Check for points format correctness - if (poly_row.size() <= 6 || poly_row.size() % 2 != 0) { - RCLCPP_ERROR( - logger_, - "[%s]: Polygon has incorrect points description", - polygon_name_.c_str()); - return false; - } - - // Obtain polygon vertices - Point point; - bool first = true; - for (double val : poly_row) { - if (first) { - point.x = val; - } else { - point.y = val; - poly_.push_back(point); - } - first = !first; - } + node, polygon_name_ + ".points", rclcpp::PARAMETER_STRING); + std::string poly_string = + node->get_parameter(polygon_name_ + ".points").as_string(); // Do not need to proceed further, if "points" parameter is defined. // Static polygon will be used. - return true; + return getPolygonFromString(poly_string, poly_); + } catch (const rclcpp::exceptions::ParameterUninitializedException &) { RCLCPP_INFO( logger_, @@ -562,4 +543,44 @@ inline bool Polygon::isPointInside(const Point & point) const return res; } +bool Polygon::getPolygonFromString( + std::string & poly_string, + std::vector & polygon) +{ + std::string error; + std::vector> vvf = nav2_util::parseVVF(poly_string, error); + + if (error != "") { + RCLCPP_ERROR( + logger_, "Error parsing polygon parameter %s: '%s'", + poly_string.c_str(), error.c_str()); + return false; + } + + // Check for minimum 4 points + if (vvf.size() <= 3) { + RCLCPP_ERROR( + logger_, + "Polygon must have at least three points."); + return false; + } + for (unsigned int i = 0; i < vvf.size(); i++) { + if (vvf[i].size() == 2) { + Point point; + point.x = vvf[i][0]; + point.y = vvf[i][1]; + polygon.push_back(point); + } else { + RCLCPP_ERROR( + logger_, + "Points in the polygon specification must be pairs of numbers" + "Found a point with %d numbers.", + static_cast(vvf[i].size())); + return false; + } + } + + return true; +} + } // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/test/collision_detector_node_test.cpp b/nav2_collision_monitor/test/collision_detector_node_test.cpp index b0b926fde0..1ac519111f 100644 --- a/nav2_collision_monitor/test/collision_detector_node_test.cpp +++ b/nav2_collision_monitor/test/collision_detector_node_test.cpp @@ -269,8 +269,11 @@ void Tester::addPolygon( cd_->set_parameter( rclcpp::Parameter(polygon_name + ".type", "polygon")); - const std::vector points { - size, size, size, -size, -size, -size, -size, size}; + const std::string points = "[[" + + std::to_string(size) + ", " + std::to_string(size) + "], [" + + std::to_string(size) + ", " + std::to_string(-size) + "], [" + + std::to_string(-size) + ", " + std::to_string(-size) + "], [" + + std::to_string(-size) + ", " + std::to_string(size) + "]]"; cd_->declare_parameter( polygon_name + ".points", rclcpp::ParameterValue(points)); cd_->set_parameter( diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index 94801a3d2a..242b9f72db 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -301,8 +301,11 @@ void Tester::addPolygon( rclcpp::Parameter(polygon_name + ".type", "polygon")); if (at != "approach") { - const std::vector points { - size, size, size, -size, -size, -size, -size, size}; + const std::string points = "[[" + + std::to_string(size) + ", " + std::to_string(size) + "], [" + + std::to_string(size) + ", " + std::to_string(-size) + "], [" + + std::to_string(-size) + ", " + std::to_string(-size) + "], [" + + std::to_string(-size) + ", " + std::to_string(size) + "]]"; cm_->declare_parameter( polygon_name + ".points", rclcpp::ParameterValue(points)); cm_->set_parameter( diff --git a/nav2_collision_monitor/test/polygons_test.cpp b/nav2_collision_monitor/test/polygons_test.cpp index 2ab46cdced..8d3ad3dc5e 100644 --- a/nav2_collision_monitor/test/polygons_test.cpp +++ b/nav2_collision_monitor/test/polygons_test.cpp @@ -50,6 +50,19 @@ static const std::vector SQUARE_POLYGON { 0.5, 0.5, 0.5, -0.5, -0.5, -0.5, -0.5, 0.5}; static const std::vector ARBITRARY_POLYGON { 1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 2.0, -1.0, -1.0, -1.0, -1.0, 1.0}; +static const char SQUARE_POLYGON_STR[]{ + "[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5], [-0.5, 0.5]]"}; +static const char ARBITRARY_POLYGON_STR[]{ + "[[1.0, 1.0], [1.0, 0.0], [2.0, 0.0], [2.0, -1.0], [-1.0, -1.0], [-1.0, 1.0]]"}; +static const char INCORRECT_POINTS_1_STR[]{ + "[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5]]" +}; +static const char INCORRECT_POINTS_2_STR[]{ + "[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5], [-0.5, 0.5], [0]]" +}; +static const char INVALID_POINTS_STR[]{ + "[[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5], [-0.5, 0.5], 0]]" +}; static const double CIRCLE_RADIUS{0.5}; static const int MIN_POINTS{2}; static const double SLOWDOWN_RATIO{0.7}; @@ -212,7 +225,7 @@ class Tester : public ::testing::Test // Working with parameters void setCommonParameters(const std::string & polygon_name, const std::string & action_type); void setPolygonParameters( - const std::vector & points, + const char * points, const bool is_static); void setCircleParameters(const double radius); bool checkUndeclaredParameter(const std::string & polygon_name, const std::string & param); @@ -311,7 +324,7 @@ void Tester::setCommonParameters(const std::string & polygon_name, const std::st } void Tester::setPolygonParameters( - const std::vector & points, const bool is_static) + const char * points, const bool is_static) { if (is_static) { test_node_->declare_parameter( @@ -360,7 +373,7 @@ bool Tester::checkUndeclaredParameter(const std::string & polygon_name, const st void Tester::createPolygon(const std::string & action_type, const bool is_static) { setCommonParameters(POLYGON_NAME, action_type); - setPolygonParameters(SQUARE_POLYGON, is_static); + setPolygonParameters(SQUARE_POLYGON_STR, is_static); polygon_ = std::make_shared( test_node_, POLYGON_NAME, @@ -546,7 +559,7 @@ TEST_F(Tester, testPolygonUndeclaredPoints) TEST_F(Tester, testPolygonIncorrectActionType) { setCommonParameters(POLYGON_NAME, "incorrect_action_type"); - setPolygonParameters(SQUARE_POLYGON, true); + setPolygonParameters(SQUARE_POLYGON_STR, true); polygon_ = std::make_shared( test_node_, POLYGON_NAME, @@ -558,12 +571,11 @@ TEST_F(Tester, testPolygonIncorrectPoints1) { setCommonParameters(POLYGON_NAME, "stop"); - std::vector incorrect_points = SQUARE_POLYGON; - incorrect_points.resize(6); // Not enough for triangle + // Triangle points test_node_->declare_parameter( - std::string(POLYGON_NAME) + ".points", rclcpp::ParameterValue(incorrect_points)); + std::string(POLYGON_NAME) + ".points", rclcpp::ParameterValue(INCORRECT_POINTS_1_STR)); test_node_->set_parameter( - rclcpp::Parameter(std::string(POLYGON_NAME) + ".points", incorrect_points)); + rclcpp::Parameter(std::string(POLYGON_NAME) + ".points", INCORRECT_POINTS_1_STR)); polygon_ = std::make_shared( test_node_, POLYGON_NAME, @@ -575,12 +587,11 @@ TEST_F(Tester, testPolygonIncorrectPoints2) { setCommonParameters(POLYGON_NAME, "stop"); - std::vector incorrect_points = SQUARE_POLYGON; - incorrect_points.resize(9); // Odd number of points + // Odd number of elements test_node_->declare_parameter( - std::string(POLYGON_NAME) + ".points", rclcpp::ParameterValue(incorrect_points)); + std::string(POLYGON_NAME) + ".points", rclcpp::ParameterValue(INCORRECT_POINTS_2_STR)); test_node_->set_parameter( - rclcpp::Parameter(std::string(POLYGON_NAME) + ".points", incorrect_points)); + rclcpp::Parameter(std::string(POLYGON_NAME) + ".points", INCORRECT_POINTS_2_STR)); polygon_ = std::make_shared( test_node_, POLYGON_NAME, @@ -592,7 +603,7 @@ TEST_F(Tester, testPolygonIncorrectPoints2) TEST_F(Tester, testPolygonMaxPoints) { setCommonParameters(POLYGON_NAME, "stop"); - setPolygonParameters(SQUARE_POLYGON, true); + setPolygonParameters(SQUARE_POLYGON_STR, true); const int max_points = 5; test_node_->declare_parameter( @@ -751,7 +762,7 @@ TEST_F(Tester, testPolygonGetPointsInsideEdge) // Test for checking edge cases in raytracing algorithm. // All points are lie on the edge lines parallel to OX, where the raytracing takes place. setCommonParameters(POLYGON_NAME, "stop"); - setPolygonParameters(ARBITRARY_POLYGON, true); + setPolygonParameters(ARBITRARY_POLYGON_STR, true); polygon_ = std::make_shared( test_node_, POLYGON_NAME, @@ -884,7 +895,7 @@ TEST_F(Tester, testPolygonDefaultVisualize) std::string(POLYGON_NAME) + ".action_type", rclcpp::ParameterValue("stop")); test_node_->set_parameter( rclcpp::Parameter(std::string(POLYGON_NAME) + ".action_type", "stop")); - setPolygonParameters(SQUARE_POLYGON, true); + setPolygonParameters(SQUARE_POLYGON_STR, true); // Create new polygon polygon_ = std::make_shared( @@ -900,6 +911,22 @@ TEST_F(Tester, testPolygonDefaultVisualize) ASSERT_EQ(test_node_->waitPolygonReceived(100ms), nullptr); } +TEST_F(Tester, testPolygonInvalidPointsString) +{ + setCommonParameters(POLYGON_NAME, "stop"); + + // Invalid points + test_node_->declare_parameter( + std::string(POLYGON_NAME) + ".points", rclcpp::ParameterValue(INVALID_POINTS_STR)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".points", INVALID_POINTS_STR)); + + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_FALSE(polygon_->configure()); +} + int main(int argc, char ** argv) { // Initialize the system diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index f039cf5d42..df0ddb2dc3 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -38,7 +38,6 @@ include_directories( add_definitions(${EIGEN3_DEFINITIONS}) add_library(nav2_costmap_2d_core SHARED - src/array_parser.cpp src/costmap_2d.cpp src/layer.cpp src/layered_costmap.cpp diff --git a/nav2_costmap_2d/src/footprint.cpp b/nav2_costmap_2d/src/footprint.cpp index c925973038..75de1feb91 100644 --- a/nav2_costmap_2d/src/footprint.cpp +++ b/nav2_costmap_2d/src/footprint.cpp @@ -34,7 +34,7 @@ #include #include "geometry_msgs/msg/point32.hpp" -#include "nav2_costmap_2d/array_parser.hpp" +#include "nav2_util/array_parser.hpp" #include "nav2_costmap_2d/costmap_math.hpp" namespace nav2_costmap_2d @@ -177,7 +177,7 @@ bool makeFootprintFromString( std::vector & footprint) { std::string error; - std::vector> vvf = parseVVF(footprint_string, error); + std::vector> vvf = nav2_util::parseVVF(footprint_string, error); if (error != "") { RCLCPP_ERROR( diff --git a/nav2_costmap_2d/test/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt index b9480fdffa..e69501277e 100644 --- a/nav2_costmap_2d/test/unit/CMakeLists.txt +++ b/nav2_costmap_2d/test/unit/CMakeLists.txt @@ -1,8 +1,3 @@ -ament_add_gtest(array_parser_test array_parser_test.cpp) -target_link_libraries(array_parser_test - nav2_costmap_2d_core -) - ament_add_gtest(collision_footprint_test footprint_collision_checker_test.cpp) target_link_libraries(collision_footprint_test nav2_costmap_2d_core diff --git a/nav2_util/CMakeLists.txt b/nav2_util/CMakeLists.txt index 4635bab0b0..1c8b9b1d64 100644 --- a/nav2_util/CMakeLists.txt +++ b/nav2_util/CMakeLists.txt @@ -50,6 +50,8 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) find_package(ament_cmake_pytest REQUIRED) find_package(launch_testing_ament_cmake REQUIRED) + # skip copyright linting + set(ament_cmake_copyright_FOUND TRUE) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/array_parser.hpp b/nav2_util/include/nav2_util/array_parser.hpp similarity index 91% rename from nav2_costmap_2d/include/nav2_costmap_2d/array_parser.hpp rename to nav2_util/include/nav2_util/array_parser.hpp index ca2dd879b6..f232cf3027 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/array_parser.hpp +++ b/nav2_util/include/nav2_util/array_parser.hpp @@ -28,13 +28,13 @@ * * author: Dave Hershberger */ -#ifndef NAV2_COSTMAP_2D__ARRAY_PARSER_HPP_ -#define NAV2_COSTMAP_2D__ARRAY_PARSER_HPP_ +#ifndef NAV2_UTIL__ARRAY_PARSER_HPP_ +#define NAV2_UTIL__ARRAY_PARSER_HPP_ #include #include -namespace nav2_costmap_2d +namespace nav2_util { /** @brief Parse a vector of vectors of floats from a string. @@ -46,6 +46,6 @@ namespace nav2_costmap_2d * anything, like part of a successful parse. */ std::vector> parseVVF(const std::string & input, std::string & error_return); -} // end namespace nav2_costmap_2d +} // end namespace nav2_util -#endif // NAV2_COSTMAP_2D__ARRAY_PARSER_HPP_ +#endif // NAV2_UTIL__ARRAY_PARSER_HPP_ diff --git a/nav2_util/src/CMakeLists.txt b/nav2_util/src/CMakeLists.txt index a639a0e59e..104966e219 100644 --- a/nav2_util/src/CMakeLists.txt +++ b/nav2_util/src/CMakeLists.txt @@ -8,6 +8,7 @@ add_library(${library_name} SHARED robot_utils.cpp node_thread.cpp odometry_utils.cpp + array_parser.cpp ) ament_target_dependencies(${library_name} diff --git a/nav2_costmap_2d/src/array_parser.cpp b/nav2_util/src/array_parser.cpp similarity index 98% rename from nav2_costmap_2d/src/array_parser.cpp rename to nav2_util/src/array_parser.cpp index eecd4eebc8..cfe2f69914 100644 --- a/nav2_costmap_2d/src/array_parser.cpp +++ b/nav2_util/src/array_parser.cpp @@ -34,7 +34,7 @@ #include #include -namespace nav2_costmap_2d +namespace nav2_util { /** @brief Parse a vector of vector of floats from a string. @@ -102,4 +102,4 @@ std::vector> parseVVF(const std::string & input, std::string return result; } -} // end namespace nav2_costmap_2d +} // end namespace nav2_util diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index 9f1ae99bbc..cfd747b9f1 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/nav2_util/test/CMakeLists.txt @@ -41,3 +41,6 @@ target_link_libraries(test_odometry_utils ${library_name}) ament_add_gtest(test_robot_utils test_robot_utils.cpp) ament_target_dependencies(test_robot_utils geometry_msgs) target_link_libraries(test_robot_utils ${library_name}) + +ament_add_gtest(test_array_parser test_array_parser.cpp) +target_link_libraries(test_array_parser ${library_name}) diff --git a/nav2_costmap_2d/test/unit/array_parser_test.cpp b/nav2_util/test/test_array_parser.cpp similarity index 88% rename from nav2_costmap_2d/test/unit/array_parser_test.cpp rename to nav2_util/test/test_array_parser.cpp index 3706f0cb77..76a691b700 100644 --- a/nav2_costmap_2d/test/unit/array_parser_test.cpp +++ b/nav2_util/test/test_array_parser.cpp @@ -31,13 +31,13 @@ #include #include "gtest/gtest.h" -#include "nav2_costmap_2d/array_parser.hpp" +#include "nav2_util/array_parser.hpp" TEST(array_parser, basic_operation) { std::string error; std::vector> vvf; - vvf = nav2_costmap_2d::parseVVF("[[1, 2.2], [.3, -4e4]]", error); + vvf = nav2_util::parseVVF("[[1, 2.2], [.3, -4e4]]", error); EXPECT_EQ(2u, vvf.size() ); EXPECT_EQ(2u, vvf[0].size() ); EXPECT_EQ(2u, vvf[1].size() ); @@ -52,7 +52,7 @@ TEST(array_parser, missing_open) { std::string error; std::vector> vvf; - vvf = nav2_costmap_2d::parseVVF("[1, 2.2], [.3, -4e4]]", error); + vvf = nav2_util::parseVVF("[1, 2.2], [.3, -4e4]]", error); EXPECT_NE(error, ""); } @@ -60,7 +60,7 @@ TEST(array_parser, missing_close) { std::string error; std::vector> vvf; - vvf = nav2_costmap_2d::parseVVF("[[1, 2.2], [.3, -4e4]", error); + vvf = nav2_util::parseVVF("[[1, 2.2], [.3, -4e4]", error); EXPECT_NE(error, ""); } @@ -68,7 +68,7 @@ TEST(array_parser, wrong_depth) { std::string error; std::vector> vvf; - vvf = nav2_costmap_2d::parseVVF("[1, 2.2], [.3, -4e4]", error); + vvf = nav2_util::parseVVF("[1, 2.2], [.3, -4e4]", error); EXPECT_NE(error, ""); }