collision_monitor: fix linter and address issues in the comment

Signed-off-by: lotusymt <[email protected]>

Author: Lotusymt

nav2_collision_monitor/CMakeLists.txt

@@ -8,7 +8,6 @@ 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)
@@ -150,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)
+  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)

nav2_collision_monitor/README.md

@@ -48,7 +48,7 @@ The data may be obtained from different data sources:
 
 > **⚠️ 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/costmap.hpp

@@ -12,25 +12,54 @@
 // 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/node_utils.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,
@@ -40,31 +69,63 @@ class CostmapSource : public Source
     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);
-  // ↑ 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_;
+  /// @brief Latest costmap message.
+  nav2_msgs::msg::Costmap::ConstSharedPtr data_;
 
-  // Threshold for considering a cell as an obstacle. Valid range: 0..255.
-  // Typical choices: 253 (inscribed), 254 (lethal). Inflation = 1..252.
+  /// @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};
 
-   // Whether 255 (NO_INFORMATION) should be treated as an obstacle.
+  /**
+   * @brief Whether cost 255 (NO_INFORMATION) is treated as an obstacle.
+   */
   bool treat_unknown_as_obstacle_{true};
-
-
 };
 
 }  // namespace nav2_collision_monitor

nav2_collision_monitor/package.xml

@@ -27,7 +27,6 @@
   <depend>nav2_ros_common</depend>
   <depend>point_cloud_transport</depend>
   <depend>point_cloud_transport_plugins</depend>
-  <!-- <depend>nav2_common</depend> -->
 
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_common</test_depend>

nav2_collision_monitor/params/collision_monitor_params.yaml

@@ -102,9 +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
+    # costmap:
+    #   type: "costmap"
+    #   topic: "local_costmap/costmap"   # relative, respects namespaces
+    #   cost_threshold: 254
+    #   enabled: True
+    #   treat_unknown_as_obstacle: True

nav2_collision_monitor/src/costmap.cpp

@@ -14,18 +14,16 @@
 // limitations under the License.
 #include "nav2_msgs/msg/costmap.hpp"
 #include "nav2_collision_monitor/costmap.hpp"
-#include <functional>
 #include <cmath>
 #include <tf2/time.hpp>
 #include <tf2_ros/buffer.hpp>
 #include <tf2_ros/transform_listener.hpp>
-#include <nav2_ros_common/lifecycle_node.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,
@@ -43,7 +41,6 @@ CostmapSource::CostmapSource(
   RCLCPP_INFO(logger_, "[%s]: Creating CostmapSource", source_name_.c_str());
 }
 
-
 CostmapSource::~CostmapSource()
 {
   RCLCPP_INFO(logger_, "[%s]: Destroying CostmapSource", source_name_.c_str());
@@ -59,19 +56,17 @@ void CostmapSource::configure()
   }
   std::string source_topic;
   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);
+     nav2::qos::StandardTopicQoS());
 }
 
 bool CostmapSource::getData(
   const rclcpp::Time & curr_time,
   std::vector<Point> & data)
 {
+  auto node = node_.lock();
   if (data_ == nullptr) {
     return false;
   }
@@ -89,7 +84,6 @@ bool CostmapSource::getData(
     }
   }
 
-
   // Extract lethal/inscribed cells and transform to base frame
   const auto & cm = *data_;
   const auto & meta = cm.metadata;
@@ -98,17 +92,18 @@ bool CostmapSource::getData(
     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);
+      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_INFO(logger_, "[%s] Lethal cell at (%f, %f)",
-          source_name_.c_str(), wx, wy);
+        RCLCPP_DEBUG_THROTTLE(logger_, *node->get_clock(), 2000 /*ms*/,
+  "[%s] Found obstacles in costmap", source_name_.c_str());
       }
     }
   }
@@ -125,9 +120,8 @@ void CostmapSource::getParameters(std::string & 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();
+  // 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);
@@ -136,9 +130,7 @@ void CostmapSource::getParameters(std::string & source_topic)
 
   // 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, unk_name, rclcpp::ParameterValue(true));
-  treat_unknown_as_obstacle_ = node->get_parameter(unk_name).as_bool();
+  treat_unknown_as_obstacle_ = node->declare_or_get_parameter<bool>(unk_name, true);
 }
 
 void CostmapSource::dataCallback(nav2_msgs::msg::Costmap::ConstSharedPtr msg)

nav2_collision_monitor/test/bags/cm_moving_obstacle/fake_cm_bag_source.py

@@ -37,8 +37,6 @@ class FakeCMSource(Node):
 
     def __init__(self):
         super().__init__('fake_cm_bag_source')
-        # Use sim time so /clock drives timestamps
-        # self.set_parameters([Parameter('use_sim_time', value=True)])
 
         # Publishers
         self.clock_pub = self.create_publisher(ClockMsg, '/clock', 10)

nav2_collision_monitor/test/collision_monitor_node_bag.cpp

@@ -52,7 +52,6 @@ class MetricsCatcher : public rclcpp::Node {
     stop_window_start_ = this->declare_parameter<double>("stop_window_start", 3.0);
     stop_window_end_ = this->declare_parameter<double>("stop_window_end", 8.0);
 
-
     // Velocity classification thresholds (on the topic we evaluate)
     stop_thresh_ = this->declare_parameter<double>("stop_thresh", 0.02);             // |vx| <= stop => stopped
     resume_thresh_ = this->declare_parameter<double>("resume_thresh", 0.10);         //  vx  >= resume => resumed