update review comments and description

Signed-off-by: nelson <[email protected]>
ros-navigation · Jan 7, 2024 · 620c7c2 · 620c7c2
1 parent abab066
commit 620c7c2
Show file tree

Hide file tree

Showing 2 changed files with 40 additions and 28 deletions.
diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp
@@ -51,20 +51,28 @@ class VelocityPolygon : public Polygon
   virtual ~VelocityPolygon();
 
   /**
-   * @brief Supporting routine obtaining velocity polygon specific ROS-parameters
+   * @brief Overriden getParameters function for VelocityPolygon parameters
+   * @param polygon_sub_topic Not used in VelocityPolygon
+   * @param polygon_pub_topic Output name of polygon publishing topic
+   * @param footprint_topic Not used in VelocityPolygon
    * @return True if all parameters were obtained or false in failure case
    */
   bool getParameters(
     std::string & /*polygon_sub_topic*/, std::string & polygon_pub_topic,
     std::string & /*footprint_topic*/) override;
 
 protected:
-  // override the base class update polygon
-  void updatePolygon(const Velocity & cmd_vel_in) override;
-
-  bool holonomic_;
-
-  // Define a structure to store the basic parameters
+  /**
+    * @brief Custom struc 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
+    * @param linear_max_ The maximum linear velocity
+    * @param theta_min_ The minimum angular velocity
+    * @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)
+    */
   struct SubPolygonParameter
   {
     std::vector<Point> poly_;
@@ -77,16 +85,25 @@ class VelocityPolygon : public Polygon
     double direction_start_angle_;
   };
 
-  // Create a vector to store instances of BasicParameters
-  std::vector<SubPolygonParameter> sub_polygons_;
+  /**
+   * @brief Overriden updatePolygon function for VelocityPolygon
+   * @param cmd_vel_in Robot twist command input
+   */
+  void updatePolygon(const Velocity & cmd_vel_in) override;
 
   /**
    * @brief Check if the velocities and direction is in expected range.
    * @param cmd_vel_in Robot twist command input
+   * @param sub_polygon_param Sub polygon parameters
    * @return True if speed and direction is within the condition
    */
   bool isInRange(const Velocity & cmd_vel_in, const SubPolygonParameter & sub_polygon_param);
 
+  // Variables
+  /// @brief Flag to indicate if the robot is holonomic
+  bool holonomic_;
+  /// @brief Vector to store the parameters of the sub-polygon
+  std::vector<SubPolygonParameter> sub_polygons_;
 };  // class VelocityPolygon
 
 }  // namespace nav2_collision_monitor

diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp
@@ -108,14 +108,14 @@ bool VelocityPolygon::getParameters(
       if (holonomic_) {
         nav2_util::declare_parameter_if_not_declared(
           node, polygon_name_ + "." + velocity_polygon_name + ".direction_end_angle",
-          rclcpp::ParameterValue(0.0));
+          rclcpp::ParameterValue(M_PI));
         direction_end_angle =
           node->get_parameter(polygon_name_ + "." + velocity_polygon_name + ".direction_end_angle")
           .as_double();
 
         nav2_util::declare_parameter_if_not_declared(
           node, polygon_name_ + "." + velocity_polygon_name + ".direction_start_angle",
-          rclcpp::ParameterValue(0.0));
+          rclcpp::ParameterValue(-M_PI));
         direction_start_angle =
           node
           ->get_parameter(polygon_name_ + "." + velocity_polygon_name + ".direction_start_angle")
@@ -169,32 +169,27 @@ void VelocityPolygon::updatePolygon(const Velocity & cmd_vel_in)
 }
 
 bool VelocityPolygon::isInRange(
-  const Velocity & cmd_vel_in, const SubPolygonParameter & sub_polygon) {
-  if (holonomic_) {
-    const double twist_linear = std::hypot(cmd_vel_in.x, cmd_vel_in.y);
+  const Velocity & cmd_vel_in, const SubPolygonParameter & sub_polygon)
+{
+  bool in_range =
+    (cmd_vel_in.x <= sub_polygon.linear_max_ && cmd_vel_in.x >= sub_polygon.linear_min_ &&
+    cmd_vel_in.tw <= sub_polygon.theta_max_ && cmd_vel_in.tw >= sub_polygon.theta_min_);
 
-    // check if direction in angle range(min -> max)
-    double direction = std::atan2(cmd_vel_in.y, cmd_vel_in.x);
-    bool direction_in_range;
+  if (holonomic_) {
+    // Additionally check if moving direction in angle range(start -> end) for holonomic case
+    const double direction = std::atan2(cmd_vel_in.y, cmd_vel_in.x);
     if (sub_polygon.direction_start_angle_ <= sub_polygon.direction_end_angle_) {
-      direction_in_range =
+      in_range &=
         (direction >= sub_polygon.direction_start_angle_ &&
         direction <= sub_polygon.direction_end_angle_);
     } else {
-      direction_in_range =
+      in_range &=
         (direction >= sub_polygon.direction_start_angle_ ||
         direction <= sub_polygon.direction_end_angle_);
     }
-
-    return twist_linear <= sub_polygon.linear_max_ && twist_linear >= sub_polygon.linear_min_ &&
-           direction_in_range && cmd_vel_in.tw <= sub_polygon.theta_max_ &&
-           cmd_vel_in.tw >= sub_polygon.theta_min_;
-  } else {
-    // non-holonomic
-    return cmd_vel_in.x <= sub_polygon.linear_max_ && cmd_vel_in.x >= sub_polygon.linear_min_ &&
-           cmd_vel_in.tw <= sub_polygon.theta_max_ && cmd_vel_in.tw >= sub_polygon.theta_min_;
   }
-  return true;
+
+  return in_range;
 }
 
 }  // namespace nav2_collision_monitor