improve param read in velocity polygon

nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp

@@ -119,6 +119,12 @@ class Polygon
    * @return Time before collision in seconds
    */
   double getTimeBeforeCollision() const;
+  /**
+   * @brief Obtains time step for robot movement simulation for current polygon.
+   * Applicable for APPROACH model.
+   * @return Simulation time step in seconds
+   */
+  double getSimulationTimeStep() const;
 
   /**
    * @brief Obtains minimum velocity before completly stopping.

nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp

@@ -70,7 +70,7 @@ class VelocityPolygon : public Polygon
 
 protected:
   /**
-    * @brief Custom struc to store the parameters of the sub-polygon
+    * @brief Custom struct to store the parameters of the sub-polygon
     * @param poly_ The points of the sub-polygon
     * @param velocity_polygon_name_ The name of the sub-polygon
     * @param linear_min_ The minimum linear velocity
@@ -79,6 +79,12 @@ class VelocityPolygon : public Polygon
     * @param theta_max_ The maximum angular velocity
     * @param direction_end_angle_ The end angle of the direction(For holonomic robot only)
     * @param direction_start_angle_ The start angle of the direction(For holonomic robot only)
+    * @param slowdown_ratio_ Robot slowdown (share of its actual speed)
+    * @param linear_limit_ Robot linear limit
+    * @param angular_limit_ Robot angular limit
+    * @param time_before_collision_ Time before collision in seconds
+    * @param simulation_time_step_ Time step for robot movement simulation
+    * @param min_vel_before_stop_ Minimum velocity before we fully stop
     */
   struct SubPolygonParameter
   {
@@ -90,6 +96,12 @@ class VelocityPolygon : public Polygon
     double theta_max_;
     double direction_end_angle_;
     double direction_start_angle_;
+    double slowdown_ratio_;
+    double linear_limit_;
+    double angular_limit_;
+    double time_before_collision_;
+    double simulation_time_step_;
+    double min_vel_before_stop_;
   };
 
   /**

nav2_collision_monitor/src/polygon.cpp

@@ -173,6 +173,11 @@ double Polygon::getTimeBeforeCollision() const
   return time_before_collision_;
 }
 
+double Polygon::getSimulationTimeStep() const
+{
+  return simulation_time_step_;
+}
+
 double Polygon::getMinVelBeforeStop() const
 {
   return min_vel_before_stop_;

nav2_collision_monitor/src/velocity_polygon.cpp

@@ -123,9 +123,56 @@ bool VelocityPolygon::getParameters(
           .as_double();
       }
 
+      double slowdown_ratio = 0.0;
+      if (action_type_ == SLOWDOWN) {
+        nav2_util::declare_parameter_if_not_declared(
+          node, polygon_name_ + "." + velocity_polygon_name + ".slowdown_ratio", rclcpp::ParameterValue(
+            0.5));
+        slowdown_ratio = node->get_parameter(
+          polygon_name_ + "." + velocity_polygon_name + ".slowdown_ratio").as_double();
+      }
+
+      double linear_limit = 0.0;
+      double angular_limit = 0.0;
+      if (action_type_ == LIMIT) {
+        nav2_util::declare_parameter_if_not_declared(
+          node, polygon_name_ + "." + velocity_polygon_name + ".linear_limit", rclcpp::ParameterValue(
+            0.5));
+        linear_limit = node->get_parameter(
+          polygon_name_ + "." + velocity_polygon_name + ".linear_limit").as_double();
+        nav2_util::declare_parameter_if_not_declared(
+          node, polygon_name_ + "." + velocity_polygon_name + ".angular_limit", rclcpp::ParameterValue(
+            0.5));
+        angular_limit = node->get_parameter(
+          polygon_name_ + "." + velocity_polygon_name + ".angular_limit").as_double();
+      }
+
+      double time_before_collision = 0.0;
+      double simulation_time_step = 0.0;
+      double min_vel_before_stop = 0.0;
+      if (action_type_ == APPROACH) {
+        nav2_util::declare_parameter_if_not_declared(
+          node, polygon_name_ + "." + velocity_polygon_name + ".time_before_collision", rclcpp::ParameterValue(
+            2.0));
+        time_before_collision = node->get_parameter(
+          polygon_name_ + "." + velocity_polygon_name + ".time_before_collision").as_double();
+        nav2_util::declare_parameter_if_not_declared(
+          node, polygon_name_ + "." + velocity_polygon_name + ".simulation_time_step", rclcpp::ParameterValue(
+            0.1));
+        simulation_time_step = node->get_parameter(
+          polygon_name_ + "." + velocity_polygon_name + ".simulation_time_step").as_double();
+        nav2_util::declare_parameter_if_not_declared(
+          node, polygon_name_ + "." + velocity_polygon_name + ".min_vel_before_stop", rclcpp::ParameterValue(
+            -1.0));
+        min_vel_before_stop = node->get_parameter(
+          polygon_name_ + "." + velocity_polygon_name + ".min_vel_before_stop").as_double();
+      }
+
       SubPolygonParameter sub_polygon = {
         poly, velocity_polygon_name, linear_min, linear_max, theta_min,
-        theta_max, direction_end_angle, direction_start_angle};
+        theta_max, direction_end_angle, direction_start_angle,
+        slowdown_ratio, linear_limit, angular_limit, time_before_collision, simulation_time_step,
+        min_vel_before_stop};
       sub_polygons_.push_back(sub_polygon);
     }
   } catch (const std::exception & ex) {
@@ -153,6 +200,14 @@ void VelocityPolygon::updatePolygon(const Velocity & cmd_vel_in)
         // p_s.z will remain 0.0
         polygon_.polygon.points.push_back(p_s);
       }
+
+      slowdown_ratio_ = sub_polygon.slowdown_ratio_;
+      linear_limit_ = sub_polygon.linear_limit_;
+      angular_limit_ = sub_polygon.angular_limit_;
+      time_before_collision_ = sub_polygon.time_before_collision_;
+      simulation_time_step_ = sub_polygon.simulation_time_step_;
+      min_vel_before_stop_ = sub_polygon.min_vel_before_stop_;
+
       return;
     }
   }

nav2_collision_monitor/test/velocity_polygons_test.cpp

@@ -67,6 +67,13 @@ static const char RIGHT_POLYGON_STR[]{
 static const bool IS_HOLONOMIC{true};
 static const bool IS_NOT_HOLONOMIC{false};
 static const int MIN_POINTS{2};
+static const double SLOWDOWN_RATIO{0.25};
+static const double LINEAR_LIMIT{0.3};
+static const double ANGULAR_LIMIT{0.2};
+static const double TIME_BEFORE_COLLISION{2.0};
+static const double SIMULATION_TIME_STEP{0.05};
+static const double MIN_VEL_BEFORE_COLLISION{-0.5};
+
 static const tf2::Duration TRANSFORM_TOLERANCE{tf2::durationFromSec(0.1)};
 
 class TestNode : public nav2_util::LifecycleNode
@@ -145,6 +152,10 @@ class Tester : public ::testing::Test
 protected:
   // Working with parameters
   void setCommonParameters(const std::string & polygon_name, const std::string & action_type);
+  void addSlowdownParameters(const std::string & polygon_name);
+  void addLimitParameters(const std::string & polygon_name);
+  void addApproachParameters(const std::string & polygon_name);
+
   void setVelocityPolygonParameters(const bool is_holonomic);
   void addPolygonVelocitySubPolygon(
     const std::string & sub_polygon_name,
@@ -211,6 +222,39 @@ void Tester::setCommonParameters(const std::string & polygon_name, const std::st
     rclcpp::Parameter(polygon_name + ".polygon_pub_topic", POLYGON_PUB_TOPIC));
 }
 
+void Tester::addSlowdownParameters(const std::string & polygon_name)
+{
+  test_node_->set_parameter(
+    rclcpp::Parameter(std::string(POLYGON_NAME) + "." + polygon_name + ".slowdown_ratio", SLOWDOWN_RATIO));
+}
+
+void Tester::addLimitParameters(const std::string & polygon_name)
+{
+  test_node_->set_parameter(
+    rclcpp::Parameter(std::string(POLYGON_NAME) + "." + polygon_name + ".linear_limit", LINEAR_LIMIT));
+
+  test_node_->set_parameter(
+    rclcpp::Parameter(std::string(POLYGON_NAME) + "." + polygon_name + ".angular_limit", ANGULAR_LIMIT));
+}
+
+void Tester::addApproachParameters(const std::string & polygon_name)
+{
+  test_node_->set_parameter(
+    rclcpp::Parameter(
+      std::string(
+        POLYGON_NAME) + "." + polygon_name + ".time_before_collision", TIME_BEFORE_COLLISION));
+
+  test_node_->set_parameter(
+    rclcpp::Parameter(
+      std::string(POLYGON_NAME) + "." + polygon_name + ".simulation_time_step",
+      SIMULATION_TIME_STEP));
+
+  test_node_->set_parameter(
+    rclcpp::Parameter(
+      std::string(POLYGON_NAME) + "." + polygon_name + ".min_vel_before_stop",
+      MIN_VEL_BEFORE_COLLISION));
+}
+
 void Tester::setVelocityPolygonParameters(const bool is_holonomic)
 {
   test_node_->declare_parameter(
@@ -270,6 +314,7 @@ void Tester::setVelocityPolygonParameters(const bool is_holonomic)
     rclcpp::Parameter(std::string(POLYGON_NAME) + ".velocity_polygons", velocity_polygons));
 }
 
+
 void Tester::addPolygonVelocitySubPolygon(
   const std::string & sub_polygon_name,
   const double linear_min, const double linear_max,
@@ -560,6 +605,35 @@ TEST_F(Tester, testVelocityPolygonHolonomicVelocitySwitching)
   EXPECT_NEAR(poly[3].y, RIGHT_POLYGON[7], EPSILON);
 }
 
+// TEST_F(Tester, testVelocityPolygonSlowdownParameters)
+// {
+//   createVelocityPolygon("slowdown", IS_NOT_HOLONOMIC);
+//   addSlowdownParameters(SUB_POLYGON_FORWARD_NAME);
+
+//   EXPECT_EQ(velocity_polygon_->getActionType(), nav2_collision_monitor::SLOWDOWN);
+//   EXPECT_NEAR(velocity_polygon_->getSlowdownRatio(), 0.25, EPSILON);
+// }
+
+// TEST_F(Tester, testVelocityPolygonLimitParameters)
+// {
+//   createVelocityPolygon("limit", IS_NOT_HOLONOMIC);
+//   addLimitParameters(SUB_POLYGON_FORWARD_NAME);
+
+//   EXPECT_EQ(velocity_polygon_->getActionType(), nav2_collision_monitor::LIMIT);
+//   EXPECT_NEAR(velocity_polygon_->getLinearLimit(), 0.3, EPSILON);
+//   EXPECT_NEAR(velocity_polygon_->getAngularLimit(), 0.2, EPSILON);
+// }
+
+// TEST_F(Tester, testVelocityPolygonApproachParameters)
+// {
+//   createVelocityPolygon("approach", IS_NOT_HOLONOMIC);
+//   addApproachParameters(SUB_POLYGON_FORWARD_NAME);
+
+//   EXPECT_EQ(velocity_polygon_->getActionType(), nav2_collision_monitor::APPROACH);
+//   EXPECT_NEAR(velocity_polygon_->getTimeBeforeCollision(), 2.0, EPSILON);
+//   EXPECT_NEAR(velocity_polygon_->getSimulationTimeStep(), 0.05, EPSILON);
+//   EXPECT_NEAR(velocity_polygon_->getMinVelBeforeStop(), -0.5, EPSILON);
+// }
 
 int main(int argc, char ** argv)
 {