diff --git a/hz/src/hz_pkg/CMakeLists.txt b/hz/src/hz_pkg/CMakeLists.txt
new file mode 100644
index 0000000..9bd3fb5
--- /dev/null
+++ b/hz/src/hz_pkg/CMakeLists.txt
@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 3.5)
+project(hz_pkg)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# 查找依赖
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+
+# 信号发生器节点
+add_executable(signal_generator src/signal_generator.cpp)
+ament_target_dependencies(signal_generator rclcpp std_msgs)
+
+# 信号处理器节点
+add_executable(signal_processor src/signal_processor.cpp)
+ament_target_dependencies(signal_processor rclcpp std_msgs)
+
+# 安装可执行文件
+install(TARGETS
+ signal_generator
+ signal_processor
+ DESTINATION lib/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+ find_package(ament_lint_auto REQUIRED)
+ ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
+
\ No newline at end of file
diff --git a/hz/src/hz_pkg/package.xml b/hz/src/hz_pkg/package.xml
new file mode 100644
index 0000000..f571fcf
--- /dev/null
+++ b/hz/src/hz_pkg/package.xml
@@ -0,0 +1,21 @@
+
+
+
+ hz_pkg
+ 0.0.0
+ ROS 2 信号发生器和处理器示例
+ Your Name
+ Apache-2.0
+
+ ament_cmake
+ rclcpp
+ std_msgs
+
+ ament_lint_auto
+ ament_lint_common
+
+
+ ament_cmake
+
+
+
\ No newline at end of file
diff --git a/hz/src/hz_pkg/src/signal_generator.cpp b/hz/src/hz_pkg/src/signal_generator.cpp
new file mode 100644
index 0000000..78de676
--- /dev/null
+++ b/hz/src/hz_pkg/src/signal_generator.cpp
@@ -0,0 +1,64 @@
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/float64.hpp"
+#include
+
+using namespace std::chrono_literals;
+
+class SignalGenerator : public rclcpp::Node
+{
+public:
+ SignalGenerator() : Node("signal_generator")
+ {
+ // 创建发布者，分别发布正弦波和方波
+ sine_publisher_ = this->create_publisher("sine_wave", 10);
+ square_publisher_ = this->create_publisher("square_wave", 10);
+
+ // 以1000Hz的频率发布信号
+ timer_ = this->create_wall_timer(
+ 5ms, std::bind(&SignalGenerator::timer_callback, this));
+
+ // 初始化时间
+ start_time_ = this->now();
+ }
+
+private:
+ void timer_callback()
+ {
+ double t = (this->now() - start_time_).seconds();
+
+ double sine_value = std::sin(2 * M_PI * 10 * t);
+
+ // 生成1Hz方波:
+ double square_value = (std::sin(2 * M_PI * 1 * t) >= 0) ? 1.0 : -1.0;
+
+ // 发布正弦波
+ auto sine_msg = std_msgs::msg::Float64();
+ sine_msg.data = sine_value;
+ sine_publisher_->publish(sine_msg);
+
+ // 发布方波
+ auto square_msg = std_msgs::msg::Float64();
+ square_msg.data = square_value;
+ square_publisher_->publish(square_msg);
+
+ if (count_ % 100 == 0) {
+ RCLCPP_INFO(this->get_logger(), "Sine: %.2f, Square: %.2f", sine_value, square_value);
+ }
+ count_++;
+ }
+
+ rclcpp::TimerBase::SharedPtr timer_;
+ rclcpp::Publisher::SharedPtr sine_publisher_;
+ rclcpp::Publisher::SharedPtr square_publisher_;
+ rclcpp::Time start_time_;
+ size_t count_ = 0;
+};
+
+int main(int argc, char * argv[])
+{
+ rclcpp::init(argc, argv);
+ rclcpp::spin(std::make_shared());
+ rclcpp::shutdown();
+ return 0;
+}
+
\ No newline at end of file
diff --git a/hz/src/hz_pkg/src/signal_processor.cpp b/hz/src/hz_pkg/src/signal_processor.cpp
new file mode 100644
index 0000000..53cdb5d
--- /dev/null
+++ b/hz/src/hz_pkg/src/signal_processor.cpp
@@ -0,0 +1,96 @@
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/float64.hpp"
+#include
+
+class SignalProcessor : public rclcpp::Node
+{
+public:
+ SignalProcessor() : Node("signal_processor")
+ {
+ // 订阅正弦波信号
+ sine_subscription_ = this->create_subscription(
+ "sine_wave", 10,
+ std::bind(&SignalProcessor::sine_callback, this, std::placeholders::_1));
+
+ // 订阅方波信号
+ square_subscription_ = this->create_subscription(
+ "square_wave", 10,
+ std::bind(&SignalProcessor::square_callback, this, std::placeholders::_1));
+
+ // 创建处理后信号的发布者
+ processed_publisher_ = this->create_publisher("processed_signal", 10);
+
+ latest_sine_ = 0.0;
+ latest_square_ = 0.0;
+ has_sine_ = false;
+ has_square_ = false;
+ }
+
+private:
+ // 处理正弦波回调
+ void sine_callback(const std_msgs::msg::Float64::SharedPtr msg)
+ {
+ std::lock_guard lock(data_mutex_);
+ latest_sine_ = msg->data;
+ has_sine_ = true;
+ process_signals();
+ }
+
+ // 处理方波回调
+ void square_callback(const std_msgs::msg::Float64::SharedPtr msg)
+ {
+ std::lock_guard lock(data_mutex_);
+ latest_square_ = msg->data;
+ has_square_ = true;
+ process_signals();
+ }
+
+ void process_signals()
+ {
+ // 确保信号收到
+ if (!has_sine_ || !has_square_) {
+ return;
+ }
+
+ double processed_value;
+ if ((latest_sine_ >= 0 && latest_square_ >= 0) ||
+ (latest_sine_ < 0 && latest_square_ < 0)) {
+ processed_value = latest_sine_;
+ } else {
+ processed_value = 0.0;
+ }
+
+ // 发布处理后的信号
+ auto msg = std_msgs::msg::Float64();
+ msg.data = processed_value;
+ processed_publisher_->publish(msg);
+
+ // 每100次处理打印一次状态
+ if (count_ % 100 == 0) {
+ RCLCPP_INFO(this->get_logger(), "Sine: %.2f, Square: %.2f, Processed: %.2f",
+ latest_sine_, latest_square_, processed_value);
+ }
+ count_++;
+ }
+
+ rclcpp::Subscription::SharedPtr sine_subscription_;
+ rclcpp::Subscription::SharedPtr square_subscription_;
+ rclcpp::Publisher::SharedPtr processed_publisher_;
+
+ // 存储最新的信号值
+ double latest_sine_;
+ double latest_square_;
+ bool has_sine_;
+ bool has_square_;
+ std::mutex data_mutex_;
+ size_t count_ = 0;
+};
+
+int main(int argc, char * argv[])
+{
+ rclcpp::init(argc, argv);
+ rclcpp::spin(std::make_shared());
+ rclcpp::shutdown();
+ return 0;
+}
+
\ No newline at end of file
diff --git a/ros2_ws/src/my_ros2_pkg/CMakeLists.txt b/ros2_ws/src/my_ros2_pkg/CMakeLists.txt
new file mode 100644
index 0000000..f39baa6
--- /dev/null
+++ b/ros2_ws/src/my_ros2_pkg/CMakeLists.txt
@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 3.5)
+project(my_ros2_pkg)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# 查找依赖
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+
+# 声明发布者节点
+add_executable(publisher_node src/publisher_node.cpp)
+ament_target_dependencies(publisher_node rclcpp std_msgs)
+
+# 声明订阅者节点
+add_executable(subscriber_node src/subscriber_node.cpp)
+ament_target_dependencies(subscriber_node rclcpp std_msgs)
+
+# 安装可执行文件
+install(TARGETS
+ publisher_node
+ subscriber_node
+ DESTINATION lib/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+ find_package(ament_lint_auto REQUIRED)
+ ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
+
\ No newline at end of file
diff --git a/ros2_ws/src/my_ros2_pkg/package.xml b/ros2_ws/src/my_ros2_pkg/package.xml
new file mode 100644
index 0000000..876c02a
--- /dev/null
+++ b/ros2_ws/src/my_ros2_pkg/package.xml
@@ -0,0 +1,21 @@
+
+
+
+ my_ros2_pkg
+ 0.0.0
+ ROS 2 C++ 发布者和订阅者示例
+ Your Name
+ Apache-2.0
+
+ ament_cmake
+ rclcpp
+ std_msgs
+
+ ament_lint_auto
+ ament_lint_common
+
+
+ ament_cmake
+
+
+
\ No newline at end of file
diff --git a/ros2_ws/src/my_ros2_pkg/src/publisher_node.cpp b/ros2_ws/src/my_ros2_pkg/src/publisher_node.cpp
new file mode 100644
index 0000000..4e521df
--- /dev/null
+++ b/ros2_ws/src/my_ros2_pkg/src/publisher_node.cpp
@@ -0,0 +1,42 @@
+#include
+#include
+#include
+#include
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+
+using namespace std::chrono_literals;
+
+
+class MinimalPublisher : public rclcpp::Node
+{
+ public:
+ MinimalPublisher()
+ : Node("minimal_publisher"), count_(0)
+ {
+ publisher_ = this->create_publisher("topic", 10);
+ timer_ = this->create_wall_timer(
+ 500ms, std::bind(&MinimalPublisher::timer_callback, this));
+ }
+
+ private:
+ void timer_callback()
+ {
+ auto message = std_msgs::msg::String();
+ message.data = "Hello, world! " + std::to_string(count_++);
+ RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
+ publisher_->publish(message);
+ }
+ rclcpp::TimerBase::SharedPtr timer_;
+ rclcpp::Publisher::SharedPtr publisher_;
+ size_t count_;
+};
+
+int main(int argc, char * argv[])
+{
+ rclcpp::init(argc, argv);
+ rclcpp::spin(std::make_shared());
+ rclcpp::shutdown();
+ return 0;
+}
\ No newline at end of file
diff --git a/ros2_ws/src/my_ros2_pkg/src/subscriber_node.cpp b/ros2_ws/src/my_ros2_pkg/src/subscriber_node.cpp
new file mode 100644
index 0000000..83345b4
--- /dev/null
+++ b/ros2_ws/src/my_ros2_pkg/src/subscriber_node.cpp
@@ -0,0 +1,31 @@
+#include
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+using std::placeholders::_1;
+
+class MinimalSubscriber : public rclcpp::Node
+{
+ public:
+ MinimalSubscriber()
+ : Node("minimal_subscriber")
+ {
+ subscription_ = this->create_subscription(
+ "topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1));
+ }
+
+ private:
+ void topic_callback(const std_msgs::msg::String::SharedPtr msg) const
+ {
+ RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str());
+ }
+ rclcpp::Subscription::SharedPtr subscription_;
+};
+
+int main(int argc, char * argv[])
+{
+ rclcpp::init(argc, argv);
+ rclcpp::spin(std::make_shared());
+ rclcpp::shutdown();
+ return 0;
+}
\ No newline at end of file
diff --git a/work1/src/filter_pkg/CMakeLists.txt b/work1/src/filter_pkg/CMakeLists.txt
new file mode 100644
index 0000000..3c6d5ea
--- /dev/null
+++ b/work1/src/filter_pkg/CMakeLists.txt
@@ -0,0 +1,29 @@
+cmake_minimum_required(VERSION 3.5)
+project(filter_visualization)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+
+add_executable(data_publisher src/data_publisher.cpp)
+ament_target_dependencies(data_publisher rclcpp std_msgs sensor_msgs)
+
+add_executable(filter_node src/filter_node.cpp)
+ament_target_dependencies(filter_node rclcpp std_msgs sensor_msgs)
+
+install(TARGETS
+ data_publisher
+ filter_node
+ DESTINATION lib/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+ find_package(ament_lint_auto REQUIRED)
+ ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
diff --git a/work1/src/filter_pkg/package.xml b/work1/src/filter_pkg/package.xml
new file mode 100644
index 0000000..fec4dc4
--- /dev/null
+++ b/work1/src/filter_pkg/package.xml
@@ -0,0 +1,24 @@
+
+
+
+ filter_visualization
+ 0.0.0
+ ROS2 package for data filtering with Foxglove visualization
+ Your Name
+ Apache-2.0
+
+ ament_cmake
+ rclcpp
+ std_msgs
+ sensor_msgs
+
+ ament_lint_auto
+ ament_lint_common
+
+ ament_cmake_pytest
+
+
+ ament_cmake
+
+
+
\ No newline at end of file
diff --git a/work1/src/filter_pkg/src/data_publisher.cpp b/work1/src/filter_pkg/src/data_publisher.cpp
new file mode 100644
index 0000000..21a4f10
--- /dev/null
+++ b/work1/src/filter_pkg/src/data_publisher.cpp
@@ -0,0 +1,63 @@
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/joy.hpp"
+#include "std_msgs/msg/float64.hpp"
+#include
+#include
+
+class DataPublisherNode : public rclcpp::Node
+{
+public:
+ DataPublisherNode() : Node("data_publisher")
+ {
+ // 创建发布者
+ raw_data_pub_ = this->create_publisher("raw_data", 10);
+
+ // 设置发布频率
+ double publish_frequency = 100.0;
+ timer_ = this->create_wall_timer(
+ std::chrono::microseconds(static_cast(1e6 / publish_frequency)),
+ std::bind(&DataPublisherNode::timer_callback, this)
+ );
+
+ std::random_device rd;
+ gen_ = std::mt19937(rd());
+ dist_ = std::normal_distribution(0.0, 0.15);
+
+ RCLCPP_INFO(this->get_logger(), "Data publisher node initialized. Publishing to 'raw_data'");
+ }
+
+private:
+ void timer_callback()
+ {
+ // 获取当前时间
+ auto time = this->now().seconds();
+
+ // 生成基础信号：2Hz的正弦波 1Hz的余弦波
+ double base_frequency1 = 2.0;
+ double base_frequency2 = 1.0;
+ double clean_data = 0.7 * sin(2 * M_PI * base_frequency1 * time) +
+ 0.3 * cos(2 * M_PI * base_frequency2 * time);
+
+ // 添加随机噪声
+ double noise = dist_(gen_);
+ double noisy_data = clean_data + noise;
+
+ // 发布原始数据
+ auto msg = std_msgs::msg::Float64();
+ msg.data = noisy_data;
+ raw_data_pub_->publish(msg);
+ }
+
+ rclcpp::Publisher::SharedPtr raw_data_pub_;
+ rclcpp::TimerBase::SharedPtr timer_;
+ std::mt19937 gen_;
+ std::normal_distribution dist_;
+};
+
+int main(int argc, char * argv[])
+{
+ rclcpp::init(argc, argv);
+ rclcpp::spin(std::make_shared());
+ rclcpp::shutdown();
+ return 0;
+}
diff --git a/work1/src/filter_pkg/src/filter_node.cpp b/work1/src/filter_pkg/src/filter_node.cpp
new file mode 100644
index 0000000..72541e7
--- /dev/null
+++ b/work1/src/filter_pkg/src/filter_node.cpp
@@ -0,0 +1,123 @@
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/float64.hpp"
+#include
+#include
+
+class FilterNode : public rclcpp::Node
+{
+public:
+ FilterNode() : Node("filter_node")
+ {
+ // 中值滤波器参数
+ median_window_size_ = 7u;
+
+ // 低通滤波器参数
+ lowpass_alpha_ = 0.15;
+ lowpass_initialized_ = false;
+ lowpass_value_ = 0.0;
+
+ // 创建订阅者
+ raw_data_sub_ = this->create_subscription(
+ "raw_data",
+ 10,
+ std::bind(&FilterNode::data_callback, this, std::placeholders::_1)
+ );
+
+ // 创建发布者
+ median_pub_ = this->create_publisher("median_filtered", 10);
+ lowpass_pub_ = this->create_publisher("lowpass_filtered", 10);
+
+ RCLCPP_INFO(this->get_logger(), "Filter node initialized");
+ RCLCPP_INFO(this->get_logger(), "Median filter window size: %zu", median_window_size_);
+ RCLCPP_INFO(this->get_logger(), "Lowpass filter alpha: %f", lowpass_alpha_);
+ }
+
+private:
+ void data_callback(const std_msgs::msg::Float64::SharedPtr msg)
+ {
+ double raw_data = msg->data;
+
+ // 中值滤波处理
+ process_median_filter(raw_data);
+
+ // 低通滤波处理
+ process_lowpass_filter(raw_data);
+ }
+
+ void process_median_filter(double data)
+ {
+ // 将新数据添加到窗口
+ median_window_.push_back(data);
+
+ if (median_window_.size() > median_window_size_)
+ {
+ median_window_.pop_front();
+ }
+
+ // 窗口填满后才进行滤波
+ if (median_window_.size() == median_window_size_)
+ {
+ std::deque sorted_data = median_window_;
+ std::sort(sorted_data.begin(), sorted_data.end());
+
+ // 计算中值
+ double median;
+ if (median_window_size_ % 2 == 1)
+ {
+ median = sorted_data[median_window_size_ / 2];
+ }
+ else
+ {
+ median = (sorted_data[median_window_size_ / 2 - 1] +
+ sorted_data[median_window_size_ / 2]) / 2.0;
+ }
+
+ // 发布中值滤波结果
+ auto msg = std_msgs::msg::Float64();
+ msg.data = median;
+ median_pub_->publish(msg);
+ }
+ }
+
+ void process_lowpass_filter(double data)
+ {
+ // 初始化
+ if (!lowpass_initialized_)
+ {
+ lowpass_value_ = data;
+ lowpass_initialized_ = true;
+ return;
+ }
+
+ lowpass_value_ = lowpass_alpha_ * data + (1 - lowpass_alpha_) * lowpass_value_;
+
+ // 发布低通滤波结果
+ auto msg = std_msgs::msg::Float64();
+ msg.data = lowpass_value_;
+ lowpass_pub_->publish(msg);
+ }
+
+ // 订阅者
+ rclcpp::Subscription::SharedPtr raw_data_sub_;
+
+ // 发布者
+ rclcpp::Publisher::SharedPtr median_pub_;
+ rclcpp::Publisher::SharedPtr lowpass_pub_;
+
+ // 中值滤波器变量
+ std::deque median_window_;
+ size_t median_window_size_;
+
+ // 低通滤波器变量
+ double lowpass_alpha_;
+ double lowpass_value_;
+ bool lowpass_initialized_;
+};
+
+int main(int argc, char * argv[])
+{
+ rclcpp::init(argc, argv);
+ rclcpp::spin(std::make_shared());
+ rclcpp::shutdown();
+ return 0;
+}
diff --git a/work2/src/armor_detection/CMakeLists.txt b/work2/src/armor_detection/CMakeLists.txt
new file mode 100644
index 0000000..561bb1f
--- /dev/null
+++ b/work2/src/armor_detection/CMakeLists.txt
@@ -0,0 +1,37 @@
+cmake_minimum_required(VERSION 3.8)
+project(armor_detection)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# 查找依赖
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(ament_index_cpp REQUIRED)
+
+# 添加可执行文件
+add_executable(armor_detector src/armor_detector.cpp)
+
+# 链接依赖库
+ament_target_dependencies(armor_detector
+ rclcpp
+ OpenCV
+ ament_index_cpp
+)
+target_link_libraries(armor_detector ${OpenCV_LIBS})
+
+# 安装可执行文件
+install(TARGETS
+ armor_detector
+ DESTINATION lib/${PROJECT_NAME}
+)
+
+# 安装图片资源
+install(DIRECTORY
+ images
+ DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()
\ No newline at end of file
diff --git a/work2/src/armor_detection/images/work2.png b/work2/src/armor_detection/images/work2.png
new file mode 100644
index 0000000..2d1da41
Binary files /dev/null and b/work2/src/armor_detection/images/work2.png differ
diff --git a/work2/src/armor_detection/package.xml b/work2/src/armor_detection/package.xml
new file mode 100644
index 0000000..62bbbcc
--- /dev/null
+++ b/work2/src/armor_detection/package.xml
@@ -0,0 +1,21 @@
+
+
+
+ armor_detection
+ 0.0.0
+ Blue armor detection with ROS 2 and OpenCV
+ your_name
+ Apache-2.0
+
+ ament_cmake
+ rclcpp
+ opencv2
+ ament_index_cpp
+
+ ament_cmake
+ ament_cmake
+
+
+ ament_cmake
+
+
\ No newline at end of file
diff --git a/work2/src/armor_detection/src/armor_detector.cpp b/work2/src/armor_detection/src/armor_detector.cpp
new file mode 100644
index 0000000..8b472a3
--- /dev/null
+++ b/work2/src/armor_detection/src/armor_detector.cpp
@@ -0,0 +1,211 @@
+#include
+#include
+#include
+#include
+#include
+#include
+
+using namespace cv;
+using namespace std;
+
+class ArmorDetector : public rclcpp::Node {
+public:
+ ArmorDetector() : Node("armor_detector") {
+ string package_path;
+ try {
+ package_path = ament_index_cpp::get_package_share_directory("armor_detection");
+ } catch (...) {
+ RCLCPP_ERROR(this->get_logger(), "找不到包 'armor_detection'!");
+ return;
+ }
+
+ string image_path = package_path + "/images/work2.png";
+ RCLCPP_INFO(this->get_logger(), "图片路径: %s", image_path.c_str());
+ detectArmor(image_path);
+ }
+
+private:
+ struct LightBar {
+ Rect rect;
+ Point center;
+ double area;
+ float ratio;
+ int y_center;
+ };
+
+ void stepShow(const string& title, const Mat& img, int delay = 1000) {
+ namedWindow(title, WINDOW_AUTOSIZE);
+ imshow(title, img);
+ RCLCPP_INFO(this->get_logger(), "显示窗口: %s（%dms后关闭）", title.c_str(), delay);
+ waitKey(delay);
+ destroyWindow(title);
+ }
+
+ // 1. 读取图片
+ void detectArmor(const string& image_path) {
+ Mat img = imread(image_path, IMREAD_COLOR);
+ if (img.empty()) {
+ RCLCPP_ERROR(this->get_logger(), "图片读取失败！请检查路径");
+ Mat err = Mat::zeros(480, 640, CV_8UC3);
+ err.setTo(Scalar(0,0,255));
+ putText(err, "Image Not Found", Point(50,240), FONT_HERSHEY_SIMPLEX, 1.0, Scalar(255,255,255), 2);
+ stepShow("错误", err, 3000);
+ return;
+ }
+ stepShow("1. 原始图像", img);
+
+ // 2. 提取蓝色
+ Mat hsv, mask;
+ cvtColor(img, hsv, COLOR_BGR2HSV);
+ Scalar lower_blue = Scalar(80, 30, 30);
+ Scalar upper_blue = Scalar(160, 255, 255);
+ inRange(hsv, lower_blue, upper_blue, mask);
+ stepShow("2. 蓝色区域（放宽阈值）", mask);
+
+ // 3. 轻度去噪
+ Mat kernel = getStructuringElement(MORPH_RECT, Size(2,2));
+ morphologyEx(mask, mask, MORPH_CLOSE, kernel, Point(-1,-1), 1);
+ stepShow("3. 轻度去噪后", mask);
+
+ // 4. 识别灯条
+ vector> contours;
+ findContours(mask, contours, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);
+
+ vector light_bars;
+ // 5. 框选所有非倒影的装甲板
+ Mat light_img = img.clone();
+ for (const auto& cnt : contours) {
+ double area = contourArea(cnt);
+ if (area < 20) {
+ RCLCPP_DEBUG(this->get_logger(), "过滤小轮廓（面积=%.1f）", area);
+ continue;
+ }
+
+ Rect rect = boundingRect(cnt);
+ float ratio = (float)rect.width / rect.height;
+ if (ratio > 0.8) {
+ RCLCPP_DEBUG(this->get_logger(), "过滤宽高比过大的轮廓（ratio=%.2f）", ratio);
+ continue;
+ }
+
+ int y_center = rect.y + rect.height / 2;
+ LightBar lb;
+ lb.rect = rect;
+ lb.center = Point(rect.x + rect.width/2, y_center);
+ lb.area = area;
+ lb.ratio = ratio;
+ lb.y_center = y_center;
+ light_bars.push_back(lb);
+
+ rectangle(light_img, rect, Scalar(0,0,255), 2);
+ putText(light_img, to_string(light_bars.size()), lb.center,
+ FONT_HERSHEY_SIMPLEX, 0.5, Scalar(255,255,255), 1);
+ }
+
+ RCLCPP_INFO(this->get_logger(), "识别到灯条数量: %zu", light_bars.size());
+ stepShow("4. 识别到的灯条", light_img, 2000);
+
+ Mat final_img = light_img.clone();
+ vector valid_armors; // 存储有效装甲板，避免重复框选
+
+ int left_area_end = img.cols * 2 / 3; // 左区域
+ int right_area_start = img.cols * 1 / 3; // 右区域
+
+ // 配对左装甲板
+ for (size_t i = 0; i < light_bars.size(); ++i) {
+ for (size_t j = i + 1; j < light_bars.size(); ++j) {
+ LightBar& l = light_bars[i];
+ LightBar& r = light_bars[j];
+ if (l.center.x > r.center.x) swap(l, r);
+
+ // 左装甲板的灯条都在左2/3区域
+ if (r.center.x > left_area_end) continue;
+ // 过滤倒影
+ if (l.y_center > img.rows * 0.65 || r.y_center > img.rows * 0.65) continue;
+ // 间距合理
+ double dist = r.center.x - l.center.x;
+ double avg_h = (l.rect.height + r.rect.height) / 2;
+ if (dist < avg_h * 1.5 || dist > avg_h * 4) continue;
+ // 高度/垂直偏差正常
+ if (abs(l.rect.height - r.rect.height) > avg_h * 0.4) continue;
+ if (abs(l.center.y - r.center.y) > avg_h * 0.3) continue;
+
+ // 左装甲板有效，加入列表
+ Rect armor(
+ min(l.rect.x, r.rect.x), min(l.rect.y, r.rect.y),
+ max(l.rect.x + l.rect.width, r.rect.x + r.rect.width) - min(l.rect.x, r.rect.x),
+ max(l.rect.y + l.rect.height, r.rect.y + r.rect.height) - min(l.rect.y, r.rect.y)
+ );
+ valid_armors.push_back(armor);
+ }
+ }
+
+ // 配对右装甲板
+ for (size_t i = 0; i < light_bars.size(); ++i) {
+ for (size_t j = i + 1; j < light_bars.size(); ++j) {
+ LightBar& l = light_bars[i];
+ LightBar& r = light_bars[j];
+ if (l.center.x > r.center.x) swap(l, r);
+
+ if (l.center.x < right_area_start) continue;
+ if (l.y_center > img.rows * 0.65 || r.y_center > img.rows * 0.65) continue;
+ double dist = r.center.x - l.center.x;
+ double avg_h = (l.rect.height + r.rect.height) / 2;
+ if (dist < avg_h * 1.5 || dist > avg_h * 4) continue;
+
+ if (abs(l.rect.height - r.rect.height) > avg_h * 0.4) continue;
+ if (abs(l.center.y - r.center.y) > avg_h * 0.3) continue;
+
+ // 右装甲板有效，加入列表
+ Rect armor(
+ min(l.rect.x, r.rect.x), min(l.rect.y, r.rect.y),
+ max(l.rect.x + l.rect.width, r.rect.x + r.rect.width) - min(l.rect.x, r.rect.x),
+ max(l.rect.y + l.rect.height, r.rect.y + r.rect.height) - min(l.rect.y, r.rect.y)
+ );
+ valid_armors.push_back(armor);
+ }
+ }
+
+ // 绘制有效装甲板
+ for (size_t k = 0; k < valid_armors.size(); ++k) {
+ bool is_dup = false;
+ for (size_t m = 0; m < k; ++m) {
+ Rect overlap = valid_armors[k] & valid_armors[m];
+ double overlap_ratio = (double)overlap.area() / valid_armors[k].area();
+ if (overlap_ratio > 0.5) {
+ is_dup = true;
+ break;
+ }
+ }
+ if (!is_dup) {
+ rectangle(final_img, valid_armors[k], Scalar(0,255,0), 2);
+ putText(final_img, "Armor " + to_string(k+1), Point(valid_armors[k].x, valid_armors[k].y-10),
+ FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0,255,0), 2);
+ }
+ }
+
+ // 提示信息
+ if (valid_armors.empty() && !light_bars.empty()) {
+ putText(final_img, "No Valid Armor (Filtered Reflection)", Point(50,50),
+ FONT_HERSHEY_SIMPLEX, 0.8, Scalar(0,0,255), 2);
+ } else if (light_bars.empty()) {
+ putText(final_img, "No Enough Light Bars", Point(50,50),
+ FONT_HERSHEY_SIMPLEX, 0.8, Scalar(0,0,255), 2);
+ }
+
+ // 显示最终结果
+ namedWindow("5. 最终结果", WINDOW_NORMAL);
+ resizeWindow("5. 最终结果", 1024, 768);
+ imshow("5. 最终结果", final_img);
+ waitKey(0);
+ destroyAllWindows();
+ }
+};
+
+int main(int argc, char**argv) {
+ rclcpp::init(argc, argv);
+ auto node = make_shared();
+ rclcpp::spin(node);
+ rclcpp::shutdown();
+ return 0;
+}
\ No newline at end of file