添加了第一版雷达数据->障碍物坐标的功能包

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

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

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',
+        ),
+    ])

src/obstacle_scanner/msg/Obstacle.msg

@@ -0,0 +1,3 @@
+float64 center_x
+float64 center_y
+float64 radius

src/obstacle_scanner/msg/ObstacleArray.msg

@@ -0,0 +1,2 @@
+std_msgs/Header header
+Obstacle[] obstacles

src/obstacle_scanner/package.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>obstacle_scanner</name>
+  <version>0.0.0</version>
+  <description>LiDAR obstacle detection via angular clustering and circle fitting.</description>
+  <maintainer email="2314753575@qq.com">sunrise</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>cv_bridge</depend>
+  <depend>libopencv-dev</depend>
+  <depend>eigen</depend>
+
+  <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>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/obstacle_scanner/src/obstacle_scanner_node.cpp

@@ -0,0 +1,357 @@
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/opencv.hpp>
+#include <Eigen/Dense>
+
+#include "obstacle_scanner/msg/obstacle.hpp"
+#include "obstacle_scanner/msg/obstacle_array.hpp"
+
+#include <algorithm>
+#include <cmath>
+#include <numeric>
+#include <vector>
+#include <cstdio>
+
+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:
+  ObstacleScannerNode()
+  : rclcpp::Node("obstacle_scanner")
+  {
+    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);
+    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<obstacle_scanner::msg::ObstacleArray>(
+        "/obstacles", 10);
+
+    if (debug_) {
+      debug_pub_ = create_publisher<sensor_msgs::msg::Image>(
+          "/processed_scan", 10);
+    }
+
+    // --- Subscriber ---
+    scan_sub_ = create_subscription<sensor_msgs::msg::LaserScan>(
+        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<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) {
+        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));
+    }
+
+    // 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<Eigen::Vector2d>& points,
+      const std::vector<obstacle_scanner::msg::Obstacle>& 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<int>(std::round(origin + pt.x() / res));
+      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);
+      }
+    }
+
+    // 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<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);
+    }
+
+    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<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_;
+};
+
+// ============================================================================
+int main(int argc, char** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<ObstacleScannerNode>());
+  rclcpp::shutdown();
+  return 0;
+}