整理文件夹目录

recharge/interface/CMakeLists.txt

@@ -1,6 +1,10 @@
 cmake_minimum_required(VERSION 3.8)
 project(interface)
 
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
@@ -10,22 +14,13 @@ find_package(ament_cmake REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 
+# 生成接口 - 这个命令会自动处理所有需要的导出
 rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/SceneControls.srv"
-  "msg/Empty.msg"
   DEPENDENCIES std_msgs
 )
 
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # comment the line when a copyright and license is added to all source files
-  set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # comment the line when this package is in a git repo and when
-  # a copyright and license is added to all source files
-  set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
-endif()
+# 对于简单的接口包，只需要这个就够了
+# rosidl_generate_interfaces 会自动处理头文件安装和导出
 
-ament_package()
+ament_package()

recharge/interface/package.xml

@@ -3,17 +3,17 @@
 <package format="3">
   <name>interface</name>
   <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="nvidia@todo.todo">nvidia</maintainer>
-  <license>TODO: License declaration</license>
+  <description>Custom interface package for SceneControls service</description>
+  <maintainer email="zhangkaifeng@users.noreply.github.com">zkf</maintainer>
+  <license>Apache-2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>rosidl_default_generators</buildtool_depend>
 
   <depend>std_msgs</depend>
+  <depend>rosidl_default_runtime</depend>
+  
   <member_of_group>rosidl_interface_packages</member_of_group>
-  <exec_depend>rosidl_default_runtime</exec_depend>
-  <build_depend>rosidl_default_generators</build_depend>
   
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
@@ -21,4 +21,4 @@
   <export>
     <build_type>ament_cmake</build_type>
   </export>
-</package>
+</package>

recharge/recharge/CMakeLists.txt

@@ -1,80 +0,0 @@
-cmake_minimum_required(VERSION 3.8)
-if(NOT CMAKE_BUILD_TYPE)
-  set(CMAKE_BUILD_TYPE "Debug")
-endif()
-project(recharge)
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-# 设置C++标准
-set(CMAKE_CXX_STANDARD 17)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -g -O0 -DDEBUG")
-set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3")
-# find dependencies
-find_package(ament_cmake REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(sensor_msgs REQUIRED)
-find_package(geometry_msgs REQUIRED)
-find_package(visualization_msgs REQUIRED)
-find_package(std_msgs REQUIRED)
-find_package(tf2 REQUIRED)
-find_package(tf2_ros REQUIRED)
-find_package(tf2_geometry_msgs REQUIRED)
-find_package(Eigen3 REQUIRED)
-find_package(interface REQUIRED)
-find_package(nav_msgs REQUIRED)
-
-include_directories(
-  ${CMAKE_CURRENT_SOURCE_DIR}/include
-)
-
-# 创建可执行文件
-add_executable(recharge_node
-  src/main.cpp
-  src/recharge.cpp
-)
-ament_target_dependencies(
-  recharge_node
-  rclcpp
-  std_msgs
-  sensor_msgs
-  geometry_msgs
-  nav_msgs
-  visualization_msgs
-  tf2
-  tf2_ros
-  tf2_geometry_msgs
-  interface
-)
-# 链接库
-target_include_directories(recharge_node PRIVATE
-    ${Eigen3_INCLUDE_DIRS}
-    ${CMAKE_CURRENT_SOURCE_DIR}/include
-)
-
-target_compile_options(recharge_node PRIVATE
-    $<$<CONFIG:Debug>:-g -O0>
-    $<$<CONFIG:Release>:-O3>
-)
-
-# 安装目标
-install(TARGETS recharge_node
-    DESTINATION lib/${PROJECT_NAME}
-)
-
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # comment the line when a copyright and license is added to all source files
-  set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # comment the line when this package is in a git repo and when
-  # a copyright and license is added to all source files
-  set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
-endif()
-
-ament_package()

recharge/recharge/include/recharge/recharge.hpp

@@ -1,238 +0,0 @@
-#ifndef RECHARGE_HPP
-#define RECHARGE_HPP
-
-#include <rclcpp/rclcpp.hpp>
-#include <sensor_msgs/msg/laser_scan.hpp>
-#include <geometry_msgs/msg/twist.hpp>
-#include <geometry_msgs/msg/point.hpp>
-#include <visualization_msgs/msg/marker_array.hpp>
-#include <visualization_msgs/msg/marker.hpp>
-#include <std_msgs/msg/bool.hpp>
-#include <std_msgs/msg/int32.hpp>
-#include <std_msgs/msg/string.hpp>
-#include <std_msgs/msg/empty.hpp>
-#include <interface/srv/scene_controls.hpp>
-#include <interface/msg/empty.hpp>
-
-#include <cmath>
-#include <vector>
-#include <memory>
-#include <string>
-#include <algorithm>
-#include <deque>
-#include <numeric>
-#include <Eigen/Dense>
-
-using namespace std::chrono_literals;
-
-// 数据结构
-struct IndexPoint
-{
-    double x;
-    double y;
-    int index;
-
-    IndexPoint(double x = 0.0, double y = 0.0, int index = 0)
-        : x(x), y(y), index(index) {}
-};
-
-struct StraightLine
-{
-    double k;
-    double b;
-
-    StraightLine(double k = 0.0, double b = 0.0) : k(k), b(b) {}
-};
-
-struct ScanFilterParam
-{
-    int min_index = 292;
-    int max_index = 382;
-    double min_intensities = 120.0;
-    double max_intensities = 220.0;
-    double min_distance = 0.28;
-    double max_distance = 3.5;
-    int min_count = 4;
-    double dock_length = 0.42;
-    int baselink_back_index = 337;
-};
-
-struct Result
-{
-    int frameid;
-    sensor_msgs::msg::LaserScan::SharedPtr laserScanData;
-    std::vector<IndexPoint> points;
-    std::vector<IndexPoint> left_points;
-    std::vector<IndexPoint> right_points;
-    IndexPoint middle_point;
-    StraightLine middle_line;
-    StraightLine left_line;
-    StraightLine right_line;
-};
-
-// 充电状态枚举
-enum class ChargingState
-{
-    IDLE,
-    STARTING,
-    NAVIGATING,
-    CHARGING,
-    UNAVAILABLE,
-    COMPLETED
-};
-
-// 工具类
-class ToolClass
-{
-public:
-    static double angleBetween(const IndexPoint &p1, const IndexPoint &p2);
-    static double angleDiff(double a1, double a2);
-    static double averageAngle(const std::vector<double> &angles);
-
-    // 分段函数
-    static std::vector<std::vector<IndexPoint>> splitPointsToSegments(
-        const std::vector<IndexPoint> &points,
-        double angle_threshold_deg = 15.0,
-        int history_window_size = 3);
-};
-
-// 主节点类
-class ScanFilterNode : public rclcpp::Node
-{
-public:
-    ScanFilterNode();
-
-private:
-    void scanCallback1(const sensor_msgs::msg::LaserScan::SharedPtr msg, int begin_index, int end_index);
-    void onPointPartition(Result &result);
-    void procData(Result &result);
-    void rvizShowResult(const Result &result);
-    sensor_msgs::msg::LaserScan::SharedPtr resetLaserData(sensor_msgs::msg::LaserScan::SharedPtr msg, int b_index, int e_index);
-    Eigen::MatrixXd getPointList(const std::vector<IndexPoint> &points);
-    std::vector<std::pair<int, int>> getSections(
-        int begin_index, int end_index,
-        const sensor_msgs::msg::LaserScan::SharedPtr msg,
-        const std::map<int, std::pair<double, double>> &xy_list);
-
-    std::map<int, std::tuple<int, int, double, double, double, double, double, int>> 
-    getSegments(const std::map<int, std::pair<double, double>> &xy_points_map);
-
-    std::map<int, std::tuple<int, int, double, double, double, double, double, int>> filterSegments(
-    const std::map<int, std::tuple<int, int, double, double, double, double, double, int>> &turn_segments);
-    void scanCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg);
-    void onCmdEnable(const std_msgs::msg::Bool::SharedPtr msg);
-    void timerCallback();
-    void publishFilteredScan(const sensor_msgs::msg::LaserScan::SharedPtr original_msg,
-                             const std::vector<float> &filtered_ranges);
-
-    // 激光数据处理函数
-    std::map<int, std::pair<double, double>> laserScanToXY(const sensor_msgs::msg::LaserScan::SharedPtr scan_msg);
-    std::map<int, std::pair<double, double>> laserScanToXY1(int begin_index, int end_index,
-                                                            const sensor_msgs::msg::LaserScan::SharedPtr scan_msg);
-
-    // 过滤函数
-    void filterScanModify0(sensor_msgs::msg::LaserScan::SharedPtr msg);
-    void filterScanModify1(sensor_msgs::msg::LaserScan::SharedPtr msg);
-    void filterScanModify2(sensor_msgs::msg::LaserScan::SharedPtr msg);
-    void filterScanModify3(sensor_msgs::msg::LaserScan::SharedPtr msg);
-    std::pair<int, int> filterScanModify4(sensor_msgs::msg::LaserScan::SharedPtr msg);
-
-    // 直线拟合
-    static std::pair<double, double> fitLineLeastSquares(const Eigen::MatrixXd &points);
-
-    // 控制函数
-    void ctrlProc(Result& result);
-    static std::pair<double, double> computeDockingVelocity(double dx, double dy, double yaw_error);
-
-    // 辅助函数
-    static bool isValid(float value);
-    int getFixedIndex(const sensor_msgs::msg::LaserScan::SharedPtr msg, int i);
-    int getFixed();
-
-    // ROS2成员
-    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
-    rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub_filtered_;
-    rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub_line_;
-    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr pub_ctrl_;
-    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr sub_cmd_enable_;
-    rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_rviz_;
-    rclcpp::TimerBase::SharedPtr timer_;
-
-    // 状态变量
-    bool cmd_enable_ = false;
-    geometry_msgs::msg::Twist ctrl_twist_;
-    rclcpp::Time ctrl_twist_time_;
-    ScanFilterParam param_;
-    double distance_threshold_;
-    int fit_window_size_;
-    double fit_residual_threshold_;
-    int frame_index_ = 0;
-    int counter_ = 0;
-    rclcpp::Time detect_station_time_;
-};
-
-// 充电管理类
-class ChargingManager : public rclcpp::Node
-{
-public:
-    ChargingManager();
-
-    void startCharging();
-    void stopCharging();
-
-private:
-    void onCtrl(const std_msgs::msg::String::SharedPtr msg);
-    void executeChargingControl();
-    void monitorCharging();
-    void changeState(ChargingState new_state);
-    void reportState(ChargingState state);
-    bool detectStation();
-    bool detectChargingVoltage();
-    std::pair<double, double> generateControlCommand();
-    void applyControlCommand(double linear_vel, double angular_vel);
-
-    // ROS2成员
-    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_ctrl_;
-
-    // 状态变量
-    ChargingState state_ = ChargingState::IDLE;
-    rclcpp::Time last_station_detection_time_;
-    rclcpp::Time last_voltage_detection_time_;
-    double station_lost_timeout_ = 3.0;
-    double control_interval_ = 0.1;
-};
-
-class RechargerRecv : public rclcpp::Node
-{
-public:
-    RechargerRecv();
-    ~RechargerRecv();
-
-private:
-    void onGetCharging(const std_msgs::msg::Int32::SharedPtr msg);
-    void onModeChange(const std_msgs::msg::String::SharedPtr msg);
-    void reportLoop();
-    void callService(int start);
-    void changeStatus(int status);
-
-    // ROS2成员
-    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr getcharging_sub_;
-    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_mode_change_;
-    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr pub_feed_;
-    rclcpp::Client<interface::srv::SceneControls>::SharedPtr client_;
-
-    // 状态变量
-    int last_report_status_;
-    int last_status_;
-    bool running_;
-    std::thread thread_;
-};
-
-// 全局函数声明
-bool isCharging();
-void setChargingVoltage(bool value);
-bool getChargingVoltage();
-void setMode(const std::string &mode);
-std::string getMode();
-std::pair<double, double> calculateAngleBisector(double k1, double b1, double k2, double b2);
-#endif // RECHARGE_HPP

recharge/recharge/package.xml

@@ -1,30 +0,0 @@
-<?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>recharge</name>
-  <version>0.0.0</version>
-  <description>Automatic recharging system for robots</description>
-  <maintainer email="nvidia@todo.todo">nvidia</maintainer>
-  <license>Apache License 2.0</license>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-
-  <depend>rclcpp</depend>
-  <depend>sensor_msgs</depend>
-  <depend>geometry_msgs</depend>
-  <depend>visualization_msgs</depend>
-  <depend>std_msgs</depend>
-  <depend>tf2</depend>
-  <depend>tf2_ros</depend>
-  <depend>tf2_geometry_msgs</depend>
-  <depend>interface</depend>
-  <build_depend>eigen</build_depend>
-  <exec_depend>eigen</exec_depend>
-  <depend>recharge_interfaces</depend>
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>

recharge/recharge/src/main.cpp

@@ -1,41 +0,0 @@
-#include "recharge/recharge.hpp"
-#include <rclcpp/executors/multi_threaded_executor.hpp>
-
-// 如果使用可视化节点，需要包含其头文件
-// #include "visualization_debug.hpp"  // 创建对应的头文件
-
-int main(int argc, char** argv) {
-    rclcpp::init(argc, argv);
-    
-    // 创建执行器
-    auto executor = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
-    
-    // 创建各个节点
-    auto default_node = std::make_shared<rclcpp::Node>("default_node");
-    auto node_ctrl = std::make_shared<ScanFilterNode>();
-    auto node_manager = std::make_shared<ChargingManager>();
-    auto node_can = std::make_shared<RechargerRecv>();
-    
-    // 如果需要可视化，取消注释
-    //auto visualizer = std::make_shared<RechargeVisualizer>();
-    
-    // 将节点添加到执行器
-    executor->add_node(default_node);
-    executor->add_node(node_ctrl);
-    executor->add_node(node_manager);
-    executor->add_node(node_can);
-    
-    // 如果需要可视化，取消注释
-    // executor->add_node(visualizer);
-    
-    try {
-        executor->spin();
-    } catch (const std::exception& e) {
-        std::cerr << "Exception: " << e.what() << std::endl;
-    } catch (...) {
-        std::cerr << "Unknown exception" << std::endl;
-    }
-    
-    rclcpp::shutdown();
-    return 0;
-}

recharge/recharge/src/recharge.cpp

@@ -1,1673 +0,0 @@
-#include "recharge/recharge.hpp"
-#include <rclcpp/executors/multi_threaded_executor.hpp>
-#include <rclcpp/qos.hpp>
-#include <tf2/LinearMath/Quaternion.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#include <chrono>
-#include <functional>
-#include <thread>
-#define DEBUG_SCAN 1
-
-using std::placeholders::_1;
-
-// 全局变量
-static std::string last_mode = "";
-static bool detect_charging_voltage = false;
-static rclcpp::Time detect_charging_voltage_time;
-
-// 全局函数实现
-bool isCharging()
-{
-    return last_mode == "recharge";
-}
-
-// 在全局函数部分添加
-std::pair<double, double> calculateAngleBisector(double k1, double b1, double k2, double b2)
-{
-    // 计算两条直线的夹角
-    double angle1 = atan(k1);
-    double angle2 = atan(k2);
-
-    // 计算两条直线之间的夹角
-    double delta_angle = fabs(angle1 - angle2);
-
-    // 计算中线角度
-    double bisector_angle1;
-    if (angle1 < angle2)
-    {
-        bisector_angle1 = angle1 + delta_angle / 2;
-    }
-    else
-    {
-        bisector_angle1 = angle2 + delta_angle / 2;
-    }
-
-    // 处理平行线（无交点）的情况
-    if (fabs(k1 - k2) < 1e-10)
-    {
-        // 平行线没有交点，返回原直线
-        return {k1, b1};
-    }
-
-    // 计算交点坐标
-    double x_intersect = (b2 - b1) / (k1 - k2);
-    double y_intersect = k1 * x_intersect + b1;
-
-    // 计算平分线的斜率
-    double k3 = tan(bisector_angle1);
-    double b3 = y_intersect - k3 * x_intersect;
-
-    return {k3, b3};
-}
-
-void setChargingVoltage(bool value)
-{
-    detect_charging_voltage = value;
-    detect_charging_voltage_time = rclcpp::Clock().now();
-}
-
-bool getChargingVoltage()
-{
-    auto now = rclcpp::Clock().now();
-    auto diff = now - detect_charging_voltage_time;
-    if (detect_charging_voltage && diff.seconds() > 0.1)
-    {
-        return true;
-    }
-    return false;
-}
-
-void setMode(const std::string &mode)
-{
-    last_mode = mode;
-    if (last_mode != "recharge")
-    {
-        setChargingVoltage(false);
-    }
-}
-
-std::string getMode()
-{
-    return last_mode;
-}
-
-// ToolClass 实现
-double ToolClass::angleBetween(const IndexPoint &p1, const IndexPoint &p2)
-{
-    return atan2(p2.y - p1.y, p2.x - p1.x);
-}
-
-double ToolClass::angleDiff(double a1, double a2)
-{
-    double diff = fabs(a1 - a2);
-    return std::min(diff, 2 * M_PI - diff) * 180.0 / M_PI;
-}
-
-double ToolClass::averageAngle(const std::vector<double> &angles)
-{
-    double sin_sum = 0.0, cos_sum = 0.0;
-    for (double a : angles)
-    {
-        sin_sum += sin(a);
-        cos_sum += cos(a);
-    }
-    return atan2(sin_sum, cos_sum);
-}
-
-std::vector<std::vector<IndexPoint>> ToolClass::splitPointsToSegments(
-    const std::vector<IndexPoint> &points,
-    double angle_threshold_deg,
-    int history_window_size)
-{
-    RCLCPP_INFO(rclcpp::get_logger("toolclass"), 
-                "splitPointsToSegments开始: %zu个点", points.size());
-    std::vector<std::vector<IndexPoint>> segments;
-    if (points.size() < 2)
-    {
-        segments.push_back(points);
-        return segments;
-    }
-
-    std::vector<IndexPoint> current_segment;
-    std::deque<double> angle_history; // 改为 deque 方便操作
-
-    current_segment.push_back(points[0]);
-
-    for (size_t i = 1; i < points.size(); i++)
-    {
-         RCLCPP_INFO(rclcpp::get_logger("toolclass"),
-                    "点[%zu]: (%.3f, %.3f), 角度历史大小: %zu", i, points[i].x, points[i].y, angle_history.size());
-        const IndexPoint &p_prev = points[i - 1];
-        const IndexPoint &p_curr = points[i];
-        double curr_angle = angleBetween(p_prev, p_curr);
-
-        // 更新方向历史
-        angle_history.push_back(curr_angle);
-        if (angle_history.size() > history_window_size)
-        {
-            angle_history.pop_front();
-        }
-
-        // 若历史方向足够，开始判断角度差
-        if (angle_history.size() >= history_window_size)
-        {
-            // 计算历史方向平均值（排除当前角度）
-            std::vector<double> hist(angle_history.begin(), angle_history.end() - 1);
-            double avg_angle = averageAngle(hist);
-            double diff = angleDiff(avg_angle, curr_angle);
-            RCLCPP_INFO(rclcpp::get_logger("toolclass"),
-                        "  角度差: %.1f° (阈值: %.1f°)", diff, angle_threshold_deg);
-            if (diff > angle_threshold_deg)
-            {
-                RCLCPP_INFO(rclcpp::get_logger("toolclass"),
-                            "  检测到角度变化，创建新分段");
-                // 发生显著拐弯，分段
-                // 将 p_prev 加入当前分段
-                current_segment.push_back(p_prev);
-                segments.push_back(current_segment);
-
-                // 开始新分段，以当前点为起点
-                current_segment.clear();
-                current_segment.push_back(p_curr);
-
-                // 重置方向历史，仅保留当前角度
-                angle_history.clear();
-                angle_history.push_back(curr_angle);
-                 // 跳过下面的 current_segment.push_back(p_curr);
-            }
-        }
-
-        // 正常情况：将当前点加入当前分段
-       // current_segment.push_back(p_curr);
-    }
-    current_segment.push_back(points.back());
-    segments.push_back(current_segment);
-    RCLCPP_INFO(rclcpp::get_logger("toolclass"),
-                "分段完成: 共%zu个分段", segments.size());
-    return segments;
-}
-
-// ScanFilterNode 实现
-ScanFilterNode::ScanFilterNode() : Node("scan_filter_node")
-{
-
-    // 声明参数
-    this->declare_parameter<double>("distance_threshold", 0.5);
-    this->declare_parameter<int>("fit_window_size", 10);
-    this->declare_parameter<double>("fit_residual_threshold", 0.09);
-
-    // 获取参数
-    distance_threshold_ = this->get_parameter("distance_threshold").as_double();
-    fit_window_size_ = this->get_parameter("fit_window_size").as_int();
-    fit_residual_threshold_ = this->get_parameter("fit_residual_threshold").as_double();
-
-    // QoS配置
-    auto qos_profile = rclcpp::QoS(rclcpp::KeepLast(10))
-                           .best_effort()
-                           .durability_volatile();
-
-    // 创建订阅者和发布者
-    scan_sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
-        "/scan/back", qos_profile,
-        std::bind(&ScanFilterNode::scanCallback, this, _1));
-
-    pub_filtered_ = this->create_publisher<sensor_msgs::msg::LaserScan>("/scan/out", 10);
-    pub_line_ = this->create_publisher<sensor_msgs::msg::LaserScan>("/scan/out_line", 10);
-    pub_ctrl_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel_raw", 10);
-
-    sub_cmd_enable_ = this->create_subscription<std_msgs::msg::Bool>(
-        "/recharge/cmd_enable", 10,
-        std::bind(&ScanFilterNode::onCmdEnable, this, _1));
-
-    pub_rviz_ = this->create_publisher<visualization_msgs::msg::MarkerArray>(
-        "/line_marker_array", 10);
-
-    // 初始化控制消息
-    ctrl_twist_.linear.x = 0.0;
-    ctrl_twist_.angular.z = 0.0;
-    ctrl_twist_time_ = this->now();
-
-    // 创建定时器
-    timer_ = this->create_wall_timer(
-        100ms, std::bind(&ScanFilterNode::timerCallback, this));
-
-    detect_station_time_ = this->now() - rclcpp::Duration(100.0, 0);
-}
-
-Eigen::MatrixXd ScanFilterNode::getPointList(const std::vector<IndexPoint> &points)
-{
-    Eigen::MatrixXd pts(points.size(), 2);
-
-    for (size_t i = 0; i < points.size(); i++)
-    {
-        pts(i, 0) = points[i].x;
-        pts(i, 1) = points[i].y;
-    }
-
-    return pts;
-}
-
-void ScanFilterNode::procData(Result &result)
-{
-    RCLCPP_INFO(get_logger(), "=== procData 开始 ===");
-    RCLCPP_INFO(get_logger(), "左点数量: %zu", result.left_points.size());
-    RCLCPP_INFO(get_logger(), "右点数量: %zu", result.right_points.size());
-    
-    if (result.left_points.size() < 2 || result.right_points.size() < 2) {
-        RCLCPP_WARN(get_logger(), "点数量不足，跳过拟合");
-        return;
-    }
-    
-    // 打印前几个点用于调试
-    for (int i = 0; i < std::min(3, (int)result.left_points.size()); i++) {
-        RCLCPP_INFO(get_logger(), "左点[%d]: (%.3f, %.3f)", 
-                   i, result.left_points[i].x, result.left_points[i].y);
-    }
-    
-    for (int i = 0; i < std::min(3, (int)result.right_points.size()); i++) {
-        RCLCPP_INFO(get_logger(), "右点[%d]: (%.3f, %.3f)", 
-                   i, result.right_points[i].x, result.right_points[i].y);
-    }
-
-    // 获取点集矩阵
-    Eigen::MatrixXd point_list_left = getPointList(result.left_points);
-    Eigen::MatrixXd point_list_right = getPointList(result.right_points);
-
-    // 拟合左边直线
-    auto [k1, b1] = fitLineLeastSquares(point_list_left);
-    RCLCPP_INFO(get_logger(), "左直线拟合: y = %.4fx + %.4f", k1, b1);
-
-    // 拟合右边直线
-    auto [k2, b2] = fitLineLeastSquares(point_list_right);
-    RCLCPP_INFO(get_logger(), "右直线拟合: y = %.4fx + %.4f", k2, b2);
-
-    // 检查是否平行
-    if (fabs(k1 - k2) < 1e-10) {
-        RCLCPP_WARN(get_logger(), "两直线平行，无交点");
-        result.middle_point = IndexPoint(0.0, 0.0, 0);
-        result.middle_line = StraightLine(0.0, 0.0);
-        result.left_line = StraightLine(k1, b1);
-        result.right_line = StraightLine(k2, b2);
-        return;
-    }
-
-    // 计算交点
-    double x = (b2 - b1) / (k1 - k2);
-    double y = k1 * x + b1;
-    RCLCPP_INFO(get_logger(), "交点: (%.4f, %.4f)", x, y);
-
-    // 更新中间点
-    result.middle_point = IndexPoint(x, y, 0);
-
-    // 计算角平分线
-    auto [k3, b3] = calculateAngleBisector(k1, b1, k2, b2);
-    RCLCPP_INFO(get_logger(), "角平分线: y = %.4fx + %.4f", k3, b3);
-
-    // 更新结果
-    result.middle_line = StraightLine(k3, b3);
-    result.left_line = StraightLine(k1, b1);
-    result.right_line = StraightLine(k2, b2);
-    
-    RCLCPP_INFO(get_logger(), "=== procData 完成 ===");
-}
-
-void ScanFilterNode::ctrlProc(Result &result)
-{
-    RCLCPP_INFO(get_logger(), "======== 进入控制模块 ========");
-    
-    // 检查是否检测到充电电压
-    if (getChargingVoltage()) {
-        RCLCPP_INFO(get_logger(), "检测到充电电压，停止移动");
-        ctrl_twist_.linear.x = 0.0;
-        ctrl_twist_.angular.z = 0.0;
-        ctrl_twist_time_ = this->now();
-        return;
-    }
-
-    // 使用 result 中已经计算好的数据
-    double dx = result.middle_point.x;
-    double dy = result.middle_point.y;
-    double k = result.middle_line.k;
-    
-    RCLCPP_INFO(get_logger(), "控制参数 - dx: %.4f, dy: %.4f, k: %.4f", dx, dy, k);
-    
-    // 调整 dy（与 Python 代码保持一致）
-    dy = dy - 0.10;
-    RCLCPP_INFO(get_logger(), "调整后 dy: %.4f", dy);
-
-    // 计算偏航角误差
-    double yaw_error = atan(k);
-    RCLCPP_INFO(get_logger(), "偏航角误差: %.4f rad (%.2f 度)", 
-               yaw_error, yaw_error * 180.0 / M_PI);
-
-    // 计算控制速度
-    auto [linear_velocity, angular_velocity] = computeDockingVelocity(dx - 0.3, dy, yaw_error);
-    RCLCPP_INFO(get_logger(), "基础控制速度 - 线速度: %.4f, 角速度: %.4f", 
-               linear_velocity, angular_velocity);
-
-    // 限制角速度（与 Python 代码保持一致）
-    if (angular_velocity > 0.0) {
-        angular_velocity = std::min(fabs(angular_velocity), 0.09);
-    } else {
-        angular_velocity = -std::min(fabs(angular_velocity), 0.09);
-    }
-    
-    // 更新检测时间
-    detect_station_time_ = this->now();
-
-    // 根据距离调整速度系数（与 Python 代码保持一致）
-    double radio_x = 1.0;
-    double radio_z = 1.0;
-
-    if (dx > 1.0) {
-        // 保持默认
-    } else if (dx > 0.8) {
-        radio_x = 0.7;
-    } else if (dx > 0.7) {
-        radio_x = 0.5;
-    } else if (dx > 0.68) {
-        radio_x = 0.3;
-        radio_z = 1.0;
-    } else if (dx > 0.65) {
-        radio_x = 0.2;
-        radio_z = 1.0;
-    } else if (dx > 0.62) {
-        radio_x = 0.1;
-        radio_z = 1.0;
-    } else if (dx > 0.2) {
-        radio_x = 0.15;
-        radio_z = 1.0;
-    }
-    
-    RCLCPP_INFO(get_logger(), "距离调整系数 - radio_x: %.2f, radio_z: %.2f", radio_x, radio_z);
-
-    // 设置控制命令
-    if (dx > 0.54) {
-        // 计算最终线速度（注意：Python 中使用了 min(linear_velocity * radio_x, -0.04)）
-        double final_linear_vel = linear_velocity * radio_x;
-        if (final_linear_vel > -0.04) {
-            final_linear_vel = -0.04;  // 确保最小后退速度
-        }
-        
-        ctrl_twist_.linear.x = final_linear_vel;
-        ctrl_twist_.angular.z = angular_velocity * radio_z;
-        ctrl_twist_time_ = this->now();
-        
-        RCLCPP_INFO(get_logger(), "控制命令 - 线速度: %.4f, 角速度: %.4f", 
-                   ctrl_twist_.linear.x, ctrl_twist_.angular.z);
-    } else {
-        // 到达充电位置
-        RCLCPP_INFO(get_logger(), "到达充电位置 (dx=%.4f <= 0.54), 停止移动", dx);
-        ctrl_twist_.linear.x = 0.0;
-        ctrl_twist_.angular.z = 0.0;
-        ctrl_twist_time_ = this->now();
-    }
-    
-    // 更新帧索引
-    frame_index_++;
-    RCLCPP_INFO(get_logger(), "======== 控制模块结束 ========");
-}
-
-std::vector<std::pair<int, int>> ScanFilterNode::getSections(
-    int begin_index, int end_index,
-    const sensor_msgs::msg::LaserScan::SharedPtr msg,
-    const std::map<int, std::pair<double, double>> &xy_list)
-{
-
-    std::vector<std::pair<int, int>> sections;
-
-    int section_start = begin_index + 1;
-    int section_end = 0;
-
-    int last_index = -1;
-    double last_x = 0.0, last_y = 0.0;
-
-    for (int i = begin_index + 1; i < end_index - 1; i++)
-    {
-        if (xy_list.find(i) == xy_list.end())
-        {
-            continue;
-        }
-
-        auto curr = xy_list.at(i);
-        if (last_index < 0)
-        {
-            last_index = i;
-            last_x = curr.first;
-            last_y = curr.second;
-            continue;
-        }
-
-        double dx = curr.first - last_x;
-        double dy = curr.second - last_y;
-        double dist = sqrt(dx * dx + dy * dy);
-
-        if (dist > 0.05 || (i - last_index) > 2)
-        {
-            section_end = i;
-            if ((section_end - section_start) >= param_.min_count)
-            {
-                sections.emplace_back(section_start, section_end);
-            }
-            section_start = i;
-        }
-
-        last_index = i;
-        last_x = curr.first;
-        last_y = curr.second;
-    }
-
-    if (last_index > section_end + 10)
-    {
-        section_end = last_index;
-        if ((section_end - section_start) > param_.min_count)
-        {
-            sections.emplace_back(section_start, section_end);
-        }
-    }
-
-    return sections;
-}
-
-// getSegments函数，与Python代码完全一致
-std::map<int, std::tuple<int, int, double, double, double, double, double, int>> 
-ScanFilterNode::getSegments(const std::map<int, std::pair<double, double>> &xy_points_map)
-{
-    std::map<int, std::tuple<int, int, double, double, double, double, double, int>> turn_segments;
-
-    if (xy_points_map.empty())
-    {
-        return turn_segments;
-    }
-
-    // 1. 将点转换为路径（与Python完全一致）
-    std::vector<IndexPoint> path;
-    for (const auto &point : xy_points_map)
-    {
-        // Python: data = (value[0], value[1], index)
-        path.emplace_back(point.second.first, point.second.second, point.first);
-    }
-
-    // 2. 使用相同的参数进行分段
-    auto segments = ToolClass::splitPointsToSegments(path, 24.0, 4);
-
-    // 3. 处理每个分段（与Python完全一致）
-    for (size_t i = 0; i < segments.size(); i++)
-    {
-        if (segments[i].size() < 2)
-            continue;
-
-        // 获取分段的前两个点（与Python完全一致）
-        const auto &s_prev = segments[i][0];
-        const auto &s_next = segments[i][1];
-
-        // 计算距离和角度（与Python完全一致）
-        double dx = s_prev.x - s_next.x;
-        double dy = s_prev.y - s_next.y;
-        double distance = sqrt(dx * dx + dy * dy);
-        double line_angle = atan2(dy, dx);  // 注意：与Python的math.atan2一致
-
-        int index_start = s_prev.index;
-        int index_end = s_next.index;
-
-        // 应用相同的过滤条件（与Python完全一致）
-        if (distance > 0.14 && (index_end - index_start) > 3)
-        {
-            // 创建与Python完全相同的8元组
-            // Python: (index_start, index_end, 0.0, value[0], value[1], distance, line_angle, 1)
-            // 但这里应该是s_prev的x,y，不是value[0], value[1]
-            turn_segments[index_start] = std::make_tuple(
-                index_start,         // 0: 开始索引
-                index_end,           // 1: 结束索引  
-                0.0,                 // 2: 0.0（与Python一致）
-                s_prev.x,            // 3: x坐标
-                s_prev.y,            // 4: y坐标
-                distance,            // 5: 距离
-                line_angle,          // 6: 线角度
-                1                    // 7: 1（与Python一致）
-            );
-        }
-    }
-
-    return turn_segments;
-}
-// filterSegments函数，与Python代码完全一致
-std::map<int, std::tuple<int, int, double, double, double, double, double, int>> ScanFilterNode::filterSegments(
-    const std::map<int, std::tuple<int, int, double, double, double, double, double, int>> &turn_segments)
-{
-    std::map<int, std::tuple<int, int, double, double, double, double, double, int>> new_segments;
-    
-    // 与Python完全相同的条件
-    if (turn_segments.empty() || turn_segments.size() < 2)
-    {
-        return new_segments;
-    }
-
-    // 变量命名和逻辑与Python保持一致
-    bool last_line_angle_valid = false;
-    double last_line_angle = 0.0;
-    std::tuple<int, int, double, double, double, double, double, int> last_seg;
-    int last_key = -1;
-
-    // 遍历所有分段（与Python完全一致）
-    for (const auto &seg : turn_segments)
-    {
-        const auto &info = seg.second;
-        double line_angle = std::get<6>(info);  // 获取角度
-
-        if (last_line_angle_valid)
-        {
-            // 计算角度差（与Python完全一致：line_angle - last_line_angle）
-            double diff_angle = line_angle - last_line_angle;
-            
-            // 目标角度计算（与Python完全一致）
-            // Python: target_angle = 0.5*math.pi* (120/90.0)
-            // Python: target_angle = math.pi - target_angle
-            double target_angle = 0.5 * M_PI * (120.0 / 90.0);
-            target_angle = M_PI - target_angle;
-            double tolerance = 0.2;
-            
-            // 角度条件判断（与Python完全一致）
-            if (diff_angle > (target_angle - tolerance) && diff_angle < (target_angle + tolerance))
-            {
-                // 索引检查（与Python完全一致）
-                // Python: if right_seg[1][0] - left_seg[1][1] < 3:
-                // right_seg[1][0] = 右分段的开始索引 = std::get<0>(info)
-                // left_seg[1][1] = 左分段的结束索引 = std::get<1>(last_seg)
-                int right_start_idx = std::get<0>(info);
-                int left_end_idx = std::get<1>(last_seg);
-                
-                if (right_start_idx - left_end_idx < 3)
-                {
-                    // 添加到结果（与Python完全一致）
-                    new_segments[last_key] = last_seg;
-                    new_segments[seg.first] = info;
-                    break;  // 找到一对就跳出循环
-                }
-            }
-        }
-        
-        // 更新上一个分段信息（与Python完全一致）
-        last_line_angle = line_angle;
-        last_line_angle_valid = true;
-        last_seg = info;
-        last_key = seg.first;
-    }
-    
-    return new_segments;
-}
-void ScanFilterNode::onPointPartition(Result &result)
-{
-    RCLCPP_INFO(get_logger(), "开始处理点分区");
-    if (result.left_points.size() == 0 || result.right_points.size() == 0)
-    {
-        RCLCPP_ERROR(get_logger(), "左右两侧没有两个以上的点模拟线");
-        RCLCPP_ERROR(get_logger(), "左侧点数: %zu, 右侧点数: %zu",
-                     result.left_points.size(), result.right_points.size());
-        return;
-    }
-    procData(result);
-    // 调用控制处理
-    ctrlProc(result);
-
-    // 显示结果（RViz可视化）
-    rvizShowResult(result);
-    RCLCPP_INFO(get_logger(), "已发布可视化标记");
-}
-
-void ScanFilterNode::rvizShowResult(const Result &result)
-{
-    RCLCPP_INFO(get_logger(), "发布可视化标记 - 坐标系: laser_frame_back1");
-    RCLCPP_INFO(get_logger(), "中间点坐标: x=%.3f, y=%.3f",
-                result.middle_point.x, result.middle_point.y);
-    RCLCPP_INFO(get_logger(), "中线参数: k=%.3f, b=%.3f",
-                result.middle_line.k, result.middle_line.b);
-    double k = result.middle_line.k;
-    double b = result.middle_line.b;
-
-    double x1 = result.middle_point.x;
-    double x2 = x1 - 1.0; // 固定显示范围
-    if (x1 > 10.0 || x1 < -10.0)
-    {
-        x1 = 0.5;
-        x2 = -0.5;
-    }
-    double y1 = result.middle_line.k * x1 + result.middle_line.b;
-    double y2 = result.middle_line.k * x2 + result.middle_line.b;
-    RCLCPP_INFO(get_logger(), "线段端点: (%.3f, %.3f) -> (%.3f, %.3f)",
-                x1, y1, x2, y2);
-    // 创建标记数组
-    auto marker_array = std::make_shared<visualization_msgs::msg::MarkerArray>();
-
-    // 添加中线标记
-    visualization_msgs::msg::Marker line_marker;
-    line_marker.header.frame_id = "laser_frame_back1";
-    line_marker.header.stamp = this->now();
-    line_marker.ns = "rotating_line";
-    line_marker.id = 0;
-    line_marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
-    line_marker.action = visualization_msgs::msg::Marker::ADD;
-    line_marker.pose.orientation.w = 1.0;
-    line_marker.scale.x = 0.02;
-    line_marker.color.r = 1.0;
-    line_marker.color.a = 1.0;
-
-    geometry_msgs::msg::Point p1, p2;
-    p1.x = x1;
-    p1.y = y1;
-    p1.z = 0.0;
-    p2.x = x2;
-    p2.y = y2;
-    p2.z = 0.0;
-    line_marker.points.push_back(p1);
-    line_marker.points.push_back(p2);
-
-    marker_array->markers.push_back(line_marker);
-
-    // 添加左点标记
-    if (!result.left_points.empty())
-    {
-        visualization_msgs::msg::Marker left_marker;
-        left_marker.header.frame_id = "laser_frame_back1";
-        left_marker.header.stamp = this->now();
-        left_marker.ns = "left_point";
-        left_marker.id = 1;
-        left_marker.type = visualization_msgs::msg::Marker::SPHERE;
-        left_marker.action = visualization_msgs::msg::Marker::ADD;
-        left_marker.pose.orientation.w = 1.0;
-        left_marker.scale.x = 0.03;
-        left_marker.scale.y = 0.03;
-        left_marker.color.r = 1.0;
-        left_marker.color.a = 1.0;
-
-        left_marker.pose.position.x = result.left_points[0].x;
-        left_marker.pose.position.y = result.left_points[0].y;
-        left_marker.pose.position.z = 0.0;
-
-        marker_array->markers.push_back(left_marker);
-    }
-
-    // 添加右点标记
-    if (!result.right_points.empty())
-    {
-        visualization_msgs::msg::Marker right_marker;
-        right_marker.header.frame_id = "laser_frame_back1";
-        right_marker.header.stamp = this->now();
-        right_marker.ns = "right_point";
-        right_marker.id = 2;
-        right_marker.type = visualization_msgs::msg::Marker::SPHERE;
-        right_marker.action = visualization_msgs::msg::Marker::ADD;
-        right_marker.pose.orientation.w = 1.0;
-        right_marker.scale.x = 0.03;
-        right_marker.scale.y = 0.03;
-        right_marker.color.g = 1.0;
-        right_marker.color.a = 1.0;
-
-        right_marker.pose.position.x = result.right_points.back().x;
-        right_marker.pose.position.y = result.right_points.back().y;
-        right_marker.pose.position.z = 0.0;
-
-        marker_array->markers.push_back(right_marker);
-    }
-
-    // 添加中间点标记
-    visualization_msgs::msg::Marker middle_marker;
-    middle_marker.header.frame_id = "laser_frame_back1";
-    middle_marker.header.stamp = this->now();
-    middle_marker.ns = "middle_point";
-    middle_marker.id = 3;
-    middle_marker.type = visualization_msgs::msg::Marker::SPHERE;
-    middle_marker.action = visualization_msgs::msg::Marker::ADD;
-    middle_marker.pose.orientation.w = 1.0;
-    middle_marker.scale.x = 0.03;
-    middle_marker.scale.y = 0.03;
-    middle_marker.color.b = 1.0;
-    middle_marker.color.a = 1.0;
-
-    middle_marker.pose.position.x = result.middle_point.x;
-    middle_marker.pose.position.y = result.middle_point.y;
-    middle_marker.pose.position.z = 0.0;
-
-    marker_array->markers.push_back(middle_marker);
-
-    // 发布标记数组
-    pub_rviz_->publish(*marker_array);
-}
-
-void ScanFilterNode::onCmdEnable(const std_msgs::msg::Bool::SharedPtr msg)
-{
-    cmd_enable_ = msg->data;
-}
-
-void ScanFilterNode::timerCallback()
-{
-    if (!isCharging())
-        return;
-
-    auto now = this->now();
-    auto time_diff = (now - ctrl_twist_time_).seconds();
-
-    if (time_diff < 2.0)
-    {
-        double radio_x = 1.0;
-        double radio_z = 1.0;
-        geometry_msgs::msg::Twist msg;
-        msg.linear.x = ctrl_twist_.linear.x * radio_x;
-        msg.angular.z = ctrl_twist_.angular.z * radio_z;
-
-        if (cmd_enable_)
-        {
-            pub_ctrl_->publish(msg);
-        }
-    }
-}
-
-bool ScanFilterNode::isValid(float value)
-{
-    if (fabs(value) < 0.001)
-        return false;
-    return !std::isinf(value) && !std::isnan(value);
-}
-
-int ScanFilterNode::getFixed()
-{
-    return 0;
-}
-
-int ScanFilterNode::getFixedIndex(const sensor_msgs::msg::LaserScan::SharedPtr msg, int i)
-{
-    int fixed = getFixed();
-    if (fixed == 0)
-        return i;
-
-    int length = msg->ranges.size();
-    int index;
-
-    if (i >= length - fixed)
-    {
-        index = i - length + fixed;
-    }
-    else
-    {
-        index = i + fixed;
-    }
-    return index;
-}
-
-void ScanFilterNode::filterScanModify0(sensor_msgs::msg::LaserScan::SharedPtr msg)
-{
-#if DEBUG_SCAN
-    RCLCPP_INFO(get_logger(), "已进入角度偏移校正过滤");
-#endif
-    int fixed = getFixed();
-    double diff_yaw = msg->angle_increment * fixed;
-    msg->angle_min -= diff_yaw;
-    msg->angle_max -= diff_yaw;
-    std::vector<float> ranges = msg->ranges;
-    for (size_t i = 0; i < msg->ranges.size(); i++)
-    {
-        int index = getFixedIndex(msg, i);
-        ranges[index] = msg->ranges[i];
-    }
-    msg->ranges = ranges;
-}
-
-void ScanFilterNode::filterScanModify1(sensor_msgs::msg::LaserScan::SharedPtr msg)
-{
-#if DEBUG_SCAN
-    RCLCPP_INFO(get_logger(), "已进入角度范围过滤");
-#endif
-    // 过滤角度，只接受后面90度
-    int min_index = 292;
-
-    int max_index = 382;
-    for (size_t i = 0; i < msg->ranges.size(); i++)
-    {
-        if (i < min_index || i > max_index)
-        {
-            msg->ranges[i] = std::numeric_limits<float>::infinity();
-        }
-    }
-}
-void ScanFilterNode::filterScanModify2(sensor_msgs::msg::LaserScan::SharedPtr msg)
-{
-    RCLCPP_INFO(get_logger(), "已进入距离过滤");
-    // 过滤距离
-    for (size_t i = 0; i < msg->ranges.size(); i++)
-    {
-        if (!isValid(msg->ranges[i]))
-            continue;
-
-        if (msg->ranges[i] < param_.min_distance ||
-            msg->ranges[i] > param_.max_distance)
-        {
-            msg->ranges[i] = std::numeric_limits<float>::infinity();
-        }
-    }
-}
-
-void ScanFilterNode::filterScanModify3(sensor_msgs::msg::LaserScan::SharedPtr msg)
-{
-#if DEBUG_SCAN
-    RCLCPP_INFO(get_logger(), "已进入反射强度过滤");
-#endif
-    // 过滤强度
-    for (size_t i = 0; i < msg->ranges.size(); i++)
-    {
-        if (!isValid(msg->ranges[i]))
-            continue;
-
-        float value = msg->intensities[i];
-        if (value < param_.min_intensities || value > param_.max_intensities)
-        {
-            msg->ranges[i] = std::numeric_limits<float>::infinity();
-        }
-
-        // 再次检查距离
-        if (msg->ranges[i] < param_.min_distance ||
-            msg->ranges[i] > param_.max_distance)
-        {
-            msg->ranges[i] = std::numeric_limits<float>::infinity();
-        }
-    }
-}
-
-std::pair<int, int> ScanFilterNode::filterScanModify4(sensor_msgs::msg::LaserScan::SharedPtr msg)
-{
-#if DEBUG_SCAN
-    RCLCPP_INFO(get_logger(), "已进入点的连续性过滤");
-#endif
-    // 过滤连续性
-    int begin_index = 0;
-    int end_index = 0;
-    std::vector<float> ranges = msg->ranges;
-
-    // 顺序找起点
-    for (int i = param_.min_index + 1; i < param_.max_index; i++)
-    {
-        if (!isValid(ranges[i]))
-            continue;
-        if (!isValid(ranges[i - 1]))
-        {
-            begin_index = i;
-            break;
-        }
-    }
-
-    if (msg->ranges.size() - begin_index < 10)
-    {
-        RCLCPP_ERROR(get_logger(), "数据太少无法继续处理");
-        return {0, 0};
-    }
-
-    // 倒序找终点
-    for (int i = param_.max_index - 2; i > 0; i--)
-    {
-        if (!isValid(ranges[i]))
-            continue;
-        if (!isValid(ranges[i + 1]))
-        {
-            end_index = i;
-            break;
-        }
-    }
-
-    if (end_index < 10)
-    {
-        return {0, 0};
-    }
-
-    // 顺序检查连续性
-    for (int i = begin_index + 1; i < end_index - 1; i++)
-    {
-        float prev = ranges[i - 1];
-        float curr = ranges[i];
-        float next = ranges[i + 1];
-
-        if (!isValid(prev) && !isValid(curr) && !isValid(next))
-        {
-            if (i - begin_index < param_.min_count)
-            {
-                begin_index = i + 1;
-            }
-            else
-            {
-                break;
-            }
-        }
-        else
-        {
-            float low = 100.0;
-            float high = 0.0;
-            for (int j = i - 1; j <= i + 1; j++)
-            {
-                if (isValid(ranges[j]))
-                {
-                    if (ranges[j] > 0.03 && ranges[j] < low)
-                    {
-                        low = ranges[j];
-                    }
-                    if (ranges[j] > high)
-                    {
-                        high = ranges[j];
-                    }
-                }
-            }
-            if (high - low > 0.08)
-            {
-                if (i - begin_index < param_.min_count)
-                {
-                    begin_index = i + 1;
-                }
-                else
-                {
-                    break;
-                }
-            }
-        }
-    }
-
-    // 倒序检查连续性
-    for (int i = end_index - 1; i > begin_index + 1; i--)
-    {
-        float prev = ranges[i - 1];
-        float curr = ranges[i];
-        float next = ranges[i + 1];
-
-        if (!isValid(prev) && !isValid(curr) && !isValid(next))
-        {
-            if (end_index - i < param_.min_count)
-            {
-                end_index = i - 1;
-            }
-            else
-            {
-                break;
-            }
-        }
-        else
-        {
-            if (isValid(next) && isValid(prev))
-            {
-                if (next - curr > 0.07)
-                {
-                    if (end_index - i < param_.min_count)
-                    {
-                        end_index = i - 1;
-                    }
-                    else
-                    {
-                        break;
-                    }
-                }
-            }
-        }
-    }
-
-    // 设置无效区域
-    for (int i = param_.min_index; i < begin_index-1; i++)
-    {
-        msg->ranges[i] = std::numeric_limits<float>::infinity();
-    }
-    for (int i = end_index + 1; i <= param_.max_index; i++)
-    {
-        msg->ranges[i] = std::numeric_limits<float>::infinity();
-    }
-
-    return {begin_index, end_index};
-}
-
-std::map<int, std::pair<double, double>> ScanFilterNode::laserScanToXY(
-    const sensor_msgs::msg::LaserScan::SharedPtr scan_msg)
-{
-    RCLCPP_INFO(get_logger(), "进入xy坐标系转换");
-    std::map<int, std::pair<double, double>> xy_points_map;
-    double angle = scan_msg->angle_min;
-
-    for (size_t i = 0; i < scan_msg->ranges.size(); i++)
-    {
-        float range_val = scan_msg->ranges[i];
-
-        // 跳过无效测量值
-        if (range_val >= scan_msg->range_min && range_val <= scan_msg->range_max)
-        {
-            // 极坐标转笛卡尔坐标
-            double x1 = range_val * cos(angle);
-            double y1 = range_val * sin(angle);
-            double x = -y1;
-            double y = x1;
-            xy_points_map[i] = {x, y};
-        }
-        angle += scan_msg->angle_increment;
-    }
-    return xy_points_map;
-}
-
-std::map<int, std::pair<double, double>> ScanFilterNode::laserScanToXY1(
-    int begin_index, int end_index,
-    const sensor_msgs::msg::LaserScan::SharedPtr scan_msg)
-{
-    RCLCPP_INFO(get_logger(), "进入xy坐标系转换");
-    std::map<int, std::pair<double, double>> xy_points_map;
-    double angle = scan_msg->angle_min ;
-
-    for (int i = begin_index-1; i <= end_index; i++)
-    {
-        float range_val = scan_msg->ranges[i];
-
-        if (range_val >= scan_msg->range_min && range_val <= scan_msg->range_max)
-        {
-            double x1 = range_val * cos(angle);
-            double y1 = range_val * sin(angle);
-            double x = -y1;
-            double y = x1;
-            xy_points_map[i] = {x, y};
-        }
-        angle += scan_msg->angle_increment;
-    }
-    return xy_points_map;
-}
-
-std::pair<double, double> ScanFilterNode::fitLineLeastSquares(const Eigen::MatrixXd &points)
-{
-    int n = points.rows();
-    Eigen::VectorXd x = points.col(0);
-    Eigen::VectorXd y = points.col(1);
-
-    // 最小二乘法拟合 y = kx + b
-    double x_mean = x.mean();
-    double y_mean = y.mean();
-
-    double numerator = 0.0;
-    double denominator = 0.0;
-
-    for (int i = 0; i < n; i++)
-    {
-        numerator += (x(i) - x_mean) * (y(i) - y_mean);
-        denominator += (x(i) - x_mean) * (x(i) - x_mean);
-    }
-
-    double k = numerator / denominator;
-    double b = y_mean - k * x_mean;
-
-    return {k, b};
-}
-
-std::pair<double, double> ScanFilterNode::computeDockingVelocity(double dx, double dy, double yaw_error)
-{
-    double max_linear_speed = 0.12;
-    double max_angular_speed = 0.16;
-
-    double Kp_linear = 0.5;
-    double Kp_angular = 0.5;
-    double Kp_lateral = 0.6;
-
-    // 线速度控制
-    double linear_velocity = Kp_linear * std::max(0.1, dx - 0.6);
-    linear_velocity = std::clamp(linear_velocity, -max_linear_speed, max_linear_speed);
-    linear_velocity = std::max(0.08, linear_velocity);
-
-    // 角速度控制
-    double angular_velocity = Kp_angular * yaw_error + Kp_lateral * dy;
-    angular_velocity = std::clamp(angular_velocity, -max_angular_speed, max_angular_speed);
-
-    return {-linear_velocity, -angular_velocity};
-}
-
-void ScanFilterNode::scanCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg)
-{
-#if DEBUG_SCAN
-    RCLCPP_INFO(get_logger(), "已进入scan回调函数");
-#else
-    if (!isCharging())
-        return;
-#endif
-    // 应用各种过滤器
-    filterScanModify0(msg);
-    filterScanModify1(msg);
-    filterScanModify2(msg);
-    filterScanModify3(msg);
-
-    auto [begin_index, end_index] = filterScanModify4(msg);
-    RCLCPP_INFO(get_logger(), "begin:%d,end:%d", begin_index, end_index);
-    resetLaserData(msg, begin_index, end_index);
-    if (end_index - begin_index < 2)
-    {
-        RCLCPP_INFO(get_logger(), "没有可连续性的点");
-        return;
-    }
-    if (true) // 测试用，可以改为条件判断
-    {
-        // 转换到XY坐标系
-        auto xy_list = laserScanToXY1(begin_index, end_index, msg);
-        if (xy_list.size() < 2)
-        {
-            RCLCPP_WARN(get_logger(), "XY转换结果点数不足: %zu", xy_list.size());
-            return;
-        }
-        // 获取分段
-        auto sections = getSections(begin_index, end_index, msg, xy_list);
-
-        if (sections.size() < 1)
-        {
-            RCLCPP_WARN(get_logger(), "没有获取到分段");
-            return;
-        }
-        RCLCPP_INFO(get_logger(), "XY转换结果点数：%d", sections.size());
-        if (true) // 选择最近的分段
-        {
-            int count = 0;
-            int nearest_index = -1;
-            double nearest_x = 100.0;
-
-            for (size_t i = 0; i < sections.size(); i++)
-            {
-                int section_start = sections[i].first;
-                int section_end = sections[i].second;
-
-                if (section_end - section_start < 10)
-                    continue;
-
-                double x_low = 100.0;
-                double x_high = -100.0;
-                int avg_count = 0;
-                double avg_total = 0.0;
-                double avg_value = 0.0;
-                for (int index = section_start; index <= section_end; index++)
-                {
-                    if (xy_list.find(index) == xy_list.end())
-                        continue;
-
-                    double x = msg->ranges[index]; // 使用距离值
-                    avg_count++;
-                    avg_total += x;
-                    x_low = std::min(x_low, x);
-                    x_high = std::max(x_high, x);
-                }
-                 avg_value = avg_total / avg_count;
-
-                if (x_high - x_low < 0.03)
-                    continue;
-
-                if (nearest_x > avg_value)
-                {
-                    nearest_x = avg_value;
-                    nearest_index = i;
-                }
-            }
-            begin_index = sections[nearest_index].first;
-            end_index = sections[nearest_index].second;
-            resetLaserData(msg, begin_index, end_index);
-        }
-    }
-    // 进一步处理激光数据
-    scanCallback1(msg, begin_index, end_index);
-}
-
-void ScanFilterNode::scanCallback1(
-    const sensor_msgs::msg::LaserScan::SharedPtr msg,
-    int begin_index, int end_index)
-{
-#if DEBUG_SCAN
-    RCLCPP_INFO(get_logger(), "进一步处理过滤数据");
-#else
-    if (!isCharging())
-        return;
-#endif
-
-    // 检查索引有效性
-    if (begin_index < 0 || end_index >= static_cast<int>(msg->ranges.size()) || begin_index >= end_index)
-    {
-        return;
-    }
-
-    // 根据距离调整结束索引
-    if (end_index > 0 && end_index < static_cast<int>(msg->ranges.size()) && msg->ranges[end_index - 1] > 0.9)
-    {
-        int count = end_index - begin_index;
-        end_index = end_index - static_cast<int>(count * 0.16);
-    }
-    // 发布过滤后的数据
-    publishFilteredScan(msg, msg->ranges);
-    // 转换到XY坐标系
-    auto xy_list = laserScanToXY(msg);
-    if (xy_list.size() < 2)
-    {
-        RCLCPP_ERROR(get_logger(), "xy坐标系没有转换成功");
-        return;
-    }
-    RCLCPP_INFO(get_logger(), "xy坐标系成功转换");
-    RCLCPP_INFO(get_logger(), "XY转换结果: %zu 个点", xy_list.size());
-    // 获取分段
-    auto turn_segments = getSegments(xy_list);
-    // 在scanCallback1中，获取分段后添加：
-    RCLCPP_INFO(get_logger(), "原始分段数: %zu", turn_segments.size());
-
-    // 打印每个分段的信息
-    for (const auto &seg : turn_segments)
-    {
-        const auto &info = seg.second;
-        int start_idx = std::get<0>(info);
-        int end_idx = std::get<1>(info);
-        double distance = std::get<5>(info);
-        double angle = std::get<6>(info);
-
-        RCLCPP_INFO(get_logger(), "分段 %d: 点数=%d, 距离=%.3f, 角度=%.3f度",
-                    seg.first, end_idx - start_idx + 1, distance, angle * 180.0 / M_PI);
-    }
-    turn_segments = filterSegments(turn_segments);
-    RCLCPP_INFO(get_logger(), "提取分段成功");
-    RCLCPP_INFO(get_logger(), "过滤后分段数: %zu", turn_segments.size());
-    // 创建结果对象
-    Result result;
-    result.frameid = frame_index_;
-    result.laserScanData = msg;
-
-    // 处理每个分段
-    int segment_index = 0;
-    for (const auto &seg : turn_segments)
-    {
-        const auto &info = seg.second;
-        int start_idx = std::get<0>(info);
-        int end_idx = std::get<1>(info);
-
-        for (int index = start_idx; index <= end_idx; index++)
-        {
-            if (xy_list.find(index) == xy_list.end())
-                continue;
-
-            double x = xy_list[index].first;
-            double y = xy_list[index].second;
-            IndexPoint item(x, y, index);
-
-            if (segment_index == 0)
-            {
-                result.left_points.push_back(item);
-            }
-            else if (segment_index == 1)
-            {
-                result.right_points.push_back(item);
-            }
-        }
-        segment_index++;
-    }
-
-    // 处理点分区
-    onPointPartition(result);
-}
-
-sensor_msgs::msg::LaserScan::SharedPtr ScanFilterNode::resetLaserData(
-    sensor_msgs::msg::LaserScan::SharedPtr msg,
-    int b_index, int e_index)
-{
-
-    std::vector<float> filtered_ranges = msg->ranges;
-    std::vector<float> intensities = msg->intensities;
-
-    for (int i = 0; i < b_index; i++)
-    {
-        filtered_ranges[i] = std::numeric_limits<float>::infinity();
-        intensities[i] = 0.0;
-    }
-
-    for (int i = e_index; i < static_cast<int>(msg->ranges.size()); i++)
-    {
-        filtered_ranges[i] = std::numeric_limits<float>::infinity();
-        intensities[i] = 0.0;
-    }
-
-    msg->ranges = filtered_ranges;
-    msg->intensities = intensities;
-
-    return msg;
-}
-
-void ScanFilterNode::publishFilteredScan(
-    const sensor_msgs::msg::LaserScan::SharedPtr original_msg,
-    const std::vector<float> &filtered_ranges)
-{
-
-    auto msg = std::make_shared<sensor_msgs::msg::LaserScan>();
-    msg->header = original_msg->header;
-    msg->angle_min = original_msg->angle_min;
-    msg->angle_max = original_msg->angle_max;
-    msg->angle_increment = original_msg->angle_increment;
-    msg->time_increment = original_msg->time_increment;
-    msg->scan_time = original_msg->scan_time;
-    msg->range_min = original_msg->range_min;
-    msg->range_max = original_msg->range_max;
-    msg->ranges = filtered_ranges;
-    msg->intensities = original_msg->intensities;
-
-    pub_filtered_->publish(*msg);
-    RCLCPP_INFO(get_logger(), "已成功发布过滤后的数据");
-}
-
-// ChargingManager 实现
-ChargingManager::ChargingManager() : Node("scan_filter")
-{
-    sub_ctrl_ = this->create_subscription<std_msgs::msg::String>(
-        "/recharge/ctrl_1", 10,
-        std::bind(&ChargingManager::onCtrl, this, _1));
-
-    state_ = ChargingState::IDLE;
-    last_station_detection_time_ = this->now();
-    last_voltage_detection_time_ = this->now();
-    setChargingVoltage(false);
-}
-
-bool ChargingManager::detectStation()
-{
-    // 这里需要实现基站检测逻辑
-    // 暂时使用简单实现
-    auto now = this->now();
-    // 假设有一个全局的 ScanFilterNode 实例
-    // 在实际中，需要通过节点间通信获取这个信息
-    return false; // 需要根据实际情况实现
-}
-
-void ChargingManager::onCtrl(const std_msgs::msg::String::SharedPtr msg)
-{
-    if (msg->data == "start")
-    {
-        setChargingVoltage(false);
-        startCharging();
-    }
-    else if (msg->data == "stop")
-    {
-        stopCharging();
-    }
-}
-
-void ChargingManager::startCharging()
-{
-    if (state_ != ChargingState::IDLE)
-    {
-        RCLCPP_WARN(this->get_logger(), "当前状态无法启动回充");
-        return;
-    }
-
-    changeState(ChargingState::STARTING);
-    last_station_detection_time_ = this->now();
-
-    // 检测基站
-    bool station_detected = false;
-    for (int i = 0; i < 80; i++)
-    {
-        if (detectStation())
-        {
-            station_detected = true;
-            break;
-        }
-        rclcpp::sleep_for(100ms);
-    }
-
-    if (!station_detected)
-    {
-        changeState(ChargingState::UNAVAILABLE);
-        return;
-    }
-
-    // 进入回充控制流程
-    executeChargingControl();
-    changeState(ChargingState::CHARGING);
-}
-
-void ChargingManager::stopCharging()
-{
-    if (state_ == ChargingState::IDLE)
-    {
-        RCLCPP_WARN(this->get_logger(), "当前状态无法停止回充");
-        return;
-    }
-    changeState(ChargingState::IDLE);
-}
-
-void ChargingManager::executeChargingControl()
-{
-    changeState(ChargingState::NAVIGATING);
-    last_station_detection_time_ = this->now();
-
-    while (state_ == ChargingState::NAVIGATING)
-    {
-        // 检测基站
-        if (detectStation())
-        {
-            last_station_detection_time_ = this->now();
-        }
-        else
-        {
-            auto now = this->now();
-            auto diff = (now - last_station_detection_time_).seconds();
-            if (diff > station_lost_timeout_)
-            {
-                changeState(ChargingState::UNAVAILABLE);
-                return;
-            }
-        }
-
-        // 检测充电电压
-        if (detectChargingVoltage())
-        {
-            changeState(ChargingState::CHARGING);
-            monitorCharging();
-            return;
-        }
-
-        // 生成并应用控制命令
-        auto [linear_vel, angular_vel] = generateControlCommand();
-        applyControlCommand(linear_vel, angular_vel);
-
-        rclcpp::sleep_for(std::chrono::milliseconds(int(control_interval_ * 1000)));
-    }
-}
-
-void ChargingManager::monitorCharging()
-{
-    last_voltage_detection_time_ = this->now();
-
-    while (state_ == ChargingState::CHARGING)
-    {
-        if (!detectChargingVoltage())
-        {
-            auto now = this->now();
-            auto diff = (now - last_voltage_detection_time_).seconds();
-            if (diff > station_lost_timeout_)
-            {
-                changeState(ChargingState::COMPLETED);
-                return;
-            }
-        }
-        else
-        {
-            last_voltage_detection_time_ = this->now();
-        }
-        rclcpp::sleep_for(std::chrono::milliseconds(int(control_interval_ * 1000)));
-    }
-}
-
-void ChargingManager::changeState(ChargingState new_state)
-{
-    state_ = new_state;
-    reportState(new_state);
-}
-
-void ChargingManager::reportState(ChargingState state)
-{
-    std::string state_str;
-    int other_status = 0;
-
-    switch (state)
-    {
-    case ChargingState::IDLE:
-        state_str = "空闲";
-        other_status = 5; // 注意：需要根据实际情况调整
-        break;
-    case ChargingState::STARTING:
-        state_str = "启动中";
-        other_status = 1;
-        break;
-    case ChargingState::NAVIGATING:
-        state_str = "回充行驶";
-        other_status = 3;
-        break;
-    case ChargingState::CHARGING:
-        state_str = "充电中";
-        other_status = 5;
-        break;
-    case ChargingState::UNAVAILABLE:
-        state_str = "回充不可用";
-        other_status = 2;
-        break;
-    case ChargingState::COMPLETED:
-        state_str = "充电结束";
-        other_status = 5;
-        break;
-    }
-
-    RCLCPP_INFO(this->get_logger(), "状态上报: %s", state_str.c_str());
-}
-
-bool ChargingManager::detectChargingVoltage()
-{
-    return getChargingVoltage();
-}
-
-std::pair<double, double> ChargingManager::generateControlCommand()
-{
-    // 这里需要实现控制命令生成逻辑
-    return {0.2, 0.0};
-}
-
-void ChargingManager::applyControlCommand(double linear_vel, double angular_vel)
-{
-    // 应用控制命令
-    // 发布到控制主题
-    auto msg = geometry_msgs::msg::Twist();
-    msg.linear.x = linear_vel;
-    msg.angular.z = angular_vel;
-
-    // 需要有一个发布者，或者通过其他方式传递控制命令
-    // 例如: pub_ctrl_->publish(msg);
-}
-
-RechargerRecv::RechargerRecv() : Node("RechargerRecv")
-{
-    setChargingVoltage(false);
-
-    last_report_status_ = -1;
-    last_status_ = 1;
-    running_ = false;
-
-    // 创建发布者和订阅者
-    pub_feed_ = this->create_publisher<std_msgs::msg::Int32>("/recharge/feed", 10);
-    client_ = this->create_client<interface::srv::SceneControls>("/feed/recharge/ctrl");
-
-    getcharging_sub_ = this->create_subscription<std_msgs::msg::Int32>(
-        "/sensor/getcharging", 10,
-        std::bind(&RechargerRecv::onGetCharging, this, _1));
-
-    sub_mode_change_ = this->create_subscription<std_msgs::msg::String>(
-        "/scene_services/mode", 10,
-        std::bind(&RechargerRecv::onModeChange, this, _1));
-
-    // 创建定时器
-    auto timer_callback = [this]() -> void
-    {
-        this->reportLoop();
-    };
-
-    auto report_timer = this->create_wall_timer(
-        std::chrono::seconds(2), timer_callback);
-
-    // 启动线程
-    thread_ = std::thread(&RechargerRecv::reportLoop, this);
-}
-
-RechargerRecv::~RechargerRecv()
-{
-    running_ = false;
-    if (thread_.joinable())
-    {
-        thread_.join();
-    }
-}
-
-void RechargerRecv::onGetCharging(const std_msgs::msg::Int32::SharedPtr msg)
-{
-    int status = msg->data;
-    bool local_detect_charging_voltage = false;
-
-    if (status == 0)
-    {
-        RCLCPP_INFO(this->get_logger(), "设备接触到充电区");
-        local_detect_charging_voltage = true;
-    }
-    else
-    {
-        local_detect_charging_voltage = false;
-    }
-
-    setChargingVoltage(local_detect_charging_voltage);
-}
-
-void RechargerRecv::onModeChange(const std_msgs::msg::String::SharedPtr msg)
-{
-    std::string mode = msg->data;
-    std::string last_mode_str = last_mode;
-
-    if (last_mode_str != mode)
-    {
-        setMode(mode); // 需要添加这个函数
-        RCLCPP_INFO(this->get_logger(), "新模式: %s", mode.c_str());
-    }
-}
-
-void RechargerRecv::reportLoop()
-{
-    while (running_)
-    {
-        if (!isCharging())
-        {
-            std::this_thread::sleep_for(std::chrono::milliseconds(100));
-            continue;
-        }
-
-        // 报告状态
-        if (last_report_status_ != last_status_)
-        {
-            callService(last_status_);
-            last_report_status_ = last_status_;
-        }
-
-        std::this_thread::sleep_for(std::chrono::seconds(2));
-    }
-}
-
-void RechargerRecv::callService(int start)
-{
-    // 发布到主题
-    auto msg = std_msgs::msg::Int32();
-    msg.data = start;
-
-    RCLCPP_INFO(this->get_logger(), "发布回充反馈: %d", start);
-    if (start != 3)
-    {
-        pub_feed_->publish(msg);
-    }
-
-    // 或者调用服务（如果使用服务）
-    // auto request = std::make_shared<interface::srv::SceneControls::Request>();
-    // request->start = start;
-    // auto future = client_->async_send_request(request);
-}
-
-void RechargerRecv::changeStatus(int status)
-{
-    if (last_status_ != status)
-    {
-        RCLCPP_INFO(this->get_logger(), "回充状态变更 %d --> %d", last_status_, status);
-        callService(status);
-        last_status_ = status;
-    }
-}

recharge2/interface/CMakeLists.txt

@@ -1,26 +0,0 @@
-cmake_minimum_required(VERSION 3.8)
-project(interface)
-
-if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 17)
-endif()
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-# find dependencies
-find_package(ament_cmake REQUIRED)
-find_package(std_msgs REQUIRED)
-find_package(rosidl_default_generators REQUIRED)
-
-# 生成接口 - 这个命令会自动处理所有需要的导出
-rosidl_generate_interfaces(${PROJECT_NAME}
-  "srv/SceneControls.srv"
-  DEPENDENCIES std_msgs
-)
-
-# 对于简单的接口包，只需要这个就够了
-# rosidl_generate_interfaces 会自动处理头文件安装和导出
-
-ament_package()

recharge2/interface/msg/Empty.msg

@@ -1 +0,0 @@
-# 空消息，可用于简单触发

recharge2/interface/package.xml

@@ -1,24 +0,0 @@
-<?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>interface</name>
-  <version>0.0.0</version>
-  <description>Custom interface package for SceneControls service</description>
-  <maintainer email="zhangkaifeng@users.noreply.github.com">zkf</maintainer>
-  <license>Apache-2.0</license>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-  <buildtool_depend>rosidl_default_generators</buildtool_depend>
-
-  <depend>std_msgs</depend>
-  <depend>rosidl_default_runtime</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>

recharge2/interface/srv/SceneControls.srv

@@ -1,4 +0,0 @@
-int32 start
----
-int32 code
-string message