cyy/yiliao2026
1
0
Fork 0
forked from zbw/yiliao2026
Code Pull Requests Activity

障碍物检测v2。在之前的基础上加上了卡尔曼滤波的跟踪，防止障碍物过远时只检测到两个点，无法识别为障碍物。

Browse Source
This commit is contained in:
Orange
2026-07-12 22:22:43 +08:00
parent c8cd3c29f5
commit f4f0e16f1f
10 changed files with 1018 additions and 267 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
src/obstacle_scanner/CMakeLists.txt
View File

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

58
src/obstacle_scanner/config/params.yaml
View File

@@ -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

45
src/obstacle_scanner/include/obstacle_scanner/detection.hpp Normal file
View File

@@ -0,0 +1,45 @@
#pragma once
#include "obstacle_scanner/tracking.hpp"
#include <Eigen/Dense>
#include <vector>
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<std::vector<int>> cluster_scan_order(
const std::vector<Eigen::Vector2d> & points,
double cluster_gap,
bool merge_wrap);
std::tuple<double, double, double> fit_circle(
const std::vector<Eigen::Vector2d> & points);
std::vector<Observation> detect_observations(
const std::vector<Eigen::Vector2d> & points,
const DetectionConfig & config,
DetectionDebug & debug);
} // namespace obstacle_scanner

103
src/obstacle_scanner/include/obstacle_scanner/tracking.hpp Normal file
View File

@@ -0,0 +1,103 @@
#pragma once
#include <Eigen/Dense>
#include <cstddef>
#include <string>
#include <vector>
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<Observation> & observations);
std::vector<TrackOutput> confirmed_tracks() const;
const std::vector<TrackOutput> & 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<Track> tracks_;
std::vector<TrackOutput> debug_outputs_;
};
std::string update_source_name(UpdateSource source);
} // namespace obstacle_scanner

5
src/obstacle_scanner/package.xml
View File

@@ -19,10 +19,11 @@
<build_depend>rosidl_default_generators</build_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<test_depend>ament_cmake_gtest</test_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<export>
<build_type>ament_cmake</build_type>

145
src/obstacle_scanner/src/detection.cpp Normal file
View File

@@ -0,0 +1,145 @@
#include "obstacle_scanner/detection.hpp"
#include <Eigen/Dense>
#include <cmath>
#include <stdexcept>
#include <tuple>
namespace obstacle_scanner
{
std::vector<std::vector<int>> cluster_scan_order(
const std::vector<Eigen::Vector2d> & points,
double cluster_gap,
bool merge_wrap)
{
std::vector<std::vector<int>> 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<int>(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<double, double, double> fit_circle(
const std::vector<Eigen::Vector2d> & points)
{
const int count = static_cast<int>(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<std::size_t>(i)].x();
const double y = points[static_cast<std::size_t>(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<Eigen::MatrixXd> 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<Observation> detect_observations(
const std::vector<Eigen::Vector2d> & points,
const DetectionConfig & config,
DetectionDebug & debug)
{
debug = DetectionDebug();
std::vector<Observation> observations;
const auto clusters = cluster_scan_order(points, config.cluster_gap, config.merge_wrap);
debug.total_clusters = static_cast<int>(clusters.size());
for (const auto & cluster : clusters) {
const int cluster_size = static_cast<int>(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<std::size_t>(cluster[0])];
observation.p2 = points[static_cast<std::size_t>(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<Eigen::Vector2d> cluster_points;
cluster_points.reserve(static_cast<std::size_t>(cluster_size));
for (const int index : cluster) {
cluster_points.push_back(points[static_cast<std::size_t>(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

461
src/obstacle_scanner/src/obstacle_scanner_node.cpp
View File

@@ -1,184 +1,24 @@
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <std_msgs/msg/string.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <Eigen/Dense>
#include "obstacle_scanner/detection.hpp"
#include "obstacle_scanner/msg/obstacle.hpp"
#include "obstacle_scanner/msg/obstacle_array.hpp"
#include "obstacle_scanner/tracking.hpp"
#include <algorithm>
#include <chrono>
#include <cmath>
#include <numeric>
#include <vector>
#include <cstdio>
#include <sstream>
#include <string>
#include <vector>
namespace obstacle_scanner
{
// ---------------------------------------------------------------------------
// Ported from scan_circle_demo.py:211-227
// Least-squares algebraic circle fitting (Al-Sharadqah & Chernov)
// ---------------------------------------------------------------------------
std::tuple<double, double, double> fit_circle(const std::vector<Eigen::Vector2d>& pts)
{
const int n = static_cast<int>(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<Eigen::MatrixXd> 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<std::vector<int>> cluster_full_scan(
const std::vector<Eigen::Vector2d>& points,
const std::vector<double>& point_angles,
double cluster_gap,
bool merge_wrap)
{
const int n = static_cast<int>(points.size());
if (n == 0) {
return {};
}
// Sort indices by angle
std::vector<int> 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<std::vector<int>> 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<msg::Obstacle> detect_circles(
const std::vector<Eigen::Vector2d>& points,
const std::vector<double>& 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<int>(clusters.size());
std::vector<msg::Obstacle> obstacles;
for (const auto& cluster : clusters) {
const int sz = static_cast<int>(cluster.size());
if (sz > max_cluster_points) {
out_discarded_large++;
continue;
}
if (sz < 3) {
out_skipped_small++;
continue;
}
std::vector<Eigen::Vector2d> 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,28 +27,44 @@ public:
{
using std::placeholders::_1;
// --- Parameters ---
scan_topic_ = declare_parameter<std::string>("scan_topic", "/scan");
frame_id_ = declare_parameter<std::string>("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<int>(declare_parameter<int>("debug_image_stride", 3)));
debug_info_stride_ = std::max(
1, static_cast<int>(declare_parameter<int>("debug_info_stride", 1)));
// --- Publishers ---
obstacles_pub_ = create_publisher<obstacle_scanner::msg::ObstacleArray>(
"/obstacles", 10);
if (debug_) {
debug_pub_ = create_publisher<sensor_msgs::msg::Image>(
"/processed_scan", 10);
if (debug_info_) {
debug_info_pub_ = create_publisher<std_msgs::msg::String>(
"/obstacle_scanner/debug_info", 10);
}
}
// --- Subscriber ---
scan_sub_ = create_subscription<sensor_msgs::msg::LaserScan>(
scan_topic_, rclcpp::SensorDataQoS(),
std::bind(&ObstacleScannerNode::scan_callback, this, _1));
@@ -217,69 +73,116 @@ public:
}
private:
// ==========================================================================
// LaserScan callback
// ==========================================================================
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<Eigen::Vector2d> points;
std::vector<double> 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<double>(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<double>(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<obstacle_scanner::TrackOutput> output_tracks;
std::vector<obstacle_scanner::TrackOutput> 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<int>(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);
if (debug_pub_ && frame_count_ % static_cast<std::uint64_t>(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<std::uint64_t>(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);
}
}
}
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<double, std::milli>(end_time - start_time).count();
}
static std::vector<obstacle_scanner::TrackOutput> tracks_from_fit_observations(
const std::vector<obstacle_scanner::Observation> & observations)
{
std::vector<obstacle_scanner::TrackOutput> 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;
}
// ==========================================================================
// Debug: render scan points + obstacles to cv::Mat, publish as Image
// ==========================================================================
sensor_msgs::msg::Image::SharedPtr render_debug_image(
const std::vector<Eigen::Vector2d> & points,
const std::vector<obstacle_scanner::msg::Obstacle>& obstacles,
const std::vector<obstacle_scanner::TrackOutput> & 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_;
@@ -288,40 +191,34 @@ 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<int>(std::round(origin + pt.x() / res));
int row = static_cast<int>(std::round(origin - pt.y() / res));
const int col = static_cast<int>(std::round(origin + pt.x() / res));
const int row = static_cast<int>(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::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<int>(std::round(origin + obs.center_x / res));
int row = static_cast<int>(std::round(origin - obs.center_y / res));
int r_px = std::max(1, static_cast<int>(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();
image_msg->header.stamp = stamp;
@@ -329,25 +226,89 @@ private:
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<int>(std::round(origin + track.center_x / res));
const int row = static_cast<int>(std::round(origin - track.center_y / res));
const int radius_px = std::max(1, static_cast<int>(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<obstacle_scanner::TrackOutput> & 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<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
rclcpp::Publisher<obstacle_scanner::msg::ObstacleArray>::SharedPtr obstacles_pub_;
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr debug_pub_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr debug_info_pub_;
};
// ============================================================================
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);

219
src/obstacle_scanner/src/tracking.cpp Normal file
View File

@@ -0,0 +1,219 @@
#include "obstacle_scanner/tracking.hpp"
#include <algorithm>
#include <cmath>
#include <limits>
namespace obstacle_scanner
{
ObstacleTracker::ObstacleTracker(const TrackerConfig & config)
: config_(config)
{
}
FrameDebug ObstacleTracker::update(const std::vector<Observation> & 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<std::size_t>(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<double>::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<int>(i);
best_distance = distance;
best_center = chord_center;
}
}
if (best_index >= 0) {
auto & track = tracks_[static_cast<std::size_t>(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<int>(tracks_.size());
debug.published_tracks = static_cast<int>(confirmed_tracks().size());
return debug;
}
std::vector<TrackOutput> ObstacleTracker::confirmed_tracks() const
{
std::vector<TrackOutput> 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<TrackOutput> & 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<double>::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<int>(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

90
src/obstacle_scanner/test/test_detection.cpp Normal file
View File

@@ -0,0 +1,90 @@
#include "obstacle_scanner/detection.hpp"
#include <gtest/gtest.h>
#include <Eigen/Dense>
#include <vector>
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<Eigen::Vector2d> 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<Eigen::Vector2d> 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<Eigen::Vector2d> 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<Eigen::Vector2d> points;
for (int i = 0; i < 4; ++i) {
points.emplace_back(1.0 + 0.01 * static_cast<double>(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

117
src/obstacle_scanner/test/test_tracking.cpp Normal file
View File

@@ -0,0 +1,117 @@
#include "obstacle_scanner/tracking.hpp"
#include <gtest/gtest.h>
#include <cmath>
#include <vector>
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