From 79f0a4d586c4099c5143f5378165dcd38db9a320 Mon Sep 17 00:00:00 2001 From: Orange <2314753575@qq.com> Date: Sun, 12 Jul 2026 15:25:11 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E4=BA=86=E7=AC=AC=E4=B8=80?= =?UTF-8?q?=E7=89=88=E9=9B=B7=E8=BE=BE=E6=95=B0=E6=8D=AE->=E9=9A=9C?= =?UTF-8?q?=E7=A2=8D=E7=89=A9=E5=9D=90=E6=A0=87=E7=9A=84=E5=8A=9F=E8=83=BD?= =?UTF-8?q?=E5=8C=85?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/obstacle_scanner/CMakeLists.txt | 55 +++ src/obstacle_scanner/config/params.yaml | 12 + .../launch/obstacle_scanner.launch.py | 22 ++ src/obstacle_scanner/msg/Obstacle.msg | 3 + src/obstacle_scanner/msg/ObstacleArray.msg | 2 + src/obstacle_scanner/package.xml | 30 ++ .../src/obstacle_scanner_node.cpp | 357 ++++++++++++++++++ 7 files changed, 481 insertions(+) create mode 100644 src/obstacle_scanner/CMakeLists.txt create mode 100644 src/obstacle_scanner/config/params.yaml create mode 100755 src/obstacle_scanner/launch/obstacle_scanner.launch.py create mode 100644 src/obstacle_scanner/msg/Obstacle.msg create mode 100644 src/obstacle_scanner/msg/ObstacleArray.msg create mode 100644 src/obstacle_scanner/package.xml create mode 100644 src/obstacle_scanner/src/obstacle_scanner_node.cpp diff --git a/src/obstacle_scanner/CMakeLists.txt b/src/obstacle_scanner/CMakeLists.txt new file mode 100644 index 0000000..9265707 --- /dev/null +++ b/src/obstacle_scanner/CMakeLists.txt @@ -0,0 +1,55 @@ +cmake_minimum_required(VERSION 3.8) +project(obstacle_scanner) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(OpenCV REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/Obstacle.msg" + "msg/ObstacleArray.msg" + DEPENDENCIES std_msgs +) + +add_executable(obstacle_scanner_node src/obstacle_scanner_node.cpp) +target_compile_features(obstacle_scanner_node PUBLIC cxx_std_17) +target_include_directories(obstacle_scanner_node PUBLIC ${EIGEN3_INCLUDE_DIRS}) +ament_target_dependencies(obstacle_scanner_node + rclcpp + sensor_msgs + std_msgs + cv_bridge + OpenCV +) +rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") +target_link_libraries(obstacle_scanner_node ${cpp_typesupport_target}) + +install(TARGETS obstacle_scanner_node + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME} +) + +ament_export_dependencies(rosidl_default_runtime) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/obstacle_scanner/config/params.yaml b/src/obstacle_scanner/config/params.yaml new file mode 100644 index 0000000..eba800c --- /dev/null +++ b/src/obstacle_scanner/config/params.yaml @@ -0,0 +1,12 @@ +obstacle_scanner: + ros__parameters: + scan_topic: "/scan" + frame_id: "laser_frame" + cluster_gap: 0.1 + max_cluster_points: 20 + radius_min: 0.03 + radius_max: 0.05 + merge_wrap: true + debug: true + debug_image_size: 500 + debug_resolution: 0.01 diff --git a/src/obstacle_scanner/launch/obstacle_scanner.launch.py b/src/obstacle_scanner/launch/obstacle_scanner.launch.py new file mode 100755 index 0000000..ccc2b5e --- /dev/null +++ b/src/obstacle_scanner/launch/obstacle_scanner.launch.py @@ -0,0 +1,22 @@ +#!/usr/bin/env python3 +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + + +def generate_launch_description(): + config = os.path.join( + get_package_share_directory('obstacle_scanner'), + 'config', + 'params.yaml', + ) + return LaunchDescription([ + Node( + package='obstacle_scanner', + executable='obstacle_scanner_node', + name='obstacle_scanner', + parameters=[config], + output='screen', + ), + ]) diff --git a/src/obstacle_scanner/msg/Obstacle.msg b/src/obstacle_scanner/msg/Obstacle.msg new file mode 100644 index 0000000..55fbc5d --- /dev/null +++ b/src/obstacle_scanner/msg/Obstacle.msg @@ -0,0 +1,3 @@ +float64 center_x +float64 center_y +float64 radius diff --git a/src/obstacle_scanner/msg/ObstacleArray.msg b/src/obstacle_scanner/msg/ObstacleArray.msg new file mode 100644 index 0000000..5fdca81 --- /dev/null +++ b/src/obstacle_scanner/msg/ObstacleArray.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +Obstacle[] obstacles diff --git a/src/obstacle_scanner/package.xml b/src/obstacle_scanner/package.xml new file mode 100644 index 0000000..afdac89 --- /dev/null +++ b/src/obstacle_scanner/package.xml @@ -0,0 +1,30 @@ + + + + obstacle_scanner + 0.0.0 + LiDAR obstacle detection via angular clustering and circle fitting. + sunrise + Apache-2.0 + + ament_cmake + + rclcpp + sensor_msgs + std_msgs + cv_bridge + libopencv-dev + eigen + + rosidl_default_generators + rosidl_default_runtime + + rosidl_interface_packages + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/obstacle_scanner/src/obstacle_scanner_node.cpp b/src/obstacle_scanner/src/obstacle_scanner_node.cpp new file mode 100644 index 0000000..78c0784 --- /dev/null +++ b/src/obstacle_scanner/src/obstacle_scanner_node.cpp @@ -0,0 +1,357 @@ +#include +#include +#include +#include +#include +#include + +#include "obstacle_scanner/msg/obstacle.hpp" +#include "obstacle_scanner/msg/obstacle_array.hpp" + +#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: + ObstacleScannerNode() + : rclcpp::Node("obstacle_scanner") + { + 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); + debug_ = declare_parameter("debug", false); + debug_image_size_ = declare_parameter("debug_image_size", 500); + debug_resolution_ = declare_parameter("debug_resolution", 0.01); + + // --- Publishers --- + obstacles_pub_ = create_publisher( + "/obstacles", 10); + + if (debug_) { + debug_pub_ = create_publisher( + "/processed_scan", 10); + } + + // --- Subscriber --- + scan_sub_ = create_subscription( + 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) + { + // 1. Filter valid ranges, convert polar → Cartesian + const double angle_increment = msg->angle_increment; + 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) { + 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)); + } + + // 2. Detect circles + int total_clusters = 0, fitted = 0, discarded_large = 0; + int skipped_small = 0, failed = 0; + + 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); + + 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); + + // 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); + 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); + } + } + + // ========================================================================== + // Debug: render scan points + obstacles to cv::Mat, publish as Image + // ========================================================================== + sensor_msgs::msg::Image::SharedPtr render_debug_image( + const std::vector& points, + const std::vector& obstacles, + const builtin_interfaces::msg::Time& stamp) + { + const int size = debug_image_size_; + const double res = debug_resolution_; + const int origin = size / 2; + + 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)); + if (col >= 0 && col < size && row >= 0 && row < size) { + 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); + + // 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); + } + + auto image_msg = cv_bridge::CvImage( + std_msgs::msg::Header(), "bgr8", img).toImageMsg(); + image_msg->header.stamp = stamp; + image_msg->header.frame_id = frame_id_; + return image_msg; + } + + // ---- Parameters ---- + 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_; + + // ---- ROS2 interfaces ---- + rclcpp::Subscription::SharedPtr scan_sub_; + rclcpp::Publisher::SharedPtr obstacles_pub_; + rclcpp::Publisher::SharedPtr debug_pub_; +}; + +// ============================================================================ +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +}