Updating polygon parameter format (#4020)

* moving libraries and using parser in collision monitor

* apply changes from review

* linting missed update

* unit test update

* collision_monitor utests + uncrustify lint

* skip copyright linting

* Combine error messages

* update yaml

* circleci update v18

* add invalid points test for cov

.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\
                 -<no value>\
@@ -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 }}\

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<Point> & polygon);
+
   // ----- Variables -----
 
   /// @brief Collision Monitor node

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

nav2_collision_monitor/params/collision_monitor_params.yaml

@@ -17,15 +17,15 @@ 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
       polygon_pub_topic: "polygon_stop"
       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

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<double> 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<Point> & polygon)
+{
+  std::string error;
+  std::vector<std::vector<float>> 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<int>(vvf[i].size()));
+      return false;
+    }
+  }
+
+  return true;
+}
+
 }  // namespace nav2_collision_monitor

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

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

nav2_collision_monitor/test/polygons_test.cpp

@@ -50,6 +50,19 @@ static const std::vector<double> SQUARE_POLYGON {
   0.5, 0.5, 0.5, -0.5, -0.5, -0.5, -0.5, 0.5};
 static const std::vector<double> 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<double> & 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<double> & 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<PolygonWrapper>(
     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<PolygonWrapper>(
     test_node_, POLYGON_NAME,
@@ -558,12 +571,11 @@ TEST_F(Tester, testPolygonIncorrectPoints1)
 {
   setCommonParameters(POLYGON_NAME, "stop");
 
-  std::vector<double> 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<PolygonWrapper>(
     test_node_, POLYGON_NAME,
@@ -575,12 +587,11 @@ TEST_F(Tester, testPolygonIncorrectPoints2)
 {
   setCommonParameters(POLYGON_NAME, "stop");
 
-  std::vector<double> 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<PolygonWrapper>(
     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<PolygonWrapper>(
     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<PolygonWrapper>(
@@ -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<PolygonWrapper>(
+    test_node_, POLYGON_NAME,
+    tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE);
+  ASSERT_FALSE(polygon_->configure());
+}
+
 int main(int argc, char ** argv)
 {
   // Initialize the system

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

nav2_costmap_2d/src/footprint.cpp

@@ -34,7 +34,7 @@
 #include <vector>
 
 #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<geometry_msgs::msg::Point> & footprint)
 {
   std::string error;
-  std::vector<std::vector<float>> vvf = parseVVF(footprint_string, error);
+  std::vector<std::vector<float>> vvf = nav2_util::parseVVF(footprint_string, error);
 
   if (error != "") {
     RCLCPP_ERROR(

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

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)