diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md
index 54841675e9..788a77ebd4 100644
--- a/nav2_collision_monitor/README.md
+++ b/nav2_collision_monitor/README.md
@@ -27,9 +27,9 @@ The following models of safety behaviors are employed by Collision Monitor:
 
 The zones around the robot can take the following shapes:
 
-* Arbitrary user-defined polygon relative to the robot base frame.
-* Circle: is made for the best performance and could be used in the cases where the zone or robot could be approximated by round shape.
-* Robot footprint polygon, which is used in the approach behavior model only. Will use the footprint topic to allow it to be dynamically adjusted over time.
+* Arbitrary user-defined polygon relative to the robot base frame, which can be static in a configuration file or dynamically changing via a topic interface.
+* Robot footprint polygon, which is used in the approach behavior model only. Will use the static user-defined polygon or the footprint topic to allow it to be dynamically adjusted over time.
+* Circle: is made for the best performance and could be used in the cases where the zone or robot footprint could be approximated by round shape.
 
 The data may be obtained from different data sources:
 
@@ -43,7 +43,7 @@ The Collision Monitor is designed to operate below Nav2 as an independent safety
 This acts as a filter on the `cmd_vel` topic coming out of the Controller Server. If no such zone is triggered, then the Controller's `cmd_vel` is used. Else, it is scaled or set to stop as appropriate.
 
 The following diagram is showing the high-level design of Collision Monitor module. All shapes (Polygons and Circles) are derived from base `Polygon` class, so without loss of generality we can call them as polygons. Subscribed footprint is also having the same properties as other polygons, but it is being obtained a footprint topic for the Approach Model.
-![HDL.png](doc/HLD.png)
+![HLD.png](doc/HLD.png)
 
 ## Configuration
 
diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp
index 807d4b3dc7..9cab7485be 100644
--- a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp
+++ b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp
@@ -66,15 +66,24 @@ class Circle : public Polygon
    */
   int getPointsInside(const std::vector<Point> & points) const override;
 
+  /**
+   * @brief Specifies that the shape is always set for a circle object
+   */
+  bool isShapeSet() override {return true;}
+
 protected:
   /**
    * @brief Supporting routine obtaining polygon-specific ROS-parameters
+   * @brief polygon_sub_topic Input name of polygon subscription topic
    * @param polygon_pub_topic Output name of polygon publishing topic
    * @param footprint_topic Output name of footprint topic. For Circle returns empty string,
    * there is no footprint subscription in this class.
    * @return True if all parameters were obtained or false in failure case
    */
-  bool getParameters(std::string & polygon_pub_topic, std::string & footprint_topic) override;
+  bool getParameters(
+    std::string & polygon_sub_topic,
+    std::string & polygon_pub_topic,
+    std::string & footprint_topic) override;
 
   // ----- Variables -----
 
diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp
index 97423dc411..e1354e434f 100644
--- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp
+++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp
@@ -21,7 +21,6 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include "geometry_msgs/msg/polygon_stamped.hpp"
-#include "geometry_msgs/msg/polygon.hpp"
 
 #include "tf2/time.h"
 #include "tf2_ros/buffer.h"
@@ -110,6 +109,12 @@ class Polygon
    */
   virtual void getPolygon(std::vector<Point> & poly) const;
 
+  /**
+   * @brief Returns true if polygon points were set.
+   * Othewise, prints a warning and returns false.
+   */
+  virtual bool isShapeSet();
+
   /**
    * @brief Updates polygon from footprint subscriber (if any)
    */
@@ -138,7 +143,7 @@ class Polygon
   /**
    * @brief Publishes polygon message into a its own topic
    */
-  void publish() const;
+  void publish();
 
 protected:
   /**
@@ -150,11 +155,29 @@ class Polygon
 
   /**
    * @brief Supporting routine obtaining polygon-specific ROS-parameters
+   * @brief polygon_sub_topic Output name of polygon subscription topic.
+   * Empty, if no polygon subscription.
    * @param polygon_pub_topic Output name of polygon publishing topic
-   * @param footprint_topic Output name of footprint topic. Empty, if no footprint subscription
+   * @param footprint_topic Output name of footprint topic.
+   * Empty, if no footprint subscription.
    * @return True if all parameters were obtained or false in failure case
    */
-  virtual bool getParameters(std::string & polygon_pub_topic, std::string & footprint_topic);
+  virtual bool getParameters(
+    std::string & polygon_sub_topic,
+    std::string & polygon_pub_topic,
+    std::string & footprint_topic);
+
+  /**
+   * @brief Updates polygon from geometry_msgs::msg::PolygonStamped message
+   * @param msg Message to update polygon from
+   */
+  void updatePolygon(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg);
+
+  /**
+   * @brief Dynamic polygon callback
+   * @param msg Shared pointer to the polygon message
+   */
+  void polygonCallback(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg);
 
   /**
    * @brief Checks if point is inside polygon
@@ -183,6 +206,8 @@ class Polygon
   double time_before_collision_;
   /// @brief Time step for robot movement simulation
   double simulation_time_step_;
+  /// @brief Polygon subscription
+  rclcpp::Subscription<geometry_msgs::msg::PolygonStamped>::SharedPtr polygon_sub_;
   /// @brief Footprint subscriber
   std::unique_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_;
 
@@ -197,8 +222,8 @@ class Polygon
   // Visualization
   /// @brief Whether to publish the polygon
   bool visualize_;
-  /// @brief Polygon points stored for later publishing
-  geometry_msgs::msg::Polygon polygon_;
+  /// @brief Polygon stored for later publishing
+  geometry_msgs::msg::PolygonStamped polygon_;
   /// @brief Polygon publisher for visualization purposes
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PolygonStamped>::SharedPtr polygon_pub_;
 
diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp
index a24859bb4a..5fd95de7ee 100644
--- a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp
+++ b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp
@@ -88,21 +88,6 @@ class Source
     const rclcpp::Time & source_time,
     const rclcpp::Time & curr_time) const;
 
-  /**
-   * @brief Obtains a transform from source_frame_id at source_time ->
-   * to base_frame_id_ at curr_time time
-   * @param source_frame_id Source frame ID to convert data from
-   * @param source_time Source timestamp to convert data from
-   * @param curr_time Current node time to interpolate data to
-   * @param tf_transform Output source->base transform
-   * @return True if got correct transform, otherwise false
-   */
-  bool getTransform(
-    const std::string & source_frame_id,
-    const rclcpp::Time & source_time,
-    const rclcpp::Time & curr_time,
-    tf2::Transform & tf_transform) const;
-
   // ----- Variables -----
 
   /// @brief Collision Monitor node
diff --git a/nav2_collision_monitor/src/circle.cpp b/nav2_collision_monitor/src/circle.cpp
index 7dc9708967..fa0fc33a4f 100644
--- a/nav2_collision_monitor/src/circle.cpp
+++ b/nav2_collision_monitor/src/circle.cpp
@@ -70,7 +70,10 @@ int Circle::getPointsInside(const std::vector<Point> & points) const
   return num;
 }
 
-bool Circle::getParameters(std::string & polygon_pub_topic, std::string & footprint_topic)
+bool Circle::getParameters(
+  std::string & polygon_sub_topic,
+  std::string & polygon_pub_topic,
+  std::string & footprint_topic)
 {
   auto node = node_.lock();
   if (!node) {
@@ -81,7 +84,8 @@ bool Circle::getParameters(std::string & polygon_pub_topic, std::string & footpr
     return false;
   }
 
-  // There is no footprint subscription for the Circle. Thus, set string as empty.
+  // There is no polygon or footprint subscription for the Circle. Thus, set strings as empty.
+  polygon_sub_topic.clear();
   footprint_topic.clear();
 
   try {
diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp
index 313d71fb0a..d706ba0cd9 100644
--- a/nav2_collision_monitor/src/collision_monitor_node.cpp
+++ b/nav2_collision_monitor/src/collision_monitor_node.cpp
@@ -389,6 +389,10 @@ bool CollisionMonitor::processStopSlowdown(
   const Velocity & velocity,
   Action & robot_action) const
 {
+  if (!polygon->isShapeSet()) {
+    return false;
+  }
+
   if (polygon->getPointsInside(collision_points) > polygon->getMaxPoints()) {
     if (polygon->getActionType() == STOP) {
       // Setting up zero velocity for STOP model
@@ -420,6 +424,10 @@ bool CollisionMonitor::processApproach(
 {
   polygon->updatePolygon();
 
+  if (!polygon->isShapeSet()) {
+    return false;
+  }
+
   // Obtain time before a collision
   const double collision_time = polygon->getCollisionTime(collision_points, velocity);
   if (collision_time >= 0.0) {
diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp
index df4e86b63e..282dec5463 100644
--- a/nav2_collision_monitor/src/pointcloud.cpp
+++ b/nav2_collision_monitor/src/pointcloud.cpp
@@ -19,6 +19,7 @@
 #include "sensor_msgs/point_cloud2_iterator.hpp"
 
 #include "nav2_util/node_utils.hpp"
+#include "nav2_util/robot_utils.hpp"
 
 namespace nav2_collision_monitor
 {
@@ -78,7 +79,12 @@ void PointCloud::getData(
   // Obtaining the transform to get data from source frame and time where it was received
   // to the base frame and current time
   tf2::Transform tf_transform;
-  if (!getTransform(data_->header.frame_id, data_->header.stamp, curr_time, tf_transform)) {
+  if (
+    !nav2_util::getTransform(
+      data_->header.frame_id, data_->header.stamp,
+      base_frame_id_, curr_time, global_frame_id_,
+      transform_tolerance_, tf_buffer_, tf_transform))
+  {
     return;
   }
 
diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp
index d17c34b46a..824ee75f67 100644
--- a/nav2_collision_monitor/src/polygon.cpp
+++ b/nav2_collision_monitor/src/polygon.cpp
@@ -21,6 +21,7 @@
 #include "geometry_msgs/msg/point32.hpp"
 
 #include "nav2_util/node_utils.hpp"
+#include "nav2_util/robot_utils.hpp"
 
 #include "nav2_collision_monitor/kinematics.hpp"
 
@@ -43,6 +44,8 @@ Polygon::Polygon(
 Polygon::~Polygon()
 {
   RCLCPP_INFO(logger_, "[%s]: Destroying Polygon", polygon_name_.c_str());
+  polygon_sub_.reset();
+  polygon_pub_.reset();
   poly_.clear();
 }
 
@@ -53,20 +56,36 @@ bool Polygon::configure()
     throw std::runtime_error{"Failed to lock node"};
   }
 
-  std::string polygon_pub_topic, footprint_topic;
+  std::string polygon_sub_topic, polygon_pub_topic, footprint_topic;
 
-  if (!getParameters(polygon_pub_topic, footprint_topic)) {
+  if (!getParameters(polygon_sub_topic, polygon_pub_topic, footprint_topic)) {
     return false;
   }
 
+  if (!polygon_sub_topic.empty()) {
+    RCLCPP_INFO(
+      logger_,
+      "[%s]: Subscribing on %s topic for polygon",
+      polygon_name_.c_str(), polygon_sub_topic.c_str());
+    rclcpp::QoS polygon_qos = rclcpp::SystemDefaultsQoS();  // set to default
+    polygon_sub_ = node->create_subscription<geometry_msgs::msg::PolygonStamped>(
+      polygon_sub_topic, polygon_qos,
+      std::bind(&Polygon::polygonCallback, this, std::placeholders::_1));
+  }
+
   if (!footprint_topic.empty()) {
+    RCLCPP_INFO(
+      logger_,
+      "[%s]: Making footprint subscriber on %s topic",
+      polygon_name_.c_str(), footprint_topic.c_str());
     footprint_sub_ = std::make_unique<nav2_costmap_2d::FootprintSubscriber>(
       node, footprint_topic, *tf_buffer_,
       base_frame_id_, tf2::durationToSec(transform_tolerance_));
   }
 
   if (visualize_) {
-    // Fill polygon_ points for future usage
+    // Fill polygon_ for future usage
+    polygon_.header.frame_id = base_frame_id_;
     std::vector<Point> poly;
     getPolygon(poly);
     for (const Point & p : poly) {
@@ -74,7 +93,7 @@ bool Polygon::configure()
       p_s.x = p.x;
       p_s.y = p.y;
       // p_s.z will remain 0.0
-      polygon_.points.push_back(p_s);
+      polygon_.polygon.points.push_back(p_s);
     }
 
     rclcpp::QoS polygon_qos = rclcpp::SystemDefaultsQoS();  // set to default
@@ -129,6 +148,15 @@ void Polygon::getPolygon(std::vector<Point> & poly) const
   poly = poly_;
 }
 
+bool Polygon::isShapeSet()
+{
+  if (poly_.empty()) {
+    RCLCPP_WARN(logger_, "[%s]: Polygon shape is not set yet", polygon_name_.c_str());
+    return false;
+  }
+  return true;
+}
+
 void Polygon::updatePolygon()
 {
   if (footprint_sub_ != nullptr) {
@@ -139,14 +167,15 @@ void Polygon::updatePolygon()
 
     std::size_t new_size = footprint_vec.size();
     poly_.resize(new_size);
-    polygon_.points.resize(new_size);
+    polygon_.header.frame_id = base_frame_id_;
+    polygon_.polygon.points.resize(new_size);
 
     geometry_msgs::msg::Point32 p_s;
     for (std::size_t i = 0; i < new_size; i++) {
       poly_[i] = {footprint_vec[i].x, footprint_vec[i].y};
       p_s.x = footprint_vec[i].x;
       p_s.y = footprint_vec[i].y;
-      polygon_.points[i] = p_s;
+      polygon_.polygon.points[i] = p_s;
     }
   }
 }
@@ -192,7 +221,7 @@ double Polygon::getCollisionTime(
   return -1.0;
 }
 
-void Polygon::publish() const
+void Polygon::publish()
 {
   if (!visualize_) {
     return;
@@ -203,15 +232,10 @@ void Polygon::publish() const
     throw std::runtime_error{"Failed to lock node"};
   }
 
-  // Fill PolygonStamped struct
-  std::unique_ptr<geometry_msgs::msg::PolygonStamped> poly_s =
-    std::make_unique<geometry_msgs::msg::PolygonStamped>();
-  poly_s->header.stamp = node->now();
-  poly_s->header.frame_id = base_frame_id_;
-  poly_s->polygon = polygon_;
-
-  // Publish polygon
-  polygon_pub_->publish(std::move(poly_s));
+  // Actualize the time to current and publish the polygon
+  polygon_.header.stamp = node->now();
+  auto msg = std::make_unique<geometry_msgs::msg::PolygonStamped>(polygon_);
+  polygon_pub_->publish(std::move(msg));
 }
 
 bool Polygon::getCommonParameters(std::string & polygon_pub_topic)
@@ -280,7 +304,10 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic)
   return true;
 }
 
-bool Polygon::getParameters(std::string & polygon_pub_topic, std::string & footprint_topic)
+bool Polygon::getParameters(
+  std::string & polygon_sub_topic,
+  std::string & polygon_pub_topic,
+  std::string & footprint_topic)
 {
   auto node = node_.lock();
   if (!node) {
@@ -291,48 +318,62 @@ bool Polygon::getParameters(std::string & polygon_pub_topic, std::string & footp
     return false;
   }
 
+  // Clear the subscription topics. They will be set later, if necessary.
+  polygon_sub_topic.clear();
+  footprint_topic.clear();
+
   try {
-    if (action_type_ == APPROACH) {
-      // Obtain the footprint topic to make a footprint subscription for approach polygon
+    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_ + ".footprint_topic",
-        rclcpp::ParameterValue("local_costmap/published_footprint"));
-      footprint_topic =
-        node->get_parameter(polygon_name_ + ".footprint_topic").as_string();
+        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;
+      }
 
-      // This is robot footprint: do not need to get polygon points from ROS parameters.
-      // It will be set dynamically later.
-      return true;
-    } else {
-      // Make it empty otherwise
-      footprint_topic.clear();
-    }
+      // 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;
+      }
 
-    // Leave it not initialized: the will cause an error if it will 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(
+      // Do not need to proceed further, if "points" parameter is defined.
+      // Static polygon will be used.
+      return true;
+    } catch (const rclcpp::exceptions::ParameterUninitializedException &) {
+      RCLCPP_INFO(
         logger_,
-        "[%s]: Polygon has incorrect points description",
+        "[%s]: Polygon points are not defined. Using dynamic subscription instead.",
         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;
+    if (action_type_ == STOP || action_type_ == SLOWDOWN) {
+      // Dynamic polygon will be used
+      nav2_util::declare_parameter_if_not_declared(
+        node, polygon_name_ + ".polygon_sub_topic", rclcpp::PARAMETER_STRING);
+      polygon_sub_topic =
+        node->get_parameter(polygon_name_ + ".polygon_sub_topic").as_string();
+    } else if (action_type_ == APPROACH) {
+      // Obtain the footprint topic to make a footprint subscription for approach polygon
+      nav2_util::declare_parameter_if_not_declared(
+        node, polygon_name_ + ".footprint_topic",
+        rclcpp::ParameterValue("local_costmap/published_footprint"));
+      footprint_topic =
+        node->get_parameter(polygon_name_ + ".footprint_topic").as_string();
     }
   } catch (const std::exception & ex) {
     RCLCPP_ERROR(
@@ -345,6 +386,54 @@ bool Polygon::getParameters(std::string & polygon_pub_topic, std::string & footp
   return true;
 }
 
+void Polygon::updatePolygon(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg)
+{
+  std::size_t new_size = msg->polygon.points.size();
+
+  if (new_size < 3) {
+    RCLCPP_ERROR(
+      logger_,
+      "[%s]: Polygon should have at least 3 points",
+      polygon_name_.c_str());
+    return;
+  }
+
+  // Get the transform from PolygonStamped frame to base_frame_id_
+  tf2::Transform tf_transform;
+  if (
+    !nav2_util::getTransform(
+      msg->header.frame_id, base_frame_id_,
+      transform_tolerance_, tf_buffer_, tf_transform))
+  {
+    return;
+  }
+
+  // Set main polygon vertices
+  poly_.resize(new_size);
+  for (std::size_t i = 0; i < new_size; i++) {
+    // Transform point coordinates from PolygonStamped frame -> to base frame
+    tf2::Vector3 p_v3_s(msg->polygon.points[i].x, msg->polygon.points[i].y, 0.0);
+    tf2::Vector3 p_v3_b = tf_transform * p_v3_s;
+
+    // Fill poly_ array
+    poly_[i] = {p_v3_b.x(), p_v3_b.y()};
+  }
+
+  if (visualize_) {
+    // Store polygon_ for visualization
+    polygon_ = *msg;
+  }
+}
+
+void Polygon::polygonCallback(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg)
+{
+  RCLCPP_INFO(
+    logger_,
+    "[%s]: Polygon shape update has been arrived",
+    polygon_name_.c_str());
+  updatePolygon(msg);
+}
+
 inline bool Polygon::isPointInside(const Point & point) const
 {
   // Adaptation of Shimrat, Moshe. "Algorithm 112: position of point relative to polygon."
diff --git a/nav2_collision_monitor/src/range.cpp b/nav2_collision_monitor/src/range.cpp
index 3ef51e2b69..0ba4be75aa 100644
--- a/nav2_collision_monitor/src/range.cpp
+++ b/nav2_collision_monitor/src/range.cpp
@@ -19,6 +19,7 @@
 #include <functional>
 
 #include "nav2_util/node_utils.hpp"
+#include "nav2_util/robot_utils.hpp"
 
 namespace nav2_collision_monitor
 {
@@ -87,7 +88,12 @@ void Range::getData(
   // Obtaining the transform to get data from source frame and time where it was received
   // to the base frame and current time
   tf2::Transform tf_transform;
-  if (!getTransform(data_->header.frame_id, data_->header.stamp, curr_time, tf_transform)) {
+  if (
+    !nav2_util::getTransform(
+      data_->header.frame_id, data_->header.stamp,
+      base_frame_id_, curr_time, global_frame_id_,
+      transform_tolerance_, tf_buffer_, tf_transform))
+  {
     return;
   }
 
diff --git a/nav2_collision_monitor/src/scan.cpp b/nav2_collision_monitor/src/scan.cpp
index d1f52b31f1..63ea1d6a76 100644
--- a/nav2_collision_monitor/src/scan.cpp
+++ b/nav2_collision_monitor/src/scan.cpp
@@ -17,6 +17,8 @@
 #include <cmath>
 #include <functional>
 
+#include "nav2_util/robot_utils.hpp"
+
 namespace nav2_collision_monitor
 {
 
@@ -76,7 +78,12 @@ void Scan::getData(
   // Obtaining the transform to get data from source frame and time where it was received
   // to the base frame and current time
   tf2::Transform tf_transform;
-  if (!getTransform(data_->header.frame_id, data_->header.stamp, curr_time, tf_transform)) {
+  if (
+    !nav2_util::getTransform(
+      data_->header.frame_id, data_->header.stamp,
+      base_frame_id_, curr_time, global_frame_id_,
+      transform_tolerance_, tf_buffer_, tf_transform))
+  {
     return;
   }
 
diff --git a/nav2_collision_monitor/src/source.cpp b/nav2_collision_monitor/src/source.cpp
index bd1028518c..a7de3bbe4e 100644
--- a/nav2_collision_monitor/src/source.cpp
+++ b/nav2_collision_monitor/src/source.cpp
@@ -18,9 +18,6 @@
 
 #include "geometry_msgs/msg/transform_stamped.hpp"
 
-#include "tf2/convert.h"
-#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
-
 #include "nav2_util/node_utils.hpp"
 
 namespace nav2_collision_monitor
@@ -76,33 +73,4 @@ bool Source::sourceValid(
   return true;
 }
 
-bool Source::getTransform(
-  const std::string & source_frame_id,
-  const rclcpp::Time & source_time,
-  const rclcpp::Time & curr_time,
-  tf2::Transform & tf2_transform) const
-{
-  geometry_msgs::msg::TransformStamped transform;
-  tf2_transform.setIdentity();  // initialize by identical transform
-
-  try {
-    // Obtaining the transform to get data from source to base frame.
-    // This also considers the time shift between source and base.
-    transform = tf_buffer_->lookupTransform(
-      base_frame_id_, curr_time,
-      source_frame_id, source_time,
-      global_frame_id_, transform_tolerance_);
-  } catch (tf2::TransformException & e) {
-    RCLCPP_ERROR(
-      logger_,
-      "[%s]: Failed to get \"%s\"->\"%s\" frame transform: %s",
-      source_name_.c_str(), source_frame_id.c_str(), base_frame_id_.c_str(), e.what());
-    return false;
-  }
-
-  // Convert TransformStamped to TF2 transform
-  tf2::fromMsg(transform.transform, tf2_transform);
-  return true;
-}
-
 }  // namespace nav2_collision_monitor
diff --git a/nav2_collision_monitor/test/polygons_test.cpp b/nav2_collision_monitor/test/polygons_test.cpp
index 87576ddda9..c8c6b46601 100644
--- a/nav2_collision_monitor/test/polygons_test.cpp
+++ b/nav2_collision_monitor/test/polygons_test.cpp
@@ -40,8 +40,10 @@ using namespace std::chrono_literals;
 static constexpr double EPSILON = std::numeric_limits<float>::epsilon();
 
 static const char BASE_FRAME_ID[]{"base_link"};
+static const char BASE2_FRAME_ID[]{"base2_link"};
 static const char FOOTPRINT_TOPIC[]{"footprint"};
-static const char POLYGON_PUB_TOPIC[]{"polygon"};
+static const char POLYGON_SUB_TOPIC[]{"polygon_sub"};
+static const char POLYGON_PUB_TOPIC[]{"polygon_pub"};
 static const char POLYGON_NAME[]{"TestPolygon"};
 static const char CIRCLE_NAME[]{"TestCircle"};
 static const std::vector<double> SQUARE_POLYGON {
@@ -68,9 +70,38 @@ class TestNode : public nav2_util::LifecycleNode
 
   ~TestNode()
   {
+    polygon_pub_.reset();
     footprint_pub_.reset();
   }
 
+  void publishPolygon(const std::string & frame_id, const bool is_correct)
+  {
+    polygon_pub_ = this->create_publisher<geometry_msgs::msg::PolygonStamped>(
+      POLYGON_SUB_TOPIC, rclcpp::SystemDefaultsQoS());
+
+    std::unique_ptr<geometry_msgs::msg::PolygonStamped> msg =
+      std::make_unique<geometry_msgs::msg::PolygonStamped>();
+
+    unsigned int polygon_size;
+    if (is_correct) {
+      polygon_size = SQUARE_POLYGON.size();
+    } else {
+      polygon_size = 2;
+    }
+
+    msg->header.frame_id = frame_id;
+    msg->header.stamp = this->now();
+
+    geometry_msgs::msg::Point32 p;
+    for (unsigned int i = 0; i < polygon_size; i = i + 2) {
+      p.x = SQUARE_POLYGON[i];
+      p.y = SQUARE_POLYGON[i + 1];
+      msg->polygon.points.push_back(p);
+    }
+
+    polygon_pub_->publish(std::move(msg));
+  }
+
   void publishFootprint()
   {
     footprint_pub_ = this->create_publisher<geometry_msgs::msg::PolygonStamped>(
@@ -112,6 +143,7 @@ class TestNode : public nav2_util::LifecycleNode
   }
 
 private:
+  rclcpp::Publisher<geometry_msgs::msg::PolygonStamped>::SharedPtr polygon_pub_;
   rclcpp::Publisher<geometry_msgs::msg::PolygonStamped>::SharedPtr footprint_pub_;
   rclcpp::Subscription<geometry_msgs::msg::PolygonStamped>::SharedPtr polygon_sub_;
 
@@ -177,13 +209,22 @@ class Tester : public ::testing::Test
 protected:
   // Working with parameters
   void setCommonParameters(const std::string & polygon_name, const std::string & action_type);
-  void setPolygonParameters(const std::vector<double> & points);
+  void setPolygonParameters(
+    const std::vector<double> & points,
+    const bool is_static);
   void setCircleParameters(const double radius);
   bool checkUndeclaredParameter(const std::string & polygon_name, const std::string & param);
   // Creating routines
-  void createPolygon(const std::string & action_type);
+  void createPolygon(const std::string & action_type, const bool is_static);
   void createCircle(const std::string & action_type);
 
+  void sendTransforms();
+
+  // Wait until polygon will be received
+  bool waitPolygon(
+    const std::chrono::nanoseconds & timeout,
+    std::vector<nav2_collision_monitor::Point> & poly);
+
   // Wait until footprint will be received
   bool waitFootprint(
     const std::chrono::nanoseconds & timeout,
@@ -257,17 +298,25 @@ void Tester::setCommonParameters(const std::string & polygon_name, const std::st
     rclcpp::Parameter(polygon_name + ".polygon_pub_topic", POLYGON_PUB_TOPIC));
 }
 
-void Tester::setPolygonParameters(const std::vector<double> & points)
+void Tester::setPolygonParameters(
+  const std::vector<double> & points, const bool is_static)
 {
-  test_node_->declare_parameter(
-    std::string(POLYGON_NAME) + ".footprint_topic", rclcpp::ParameterValue(FOOTPRINT_TOPIC));
-  test_node_->set_parameter(
-    rclcpp::Parameter(std::string(POLYGON_NAME) + ".footprint_topic", FOOTPRINT_TOPIC));
-
-  test_node_->declare_parameter(
-    std::string(POLYGON_NAME) + ".points", rclcpp::ParameterValue(points));
-  test_node_->set_parameter(
-    rclcpp::Parameter(std::string(POLYGON_NAME) + ".points", points));
+  if (is_static) {
+    test_node_->declare_parameter(
+      std::string(POLYGON_NAME) + ".points", rclcpp::ParameterValue(points));
+    test_node_->set_parameter(
+      rclcpp::Parameter(std::string(POLYGON_NAME) + ".points", points));
+  } else {
+    test_node_->declare_parameter(
+      std::string(POLYGON_NAME) + ".polygon_sub_topic", rclcpp::ParameterValue(POLYGON_SUB_TOPIC));
+    test_node_->set_parameter(
+      rclcpp::Parameter(std::string(POLYGON_NAME) + ".polygon_sub_topic", POLYGON_SUB_TOPIC));
+
+    test_node_->declare_parameter(
+      std::string(POLYGON_NAME) + ".footprint_topic", rclcpp::ParameterValue(FOOTPRINT_TOPIC));
+    test_node_->set_parameter(
+      rclcpp::Parameter(std::string(POLYGON_NAME) + ".footprint_topic", FOOTPRINT_TOPIC));
+  }
 }
 
 void Tester::setCircleParameters(const double radius)
@@ -296,10 +345,10 @@ bool Tester::checkUndeclaredParameter(const std::string & polygon_name, const st
   return ret;
 }
 
-void Tester::createPolygon(const std::string & action_type)
+void Tester::createPolygon(const std::string & action_type, const bool is_static)
 {
   setCommonParameters(POLYGON_NAME, action_type);
-  setPolygonParameters(SQUARE_POLYGON);
+  setPolygonParameters(SQUARE_POLYGON, is_static);
 
   polygon_ = std::make_shared<PolygonWrapper>(
     test_node_, POLYGON_NAME,
@@ -320,6 +369,45 @@ void Tester::createCircle(const std::string & action_type)
   circle_->activate();
 }
 
+void Tester::sendTransforms()
+{
+  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster =
+    std::make_shared<tf2_ros::TransformBroadcaster>(test_node_);
+
+  geometry_msgs::msg::TransformStamped transform;
+
+  // base_frame -> base2_frame transform
+  transform.header.frame_id = BASE_FRAME_ID;
+  transform.child_frame_id = BASE2_FRAME_ID;
+
+  transform.header.stamp = test_node_->now();
+  transform.transform.translation.x = 0.1;
+  transform.transform.translation.y = 0.1;
+  transform.transform.translation.z = 0.0;
+  transform.transform.rotation.x = 0.0;
+  transform.transform.rotation.y = 0.0;
+  transform.transform.rotation.z = 0.0;
+  transform.transform.rotation.w = 1.0;
+
+  tf_broadcaster->sendTransform(transform);
+}
+
+bool Tester::waitPolygon(
+  const std::chrono::nanoseconds & timeout,
+  std::vector<nav2_collision_monitor::Point> & poly)
+{
+  rclcpp::Time start_time = test_node_->now();
+  while (rclcpp::ok() && test_node_->now() - start_time <= rclcpp::Duration(timeout)) {
+    polygon_->getPolygon(poly);
+    if (poly.size() > 0) {
+      return true;
+    }
+    rclcpp::spin_some(test_node_->get_node_base_interface());
+    std::this_thread::sleep_for(10ms);
+  }
+  return false;
+}
+
 bool Tester::waitFootprint(
   const std::chrono::nanoseconds & timeout,
   std::vector<nav2_collision_monitor::Point> & footprint)
@@ -339,7 +427,7 @@ bool Tester::waitFootprint(
 
 TEST_F(Tester, testPolygonGetStopParameters)
 {
-  createPolygon("stop");
+  createPolygon("stop", true);
 
   // Check that common parameters set correctly
   EXPECT_EQ(polygon_->getName(), POLYGON_NAME);
@@ -363,7 +451,7 @@ TEST_F(Tester, testPolygonGetStopParameters)
 
 TEST_F(Tester, testPolygonGetSlowdownParameters)
 {
-  createPolygon("slowdown");
+  createPolygon("slowdown", true);
 
   // Check that common parameters set correctly
   EXPECT_EQ(polygon_->getName(), POLYGON_NAME);
@@ -376,7 +464,7 @@ TEST_F(Tester, testPolygonGetSlowdownParameters)
 
 TEST_F(Tester, testPolygonGetApproachParameters)
 {
-  createPolygon("approach");
+  createPolygon("approach", true);
 
   // Check that common parameters set correctly
   EXPECT_EQ(polygon_->getName(), POLYGON_NAME);
@@ -415,7 +503,7 @@ TEST_F(Tester, testPolygonUndeclaredActionType)
 
 TEST_F(Tester, testPolygonUndeclaredPoints)
 {
-  // "points" parameter is not initialized
+  // "points" and "polygon_sub_topic" parameters are not initialized
   test_node_->declare_parameter(
     std::string(POLYGON_NAME) + ".action_type", rclcpp::ParameterValue("stop"));
   test_node_->set_parameter(
@@ -424,14 +512,15 @@ TEST_F(Tester, testPolygonUndeclaredPoints)
     test_node_, POLYGON_NAME,
     tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE);
   ASSERT_FALSE(polygon_->configure());
-  // Check that "points" parameter is not set after configuring
+  // Check that "points" and "polygon_sub_topic" parameters are not set after configuring
   ASSERT_TRUE(checkUndeclaredParameter(POLYGON_NAME, "points"));
+  ASSERT_TRUE(checkUndeclaredParameter(POLYGON_NAME, "polygon_sub_topic"));
 }
 
 TEST_F(Tester, testPolygonIncorrectActionType)
 {
   setCommonParameters(POLYGON_NAME, "incorrect_action_type");
-  setPolygonParameters(SQUARE_POLYGON);
+  setPolygonParameters(SQUARE_POLYGON, true);
 
   polygon_ = std::make_shared<PolygonWrapper>(
     test_node_, POLYGON_NAME,
@@ -486,12 +575,79 @@ TEST_F(Tester, testCircleUndeclaredRadius)
   ASSERT_TRUE(checkUndeclaredParameter(CIRCLE_NAME, "radius"));
 }
 
-TEST_F(Tester, testPolygonUpdate)
+TEST_F(Tester, testPolygonTopicUpdate)
+{
+  createPolygon("stop", false);
+
+  std::vector<nav2_collision_monitor::Point> poly;
+  polygon_->getPolygon(poly);
+  ASSERT_FALSE(polygon_->isShapeSet());
+  ASSERT_EQ(poly.size(), 0u);
+
+  // Publish incorrect shape and check that polygon was not updated
+  test_node_->publishPolygon(BASE_FRAME_ID, false);
+  ASSERT_FALSE(waitPolygon(100ms, poly));
+  ASSERT_FALSE(polygon_->isShapeSet());
+
+  // Publush correct polygon and make shure that it was set correctly
+  test_node_->publishPolygon(BASE_FRAME_ID, true);
+  ASSERT_TRUE(waitPolygon(500ms, poly));
+  ASSERT_TRUE(polygon_->isShapeSet());
+  ASSERT_EQ(poly.size(), 4u);
+  EXPECT_NEAR(poly[0].x, SQUARE_POLYGON[0], EPSILON);
+  EXPECT_NEAR(poly[0].y, SQUARE_POLYGON[1], EPSILON);
+  EXPECT_NEAR(poly[1].x, SQUARE_POLYGON[2], EPSILON);
+  EXPECT_NEAR(poly[1].y, SQUARE_POLYGON[3], EPSILON);
+  EXPECT_NEAR(poly[2].x, SQUARE_POLYGON[4], EPSILON);
+  EXPECT_NEAR(poly[2].y, SQUARE_POLYGON[5], EPSILON);
+  EXPECT_NEAR(poly[3].x, SQUARE_POLYGON[6], EPSILON);
+  EXPECT_NEAR(poly[3].y, SQUARE_POLYGON[7], EPSILON);
+}
+
+TEST_F(Tester, testPolygonTopicUpdateDifferentFrame)
+{
+  createPolygon("stop", false);
+  sendTransforms();
+
+  std::vector<nav2_collision_monitor::Point> poly;
+  polygon_->getPolygon(poly);
+  ASSERT_EQ(poly.size(), 0u);
+
+  // Publush polygon in different frame and make shure that it was set correctly
+  test_node_->publishPolygon(BASE2_FRAME_ID, true);
+  ASSERT_TRUE(waitPolygon(500ms, poly));
+  ASSERT_EQ(poly.size(), 4u);
+  EXPECT_NEAR(poly[0].x, SQUARE_POLYGON[0] + 0.1, EPSILON);
+  EXPECT_NEAR(poly[0].y, SQUARE_POLYGON[1] + 0.1, EPSILON);
+  EXPECT_NEAR(poly[1].x, SQUARE_POLYGON[2] + 0.1, EPSILON);
+  EXPECT_NEAR(poly[1].y, SQUARE_POLYGON[3] + 0.1, EPSILON);
+  EXPECT_NEAR(poly[2].x, SQUARE_POLYGON[4] + 0.1, EPSILON);
+  EXPECT_NEAR(poly[2].y, SQUARE_POLYGON[5] + 0.1, EPSILON);
+  EXPECT_NEAR(poly[3].x, SQUARE_POLYGON[6] + 0.1, EPSILON);
+  EXPECT_NEAR(poly[3].y, SQUARE_POLYGON[7] + 0.1, EPSILON);
+}
+
+TEST_F(Tester, testPolygonTopicUpdateIncorrectFrame)
+{
+  createPolygon("stop", false);
+  sendTransforms();
+
+  std::vector<nav2_collision_monitor::Point> poly;
+  polygon_->getPolygon(poly);
+  ASSERT_EQ(poly.size(), 0u);
+
+  // Publush polygon in incorrect frame and check that polygon was not updated
+  test_node_->publishPolygon("incorrect_frame", true);
+  ASSERT_FALSE(waitPolygon(100ms, poly));
+}
+
+TEST_F(Tester, testPolygonFootprintUpdate)
 {
-  createPolygon("approach");
+  createPolygon("approach", false);
 
   std::vector<nav2_collision_monitor::Point> poly;
   polygon_->getPolygon(poly);
+  ASSERT_FALSE(polygon_->isShapeSet());
   ASSERT_EQ(poly.size(), 0u);
 
   test_node_->publishFootprint();
@@ -499,6 +655,7 @@ TEST_F(Tester, testPolygonUpdate)
   std::vector<nav2_collision_monitor::Point> footprint;
   ASSERT_TRUE(waitFootprint(500ms, footprint));
 
+  ASSERT_TRUE(polygon_->isShapeSet());
   ASSERT_EQ(footprint.size(), 4u);
   EXPECT_NEAR(footprint[0].x, SQUARE_POLYGON[0], EPSILON);
   EXPECT_NEAR(footprint[0].y, SQUARE_POLYGON[1], EPSILON);
@@ -512,7 +669,7 @@ TEST_F(Tester, testPolygonUpdate)
 
 TEST_F(Tester, testPolygonGetPointsInside)
 {
-  createPolygon("stop");
+  createPolygon("stop", true);
 
   std::vector<nav2_collision_monitor::Point> points;
 
@@ -533,7 +690,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);
+  setPolygonParameters(ARBITRARY_POLYGON, true);
 
   polygon_ = std::make_shared<PolygonWrapper>(
     test_node_, POLYGON_NAME,
@@ -572,7 +729,7 @@ TEST_F(Tester, testCircleGetPointsInside)
 
 TEST_F(Tester, testPolygonGetCollisionTime)
 {
-  createPolygon("approach");
+  createPolygon("approach", false);
 
   // Set footprint for Polygon
   test_node_->publishFootprint();
@@ -640,7 +797,7 @@ TEST_F(Tester, testPolygonGetCollisionTime)
 
 TEST_F(Tester, testPolygonPublish)
 {
-  createPolygon("stop");
+  createPolygon("stop", true);
   polygon_->publish();
   geometry_msgs::msg::PolygonStamped::SharedPtr polygon_received =
     test_node_->waitPolygonReceived(500ms);
@@ -666,7 +823,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);
+  setPolygonParameters(SQUARE_POLYGON, true);
 
   // Create new polygon
   polygon_ = std::make_shared<PolygonWrapper>(
diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp
index b9712044b6..22306cb545 100644
--- a/nav2_util/include/nav2_util/robot_utils.hpp
+++ b/nav2_util/include/nav2_util/robot_utils.hpp
@@ -18,8 +18,10 @@
 #define NAV2_UTIL__ROBOT_UTILS_HPP_
 
 #include <string>
+#include <memory>
 
 #include "geometry_msgs/msg/pose_stamped.hpp"
+#include "tf2/time.h"
 #include "tf2_ros/buffer.h"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "rclcpp/rclcpp.hpp"
@@ -56,6 +58,57 @@ bool transformPoseInTargetFrame(
   tf2_ros::Buffer & tf_buffer, const std::string target_frame,
   const double transform_timeout = 0.1);
 
+/**
+ * @brief Obtains a transform from source_frame_id at source_time ->
+ * to target_frame_id at target_time time
+ * @param source_frame_id Source frame ID to convert from
+ * @param source_time Source timestamp to convert from
+ * @param target_frame_id Target frame ID to convert to
+ * @param target_time Target time to interpolate to
+ * @param transform_tolerance Transform tolerance
+ * @param tf_transform Output source->target transform
+ * @return True if got correct transform, otherwise false
+ */
+
+/**
+ * @brief Obtains a transform from source_frame_id -> to target_frame_id
+ * @param source_frame_id Source frame ID to convert from
+ * @param target_frame_id Target frame ID to convert to
+ * @param transform_tolerance Transform tolerance
+ * @param tf_buffer TF buffer to use for the transformation
+ * @param tf_transform Output source->target transform
+ * @return True if got correct transform, otherwise false
+ */
+bool getTransform(
+  const std::string & source_frame_id,
+  const std::string & target_frame_id,
+  const tf2::Duration & transform_tolerance,
+  const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
+  tf2::Transform & tf2_transform);
+
+/**
+ * @brief Obtains a transform from source_frame_id at source_time ->
+ * to target_frame_id at target_time time
+ * @param source_frame_id Source frame ID to convert from
+ * @param source_time Source timestamp to convert from
+ * @param target_frame_id Target frame ID to convert to
+ * @param target_time Current node time to interpolate to
+ * @param fixed_frame_id The frame in which to assume the transform is constant in time
+ * @param transform_tolerance Transform tolerance
+ * @param tf_buffer TF buffer to use for the transformation
+ * @param tf_transform Output source->target transform
+ * @return True if got correct transform, otherwise false
+ */
+bool getTransform(
+  const std::string & source_frame_id,
+  const rclcpp::Time & source_time,
+  const std::string & target_frame_id,
+  const rclcpp::Time & target_time,
+  const std::string & fixed_frame_id,
+  const tf2::Duration & transform_tolerance,
+  const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
+  tf2::Transform & tf2_transform);
+
 }  // end namespace nav2_util
 
 #endif  // NAV2_UTIL__ROBOT_UTILS_HPP_
diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp
index e99d9e3334..8714f05a99 100644
--- a/nav2_util/src/robot_utils.cpp
+++ b/nav2_util/src/robot_utils.cpp
@@ -17,6 +17,7 @@
 #include <string>
 #include <memory>
 
+#include "tf2/convert.h"
 #include "nav2_util/robot_utils.hpp"
 #include "rclcpp/logger.hpp"
 
@@ -75,4 +76,70 @@ bool transformPoseInTargetFrame(
   return false;
 }
 
+bool getTransform(
+  const std::string & source_frame_id,
+  const std::string & target_frame_id,
+  const tf2::Duration & transform_tolerance,
+  const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
+  tf2::Transform & tf2_transform)
+{
+  geometry_msgs::msg::TransformStamped transform;
+  tf2_transform.setIdentity();  // initialize by identical transform
+
+  if (source_frame_id == target_frame_id) {
+    // We are already in required frame
+    return true;
+  }
+
+  try {
+    // Obtaining the transform to get data from source to target frame
+    transform = tf_buffer->lookupTransform(
+      target_frame_id, source_frame_id,
+      tf2::TimePointZero, transform_tolerance);
+  } catch (tf2::TransformException & e) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("getTransform"),
+      "Failed to get \"%s\"->\"%s\" frame transform: %s",
+      source_frame_id.c_str(), target_frame_id.c_str(), e.what());
+    return false;
+  }
+
+  // Convert TransformStamped to TF2 transform
+  tf2::fromMsg(transform.transform, tf2_transform);
+  return true;
+}
+
+bool getTransform(
+  const std::string & source_frame_id,
+  const rclcpp::Time & source_time,
+  const std::string & target_frame_id,
+  const rclcpp::Time & target_time,
+  const std::string & fixed_frame_id,
+  const tf2::Duration & transform_tolerance,
+  const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
+  tf2::Transform & tf2_transform)
+{
+  geometry_msgs::msg::TransformStamped transform;
+  tf2_transform.setIdentity();  // initialize by identical transform
+
+  try {
+    // Obtaining the transform to get data from source to target frame.
+    // This also considers the time shift between source and target.
+    transform = tf_buffer->lookupTransform(
+      target_frame_id, target_time,
+      source_frame_id, source_time,
+      fixed_frame_id, transform_tolerance);
+  } catch (tf2::TransformException & ex) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("getTransform"),
+      "Failed to get \"%s\"->\"%s\" frame transform: %s",
+      source_frame_id.c_str(), target_frame_id.c_str(), ex.what());
+    return false;
+  }
+
+  // Convert TransformStamped to TF2 transform
+  tf2::fromMsg(transform.transform, tf2_transform);
+  return true;
+}
+
 }  // end namespace nav2_util