[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_detector_node.hpp

@@ -39,6 +39,7 @@
 #include "nav2_collision_monitor/pointcloud.hpp"
 #include "nav2_collision_monitor/range.hpp"
 #include "nav2_collision_monitor/polygon_source.hpp"
+#include "nav2_collision_monitor/costmap.hpp"
 
 namespace nav2_collision_monitor
 {

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/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(

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

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>

nav2_collision_monitor/params/collision_detector_params.yaml

@@ -27,3 +27,9 @@ collision_detector:
       min_height: 0.1
       max_height: 0.5
       enabled: True
+    # costmap:
+    #   type: "costmap"
+    #   topic: "local_costmap/costmap"
+    #   cost_threshold: 254
+    #   treat_unknown_as_obstacle: True
+    #   enabled: True

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"
+    #   cost_threshold: 254
+    #   enabled: True
+    #   treat_unknown_as_obstacle: True

nav2_collision_monitor/src/collision_detector_node.cpp

@@ -277,6 +277,14 @@ bool CollisionDetector::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(),

nav2_collision_monitor/src/collision_monitor_node.cpp

@@ -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(),