diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt
index eb91f295d47..e4243de004d 100644
--- a/nav2_collision_monitor/CMakeLists.txt
+++ b/nav2_collision_monitor/CMakeLists.txt
@@ -37,7 +37,9 @@ add_library(${monitor_library_name} SHARED
   src/polygon_source.cpp
   src/range.cpp
   src/kinematics.cpp
+  src/costmap.cpp
 )
+
 target_include_directories(${monitor_library_name}
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
@@ -146,6 +148,8 @@ install(DIRECTORY params DESTINATION share/${PROJECT_NAME})
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
+  find_package(launch_testing_ament_cmake REQUIRED)
+  find_package(rosgraph_msgs REQUIRED)
   # the following line skips the linter which checks for copyrights
   set(ament_cmake_copyright_FOUND TRUE)
   set(ament_cmake_cpplint_FOUND TRUE)
@@ -165,6 +169,7 @@ ament_export_dependencies(
   nav2_costmap_2d
   nav2_msgs
   nav2_util
+  rosgraph_msgs
   rclcpp
   rclcpp_lifecycle
   sensor_msgs
diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md
index a34976cce23..b8722a96f7c 100644
--- a/nav2_collision_monitor/README.md
+++ b/nav2_collision_monitor/README.md
@@ -44,6 +44,12 @@ The data may be obtained from different data sources:
 * Laser scanners (`sensor_msgs::msg::LaserScan` messages)
 * PointClouds (`sensor_msgs::msg::PointCloud2` messages)
 * IR/Sonars (`sensor_msgs::msg::Range` messages)
+* Costmap (`nav2_msgs::msg::Costmap` messages)
+
+> **⚠️ when using CostmapSource**
+> Collision Monitor normally **bypasses the costmap** to minimize reaction latency using fresh sensor data.
+> Use at your own caution or when using external costmap sources from derived sources.
+
 
 ### Design
 
diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp
index 85a04288087..7c3408903f3 100644
--- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp
+++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp
@@ -43,6 +43,7 @@
 #include "nav2_collision_monitor/scan.hpp"
 #include "nav2_collision_monitor/pointcloud.hpp"
 #include "nav2_collision_monitor/range.hpp"
+#include "nav2_collision_monitor/costmap.hpp"
 #include "nav2_collision_monitor/polygon_source.hpp"
 
 namespace nav2_collision_monitor
diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/costmap.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/costmap.hpp
new file mode 100644
index 00000000000..7ad42ade6e9
--- /dev/null
+++ b/nav2_collision_monitor/include/nav2_collision_monitor/costmap.hpp
@@ -0,0 +1,133 @@
+// Copyright (c) 2025 Angsa Robotics
+// Copyright (c) 2025 lotusymt
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_COLLISION_MONITOR__COSTMAP_HPP_
+#define NAV2_COLLISION_MONITOR__COSTMAP_HPP_
+
+/**
+ * @file costmap.hpp
+ * @brief Observation source that converts a Nav2 costmap topic into 2D points for Collision Monitor.
+ */
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "nav2_collision_monitor/source.hpp"
+#include "nav2_msgs/msg/costmap.hpp"
+#include <nav2_ros_common/lifecycle_node.hpp>
+#include <nav2_ros_common/subscription.hpp>
+
+namespace nav2_collision_monitor
+{
+
+/**
+ * @class CostmapSource
+ * @brief Reads nav2_msgs::msg::Costmap and produces obstacle points for Collision Monitor.
+ *
+ * Cells with cost >= @ref cost_threshold_ are exported as points. Optionally, NO_INFORMATION (255)
+ * can be treated as obstacles via @ref treat_unknown_as_obstacle_.
+ *
+ * Parameters (declared/queried in @ref getParameters):
+ * - `topic` (std::string): costmap topic name (relative is recommended).
+ * - `cost_threshold` (int, 0..255): minimum cost to consider a cell occupied.
+ * - `treat_unknown_as_obstacle` (bool): whether cost 255 should be treated as occupied.
+ */
+class CostmapSource : public Source
+{
+public:
+  /**
+   * @brief Construct a CostmapSource.
+   * @param node Weak pointer to the lifecycle node.
+   * @param source_name Logical name of this source instance (for params/logs).
+   * @param tf_buffer Shared TF buffer for frame transforms.
+   * @param base_frame_id Robot base frame (e.g., "base_footprint").
+   * @param global_frame_id Global frame of the costmap (e.g., "odom" or "map").
+   * @param transform_tolerance Allowed TF age for transforms.
+   * @param source_timeout Max age of data before it is considered stale.
+   * @param base_shift_correction Whether to compensate robot motion during simulation checks.
+   */
+  CostmapSource(
+    const nav2::LifecycleNode::WeakPtr & node,
+    const std::string & source_name,
+    const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
+    const std::string & base_frame_id,
+    const std::string & global_frame_id,
+    const tf2::Duration & transform_tolerance,
+    const rclcpp::Duration & source_timeout,
+    const bool base_shift_correction);
+
+  /// @brief Destructor.
+  ~CostmapSource();
+
+  /**
+   * @brief Declare and get parameters; create the subscription.
+   *
+   * Must be called during the node’s configuration phase (after construction, before use).
+   * Reads `topic`, `cost_threshold`, and `treat_unknown_as_obstacle`.
+   */
+  void configure();
+
+  /**
+   * @brief Produce current obstacle points from the latest costmap.
+   * @param curr_time Current time used for staleness checks and TF queries.
+   * @param[out] data Output vector of points in the base frame.
+   * @return true if valid, non-stale data were produced; false otherwise.
+   *
+   * @details
+   * - Returns false if no message has arrived or data are older than @ref source_timeout_.
+   * - Transforms points from costmap frame to @ref base_frame_id using @ref tf_buffer_.
+   * - Applies @ref cost_threshold_ and @ref treat_unknown_as_obstacle_.
+   */
+  bool getData(
+    const rclcpp::Time & curr_time,
+    std::vector<Point> & data) override;
+
+  /**
+   * @brief Read parameters specific to the costmap source.
+   * @param[out] source_topic Resolved topic name to subscribe to.
+   *
+   * Declares/gets: `topic`, `cost_threshold`, `treat_unknown_as_obstacle`.
+   */
+  void getParameters(std::string & source_topic);
+
+private:
+  /**
+   * @brief Subscription callback to store the latest costmap message.
+   * @param msg Incoming costmap.
+   */
+  void dataCallback(nav2_msgs::msg::Costmap::ConstSharedPtr msg);
+
+  /// @brief Latest costmap message.
+  nav2_msgs::msg::Costmap::ConstSharedPtr data_;
+
+  /// @brief Subscription handle for the costmap topic.
+  nav2::Subscription<nav2_msgs::msg::Costmap>::SharedPtr data_sub_;
+
+  /**
+   * @brief Minimum cost (0..255) considered as an obstacle.
+   * @note Typical values: 253 (inscribed), 254 (lethal). Inflation = 1..252.
+   */
+  int cost_threshold_{253};
+
+  /**
+   * @brief Whether cost 255 (NO_INFORMATION) is treated as an obstacle.
+   */
+  bool treat_unknown_as_obstacle_{true};
+};
+
+}  // namespace nav2_collision_monitor
+
+#endif  // NAV2_COLLISION_MONITOR__COSTMAP_HPP_
diff --git a/nav2_collision_monitor/launch/collision_detector_node.launch.py b/nav2_collision_monitor/launch/collision_detector_node.launch.py
index 43714ab3607..5355acce301 100644
--- a/nav2_collision_monitor/launch/collision_detector_node.launch.py
+++ b/nav2_collision_monitor/launch/collision_detector_node.launch.py
@@ -99,7 +99,8 @@ def generate_launch_description() -> LaunchDescription:
                 name='lifecycle_manager_collision_detector',
                 output='screen',
                 emulate_tty=True,  # https://github.com/ros2/launch/issues/188
-                parameters=[{'autostart': autostart}, {'node_names': lifecycle_nodes}],
+                parameters=[{'autostart': autostart},
+                            {'node_names': lifecycle_nodes}],
                 remappings=remappings,
             ),
             Node(
diff --git a/nav2_collision_monitor/launch/collision_monitor_node.launch.py b/nav2_collision_monitor/launch/collision_monitor_node.launch.py
index 5bad8f7b7d3..21d17d78496 100644
--- a/nav2_collision_monitor/launch/collision_monitor_node.launch.py
+++ b/nav2_collision_monitor/launch/collision_monitor_node.launch.py
@@ -11,7 +11,7 @@
 # Unless required by applicable law or agreed to in writing, software
 # distributed under the License is distributed on an "AS IS" BASIS,
 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
+# See the License for the specific languazge governing permissions and
 # limitations under the License.
 
 import os
diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml
index 6b009f1634b..956119364d5 100644
--- a/nav2_collision_monitor/package.xml
+++ b/nav2_collision_monitor/package.xml
@@ -31,6 +31,9 @@
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>
+  <test_depend>launch_testing_ament_cmake</test_depend>
+  <test_depend>rosgraph_msgs</test_depend>
+
 
   <export>
     <build_type>ament_cmake</build_type>
diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml
index 7287b69115e..f9f8b7c7efc 100644
--- a/nav2_collision_monitor/params/collision_monitor_params.yaml
+++ b/nav2_collision_monitor/params/collision_monitor_params.yaml
@@ -102,3 +102,9 @@ collision_monitor:
       max_height: 0.5
       use_global_height: False
       enabled: True
+    # costmap:
+    #   type: "costmap"
+    #   topic: "local_costmap/costmap"   # relative, respects namespaces
+    #   cost_threshold: 254
+    #   enabled: True
+    #   treat_unknown_as_obstacle: True
diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp
index 5d2cdc2f3a2..28a2e58bf02 100644
--- a/nav2_collision_monitor/src/collision_monitor_node.cpp
+++ b/nav2_collision_monitor/src/collision_monitor_node.cpp
@@ -1,16 +1,16 @@
-// Copyright (c) 2022 Samsung R&D Institute Russia
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
+  // Copyright (c) 2022 Samsung R&D Institute Russia
+  //
+  // Licensed under the Apache License, Version 2.0 (the "License");
+  // you may not use this file except in compliance with the License.
+  // You may obtain a copy of the License at
+  //
+  //     http://www.apache.org/licenses/LICENSE-2.0
+  //
+  // Unless required by applicable law or agreed to in writing, software
+  // distributed under the License is distributed on an "AS IS" BASIS,
+  // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  // See the License for the specific language governing permissions and
+  // limitations under the License.
 
 #include "nav2_collision_monitor/collision_monitor_node.hpp"
 
@@ -377,6 +377,14 @@ bool CollisionMonitor::configureSources(
         ps->configure();
 
         sources_.push_back(ps);
+      } else if (source_type == "costmap") {
+        auto src = std::make_shared<CostmapSource>(
+          node, source_name, tf_buffer_, base_frame_id, odom_frame_id,
+          transform_tolerance, source_timeout, base_shift_correction);
+
+        src->configure();
+
+        sources_.push_back(src);
       } else {  // Error if something else
         RCLCPP_ERROR(
           get_logger(),
@@ -667,7 +675,7 @@ void CollisionMonitor::toggleCMServiceCallback(
 
 #include "rclcpp_components/register_node_macro.hpp"
 
-// Register the component with class_loader.
-// This acts as a sort of entry point, allowing the component to be discoverable when its library
-// is being loaded into a running process.
+  // Register the component with class_loader.
+  // This acts as a sort of entry point, allowing the component to be discoverable when its library
+  // is being loaded into a running process.
 RCLCPP_COMPONENTS_REGISTER_NODE(nav2_collision_monitor::CollisionMonitor)
diff --git a/nav2_collision_monitor/src/costmap.cpp b/nav2_collision_monitor/src/costmap.cpp
new file mode 100644
index 00000000000..6eba5161053
--- /dev/null
+++ b/nav2_collision_monitor/src/costmap.cpp
@@ -0,0 +1,146 @@
+// Copyright (c) 2025 Angsa Robotics
+// Copyright (c) 2025 lotusymt
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#include "nav2_msgs/msg/costmap.hpp"
+#include "nav2_collision_monitor/costmap.hpp"
+#include <cmath>
+#include <tf2/time.hpp>
+#include <tf2_ros/buffer.hpp>
+#include <tf2_ros/transform_listener.hpp>
+#include <nav2_ros_common/node_utils.hpp>
+#include <nav2_ros_common/qos_profiles.hpp>
+#include <nav2_costmap_2d/cost_values.hpp>
+
+namespace nav2_collision_monitor
+{
+CostmapSource::CostmapSource(
+  const nav2::LifecycleNode::WeakPtr & node,
+  const std::string & source_name,
+  const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
+  const std::string & base_frame_id,
+  const std::string & global_frame_id,
+  const tf2::Duration & transform_tolerance,
+  const rclcpp::Duration & source_timeout,
+  const bool base_shift_correction)
+: Source(
+    node, source_name, tf_buffer, base_frame_id, global_frame_id,
+    transform_tolerance, source_timeout, base_shift_correction),
+  data_(nullptr)
+{
+  RCLCPP_INFO(logger_, "[%s]: Creating CostmapSource", source_name_.c_str());
+}
+
+CostmapSource::~CostmapSource()
+{
+  RCLCPP_INFO(logger_, "[%s]: Destroying CostmapSource", source_name_.c_str());
+  data_sub_.reset();
+}
+
+void CostmapSource::configure()
+{
+  Source::configure();
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+  std::string source_topic;
+  getParameters(source_topic);
+  data_sub_ = node->create_subscription<nav2_msgs::msg::Costmap>(
+      source_topic,
+     std::bind(&CostmapSource::dataCallback, this, std::placeholders::_1),
+     nav2::qos::StandardTopicQoS());
+}
+
+bool CostmapSource::getData(
+  const rclcpp::Time & curr_time,
+  std::vector<Point> & data)
+{
+  auto node = node_.lock();
+  if (data_ == nullptr) {
+    return false;
+  }
+
+  if (!sourceValid(data_->header.stamp, curr_time)) {
+    return false;
+  }
+  tf2::Transform tf_transform; tf_transform.setIdentity();
+  const std::string src = data_->header.frame_id;
+
+  // This branch is for malformed /local_costmap/costmap in tests or bags.
+  // It is not expected in the happy path, so we exclude it from coverage.
+  if (src != base_frame_id_) {
+    // LCOV_EXCL_START  <-- tell lcov/gcovr to ignore below
+    if (!getTransform(curr_time, data_->header, tf_transform)) {
+      RCLCPP_WARN(logger_, "[%s] TF %s->%s unavailable at t=%.3f",
+        source_name_.c_str(), src.c_str(), base_frame_id_.c_str(), curr_time.seconds());
+      return false;
+    }
+    // LCOV_EXCL_STOP
+  }
+
+  // Extract lethal/inscribed cells and transform to base frame
+  const auto & cm = *data_;
+  const auto & meta = cm.metadata;
+
+  for (unsigned int y = 0; y < meta.size_y; ++y) {
+    for (unsigned int x = 0; x < meta.size_x; ++x) {
+      const int idx = y * meta.size_x + x;
+
+      const uint8_t cell_cost = cm.data[idx];
+      const bool is_obstacle =
+        (cell_cost >= cost_threshold_ && cell_cost < nav2_costmap_2d::NO_INFORMATION) ||
+        (treat_unknown_as_obstacle_ && cell_cost == nav2_costmap_2d::NO_INFORMATION);
+      if (is_obstacle) {
+        const double wx = meta.origin.position.x + (x + 0.5) * meta.resolution;
+        const double wy = meta.origin.position.y + (y + 0.5) * meta.resolution;
+        tf2::Vector3 p_v3_s(wx, wy, 0.0);
+        tf2::Vector3 p_v3_b = tf_transform * p_v3_s;
+        data.push_back({p_v3_b.x(), p_v3_b.y()});
+        RCLCPP_DEBUG_THROTTLE(logger_, *node->get_clock(), 2000 /*ms*/,
+  "[%s] Found obstacles in costmap", source_name_.c_str());
+      }
+    }
+  }
+  return true;
+}
+
+void CostmapSource::getParameters(std::string & source_topic)
+{
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+  getCommonParameters(source_topic);
+
+  // Cost threshold (0–255). 253 = inscribed 254 = lethal; 255 = NO_INFORMATION.
+  const auto thresh_name = source_name_ + ".cost_threshold";
+  // Minimal change (no range descriptor)
+  int v = node->declare_or_get_parameter<int>(thresh_name, 253);  // declare if missing, else get
+  v = std::max(0, std::min(255, v));  // clamp
+  if (v != node->get_parameter(thresh_name).as_int()) {
+    RCLCPP_WARN(node->get_logger(), "Clamping %s to %d", thresh_name.c_str(), v);
+  }
+  cost_threshold_ = v;
+
+  // Whether 255 (NO_INFORMATION) should be treated as an obstacle.
+  const auto unk_name = source_name_ + ".treat_unknown_as_obstacle";
+  treat_unknown_as_obstacle_ = node->declare_or_get_parameter<bool>(unk_name, true);
+}
+
+void CostmapSource::dataCallback(nav2_msgs::msg::Costmap::ConstSharedPtr msg)
+{
+  data_ = msg;
+}
+
+}  // namespace nav2_collision_monitor
diff --git a/nav2_collision_monitor/test/CMakeLists.txt b/nav2_collision_monitor/test/CMakeLists.txt
index e2b668b517e..22321bdc926 100644
--- a/nav2_collision_monitor/test/CMakeLists.txt
+++ b/nav2_collision_monitor/test/CMakeLists.txt
@@ -58,3 +58,30 @@ target_link_libraries(collision_detector_node_test
   tf2_ros::tf2_ros
   ${visualization_msgs_TARGETS}
 )
+
+
+# Install bag + params so the launch file can find them via get_package_share_directory()
+install(DIRECTORY bags/cm_moving_obstacle
+  DESTINATION share/${PROJECT_NAME}/test/bags)
+install(FILES collision_monitor_node_bag.yaml
+  DESTINATION share/${PROJECT_NAME}/test)
+
+find_package(GTest REQUIRED)
+
+add_executable(collision_monitor_node_bag_gtest
+  collision_monitor_node_bag.cpp
+)
+target_link_libraries(collision_monitor_node_bag_gtest
+  GTest::gtest_main
+  ${monitor_library_name}
+  rclcpp::rclcpp
+  tf2_ros::tf2_ros
+  nav2_util::nav2_util_core
+  ${nav2_msgs_TARGETS}
+)
+
+add_launch_test(collision_monitor_node_bag.launch.py
+  TIMEOUT 120
+  WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+  ENV TEST_EXECUTABLE=$<TARGET_FILE:collision_monitor_node_bag_gtest>
+)
diff --git a/nav2_collision_monitor/test/bags/cm_moving_obstacle/_tmp_cm_bag_0.mcap.zstd b/nav2_collision_monitor/test/bags/cm_moving_obstacle/_tmp_cm_bag_0.mcap.zstd
new file mode 100644
index 00000000000..846b3e8ee69
Binary files /dev/null and b/nav2_collision_monitor/test/bags/cm_moving_obstacle/_tmp_cm_bag_0.mcap.zstd differ
diff --git a/nav2_collision_monitor/test/bags/cm_moving_obstacle/fake_cm_bag_source.py b/nav2_collision_monitor/test/bags/cm_moving_obstacle/fake_cm_bag_source.py
new file mode 100644
index 00000000000..fe979c589cb
--- /dev/null
+++ b/nav2_collision_monitor/test/bags/cm_moving_obstacle/fake_cm_bag_source.py
@@ -0,0 +1,117 @@
+# Copyright (c) 2025 lotusymt
+
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+
+#     http://www.apache.org/licenses/LICENSE-2.0
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+# AI-assisted: initial draft generated by an AI tool; fully reviewed and edited by the author.
+
+import time
+
+from geometry_msgs.msg import Point, Pose, Quaternion, Twist
+from nav2_msgs.msg import Costmap, CostmapMetaData
+import numpy as np
+import rclpy
+from rclpy.node import Node
+from rosgraph_msgs.msg import Clock as ClockMsg
+from std_msgs.msg import Header
+
+# --- standardized imports ---
+# This node is meant to *mimic* what a recorded bag would provide:
+# - /clock: so the rest of the system can run in sim time
+# - /cmd_vel_smoothed: so there is a "robot is trying to move" signal
+# - /local_costmap/costmap: so Collision Monitor sees an obstacle only in a time window
+#
+# Test intent:
+#   0..3s   → no obstacle → robot should keep going
+#   3..8s   → obstacle right under robot → CM should stop it
+#   8s..+   → obstacle gone → CM should let it move again
+# This must match the YAML used by the real CM test.
+
+
+class FakeCMSource(Node):
+
+    def __init__(self) -> None:
+        super().__init__('fake_cm_bag_source')
+
+        # Publishers
+        self.clock_pub = self.create_publisher(ClockMsg, '/clock', 10)
+        self.costmap_pub = self.create_publisher(
+            Costmap, '/local_costmap/costmap', 10)
+        self.cmd_pub = self.create_publisher(Twist, '/cmd_vel_smoothed', 10)
+
+        # Simple 5m x 5m costmap centered on robot, 0.05 m resolution (100x100)
+        self.res = 0.05
+        self.size_x = 100
+        self.size_y = 100
+        self.origin = Pose(
+            position=Point(x=-2.5, y=-2.5, z=0.0),
+            orientation=Quaternion(w=1.0)
+        )
+
+        self.t0_ns = time.monotonic_ns()
+
+        self.timer = self.create_timer(0.05, self.tick)  # 20 Hz
+
+    def tick(self) -> None:
+        # 1) /clock
+        # in tick()
+        now_ns = time.monotonic_ns() - self.t0_ns
+        now_s = now_ns / 1e9  # convert to seconds
+        clk = ClockMsg()
+        clk.clock.sec = now_ns // 1_000_000_000
+        clk.clock.nanosec = now_ns % 1_000_000_000
+        self.clock_pub.publish(clk)
+
+        # 2) /cmd_vel_smoothed: small forward velocity
+        t = Twist()
+        t.linear.x = 0.20
+        self.cmd_pub.publish(t)
+
+        # 3) /local_costmap/costmap
+        #    0..3s SAFE → 3..8s DANGER (lethal cell at robot) → 8s+ SAFE (recover)
+        cm = Costmap()
+        cm.header = Header()
+        cm.header.stamp = clk.clock
+        cm.header.frame_id = 'base_footprint'  # match YAML base_frame_id
+
+        meta = CostmapMetaData()
+        meta.map_load_time = clk.clock
+        meta.resolution = float(self.res)
+        meta.size_x = self.size_x
+        meta.size_y = self.size_y
+        meta.origin = self.origin
+        cm.metadata = meta
+
+        # Start with all-free costmap
+        data = np.zeros((self.size_y, self.size_x), dtype=np.uint8)
+
+        if 3.0 <= now_s <= 8.0:
+            # During the "danger window", make the cell under the robot lethal.
+            # This is what should make CM stop the robot.
+            cx = self.size_x // 2
+            cy = self.size_y // 2
+            data[cy, cx] = 254  # lethal
+
+        cm.data = data.flatten().tolist()
+        self.costmap_pub.publish(cm)
+
+
+def main(args: list[str] | None = None) -> None:
+    rclpy.init()
+    node = FakeCMSource()
+    try:
+        rclpy.spin(node)
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
diff --git a/nav2_collision_monitor/test/bags/cm_moving_obstacle/metadata.yaml b/nav2_collision_monitor/test/bags/cm_moving_obstacle/metadata.yaml
new file mode 100644
index 00000000000..d75e8032582
--- /dev/null
+++ b/nav2_collision_monitor/test/bags/cm_moving_obstacle/metadata.yaml
@@ -0,0 +1,88 @@
+rosbag2_bagfile_information:
+  version: 9
+  storage_identifier: mcap
+  duration:
+    nanoseconds: 11500531887
+  starting_time:
+    nanoseconds_since_epoch: 1760908698048734123
+  message_count: 693
+  topics_with_message_count:
+    - topic_metadata:
+        name: /local_costmap/costmap
+        type: nav2_msgs/msg/Costmap
+        serialization_format: cdr
+        offered_qos_profiles:
+          - history: unknown
+            depth: 0
+            reliability: reliable
+            durability: volatile
+            deadline:
+              sec: 9223372036
+              nsec: 854775807
+            lifespan:
+              sec: 9223372036
+              nsec: 854775807
+            liveliness: automatic
+            liveliness_lease_duration:
+              sec: 9223372036
+              nsec: 854775807
+            avoid_ros_namespace_conventions: false
+        type_description_hash: RIHS01_d51bf7aecafdbb3f8f98215de648abe6d53f8db69156d6e143125d2ad2bf0027
+      message_count: 231
+    - topic_metadata:
+        name: /cmd_vel_smoothed
+        type: geometry_msgs/msg/Twist
+        serialization_format: cdr
+        offered_qos_profiles:
+          - history: unknown
+            depth: 0
+            reliability: reliable
+            durability: volatile
+            deadline:
+              sec: 9223372036
+              nsec: 854775807
+            lifespan:
+              sec: 9223372036
+              nsec: 854775807
+            liveliness: automatic
+            liveliness_lease_duration:
+              sec: 9223372036
+              nsec: 854775807
+            avoid_ros_namespace_conventions: false
+        type_description_hash: RIHS01_9c45bf16fe0983d80e3cfe750d6835843d265a9a6c46bd2e609fcddde6fb8d2a
+      message_count: 231
+    - topic_metadata:
+        name: /clock
+        type: rosgraph_msgs/msg/Clock
+        serialization_format: cdr
+        offered_qos_profiles:
+          - history: unknown
+            depth: 0
+            reliability: reliable
+            durability: volatile
+            deadline:
+              sec: 9223372036
+              nsec: 854775807
+            lifespan:
+              sec: 9223372036
+              nsec: 854775807
+            liveliness: automatic
+            liveliness_lease_duration:
+              sec: 9223372036
+              nsec: 854775807
+            avoid_ros_namespace_conventions: false
+        type_description_hash: RIHS01_692f7a66e93a3c83e71765d033b60349ba68023a8c689a79e48078bcb5c58564
+      message_count: 231
+  compression_format: zstd
+  compression_mode: FILE
+  relative_file_paths:
+    - _tmp_cm_bag_0.mcap.zstd
+  files:
+    - path: _tmp_cm_bag_0.mcap
+      starting_time:
+        nanoseconds_since_epoch: 1760908698048734123
+      duration:
+        nanoseconds: 11500531887
+      message_count: 693
+  custom_data: ~
+  ros_distro: rolling
diff --git a/nav2_collision_monitor/test/collision_monitor_node_bag.cpp b/nav2_collision_monitor/test/collision_monitor_node_bag.cpp
new file mode 100644
index 00000000000..8697948b9e2
--- /dev/null
+++ b/nav2_collision_monitor/test/collision_monitor_node_bag.cpp
@@ -0,0 +1,258 @@
+// Copyright (c) 2025 lotusymt
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// You may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// AI-assisted: initial draft generated by an AI tool; fully reviewed and edited by the author.
+//
+// @brief Dataset-based test for Collision Monitor. Replays a tiny bag and checks:
+//  (1) time-to-stop, (2) hold-stop%, (3) time-to-resume, (4) false-stop% outside window.
+// In other words: this file is the *logic* that decides “pass/fail” based on the topics.
+
+#include <gtest/gtest.h>
+#include <algorithm>
+#include <cmath>
+#include <chrono>
+#include <thread>
+#include <limits>
+#include "rclcpp/rclcpp.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "nav2_msgs/msg/collision_monitor_state.hpp"
+#include "nav2_msgs/msg/costmap.hpp"
+#include "rosgraph_msgs/msg/clock.hpp"
+
+using nav2_msgs::msg::CollisionMonitorState;
+
+// Single sample of what we saw on /cmd_vel at a certain sim time
+struct Sample
+{
+  double t;          // sim time (seconds)
+  double vx;         // cmd_vel.linear.x (what robot actually commanded)
+  int action;        // last known CM action (for debugging only)
+};
+
+class MetricsCatcher : public rclcpp::Node {
+public:
+  MetricsCatcher()
+  : rclcpp::Node("cm_metrics_catcher"),
+    got_clock_(false), got_costmap_(false),
+    last_action_(0)
+  {
+    // We expect /clock from the bag → run in sim time
+    this->set_parameter(rclcpp::Parameter("use_sim_time", true));
+
+    // Window where the bag has the obstacle (must match fake/bag)
+    stop_window_start_ = this->declare_parameter<double>("stop_window_start", 3.0);
+    stop_window_end_ = this->declare_parameter<double>("stop_window_end", 8.0);
+
+    // How we interpret velocities on /cmd_vel:
+    // - <= stop_thresh_ → we say "stopped"
+    // - >= resume_thresh_ → we say "moving again"
+    // We also debounce to avoid flapping.
+    stop_thresh_ = this->declare_parameter<double>("stop_thresh", 0.02);             // |vx| <= stop
+    resume_thresh_ = this->declare_parameter<double>(
+      "resume_thresh", 0.10);  //  vx  >= resume
+
+    debounce_n_ = this->declare_parameter<int>(
+      "debounce_n", 3);  // need K in arow
+
+    // Tolerances for this test. If CM is slower/worse → test fails.
+    max_time_to_stop_ = this->declare_parameter<double>("max_time_to_stop", 0.6);
+    min_hold_pct_ = this->declare_parameter<double>("min_hold_pct", 0.90);
+    max_time_to_resume_ = this->declare_parameter<double>("max_time_to_resume", 0.6);
+    max_false_stop_pct_ = this->declare_parameter<double>("max_false_stop_pct", 0.05);
+
+    // We only start collecting after we have seen *both* /clock and /local_costmap
+    // This avoids counting "startup zero cmd" as real data.
+    cm_sub_ = this->create_subscription<nav2_msgs::msg::Costmap>(
+      "/local_costmap/costmap", rclcpp::QoS(1).reliable().durability_volatile(),
+      [this](const nav2_msgs::msg::Costmap &){
+        if (!got_costmap_) {got_costmap_ = true; cm_sub_.reset();}
+      });
+
+    clock_sub_ = this->create_subscription<rosgraph_msgs::msg::Clock>(
+      "/clock", rclcpp::QoS(1).best_effort().durability_volatile(),
+      [this](const rosgraph_msgs::msg::Clock &){
+        if (!got_clock_) {got_clock_ = true; clock_sub_.reset();}
+      });
+
+    // This is the *output* we evaluate. In the launch file you can remap it.
+    cmd_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
+      "/cmd_vel", rclcpp::QoS(60),
+      [this](const geometry_msgs::msg::Twist & msg){
+        if (!got_clock_ || !got_costmap_) {
+          // don't collect before system is “live”
+          return;
+        }
+        const double t = this->now().seconds();
+        samples_.push_back(Sample{t, msg.linear.x, last_action_});
+      });
+
+    // Optional: subscribe to CM state to help debugging (not used in asserts)
+    state_sub_ = this->create_subscription<CollisionMonitorState>(
+      "/collision_state", rclcpp::QoS(10),
+      [this](const CollisionMonitorState & msg){
+        last_action_ = msg.action_type;
+      });
+  }
+
+  // Spin until we passed the obstacle window (8s) + margin
+  void run_and_collect(rclcpp::executors::SingleThreadedExecutor & exec, double margin_s = 2.0)
+  {
+    // First, up to 5s WALL-time to see /clock
+    auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(5);
+    while (rclcpp::ok() && !got_clock_) {
+      exec.spin_some();
+      if (std::chrono::steady_clock::now() > deadline) {break;}
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+
+    // Then collect data until the obstacle window ends + margin
+    // or 30s WALL-time (safety for CI)
+    auto wall_deadline = std::chrono::steady_clock::now() + std::chrono::seconds(30);
+    while (rclcpp::ok()) {
+      exec.spin_some();
+      if (this->now().seconds() >= stop_window_end_ + margin_s) {break;}
+      if (std::chrono::steady_clock::now() > wall_deadline) {break;}
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+  }
+
+  // ----- Metrics we keep -----
+  struct Results
+  {
+    double time_to_stop_s;   // how long after 3.0s did CM actually stop us
+    double hold_stop_pct;    // inside 3.0..8.0s, how many samples were "stopped"
+    double time_to_resume_s;  // after 8.0s, how long until CM let us move again
+    double false_stop_pct;   // outside 3.0..8.0s, how often were we wrongly stopped
+    bool   have_data;
+  };
+
+  Results compute() const
+  {
+    Results R{};
+    R.have_data = !samples_.empty();
+
+    if (!R.have_data) {return R;}
+
+    // helper: pick samples in a time range
+    auto in_range = [&](double t0, double t1){
+        std::vector<Sample> out; out.reserve(samples_.size());
+        for (const auto & s : samples_) {if (s.t >= t0 && s.t <= t1) {out.push_back(s);}}
+        return out;
+      };
+
+    // Segments:
+    // 0..(3 - 0.2) → must be clean (no stop)  (guard band)
+    const auto stop_seg = in_range(stop_window_start_, stop_window_end_);
+    const auto pre_seg = in_range(0.0, std::max(0.0, stop_window_start_ - 0.2));
+    const auto post_seg = in_range(stop_window_end_ + 0.2, samples_.back().t);
+
+    // helper: find first time we saw K consecutive samples matching a predicate
+    auto first_transition_time = [&](const std::vector<Sample> & seg,
+      bool to_stop, double thr, int k)->double {
+        int run = 0;
+        for (size_t i = 0; i < seg.size(); ++i) {
+          const bool ok = to_stop ? (std::fabs(seg[i].vx) <= thr) : (seg[i].vx >= thr);
+          run = ok ? (run + 1) : 0;
+          if (run >= k) {return seg[i].t;}
+        }
+        return std::numeric_limits<double>::quiet_NaN();
+      };
+
+    // 1) time-to-stop: should be small if CM reacted
+    const double t_stop_first = first_transition_time(stop_seg, /*to_stop=*/true, stop_thresh_,
+      debounce_n_);
+    R.time_to_stop_s = std::isnan(t_stop_first) ? std::numeric_limits<double>::infinity() :
+      (t_stop_first - stop_window_start_);
+
+    // 2) hold-stop%: inside the obstacle window, how many samples are “stopped”
+    if (!stop_seg.empty()) {
+      size_t cnt = 0;
+      for (const auto & s : stop_seg) {if (std::fabs(s.vx) <= stop_thresh_) {++cnt;}}
+      R.hold_stop_pct = static_cast<double>(cnt) / static_cast<double>(stop_seg.size());
+    } else {
+      R.hold_stop_pct = 0.0;
+    }
+
+    // 3) time-to-resume: after the obstacle goes away, how fast do we go again
+    const double t_resume_first = first_transition_time(post_seg, /*to_stop=*/false, resume_thresh_,
+      debounce_n_);
+    R.time_to_resume_s = std::isnan(t_resume_first) ? std::numeric_limits<double>::infinity() :
+      (t_resume_first - stop_window_end_);
+
+    // 4) false-stop%: outside the window we should mostly be moving
+    const size_t clean_total = pre_seg.size() + post_seg.size();
+    if (clean_total > 0) {
+      auto count_stopped = [&](const std::vector<Sample> & seg){
+          return std::count_if(seg.begin(), seg.end(),
+                   [&](const Sample & s){return std::fabs(s.vx) <= stop_thresh_;});
+        };
+      const size_t clean_stopped = count_stopped(pre_seg) + count_stopped(post_seg);
+      R.false_stop_pct = static_cast<double>(clean_stopped) / static_cast<double>(clean_total);
+    } else {
+      R.false_stop_pct = 0.0;
+    }
+
+    return R;
+  }
+
+  void assert_results(const Results & R)
+  {
+    ASSERT_TRUE(R.have_data) << "No /cmd_vel samples collected";
+
+    EXPECT_LE(R.time_to_stop_s, max_time_to_stop_) << "time-to-stop too large";
+    EXPECT_GE(R.hold_stop_pct, min_hold_pct_) << "hold-stop% too low";
+    EXPECT_LE(R.time_to_resume_s, max_time_to_resume_) << "time-to-resume too large";
+    EXPECT_LE(R.false_stop_pct, max_false_stop_pct_) << "false-stop% too high";
+
+    RCLCPP_INFO(this->get_logger(),
+      "Results: t_stop=%.3fs, hold=%.1f%%, t_resume=%.3fs, false=%.1f%%",
+      R.time_to_stop_s, R.hold_stop_pct * 100.0, R.time_to_resume_s, R.false_stop_pct * 100.0);
+  }
+
+private:
+  // Subscriptions
+  rclcpp::Subscription<nav2_msgs::msg::Costmap>::SharedPtr cm_sub_;
+  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_sub_;
+  rclcpp::Subscription<CollisionMonitorState>::SharedPtr state_sub_;
+  rclcpp::Subscription<rosgraph_msgs::msg::Clock>::SharedPtr clock_sub_;
+
+  // Buffers/state
+  std::vector<Sample> samples_;
+  bool got_clock_, got_costmap_;
+  int  last_action_;
+
+  // Params / thresholds
+  double stop_window_start_, stop_window_end_;
+  double stop_thresh_, resume_thresh_, max_time_to_stop_, min_hold_pct_;
+  double max_time_to_resume_, max_false_stop_pct_;
+  int    debounce_n_;
+};
+
+TEST(CollisionMonitorNodeBag, TrajectoryAndMetrics)
+{
+  rclcpp::init(0, nullptr);
+  auto node = std::make_shared<MetricsCatcher>();
+
+  rclcpp::executors::SingleThreadedExecutor exec;
+  exec.add_node(node);
+
+  // Run until we have enough simulated time
+  node->run_and_collect(exec, /*margin_s=*/2.0);
+
+  // Compute metrics from collected /cmd_vel
+  auto R = node->compute();
+  node->assert_results(R);
+
+  rclcpp::shutdown();
+}
diff --git a/nav2_collision_monitor/test/collision_monitor_node_bag.launch.py b/nav2_collision_monitor/test/collision_monitor_node_bag.launch.py
new file mode 100644
index 00000000000..0c7c601141f
--- /dev/null
+++ b/nav2_collision_monitor/test/collision_monitor_node_bag.launch.py
@@ -0,0 +1,126 @@
+# Copyright (c) 2025 lotusymt
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+# AI-assisted: initial draft generated by an AI tool; fully reviewed and edited by the author.
+
+import os
+from typing import Any, cast
+import unittest
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess, SetEnvironmentVariable
+from launch_ros.actions import Node
+import launch_testing
+from launch_testing.actions import ReadyToTest
+
+
+def generate_test_description() -> tuple[LaunchDescription, dict[str, Any]]:
+    # Where our package keeps test assets (bags + YAML)
+    pkg_share = get_package_share_directory('nav2_collision_monitor')
+
+    # This is the *real* rosbag we want to replay in the test
+    bag_dir = os.path.join(pkg_share, 'test', 'bags', 'cm_moving_obstacle')
+
+    # CM node parameters (must match the topics from the bag)
+    params_yaml = os.path.join(
+        pkg_share, 'test', 'collision_monitor_node_bag.yaml')
+
+    # The actual gtest binary is passed in via env
+    # so colcon/ctest can control which binary to run.
+    test_exe = os.environ.get('TEST_EXECUTABLE')
+    if not test_exe:
+        # Fail early if we forgot to set it in CMake/CTest
+        raise RuntimeError('TEST_EXECUTABLE env var not set')
+
+    # Where to drop the gtest XML (so CI can read it)
+    results_dir = os.environ.get('AMENT_TEST_RESULTS_DIR', '/tmp')
+    xml_dir = os.path.join(results_dir, 'nav2_collision_monitor')
+    os.makedirs(xml_dir, exist_ok=True)
+    gtest_xml = os.path.join(
+        xml_dir, 'collision_monitor_node_bag_gtest.gtest.xml')
+
+    # Some tests set ROSCONSOLE_CONFIG_FILE → clear it to make logs visible
+    clear_rosconsole = SetEnvironmentVariable(
+        name='ROSCONSOLE_CONFIG_FILE', value='')
+
+    # Bring up lifecycle manager so CM can transition automatically
+    lifecycle_mgr = Node(
+        package='nav2_lifecycle_manager',
+        executable='lifecycle_manager',
+        name='lifecycle_manager_collision_monitor',
+        output='screen',
+        parameters=[
+            {'use_sim_time': True},     # we are using /clock from bag
+            {'autostart': True},        # immediately activate CM
+            {'bond_timeout': 0.0},      # do not hang test on bond
+            {'node_names': ['collision_monitor']},
+        ],
+    )
+
+    # The node under test
+    collision_monitor = Node(
+        package='nav2_collision_monitor',
+        executable='collision_monitor',
+        name='collision_monitor',
+        output='screen',
+        parameters=[params_yaml, {'use_sim_time': True}],
+    )
+
+    # Play the bag that has: /clock, /cmd_vel (or /cmd_vel_smoothed), and costmap
+    bag_play = ExecuteProcess(
+        cmd=['ros2', 'bag', 'play', bag_dir, '--rate', '1.0',
+             '--read-ahead-queue-size', '1000'],
+        output='screen',
+    )
+
+    # Run the *C++* test that actually measures “did CM stop fast enough?”
+    # This runs in the same process space as launch_testing → we can assert on it later.
+    cm_gtest = ExecuteProcess(
+        cmd=[test_exe, f'--gtest_output=xml:{gtest_xml}'],
+        output='screen',
+    )
+
+    # We return everything as a single launch description.
+    # Important: ReadyToTest() tells launch_testing “ok, start the Python assertions”.
+    ld = LaunchDescription([
+        clear_rosconsole,
+        lifecycle_mgr,
+        collision_monitor,
+        bag_play,
+        cm_gtest,
+        ReadyToTest(),  # type: ignore[no-untyped-call]
+    ])
+    # The dict here exposes cm_gtest to the test class below
+    return ld, {'cm_gtest': cm_gtest}
+
+
+class TestWaitForGTest(unittest.TestCase):
+    # This part just waits until the gtest process finished (or times out)
+    def test_gtest_completed(self, proc_info: Any, cm_gtest: Any) -> None:
+        # 120s is generous for slow CI
+        proc_info.assertWaitForShutdown(process=cm_gtest, timeout=120.0)
+
+
+# make the decorator explicitly untyped for mypy
+post_shutdown_test = cast(Any, launch_testing.post_shutdown_test)
+
+
+@post_shutdown_test()
+class TestGTestExitCode(unittest.TestCase):
+    # And this part says: the gtest must have *passed*
+    def test_gtest_passed(self, proc_info: Any, cm_gtest: Any) -> None:
+        from launch_testing.asserts import assertExitCodes
+
+        # assertExitCodes is also untyped, tell mypy that we know it
+        assert_exit_codes = cast(Any, assertExitCodes)
+        assert_exit_codes(proc_info, process=cm_gtest)
diff --git a/nav2_collision_monitor/test/collision_monitor_node_bag.yaml b/nav2_collision_monitor/test/collision_monitor_node_bag.yaml
new file mode 100644
index 00000000000..3707944d306
--- /dev/null
+++ b/nav2_collision_monitor/test/collision_monitor_node_bag.yaml
@@ -0,0 +1,25 @@
+# collision_monitor_node_bag.yaml
+collision_monitor:
+  ros__parameters:
+    use_sim_time: true
+    base_frame_id: base_footprint
+    odom_frame_id: odom
+    state_topic: collision_state
+    transform_tolerance: 0.05
+    source_timeout: 1.0
+    enable_stamped_cmd_vel: false
+
+
+    observation_sources: ["costmap"]
+    costmap:
+      type: "costmap"
+      topic: "/local_costmap/costmap"
+      cost_threshold: 253
+      enabled: True
+
+    polygons: [Detection]
+    Detection:
+      type: circle
+      action_type: stop
+      min_points: 1
+      radius: 1.0
diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp
index 1710fbc1d90..d2213095631 100644
--- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp
+++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp
@@ -945,6 +945,7 @@ TEST_F(Tester, testPolygonSource)
   sendTransforms(curr_time);
 
   // 1. Obstacle is far away from robot
+  curr_time = cm_->now();
   publishPolygon(4.5, curr_time);
   ASSERT_TRUE(waitData(std::hypot(1.0, 4.5), 500ms, curr_time));
   publishCmdVel(0.5, 0.2, 0.1);
@@ -954,6 +955,7 @@ TEST_F(Tester, testPolygonSource)
   ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1, EPSILON);
 
   // 2. Obstacle is in limit robot zone
+  curr_time = cm_->now();
   publishPolygon(3.0, curr_time);
   EXPECT_TRUE(waitData(std::hypot(1.0, 3.0), 500ms, curr_time));
   publishCmdVel(0.5, 0.2, 0.1);
@@ -970,6 +972,7 @@ TEST_F(Tester, testPolygonSource)
   EXPECT_EQ(action_state_->polygon_name, "Limit");
 
   // 3. Obstacle is in slowdown robot zone
+  curr_time = cm_->now();
   publishPolygon(1.5, curr_time);
   EXPECT_TRUE(waitData(std::hypot(1.0, 1.5), 500ms, curr_time));
   publishCmdVel(0.5, 0.2, 0.1);
@@ -995,6 +998,7 @@ TEST_F(Tester, testPolygonSource)
   EXPECT_EQ(action_state_->polygon_name, "Stop");
 
   // 5. Restoring back normal operation
+  curr_time = cm_->now();
   publishPolygon(4.5, curr_time);
   ASSERT_TRUE(waitData(std::hypot(1.0, 4.5), 500ms, curr_time));
   publishCmdVel(0.5, 0.2, 0.1);