diff --git a/src/obstacle_scanner/CMakeLists.txt b/src/obstacle_scanner/CMakeLists.txt index 9265707..9c76410 100644 --- a/src/obstacle_scanner/CMakeLists.txt +++ b/src/obstacle_scanner/CMakeLists.txt @@ -20,9 +20,13 @@ rosidl_generate_interfaces(${PROJECT_NAME} DEPENDENCIES std_msgs ) -add_executable(obstacle_scanner_node src/obstacle_scanner_node.cpp) +add_executable(obstacle_scanner_node + src/obstacle_scanner_node.cpp + src/detection.cpp + src/tracking.cpp +) target_compile_features(obstacle_scanner_node PUBLIC cxx_std_17) -target_include_directories(obstacle_scanner_node PUBLIC ${EIGEN3_INCLUDE_DIRS}) +target_include_directories(obstacle_scanner_node PUBLIC include ${EIGEN3_INCLUDE_DIRS}) ament_target_dependencies(obstacle_scanner_node rclcpp sensor_msgs @@ -47,9 +51,17 @@ ament_export_dependencies(rosidl_default_runtime) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_gtest REQUIRED) set(ament_cmake_copyright_FOUND TRUE) set(ament_cmake_cpplint_FOUND TRUE) + set(ament_cmake_uncrustify_FOUND TRUE) ament_lint_auto_find_test_dependencies() + + ament_add_gtest(test_tracking test/test_tracking.cpp src/tracking.cpp) + target_include_directories(test_tracking PUBLIC include ${EIGEN3_INCLUDE_DIRS}) + + ament_add_gtest(test_detection test/test_detection.cpp src/detection.cpp) + target_include_directories(test_detection PUBLIC include ${EIGEN3_INCLUDE_DIRS}) endif() ament_package() diff --git a/src/obstacle_scanner/config/params.yaml b/src/obstacle_scanner/config/params.yaml index eba800c..ba5f948 100644 --- a/src/obstacle_scanner/config/params.yaml +++ b/src/obstacle_scanner/config/params.yaml @@ -1,12 +1,70 @@ obstacle_scanner: ros__parameters: + # Input LaserScan topic. scan_topic: "/scan" + + # Frame id used on published /obstacles and debug images. frame_id: "laser_frame" + + # Maximum Euclidean distance in meters between adjacent scan points before + # starting a new cluster. The detector now uses scan order, so this is O(N). cluster_gap: 0.1 + + # Drop clusters with more points than this before circle fitting. This keeps + # walls and large surfaces from becoming circular obstacles. max_cluster_points: 20 + + # Minimum accepted fitted radius in meters for 3+ point circle observations. radius_min: 0.03 + + # Maximum accepted fitted radius in meters for 3+ point circle observations. radius_max: 0.05 + + # Merge the last and first scan-order clusters when their endpoint distance + # is below cluster_gap. Useful for near-360 degree scans; harmless otherwise + # unless scan edge points are physically close. merge_wrap: true + + # Enable short-term laser-frame tracking. When false, only current-frame + # circle-fit observations are published. + enable_tracking: true + + # Number of successful updates before a track may be published. + track_confirm_hits: 1 + + # Delete a track after this many consecutive missed frames. + track_delete_misses: 2 + + # Maximum distance in meters for nearest-neighbor observation-to-track match. + association_gate: 0.25 + + # Extra tolerance in meters for accepting a two-point chord whose length is + # close to the predicted track diameter. + chord_tolerance: 0.02 + + # Weight for center updates from 3+ point circle fitting. Higher values trust + # the current fit more. + fit_update_alpha: 0.7 + + # Weight for center updates from 2-point chord recovery. Lower values keep + # chord updates from pulling the track too aggressively. + chord_update_alpha: 0.3 + + # Enable debug publishers. This creates /processed_scan and, when debug_info + # is true, /obstacle_scanner/debug_info. debug: true + + # Publish compact JSON debug information on /obstacle_scanner/debug_info. + debug_info: true + + # Debug image width and height in pixels. debug_image_size: 500 + + # Meters per pixel for the debug image. debug_resolution: 0.01 + + # Publish one debug image every N scan frames to reduce OpenCV drawing cost. + debug_image_stride: 3 + + # Publish one JSON debug message every N scan frames. + debug_info_stride: 1 diff --git a/src/obstacle_scanner/include/obstacle_scanner/detection.hpp b/src/obstacle_scanner/include/obstacle_scanner/detection.hpp new file mode 100644 index 0000000..bdd5eb5 --- /dev/null +++ b/src/obstacle_scanner/include/obstacle_scanner/detection.hpp @@ -0,0 +1,45 @@ +#pragma once + +#include "obstacle_scanner/tracking.hpp" + +#include + +#include + +namespace obstacle_scanner +{ + +struct DetectionConfig +{ + double cluster_gap{0.1}; + int max_cluster_points{20}; + double radius_min{0.03}; + double radius_max{0.05}; + bool merge_wrap{true}; +}; + +struct DetectionDebug +{ + int total_clusters{0}; + int fit_observations{0}; + int chord_observations{0}; + int discarded_large{0}; + int skipped_small{0}; + int fit_failed{0}; + int radius_rejected{0}; +}; + +std::vector> cluster_scan_order( + const std::vector & points, + double cluster_gap, + bool merge_wrap); + +std::tuple fit_circle( + const std::vector & points); + +std::vector detect_observations( + const std::vector & points, + const DetectionConfig & config, + DetectionDebug & debug); + +} // namespace obstacle_scanner diff --git a/src/obstacle_scanner/include/obstacle_scanner/tracking.hpp b/src/obstacle_scanner/include/obstacle_scanner/tracking.hpp new file mode 100644 index 0000000..9ff4894 --- /dev/null +++ b/src/obstacle_scanner/include/obstacle_scanner/tracking.hpp @@ -0,0 +1,103 @@ +#pragma once + +#include + +#include +#include +#include + +namespace obstacle_scanner +{ + +enum class ObservationType +{ + CircleFit, + TwoPointChord +}; + +enum class UpdateSource +{ + Fit, + Chord, + Predict +}; + +struct Observation +{ + ObservationType type{ObservationType::CircleFit}; + Eigen::Vector2d center{0.0, 0.0}; + Eigen::Vector2d p1{0.0, 0.0}; + Eigen::Vector2d p2{0.0, 0.0}; + double radius{0.0}; + int cluster_size{0}; +}; + +struct TrackerConfig +{ + int track_confirm_hits{1}; + int track_delete_misses{2}; + double association_gate{0.25}; + double chord_tolerance{0.02}; + double fit_update_alpha{0.7}; + double chord_update_alpha{0.3}; +}; + +struct TrackOutput +{ + int id{0}; + double center_x{0.0}; + double center_y{0.0}; + double radius{0.0}; + int hit_count{0}; + int missed_count{0}; + UpdateSource last_source{UpdateSource::Predict}; +}; + +struct FrameDebug +{ + int fit_observations{0}; + int chord_observations{0}; + int chord_updates{0}; + int chord_rejections{0}; + int active_tracks{0}; + int published_tracks{0}; +}; + +class ObstacleTracker +{ +public: + explicit ObstacleTracker(const TrackerConfig & config = TrackerConfig()); + + FrameDebug update(const std::vector & observations); + std::vector confirmed_tracks() const; + const std::vector & tracks_for_debug() const; + +private: + struct Track + { + int id{0}; + Eigen::Vector2d center{0.0, 0.0}; + double radius{0.0}; + int hit_count{0}; + int missed_count{0}; + UpdateSource last_source{UpdateSource::Predict}; + bool matched{false}; + }; + + int find_nearest_unmatched_track(const Eigen::Vector2d & center) const; + bool chord_center_for_track( + const Observation & observation, + const Track & track, + Eigen::Vector2d & center) const; + static TrackOutput to_output(const Track & track); + void refresh_debug_outputs(); + + TrackerConfig config_; + int next_id_{1}; + std::vector tracks_; + std::vector debug_outputs_; +}; + +std::string update_source_name(UpdateSource source); + +} // namespace obstacle_scanner diff --git a/src/obstacle_scanner/package.xml b/src/obstacle_scanner/package.xml index afdac89..8beca50 100644 --- a/src/obstacle_scanner/package.xml +++ b/src/obstacle_scanner/package.xml @@ -19,10 +19,11 @@ rosidl_default_generators rosidl_default_runtime - rosidl_interface_packages - ament_lint_auto ament_lint_common + ament_cmake_gtest + + rosidl_interface_packages ament_cmake diff --git a/src/obstacle_scanner/src/detection.cpp b/src/obstacle_scanner/src/detection.cpp new file mode 100644 index 0000000..c17730d --- /dev/null +++ b/src/obstacle_scanner/src/detection.cpp @@ -0,0 +1,145 @@ +#include "obstacle_scanner/detection.hpp" + +#include + +#include +#include +#include + +namespace obstacle_scanner +{ + +std::vector> cluster_scan_order( + const std::vector & points, + double cluster_gap, + bool merge_wrap) +{ + std::vector> clusters; + if (points.empty()) { + return clusters; + } + if (cluster_gap <= 0.0) { + throw std::runtime_error("cluster_gap must be positive."); + } + + clusters.push_back({0}); + for (std::size_t i = 1; i < points.size(); ++i) { + const double distance = (points[i] - points[i - 1]).norm(); + if (distance > cluster_gap) { + clusters.emplace_back(); + } + clusters.back().push_back(static_cast(i)); + } + + if (merge_wrap && clusters.size() > 1) { + const int first_index = clusters.front().front(); + const int last_index = clusters.back().back(); + const double wrap_distance = (points[first_index] - points[last_index]).norm(); + if (wrap_distance <= cluster_gap) { + auto first = clusters.front(); + clusters.back().insert(clusters.back().end(), first.begin(), first.end()); + clusters.erase(clusters.begin()); + } + } + + return clusters; +} + +std::tuple fit_circle( + const std::vector & points) +{ + const int count = static_cast(points.size()); + if (count < 3) { + throw std::runtime_error("Need at least 3 points to fit a circle."); + } + + Eigen::MatrixXd system(count, 3); + Eigen::VectorXd target(count); + for (int i = 0; i < count; ++i) { + const double x = points[static_cast(i)].x(); + const double y = points[static_cast(i)].y(); + system(i, 0) = 2.0 * x; + system(i, 1) = 2.0 * y; + system(i, 2) = 1.0; + target(i) = x * x + y * y; + } + + Eigen::JacobiSVD svd( + system, Eigen::ComputeThinU | Eigen::ComputeThinV); + if (svd.rank() < 3) { + throw std::runtime_error("Points are nearly collinear."); + } + + const Eigen::Vector3d solution = svd.solve(target); + const double center_x = solution(0); + const double center_y = solution(1); + const double constant = solution(2); + const double radius_squared = constant + center_x * center_x + center_y * center_y; + if (radius_squared <= 0.0 || !std::isfinite(radius_squared)) { + throw std::runtime_error("Circle fitting produced an invalid radius."); + } + return {center_x, center_y, std::sqrt(radius_squared)}; +} + +std::vector detect_observations( + const std::vector & points, + const DetectionConfig & config, + DetectionDebug & debug) +{ + debug = DetectionDebug(); + std::vector observations; + const auto clusters = cluster_scan_order(points, config.cluster_gap, config.merge_wrap); + debug.total_clusters = static_cast(clusters.size()); + + for (const auto & cluster : clusters) { + const int cluster_size = static_cast(cluster.size()); + if (cluster_size > config.max_cluster_points) { + debug.discarded_large++; + continue; + } + + if (cluster_size == 2) { + Observation observation; + observation.type = ObservationType::TwoPointChord; + observation.p1 = points[static_cast(cluster[0])]; + observation.p2 = points[static_cast(cluster[1])]; + observation.cluster_size = cluster_size; + observations.push_back(observation); + debug.chord_observations++; + continue; + } + + if (cluster_size < 3) { + debug.skipped_small++; + continue; + } + + std::vector cluster_points; + cluster_points.reserve(static_cast(cluster_size)); + for (const int index : cluster) { + cluster_points.push_back(points[static_cast(index)]); + } + + try { + auto [center_x, center_y, radius] = fit_circle(cluster_points); + if (radius < config.radius_min || radius > config.radius_max) { + debug.radius_rejected++; + continue; + } + + Observation observation; + observation.type = ObservationType::CircleFit; + observation.center = Eigen::Vector2d(center_x, center_y); + observation.radius = radius; + observation.cluster_size = cluster_size; + observations.push_back(observation); + debug.fit_observations++; + } catch (const std::exception &) { + debug.fit_failed++; + } + } + + return observations; +} + +} // namespace obstacle_scanner diff --git a/src/obstacle_scanner/src/obstacle_scanner_node.cpp b/src/obstacle_scanner/src/obstacle_scanner_node.cpp index 78c0784..13d4d55 100644 --- a/src/obstacle_scanner/src/obstacle_scanner_node.cpp +++ b/src/obstacle_scanner/src/obstacle_scanner_node.cpp @@ -1,184 +1,24 @@ #include -#include #include +#include +#include #include #include #include +#include "obstacle_scanner/detection.hpp" #include "obstacle_scanner/msg/obstacle.hpp" #include "obstacle_scanner/msg/obstacle_array.hpp" +#include "obstacle_scanner/tracking.hpp" #include +#include #include -#include -#include #include +#include +#include +#include -namespace obstacle_scanner -{ - -// --------------------------------------------------------------------------- -// Ported from scan_circle_demo.py:211-227 -// Least-squares algebraic circle fitting (Al-Sharadqah & Chernov) -// --------------------------------------------------------------------------- -std::tuple fit_circle(const std::vector& pts) -{ - const int n = static_cast(pts.size()); - if (n < 3) { - throw std::runtime_error("Need at least 3 points to fit a circle."); - } - - Eigen::MatrixXd A(n, 3); - Eigen::VectorXd b(n); - for (int i = 0; i < n; ++i) { - const double x = pts[i].x(); - const double y = pts[i].y(); - A(i, 0) = 2.0 * x; - A(i, 1) = 2.0 * y; - A(i, 2) = 1.0; - b(i) = x * x + y * y; - } - - Eigen::JacobiSVD svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV); - if (svd.rank() < 3) { - throw std::runtime_error("Points are nearly collinear; no stable circle exists."); - } - - Eigen::Vector3d sol = svd.solve(b); - const double cx = sol(0); - const double cy = sol(1); - const double c = sol(2); - const double r2 = c + cx * cx + cy * cy; - - if (r2 <= 0.0 || !std::isfinite(r2)) { - throw std::runtime_error("Circle fitting produced an invalid radius."); - } - - return {cx, cy, std::sqrt(r2)}; -} - -// --------------------------------------------------------------------------- -// Ported from demo4.py:149-177 -// Sort points by angle, split clusters where Euclidean gap > threshold, -// optionally merge first and last cluster across 360°. -// --------------------------------------------------------------------------- -std::vector> cluster_full_scan( - const std::vector& points, - const std::vector& point_angles, - double cluster_gap, - bool merge_wrap) -{ - const int n = static_cast(points.size()); - if (n == 0) { - return {}; - } - - // Sort indices by angle - std::vector order(n); - std::iota(order.begin(), order.end(), 0); - std::sort(order.begin(), order.end(), - [&](int a, int b) { return point_angles[a] < point_angles[b]; }); - - std::vector> clusters; - clusters.push_back({order[0]}); - - for (size_t k = 1; k < order.size(); ++k) { - int prev_idx = order[k - 1]; - int cur_idx = order[k]; - double dx = points[cur_idx].x() - points[prev_idx].x(); - double dy = points[cur_idx].y() - points[prev_idx].y(); - double dist = std::sqrt(dx * dx + dy * dy); - - if (dist > cluster_gap) { - clusters.emplace_back(); - } - clusters.back().push_back(cur_idx); - } - - // Optional wrap-around merge - if (merge_wrap && clusters.size() > 1) { - int first_idx = clusters.front().front(); - int last_idx = clusters.back().back(); - double dx = points[first_idx].x() - points[last_idx].x(); - double dy = points[first_idx].y() - points[last_idx].y(); - if (std::sqrt(dx * dx + dy * dy) <= cluster_gap) { - auto& last = clusters.back(); - const auto& first = clusters.front(); - last.insert(last.end(), first.begin(), first.end()); - clusters.erase(clusters.begin()); - } - } - - return clusters; -} - -// --------------------------------------------------------------------------- -// Ported from demo4.py:180-238 -// Cluster → fit circle → filter by radius range. -// Returns only qualified obstacles (radius_min <= R <= radius_max). -// --------------------------------------------------------------------------- -std::vector detect_circles( - const std::vector& points, - const std::vector& point_angles, - double cluster_gap, - int max_cluster_points, - double radius_min, - double radius_max, - bool merge_wrap, - int& out_total_clusters, - int& out_fitted, - int& out_discarded_large, - int& out_skipped_small, - int& out_failed) -{ - auto clusters = cluster_full_scan(points, point_angles, cluster_gap, merge_wrap); - - out_total_clusters = static_cast(clusters.size()); - - std::vector obstacles; - - for (const auto& cluster : clusters) { - const int sz = static_cast(cluster.size()); - - if (sz > max_cluster_points) { - out_discarded_large++; - continue; - } - if (sz < 3) { - out_skipped_small++; - continue; - } - - std::vector cluster_pts; - cluster_pts.reserve(sz); - for (int idx : cluster) { - cluster_pts.push_back(points[idx]); - } - - try { - auto [cx, cy, r] = fit_circle(cluster_pts); - out_fitted++; - - if (r >= radius_min && r <= radius_max) { - msg::Obstacle obs; - obs.center_x = cx; - obs.center_y = cy; - obs.radius = r; - obstacles.push_back(obs); - } - } catch (const std::exception&) { - out_failed++; - } - } - - return obstacles; -} - -} // namespace obstacle_scanner - -// ============================================================================ -// ROS2 Node -// ============================================================================ class ObstacleScannerNode : public rclcpp::Node { public: @@ -187,100 +27,163 @@ public: { using std::placeholders::_1; - // --- Parameters --- scan_topic_ = declare_parameter("scan_topic", "/scan"); frame_id_ = declare_parameter("frame_id", "laser_frame"); - cluster_gap_ = declare_parameter("cluster_gap", 0.1); - max_cluster_points_ = declare_parameter("max_cluster_points", 20); - radius_min_ = declare_parameter("radius_min", 0.03); - radius_max_ = declare_parameter("radius_max", 0.05); - merge_wrap_ = declare_parameter("merge_wrap", true); + detection_config_.cluster_gap = declare_parameter("cluster_gap", 0.1); + detection_config_.max_cluster_points = declare_parameter("max_cluster_points", 20); + detection_config_.radius_min = declare_parameter("radius_min", 0.03); + detection_config_.radius_max = declare_parameter("radius_max", 0.05); + detection_config_.merge_wrap = declare_parameter("merge_wrap", true); + + enable_tracking_ = declare_parameter("enable_tracking", true); + tracker_config_.track_confirm_hits = declare_parameter("track_confirm_hits", 1); + tracker_config_.track_delete_misses = declare_parameter("track_delete_misses", 2); + tracker_config_.association_gate = declare_parameter("association_gate", 0.25); + tracker_config_.chord_tolerance = declare_parameter("chord_tolerance", 0.02); + tracker_config_.fit_update_alpha = declare_parameter("fit_update_alpha", 0.7); + tracker_config_.chord_update_alpha = declare_parameter("chord_update_alpha", 0.3); + tracker_ = obstacle_scanner::ObstacleTracker(tracker_config_); + debug_ = declare_parameter("debug", false); + debug_info_ = declare_parameter("debug_info", true); debug_image_size_ = declare_parameter("debug_image_size", 500); debug_resolution_ = declare_parameter("debug_resolution", 0.01); + debug_image_stride_ = std::max( + 1, static_cast(declare_parameter("debug_image_stride", 3))); + debug_info_stride_ = std::max( + 1, static_cast(declare_parameter("debug_info_stride", 1))); - // --- Publishers --- obstacles_pub_ = create_publisher( - "/obstacles", 10); + "/obstacles", 10); if (debug_) { debug_pub_ = create_publisher( - "/processed_scan", 10); + "/processed_scan", 10); + if (debug_info_) { + debug_info_pub_ = create_publisher( + "/obstacle_scanner/debug_info", 10); + } } - // --- Subscriber --- scan_sub_ = create_subscription( - scan_topic_, rclcpp::SensorDataQoS(), - std::bind(&ObstacleScannerNode::scan_callback, this, _1)); + scan_topic_, rclcpp::SensorDataQoS(), + std::bind(&ObstacleScannerNode::scan_callback, this, _1)); RCLCPP_INFO(get_logger(), "ObstacleScannerNode started"); } private: - // ========================================================================== - // LaserScan callback - // ========================================================================== - void scan_callback(const sensor_msgs::msg::LaserScan::ConstSharedPtr& msg) + void scan_callback(const sensor_msgs::msg::LaserScan::ConstSharedPtr & msg) { - // 1. Filter valid ranges, convert polar → Cartesian - const double angle_increment = msg->angle_increment; + const auto start_time = std::chrono::steady_clock::now(); + frame_count_++; + std::vector points; - std::vector angles; - points.reserve(msg->ranges.size()); - angles.reserve(msg->ranges.size()); - for (size_t i = 0; i < msg->ranges.size(); ++i) { - const double r = msg->ranges[i]; - if (!std::isfinite(r) || r <= 0.0) { + for (std::size_t i = 0; i < msg->ranges.size(); ++i) { + const double range = msg->ranges[i]; + if (!std::isfinite(range) || range <= 0.0) { continue; } - const double a = msg->angle_min + static_cast(i) * angle_increment; - const double x = r * std::cos(a); - const double y = r * std::sin(a); - points.emplace_back(x, y); - angles.push_back(std::fmod(std::atan2(y, x) + 2.0 * M_PI, 2.0 * M_PI)); + const double angle = msg->angle_min + static_cast(i) * msg->angle_increment; + points.emplace_back(range * std::cos(angle), range * std::sin(angle)); } - // 2. Detect circles - int total_clusters = 0, fitted = 0, discarded_large = 0; - int skipped_small = 0, failed = 0; + obstacle_scanner::DetectionDebug detection_debug; + const auto observations = obstacle_scanner::detect_observations( + points, detection_config_, detection_debug); - auto obstacles = obstacle_scanner::detect_circles( - points, angles, - cluster_gap_, max_cluster_points_, - radius_min_, radius_max_, - merge_wrap_, - total_clusters, fitted, discarded_large, skipped_small, failed); + obstacle_scanner::FrameDebug tracker_debug; + std::vector output_tracks; + std::vector debug_tracks; + if (enable_tracking_) { + tracker_debug = tracker_.update(observations); + output_tracks = tracker_.confirmed_tracks(); + debug_tracks = tracker_.tracks_for_debug(); + } else { + output_tracks = tracks_from_fit_observations(observations); + debug_tracks = output_tracks; + tracker_debug.published_tracks = static_cast(output_tracks.size()); + tracker_debug.active_tracks = tracker_debug.published_tracks; + } - RCLCPP_DEBUG(get_logger(), - "points=%zu clusters=%d fitted=%d qualified=%zu " // 有效点数、聚类个体数、拟合圆数、障碍物数 - "discarded_large=%d skipped_small=%d failed=%d", // 过大、过小、拟合失败个数 - points.size(), total_clusters, fitted, - obstacles.size(), discarded_large, skipped_small, failed); + const double processing_ms = elapsed_ms(start_time); + + RCLCPP_DEBUG( + get_logger(), + "points=%zu clusters=%d fit=%d chord=%d tracks=%d published=%zu ms=%.3f", + points.size(), detection_debug.total_clusters, + detection_debug.fit_observations, detection_debug.chord_observations, + tracker_debug.active_tracks, output_tracks.size(), processing_ms); - // 3. Publish ObstacleArray auto out_msg = obstacle_scanner::msg::ObstacleArray(); out_msg.header.stamp = msg->header.stamp; out_msg.header.frame_id = frame_id_; - out_msg.obstacles = std::move(obstacles); + for (const auto & track : output_tracks) { + obstacle_scanner::msg::Obstacle obstacle; + obstacle.center_x = track.center_x; + obstacle.center_y = track.center_y; + obstacle.radius = track.radius; + out_msg.obstacles.push_back(obstacle); + } obstacles_pub_->publish(out_msg); - // 4. Debug image if (debug_) { - auto img_msg = render_debug_image(points, out_msg.obstacles, - msg->header.stamp); - debug_pub_->publish(*img_msg); + if (debug_pub_ && frame_count_ % static_cast(debug_image_stride_) == 0) { + auto img_msg = render_debug_image( + points, debug_tracks, detection_debug, tracker_debug, + processing_ms, msg->header.stamp); + debug_pub_->publish(*img_msg); + } + if ( + debug_info_pub_ && + frame_count_ % static_cast(debug_info_stride_) == 0) + { + std_msgs::msg::String debug_msg; + debug_msg.data = build_debug_json( + points.size(), detection_debug, tracker_debug, + debug_tracks, output_tracks.size(), processing_ms); + debug_info_pub_->publish(debug_msg); + } } } - // ========================================================================== - // Debug: render scan points + obstacles to cv::Mat, publish as Image - // ========================================================================== + static double elapsed_ms(const std::chrono::steady_clock::time_point & start_time) + { + const auto end_time = std::chrono::steady_clock::now(); + return std::chrono::duration(end_time - start_time).count(); + } + + static std::vector tracks_from_fit_observations( + const std::vector & observations) + { + std::vector tracks; + int id = 1; + for (const auto & observation : observations) { + if (observation.type != obstacle_scanner::ObservationType::CircleFit) { + continue; + } + obstacle_scanner::TrackOutput track; + track.id = id++; + track.center_x = observation.center.x(); + track.center_y = observation.center.y(); + track.radius = observation.radius; + track.hit_count = 1; + track.missed_count = 0; + track.last_source = obstacle_scanner::UpdateSource::Fit; + tracks.push_back(track); + } + return tracks; + } + sensor_msgs::msg::Image::SharedPtr render_debug_image( - const std::vector& points, - const std::vector& obstacles, - const builtin_interfaces::msg::Time& stamp) + const std::vector & points, + const std::vector & tracks, + const obstacle_scanner::DetectionDebug & detection_debug, + const obstacle_scanner::FrameDebug & tracker_debug, + double processing_ms, + const builtin_interfaces::msg::Time & stamp) { const int size = debug_image_size_; const double res = debug_resolution_; @@ -288,67 +191,125 @@ private: cv::Mat img(size, size, CV_8UC3, cv::Scalar(255, 255, 255)); - // Draw scan points in red (small filled circles) - for (const auto& pt : points) { - int col = static_cast(std::round(origin + pt.x() / res)); - int row = static_cast(std::round(origin - pt.y() / res)); + for (const auto & pt : points) { + const int col = static_cast(std::round(origin + pt.x() / res)); + const int row = static_cast(std::round(origin - pt.y() / res)); if (col >= 0 && col < size && row >= 0 && row < size) { - cv::circle(img, cv::Point(col, row), 1, - cv::Scalar(0, 0, 255), cv::FILLED); + cv::circle(img, cv::Point(col, row), 1, cv::Scalar(0, 0, 255), cv::FILLED); } } - // Draw origin (yellow square, 4x4 px) - cv::rectangle(img, cv::Point(origin - 2, origin - 2), - cv::Point(origin + 2, origin + 2), - cv::Scalar(0, 255, 255), cv::FILLED); + cv::rectangle( + img, cv::Point(origin - 2, origin - 2), + cv::Point(origin + 2, origin + 2), + cv::Scalar(0, 255, 255), cv::FILLED); - // Draw qualified obstacles in green - for (const auto& obs : obstacles) { - int col = static_cast(std::round(origin + obs.center_x / res)); - int row = static_cast(std::round(origin - obs.center_y / res)); - int r_px = std::max(1, static_cast(std::round(obs.radius / res))); - - // Circle outline - cv::circle(img, cv::Point(col, row), r_px, - cv::Scalar(0, 255, 0), 1); - - // Radius label - char buf[32]; - std::snprintf(buf, sizeof(buf), "R=%.3g", obs.radius); - cv::putText(img, buf, - cv::Point(col + r_px + 2, row), - cv::FONT_HERSHEY_SIMPLEX, 0.3, - cv::Scalar(0, 255, 0), 1); + for (const auto & track : tracks) { + draw_track(img, track, origin, res); } + char summary[160]; + std::snprintf( + summary, sizeof(summary), + "pts=%zu cls=%d fit=%d chord=%d trk=%d pub=%d ms=%.1f", + points.size(), detection_debug.total_clusters, + detection_debug.fit_observations, detection_debug.chord_observations, + tracker_debug.active_tracks, tracker_debug.published_tracks, processing_ms); + cv::putText( + img, summary, cv::Point(8, size - 12), + cv::FONT_HERSHEY_SIMPLEX, 0.32, cv::Scalar(40, 40, 40), 1); + auto image_msg = cv_bridge::CvImage( - std_msgs::msg::Header(), "bgr8", img).toImageMsg(); + std_msgs::msg::Header(), "bgr8", img).toImageMsg(); image_msg->header.stamp = stamp; image_msg->header.frame_id = frame_id_; return image_msg; } - // ---- Parameters ---- + static void draw_track( + cv::Mat & img, + const obstacle_scanner::TrackOutput & track, + int origin, + double res) + { + const int col = static_cast(std::round(origin + track.center_x / res)); + const int row = static_cast(std::round(origin - track.center_y / res)); + const int radius_px = std::max(1, static_cast(std::round(track.radius / res))); + + cv::Scalar color(160, 160, 160); + int line_type = cv::LINE_8; + if (track.last_source == obstacle_scanner::UpdateSource::Fit) { + color = cv::Scalar(0, 180, 0); + } else if (track.last_source == obstacle_scanner::UpdateSource::Chord) { + color = cv::Scalar(255, 0, 0); + } else { + line_type = cv::LINE_4; + } + + cv::circle(img, cv::Point(col, row), radius_px, color, 1, line_type); + cv::circle(img, cv::Point(col, row), 2, color, cv::FILLED); + + char label[48]; + std::snprintf( + label, sizeof(label), "#%d %s h%d/m%d", + track.id, obstacle_scanner::update_source_name(track.last_source).c_str(), + track.hit_count, track.missed_count); + cv::putText( + img, label, cv::Point(col + radius_px + 2, row), + cv::FONT_HERSHEY_SIMPLEX, 0.3, color, 1); + } + + static std::string build_debug_json( + std::size_t point_count, + const obstacle_scanner::DetectionDebug & detection_debug, + const obstacle_scanner::FrameDebug & tracker_debug, + const std::vector & tracks, + std::size_t published_count, + double processing_ms) + { + std::ostringstream out; + out << "{"; + out << "\"points\":" << point_count << ","; + out << "\"clusters\":" << detection_debug.total_clusters << ","; + out << "\"fit\":" << detection_debug.fit_observations << ","; + out << "\"chord\":" << detection_debug.chord_observations << ","; + out << "\"chord_updates\":" << tracker_debug.chord_updates << ","; + out << "\"chord_rejections\":" << tracker_debug.chord_rejections << ","; + out << "\"tracks\":" << tracker_debug.active_tracks << ","; + out << "\"published\":" << published_count << ","; + out << "\"ms\":" << processing_ms << ","; + out << "\"sources\":["; + for (std::size_t i = 0; i < tracks.size(); ++i) { + if (i > 0) { + out << ","; + } + out << "\"" << obstacle_scanner::update_source_name(tracks[i].last_source) << "\""; + } + out << "]}"; + return out.str(); + } + std::string scan_topic_; std::string frame_id_; - double cluster_gap_; - int max_cluster_points_; - double radius_min_; - double radius_max_; - bool merge_wrap_; - bool debug_; - int debug_image_size_; - double debug_resolution_; + obstacle_scanner::DetectionConfig detection_config_; + obstacle_scanner::TrackerConfig tracker_config_; + obstacle_scanner::ObstacleTracker tracker_; + bool enable_tracking_{true}; + bool debug_{false}; + bool debug_info_{true}; + int debug_image_size_{500}; + double debug_resolution_{0.01}; + int debug_image_stride_{3}; + int debug_info_stride_{1}; + std::uint64_t frame_count_{0}; - // ---- ROS2 interfaces ---- rclcpp::Subscription::SharedPtr scan_sub_; rclcpp::Publisher::SharedPtr obstacles_pub_; rclcpp::Publisher::SharedPtr debug_pub_; + rclcpp::Publisher::SharedPtr debug_info_pub_; }; -// ============================================================================ -int main(int argc, char** argv) +int main(int argc, char ** argv) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); diff --git a/src/obstacle_scanner/src/tracking.cpp b/src/obstacle_scanner/src/tracking.cpp new file mode 100644 index 0000000..0dc7ab4 --- /dev/null +++ b/src/obstacle_scanner/src/tracking.cpp @@ -0,0 +1,219 @@ +#include "obstacle_scanner/tracking.hpp" + +#include +#include +#include + +namespace obstacle_scanner +{ + +ObstacleTracker::ObstacleTracker(const TrackerConfig & config) +: config_(config) +{ +} + +FrameDebug ObstacleTracker::update(const std::vector & observations) +{ + FrameDebug debug; + for (auto & track : tracks_) { + track.matched = false; + } + + for (const auto & observation : observations) { + if (observation.type != ObservationType::CircleFit) { + continue; + } + debug.fit_observations++; + + const int track_index = find_nearest_unmatched_track(observation.center); + if (track_index >= 0) { + auto & track = tracks_[static_cast(track_index)]; + const double alpha = config_.fit_update_alpha; + track.center = (1.0 - alpha) * track.center + alpha * observation.center; + track.radius = 0.8 * track.radius + 0.2 * observation.radius; + track.hit_count++; + track.missed_count = 0; + track.last_source = UpdateSource::Fit; + track.matched = true; + continue; + } + + Track track; + track.id = next_id_++; + track.center = observation.center; + track.radius = observation.radius; + track.hit_count = 1; + track.missed_count = 0; + track.last_source = UpdateSource::Fit; + track.matched = true; + tracks_.push_back(track); + } + + for (const auto & observation : observations) { + if (observation.type != ObservationType::TwoPointChord) { + continue; + } + debug.chord_observations++; + + int best_index = -1; + double best_distance = std::numeric_limits::infinity(); + Eigen::Vector2d best_center{0.0, 0.0}; + bool had_geometry_rejection = false; + + for (std::size_t i = 0; i < tracks_.size(); ++i) { + const auto & track = tracks_[i]; + if (track.matched) { + continue; + } + + Eigen::Vector2d chord_center{0.0, 0.0}; + if (!chord_center_for_track(observation, track, chord_center)) { + had_geometry_rejection = true; + continue; + } + + const double distance = (chord_center - track.center).norm(); + if (distance < config_.association_gate && distance < best_distance) { + best_index = static_cast(i); + best_distance = distance; + best_center = chord_center; + } + } + + if (best_index >= 0) { + auto & track = tracks_[static_cast(best_index)]; + const double alpha = config_.chord_update_alpha; + track.center = (1.0 - alpha) * track.center + alpha * best_center; + track.hit_count++; + track.missed_count = 0; + track.last_source = UpdateSource::Chord; + track.matched = true; + debug.chord_updates++; + } else if (had_geometry_rejection) { + debug.chord_rejections++; + } + } + + for (auto & track : tracks_) { + if (track.matched) { + continue; + } + track.missed_count++; + track.last_source = UpdateSource::Predict; + } + + tracks_.erase( + std::remove_if( + tracks_.begin(), tracks_.end(), + [&](const Track & track) { + return track.missed_count >= config_.track_delete_misses; + }), + tracks_.end()); + + refresh_debug_outputs(); + debug.active_tracks = static_cast(tracks_.size()); + debug.published_tracks = static_cast(confirmed_tracks().size()); + return debug; +} + +std::vector ObstacleTracker::confirmed_tracks() const +{ + std::vector outputs; + for (const auto & track : tracks_) { + if (track.hit_count >= config_.track_confirm_hits) { + outputs.push_back(to_output(track)); + } + } + return outputs; +} + +const std::vector & ObstacleTracker::tracks_for_debug() const +{ + return debug_outputs_; +} + +int ObstacleTracker::find_nearest_unmatched_track(const Eigen::Vector2d & center) const +{ + int best_index = -1; + double best_distance = std::numeric_limits::infinity(); + for (std::size_t i = 0; i < tracks_.size(); ++i) { + const auto & track = tracks_[i]; + if (track.matched) { + continue; + } + const double distance = (center - track.center).norm(); + if (distance < config_.association_gate && distance < best_distance) { + best_index = static_cast(i); + best_distance = distance; + } + } + return best_index; +} + +bool ObstacleTracker::chord_center_for_track( + const Observation & observation, + const Track & track, + Eigen::Vector2d & center) const +{ + const Eigen::Vector2d chord = observation.p2 - observation.p1; + const double chord_length = chord.norm(); + if (chord_length <= 1e-9) { + return false; + } + if (chord_length > 2.0 * track.radius + config_.chord_tolerance) { + return false; + } + + const double half_chord = 0.5 * chord_length; + const double radius_squared = track.radius * track.radius; + const double h_squared = radius_squared - half_chord * half_chord; + if (h_squared < 0.0) { + return false; + } + + const Eigen::Vector2d middle = 0.5 * (observation.p1 + observation.p2); + const Eigen::Vector2d direction = chord / chord_length; + const Eigen::Vector2d normal(-direction.y(), direction.x()); + const double h = std::sqrt(h_squared); + const Eigen::Vector2d candidate_a = middle + h * normal; + const Eigen::Vector2d candidate_b = middle - h * normal; + center = candidate_a.norm() >= candidate_b.norm() ? candidate_a : candidate_b; + return true; +} + +TrackOutput ObstacleTracker::to_output(const Track & track) +{ + TrackOutput output; + output.id = track.id; + output.center_x = track.center.x(); + output.center_y = track.center.y(); + output.radius = track.radius; + output.hit_count = track.hit_count; + output.missed_count = track.missed_count; + output.last_source = track.last_source; + return output; +} + +void ObstacleTracker::refresh_debug_outputs() +{ + debug_outputs_.clear(); + debug_outputs_.reserve(tracks_.size()); + for (const auto & track : tracks_) { + debug_outputs_.push_back(to_output(track)); + } +} + +std::string update_source_name(UpdateSource source) +{ + switch (source) { + case UpdateSource::Fit: + return "fit"; + case UpdateSource::Chord: + return "chord"; + case UpdateSource::Predict: + return "predict"; + } + return "unknown"; +} + +} // namespace obstacle_scanner diff --git a/src/obstacle_scanner/test/test_detection.cpp b/src/obstacle_scanner/test/test_detection.cpp new file mode 100644 index 0000000..dfc192e --- /dev/null +++ b/src/obstacle_scanner/test/test_detection.cpp @@ -0,0 +1,90 @@ +#include "obstacle_scanner/detection.hpp" + +#include + +#include + +#include + +namespace obstacle_scanner +{ +namespace +{ + +DetectionConfig test_config() +{ + DetectionConfig config; + config.cluster_gap = 0.1; + config.max_cluster_points = 20; + config.radius_min = 0.03; + config.radius_max = 0.05; + config.merge_wrap = false; + return config; +} + +TEST(DetectionTest, ClustersScanOrderWithoutSorting) +{ + const std::vector points{ + {1.0, 0.0}, {1.02, 0.0}, {1.04, 0.0}, + {1.5, 0.0}, {1.52, 0.0} + }; + + const auto clusters = cluster_scan_order(points, 0.1, false); + + ASSERT_EQ(clusters.size(), 2u); + EXPECT_EQ(clusters[0].size(), 3u); + EXPECT_EQ(clusters[1].size(), 2u); +} + +TEST(DetectionTest, ThreePointClusterCreatesCircleFitObservation) +{ + auto config = test_config(); + const std::vector points{ + {1.04, 0.0}, {1.0, 0.04}, {0.96, 0.0} + }; + + DetectionDebug debug; + const auto observations = detect_observations(points, config, debug); + + ASSERT_EQ(observations.size(), 1u); + EXPECT_EQ(observations[0].type, ObservationType::CircleFit); + EXPECT_NEAR(observations[0].center.x(), 1.0, 1e-6); + EXPECT_NEAR(observations[0].center.y(), 0.0, 1e-6); + EXPECT_NEAR(observations[0].radius, 0.04, 1e-6); + EXPECT_EQ(debug.fit_observations, 1); +} + +TEST(DetectionTest, TwoPointClusterCreatesChordObservation) +{ + auto config = test_config(); + const std::vector points{ + {1.0, -0.02}, {1.0, 0.02} + }; + + DetectionDebug debug; + const auto observations = detect_observations(points, config, debug); + + ASSERT_EQ(observations.size(), 1u); + EXPECT_EQ(observations[0].type, ObservationType::TwoPointChord); + EXPECT_EQ(observations[0].cluster_size, 2); + EXPECT_EQ(debug.chord_observations, 1); +} + +TEST(DetectionTest, LargeClusterIsDiscarded) +{ + auto config = test_config(); + config.max_cluster_points = 3; + std::vector points; + for (int i = 0; i < 4; ++i) { + points.emplace_back(1.0 + 0.01 * static_cast(i), 0.0); + } + + DetectionDebug debug; + const auto observations = detect_observations(points, config, debug); + + EXPECT_TRUE(observations.empty()); + EXPECT_EQ(debug.discarded_large, 1); +} + +} // namespace +} // namespace obstacle_scanner diff --git a/src/obstacle_scanner/test/test_tracking.cpp b/src/obstacle_scanner/test/test_tracking.cpp new file mode 100644 index 0000000..e946ed7 --- /dev/null +++ b/src/obstacle_scanner/test/test_tracking.cpp @@ -0,0 +1,117 @@ +#include "obstacle_scanner/tracking.hpp" + +#include + +#include +#include + +namespace obstacle_scanner +{ +namespace +{ + +TrackerConfig test_config() +{ + TrackerConfig config; + config.track_confirm_hits = 1; + config.track_delete_misses = 2; + config.association_gate = 0.25; + config.chord_tolerance = 0.02; + config.fit_update_alpha = 0.7; + config.chord_update_alpha = 0.3; + return config; +} + +TEST(ObstacleTrackerTest, CircleFitCreatesConfirmedTrack) +{ + ObstacleTracker tracker(test_config()); + Observation obs; + obs.type = ObservationType::CircleFit; + obs.center = Eigen::Vector2d(1.0, 0.2); + obs.radius = 0.04; + obs.cluster_size = 4; + + FrameDebug debug = tracker.update({obs}); + const auto tracks = tracker.confirmed_tracks(); + + ASSERT_EQ(tracks.size(), 1u); + EXPECT_EQ(debug.fit_observations, 1); + EXPECT_EQ(debug.published_tracks, 1); + EXPECT_NEAR(tracks[0].center_x, 1.0, 1e-9); + EXPECT_NEAR(tracks[0].center_y, 0.2, 1e-9); + EXPECT_NEAR(tracks[0].radius, 0.04, 1e-9); + EXPECT_EQ(tracks[0].last_source, UpdateSource::Fit); +} + +TEST(ObstacleTrackerTest, TwoPointChordUpdatesExistingTrack) +{ + ObstacleTracker tracker(test_config()); + Observation fit; + fit.type = ObservationType::CircleFit; + fit.center = Eigen::Vector2d(1.0, 0.0); + fit.radius = 0.05; + fit.cluster_size = 4; + tracker.update({fit}); + + Observation chord; + chord.type = ObservationType::TwoPointChord; + chord.p1 = Eigen::Vector2d(0.985, -0.02); + chord.p2 = Eigen::Vector2d(0.985, 0.02); + chord.cluster_size = 2; + + FrameDebug debug = tracker.update({chord}); + const auto tracks = tracker.confirmed_tracks(); + + ASSERT_EQ(tracks.size(), 1u); + EXPECT_EQ(debug.chord_observations, 1); + EXPECT_EQ(debug.chord_updates, 1); + EXPECT_EQ(debug.chord_rejections, 0); + EXPECT_EQ(tracks[0].last_source, UpdateSource::Chord); + EXPECT_GT(tracks[0].center_x, 1.0); + EXPECT_NEAR(tracks[0].center_y, 0.0, 1e-6); +} + +TEST(ObstacleTrackerTest, TwoPointChordLongerThanDiameterIsRejected) +{ + ObstacleTracker tracker(test_config()); + Observation fit; + fit.type = ObservationType::CircleFit; + fit.center = Eigen::Vector2d(1.0, 0.0); + fit.radius = 0.04; + fit.cluster_size = 4; + tracker.update({fit}); + + Observation chord; + chord.type = ObservationType::TwoPointChord; + chord.p1 = Eigen::Vector2d(1.0, -0.08); + chord.p2 = Eigen::Vector2d(1.0, 0.08); + chord.cluster_size = 2; + + FrameDebug debug = tracker.update({chord}); + const auto tracks = tracker.confirmed_tracks(); + + ASSERT_EQ(tracks.size(), 1u); + EXPECT_EQ(debug.chord_updates, 0); + EXPECT_EQ(debug.chord_rejections, 1); + EXPECT_EQ(tracks[0].last_source, UpdateSource::Predict); +} + +TEST(ObstacleTrackerTest, StaleTracksAreDeleted) +{ + ObstacleTracker tracker(test_config()); + Observation fit; + fit.type = ObservationType::CircleFit; + fit.center = Eigen::Vector2d(1.0, 0.0); + fit.radius = 0.04; + fit.cluster_size = 4; + tracker.update({fit}); + + tracker.update({}); + EXPECT_EQ(tracker.confirmed_tracks().size(), 1u); + + tracker.update({}); + EXPECT_TRUE(tracker.confirmed_tracks().empty()); +} + +} // namespace +} // namespace obstacle_scanner