Code clean-up

* Corrected headers
* Functions ordering
* Comment fixes

Author: AlexeyMerzlyakov

nav2_map_server/include/nav2_map_server/vector_object_server.hpp

@@ -19,6 +19,7 @@
 #include <string>
 #include <vector>
 
+#include "rclcpp/rclcpp.hpp"
 #include "nav_msgs/msg/occupancy_grid.hpp"
 
 #include "tf2_ros/buffer.h"
@@ -28,7 +29,6 @@
 #include "nav2_msgs/srv/remove_shapes.hpp"
 #include "nav2_msgs/srv/get_shapes.hpp"
 #include "nav2_util/lifecycle_node.hpp"
-#include "nav2_util/occ_grid_values.hpp"
 
 #include "nav2_map_server/vector_object_utils.hpp"
 #include "nav2_map_server/vector_object_shapes.hpp"
@@ -79,6 +79,12 @@ class VectorObjectServer : public nav2_util::LifecycleNode
    */
   nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
 
+  /**
+   * @brief Supporting routine obtaining all ROS-parameters
+   * @return True if all parameters were obtained or false in the failure case
+   */
+  bool obtainParams();
+
   /**
    * @brief Finds the shape with given UUID
    * @param uuid Given UUID to search with
@@ -88,6 +94,7 @@ class VectorObjectServer : public nav2_util::LifecycleNode
 
   /**
    * @brief Transform all vector shapes from their local frame to output map frame
+   * @return Whether all vector objects were transformed successfully
    */
   bool transformVectorObjects();
 
@@ -124,8 +131,8 @@ class VectorObjectServer : public nav2_util::LifecycleNode
   void publishMap();
 
   /**
-   * @brief Calculates new map sizes, updates map, process all vector objects on it
-   * and publish output map one time
+   * @brief Calculates new map sizes, updates map, processes all vector objects on it
+   * and publishes output map one time
    */
   void processMap();
 
@@ -174,12 +181,6 @@ class VectorObjectServer : public nav2_util::LifecycleNode
     std::shared_ptr<nav2_msgs::srv::RemoveShapes::Response> response);
 
 protected:
-  /**
-   * @brief Supporting routine obtaining all ROS-parameters
-   * @return True if all parameters were obtained or false in the failure case
-   */
-  bool obtainParams();
-
   /// @brief TF buffer
   std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
   /// @brief TF listener

nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp

@@ -15,11 +15,10 @@
 #ifndef NAV2_MAP_SERVER__VECTOR_OBJECT_SHAPES_HPP_
 #define NAV2_MAP_SERVER__VECTOR_OBJECT_SHAPES_HPP_
 
-#include <cmath>
 #include <memory>
 #include <string>
-#include <vector>
 
+#include "rclcpp/rclcpp.hpp"
 #include "geometry_msgs/msg/polygon.hpp"
 #include "geometry_msgs/msg/point32.hpp"
 #include "nav_msgs/msg/occupancy_grid.hpp"
@@ -71,42 +70,6 @@ class Shape
    */
   bool obtainShapeUUID(const std::string & shape_name, unsigned char * out_uuid);
 
-  /**
-   * @brief Supporting routine obtaining ROS-parameters for the given vector object.
-   * Empty virtual method intended to be used in child implementations
-   * @param shape_name Name of the shape
-   * @return True if all parameters were obtained or false in failure case
-   */
-  virtual bool obtainParams(const std::string & shape_name) = 0;
-
-  /**
-   * @brief Gets shape boundaries.
-   * Empty virtual method intended to be used in child implementations
-   * @param min_x output min X-boundary of shape
-   * @param min_y output min Y-boundary of shape
-   * @param max_x output max X-boundary of shape
-   * @param max_y output max Y-boundary of shape
-   */
-  virtual void getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y) = 0;
-
-  /**
-   * @brief Is the point inside the shape.
-   * Empty virtual method intended to be used in child implementations
-   * @param px X-coordinate of the given point to check
-   * @param py Y-coordinate of the given point to check
-   * @return True if given point inside the shape
-   */
-  virtual bool isPointInside(const double px, const double py) const = 0;
-
-  /**
-   * @brief Puts shape borders on map.
-   * Empty virtual method intended to be used in child implementations
-   * @param map Output map pointer
-   * @param overlay_type Overlay type
-   */
-  virtual void putBorders(
-    nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type) = 0;
-
   /**
    * @brief Gets the value of the shape.
    * Empty virtual method intended to be used in child implementations
@@ -143,6 +106,14 @@ class Shape
    */
   virtual bool isFill() const = 0;
 
+  /**
+   * @brief Supporting routine obtaining ROS-parameters for the given vector object.
+   * Empty virtual method intended to be used in child implementations
+   * @param shape_name Name of the shape
+   * @return True if all parameters were obtained or false in failure case
+   */
+  virtual bool obtainParams(const std::string & shape_name) = 0;
+
   /**
    * @brief Transforms shape coordinates to a new frame.
    * Empty virtual method intended to be used in child implementations
@@ -156,67 +127,52 @@ class Shape
     const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
     const double transform_tolerance) = 0;
 
-protected:
-  /// @brief Type of shape
-  ShapeType type_;
-
-  /// @brief VectorObjectServer node
-  nav2_util::LifecycleNode::WeakPtr node_;
-};
-
-/// @brief Polygon shape class
-class Polygon : public Shape
-{
-public:
-  /*
-   * @brief Polygon class constructor
-   * @param node Vector Object server node pointer
-   * @note setParams()/obtainParams() should be called after to configure the shape
-   */
-  explicit Polygon(const nav2_util::LifecycleNode::WeakPtr & node);
-
-  /**
-   * @brief Supporting routine obtaining ROS-parameters for the given vector object.
-   * @param shape_name Name of the shape
-   * @return True if all parameters were obtained or false in failure case
-   */
-  bool obtainParams(const std::string & shape_name);
-
   /**
-   * @brief Gets shape boundaries
+   * @brief Gets shape boundaries.
+   * Empty virtual method intended to be used in child implementations
    * @param min_x output min X-boundary of shape
    * @param min_y output min Y-boundary of shape
    * @param max_x output max X-boundary of shape
    * @param max_y output max Y-boundary of shape
    */
-  void getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y);
+  virtual void getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y) = 0;
 
   /**
    * @brief Is the point inside the shape.
+   * Empty virtual method intended to be used in child implementations
    * @param px X-coordinate of the given point to check
    * @param py Y-coordinate of the given point to check
    * @return True if given point inside the shape
    */
-  bool isPointInside(const double px, const double py) const;
+  virtual bool isPointInside(const double px, const double py) const = 0;
 
   /**
    * @brief Puts shape borders on map.
+   * Empty virtual method intended to be used in child implementations
    * @param map Output map pointer
    * @param overlay_type Overlay type
    */
-  void putBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type);
+  virtual void putBorders(
+    nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type) = 0;
 
-  /**
-   * @brief Gets Polygon parameters
-   * @return Polygon parameters
-   */
-  nav2_msgs::msg::PolygonObject::SharedPtr getParams() const;
+protected:
+  /// @brief Type of shape
+  ShapeType type_;
 
-  /**
-   * @brief Tries to update Polygon parameters
-   * @return False in case of inconsistent shape
+  /// @brief VectorObjectServer node
+  nav2_util::LifecycleNode::WeakPtr node_;
+};
+
+/// @brief Polygon shape class
+class Polygon : public Shape
+{
+public:
+  /*
+   * @brief Polygon class constructor
+   * @param node Vector Object server node pointer
+   * @note setParams()/obtainParams() should be called after to configure the shape
    */
-  bool setParams(const nav2_msgs::msg::PolygonObject::SharedPtr params);
+  explicit Polygon(const nav2_util::LifecycleNode::WeakPtr & node);
 
   /**
    * @brief Gets the value of the shape.
@@ -249,6 +205,25 @@ class Polygon : public Shape
    */
   bool isFill() const;
 
+  /**
+   * @brief Supporting routine obtaining ROS-parameters for the given vector object.
+   * @param shape_name Name of the shape
+   * @return True if all parameters were obtained or false in failure case
+   */
+  bool obtainParams(const std::string & shape_name);
+
+  /**
+   * @brief Gets Polygon parameters
+   * @return Polygon parameters
+   */
+  nav2_msgs::msg::PolygonObject::SharedPtr getParams() const;
+
+  /**
+   * @brief Tries to update Polygon parameters
+   * @return False in case of inconsistent shape
+   */
+  bool setParams(const nav2_msgs::msg::PolygonObject::SharedPtr params);
+
   /**
    * @brief Transforms shape coordinates to a new frame.
    * @param to_frame Frame ID to transform to
@@ -261,37 +236,6 @@ class Polygon : public Shape
     const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
     const double transform_tolerance);
 
-protected:
-  /**
-   * @brief Checks that shape is consistent for further operation
-   * @return False in case of inconsistent shape
-   */
-  bool checkConsistency();
-
-  /// @brief Input polygon parameters (could be in any frame)
-  nav2_msgs::msg::PolygonObject::SharedPtr params_;
-  /// @brief Polygon in the map's frame
-  geometry_msgs::msg::Polygon::SharedPtr polygon_;
-};
-
-/// @brief Circle shape class
-class Circle : public Shape
-{
-public:
-  /*
-   * @brief Circle class constructor
-   * @param node Vector Object server node pointer
-   * @note setParams()/obtainParams() should be called after to configure the shape
-   */
-  explicit Circle(const nav2_util::LifecycleNode::WeakPtr & node);
-
-  /**
-   * @brief Supporting routine obtaining ROS-parameters for the given vector object.
-   * @param shape_name Name of the shape
-   * @return True if all parameters were obtained or false in failure case
-   */
-  bool obtainParams(const std::string & shape_name);
-
   /**
    * @brief Gets shape boundaries
    * @param min_x output min X-boundary of shape
@@ -316,17 +260,29 @@ class Circle : public Shape
    */
   void putBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type);
 
+protected:
   /**
-   * @brief Gets Circle parameters
-   * @return Circle parameters
+   * @brief Checks that shape is consistent for further operation
+   * @return False in case of inconsistent shape
    */
-  nav2_msgs::msg::CircleObject::SharedPtr getParams() const;
+  bool checkConsistency();
 
-  /**
-   * @brief Tries to update Circle parameters
-   * @return False in case of inconsistent shape
+  /// @brief Input polygon parameters (could be in any frame)
+  nav2_msgs::msg::PolygonObject::SharedPtr params_;
+  /// @brief Polygon in the map's frame
+  geometry_msgs::msg::Polygon::SharedPtr polygon_;
+};
+
+/// @brief Circle shape class
+class Circle : public Shape
+{
+public:
+  /*
+   * @brief Circle class constructor
+   * @param node Vector Object server node pointer
+   * @note setParams()/obtainParams() should be called after to configure the shape
    */
-  bool setParams(const nav2_msgs::msg::CircleObject::SharedPtr params);
+  explicit Circle(const nav2_util::LifecycleNode::WeakPtr & node);
 
   /**
    * @brief Gets the value of the shape.
@@ -359,6 +315,25 @@ class Circle : public Shape
    */
   bool isFill() const;
 
+  /**
+   * @brief Supporting routine obtaining ROS-parameters for the given vector object.
+   * @param shape_name Name of the shape
+   * @return True if all parameters were obtained or false in failure case
+   */
+  bool obtainParams(const std::string & shape_name);
+
+  /**
+   * @brief Gets Circle parameters
+   * @return Circle parameters
+   */
+  nav2_msgs::msg::CircleObject::SharedPtr getParams() const;
+
+  /**
+   * @brief Tries to update Circle parameters
+   * @return False in case of inconsistent shape
+   */
+  bool setParams(const nav2_msgs::msg::CircleObject::SharedPtr params);
+
   /**
    * @brief Transforms shape coordinates to a new frame.
    * @param to_frame Frame ID to transform to
@@ -371,6 +346,30 @@ class Circle : public Shape
     const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
     const double transform_tolerance);
 
+  /**
+   * @brief Gets shape boundaries
+   * @param min_x output min X-boundary of shape
+   * @param min_y output min Y-boundary of shape
+   * @param max_x output max X-boundary of shape
+   * @param max_y output max Y-boundary of shape
+   */
+  void getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y);
+
+  /**
+   * @brief Is the point inside the shape.
+   * @param px X-coordinate of the given point to check
+   * @param py Y-coordinate of the given point to check
+   * @return True if given point inside the shape
+   */
+  bool isPointInside(const double px, const double py) const;
+
+  /**
+   * @brief Puts shape borders on map.
+   * @param map Output map pointer
+   * @param overlay_type Overlay type
+   */
+  void putBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type);
+
 protected:
   /**
    * @brief Checks that shape is consistent for further operation

nav2_map_server/include/nav2_map_server/vector_object_utils.hpp

@@ -19,6 +19,7 @@
 #include <string>
 #include <stdexcept>
 
+#include "rclcpp/rclcpp.hpp"
 #include "nav_msgs/msg/occupancy_grid.hpp"
 
 #include "nav2_util/lifecycle_node.hpp"