nav2_collision_monitor: modified CostmapSource + dataset-based bag test (v1)

Signed-off-by: Mengting Yang <[email protected]>

Author: Lotusymt

nav2_collision_monitor/CMakeLists.txt

@@ -8,6 +8,7 @@ find_package(nav2_common REQUIRED)
 find_package(nav2_costmap_2d REQUIRED)
 find_package(nav2_msgs REQUIRED)
 find_package(nav2_util REQUIRED)
+find_package(rosgraph_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
@@ -148,6 +149,7 @@ install(DIRECTORY params DESTINATION share/${PROJECT_NAME})
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
+  find_package(launch_testing_ament_cmake REQUIRED)
   # the following line skips the linter which checks for copyrights
   set(ament_cmake_copyright_FOUND TRUE)
   set(ament_cmake_cpplint_FOUND TRUE)
@@ -167,6 +169,7 @@ ament_export_dependencies(
   nav2_costmap_2d
   nav2_msgs
   nav2_util
+  rosgraph_msgs
   rclcpp
   rclcpp_lifecycle
   sensor_msgs

nav2_collision_monitor/include/nav2_collision_monitor/costmap.hpp

@@ -1,55 +1,70 @@
-// Copyright (c) 2025 <Your Name>
+// 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_
 
-#include <memory>                   // shared_ptr / weak_ptr
-#include <string>                   // std::string
-#include <vector>                   // std::vector
-#include "nav2_collision_monitor/source.hpp"        // Base class Source (CM’s plugin interface)
-#include "nav2_msgs/msg/costmap.hpp"                // CHANGE: use Nav2 Costmap message
+#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/node_utils.hpp>
 
 namespace nav2_collision_monitor
 {
 
-class CostmapSource : public Source                  // Inherit CM’s Source (same as other inputs)
+class CostmapSource : public Source
 {
 public:
-  CostmapSource(                                     // Constructor declaration (no return type)
-    const nav2::LifecycleNode::WeakPtr & node ,  // Non-owning pointer to lifecycle node
-    const std::string & source_name,                 // Name used in params (e.g., "costmap")
-    const std::shared_ptr<tf2_ros::Buffer> tf_buffer,// Shared TF buffer
-    const std::string & base_frame_id,               // Usually "base_link"
-    const std::string & global_frame_id,             // Usually "odom" or "map"
-    const tf2::Duration & transform_tolerance,       // TF timing slack
-    const rclcpp::Duration & source_timeout,         // Max data age allowed
-    const bool base_shift_correction);               // Whether to correct base motion drift
+  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);
 
-  ~CostmapSource();                                  // Virtual dtor not required; explicit is fine
+  ~CostmapSource();
 
-  void configure();                         // Where we declare params & create subscription
+  void configure();
 
-  bool getData(                                      // CM calls this each tick to fetch obstacle points
+  bool getData(
     const rclcpp::Time & curr_time,
     std::vector<Point> & data) override;
 
-  void getParameters(std::string & source_topic);    // Read: <source_name>.topic, <source_name>.cost_threshold
+  void getParameters(std::string & source_topic);
 
 private:
   void dataCallback(nav2_msgs::msg::Costmap::ConstSharedPtr msg);
   // ↑ Store the latest Costmap message; we’ll read it in getData()
 
   nav2_msgs::msg::Costmap::ConstSharedPtr data_;     // Latest costmap message (thread-safe shared ptr)
   rclcpp::Subscription<nav2_msgs::msg::Costmap>::SharedPtr data_sub_;
-  // ↑ The actual ROS2 subscription
 
-  int cost_threshold_{254};                          // Default: lethal cells only (254). Make it a param.
-  // (Nav2 costs: 0 free, 253 inscribed, 254 lethal, 255 unknown)
+  // Threshold for considering a cell as an obstacle. Valid range: 0..255.
+  // Typical choices: 253 (inscribed), 254 (lethal). Inflation = 1..252.
+  int cost_threshold_{253};
+
+   // Whether 255 (NO_INFORMATION) should be treated as an obstacle.
+  bool treat_unknown_as_obstacle_{true};
+
 
-  
 };
 
 }  // namespace nav2_collision_monitor

nav2_collision_monitor/package.xml

@@ -32,6 +32,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_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_monitor_node.cpp

@@ -308,7 +308,7 @@ bool CollisionMonitor::configurePolygons(
         polygons_.push_back(
           std::make_shared<VelocityPolygon>(
             node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance));
-      }else {  // Error if something else
+      } else {  // Error if something else
         RCLCPP_ERROR(
           get_logger(),
           "[%s]: Unknown polygon type: %s",
@@ -381,10 +381,11 @@ bool CollisionMonitor::configureSources(
         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);
-        continue;
-      }  
-      else {  // Error if something else
+      } else { // Error if something else
         RCLCPP_ERROR(
           get_logger(),
           "[%s]: Unknown source type: %s",

nav2_collision_monitor/src/costmap.cpp

@@ -1,19 +1,32 @@
-#include "nav2_msgs/msg/costmap.hpp"             
-#include "nav2_collision_monitor/costmap.hpp"   
+// 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 <functional>
 #include <cmath>
-#include <tf2/time.hpp>                     // tf2::Duration, tf2::TimePoint, durationFromSec()
-#include <tf2_ros/buffer.hpp>               // tf2_ros::Buffer (you already use it)
+#include <tf2/time.hpp>
+#include <tf2_ros/buffer.hpp>
 #include <tf2_ros/transform_listener.hpp>
-#include <nav2_ros_common/lifecycle_node.hpp>  // nav2::LifecycleNode
+#include <nav2_ros_common/lifecycle_node.hpp>
 #include <nav2_ros_common/node_utils.hpp>
 
 namespace nav2_collision_monitor
 {
 
 
-CostmapSource::CostmapSource(                     
-  // const nav2_util::LifecycleNode::WeakPtr & node,
+CostmapSource::CostmapSource(
   const nav2::LifecycleNode::WeakPtr & node,
   const std::string & source_name,
   const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
@@ -27,17 +40,17 @@ CostmapSource::CostmapSource(
     transform_tolerance, source_timeout, base_shift_correction),
   data_(nullptr)
 {
-  RCLCPP_INFO(logger_, "[%s]: Creating CostmapSource", source_name_.c_str());  
+  RCLCPP_INFO(logger_, "[%s]: Creating CostmapSource", source_name_.c_str());
 }
 
 
-CostmapSource::~CostmapSource()                                                         
+CostmapSource::~CostmapSource()
 {
-  RCLCPP_INFO(logger_, "[%s]: Destroying CostmapSource", source_name_.c_str());       
+  RCLCPP_INFO(logger_, "[%s]: Destroying CostmapSource", source_name_.c_str());
   data_sub_.reset();
 }
 
-void CostmapSource::configure()                                                         
+void CostmapSource::configure()
 {
   Source::configure();
   auto node = node_.lock();
@@ -48,14 +61,14 @@ void CostmapSource::configure()
   getParameters(source_topic);
   rclcpp::QoS qos = rclcpp::SystemDefaultsQoS();
 
- 
+
   data_sub_ = node->create_subscription<nav2_msgs::msg::Costmap>(
       source_topic,
      std::bind(&CostmapSource::dataCallback, this, std::placeholders::_1),
       qos);
 }
 
-bool CostmapSource::getData(                                                            
+bool CostmapSource::getData(
   const rclcpp::Time & curr_time,
   std::vector<Point> & data)
 {
@@ -66,51 +79,71 @@ bool CostmapSource::getData(
   if (!sourceValid(data_->header.stamp, curr_time)) {
     return false;
   }
-  tf2::Transform tf_transform;
-  if (!getTransform(curr_time, data_->header, tf_transform)) {
-    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;                                              
-
-      // CHANGE: Costmap costs are 0..255 (0 free, 253 inscribed, 254 lethal, 255 unknown)
-      if (cm.data[idx] >= cost_threshold_) {                                            
-        const double wx = meta.origin.position.x + (x + 0.5) * meta.resolution;        
-        const double wy = meta.origin.position.y + (y + 0.5) * meta.resolution;        
+  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 c = cm.data[idx];
+      const bool is_obstacle = (c >= cost_threshold_ && c < 255) ||
+        (treat_unknown_as_obstacle_ && c == 255);
+      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_INFO(logger_, "[%s] Lethal cell at (%f, %f)",
+          source_name_.c_str(), wx, wy);
       }
     }
   }
   return true;
 }
 
-void CostmapSource::getParameters(std::string & source_topic)                           
+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";
+  nav2::declare_parameter_if_not_declared(
+      node, thresh_name, rclcpp::ParameterValue(253));
+  int v = node->get_parameter(thresh_name).as_int();
+  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;
 
-
-  // Todo: use a cost threshold suitable for Nav2 costmaps; default 254 (lethal)
+  // Whether 255 (NO_INFORMATION) should be treated as an obstacle.
+  const auto unk_name = source_name_ + ".treat_unknown_as_obstacle";
   nav2::declare_parameter_if_not_declared(
-    node, source_name_ + ".cost_threshold", rclcpp::ParameterValue(254));               
-  cost_threshold_ = node->get_parameter(source_name_ + ".cost_threshold").as_int();     
+      node, unk_name, rclcpp::ParameterValue(true));
+  treat_unknown_as_obstacle_ = node->get_parameter(unk_name).as_bool();
 }
 
-void CostmapSource::dataCallback(nav2_msgs::msg::Costmap::ConstSharedPtr msg)          
+void CostmapSource::dataCallback(nav2_msgs::msg::Costmap::ConstSharedPtr msg)
 {
   data_ = msg;
 }
 
-}  // namespace nav2_collision_monitor
+}

nav2_collision_monitor/test/CMakeLists.txt

@@ -60,37 +60,29 @@ target_link_libraries(collision_detector_node_test
 )
 
 
+# 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)
 
-ament_add_gtest(costmap_source_test
-  test_costmap_source.cpp   # (correct path — no "test/" prefix)
+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}
 )
 
-if (TARGET costmap_source_test)
-  # Let the test see your package headers (optional if you link the core lib PUBLIC)
-  target_include_directories(costmap_source_test PRIVATE
-    ${CMAKE_CURRENT_SOURCE_DIR}/../include
-  )
-
-  # Link against your core library so PUBLIC includes/defs propagate
-  # (use your real target name; from earlier it’s collision_monitor_core)
-  target_link_libraries(costmap_source_test
-    collision_monitor_core
-    ${geometry_msgs_TARGETS}
-    nav2_costmap_2d::nav2_costmap_2d_core
-    nav2_costmap_2d::nav2_costmap_2d_client
-    ${nav2_msgs_TARGETS}
-    nav2_util::nav2_util_core
-    rclcpp::rclcpp
-    rclcpp_lifecycle::rclcpp_lifecycle
-    ${sensor_msgs_TARGETS}
-    tf2::tf2
-    tf2_ros::tf2_ros
-    ${visualization_msgs_TARGETS}
-    point_cloud_transport::point_cloud_transport
-  )
-
-  # The deprecation messages are produced via `#warning` → silence them for this test only
-  target_compile_options(costmap_source_test PRIVATE -Wno-error=cpp)
-endif()
+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>
+)