diff --git a/nav2_map_server/include/nav2_map_server/vector_object_server.hpp b/nav2_map_server/include/nav2_map_server/vector_object_server.hpp
index 39afec55cfc..7e94072c8ee 100644
--- a/nav2_map_server/include/nav2_map_server/vector_object_server.hpp
+++ b/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
diff --git a/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp b/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp
index d7fabec4f91..aeae1855531 100644
--- a/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp
+++ b/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
diff --git a/nav2_map_server/include/nav2_map_server/vector_object_utils.hpp b/nav2_map_server/include/nav2_map_server/vector_object_utils.hpp
index 8814dc3253d..1903bc5d002 100644
--- a/nav2_map_server/include/nav2_map_server/vector_object_utils.hpp
+++ b/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"
diff --git a/nav2_map_server/src/vo_server/vector_object_server.cpp b/nav2_map_server/src/vo_server/vector_object_server.cpp
index c76de4c49d6..291b4622005 100644
--- a/nav2_map_server/src/vo_server/vector_object_server.cpp
+++ b/nav2_map_server/src/vo_server/vector_object_server.cpp
@@ -14,14 +14,19 @@
 
 #include "nav2_map_server/vector_object_server.hpp"
 
-#include <cmath>
+#include <chrono>
+#include <exception>
 #include <functional>
 #include <limits>
 #include <stdexcept>
+#include <utility>
+
+#include "rclcpp/create_timer.hpp"
 
 #include "tf2_ros/create_timer_ros.h"
 
 #include "nav2_util/occ_grid_utils.hpp"
+#include "nav2_util/occ_grid_values.hpp"
 
 using namespace std::placeholders;
 
@@ -133,6 +138,63 @@ VectorObjectServer::on_shutdown(const rclcpp_lifecycle::State & /*state*/)
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
+bool VectorObjectServer::obtainParams()
+{
+  // Main ROS-parameters
+  map_topic_ = getParameter(
+    shared_from_this(), "map_topic", "vo_map").as_string();
+  global_frame_id_ = getParameter(
+    shared_from_this(), "global_frame_id", "map").as_string();
+  resolution_ = getParameter(
+    shared_from_this(), "resolution", 0.05).as_double();
+  default_value_ = getParameter(
+    shared_from_this(), "default_value", nav2_util::OCC_GRID_UNKNOWN).as_int();
+  overlay_type_ = static_cast<OverlayType>(getParameter(
+      shared_from_this(), "overlay_type", static_cast<int>(OverlayType::OVERLAY_SEQ)).as_int());
+  update_frequency_ = getParameter(
+    shared_from_this(), "update_frequency", 1.0).as_double();
+  transform_tolerance_ = getParameter(
+    shared_from_this(), "transform_tolerance", 0.1).as_double();
+
+  // Shapes
+  std::vector<std::string> shape_names = getParameter(
+    shared_from_this(), "shapes", std::vector<std::string>()).as_string_array();
+  for (std::string shape_name : shape_names) {
+    std::string shape_type;
+
+    try {
+      shape_type = getParameter(
+        shared_from_this(), shape_name + ".type", rclcpp::PARAMETER_STRING).as_string();
+    } catch (const std::exception & ex) {
+      RCLCPP_ERROR(
+        get_logger(),
+        "Error while getting shape %s type: %s",
+        shape_name.c_str(), ex.what());
+      return false;
+    }
+
+    if (shape_type == "polygon") {
+      std::shared_ptr<Polygon> polygon = std::make_shared<Polygon>(shared_from_this());
+      if (!polygon->obtainParams(shape_name)) {
+        return false;
+      }
+      shapes_.push_back(polygon);
+    } else if (shape_type == "circle") {
+      std::shared_ptr<Circle> circle = std::make_shared<Circle>(shared_from_this());
+
+      if (!circle->obtainParams(shape_name)) {
+        return false;
+      }
+      shapes_.push_back(circle);
+    } else {
+      RCLCPP_ERROR(get_logger(), "Please specify correct shape %s type", shape_name.c_str());
+      return false;
+    }
+  }
+
+  return true;
+}
+
 std::vector<std::shared_ptr<Shape>>::iterator
 VectorObjectServer::findShape(const unsigned char * uuid)
 {
@@ -486,63 +548,6 @@ void VectorObjectServer::removeShapesCallback(
   switchMapUpdate();
 }
 
-bool VectorObjectServer::obtainParams()
-{
-  // Main ROS-parameters
-  map_topic_ = getParameter(
-    shared_from_this(), "map_topic", "vo_map").as_string();
-  global_frame_id_ = getParameter(
-    shared_from_this(), "global_frame_id", "map").as_string();
-  resolution_ = getParameter(
-    shared_from_this(), "resolution", 0.05).as_double();
-  default_value_ = getParameter(
-    shared_from_this(), "default_value", nav2_util::OCC_GRID_UNKNOWN).as_int();
-  overlay_type_ = static_cast<OverlayType>(getParameter(
-      shared_from_this(), "overlay_type", static_cast<int>(OverlayType::OVERLAY_SEQ)).as_int());
-  update_frequency_ = getParameter(
-    shared_from_this(), "update_frequency", 1.0).as_double();
-  transform_tolerance_ = getParameter(
-    shared_from_this(), "transform_tolerance", 0.1).as_double();
-
-  // Shapes
-  std::vector<std::string> shape_names = getParameter(
-    shared_from_this(), "shapes", std::vector<std::string>()).as_string_array();
-  for (std::string shape_name : shape_names) {
-    std::string shape_type;
-
-    try {
-      shape_type = getParameter(
-        shared_from_this(), shape_name + ".type", rclcpp::PARAMETER_STRING).as_string();
-    } catch (const std::exception & ex) {
-      RCLCPP_ERROR(
-        get_logger(),
-        "Error while getting shape %s type: %s",
-        shape_name.c_str(), ex.what());
-      return false;
-    }
-
-    if (shape_type == "polygon") {
-      std::shared_ptr<Polygon> polygon = std::make_shared<Polygon>(shared_from_this());
-      if (!polygon->obtainParams(shape_name)) {
-        return false;
-      }
-      shapes_.push_back(polygon);
-    } else if (shape_type == "circle") {
-      std::shared_ptr<Circle> circle = std::make_shared<Circle>(shared_from_this());
-
-      if (!circle->obtainParams(shape_name)) {
-        return false;
-      }
-      shapes_.push_back(circle);
-    } else {
-      RCLCPP_ERROR(get_logger(), "Please specify correct shape %s type", shape_name.c_str());
-      return false;
-    }
-  }
-
-  return true;
-}
-
 }  // namespace nav2_map_server
 
 #include "rclcpp_components/register_node_macro.hpp"
diff --git a/nav2_map_server/src/vo_server/vector_object_shapes.cpp b/nav2_map_server/src/vo_server/vector_object_shapes.cpp
index 165e199cae9..44022bc061d 100644
--- a/nav2_map_server/src/vo_server/vector_object_shapes.cpp
+++ b/nav2_map_server/src/vo_server/vector_object_shapes.cpp
@@ -15,10 +15,11 @@
 #include "nav2_map_server/vector_object_shapes.hpp"
 
 #include <uuid/uuid.h>
-#include <math.h>
 #include <cmath>
+#include <exception>
 #include <limits>
 #include <stdexcept>
+#include <vector>
 
 #include "geometry_msgs/msg/pose_stamped.hpp"
 
@@ -87,6 +88,31 @@ Polygon::Polygon(
   type_ = POLYGON;
 }
 
+int8_t Polygon::getValue() const
+{
+  return params_->value;
+}
+
+std::string Polygon::getFrameID() const
+{
+  return params_->header.frame_id;
+}
+
+std::string Polygon::getUUID() const
+{
+  return unparseUUID(params_->uuid.uuid.data());
+}
+
+bool Polygon::isUUID(const unsigned char * uuid) const
+{
+  return uuid_compare(params_->uuid.uuid.data(), uuid) == 0;
+}
+
+bool Polygon::isFill() const
+{
+  return params_->closed;
+}
+
 bool Polygon::obtainParams(const std::string & shape_name)
 {
   auto node = node_.lock();
@@ -149,6 +175,54 @@ bool Polygon::obtainParams(const std::string & shape_name)
   return obtainShapeUUID(shape_name, params_->uuid.uuid.data());
 }
 
+nav2_msgs::msg::PolygonObject::SharedPtr Polygon::getParams() const
+{
+  return params_;
+}
+
+bool Polygon::setParams(const nav2_msgs::msg::PolygonObject::SharedPtr params)
+{
+  params_ = params;
+
+  if (!polygon_) {
+    polygon_ = std::make_shared<geometry_msgs::msg::Polygon>();
+  }
+  polygon_->points = params_->points;
+
+  // If no UUID was specified, generate a new one
+  if (uuid_is_null(params_->uuid.uuid.data())) {
+    uuid_generate(params_->uuid.uuid.data());
+  }
+
+  return checkConsistency();
+}
+
+bool Polygon::toFrame(
+  const std::string & to_frame,
+  const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
+  const double transform_tolerance)
+{
+  geometry_msgs::msg::PoseStamped from_pose, to_pose;
+  from_pose.header = params_->header;
+  for (unsigned int i = 0; i < params_->points.size(); i++) {
+    from_pose.pose.position.x = params_->points[i].x;
+    from_pose.pose.position.y = params_->points[i].y;
+    from_pose.pose.position.z = params_->points[i].z;
+    if (
+      nav2_util::transformPoseInTargetFrame(
+        from_pose, to_pose, *tf_buffer, to_frame, transform_tolerance))
+    {
+      polygon_->points[i].x = to_pose.pose.position.x;
+      polygon_->points[i].y = to_pose.pose.position.y;
+      polygon_->points[i].z = to_pose.pose.position.z;
+    } else {
+      return false;
+    }
+  }
+
+  return true;
+}
+
 void Polygon::getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y)
 {
   min_x = std::numeric_limits<double>::max();
@@ -210,79 +284,6 @@ void Polygon::putBorders(
   }
 }
 
-nav2_msgs::msg::PolygonObject::SharedPtr Polygon::getParams() const
-{
-  return params_;
-}
-
-bool Polygon::setParams(const nav2_msgs::msg::PolygonObject::SharedPtr params)
-{
-  params_ = params;
-
-  if (!polygon_) {
-    polygon_ = std::make_shared<geometry_msgs::msg::Polygon>();
-  }
-  polygon_->points = params_->points;
-
-  // If no UUID was specified, generate a new one
-  if (uuid_is_null(params_->uuid.uuid.data())) {
-    uuid_generate(params_->uuid.uuid.data());
-  }
-
-  return checkConsistency();
-}
-
-int8_t Polygon::getValue() const
-{
-  return params_->value;
-}
-
-std::string Polygon::getFrameID() const
-{
-  return params_->header.frame_id;
-}
-
-std::string Polygon::getUUID() const
-{
-  return unparseUUID(params_->uuid.uuid.data());
-}
-
-bool Polygon::isUUID(const unsigned char * uuid) const
-{
-  return uuid_compare(params_->uuid.uuid.data(), uuid) == 0;
-}
-
-bool Polygon::isFill() const
-{
-  return params_->closed;
-}
-
-bool Polygon::toFrame(
-  const std::string & to_frame,
-  const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
-  const double transform_tolerance)
-{
-  geometry_msgs::msg::PoseStamped from_pose, to_pose;
-  from_pose.header = params_->header;
-  for (unsigned int i = 0; i < params_->points.size(); i++) {
-    from_pose.pose.position.x = params_->points[i].x;
-    from_pose.pose.position.y = params_->points[i].y;
-    from_pose.pose.position.z = params_->points[i].z;
-    if (
-      nav2_util::transformPoseInTargetFrame(
-        from_pose, to_pose, *tf_buffer, to_frame, transform_tolerance))
-    {
-      polygon_->points[i].x = to_pose.pose.position.x;
-      polygon_->points[i].y = to_pose.pose.position.y;
-      polygon_->points[i].z = to_pose.pose.position.z;
-    } else {
-      return false;
-    }
-  }
-
-  return true;
-}
-
 bool Polygon::checkConsistency()
 {
   if (params_->points.size() < 3) {
@@ -310,6 +311,31 @@ Circle::Circle(
   type_ = CIRCLE;
 }
 
+int8_t Circle::getValue() const
+{
+  return params_->value;
+}
+
+std::string Circle::getFrameID() const
+{
+  return params_->header.frame_id;
+}
+
+std::string Circle::getUUID() const
+{
+  return unparseUUID(params_->uuid.uuid.data());
+}
+
+bool Circle::isUUID(const unsigned char * uuid) const
+{
+  return uuid_compare(params_->uuid.uuid.data(), uuid) == 0;
+}
+
+bool Circle::isFill() const
+{
+  return params_->fill;
+}
+
 bool Circle::obtainParams(const std::string & shape_name)
 {
   auto node = node_.lock();
@@ -371,65 +397,66 @@ bool Circle::obtainParams(const std::string & shape_name)
   return obtainShapeUUID(shape_name, params_->uuid.uuid.data());
 }
 
-// Get/update shape boundaries
-void Circle::getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y)
+nav2_msgs::msg::CircleObject::SharedPtr Circle::getParams() const
 {
-  min_x = center_->x - params_->radius;
-  min_y = center_->y - params_->radius;
-  max_x = center_->x + params_->radius;
-  max_y = center_->y + params_->radius;
+  return params_;
 }
 
-bool Circle::isPointInside(const double px, const double py) const
+bool Circle::setParams(const nav2_msgs::msg::CircleObject::SharedPtr params)
 {
-  return ( (px - center_->x) * (px - center_->x) + (py - center_->y) * (py - center_->y) ) <=
-         params_->radius * params_->radius;
-}
+  params_ = params;
 
-bool Circle::centerToMap(
-  nav_msgs::msg::OccupancyGrid::ConstSharedPtr map,
-  unsigned int & mcx, unsigned int & mcy)
-{
-  auto node = node_.lock();
-  if (!node) {
-    throw std::runtime_error{"Failed to lock node"};
+  if (!center_) {
+    center_ = std::make_shared<geometry_msgs::msg::Point32>();
   }
+  *center_ = params_->center;
 
-  // Get center of circle in map coordinates
-  if (center_->x < map->info.origin.position.x || center_->y < map->info.origin.position.y) {
-    RCLCPP_ERROR(
-      node->get_logger(),
-      "[UUID: %s] Can not convert (%f, %f) circle center to map",
-      getUUID().c_str(), center_->x, center_->y);
-    return false;
+  // If no UUID was specified, generate a new one
+  if (uuid_is_null(params_->uuid.uuid.data())) {
+    uuid_generate(params_->uuid.uuid.data());
   }
-  // We need the circle center to be always shifted one cell less its logical center
-  // and to avoid any FP-accuracy loosing on small values, so we are using another
-  // than nav2_util::worldToMap() approach
-  mcx = static_cast<unsigned int>(
-    std::round((center_->x - map->info.origin.position.x) / map->info.resolution)) - 1;
-  mcy = static_cast<unsigned int>(
-    std::round((center_->y - map->info.origin.position.y) / map->info.resolution)) - 1;
-  if (mcx >= map->info.width || mcy >= map->info.height) {
-    RCLCPP_ERROR(
-      node->get_logger(),
-      "[UUID: %s] Can not convert (%f, %f) point to map",
-      getUUID().c_str(), center_->x, center_->y);
+
+  return checkConsistency();
+}
+
+bool Circle::toFrame(
+  const std::string & to_frame,
+  const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
+  const double transform_tolerance)
+{
+  geometry_msgs::msg::PoseStamped from_pose, to_pose;
+  from_pose.header = params_->header;
+  from_pose.pose.position.x = params_->center.x;
+  from_pose.pose.position.y = params_->center.y;
+  from_pose.pose.position.z = params_->center.z;
+  if (
+    nav2_util::transformPoseInTargetFrame(
+      from_pose, to_pose, *tf_buffer, to_frame, transform_tolerance))
+  {
+    center_->x = to_pose.pose.position.x;
+    center_->y = to_pose.pose.position.y;
+    center_->z = to_pose.pose.position.z;
+  } else {
     return false;
   }
 
   return true;
 }
 
-inline void Circle::putPoint(
-  unsigned int mx, unsigned int my,
-  nav_msgs::msg::OccupancyGrid::SharedPtr map,
-  const OverlayType overlay_type)
+void Circle::getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y)
 {
-  fillMap(map, my * map->info.width + mx, params_->value, overlay_type);
+  min_x = center_->x - params_->radius;
+  min_y = center_->y - params_->radius;
+  max_x = center_->x + params_->radius;
+  max_y = center_->y + params_->radius;
+}
+
+bool Circle::isPointInside(const double px, const double py) const
+{
+  return ( (px - center_->x) * (px - center_->x) + (py - center_->y) * (py - center_->y) ) <=
+         params_->radius * params_->radius;
 }
 
-// Put params_gons line borders on map
 void Circle::putBorders(
   nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type)
 {
@@ -479,77 +506,6 @@ void Circle::putBorders(
   }
 }
 
-nav2_msgs::msg::CircleObject::SharedPtr Circle::getParams() const
-{
-  return params_;
-}
-
-bool Circle::setParams(const nav2_msgs::msg::CircleObject::SharedPtr params)
-{
-  params_ = params;
-
-  if (!center_) {
-    center_ = std::make_shared<geometry_msgs::msg::Point32>();
-  }
-  *center_ = params_->center;
-
-  // If no UUID was specified, generate a new one
-  if (uuid_is_null(params_->uuid.uuid.data())) {
-    uuid_generate(params_->uuid.uuid.data());
-  }
-
-  return checkConsistency();
-}
-
-int8_t Circle::getValue() const
-{
-  return params_->value;
-}
-
-std::string Circle::getFrameID() const
-{
-  return params_->header.frame_id;
-}
-
-std::string Circle::getUUID() const
-{
-  return unparseUUID(params_->uuid.uuid.data());
-}
-
-bool Circle::isUUID(const unsigned char * uuid) const
-{
-  return uuid_compare(params_->uuid.uuid.data(), uuid) == 0;
-}
-
-bool Circle::isFill() const
-{
-  return params_->fill;
-}
-
-bool Circle::toFrame(
-  const std::string & to_frame,
-  const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
-  const double transform_tolerance)
-{
-  geometry_msgs::msg::PoseStamped from_pose, to_pose;
-  from_pose.header = params_->header;
-  from_pose.pose.position.x = params_->center.x;
-  from_pose.pose.position.y = params_->center.y;
-  from_pose.pose.position.z = params_->center.z;
-  if (
-    nav2_util::transformPoseInTargetFrame(
-      from_pose, to_pose, *tf_buffer, to_frame, transform_tolerance))
-  {
-    center_->x = to_pose.pose.position.x;
-    center_->y = to_pose.pose.position.y;
-    center_->z = to_pose.pose.position.z;
-  } else {
-    return false;
-  }
-
-  return true;
-}
-
 bool Circle::checkConsistency()
 {
   if (params_->radius < 0.0) {
@@ -567,4 +523,47 @@ bool Circle::checkConsistency()
   return true;
 }
 
+bool Circle::centerToMap(
+  nav_msgs::msg::OccupancyGrid::ConstSharedPtr map,
+  unsigned int & mcx, unsigned int & mcy)
+{
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  // Get center of circle in map coordinates
+  if (center_->x < map->info.origin.position.x || center_->y < map->info.origin.position.y) {
+    RCLCPP_ERROR(
+      node->get_logger(),
+      "[UUID: %s] Can not convert (%f, %f) circle center to map",
+      getUUID().c_str(), center_->x, center_->y);
+    return false;
+  }
+  // We need the circle center to be always shifted one cell less its logical center
+  // and to avoid any FP-accuracy loosing on small values, so we are using another
+  // than nav2_util::worldToMap() approach
+  mcx = static_cast<unsigned int>(
+    std::round((center_->x - map->info.origin.position.x) / map->info.resolution)) - 1;
+  mcy = static_cast<unsigned int>(
+    std::round((center_->y - map->info.origin.position.y) / map->info.resolution)) - 1;
+  if (mcx >= map->info.width || mcy >= map->info.height) {
+    RCLCPP_ERROR(
+      node->get_logger(),
+      "[UUID: %s] Can not convert (%f, %f) point to map",
+      getUUID().c_str(), center_->x, center_->y);
+    return false;
+  }
+
+  return true;
+}
+
+inline void Circle::putPoint(
+  unsigned int mx, unsigned int my,
+  nav_msgs::msg::OccupancyGrid::SharedPtr map,
+  const OverlayType overlay_type)
+{
+  fillMap(map, my * map->info.width + mx, params_->value, overlay_type);
+}
+
 }  // namespace nav2_map_server