From 1743667b776f81a7b76cad311bc728964b08c279 Mon Sep 17 00:00:00 2001 From: Mengting Yang Date: Sun, 28 Sep 2025 14:00:49 -0700 Subject: [PATCH 1/6] Collision Monitor: add costmap sourced(unit tests) Signed-off-by: Mengting Yang --- nav2_collision_monitor/CMakeLists.txt | 2 + .../collision_monitor_node.hpp | 1 + .../nav2_collision_monitor/costmap.hpp | 57 ++++ nav2_collision_monitor/package.xml | 3 +- .../src/collision_monitor_node.cpp | 11 +- nav2_collision_monitor/src/costmap.cpp | 116 +++++++ nav2_collision_monitor/test/CMakeLists.txt | 36 +++ .../test/test_costmap_source.cpp | 301 ++++++++++++++++++ 8 files changed, 524 insertions(+), 3 deletions(-) create mode 100644 nav2_collision_monitor/include/nav2_collision_monitor/costmap.hpp create mode 100644 nav2_collision_monitor/src/costmap.cpp create mode 100644 nav2_collision_monitor/test/test_costmap_source.cpp diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt index eb91f295d47..c1a1263929b 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 "$" 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..d8250263487 --- /dev/null +++ b/nav2_collision_monitor/include/nav2_collision_monitor/costmap.hpp @@ -0,0 +1,57 @@ +// Copyright (c) 2025 +// Licensed under the Apache License, Version 2.0 (the "License"); + +#ifndef NAV2_COLLISION_MONITOR__COSTMAP_HPP_ +#define NAV2_COLLISION_MONITOR__COSTMAP_HPP_ + +#include // shared_ptr / weak_ptr +#include // std::string +#include // 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 +#include + +namespace nav2_collision_monitor +{ + +class CostmapSource : public Source // Inherit CM’s Source (same as other inputs) +{ +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 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(); // Virtual dtor not required; explicit is fine + + void configure(); // Where we declare params & create subscription + + bool getData( // CM calls this each tick to fetch obstacle points + const rclcpp::Time & curr_time, + std::vector & data) override; + + void getParameters(std::string & source_topic); // Read: .topic, .cost_threshold + +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::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) + + +}; + +} // namespace nav2_collision_monitor + +#endif // NAV2_COLLISION_MONITOR__COSTMAP_HPP_ diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index 6b009f1634b..684314d3c46 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -27,11 +27,12 @@ nav2_ros_common point_cloud_transport point_cloud_transport_plugins + ament_cmake_gtest ament_lint_common ament_lint_auto - + ament_cmake diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 5d2cdc2f3a2..0c519c5d548 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -308,7 +308,7 @@ bool CollisionMonitor::configurePolygons( polygons_.push_back( std::make_shared( 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", @@ -377,7 +377,14 @@ bool CollisionMonitor::configureSources( ps->configure(); sources_.push_back(ps); - } else { // Error if something else + } else if (source_type == "costmap") { + auto src = std::make_shared( + node, source_name, tf_buffer_, base_frame_id, odom_frame_id, + transform_tolerance, source_timeout, base_shift_correction); + sources_.push_back(src); + continue; + } + else { // Error if something else RCLCPP_ERROR( get_logger(), "[%s]: Unknown source type: %s", diff --git a/nav2_collision_monitor/src/costmap.cpp b/nav2_collision_monitor/src/costmap.cpp new file mode 100644 index 00000000000..d52d2405cfe --- /dev/null +++ b/nav2_collision_monitor/src/costmap.cpp @@ -0,0 +1,116 @@ +#include "nav2_msgs/msg/costmap.hpp" +#include "nav2_collision_monitor/costmap.hpp" +#include +#include +#include // tf2::Duration, tf2::TimePoint, durationFromSec() +#include // tf2_ros::Buffer (you already use it) +#include +#include // nav2::LifecycleNode +#include + +namespace nav2_collision_monitor +{ + + +CostmapSource::CostmapSource( + // const nav2_util::LifecycleNode::WeakPtr & node, + const nav2::LifecycleNode::WeakPtr & node, + const std::string & source_name, + const std::shared_ptr 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); + rclcpp::QoS qos = rclcpp::SystemDefaultsQoS(); + + + data_sub_ = node->create_subscription( + source_topic, + std::bind(&CostmapSource::dataCallback, this, std::placeholders::_1), + qos); +} + +bool CostmapSource::getData( + const rclcpp::Time & curr_time, + std::vector & data) +{ + if (data_ == nullptr) { + return false; + } + + if (!sourceValid(data_->header.stamp, curr_time)) { + return false; + } + tf2::Transform tf_transform; + if (!getTransform(curr_time, data_->header, tf_transform)) { + 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; + 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()}); + } + } + } + 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); + + + + // Todo: use a cost threshold suitable for Nav2 costmaps; default 254 (lethal) + 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(); +} + +void CostmapSource::dataCallback(nav2_msgs::msg::Costmap::ConstSharedPtr msg) +{ + data_ = msg; +} + +} // namespace nav2_collision_monitor \ No newline at end of file diff --git a/nav2_collision_monitor/test/CMakeLists.txt b/nav2_collision_monitor/test/CMakeLists.txt index e2b668b517e..d9dabd5ea4c 100644 --- a/nav2_collision_monitor/test/CMakeLists.txt +++ b/nav2_collision_monitor/test/CMakeLists.txt @@ -58,3 +58,39 @@ target_link_libraries(collision_detector_node_test tf2_ros::tf2_ros ${visualization_msgs_TARGETS} ) + + + + +ament_add_gtest(costmap_source_test + test_costmap_source.cpp # (correct path — no "test/" prefix) +) + +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() + diff --git a/nav2_collision_monitor/test/test_costmap_source.cpp b/nav2_collision_monitor/test/test_costmap_source.cpp new file mode 100644 index 00000000000..36943006a26 --- /dev/null +++ b/nav2_collision_monitor/test/test_costmap_source.cpp @@ -0,0 +1,301 @@ +#include +#include +#include +#include +#include +#include "nav2_msgs/msg/costmap.hpp" +#include "nav2_collision_monitor/costmap.hpp" + +// Helper: build a tiny costmap with exactly one lethal cell. +// Frame is "base_link" so TF transform is identity (keeps the test simple). +static nav2_msgs::msg::Costmap makeCostmapMsg( + const std::string & frame_id, + uint32_t w, uint32_t h, float res, + uint32_t lethal_x, uint32_t lethal_y) +{ + nav2_msgs::msg::Costmap m; + m.header.stamp = rclcpp::Clock().now(); // fresh timestamp (freshness check passes) + m.header.frame_id = frame_id; // same as base_frame_id => identity TF + m.metadata.size_x = w; + m.metadata.size_y = h; + m.metadata.resolution = res; + m.metadata.origin.position.x = 0.0; + m.metadata.origin.position.y = 0.0; + m.data.assign(w * h, 0); + m.data[lethal_y * w + lethal_x] = 254; // mark one lethal cell + return m; +} + +TEST(CostmapSource, LethalCellProducesExactlyOnePoint) +{ + // Node / TF setup + auto node = std::make_shared("cm_test_node"); + auto tf_buffer = std::make_shared(node->get_clock()); + auto tf_listener = std::make_shared(*tf_buffer); + + // Construct the class under test (regular class, not pluginlib) + nav2_collision_monitor::CostmapSource src( + node, // Lifecycle node + "grid", // source_name (param prefix) + tf_buffer, // TF buffer + "base_link", // base_frame_id + "base_link", // global_frame_id (unused here) + tf2::durationFromSec(0.1), + rclcpp::Duration::from_seconds(1.0), // source_timeout + false); + + // Declare parameters the source expects + node->declare_parameter("grid.topic", "/test_costmap"); + node->declare_parameter("grid.enabled", true); + node->declare_parameter("grid.cost_threshold", 254); + node->declare_parameter("grid.source_timeout", 1.0); + + // Configure (creates the subscription) + src.configure(); + + // Publisher + executor (to drive callbacks) + auto pub_node = std::make_shared("pub_node_costmap_ok"); // <-- plain node + auto pub = pub_node->create_publisher("/test_costmap", 10); + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(node->get_node_base_interface()); // SMAP/CostmapSource node + exec.add_node(pub_node); // publisher node + + // Publish a 3x3 grid with one lethal cell at (x=1, y=0) + auto msg = makeCostmapMsg("base_link", 3, 3, 0.1f, 1, 0); + msg.header.stamp = node->now(); // align with SMAP node clock + pub->publish(msg); + + // Pump callbacks until getData() returns points or timeout + const auto start = node->now(); + const auto deadline = rclcpp::Duration::from_seconds(1.0); + while (rclcpp::ok() && (node->now() - start) < deadline) { + exec.spin_some(); + std::vector pts; + if (src.getData(node->now(), pts)) { + // Exactly one obstacle expected + ASSERT_EQ(pts.size(), 1u); + + // The cell center for (1,0) with resolution 0.1: + // x = (1 + 0.5) * 0.1 = 0.15, y = (0 + 0.5) * 0.1 = 0.05 + EXPECT_NEAR(pts[0].x, 0.15, 1e-3); + EXPECT_NEAR(pts[0].y, 0.05, 1e-3); + return; // success + } + rclcpp::sleep_for(std::chrono::milliseconds(20)); + } + FAIL() << "CostmapSource did not produce points in time"; +} + +TEST(CostmapSource, StaleMessageIsIgnored) +{ + auto node = std::make_shared("cm_test_node2"); + auto tf_buffer = std::make_shared(node->get_clock()); + auto tf_listener = std::make_shared(*tf_buffer); + + nav2_collision_monitor::CostmapSource src( + node, "grid", tf_buffer, "base_link", "base_link", + tf2::durationFromSec(0.1), + rclcpp::Duration::from_seconds(0.01), // very short timeout + false); + + node->declare_parameter("grid.topic", "/test_costmap2"); + node->declare_parameter("grid.enabled", true); + node->declare_parameter("grid.cost_threshold", 254); + node->declare_parameter("grid.source_timeout", 0.01); + + src.configure(); + + auto pub_node = std::make_shared("pub_node_costmap_stale"); // <-- plain node + auto pub = pub_node->create_publisher("/test_costmap2", 10); + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(node->get_node_base_interface()); + exec.add_node(pub_node); + + // Publish a stale message (timestamp way in the past) + auto msg = makeCostmapMsg("base_link", 2, 2, 0.1f, 0, 0); + msg.header.stamp = node->now() - rclcpp::Duration::from_seconds(10.0); + pub->publish(msg); + + // Spin once to deliver the stale message, then ensure getData() rejects it + exec.spin_some(); + std::vector pts; + EXPECT_FALSE(src.getData(node->now(), pts)); // should be ignored due to freshness check +} + +TEST(CostmapSource, RespectsCostThreshold) +{ + // Single publisher node shared by both runs (plain rclcpp node; no lifecycle) + auto pub_node = std::make_shared("pub_node_costmap_thresh"); + auto pub = pub_node->create_publisher("/th_costmap", 10); + + // Helper to publish the same 3x1 costmap: [254, 253, 0] + auto make_msg = [](const rclcpp::Time & stamp) { + nav2_msgs::msg::Costmap m = makeCostmapMsg("base_link", 3, 1, 0.1f, 0, 0); + m.data[0] = 254; // lethal + m.data[1] = 253; // inscribed + m.data[2] = 0; + m.header.stamp = stamp; + return m; + }; + + // ---------- Run A: threshold = 254 -> expect 1 point ---------- + { + auto node = std::make_shared("cm_thresh_node_A"); + auto tf_buffer = std::make_shared(node->get_clock()); + auto tf_listener = std::make_shared(*tf_buffer); + + // Set params BEFORE configure() + node->declare_parameter("grid.topic", "/th_costmap"); + node->declare_parameter("grid.enabled", true); + node->declare_parameter("grid.source_timeout", 1.0); + node->declare_parameter("grid.cost_threshold", 254); + + nav2_collision_monitor::CostmapSource src( + node, "grid", tf_buffer, + "base_link", "base_link", + tf2::durationFromSec(0.05), + rclcpp::Duration::from_seconds(1.0), + false); + + src.configure(); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(node->get_node_base_interface()); + + auto msg = make_msg(node->now()); + pub->publish(msg); + + auto start = node->now(); + bool ok = false; + while ((node->now() - start) < rclcpp::Duration::from_seconds(1.0)) { + exec.spin_some(); + std::vector pts; + if (src.getData(node->now(), pts)) { + ASSERT_EQ(pts.size(), 1u); + ok = true; + break; + } + rclcpp::sleep_for(std::chrono::milliseconds(20)); + } + ASSERT_TRUE(ok) << "threshold=254 case didn’t produce points"; + } + + // ---------- Run B: threshold = 253 -> expect 2 points ---------- + { + auto node = std::make_shared("cm_thresh_node_B"); + auto tf_buffer = std::make_shared(node->get_clock()); + auto tf_listener = std::make_shared(*tf_buffer); + + // Set params BEFORE configure() + node->declare_parameter("grid.topic", "/th_costmap"); + node->declare_parameter("grid.enabled", true); + node->declare_parameter("grid.source_timeout", 1.0); + node->declare_parameter("grid.cost_threshold", 253); + + nav2_collision_monitor::CostmapSource src( + node, "grid", tf_buffer, + "base_link", "base_link", + tf2::durationFromSec(0.05), + rclcpp::Duration::from_seconds(1.0), + false); + + src.configure(); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(node->get_node_base_interface()); + + auto msg = make_msg(node->now()); + pub->publish(msg); + + auto start = node->now(); + bool ok = false; + while ((node->now() - start) < rclcpp::Duration::from_seconds(1.0)) { + exec.spin_some(); + std::vector pts; + if (src.getData(node->now(), pts)) { + ASSERT_EQ(pts.size(), 2u); + ok = true; + break; + } + rclcpp::sleep_for(std::chrono::milliseconds(20)); + } + ASSERT_TRUE(ok) << "threshold=253 case didn’t produce points"; + } +} + + +#include + +TEST(CostmapSource, TransformsFromGlobalToBase) +{ + auto node = std::make_shared("cm_tf_node"); + auto tf_buffer = std::make_shared(node->get_clock()); + auto tf_listener = std::make_shared(*tf_buffer); + + // Inject a static transform: map -> base_link is a +0.5m x-translation + geometry_msgs::msg::TransformStamped tf; + tf.header.stamp = node->now(); + tf.header.frame_id = "map"; + tf.child_frame_id = "base_link"; + tf.transform.translation.x = 0.5; + tf.transform.translation.y = 0.0; + tf.transform.translation.z = 0.0; + tf.transform.rotation.x = 0.0; + tf.transform.rotation.y = 0.0; + tf.transform.rotation.z = 0.0; + tf.transform.rotation.w = 1.0; + ASSERT_TRUE(tf_buffer->setTransform(tf, "test_authority")); + + nav2_collision_monitor::CostmapSource src( + node, "grid", tf_buffer, + "base_link", "map", // base in base_link, message in map + tf2::durationFromSec(0.1), + rclcpp::Duration::from_seconds(1.0), + false); + + node->declare_parameter("grid.topic", "/tf_costmap"); + node->declare_parameter("grid.enabled", true); + node->declare_parameter("grid.cost_threshold", 254); + node->declare_parameter("grid.source_timeout", 1.0); + + src.configure(); + + auto pub_node = std::make_shared("pub_node_costmap_tf"); // <-- plain node + auto pub = pub_node->create_publisher("/tf_costmap", 10); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(node->get_node_base_interface()); + exec.add_node(pub_node); + + // Build a 1x1 costmap with a lethal cell at (0,0) in the MAP frame, + // resolution 1.0, origin (0,0) => center at (0.5, 0.5) in MAP. + nav2_msgs::msg::Costmap msg = makeCostmapMsg("map", 1, 1, 1.0f, 0, 0); + msg.data[0] = 254; + msg.header.stamp = node->now(); // align with SMAP node clock + pub->publish(msg); + + // After transform map->base_link: (0.5, 0.5) - (0.5, 0) = (0.0, 0.5) + const auto start = node->now(); + while ((node->now() - start) < rclcpp::Duration::from_seconds(1.0)) { + exec.spin_some(); + std::vector pts; + if (src.getData(node->now(), pts)) { + ASSERT_EQ(pts.size(), 1u); + EXPECT_NEAR(pts[0].x, 0.0, 1e-3); + EXPECT_NEAR(pts[0].y, 0.5, 1e-3); + return; + } + rclcpp::sleep_for(std::chrono::milliseconds(20)); + } + FAIL() << "No transformed point arrived in time"; +} + + +// --- add this at the very end of test_costmap_source.cpp --- +int main(int argc, char ** argv) { + rclcpp::init(argc, argv); // initialize ROS 2 context (fixes "context is null") + ::testing::InitGoogleTest(&argc, argv); // initialize gtest + int ret = RUN_ALL_TESTS(); // run tests + rclcpp::shutdown(); // cleanly shutdown ROS 2 + return ret; +} From 80bfe5c543624a92777b301233b162d1dca54ef0 Mon Sep 17 00:00:00 2001 From: Mengting Yang Date: Wed, 22 Oct 2025 20:54:24 -0700 Subject: [PATCH 2/6] nav2_collision_monitor: modified CostmapSource + dataset-based bag test (v1) Signed-off-by: Mengting Yang --- nav2_collision_monitor/CMakeLists.txt | 3 + .../nav2_collision_monitor/costmap.hpp | 65 ++-- nav2_collision_monitor/package.xml | 3 + .../params/collision_monitor_params.yaml | 6 + .../src/collision_monitor_node.cpp | 9 +- nav2_collision_monitor/src/costmap.cpp | 101 ++++-- nav2_collision_monitor/test/CMakeLists.txt | 50 ++- .../_tmp_cm_bag_0.mcap.zstd | Bin 0 -> 20010 bytes .../cm_moving_obstacle/fake_cm_bag_source.py | 114 +++++++ .../bags/cm_moving_obstacle/metadata.yaml | 88 +++++ .../test/collision_monitor_node_bag.cpp | 240 ++++++++++++++ .../test/collision_monitor_node_bag.launch.py | 97 ++++++ .../test/collision_monitor_node_bag.yaml | 25 ++ .../test/test_costmap_source.cpp | 301 ------------------ 14 files changed, 709 insertions(+), 393 deletions(-) create mode 100644 nav2_collision_monitor/test/bags/cm_moving_obstacle/_tmp_cm_bag_0.mcap.zstd create mode 100644 nav2_collision_monitor/test/bags/cm_moving_obstacle/fake_cm_bag_source.py create mode 100644 nav2_collision_monitor/test/bags/cm_moving_obstacle/metadata.yaml create mode 100644 nav2_collision_monitor/test/collision_monitor_node_bag.cpp create mode 100644 nav2_collision_monitor/test/collision_monitor_node_bag.launch.py create mode 100644 nav2_collision_monitor/test/collision_monitor_node_bag.yaml delete mode 100644 nav2_collision_monitor/test/test_costmap_source.cpp diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt index c1a1263929b..bfd7e01e921 100644 --- a/nav2_collision_monitor/CMakeLists.txt +++ b/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 diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/costmap.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/costmap.hpp index d8250263487..6bf4068bdf4 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/costmap.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/costmap.hpp @@ -1,42 +1,54 @@ -// Copyright (c) 2025 +// 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 // shared_ptr / weak_ptr -#include // std::string -#include // 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 +#include +#include +#include "nav2_collision_monitor/source.hpp" +#include "nav2_msgs/msg/costmap.hpp" #include #include 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 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 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 & data) override; - void getParameters(std::string & source_topic); // Read: .topic, .cost_threshold + void getParameters(std::string & source_topic); private: void dataCallback(nav2_msgs::msg::Costmap::ConstSharedPtr msg); @@ -44,12 +56,15 @@ class CostmapSource : public Source // Inherit CM’s Source (s nav2_msgs::msg::Costmap::ConstSharedPtr data_; // Latest costmap message (thread-safe shared ptr) rclcpp::Subscription::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 diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index 684314d3c46..d80ff4951fc 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -32,6 +32,9 @@ ament_cmake_gtest ament_lint_common ament_lint_auto + launch_testing_ament_cmake + rosgraph_msgs + ament_cmake diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml index 7287b69115e..7b6f3525f8d 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" + 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 0c519c5d548..0384809fac9 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -308,7 +308,7 @@ bool CollisionMonitor::configurePolygons( polygons_.push_back( std::make_shared( 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( 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", diff --git a/nav2_collision_monitor/src/costmap.cpp b/nav2_collision_monitor/src/costmap.cpp index d52d2405cfe..c367ea5979f 100644 --- a/nav2_collision_monitor/src/costmap.cpp +++ b/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 #include -#include // tf2::Duration, tf2::TimePoint, durationFromSec() -#include // tf2_ros::Buffer (you already use it) +#include +#include #include -#include // nav2::LifecycleNode +#include #include 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 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( source_topic, std::bind(&CostmapSource::dataCallback, this, std::placeholders::_1), qos); } -bool CostmapSource::getData( +bool CostmapSource::getData( const rclcpp::Time & curr_time, std::vector & data) { @@ -66,33 +79,43 @@ 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) { @@ -100,17 +123,27 @@ void CostmapSource::getParameters(std::string & source_topic) } 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 \ No newline at end of file +} diff --git a/nav2_collision_monitor/test/CMakeLists.txt b/nav2_collision_monitor/test/CMakeLists.txt index d9dabd5ea4c..4aae78f889d 100644 --- a/nav2_collision_monitor/test/CMakeLists.txt +++ b/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=$ +) 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 0000000000000000000000000000000000000000..846b3e8ee69bf5f3a7e31263e686038952fc0612 GIT binary patch literal 20010 zcmaHxWl-F1xVLv%+})wLySux4|4`hDwm8Mz-HN+wvEs$u-3rC6NU;`&m+*c#AI{9# znP)b?>q<7BC%HFt-&vnP|6S{VDiViD)Ew3#1i0m=R_G;J1G|e3i6|%@_=+#mrQrU2 zGcYXSs%cPZp5*PMPQ3mw8WU_+pCmU6`?PWZ{tp{Hpfp8C&pJj{6SwI63-%l#E99Lw zHAe|J9h?jPTlpl!*e)7aaQw$vxNG+GBP)Wqyj^MnwQK!Ao=k-Kg9`ZF;_Rf=zaUfJevm} zS#V=@rvLIQ?c~5|XNtu-#v>;shJL6>9DQ{dsxN0GqLCy41KEobxi4qX$Jd1C9AdA< zensxwePc~_d|n(5=|qEYF8g~gn^3}|RL#~jG#mW0R4NTUjbUgUYfznNAQ&>I;x0E> z2Ru(1QdB7WtP@GA2Nu$!LQ}&MuWHs8)t8_$~7Hr*Jv=tjRmZNhOzZu*d$v8jBe-FF+PB5g? zUQvA8iQJ2Znd9T>tD1=x;->P)3_c$XUQ|EL_!$c1J9%I;yIbGF=TCp`*Wghi3lvPQS5Jgr^i*^{3teSR5}8fs z(T%38e*01B-0EE1@X_EPasT(Pok6Zh^V66I7yGZQ644Y{-~4=jMD2oku6uTbR?BQzfjS5kENLNIPaM<`aZL~cvhi^ zn%E@|TEZ&0Z_@f@q8^qUd(-jYL%YLwuBB9M#_y@mTQ?mx%ZBzEjLxTZex>pm>ka(- zr@gm~r-nLDeO)s(?QZW^mHje)G}PZ_+kUl{HBxEs(tq}K8_F=NOd_;hI$4dTU*bjg zJ$d4?Q=Q)5*x*4Te*|yQj%8y?@IlCn0^Z%;IZ+Pk6l4GAXY+p8WscM8mZz;-#AMQrE8Q(=K9uysiUWFO|6m`jh*x4m@1-G_tjb=u@sMWlNR;@dkXEx z9e>8`R99Q8B>8hbuC0%Cwzp38(D8no`t2bfWLoZ=H!gQ-(Eb;}z*=<0?)~ppy>!Ov zn}(spe*JQ3%))TPpA7=pagv$#RO;8QVW&$C?K}r1S6y23=J%)ZOXO&!)JZEoD_E!b zYHKB$-?_%RT-**?_2{nkP(L^Q>FU6L-TN~xHnXm*vZN~S6`iXZZa@2-tw-;889qFZ zw})yAR6d<=*zO(VOuU}|c*oH{oPS#9dR>+Ntf@P=J}^|w5Z>`+`M6TFg3Y6dfV!Lx|m}CnerejAb`t^zwCMU>^LnKqRs*JYCmbD_2I6~u0h`OA;o-m^5lGgPgV-s_q0l)op`z08le;xfq2|nyDcht6asSn*S40z;SJh~BtYg3s#qufp?dVUU3eIMk(4A>r+f5 zw)!G(3WGwXQJWYF5wOtT)CEVpv0`Lavj5oo{HeI!u?PZ9XnOPH9OSP8M@}WyhnInt zN&>(25sn+aYzr)a9;||57daVAVPREdK0-cun1&?=FDk%S@t9W_1f~FynFN8t894K) zE9OB2pzURTki>2IWvgi<1N%+)s`yc@o0@<~PDgw&2C0nsrc?0upxK`M{V^0`yDu)0 zVKVX;({wzNWEZq*7j=EeDP(fyuP}es3>iiX8@(_(qErzbOEr~DQt7-G3WtS8ahZ-L zhef;yU2G{nu?Ic~4z>k^%-0`-K%Rx`?a-g9T668Yva0NL!U~@cLSSOYJFGL?42t_IitdlAF230ogw|d>X`4@hTmFSy^ZX@Y1qUz zP8Ofapi4kD$7Lgl95s}K=}W%o2v!QFK8Tz+7xVHAUA6!H_|3)8mfBvezwY6$$!(YJ z*_}~9aM@O})35O8@bGXd`9xF`m|(KOGzPO6Fs3osIzEhCfp8a|97+N@FgzR@Q;rHV z0^S-Cu>wMasJS{Z!uCmVOT9GfY`&~9eJXyvsn=1HV7+&Bi6*%!`N%-d?l@4 z-0GO8inAn*ayR_+Shl<|LhVxSMb$8t1ujZ!*z+qK>j_TO*lbv^^0){|>74i;`iW;{ z_cxhnn`dc!Mp(Sk^S?3I)t@=*KB#$sk>7uSF5NcLtWb23y)Q_qs=j{2<9FAoWtDfI zuSljf-9u%*E23qd$>us9Xlua5fh{kkbZ-xy%u8(0w+0hM^9pN;1d{y0-i=>ic%f5) zdaQElDOw~1qwx>j^o=-ctk%6Z&%>QxSyH+Gv6*Iw51MZZ2p%FC+_&15rxcL%ZSQi) z-GsBG4Y-IeuTG>+GgFVew_w&c0>Pmqa~J2(m~;C;*^ZpY@VH+nDVwIJY@ z=lRD9C(a(mbQ!Ie z4ipj1<^EaE#aU)DG%6&hbXA##Rvb^?9Qy|>=2ue)a{Xk8SCs~l6e|6{CT6^O#x?8r z&92?^EJeXqdLWbM{QaKywMYp9Mg3?~TNQT5=CDdlPN~NJbSQt}V&?IpwrcY(BGH1+ zwbA*pyGJsDQzXP)zqUM-2Wihk=D;pVy=fx973YqO)4RzII+> z(*`@j?p?U@pavYqEVYC0Ii)^P_iEJoQGh%j><*7$IugP296O}M(EUE{{)>ih>* zS+k@ikq@GnPhdbaMQ?be5~U}>fT5azIqZP#Cd#th+-VlJT$_D$bvIqLrBS$E=kLY~| zS6#>YDG5g3003{=Muz^292y!Y8l!>WjSk_bjqr<44(UWsydeM+V|I_)xps~!i1pHI z2PBbnTXheNTH^M<0RSeCHBZd-HBG1%w=r!Av`h$3H3#5#Oc=Vhzaao`t2IvBc+^kF zb2qUX4t^4D3`cICp5ADE0|12KYM6^Js+(i~@|^?zhV`YsC%tWsbiMTr0MOW_ZZWi@ zX3^WUp8Ez;zi4J&ryS9`xTw+ch5&4`Ub=!aQnI2}2}NGEf;?J^U0Ah3Y*M+>QIfqT zubj081pZK8571`IH$;4G#s#8Y~N9=3%QrlPSmJ=mlU9_TaoM zU5X$=^L5_rAXc#-)y>KFA+EERf4d}uJGU1&8N~49n_YD&)g0k-8yGoKd9b-W59}*D zoILi7{%m|RSRy#FG~7rem3(%HW*Hrj&6|1F*f}_J22yX+^h>KlIr;TE@f~5hTTu8c z$y1N_2`;^gdrfIX4F5mx4gm2d{0UET)h zSI)**)7NbgzLagirb#}-_%f$%#|?iC)5+P^HqF|`^NZPIPK(|HbZz(F<_%Tzm=Lps zz4PYyJq*sM!_RjShk%pEzM2Y1L5R|Hi#sGsj6DRAM;;RlgdYP=aP8tY-gGQNZ!Yfm z>O;&iH+A3{sKEaWa61)8|HZLU1x^u%pM4_^JL5j^x@hV3xB%Q@>Gp+{_k~cP*B)Ka z1p;2cMdeTTYup0YYrsCo{%W-Qa=k9@F8k{516YqQzr10-+;Dli zqbhpeh?RKV6nK8VhkUoc2W(UAqUK**jL-6HF8BHGo$vWX?4GXGY@PrI;eGF5{{%U; zeWKlSe#*OWe2R8822p?=@D&;iff1y7hLm>~p^fX1EV@+8UlXO~wjqSdO&{W?HbApu zZ3GFhHN-YFhiFuqz99hnrVq^}dKs~f<4I37Dw)VV?s-`@-yny@~&O3)ulNETHFj2Wv zrQ8`rPvMd!NFhjJ8HxphDY^e33gmGCA(9i+Gufl658&eXq3+)N{lr6+z0xOtjo}yr zKB+xf_q5Mju()EAlRXHcL3UrAm3Fa!ixD}E5p0*GJ}iRN0KI)BG@Q|;nI&ung~b~H zAkDTO0*cLl2pP&fOs4T45E>Wt`bLcq9-)l@0<;oeZDj9b4dj!_c2-VQ9pqa|ZS9YG z$nloCZwSC@#cF75d@5)KWX&80^y+B;eNclM(?(0}(RxDw=9QGewCt0_l#Qw4c3&V| zlZKcnV=7}Ry#W9|8B60_!b#$+tyS{%?wW>4!#^qDq#!E**)ftA{{ssTKIC;DH~T?@ zg8#}h^Csm9;+yvh;6LZl@esLxMJO$MP%>@!g z%P$_PWe7dhfBJN90DzX^#I%-qM6_C{*4LiK#1iIYa|!RXP*L;97_j}`F^DeBHj#hL zCF4`4Wf)JPdCj6r{?TO)MQKxDa9E(83IhoyNQ(?Cjl~tBu^B9746XI}E=u+s>}kYz zS#%UaDH$2YHXPCy&hC#Sn-efV!$x7XHLUPEa8}SC@d}+GR>1H-J}0J9!SAhKrQW^o z|2|N13XEY-gcnPk?bAz^N)kHg&qU0RJDbhoS3Dj5;c&-vZiM2{f$DD`*#^EC##IjA z4Y833aqAGBozOeFvKPEi&dm$u&Rt@nBE>ezAF`XnTKKL=K1%L{-r2>W{a%Hajw(uS26OSyfHyV1Rw($Y-S!q zEN0xD>fQqxl6@r(zjh&hPS8!y1%EAGqdkXn?ljpMZK6|(u$L^lf+hWNbAFp-e$WJnIlhc4ZBT~R4b;boT{N7sicVo*NqFm zo6WvGfLKnS=YPm55_FMu9OP^kk$1n{U5)&@l8qF5q;lX5MMamsN=`hR%@zP}mhL4| zBzQ3eH=eg8*~};Jei&;y@rlQ`4U3TcgBih^>v`hYE{UZ6nVWDMR*G((uD~fpJA^tKvZ}j>fS}o8Ic7|=+{fZNC!N~lW9q_Cv1`9@49Bsz)wx|iAkNa?QtDI zH_Lz^LdsJ*x}lRFmKStN)aG^EPseqod&hJEEzA1?<0v$tJA6H!t2V1U|7k{7@8^g< zA;+-(O-_WusD5+i$gSp#zKHLXen#J*kzixL5ingY#jSeckWu3PU{>>l(OuEF5qx>4 zDc4blDPTX?pZnr>nacz;BlAZ?EAA%yG1E$o5itn zlNFXlqZQyFtq0MHZ=VV{sy?YXZxEIv)_%f2s`&()+W2V|vEhvYJpQi8(YLSAu~BA;Bub{(QE|11 zb-luo(4p)N0ob%5%Y~jQ(}gp0k}UXLwhOjv76iHgNd6H(_m~O+hnd@yvPFY2#jx3+ zzbM0ps9@w{W!!dA>&`&1AT7;#fBawS??>iH|AM})EmXNx@!S|4Pf|wg6MkY}gMr7NZpWF}p5eli7ojxvwxRgHZtOQ_ExU3B9qNQoOC7f0sl6U<$8kPz3gOd^e$wX-MaMa3R(CDDwV~skoHK3_XYpM{obgC$ zL461#;b-XH`P}j7X`nE~vElx~c85&Pv|wov7b^&E5E=sNhQ`N{7Iw18jT!%bw@w4A z$>Oh5?Ymk6H^p^h@(wCiw*B*ADo(e=;b-(Lz7gYp;gof1D?Z=~`XzN}Pxb=C(WuK7 z!aWI0ThS!D_Vk@Wq$^G`edS=!s?z!SM=&)Ui}DB8@a*pc+ox!_@IoTimS1E0n$CZb zz$vi0zc2EeDSe(|y#0?1F+{H3VIvK>hyTbDBZYJ zadPV)s-RU$Z`Yp#VtKd1>Ds4dhWEjB9>4OD_$(5;Y)%phWSsLRKHLYOq_q6^!k_FtfpDt;-RePPlKZP*+^5%`$gbJj*A=UOoJSV^xFRJ)GIYo5?~Y=JNB zlskT>L!;?mz4{K0%fTeJ{SX~T;&avrY#k1=$x5<2aA)={om7wV^*aLR)+fom4dN@6 z;!YNjT(*<_1ml^{v-DIrSqdG7EC^G7BN^#^2SQ7gjH6jc}-Nwj~Wa+Lpb%}!*{p+ehdhWUiS|LY^r4=s&!x}eNRAOS7>OfLrCcJT#kFh z=QOtn#tNx5V_WwX8?T7Y9?yu`N7v}-d*^6iDn`Rn8Yj2tR3F!!Ki<(58D7ywg^qD_ z%norduh#ZhWNIAOag^3AZhp%p?hl4rqN8HPX_IDm@bb#0X@PPkz0?+Icw**n2*9!L z4Kq1u3^EJ${vsKqB`X;sUYKWk(3%4|(7vp9uHGsC2P6?889o3#n3bxZ4#5|5lg*sv zgFIP4xLiirq%;aZGfVFdd^6bcpAQQ7p40_XreeT3oR8n zKT5$RS=NtCC0MG2#D5J9QtXYF*^F__<28q(Ur<7~{HV^u`FKTbDFGwomdpNAEbrwDz z%j~kK1hJwSpDug+;J$_#bNJ_pI3q$))kJ=4Vku47+1sye-il_)0u6FIxi|+*;na3h z9|+&+<+EF7eU1~ET|VWgF1|P?a+NdFS^oW{HoAG&mfIRLbL@V0UaFG1_(=Kb>tKB@ zn_XKvmr+l4^Ra*C71ThL>8y(s{zV7|dd6w7<2eepLPL8tqGChPJ7tB2E{R;yyy);s zwrg7IRxTGF{yHwMh>S?C#*$teYpcHB+Z-61m=(q1_pa!(9o&Qd#U7t`1?MspE0wDDPM*!w}O z8(ktcX0vW+7?aqSS8(@KF0ffqcsUkTGdT8HC?-&?9Cb7D<_`Lp3>qApcskvLQbS9d z*J)zVFNDb^-1)l?R}@*pL)d3()KUkT9wo_Qu!NM zT+Xm8MAo?2mYqce_T9QgD>yR^LRZ)lDZ-AX#1o2FeLY-qSGSZl)DsODmC_k;N!7Oe zpR~KrX7yW>*Na+73wh+6b+4M3C#rX+m8EskXuqWtvi*A=F{Y2Xgk~M=c3ylbYn@3* z+KBUI8F}W?<*fF{7S1hwP(>=(u^eZXzA-DA|3`48yZ6M7@lPaG#Vdc0x$lS9bmbF9PL+E91*^r?l1RQ(QZ|Bewob0`@+Y(2&723?L*8H5M{=`Y zjM5#P15PotM1?G41tET+JCx|0??3k}eD0Qc4qfQJ3`z?^?CuvnhcK=N2>G+4WrYyB z=aIN1qdel!UYMaGBT?LRTT;=(RrHMror{;O1yJ>H=$dK37LO4xGn$?Sc=2nZ_9JI{ zY_9gVLW<2wh0cY&+$wsJq0UK_)y4Pzqok)&6)J%{5ff9FBV8#+&Tw!0UL(|2YG@nm zME652OIEh3Sa8vTBY~FX6t)6oD?et))nzL%V<&u)PH^kbS7SIQGzE*@M{UP2JvH0k z5)X&ST^X8^2`7ThG1jRNoTCzfOZtO)tn9Qeno*22Sw5~xMOa%?ywRATU|f)ClU`2q z)qgpm<)|PP`Ow*paIVyppAaDiR<@nbMa4>C(Aw$qGFsi3-4~ zWB(K(m8zH#mFz~BulN=uSJ4{Resx>(ltnIpEYe0WwX#gZlm&+x4!ww6Zt_II8JUmM0I%Q5;U zK(a>lT8v=!GC;96{Xt-B%D|^-B_XA51(es*k5X-h<$Q1Y*5*UHRvKLTj$>rn4nSRu zcCiU*D$+6WvUVK5W9bM{C+@1_AnXEM{Y5?Zq?$O<6b)IIuGG6OojTm!qcogez_nS_ zB%Dr~<7x&F^%A!b_M(CO)61rV!~4J&p%(R%T(FpAlw{yiX=oWaGAxb!{+DX+Yj7vo z&e3|qZ@TrKmpo`pZHABQGfk{VvDaagV%jSnUhh*Eiv)+ig^TGZu+k?{d(l zDQ`bZvc)LtU*VLsoS>JWmZ)D#UFRPe8(z{&SQ5->P?N%M(#Pe@GaNV0^%7$9ieHVgX!f`Lv; z{DBjaI&K+Q!U2=d1OZy41D$pxZwSCGVrau594N!j^Tm7(WnGo1Xf8Oz+S=G}0Dwc+ z2xI-?@MEyA+w+VI7PO~@I9BL@u$YZLhR%yNHdBN+S>gge3FtdHq~(fs1fou~NgIAN zX?~cQW+uoCV0iGsi7)7O&{B9vGfwV^Gl2;&bGkQAbASUMJ{Vkgy*!r-!_Ip-L(jY6 zKQ12h-!B4=ZSFS1>;mx-mISn<8St_w>3_BI)%8{B$58DM06jm zeQZBm1C-OmJ^|0~wQQXu1j^Gj;e?a5uFazjNvgvQz<#9Mm;1eOgL8C{`DcR-?`(q; zDed0}kfhY|)|F*@8F1+A*o= z&tsO!>9co8Q)hr1J{Y&;nK|R-ofeTG1xbU2$=^?4N}n^18OM&B^wqW-laUstl4(zg7~0TLr5sQ3)$?l!HJto{g{v z2ZqqnlvGSkzm%g(t+9pI ze^D+U@zyU|u;uV%ZJb0_f?P@wOr|%x33Q?MtfEgX@9ibsWu9m#rmMzRufS`h+M~k6#FgAsOz+;WZvcS1 z1#viYS+O|NQyTM}`f)m2Nh}kI(^Cx{f%w24lur-9zgyb<`^+I0xQ-V3%Rb~aVFcv| z5OJ0H6B(ZEU~`XeiUhFi1QX%O2LoBQ(U0`=urKM#({7Z3-?V2?w{hUg)6U5ka^Uw| zcHGH*SbxHJuzmgVQP!J$smALy`>?iieWC)q7`8nXaKu#VP8S<8RqZM}uBSM>1`;$? z8`>H?Yue8W<7DrAX2fkeWF2T(96!GS0G)1agm|d9wqwaM1amWyu6G(9tX4)A10UNR z`C@wkFU;$qh2&lvXOln*+ne{<-Lh}1P;c`$4Np&09^s}RChQ7AS=Nni^J(ZJ$cT-! zjS>x(1W!#+lud0phWu+1A*)w^y&#foK`=|1y768|khE93HC`+efB0FNov+sTQN~#A zAu2B$T|JCV9i6+wpztWnt=?rp5tCT`%Ikp|@?DQF5rGOmh)>W)sRc>4B=;m?_^jWl z9u*&3EAN@Ohs;smJZ7&bU)kk=h~fN6>d95_FLE2DFupl`o+oKfQCH?*Zvw&pUJBmb z-h?ZcX_B$(u;2Rnh3$~_na{{-0b-#losZf{xkY(U!C0@ID=76$n+MqFa95frMyLr( zln@F@z`riSwson4NVKbPVwR+Y1*N3437>Q!G_N$E5_MjnI8y@D_?o|oO0VFHNDuV# z%Qf=J0q!d?qYuzEG%-ZS7`V>r#FipwD)u9ow36pw}8W zr_l-{`GY~e*rQEvQO~NCiO34%Xgc!uE?~0v2_L$QAz0+740M7pvBSqi*UvjnU{}-T~6%bNXh>ehME%mhBtu{BdizK|4+fLV@`a1>0qa--1+gxJ1QDwO;`Wy>Vef$D8~On>c4Ef_G^|&-PWfGgt9LA zlX?$lAAhOwJHxWa`Hf`X;WzbvsR0$}bf`Jn1*6>>e^-t?t0rjbZVeYkn`pX%Kum81x zdSd=oKfTwc)e5NjN%~eiJwkh{o!-1vPDkDULzsU&i|npt5YBwduYv*2uh(EJB^{_ z+hG}t=wQAf0Iy)cn$=3dn7I~Cka-9wlfeFkMl{3oM0f)LM52IL5~4v_@>!2kVrzDw zLVl+rSSGr`zX1SBG=r>7lV7Z;&xdI0e|Fc%o55I1kw9Am8EA0r|7^qJ|JttX3;kiO z`p5G8*oytQvR?zf`2?&<1>?U@myw+2ne!C}s+VL!a-u46>r(s1v$+y$ttOHLat$j~hpA8?O@O2-ASLJ|! z^iqp-@ar$dzK;>(o=@QCvih;6-x|LI5iqGF@Sy{cP{b7?-(hyMO&fE`v&}?PN$J!$x#WW{RI3;t9MjW4g;4>&h7 zY3sgcg}VON-NH@wzefJz#j84zo%F;8m7c+c#wCjDi&XVOA=dRHs&qg0&X+S#356St zstf54%%UGgUNhXFJ*2{XF+$rD}3+}hpI5RswV?kVtH_oX4WtQx(+EG24MsrwMo2KJ3;EEjFthopd1JjCJKSiTwySP>TDJAE zf4fkZvtv&5U#JL-;C}oTu@~t~&ajA4nljpiGla3IqM7)YIges8z-T}(i^6QQoNv)m zRoHQVzAUgnsKvtBF>=rYZ9A=k2&q3kHtb)d^Y9WXPAQz)^N5+(ISc({hh!)9FTwK9VNy;bZrNV1`!O1PaM@_ zhG*Jx=kWEde$LlP8wXOVl$pW3 zCzkyfJ#Aitn_tg&WGUZYPSLy>21B@yhRMm&M$T*F6) z_e4H9|>3C7r->Kd)Awl>jT6|GJwe#gh4r0?5YxPtuKQt~9PuTfu?&fSbXVP1Qq>^Fm(yv!9=`OSk0wo;u?7dNqCq@N#} zIs%)K710fc+H1px)J7LEp~WQ}5b~oauqEFX#ea86+vZH)POe_pitQspTZ*tZ zI``saE+i=n73bPCl2Oo<5t$-{+5vy9IYDs5{lJ`$IAS?j_@vqFA<=8AlwSxN)Q<=H zy*BIrt8K;~xbCmYGGos98;#8FFD?83ce~`zWH=qvY&#VMNCuQAn8El0g6W*Sxu9vf zxz_s0(A4CKP{1{^K3E*FnN5ByoDIFno(V0bAC6#l7>od%McT6VURS zug2vmTGD{N7K^Sb4VXGMWrf7gCZifYMH{*CH4xsY)nx|kR%gO8{3MZUiG`{g(P+sW zz-oR208AJv%)wO3&$%p&!Y$(ZSlGB#krT~V{+cffi9Cz*PtgnWdt9d}*IEUGi@m>8 z<&Se$z5xI-P^1<8J^ETSWy5P!dYDS^K1<3sbIQg)u`I91Mlu636Y8wdT8@p1X)H7t3~vmHtm3ES!w@>*-KsXNjQjR1iIrBVwYL zNPH`+DkIuKg~v%OSUf0umKVc%^A4}$Fc$ra{DI4XXwKi|0M2Cky!*KJgBY%{6E#~H zCA~X1BVt{*md(46>A$r8C`#nZ73vMEQW&J2XIXa*B-jb0tN?2HilzeFf!y62lj1ghz8W%tS9_BiQx3e;P_;q)3eHt=+(= z%_>KzP9Tdp}BGCG7Vk*4nqANIzhd5$Whhv^J6Dt_y5?(Wh@jgj(HL!mrwj-;7{i|u; zS*5WZHJF+Z;})-Jzake|Ynd5dYeCV((VooM8sYO2U;C9L9?0;G5e*$H;SF(~P24pr zlw}d(hOrHy@G)-yfTs_hO@-_pO$G%eeDy+JO|li9c3gfe``<=F)*w6Ti6d#nrXM6HdYdsb-Pdw(FePrpsVE z)YeW`%vN&zbJrJv&j#BD+YdgLy=(>+y-x(U@YM~IZkAYucDD{8$S#OED{X{ z8dcAcX0I_@40T~G2H1$r-v9tBH*|;1`~MsEk3B}s#nc;qoYbv2G#b8xGXlt43o7$3 zs(O8%+X@YASaTA1Eo4!FLn)z!@~QkKDA=IrpW_Bn(XzJ~@^l9%!jKLGRNZ?a;5I6! z==P|yyiX)w_nb@>RP3fn#gT9=(fwhNGIi_8SbND1h1 zU<8bOsU-VP*Bn$VTV}J#&m)B1Z$_|@o3(Nkd^%D&#*|P-{Uelm29HH?DZOvkMY~QO zOs|G%QNz!V#K>bGohRv0u7l6T4GvEG)6wIUxvysEy9zT*?{>gW>qp@=E?*x*iL7_1 zZ=!RPmbCQF9hL{#w9~+d62O50R8aqzV96QvgR?z;iq4n`l zZ_Mja_cg!1w$Pz6DVn7`8E&zO9dyE+sS?zyHF>|P`33;^@wf8Kmvg0=rl}3Q1tOK1 ztW0IJQT3VP7PU77;JIFfdHGrS`QXAe!rQZdQwpDt)aMm`sJ#IIVn2#3si@=#)Z&W{ z?OW(cpf8I92ThIu-!1>DE-)ADHsl3!f|iH4s#u13v6;@@^7k%aqKj$vh4uYDZH;QMuvx@ zMTU3BNq}eP*B9D-Ofs5%z{#(*oL48f({d#-?~6n-?LQDQ9i_I@9Rco%z1_f#fSM_m zmi9e4mtMNVf* z^M~Dx`j3D4>)$+OePFPx1dZEo9?WhDGH{MLJaN9c%ptUlasNv>U&1De#9n|(*EWis zqIWqsHQ4^6lJb|l2Q;AoHle7>GaGSLkfs@$%VRpFn%tg4&pM?IrG?F5K#k}D0&;?P zjpP44g+-_V{zMaJ35`(vGhB`sehzi&oPo(t0!=K^!)J&p8S+%}31;1Y?+umux)nh2 z8Hgg>Hv!n6W54wDR$w3%|3Po@U(vt54Gd9^HKfQFT&;>YQijAHZu72NkIWWMw|>UL z3qNbz?V;r#%H~jD;Q;|xWS2f;K3e5UNH5u?v&k9nL zyga3>KR*F#YvZ3j6X^N<%FE>_%yR*QlEgiT>h&TT#c&7cVde`(XKcp%e}o_~fE$%w za(b^Jz@Vke6-3>s@}H9DbztCqSoWQ|6aC!x0UfKmf!W};VPE_&iF#N0 zVcX5EgKhEko_YZ%9xvO34`We;`=`DCYCN8Q>}&7)>9v+G9vUoW&abIKf+?0n1&*hL zlC#3cc`fF%UHCDP=EI@*XhqSOOj8HI??(FXxBv9jib|nJC)#4hXf!st3t~XR)me$W zrBc+)j%8R};$DG4s{CCqOX2ofm@4ffONPQ2#sN>bFIjHM5z>os)Vm_j+4)Xx2Zb*^XYEG~xH+GIq%9`MWh z6bD3B5Xo2d&|V~{@3-jFov&lF<5YaarRd)BT=n2DSv^0vr#If4Fs3$5a%7BND#ore z{NIdgeouc%1^j`biS0Zo!9`k`)pg31*F@>{G((j1 zcbZ55oJKv%oi_uMut4-7e*QHJfX{0uNOx$*Ne$Wh>37!J$S2JBXGwVmr{4eozlaTz z7+ilIA-{zl0dkB?50%(qCsjs8ajj5tw=Cyyd)#0~#jIx=Ev#A# zZ4_B8$M0sV_13z`9$Ms!t~UTcm*o})^S)*VjIwXs(86sDf9_f|Y&sd%IDWh#09T6F zGLtZWV|HX}wbEs)4HRrV%WQCD+KR4cJ4~!)Lw-HVjyq96i4*G?^)MGLY^-Q4OgNRC zEZgOrfWt$BgJdZ>!B)n})^o<2)N%@QmGMlkmGA)4jYGiL53bB=r}BuQ(v1U`BZvRQ zAe&#Q;U1pc#P(V7IiFt=62vVM>i@4WajqBjz5YH&4dw(jhJGccCB(Pl&ca z3b>hb`XVZ3Q%3D)06i|O3E}3j{_uB{8~kRHWwe$f6$c-QHP~N_9LsHeu=Lt(bCamz z_5uER*RwEu^xAa<1MoJ2wUL$KUYLhpST zUViy5HmmUNtpmi|%JwkSgQ9ShHpwd-zEDN-b&BPlBJ6g&gu*_}}h z_Oa}S44Lp?-p2SntoQ9l7dUh_tI6)WUDHYXwy=;8O3k>FgmR@2dH5I`P@BINzRD&p zHwnBcj&yvzW|lO1Uyqpsy4@DXzGmQ>mUi|&9R9J3u2!t{{!OO0 zwO8AGtT%V8<0Y*t?sI3KiYpZxDX&~IU(=RI4cB-Zqjba`DGiq3% zk2hNVoXLm%K~)sT@a)LTz$+=4Kb>bmUdyqJVOwgRrcVdoB-=C`9A0%U#k1>-}g-6GP?T>)z_S!H2_f`XVh_Sp;#_X|eHP|XM_ z&60`PV(w_vXzt~lM_Rd0Xhh?;W316;`|Ua$%5@v|dCG6HSXjrOUI^Nanx+}b&6~B& zKG?I3PBo@DnMQ!Xpw9_x6h*?VWjqks64p){c)@=n2BMHZqFcsB+o>Xme}^*|FpN2~c|wi&p-T3{iVnJ7d!E+Mjf5AS+ot~)=m{70)@;SNzbokX z98B_F#@1u|jrjMyzZG)yZwc$Y&9wP;#&bI447PIyWro63vSn+HqqH8b;3BaY!a0Y4 z8*x#Lyp)g-KB2zr#NNZq{%S*$R<_8wZOvD*W-E4S+vjHLnFmbrFjRV^t=}Bo)(q0- z8)}r|=ch7qXwHW6X={G@%7+1Q^X19bhu*<^nESRjwmm-{+8k-K zZKrbbeaZEyWa>ex?J&DrjH%?%OIW9a__`3U6d|%=>eub& zmap7kG8cADKE{J_CV7mh&(ULS_72x)ivt|buqFDz?xAg|gnl39`e!zXrRk~UIh(D6 zDn~gR#FnQ{>!wx6?2qJGZaw4ZRAL?j&qbtTf*Lteh(U)KlJEeAh`XV~3m26c?h402 z4l@J*fCd^$P$5N03nz>WpaBh$$^%XT7eIn2F`3dP~n`rAiJGi&bb%PRh)A$oU0765jy8aDitAAyeKcsl~>+K zYoy)TvvrR@m6wX5wZ+)Ksf@N(D4FElk9F#v8-3IIFHZ;gyjagXGK;n&?dQQHFT*G) zp@=nGu@IO_7J^l#vY%G?R8s5TpAFtqS#`XJk+yQAEu&uxR$0fxY^~k8ah$fITxPqK z7$C7NTQs(4KN$81SqBNZJVjoXCR*h~w=(kVr!_^|nq922^^W$*fJxqyCLnVw2GruX){j)0z5-#nPj>022jHrymRei7?;>5vym&29> zA81QONoc;?IMrqV05ZqG3A|4=;~5WiCSA5tAZ!z|S{YHMNbH^#nLNf?q8iOi?btx$ zxW6j610Tkf@6ZJz1DCjRplgeusO>bd^$kH}yVneFkH|g3qDpKnI=gFduSw#8m-C6) zvc(-=IyjIyxI}LiV|}ge@R75_-QlxrDD6!oJrS`fo+oxuPfS=);j{F_JLukeuf|o> z;uA_w)W25HeUDv!2$$rLSAU4T|Ik$dBGrIMPbv__BpZkU=MJgRl;{Y6kEKspJ6P-m zYD&KVNOv1{`28(1j_yw`pOQCO0oq060iX`Gw0<;ZaL1fB%ho%#vcE1NKfrIiMQ-F=#%lGk!S;vtQ zd~!gHGJ4)%@&2|)T42lf36YZd_fPcJIawuz?}yimXOnp)-1R>Y^^!jLW1?OK2+S;M zUya_iL4R`nIwHX(|ERE``u8v&7D;of^R;S~r~|2^$rSTK5~@4>9CNW88DkPb3%Hv$ z(PX!f^9TXp^8`Ki{Qxwoe5Q^guH7lQ{eI2PYQH+H;BmVF*P?3LOTjP>1wsY0c*78} z7z@%QT|zy^?t=5d0tNrrHjH6H6kqXzc%>psi~6?=Rqc_{I?HGh+X$$4--yHd7XQI{ z!1ru!IBw0~y1%me_)QEWB$BXAc#juks*4kZU)-yC=+u~`HCK(~KQnp;i}OVzvR0*$ zSLn10H|X-=d?gf?%om1GFb|98}iiS0Di& zc5}jUq5=gXr{*7&P-r_`#R`LkmR~a}AZJrVFcvH!kYo}HUh8bs-P3^f2YDwHQG)Ga zym}2LdOoxb4W-H=b6h=~FV;a|lF1d>x%dz!RjJj&EZnJ`O9L8s=}c$DZrYi>giI{q z3MUP4$?^qQ9rFxO6s;ufJNNx!$k14HZ+-<(RiHIEkBd>ST#XCh9~}WP2@s&nm@yo< z61I%*u}X$v_Uy!Ft4Zc!%p+P1kZB|sNfmGPB%BAi>#J5kE7^u76cAvBNInOrn*g7Y z_3OEcA{>J*(Bl;s!@3u1Op?C43=yYum}&I}Vz66$^FJzc2eZJ(<%Ud!CAfX$h6G(J zvPkx~t_9g+GmtuGr%LAE^0$tF%2-?2ZM50NcOL;&$)p_2teFeB#CX(;QsW~WJ`llA zq;RX~Y6WQ7I{@7P--1_jB3JzW{GZQtqFmk+S-C!N67#Ogy-?59+=eqrtMz6I>hui}5klz%pR_MaNH4{q82#(Do$H|~2p^bh&k zJpq*ec<`QYhx{ED1N_&~AXbtK1+RoqWM;){IyKl0`r&n?Bp!j9((0;mSr87z&{Wr=gG=49=+t9jF*gLocS%l$N!KDcz5 zG)`o2R^WLDc%S>J<}sxFsnIttB|_OZCFB_8luOlA&RCe`_H z(#9H+|CU5_o@IjqniBYSUc42`={RX9 zJyA_W!@BD8E3C(6Ykj!Y^;h?|CtqU!2Pb=3?CeYO^xw?b|3P>AgIe6z>+^qYv;XcL z@1eN;C+7Q}(*f+(UJxzf6$(rL;3RyC$?s>d@!x|h10zuM1eZWXc*|qo$cTZxiXEgELT`+lc%;fi&1}DaTgi zBUR9>`~k@%e48_$AE@&O(vf>lQEs)`vb(Fy24!z{vE{j)8yH!yp)`UQktK#i3Ar-; zWz8v{KTR`DDzSCy^t4@5@Sa6995A%cyG7i;nDZZM*!_zW?-7r^ z??3w=hXDL@RFK?C3JK@*;0!5>t5#*eD$B#`Tq7I=L-~ZPMP8rD)NJjh2yZ+Jl?!^2 zoe|jcmb|kM$~l2mPbSpz9pYCy7|9y@nC78Lx7D|tgP|rKTRi(ri-d$j1yg(4`(TK& jga6KW;haPimS=FEJ$$DTb?lRHPHu60L;yf>C9N(ZfeuTd literal 0 HcmV?d00001 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..b1ba5fdcd42 --- /dev/null +++ b/nav2_collision_monitor/test/bags/cm_moving_obstacle/fake_cm_bag_source.py @@ -0,0 +1,114 @@ +# 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. + +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 --- + + +# - /clock: simulated time (monotonic) +# - /cmd_vel_smoothed: small forward velocity +# - /local_costmap/costmap: lethal cell near robot only from t∈[3,8]s +# +# Matches your YAML: base_frame_id: base_footprint, cost_threshold: 254, radius: 0.4 + + +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) + 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): + # 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 + + data = np.zeros((self.size_y, self.size_x), dtype=np.uint8) + + if 3.0 <= now_s <= 8.0: + # Put a lethal cell exactly at robot center (inside 0.4 m circle) + 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(): + 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..249f8b42572 --- /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 \ No newline at end of file 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..a638568e352 --- /dev/null +++ b/nav2_collision_monitor/test/collision_monitor_node_bag.cpp @@ -0,0 +1,240 @@ +// 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. + +// File: collision_monitor_node_bag_metrics.cpp +// @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. + +#include +#include +#include +#include +#include +#include +#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; + +struct Sample +{ + double t; // sim time (seconds) + double vx; // cmd_vel.linear.x + int action; // last known CM action +}; + +class MetricsCatcher : public rclcpp::Node { +public: + MetricsCatcher() + : rclcpp::Node("cm_metrics_catcher"), + got_clock_(false), got_costmap_(false), + last_action_(0) + { + // Use bag /clock as time base + this->set_parameter(rclcpp::Parameter("use_sim_time", true)); + + // Window where obstacle exists in the bag + stop_window_start_ = this->declare_parameter("stop_window_start", 3.0); + stop_window_end_ = this->declare_parameter("stop_window_end", 8.0); + + + // Velocity classification thresholds (on the topic we evaluate) + stop_thresh_ = this->declare_parameter("stop_thresh", 0.02); // |vx| <= stop => stopped + resume_thresh_ = this->declare_parameter("resume_thresh", 0.10); // vx >= resume => resumed + debounce_n_ = this->declare_parameter("debounce_n", 3); // need K consecutive samples + + // Acceptable limits for metrics + max_time_to_stop_ = this->declare_parameter("max_time_to_stop", 0.6); + min_hold_pct_ = this->declare_parameter("min_hold_pct", 0.90); + max_time_to_resume_ = this->declare_parameter("max_time_to_resume", 0.6); + max_false_stop_pct_ = this->declare_parameter("max_false_stop_pct", 0.05); + + // One-shot gates to avoid “startup STOP” counting + cm_sub_ = this->create_subscription( + "/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( + "/clock", rclcpp::QoS(1).best_effort().durability_volatile(), + [this](const rosgraph_msgs::msg::Clock &){ + if (!got_clock_) {got_clock_ = true; clock_sub_.reset();} + }); + + // Output we evaluate (by default: /cmd_vel — change via launch remap if needed) + cmd_sub_ = this->create_subscription( + "/cmd_vel", rclcpp::QoS(60), + [this](const geometry_msgs::msg::Twist & msg){ + if (!got_clock_ || !got_costmap_) {return;} + const double t = this->now().seconds(); + samples_.push_back(Sample{t, msg.linear.x, last_action_}); + }); + + // Optional: CM state for debugging / correlation (not used in assertions) + state_sub_ = this->create_subscription( + "/collision_state", rclcpp::QoS(10), + [this](const CollisionMonitorState & msg){ + last_action_ = msg.action_type; + }); + } + + // Spin until we have enough sim time (end + margin) + void run_and_collect(rclcpp::executors::SingleThreadedExecutor & exec, double margin_s = 2.0) + { + // Wait up to 5s WALL for /clock to start + 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)); + } + + // Collect until stop window is over + margin (or 30s WALL safety) + 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; + double hold_stop_pct; + double time_to_resume_s; + double false_stop_pct; + bool have_data; + }; + + Results compute() const + { + Results R{}; + R.have_data = !samples_.empty(); + + if (!R.have_data) {return R;} + + auto in_range = [&](double t0, double t1){ + std::vector out; out.reserve(samples_.size()); + for (const auto & s : samples_) {if (s.t >= t0 && s.t <= t1) {out.push_back(s);}} + return out; + }; + + 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)); // small guard band + const auto post_seg = in_range(stop_window_end_ + 0.2, samples_.back().t); // small guard band + + // First time we see K-consecutive samples meeting a predicate + auto first_transition_time = [&](const std::vector & 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::quiet_NaN(); + }; + + // 1) time-to-stop + 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::infinity() : + (t_stop_first - stop_window_start_); + + // 2) hold-stop % + 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(cnt) / static_cast(stop_seg.size()); + } else { + R.hold_stop_pct = 0.0; + } + + // 3) time-to-resume + 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::infinity() : + (t_resume_first - stop_window_end_); + + // 4) no-false-stop % outside window + const size_t clean_total = pre_seg.size() + post_seg.size(); + if (clean_total > 0) { + auto count_stopped = [&](const std::vector & 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(clean_stopped) / static_cast(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::SharedPtr cm_sub_; + rclcpp::Subscription::SharedPtr cmd_sub_; + rclcpp::Subscription::SharedPtr state_sub_; + rclcpp::Subscription::SharedPtr clock_sub_; + + // Buffers/state + std::vector 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(); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(node); + + node->run_and_collect(exec, /*margin_s=*/2.0); + + 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..6a6ee9f74f1 --- /dev/null +++ b/nav2_collision_monitor/test/collision_monitor_node_bag.launch.py @@ -0,0 +1,97 @@ +# 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. +import os +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 +from launch_testing.asserts import assertExitCodes + + +def generate_test_description(): + pkg_share = get_package_share_directory('nav2_collision_monitor') + bag_dir = os.path.join(pkg_share, 'test', 'bags', 'cm_moving_obstacle') + params_yaml = os.path.join( + pkg_share, 'test', 'collision_monitor_node_bag.yaml') + + test_exe = os.environ.get('TEST_EXECUTABLE') + if not test_exe: + raise RuntimeError('TEST_EXECUTABLE env var not set') + + 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') + + clear_rosconsole = SetEnvironmentVariable( + name='ROSCONSOLE_CONFIG_FILE', value='') + + lifecycle_mgr = Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_collision_monitor', + output='screen', + parameters=[ + {'use_sim_time': True}, + {'autostart': True}, + {'bond_timeout': 0.0}, + {'node_names': ['collision_monitor']}, + ], + ) + + collision_monitor = Node( + package='nav2_collision_monitor', + executable='collision_monitor', + name='collision_monitor', + output='screen', + parameters=[params_yaml, {'use_sim_time': True}], + ) + + bag_play = ExecuteProcess( + cmd=['ros2', 'bag', 'play', bag_dir, '--rate', '1.0', + '--read-ahead-queue-size', '1000'], + output='screen', + ) + + cm_gtest = ExecuteProcess( + cmd=[test_exe, f'--gtest_output=xml:{gtest_xml}'], + output='screen', + ) + + ld = LaunchDescription([ + clear_rosconsole, + lifecycle_mgr, + collision_monitor, + bag_play, + cm_gtest, + ReadyToTest(), + ]) + return ld, {'cm_gtest': cm_gtest} + + +class TestWaitForGTest(unittest.TestCase): + + def test_gtest_completed(self, proc_info, cm_gtest): + proc_info.assertWaitForShutdown(process=cm_gtest, timeout=120.0) + + +@launch_testing.post_shutdown_test() +class TestGTestExitCode(unittest.TestCase): + + def test_gtest_passed(self, proc_info, cm_gtest): + assertExitCodes(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/test_costmap_source.cpp b/nav2_collision_monitor/test/test_costmap_source.cpp deleted file mode 100644 index 36943006a26..00000000000 --- a/nav2_collision_monitor/test/test_costmap_source.cpp +++ /dev/null @@ -1,301 +0,0 @@ -#include -#include -#include -#include -#include -#include "nav2_msgs/msg/costmap.hpp" -#include "nav2_collision_monitor/costmap.hpp" - -// Helper: build a tiny costmap with exactly one lethal cell. -// Frame is "base_link" so TF transform is identity (keeps the test simple). -static nav2_msgs::msg::Costmap makeCostmapMsg( - const std::string & frame_id, - uint32_t w, uint32_t h, float res, - uint32_t lethal_x, uint32_t lethal_y) -{ - nav2_msgs::msg::Costmap m; - m.header.stamp = rclcpp::Clock().now(); // fresh timestamp (freshness check passes) - m.header.frame_id = frame_id; // same as base_frame_id => identity TF - m.metadata.size_x = w; - m.metadata.size_y = h; - m.metadata.resolution = res; - m.metadata.origin.position.x = 0.0; - m.metadata.origin.position.y = 0.0; - m.data.assign(w * h, 0); - m.data[lethal_y * w + lethal_x] = 254; // mark one lethal cell - return m; -} - -TEST(CostmapSource, LethalCellProducesExactlyOnePoint) -{ - // Node / TF setup - auto node = std::make_shared("cm_test_node"); - auto tf_buffer = std::make_shared(node->get_clock()); - auto tf_listener = std::make_shared(*tf_buffer); - - // Construct the class under test (regular class, not pluginlib) - nav2_collision_monitor::CostmapSource src( - node, // Lifecycle node - "grid", // source_name (param prefix) - tf_buffer, // TF buffer - "base_link", // base_frame_id - "base_link", // global_frame_id (unused here) - tf2::durationFromSec(0.1), - rclcpp::Duration::from_seconds(1.0), // source_timeout - false); - - // Declare parameters the source expects - node->declare_parameter("grid.topic", "/test_costmap"); - node->declare_parameter("grid.enabled", true); - node->declare_parameter("grid.cost_threshold", 254); - node->declare_parameter("grid.source_timeout", 1.0); - - // Configure (creates the subscription) - src.configure(); - - // Publisher + executor (to drive callbacks) - auto pub_node = std::make_shared("pub_node_costmap_ok"); // <-- plain node - auto pub = pub_node->create_publisher("/test_costmap", 10); - rclcpp::executors::SingleThreadedExecutor exec; - exec.add_node(node->get_node_base_interface()); // SMAP/CostmapSource node - exec.add_node(pub_node); // publisher node - - // Publish a 3x3 grid with one lethal cell at (x=1, y=0) - auto msg = makeCostmapMsg("base_link", 3, 3, 0.1f, 1, 0); - msg.header.stamp = node->now(); // align with SMAP node clock - pub->publish(msg); - - // Pump callbacks until getData() returns points or timeout - const auto start = node->now(); - const auto deadline = rclcpp::Duration::from_seconds(1.0); - while (rclcpp::ok() && (node->now() - start) < deadline) { - exec.spin_some(); - std::vector pts; - if (src.getData(node->now(), pts)) { - // Exactly one obstacle expected - ASSERT_EQ(pts.size(), 1u); - - // The cell center for (1,0) with resolution 0.1: - // x = (1 + 0.5) * 0.1 = 0.15, y = (0 + 0.5) * 0.1 = 0.05 - EXPECT_NEAR(pts[0].x, 0.15, 1e-3); - EXPECT_NEAR(pts[0].y, 0.05, 1e-3); - return; // success - } - rclcpp::sleep_for(std::chrono::milliseconds(20)); - } - FAIL() << "CostmapSource did not produce points in time"; -} - -TEST(CostmapSource, StaleMessageIsIgnored) -{ - auto node = std::make_shared("cm_test_node2"); - auto tf_buffer = std::make_shared(node->get_clock()); - auto tf_listener = std::make_shared(*tf_buffer); - - nav2_collision_monitor::CostmapSource src( - node, "grid", tf_buffer, "base_link", "base_link", - tf2::durationFromSec(0.1), - rclcpp::Duration::from_seconds(0.01), // very short timeout - false); - - node->declare_parameter("grid.topic", "/test_costmap2"); - node->declare_parameter("grid.enabled", true); - node->declare_parameter("grid.cost_threshold", 254); - node->declare_parameter("grid.source_timeout", 0.01); - - src.configure(); - - auto pub_node = std::make_shared("pub_node_costmap_stale"); // <-- plain node - auto pub = pub_node->create_publisher("/test_costmap2", 10); - rclcpp::executors::SingleThreadedExecutor exec; - exec.add_node(node->get_node_base_interface()); - exec.add_node(pub_node); - - // Publish a stale message (timestamp way in the past) - auto msg = makeCostmapMsg("base_link", 2, 2, 0.1f, 0, 0); - msg.header.stamp = node->now() - rclcpp::Duration::from_seconds(10.0); - pub->publish(msg); - - // Spin once to deliver the stale message, then ensure getData() rejects it - exec.spin_some(); - std::vector pts; - EXPECT_FALSE(src.getData(node->now(), pts)); // should be ignored due to freshness check -} - -TEST(CostmapSource, RespectsCostThreshold) -{ - // Single publisher node shared by both runs (plain rclcpp node; no lifecycle) - auto pub_node = std::make_shared("pub_node_costmap_thresh"); - auto pub = pub_node->create_publisher("/th_costmap", 10); - - // Helper to publish the same 3x1 costmap: [254, 253, 0] - auto make_msg = [](const rclcpp::Time & stamp) { - nav2_msgs::msg::Costmap m = makeCostmapMsg("base_link", 3, 1, 0.1f, 0, 0); - m.data[0] = 254; // lethal - m.data[1] = 253; // inscribed - m.data[2] = 0; - m.header.stamp = stamp; - return m; - }; - - // ---------- Run A: threshold = 254 -> expect 1 point ---------- - { - auto node = std::make_shared("cm_thresh_node_A"); - auto tf_buffer = std::make_shared(node->get_clock()); - auto tf_listener = std::make_shared(*tf_buffer); - - // Set params BEFORE configure() - node->declare_parameter("grid.topic", "/th_costmap"); - node->declare_parameter("grid.enabled", true); - node->declare_parameter("grid.source_timeout", 1.0); - node->declare_parameter("grid.cost_threshold", 254); - - nav2_collision_monitor::CostmapSource src( - node, "grid", tf_buffer, - "base_link", "base_link", - tf2::durationFromSec(0.05), - rclcpp::Duration::from_seconds(1.0), - false); - - src.configure(); - - rclcpp::executors::SingleThreadedExecutor exec; - exec.add_node(node->get_node_base_interface()); - - auto msg = make_msg(node->now()); - pub->publish(msg); - - auto start = node->now(); - bool ok = false; - while ((node->now() - start) < rclcpp::Duration::from_seconds(1.0)) { - exec.spin_some(); - std::vector pts; - if (src.getData(node->now(), pts)) { - ASSERT_EQ(pts.size(), 1u); - ok = true; - break; - } - rclcpp::sleep_for(std::chrono::milliseconds(20)); - } - ASSERT_TRUE(ok) << "threshold=254 case didn’t produce points"; - } - - // ---------- Run B: threshold = 253 -> expect 2 points ---------- - { - auto node = std::make_shared("cm_thresh_node_B"); - auto tf_buffer = std::make_shared(node->get_clock()); - auto tf_listener = std::make_shared(*tf_buffer); - - // Set params BEFORE configure() - node->declare_parameter("grid.topic", "/th_costmap"); - node->declare_parameter("grid.enabled", true); - node->declare_parameter("grid.source_timeout", 1.0); - node->declare_parameter("grid.cost_threshold", 253); - - nav2_collision_monitor::CostmapSource src( - node, "grid", tf_buffer, - "base_link", "base_link", - tf2::durationFromSec(0.05), - rclcpp::Duration::from_seconds(1.0), - false); - - src.configure(); - - rclcpp::executors::SingleThreadedExecutor exec; - exec.add_node(node->get_node_base_interface()); - - auto msg = make_msg(node->now()); - pub->publish(msg); - - auto start = node->now(); - bool ok = false; - while ((node->now() - start) < rclcpp::Duration::from_seconds(1.0)) { - exec.spin_some(); - std::vector pts; - if (src.getData(node->now(), pts)) { - ASSERT_EQ(pts.size(), 2u); - ok = true; - break; - } - rclcpp::sleep_for(std::chrono::milliseconds(20)); - } - ASSERT_TRUE(ok) << "threshold=253 case didn’t produce points"; - } -} - - -#include - -TEST(CostmapSource, TransformsFromGlobalToBase) -{ - auto node = std::make_shared("cm_tf_node"); - auto tf_buffer = std::make_shared(node->get_clock()); - auto tf_listener = std::make_shared(*tf_buffer); - - // Inject a static transform: map -> base_link is a +0.5m x-translation - geometry_msgs::msg::TransformStamped tf; - tf.header.stamp = node->now(); - tf.header.frame_id = "map"; - tf.child_frame_id = "base_link"; - tf.transform.translation.x = 0.5; - tf.transform.translation.y = 0.0; - tf.transform.translation.z = 0.0; - tf.transform.rotation.x = 0.0; - tf.transform.rotation.y = 0.0; - tf.transform.rotation.z = 0.0; - tf.transform.rotation.w = 1.0; - ASSERT_TRUE(tf_buffer->setTransform(tf, "test_authority")); - - nav2_collision_monitor::CostmapSource src( - node, "grid", tf_buffer, - "base_link", "map", // base in base_link, message in map - tf2::durationFromSec(0.1), - rclcpp::Duration::from_seconds(1.0), - false); - - node->declare_parameter("grid.topic", "/tf_costmap"); - node->declare_parameter("grid.enabled", true); - node->declare_parameter("grid.cost_threshold", 254); - node->declare_parameter("grid.source_timeout", 1.0); - - src.configure(); - - auto pub_node = std::make_shared("pub_node_costmap_tf"); // <-- plain node - auto pub = pub_node->create_publisher("/tf_costmap", 10); - - rclcpp::executors::SingleThreadedExecutor exec; - exec.add_node(node->get_node_base_interface()); - exec.add_node(pub_node); - - // Build a 1x1 costmap with a lethal cell at (0,0) in the MAP frame, - // resolution 1.0, origin (0,0) => center at (0.5, 0.5) in MAP. - nav2_msgs::msg::Costmap msg = makeCostmapMsg("map", 1, 1, 1.0f, 0, 0); - msg.data[0] = 254; - msg.header.stamp = node->now(); // align with SMAP node clock - pub->publish(msg); - - // After transform map->base_link: (0.5, 0.5) - (0.5, 0) = (0.0, 0.5) - const auto start = node->now(); - while ((node->now() - start) < rclcpp::Duration::from_seconds(1.0)) { - exec.spin_some(); - std::vector pts; - if (src.getData(node->now(), pts)) { - ASSERT_EQ(pts.size(), 1u); - EXPECT_NEAR(pts[0].x, 0.0, 1e-3); - EXPECT_NEAR(pts[0].y, 0.5, 1e-3); - return; - } - rclcpp::sleep_for(std::chrono::milliseconds(20)); - } - FAIL() << "No transformed point arrived in time"; -} - - -// --- add this at the very end of test_costmap_source.cpp --- -int main(int argc, char ** argv) { - rclcpp::init(argc, argv); // initialize ROS 2 context (fixes "context is null") - ::testing::InitGoogleTest(&argc, argv); // initialize gtest - int ret = RUN_ALL_TESTS(); // run tests - rclcpp::shutdown(); // cleanly shutdown ROS 2 - return ret; -} From 7bb7663b819d00ac7b2db30f8c6eb51fb2aa399e Mon Sep 17 00:00:00 2001 From: Mengting Yang Date: Wed, 22 Oct 2025 22:16:41 -0700 Subject: [PATCH 3/6] modified: nav2_collision_monitor/README.md Signed-off-by: Mengting Yang --- nav2_collision_monitor/README.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md index a34976cce23..c6b681e82d2 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. + + ### Design From 42032838a933d4fa8d1faeec45fa3110d2e7b4e5 Mon Sep 17 00:00:00 2001 From: Mengting Yang Date: Wed, 22 Oct 2025 23:43:48 -0700 Subject: [PATCH 4/6] - annotate files to indicate AI-assisted drafts Signed-off-by: Mengting Yang --- .../test/bags/cm_moving_obstacle/fake_cm_bag_source.py | 1 + nav2_collision_monitor/test/collision_monitor_node_bag.cpp | 3 ++- .../test/collision_monitor_node_bag.launch.py | 2 ++ 3 files changed, 5 insertions(+), 1 deletion(-) 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 index b1ba5fdcd42..bf4e50346b5 100644 --- 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 @@ -10,6 +10,7 @@ # 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 diff --git a/nav2_collision_monitor/test/collision_monitor_node_bag.cpp b/nav2_collision_monitor/test/collision_monitor_node_bag.cpp index a638568e352..50295b2509a 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_bag.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_bag.cpp @@ -12,7 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -// File: collision_monitor_node_bag_metrics.cpp +// 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. diff --git a/nav2_collision_monitor/test/collision_monitor_node_bag.launch.py b/nav2_collision_monitor/test/collision_monitor_node_bag.launch.py index 6a6ee9f74f1..ae3603f2d60 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_bag.launch.py +++ b/nav2_collision_monitor/test/collision_monitor_node_bag.launch.py @@ -10,6 +10,8 @@ # 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 import unittest From fc27b4c916cf3d6b03527e5204dee2055dc41840 Mon Sep 17 00:00:00 2001 From: lotusymt Date: Fri, 31 Oct 2025 03:21:19 +0000 Subject: [PATCH 5/6] collision_monitor: fix linter and address issues in the comment Signed-off-by: lotusymt --- nav2_collision_monitor/CMakeLists.txt | 2 +- nav2_collision_monitor/README.md | 2 +- .../nav2_collision_monitor/costmap.hpp | 81 ++++++++++++++++--- nav2_collision_monitor/package.xml | 1 - .../params/collision_monitor_params.yaml | 12 +-- nav2_collision_monitor/src/costmap.cpp | 34 +++----- .../cm_moving_obstacle/fake_cm_bag_source.py | 2 - .../test/collision_monitor_node_bag.cpp | 1 - 8 files changed, 92 insertions(+), 43 deletions(-) diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt index bfd7e01e921..e4243de004d 100644 --- a/nav2_collision_monitor/CMakeLists.txt +++ b/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) diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md index c6b681e82d2..b8722a96f7c 100644 --- a/nav2_collision_monitor/README.md +++ b/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 diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/costmap.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/costmap.hpp index 6bf4068bdf4..7ad42ade6e9 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/costmap.hpp +++ b/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 #include #include + #include "nav2_collision_monitor/source.hpp" #include "nav2_msgs/msg/costmap.hpp" #include -#include +#include 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 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 & 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::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::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 diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index d80ff4951fc..6623ae712a1 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -27,7 +27,6 @@ nav2_ros_common point_cloud_transport point_cloud_transport_plugins - ament_cmake_gtest ament_lint_common diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml index 7b6f3525f8d..f9f8b7c7efc 100644 --- a/nav2_collision_monitor/params/collision_monitor_params.yaml +++ b/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 diff --git a/nav2_collision_monitor/src/costmap.cpp b/nav2_collision_monitor/src/costmap.cpp index c367ea5979f..7babf853e5d 100644 --- a/nav2_collision_monitor/src/costmap.cpp +++ b/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 #include #include #include #include -#include #include +#include +#include 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( source_topic, std::bind(&CostmapSource::dataCallback, this, std::placeholders::_1), - qos); + nav2::qos::StandardTopicQoS()); } bool CostmapSource::getData( const rclcpp::Time & curr_time, std::vector & 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(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(unk_name, true); } void CostmapSource::dataCallback(nav2_msgs::msg::Costmap::ConstSharedPtr msg) 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 index bf4e50346b5..2ccd6d47feb 100644 --- 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 @@ -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) diff --git a/nav2_collision_monitor/test/collision_monitor_node_bag.cpp b/nav2_collision_monitor/test/collision_monitor_node_bag.cpp index 50295b2509a..7edf901c64d 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_bag.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_bag.cpp @@ -52,7 +52,6 @@ class MetricsCatcher : public rclcpp::Node { stop_window_start_ = this->declare_parameter("stop_window_start", 3.0); stop_window_end_ = this->declare_parameter("stop_window_end", 8.0); - // Velocity classification thresholds (on the topic we evaluate) stop_thresh_ = this->declare_parameter("stop_thresh", 0.02); // |vx| <= stop => stopped resume_thresh_ = this->declare_parameter("resume_thresh", 0.10); // vx >= resume => resumed From 09d2f6a339870d17387a9a25a38063edae1421c9 Mon Sep 17 00:00:00 2001 From: lotusymt Date: Mon, 3 Nov 2025 00:04:45 +0000 Subject: [PATCH 6/6] nav2_collision_monitor: fix lint, coverage, and collision_monitor_node_test.cpp Signed-off-by: lotusymt --- .../launch/collision_detector_node.launch.py | 3 +- .../launch/collision_monitor_node.launch.py | 2 +- nav2_collision_monitor/package.xml | 2 +- .../src/collision_monitor_node.cpp | 34 ++++---- nav2_collision_monitor/src/costmap.cpp | 7 +- nav2_collision_monitor/test/CMakeLists.txt | 1 - .../cm_moving_obstacle/fake_cm_bag_source.py | 26 ++++--- .../bags/cm_moving_obstacle/metadata.yaml | 2 +- .../test/collision_monitor_node_bag.cpp | 78 ++++++++++++------- .../test/collision_monitor_node_bag.launch.py | 53 +++++++++---- .../test/collision_monitor_node_test.cpp | 4 + 11 files changed, 135 insertions(+), 77 deletions(-) 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 6623ae712a1..956119364d5 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -34,7 +34,7 @@ launch_testing_ament_cmake rosgraph_msgs - + ament_cmake diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 0384809fac9..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" @@ -385,7 +385,7 @@ bool CollisionMonitor::configureSources( src->configure(); sources_.push_back(src); - } else { // Error if something else + } else { // Error if something else RCLCPP_ERROR( get_logger(), "[%s]: Unknown source type: %s", @@ -675,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 index 7babf853e5d..6eba5161053 100644 --- a/nav2_collision_monitor/src/costmap.cpp +++ b/nav2_collision_monitor/src/costmap.cpp @@ -76,12 +76,17 @@ bool CostmapSource::getData( } 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 @@ -138,4 +143,4 @@ 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 4aae78f889d..22321bdc926 100644 --- a/nav2_collision_monitor/test/CMakeLists.txt +++ b/nav2_collision_monitor/test/CMakeLists.txt @@ -85,4 +85,3 @@ add_launch_test(collision_monitor_node_bag.launch.py WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} ENV TEST_EXECUTABLE=$ ) - 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 index 2ccd6d47feb..fe979c589cb 100644 --- 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 @@ -22,20 +22,22 @@ from rosgraph_msgs.msg import Clock as ClockMsg from std_msgs.msg import Header - # --- standardized imports --- - - -# - /clock: simulated time (monotonic) -# - /cmd_vel_smoothed: small forward velocity -# - /local_costmap/costmap: lethal cell near robot only from t∈[3,8]s +# 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 # -# Matches your YAML: base_frame_id: base_footprint, cost_threshold: 254, radius: 0.4 +# 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): + def __init__(self) -> None: super().__init__('fake_cm_bag_source') # Publishers @@ -57,7 +59,7 @@ def __init__(self): self.timer = self.create_timer(0.05, self.tick) # 20 Hz - def tick(self): + def tick(self) -> None: # 1) /clock # in tick() now_ns = time.monotonic_ns() - self.t0_ns @@ -87,10 +89,12 @@ def tick(self): 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: - # Put a lethal cell exactly at robot center (inside 0.4 m circle) + # 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 @@ -99,7 +103,7 @@ def tick(self): self.costmap_pub.publish(cm) -def main(): +def main(args: list[str] | None = None) -> None: rclpy.init() node = FakeCMSource() try: diff --git a/nav2_collision_monitor/test/bags/cm_moving_obstacle/metadata.yaml b/nav2_collision_monitor/test/bags/cm_moving_obstacle/metadata.yaml index 249f8b42572..d75e8032582 100644 --- a/nav2_collision_monitor/test/bags/cm_moving_obstacle/metadata.yaml +++ b/nav2_collision_monitor/test/bags/cm_moving_obstacle/metadata.yaml @@ -85,4 +85,4 @@ rosbag2_bagfile_information: nanoseconds: 11500531887 message_count: 693 custom_data: ~ - ros_distro: rolling \ No newline at end of file + 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 index 7edf901c64d..8697948b9e2 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_bag.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_bag.cpp @@ -1,7 +1,7 @@ // 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 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 @@ -11,11 +11,12 @@ // 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 #include @@ -31,11 +32,12 @@ 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 - int action; // last known CM action + double vx; // cmd_vel.linear.x (what robot actually commanded) + int action; // last known CM action (for debugging only) }; class MetricsCatcher : public rclcpp::Node { @@ -45,25 +47,32 @@ class MetricsCatcher : public rclcpp::Node { got_clock_(false), got_costmap_(false), last_action_(0) { - // Use bag /clock as time base + // We expect /clock from the bag → run in sim time this->set_parameter(rclcpp::Parameter("use_sim_time", true)); - // Window where obstacle exists in the bag + // Window where the bag has the obstacle (must match fake/bag) stop_window_start_ = this->declare_parameter("stop_window_start", 3.0); stop_window_end_ = this->declare_parameter("stop_window_end", 8.0); - // Velocity classification thresholds (on the topic we evaluate) - stop_thresh_ = this->declare_parameter("stop_thresh", 0.02); // |vx| <= stop => stopped - resume_thresh_ = this->declare_parameter("resume_thresh", 0.10); // vx >= resume => resumed - debounce_n_ = this->declare_parameter("debounce_n", 3); // need K consecutive samples + // 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("stop_thresh", 0.02); // |vx| <= stop + resume_thresh_ = this->declare_parameter( + "resume_thresh", 0.10); // vx >= resume + + debounce_n_ = this->declare_parameter( + "debounce_n", 3); // need K in arow - // Acceptable limits for metrics + // Tolerances for this test. If CM is slower/worse → test fails. max_time_to_stop_ = this->declare_parameter("max_time_to_stop", 0.6); min_hold_pct_ = this->declare_parameter("min_hold_pct", 0.90); max_time_to_resume_ = this->declare_parameter("max_time_to_resume", 0.6); max_false_stop_pct_ = this->declare_parameter("max_false_stop_pct", 0.05); - // One-shot gates to avoid “startup STOP” counting + // 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( "/local_costmap/costmap", rclcpp::QoS(1).reliable().durability_volatile(), [this](const nav2_msgs::msg::Costmap &){ @@ -76,16 +85,19 @@ class MetricsCatcher : public rclcpp::Node { if (!got_clock_) {got_clock_ = true; clock_sub_.reset();} }); - // Output we evaluate (by default: /cmd_vel — change via launch remap if needed) + // This is the *output* we evaluate. In the launch file you can remap it. cmd_sub_ = this->create_subscription( "/cmd_vel", rclcpp::QoS(60), [this](const geometry_msgs::msg::Twist & msg){ - if (!got_clock_ || !got_costmap_) {return;} + 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: CM state for debugging / correlation (not used in assertions) + // Optional: subscribe to CM state to help debugging (not used in asserts) state_sub_ = this->create_subscription( "/collision_state", rclcpp::QoS(10), [this](const CollisionMonitorState & msg){ @@ -93,10 +105,10 @@ class MetricsCatcher : public rclcpp::Node { }); } - // Spin until we have enough sim time (end + margin) + // Spin until we passed the obstacle window (8s) + margin void run_and_collect(rclcpp::executors::SingleThreadedExecutor & exec, double margin_s = 2.0) { - // Wait up to 5s WALL for /clock to start + // 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(); @@ -104,7 +116,8 @@ class MetricsCatcher : public rclcpp::Node { std::this_thread::sleep_for(std::chrono::milliseconds(10)); } - // Collect until stop window is over + margin (or 30s WALL safety) + // 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(); @@ -117,10 +130,10 @@ class MetricsCatcher : public rclcpp::Node { // ----- Metrics we keep ----- struct Results { - double time_to_stop_s; - double hold_stop_pct; - double time_to_resume_s; - double false_stop_pct; + 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; }; @@ -131,17 +144,20 @@ class MetricsCatcher : public rclcpp::Node { if (!R.have_data) {return R;} + // helper: pick samples in a time range auto in_range = [&](double t0, double t1){ std::vector 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)); // small guard band - const auto post_seg = in_range(stop_window_end_ + 0.2, samples_.back().t); // small guard band + 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); - // First time we see K-consecutive samples meeting a predicate + // helper: find first time we saw K consecutive samples matching a predicate auto first_transition_time = [&](const std::vector & seg, bool to_stop, double thr, int k)->double { int run = 0; @@ -153,13 +169,13 @@ class MetricsCatcher : public rclcpp::Node { return std::numeric_limits::quiet_NaN(); }; - // 1) time-to-stop + // 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::infinity() : (t_stop_first - stop_window_start_); - // 2) hold-stop % + // 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;}} @@ -168,13 +184,13 @@ class MetricsCatcher : public rclcpp::Node { R.hold_stop_pct = 0.0; } - // 3) time-to-resume + // 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::infinity() : (t_resume_first - stop_window_end_); - // 4) no-false-stop % outside window + // 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 & seg){ @@ -231,8 +247,10 @@ TEST(CollisionMonitorNodeBag, TrajectoryAndMetrics) 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); diff --git a/nav2_collision_monitor/test/collision_monitor_node_bag.launch.py b/nav2_collision_monitor/test/collision_monitor_node_bag.launch.py index ae3603f2d60..0c7c601141f 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_bag.launch.py +++ b/nav2_collision_monitor/test/collision_monitor_node_bag.launch.py @@ -1,9 +1,9 @@ # 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, @@ -13,6 +13,7 @@ # 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 @@ -21,41 +22,52 @@ from launch_ros.actions import Node import launch_testing from launch_testing.actions import ReadyToTest -from launch_testing.asserts import assertExitCodes -def generate_test_description(): +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}, - {'autostart': True}, - {'bond_timeout': 0.0}, + {'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', @@ -64,36 +76,51 @@ def generate_test_description(): 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(), + 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): - - def test_gtest_completed(self, proc_info, cm_gtest): + # 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) -@launch_testing.post_shutdown_test() +# 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 - def test_gtest_passed(self, proc_info, cm_gtest): - assertExitCodes(proc_info, process=cm_gtest) + # 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_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);