[Draft] CollisionMonitor: add CostmapSource + dataset-based bag test

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

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
 

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

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_

nav2_collision_monitor/package.xml

@@ -31,7 +31,10 @@
   <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>
   </export>

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

nav2_collision_monitor/src/collision_monitor_node.cpp

@@ -377,7 +377,15 @@ bool CollisionMonitor::configureSources(
         ps->configure();
 
         sources_.push_back(ps);
-      } else {  // Error if something else
+      } 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(),
           "[%s]: Unknown source type: %s",

nav2_collision_monitor/src/costmap.cpp

@@ -0,0 +1,141 @@
+// 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;
+  if (src != base_frame_id_) {
+    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;
+    }
+  }
+
+  // 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;
+}
+
+}

nav2_collision_monitor/test/CMakeLists.txt

@@ -58,3 +58,31 @@ 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>
+)
+