first commit:auto drive misson center webots simulation

.gitignore

@@ -0,0 +1,13 @@
+.vscode/
+.caude/
+.agent/
+.trae/
+.codex/
+.reasonix
+.codegraph
+build/
+install/
+log/
+logs/
+data/
+files/

README.md

@@ -0,0 +1,2 @@
+auto drive misson center webots simulation:
+

auto_drive_misson_center/CMakeLists.txt

@@ -0,0 +1,137 @@
+cmake_minimum_required(VERSION 3.8)
+project(auto_drive_misson_center)
+
+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(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
+find_package(auto_drive_proj_utils REQUIRED)
+find_package(auto_drive_proj_msg REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(fmt REQUIRED)
+
+include_directories(
+  include
+  ${EIGEN3_INCLUDE_DIR}
+)
+
+# Find curl
+find_package(PkgConfig QUIET)
+if(PKG_CONFIG_FOUND)
+  pkg_check_modules(CURL libcurl)
+endif()
+
+if(NOT CURL_FOUND)
+  find_library(CURL_LIBRARY NAMES curl libcurl)
+  find_path(CURL_INCLUDE_DIR NAMES curl/curl.h)
+  if(CURL_LIBRARY AND CURL_INCLUDE_DIR)
+    set(CURL_FOUND TRUE)
+    set(CURL_LIBRARIES ${CURL_LIBRARY})
+    set(CURL_INCLUDE_DIRS ${CURL_INCLUDE_DIR})
+  endif()
+endif()
+
+# Include directories
+include_directories(include)
+include_directories(${CMAKE_CURRENT_BINARY_DIR}/msg)
+
+# Define the executables
+add_executable(mission_center_node src/mission_center_node.cpp src/redis_utils.cpp src/logger.cpp)
+add_executable(auto_mission_handler_node src/auto_mission_handler.cpp src/redis_utils.cpp src/logger.cpp)
+add_executable(path_follow_handler_node src/path_follow_handler.cpp src/redis_utils.cpp src/logger.cpp)
+
+# 链接fmt库
+target_link_libraries(mission_center_node
+    fmt::fmt
+)
+
+target_link_libraries(auto_mission_handler_node
+    fmt::fmt
+)
+
+target_link_libraries(path_follow_handler_node
+    fmt::fmt
+)
+
+# Link libraries
+ament_target_dependencies(mission_center_node
+  rclcpp
+  std_msgs
+  std_srvs
+  builtin_interfaces
+  auto_drive_proj_utils
+  auto_drive_proj_msg
+)
+
+ament_target_dependencies(auto_mission_handler_node
+  rclcpp
+  std_msgs
+  std_srvs
+  auto_drive_proj_utils
+  auto_drive_proj_msg
+)
+
+ament_target_dependencies(path_follow_handler_node
+  rclcpp
+  std_msgs
+  std_srvs
+  geometry_msgs
+  auto_drive_proj_utils
+  auto_drive_proj_msg
+)
+
+# Find hiredis library
+find_library(HIREDIS_LIBRARY NAMES hiredis)
+find_path(HIREDIS_INCLUDE_DIR NAMES hiredis/hiredis.h)
+
+if(HIREDIS_LIBRARY AND HIREDIS_INCLUDE_DIR)
+  set(HIREDIS_FOUND TRUE)
+  set(HIREDIS_LIBRARIES ${HIREDIS_LIBRARY})
+  set(HIREDIS_INCLUDE_DIRS ${HIREDIS_INCLUDE_DIR})
+  message(STATUS "Found hiredis: ${HIREDIS_LIBRARY}")
+else()
+  message(WARNING "hiredis library not found, Redis functionality will be disabled")
+endif()
+
+# Link curl library
+if(CURL_FOUND)
+  target_link_libraries(mission_center_node ${CURL_LIBRARIES})
+  target_include_directories(mission_center_node PRIVATE ${CURL_INCLUDE_DIRS})
+endif()
+
+# Link hiredis library
+if(HIREDIS_FOUND)
+  target_link_libraries(mission_center_node ${HIREDIS_LIBRARIES})
+  target_include_directories(mission_center_node PRIVATE ${HIREDIS_INCLUDE_DIRS})
+  target_link_libraries(auto_mission_handler_node ${HIREDIS_LIBRARIES})
+  target_include_directories(auto_mission_handler_node PRIVATE ${HIREDIS_INCLUDE_DIRS})
+  target_link_libraries(path_follow_handler_node ${HIREDIS_LIBRARIES})
+  target_include_directories(path_follow_handler_node PRIVATE ${HIREDIS_INCLUDE_DIRS})
+endif()
+
+# Install executables
+install(TARGETS
+  mission_center_node
+  auto_mission_handler_node
+  path_follow_handler_node
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+# 添加launch文件的安装规则
+install(DIRECTORY
+  config
+  launch
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+install(DIRECTORY
+  scripts
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+ament_package()

auto_drive_misson_center/config/misson_center_params.yaml

@@ -0,0 +1,55 @@
+auto_mission_handler_node:
+  # parameters
+  ros__parameters:
+    # redis
+    redis_host: "192.168.1.110" # "127.0.0.1"
+    redis_port: 6379
+    redis_db: 0
+
+    device_id: "2072868304711761920"
+    
+    # logger
+    logger_logger_name: "auto_mission"
+    logger_filename: "logs/mission_center_logger.txt"
+    logger_node_name: "auto_mission"
+    logger_max_size: 10485760 # 10 MB, 10 * 1024 * 1024
+    logger_max_files: 5
+    logger_set_level: 1 # trace = 0, debug = 1, info = 2, warn = 3, err = 4, critical = 5, off = 6,
+
+path_follow_handler_node:
+  # parameters
+  ros__parameters:
+    # redis
+    redis_host: "192.168.1.110" # "127.0.0.1"
+    redis_port: 6379
+    redis_db: 0
+
+    device_id: "2072868304711761920"
+
+    controller_type: "pure_pursuit"  # "pure_pursuit", "lqr", "pid"
+    
+    # logger
+    logger_logger_name: "path_follow"
+    logger_filename: "logs/mission_center_logger.txt"
+    logger_node_name: "path_follow"
+    logger_max_size: 10485760 # 10 MB, 10 * 1024 * 1024
+    logger_max_files: 5
+    logger_set_level: 1 # trace = 0, debug = 1, info = 2, warn = 3, err = 4, critical = 5, off = 6,
+
+mission_center_node:
+  # parameters
+  ros__parameters:
+    # redis
+    redis_host: "192.168.1.110" # "127.0.0.1"
+    redis_port: 6379
+    redis_db: 0
+
+    device_id: "2072868304711761920"
+    
+    # logger
+    logger_logger_name: "center_node"
+    logger_filename: "logs/mission_center_logger.txt"
+    logger_node_name: "center_node"
+    logger_max_size: 10485760 # 10 MB, 10 * 1024 * 1024
+    logger_max_files: 5
+    logger_set_level: 1 # trace = 0, debug = 1, info = 2, warn = 3, err = 4, critical = 5, off = 6,

auto_drive_misson_center/include/auto_drive_misson_center/auto_mission_handler.hpp

@@ -0,0 +1,116 @@
+#ifndef AUTO_MISSION_HANDLER_HPP_
+#define AUTO_MISSION_HANDLER_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/int32.hpp>
+#include <auto_drive_proj_msg/srv/req_string.hpp>
+#include <std_msgs/msg/string.hpp>
+#include <auto_drive_proj_utils/Json/json.hpp>
+#include <unordered_map>
+#include <memory>
+#include <mutex>
+#include <vector>
+#include <string>
+
+using json = nlohmann::json;
+
+namespace auto_drive_misson_center {
+
+enum class MissionState {
+    IDLE,
+    RUNNING,
+    PAUSE,
+    CONTINUE
+};
+
+class AutoMissionHandler : public rclcpp::Node
+{
+public:
+    explicit AutoMissionHandler(const std::string & node_name = "auto_mission_handler");
+
+private:
+    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr path_id_sub_;
+    rclcpp::Service<auto_drive_proj_msg::srv::ReqString>::SharedPtr auto_mission_service_;
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr event_info_pub_;
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr event_warn_pub_;
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr event_error_pub_;
+    rclcpp::TimerBase::SharedPtr main_timer_;
+
+    std::unordered_map<std::string, rclcpp::Client<auto_drive_proj_msg::srv::ReqString>::SharedPtr> cached_clients_;
+
+    MissionState mission_state_{MissionState::IDLE};
+    int current_path_id_{0};
+    json mission_json_;
+    size_t current_mission_index_{0};
+
+    json executed_commands_;
+
+    std::vector<std::pair<std::string, json>> continue_command_queue_;
+    size_t continue_command_index_{0};
+
+    void path_id_callback(const std_msgs::msg::Int32::SharedPtr msg);
+    void auto_mission_callback(
+        const auto_drive_proj_msg::srv::ReqString::Request::SharedPtr request,
+        auto_drive_proj_msg::srv::ReqString::Response::SharedPtr response);
+    void mainTimerCallback();
+
+    void processRunningState();
+    void processPauseState();
+    void processContinueState();
+    bool execute_function(const std::string& function_id, const json& inputs);
+    rclcpp::Client<auto_drive_proj_msg::srv::ReqString>::SharedPtr get_cached_client(const std::string& function_id);
+
+    bool is_idle() const { return mission_state_ == MissionState::IDLE; }
+    bool is_running() const { return mission_state_ == MissionState::RUNNING; }
+    bool is_pause() const { return mission_state_ == MissionState::PAUSE; }
+    bool is_continue() const { return mission_state_ == MissionState::CONTINUE; }
+    std::string missionStateToString(MissionState state) {
+        switch (state) {
+            case MissionState::IDLE:     return "IDLE";
+            case MissionState::RUNNING:  return "RUNNING";
+            case MissionState::PAUSE:    return "PAUSE";
+            case MissionState::CONTINUE: return "CONTINUE";
+            default:                     return "UNKNOWN";
+        }
+    }
+
+    bool save_to_redis(const std::string& key, const std::string& value);
+    std::string load_from_redis(const std::string& key);
+    bool save_executed_commands();
+    bool save_current_mission_index();
+    bool save_mission_content(const std::string& content);
+    bool load_executed_commands();
+    bool load_current_mission_index();
+    std::string load_mission_content();
+
+    void reportEvent(const std::string& level, const std::string& event_name, const std::string& content);
+    void resetMissionState();
+
+    void get_params();
+
+    // 回调组
+    // rclcpp::CallbackGroup::SharedPtr subscription_cb_group_;
+    // rclcpp::CallbackGroup::SharedPtr service_cb_group_;
+    // rclcpp::CallbackGroup::SharedPtr publisher_cb_group_;
+    // rclcpp::CallbackGroup::SharedPtr timer_cb_group_;
+    rclcpp::CallbackGroup::SharedPtr service_cached_client_cb_group_;  // cached_clients_
+
+    // redis
+    std::string redis_host_;
+    int redis_port_;
+    int redis_db_;
+
+    std::string device_id_;
+
+    // logger
+    std::string logger_logger_name_;
+    std::string logger_filename_;
+    std::string logger_node_name_;
+    int logger_max_size_;
+    int logger_max_files_;
+    int logger_set_level_;
+};
+
+} // namespace auto_drive_misson_center
+
+#endif // AUTO_MISSION_HANDLER_HPP_

auto_drive_misson_center/include/auto_drive_misson_center/base_following_controller.hpp

@@ -0,0 +1,27 @@
+#ifndef _BASE_FOLLOWING_CONTROLLER_HPP_
+#define _BASE_FOLLOWING_CONTROLLER_HPP_
+
+#include <iostream>
+#include "spdlog/logger.h"
+#include "spdlog/spdlog.h"
+#include "spdlog/sinks/rotating_file_sink.h"
+
+class BaseFollowingController
+{
+public:
+    BaseFollowingController(std::shared_ptr<spdlog::logger> logger_recorder)
+        : logger_(logger_recorder)
+    {
+        
+    };
+    virtual ~BaseFollowingController() = default;
+    virtual void init(const float max_linear_vel, const float max_angular_vel, const float kp, const float ki, \
+        const float kd, const float look_ahead_dis, const float kv) = 0;
+    virtual float update(const float alpha, const float linear_val, const float dis) = 0;
+    virtual void reset() = 0;
+    virtual void test()=0;
+private:
+    std::shared_ptr<spdlog::logger> logger_;
+};
+
+#endif // _BASE_FOLLOWING_CONTROLLER_HPP_

auto_drive_misson_center/include/auto_drive_misson_center/logger.hpp

@@ -0,0 +1,27 @@
+#ifndef MISSION_CENTER_LOGGER_HPP_
+#define MISSION_CENTER_LOGGER_HPP_
+
+#include <spdlog/spdlog.h>
+#include <spdlog/sinks/rotating_file_sink.h>
+#include <memory>
+
+namespace auto_drive_misson_center {
+
+extern std::shared_ptr<spdlog::logger> logger;
+
+void initLogger(const std::string logger_logger_name = "mission_center_logger", 
+                const std::string logger_filename = "logs/mission_center_logger.txt", 
+                const std::string logger_node_name = "mission_center_logger",
+                size_t logger_max_size = 10485760, 
+                size_t logger_max_files = 3, 
+                const int logger_set_level = 2);
+
+}
+
+#define LOG_INFO(...)    auto_drive_misson_center::logger->info(__VA_ARGS__)
+#define LOG_WARN(...)    auto_drive_misson_center::logger->warn(__VA_ARGS__)
+#define LOG_ERROR(...)   auto_drive_misson_center::logger->error(__VA_ARGS__)
+#define LOG_DEBUG(...)   auto_drive_misson_center::logger->debug(__VA_ARGS__)
+#define LOG_TRACE(...)   auto_drive_misson_center::logger->trace(__VA_ARGS__)
+
+#endif

auto_drive_misson_center/include/auto_drive_misson_center/lqr_control.hpp

@@ -0,0 +1,95 @@
+#ifndef _LQR_CONTROL_HPP_
+#define _LQR_CONTROL_HPP_
+
+#include <iostream>
+#include <vector>
+#include <Eigen/Dense>
+#include "base_following_controller.hpp"
+
+using namespace Eigen;
+
+class LQR_Controller_C : public BaseFollowingController
+{
+private:
+    MatrixXd B, Q, R;
+
+    std::shared_ptr<spdlog::logger> logger_;
+
+public:
+    LQR_Controller_C(std::shared_ptr<spdlog::logger> logger_recorder)
+        : BaseFollowingController(logger_recorder), logger_(logger_recorder)
+    {
+        B.resize(2, 1); // 控制输入矩阵
+        Q.resize(2, 2); // 状态误差的加权矩阵
+        R.resize(1, 1); // 控制输入的加权矩阵
+        B << -1, 1;     // 控制输入矩阵
+
+        // 设计LQR的代价函数矩阵
+        Q << 50, 0, 0, 15; // 状态误差的加权矩阵：惩罚横向误差e_x和角度误差θ
+
+        R << 10; // 控制输入的加权矩阵：惩罚角速度ω
+    };
+    ~LQR_Controller_C() {};
+    // LQR求解器函数
+    MatrixXd lqr(const MatrixXd &A, const MatrixXd &B, const MatrixXd &Q, const MatrixXd &R)
+    {
+        MatrixXd P = Q; // 初始化P为Q
+        MatrixXd P_prev = MatrixXd::Zero(A.rows(), A.cols());
+        for (int i = 0; i < 1000; ++i)
+        {
+            P_prev = P;
+            P = Q + A.transpose() * P * A - A.transpose() * P * B * (R + B.transpose() * P * B).inverse() * B.transpose() * P * A;
+            if ((P - P_prev).norm() < 1e-6)
+                break; // 收敛条件
+        }
+        // LQR增益矩阵K
+        MatrixXd K = (R + B.transpose() * P * B).inverse() * B.transpose() * P * A;
+        return K;
+    };
+    float update(const float alpha, const float linear_val, const float dis)
+    {
+        // std::cout << "LQR update!" << std::endl;
+        logger_->trace("LQR update!");
+        MatrixXd A(2, 2);
+        A << 0, linear_val, 0, 0;
+        Eigen::VectorXd x(2);
+
+        x << dis, alpha;
+        // 求解LQR增益矩阵K
+        MatrixXd K = lqr(A, B, Q, R);
+        // std::cout << "LQR Gain Matrix K:" << std::endl
+        //      << K << std::endl;
+        std::stringstream ss;
+        ss << K;
+        logger_->trace("LQR Gain Matrix K:\n{}", ss.str());
+        return -(K * x)(0, 0);
+    };
+    /*void init_lqr(const MatrixXd B, const MatrixXd Q, const MatrixXd R)
+    {
+        this->B = B;
+        this->Q = Q;
+        this->R = R;
+    }*/
+    void init(const float max_linear_vel, const float max_angular_vel, const float kp, const float ki,
+              const float kd, const float look_ahead_dis, const float kv)
+    {
+        // std::cout << "LQR init!" << std::endl;
+        logger_->info("LQR init!");
+    };
+    void reset()
+    {
+        // Reset any internal state if necessary
+        B.resize(2, 1); // 控制输入矩阵
+        Q.resize(2, 2); // 状态误差的加权矩阵
+        R.resize(1, 1); // 控制输入的加权矩阵
+        B << -1, 1;     // 控制输入矩阵
+
+        // 设计LQR的代价函数矩阵
+        Q << 50, 0, 0, 15; // 状态误差的加权矩阵：惩罚横向误差e_x和角度误差θ
+        R << 10;           // 控制输入的加权矩阵：惩罚角速度ω
+    };
+
+    void test() { std::cout << "lqr test!" << std::endl; logger_->debug("lqr test!");};
+};
+
+#endif // _LQR_CONTROL_HPP_

auto_drive_misson_center/include/auto_drive_misson_center/mission_center_node.hpp

@@ -0,0 +1,152 @@
+#ifndef MISSION_CENTER_NODE_HPP_
+#define MISSION_CENTER_NODE_HPP_
+
+#include <string>
+#include <memory>
+#include <atomic>
+#include <unordered_map>
+#include <curl/curl.h>
+
+#include <rclcpp/rclcpp.hpp>
+#include <auto_drive_proj_msg/srv/req_string.hpp>
+#include <std_srvs/srv/set_bool.hpp>
+#include <std_msgs/msg/string.hpp>
+#include <auto_drive_proj_utils/Json/json.hpp>
+#include <auto_drive_proj_utils/GeodeticConverter/GeodeticConverter.hpp>
+
+using json = nlohmann::json;
+
+namespace auto_drive_misson_center {
+
+enum class MainState {
+    INIT,
+    PAUSE_MIS,
+    PAUSE_PATH,
+    PAUSE,
+    STOP_MIS,
+    STOP_PATH,
+    IDLE,
+    WAIT_MIS_RSP,
+    WAIT_PATH_RSP,
+    AUTO
+};
+
+class MissionCenterNode : public rclcpp::Node
+{
+public:
+    explicit MissionCenterNode(const std::string & node_name = "mission_center_node");
+    void run();
+
+private:
+    MainState main_state_{MainState::INIT};
+    rclcpp::Time last_main_state_report_time_;
+    bool mission_finished_{false};
+    bool path_finished_{false};
+
+    rclcpp::Service<auto_drive_proj_msg::srv::ReqString>::SharedPtr auto_service_;
+    rclcpp::Service<auto_drive_proj_msg::srv::ReqString>::SharedPtr mission_on_service_;
+    rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr stop_mission_service_;
+    rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr stop_path_service_;
+
+    rclcpp::Client<auto_drive_proj_msg::srv::ReqString>::SharedPtr auto_mission_client_;
+    rclcpp::Client<auto_drive_proj_msg::srv::ReqString>::SharedPtr auto_follow_client_;
+
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr event_info_pub_;
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr event_warn_pub_;
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr event_error_pub_;
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr properties_report_pub_;
+
+    std::vector<std::pair<int, int>> interpolation_map_;
+
+    std::shared_future<typename auto_drive_proj_msg::srv::ReqString::Response::SharedPtr> mission_future_;
+    std::shared_future<typename auto_drive_proj_msg::srv::ReqString::Response::SharedPtr> follow_future_;
+    std::atomic<bool> has_pending_request_{false};
+    rclcpp::Time request_start_time_;
+    static constexpr int REQUEST_TIMEOUT_SECONDS = 10;
+    static constexpr int LAST_MAIN_STATE_REPORT_TIMEOUT_SECONDS = 5;
+
+    void autoCallback(
+        const auto_drive_proj_msg::srv::ReqString::Request::SharedPtr request,
+        auto_drive_proj_msg::srv::ReqString::Response::SharedPtr response);
+
+    void missionOnCallback(
+        const auto_drive_proj_msg::srv::ReqString::Request::SharedPtr request,
+        auto_drive_proj_msg::srv::ReqString::Response::SharedPtr response);
+
+    void stopMissionCallback(
+        const std_srvs::srv::SetBool::Request::SharedPtr request,
+        std_srvs::srv::SetBool::Response::SharedPtr response);
+
+    void stopPathCallback(
+        const std_srvs::srv::SetBool::Request::SharedPtr request,
+        std_srvs::srv::SetBool::Response::SharedPtr response);
+
+    std::string downloadFile(const std::string& url);
+    bool processDownloadedData(const std::string& downloaded_data,
+                              std::string& mission_data,
+                              std::string& follow_data);
+    std::vector<std::vector<double>> interpolatePoints(const std::vector<std::vector<double>>& coordinates,
+                                                      double max_distance = 0.5);
+    int updatePathIndices(const int original_ind, const std::vector<std::pair<int, int>>& interpolation_map);
+
+    void handleAsyncResponses();
+    void processWaitMisRspState();
+    void processWaitPathRspState();
+    void syncSendPauseMission();
+    void syncSendPauseFollow();
+    void syncSendStopMission();
+    void syncSendStopFollow();
+    void reportEvent(const std::string& level, const std::string& event_name, const std::string& content);
+    void publishPropertyReport(bool reportImmediately = false);
+    bool saveState();
+    MainState loadStateFromRedis();
+
+    bool isIdle() const { return main_state_ == MainState::IDLE; }
+    bool isAuto() const { return main_state_ == MainState::AUTO; }
+    bool isPause() const { return main_state_ == MainState::PAUSE; }
+    bool isWaitMisRsp() const { return main_state_ == MainState::WAIT_MIS_RSP; }
+    bool isWaitPathRsp() const { return main_state_ == MainState::WAIT_PATH_RSP; }
+    std::string mainStateToString(MainState state) {
+        switch (state) {
+            case MainState::INIT:           return "INIT";
+            case MainState::PAUSE_MIS:      return "PAUSE_MIS";
+            case MainState::PAUSE_PATH:     return "PAUSE_PATH";
+            case MainState::PAUSE:          return "PAUSE";
+            case MainState::STOP_MIS:       return "STOP_MIS";
+            case MainState::STOP_PATH:      return "STOP_PATH";
+            case MainState::IDLE:           return "IDLE";
+            case MainState::WAIT_MIS_RSP:   return "WAIT_MIS_RSP";
+            case MainState::WAIT_PATH_RSP:  return "WAIT_PATH_RSP";
+            case MainState::AUTO:           return "AUTO";
+            default:                        return "UNKNOWN";
+        }
+    }
+    void changeMainState(MainState state);
+    MainState getMainState() const { return main_state_;}
+
+    void get_params();
+
+    // 回调组
+    rclcpp::CallbackGroup::SharedPtr service_cb_group_;
+    rclcpp::CallbackGroup::SharedPtr client_cb_group_;
+    rclcpp::CallbackGroup::SharedPtr publisher_cb_group_;
+
+    // redis
+    std::string redis_host_;
+    int redis_port_;
+    int redis_db_;
+
+    std::string device_id_;
+
+    // logger
+    std::string logger_logger_name_;
+    std::string logger_filename_;
+    std::string logger_node_name_;
+    int logger_max_size_;
+    int logger_max_files_;
+    int logger_set_level_;
+};
+
+} // namespace auto_drive_misson_center
+
+#endif // MISSION_CENTER_NODE_HPP_

auto_drive_misson_center/include/auto_drive_misson_center/path_follow_handler.hpp

@@ -0,0 +1,166 @@
+#ifndef AUTO_DRIVE_MISSION_CENTER_PATH_FOLLOW_HANDLER_HPP_
+#define AUTO_DRIVE_MISSION_CENTER_PATH_FOLLOW_HANDLER_HPP_
+
+#include <string>
+#include <memory>
+#include <vector>
+#include <map>
+
+#include <rclcpp/rclcpp.hpp>
+#include <geometry_msgs/msg/pose2_d.hpp>
+#include <geometry_msgs/msg/pose_array.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <std_msgs/msg/int32.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+#include <auto_drive_proj_msg/srv/req_string.hpp>
+#include <auto_drive_proj_utils/Json/json.hpp>
+#include "auto_drive_misson_center/path_tracking_controller.hpp"
+#include "auto_drive_misson_center/redis_utils.hpp"
+#include <auto_drive_proj_msg/srv/set_pose.hpp>
+#include <std_srvs/srv/set_bool.hpp>
+#include <std_msgs/msg/string.hpp>
+
+using json = nlohmann::json;
+
+namespace auto_drive_misson_center {
+
+enum class MainState {
+    IDLE,
+    NEW_TASK,
+    READY_SET_START,
+    READY_TO_RUN,
+    RUNNING,
+    AUTO_OFF,
+    POS_LOST,
+    PAUSE
+};
+
+class PathFollowHandler : public rclcpp::Node
+{
+public:
+    explicit PathFollowHandler(const std::string & node_name = "path_follow_handler");
+
+    void run();
+
+private:
+    MainState main_state_{MainState::IDLE};
+    bool is_resuming_{false};
+    std::string controller_type_{"pure_pursuit"};
+
+    rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_;
+    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
+    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr path_ind_pub_;
+    rclcpp::Service<auto_drive_proj_msg::srv::ReqString>::SharedPtr auto_follow_service_;
+    rclcpp::Client<auto_drive_proj_msg::srv::SetPose>::SharedPtr set_pose_client_;
+    rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr stop_path_client_;
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr event_info_pub_;
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr event_warn_pub_;
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr event_error_pub_;
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr properties_report_pub_;
+
+    std::shared_future<typename auto_drive_proj_msg::srv::SetPose::Response::SharedPtr> set_pose_future_;
+    bool has_pending_set_pose_{false};
+    rclcpp::Time set_pose_start_time_;
+    static constexpr double SET_POSE_TIMEOUT_SECONDS = 5.0;
+
+    geometry_msgs::msg::PoseWithCovarianceStamped current_pose_;
+    bool pose_initialized_{false};
+    rclcpp::Time last_pose_time_;
+
+    double turn_radius_{1.0};
+    double mission_speed_{0.0};
+    double start_lat_{0.0};
+    double start_lon_{0.0};
+    double start_yaw_{0.0};
+    std::vector<int> turn_inds_;
+    std::vector<std::vector<double>> coordinates_;
+
+    std::vector<std::vector<PathPoint>> path_segments_;
+    int current_segment_index_{-1};
+    int path_ind_start_{0};
+
+    int resume_inner_point_index_{0}; // 续跑恢复的片段内部点下标，默认0
+
+    std::shared_ptr<PathTrackingController> path_controller_;
+
+    std::unique_ptr<RedisUtils> redis_utils_;
+
+    int pause_counter_{0};
+    int pos_lost_counter_{0};
+    rclcpp::Time pos_lost_start_time_;
+    static constexpr double POS_LOST_TIMEOUT_SECONDS = 0.5;
+    static constexpr int POS_RECOVERY_COUNT = 5;
+    static constexpr double POS_RECOVERY_TIME_SECONDS = 1.0;
+    static constexpr double POS_GAP_THRESHOLD_METERS = 5.0;
+    static constexpr int ZERO_VELOCITY_COUNT = 5;
+
+    void poseCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
+    void autoFollowCallback(
+        const auto_drive_proj_msg::srv::ReqString::Request::SharedPtr request,
+        auto_drive_proj_msg::srv::ReqString::Response::SharedPtr response);
+    
+    void processMainLoop();
+
+    void processRunningState();
+    void processPosLostState();
+    void processPauseState();
+    void processAutoOffState();
+    void processNewTaskState();
+    void processReadySetStartState();
+    void processReadyToRunState();
+    bool parseMissionJson(const std::string& json_str);
+    bool loadPathTrackingController(const std::string& controller_type);
+    int findNearestPathSegment(const geometry_msgs::msg::Pose& pose);
+    double calculateDistance(const geometry_msgs::msg::Pose& pose1, const geometry_msgs::msg::Pose& pose2);
+    void publishZeroVelocity();
+    void resetTaskState();
+    void notifyCenterStopPath();
+    void saveCurrentStateToRedis();
+    void loadSavedStateFromRedis();
+
+    bool isIdle() const { return main_state_ == MainState::IDLE; }
+    bool isPause() const { return main_state_ == MainState::PAUSE; }
+    std::string mainStateToString(MainState state) {
+        switch (state) {
+            case MainState::IDLE:           return "IDLE";
+            case MainState::NEW_TASK:       return "NEW_TASK";
+            case MainState::READY_SET_START: return "READY_SET_START";
+            case MainState::READY_TO_RUN:   return "READY_TO_RUN";
+            case MainState::RUNNING:        return "RUNNING";
+            case MainState::AUTO_OFF:       return "AUTO_OFF";
+            case MainState::POS_LOST:       return "POS_LOST";
+            case MainState::PAUSE:          return "PAUSE";
+            default:                        return "UNKNOWN";
+        }
+    }
+
+    void reportEvent(const std::string& level, const std::string& event_name, const std::string& content);
+    void publishPropertyReport(const std::map<std::string, double>& properties);
+
+    void get_params();
+
+    // 回调组
+    rclcpp::CallbackGroup::SharedPtr subscription_cb_group_;
+    rclcpp::CallbackGroup::SharedPtr publisher_cb_group_;
+    rclcpp::CallbackGroup::SharedPtr service_cb_group_;
+    rclcpp::CallbackGroup::SharedPtr client_cb_group_;
+
+    // redis
+    std::string redis_host_;
+    int redis_port_;
+    int redis_db_;
+
+    std::string device_id_;
+
+    // logger
+    std::string logger_logger_name_;
+    std::string logger_filename_;
+    std::string logger_node_name_;
+    int logger_max_size_;
+    int logger_max_files_;
+    int logger_set_level_;
+};
+
+} // namespace auto_drive_misson_center
+
+#endif // AUTO_DRIVE_MISSION_CENTER_PATH_FOLLOW_HANDLER_HPP_

auto_drive_misson_center/include/auto_drive_misson_center/path_tracking_controller.hpp

@@ -0,0 +1,204 @@
+#ifndef AUTO_DRIVE_MISSION_CENTER_PATH_TRACKING_CONTROLLER_HPP_
+#define AUTO_DRIVE_MISSION_CENTER_PATH_TRACKING_CONTROLLER_HPP_
+
+#include <vector>
+#include <memory>
+#include <string>
+#include "spdlog/logger.h"
+#include "spdlog/spdlog.h"
+#include "spdlog/sinks/rotating_file_sink.h"
+
+namespace auto_drive_misson_center {
+
+// 路径点结构体
+struct PathPoint {
+    double x;          // 经度或UTM X坐标
+    double y;          // 纬度或UTM Y坐标
+    double yaw;        // 朝向角度（弧度）
+    
+    PathPoint(double x_val = 0.0, double y_val = 0.0, double yaw_val = 0.0)
+        : x(x_val), y(y_val), yaw(yaw_val) {}
+};
+
+// 控制输出结构体
+struct ControlOutput {
+    double linear_velocity;   // 线速度
+    double angular_velocity;  // 角速度
+    bool is_completed;        // 是否完成路径跟踪
+    
+    ControlOutput(double linear = 0.0, double angular = 0.0, bool completed = false)
+        : linear_velocity(linear), angular_velocity(angular), is_completed(completed) {}
+};
+
+// 路径跟踪控制器基类
+class PathTrackingController {
+public:
+    PathTrackingController() = default;
+    explicit PathTrackingController(std::shared_ptr<spdlog::logger> logger)
+        : logger_(logger)
+    {}
+    virtual ~PathTrackingController() = default;
+    
+    // 初始化控制器
+    bool initialize(double mission_speed, double turn_radius, 
+                   const std::vector<PathPoint>& path_points) {
+        if(path_points.empty()){
+            if(logger_) logger_->error("path points empty, initialize failed");
+            return false;
+        }
+        mission_speed_ = mission_speed;
+        turn_radius_ = turn_radius;
+        path_points_ = path_points;
+        current_point_index_ = 0;
+        is_initialized_ = false;
+        is_first_update_ = false;
+        
+        // 调用子类的初始化方法
+        if (onInitialize()) {
+            is_initialized_ = true;
+            return true;
+        }
+        return false;
+    }
+    
+    // 运行一次控制计算
+    ControlOutput run(const PathPoint& current_pose) {
+        if (!is_initialized_) {
+            return ControlOutput(0.0, 0.0, false);
+        }
+
+        // 每次执行控制前，先更新路径点下标
+        updateCurrentIndex(current_pose);
+        
+        return onRun(current_pose);
+    }
+    
+    // 重置控制器状态
+    void reset() {
+        current_point_index_ = 0;
+        is_initialized_ = false;
+        is_first_update_ = false;
+        onReset();
+    }
+    
+    // 获取当前路径点索引
+    int getCurrentPointIndex() const { return current_point_index_; }
+    
+    // 设置当前路径点索引
+    void setCurrentPointIndex(int index) { 
+        if (index >= 0 && index < static_cast<int>(path_points_.size())) {
+            current_point_index_ = index; 
+        }
+    }
+    
+    // 检查是否完成路径跟踪
+    virtual bool isPathCompleted(const PathPoint& current_pose) const {
+        return current_point_index_ >= static_cast<int>(path_points_.size()) - 1;
+    }
+
+    virtual bool isPathCompleted() const {
+        return current_point_index_ >= static_cast<int>(path_points_.size()) - 1;
+    }
+
+    // 获取路径点总数
+    int getPathPointCount() const { return static_cast<int>(path_points_.size()); }
+
+protected:
+    // 子类需要实现的虚函数
+    virtual bool onInitialize() = 0;
+    virtual ControlOutput onRun(const PathPoint& current_pose) = 0;
+    virtual void onReset() = 0;
+    
+    // 当前下标更新方法
+    void updateCurrentIndex(const PathPoint& current_pose) {
+        if (path_points_.empty()) {
+            logger_->warn("Path Tracking Controller, path points is empty");
+            return;
+        }
+        logger_->debug("Path Tracking Controller, current_pose(x/y):{}/{}, current_point_index_:{}",
+                        current_pose.x, current_pose.y, current_point_index_);
+        
+        // 如果是第一次进入，初始化current_point_index_
+        if (current_point_index_ == 0 && !is_first_update_) {
+            logger_->info("current_point_index_:{}, is_first_update_:{}",
+                        current_point_index_, is_first_update_);
+            // 寻找距离最近、第二近、第三近的三个点下标
+            std::vector<std::pair<int, double>> distances;
+            for (size_t i = 0; i < path_points_.size(); ++i) {
+                double dx = path_points_[i].x - current_pose.x;
+                double dy = path_points_[i].y - current_pose.y;
+                double distance = std::sqrt(dx * dx + dy * dy);
+                distances.emplace_back(i, distance);
+            }
+            
+            // 按距离排序
+            std::sort(distances.begin(), distances.end(), 
+                     [](const auto& a, const auto& b) { return a.second < b.second; });
+            
+            // 以最小下标为current_point_index_
+            if (distances.size() >= 3) {
+                current_point_index_ = std::min({distances[0].first, distances[1].first, distances[2].first});
+            } else if (!distances.empty()) {
+                current_point_index_ = distances[0].first;
+            }
+            
+            is_first_update_ = true;
+        }
+        
+        // 根据current_point_index_和current_point_index_+1，取出路径坐标，与当前坐标计算距离
+        if (current_point_index_ < static_cast<int>(path_points_.size()) - 1) {
+            // 计算当前点到current_point_index_的距离
+            double dx1 = path_points_[current_point_index_].x - current_pose.x;
+            double dy1 = path_points_[current_point_index_].y - current_pose.y;
+            double distance1 = std::sqrt(dx1 * dx1 + dy1 * dy1);
+            
+            // 计算当前点到current_point_index_+1的距离
+            double dx2 = path_points_[current_point_index_ + 1].x - current_pose.x;
+            double dy2 = path_points_[current_point_index_ + 1].y - current_pose.y;
+            double distance2 = std::sqrt(dx2 * dx2 + dy2 * dy2);
+
+            logger_->debug("current_point_index_:{}, path_points_ size:{}, distance2:{}, distance1:{}",
+                        current_point_index_, path_points_.size(), distance2, distance1);
+            
+            // 如果current_point_index_+1的距离小于current_point_index_距离，current_point_index_++
+            if (distance2 < distance1) {
+                current_point_index_++;
+                
+                // 重复计算，直到不满足条件
+                while (current_point_index_ < static_cast<int>(path_points_.size()) - 1) {
+                    dx1 = path_points_[current_point_index_].x - current_pose.x;
+                    dy1 = path_points_[current_point_index_].y - current_pose.y;
+                    distance1 = std::sqrt(dx1 * dx1 + dy1 * dy1);
+                    
+                    dx2 = path_points_[current_point_index_ + 1].x - current_pose.x;
+                    dy2 = path_points_[current_point_index_ + 1].y - current_pose.y;
+                    distance2 = std::sqrt(dx2 * dx2 + dy2 * dy2);
+                    
+                    if (distance2 >= distance1) {
+                        break;
+                    }
+                    current_point_index_++;
+                }
+            }
+        }
+    }
+    
+    // 保护成员变量，子类可以访问
+    double mission_speed_{0.0};
+    double turn_radius_{1.0};
+    std::vector<PathPoint> path_points_;
+    int current_point_index_{0};
+    bool is_initialized_{false};
+    bool is_first_update_{false};
+    mutable bool angle_was_small_{true};
+
+private:
+    std::shared_ptr<spdlog::logger> logger_;
+};
+
+// 控制器工厂函数类型
+typedef std::shared_ptr<PathTrackingController> (*ControllerFactory)(void);
+
+} // namespace auto_drive_misson_center
+
+#endif // AUTO_DRIVE_MISSION_CENTER_PATH_TRACKING_CONTROLLER_HPP_

auto_drive_misson_center/include/auto_drive_misson_center/path_tracking_controller_adapter.hpp

@@ -0,0 +1,283 @@
+// path_tracking_controller_adapter.hpp
+#ifndef AUTO_DRIVE_MISSION_CENTER_PATH_TRACKING_CONTROLLER_ADAPTER_HPP_
+#define AUTO_DRIVE_MISSION_CENTER_PATH_TRACKING_CONTROLLER_ADAPTER_HPP_
+
+#include <memory>
+#include "auto_drive_misson_center/path_tracking_controller.hpp"
+#include "base_following_controller.hpp"
+#include "lqr_control.hpp"
+#include "pid_control.hpp"
+#include "pure_pursuit_control.hpp"
+#include "spdlog/logger.h"
+#include "spdlog/spdlog.h"
+#include "spdlog/sinks/rotating_file_sink.h"
+
+namespace auto_drive_misson_center {
+
+// LQR控制器适配器
+class LQRControllerAdapter : public PathTrackingController {
+public:
+    LQRControllerAdapter(std::shared_ptr<spdlog::logger> logger = nullptr) 
+        : logger_(logger), PathTrackingController(logger), controller_(std::make_shared<LQR_Controller_C>(logger)) {}
+    
+    ~LQRControllerAdapter() = default;
+
+protected:
+    bool onInitialize() override {
+        // 从PathTrackingController的参数映射到原有控制器的init参数
+        // 注意：原有控制器需要max_linear_vel, max_angular_vel, kp, ki, kd, look_ahead_dis, kv
+        // 这里需要根据需求设置合适的参数值
+        float max_linear_vel = mission_speed_;
+        float max_angular_vel = mission_speed_ / turn_radius_; // 根据转弯半径估算最大角速度
+        float kp = 1.76;  // 需要根据实际情况调整
+        float ki = 0.0;
+        float kd = 0.8;
+        float look_ahead_dis = 0.4;  // 需要根据实际情况调整
+        float kv = 1.0;   // 需要根据实际情况调整
+        
+        controller_->init(max_linear_vel, max_angular_vel, kp, ki, kd, look_ahead_dis, kv);
+        return true;
+    }
+    
+    ControlOutput onRun(const PathPoint& current_pose) override {
+        // 从PathPoint计算所需参数
+        float alpha = 0.0;        // 角度误差
+        float linear_val = 0.0;   // 线速度
+        float dis = 0.0;          // 横向误差
+        
+        computeControlParams(current_pose, alpha, linear_val, dis);
+        logger_->debug("PPAdapter alpha={}, linear_val={}, dis={}", alpha, linear_val, dis);
+        
+        // 调用原有控制器的update方法
+        float angular_velocity = controller_->update(alpha, linear_val, dis);
+        logger_->debug("PP output angular={}", angular_velocity);
+        
+        // 判断是否完成
+        bool completed = isPathCompleted(current_pose);
+        
+        return ControlOutput(mission_speed_, angular_velocity, completed);
+    }
+    
+    void onReset() override {
+        controller_->reset();
+    }
+
+private:
+    std::shared_ptr<LQR_Controller_C> controller_;
+    std::shared_ptr<spdlog::logger> logger_;
+    
+    void computeControlParams(const PathPoint& current_pose, float& alpha, float& linear_val, float& dis) {
+        // 获取当前路径点的索引
+        int idx = current_point_index_;
+        if (idx >= static_cast<int>(path_points_.size()) - 1) {
+            idx = path_points_.size() - 1;
+        }
+        
+        // 计算横向误差（相对于当前路径点）
+        double dx = current_pose.x - path_points_[idx].x;
+        double dy = current_pose.y - path_points_[idx].y;
+        double path_yaw = path_points_[idx].yaw;
+        
+        // 横向误差 = 垂直于路径方向的距离
+        dis = static_cast<float>(-dx * sin(path_yaw) + dy * cos(path_yaw));
+        
+        // 角度误差
+        alpha = static_cast<float>(path_yaw - current_pose.yaw);
+        // 归一化到 [-pi, pi]
+        while (alpha > M_PI) alpha -= 2 * M_PI;
+        while (alpha < -M_PI) alpha += 2 * M_PI;
+        
+        // 线速度
+        linear_val = static_cast<float>(mission_speed_);
+    }
+};
+
+// PID控制器适配器
+class PIDControllerAdapter : public PathTrackingController {
+public:
+    PIDControllerAdapter(std::shared_ptr<spdlog::logger> logger = nullptr)
+        : PathTrackingController(logger), controller_(std::make_shared<PID_Controller_C>(logger)) {}
+    
+    ~PIDControllerAdapter() = default;
+
+protected:
+    bool onInitialize() override {
+        float max_linear_vel = mission_speed_;
+        float max_angular_vel = mission_speed_ / turn_radius_;
+        float kp = 1.76;   // 需要根据实际情况调整
+        float ki = 0.5;   // 需要根据实际情况调整
+        float kd = 0.8;   // 需要根据实际情况调整
+        float look_ahead_dis = 0.4;
+        float kv = 1.0;
+        
+        controller_->init(max_linear_vel, max_angular_vel, kp, ki, kd, look_ahead_dis, kv);
+        return true;
+    }
+    
+    ControlOutput onRun(const PathPoint& current_pose) override {
+        float alpha = 0.0;
+        float linear_val = 0.0;
+        float dis = 0.0;
+        
+        computeControlParams(current_pose, alpha, linear_val, dis);
+        
+        float angular_velocity = controller_->update(alpha, linear_val, dis);
+        bool completed = isPathCompleted(current_pose);
+        
+        return ControlOutput(mission_speed_, angular_velocity, completed);
+    }
+    
+    void onReset() override {
+        controller_->reset();
+    }
+
+private:
+    std::shared_ptr<PID_Controller_C> controller_;
+    
+    void computeControlParams(const PathPoint& current_pose, float& alpha, float& linear_val, float& dis) {
+        int idx = current_point_index_;
+        if (idx >= static_cast<int>(path_points_.size()) - 1) {
+            idx = path_points_.size() - 1;
+        }
+        
+        double dx = current_pose.x - path_points_[idx].x;
+        double dy = current_pose.y - path_points_[idx].y;
+        double path_yaw = path_points_[idx].yaw;
+        
+        // PID控制器主要使用横向误差
+        dis = static_cast<float>(-dx * sin(path_yaw) + dy * cos(path_yaw));
+        alpha = static_cast<float>(path_yaw - current_pose.yaw);
+        linear_val = static_cast<float>(mission_speed_);
+        
+        // 归一化角度
+        while (alpha > M_PI) alpha -= 2 * M_PI;
+        while (alpha < -M_PI) alpha += 2 * M_PI;
+    }
+};
+
+// Pure Pursuit控制器适配器
+class PurePursuitControllerAdapter : public PathTrackingController {
+public:
+    PurePursuitControllerAdapter(std::shared_ptr<spdlog::logger> logger = nullptr)
+        : logger_(logger), PathTrackingController(logger), controller_(std::make_shared<PurePursuit_Controller_C>(logger)) {}
+    
+    ~PurePursuitControllerAdapter() = default;
+
+protected:
+    bool onInitialize() override {
+        float max_linear_vel = mission_speed_;
+        float max_angular_vel = mission_speed_ / turn_radius_;
+        float kp = 1.76;
+        float ki = 0.0;
+        float kd = 0.8;
+        float look_ahead_dis = 0.4;   // 前视距离，需要根据实际情况调整
+        adapter_look_ahead_dis_ = look_ahead_dis;
+        float kv = 1.0;               // 速度系数，需要根据实际情况调整
+        
+        controller_->init(max_linear_vel, max_angular_vel, kp, ki, kd, look_ahead_dis, kv);
+        return true;
+    }
+    
+    ControlOutput onRun(const PathPoint& current_pose) override {
+        float alpha = 0.0;
+        float linear_val = 0.0;
+        float dis = 0.0;
+        
+        // Pure Pursuit使用前视点，计算需要特殊处理
+        computeControlParamsWithLookAhead(current_pose, alpha, linear_val, dis);
+        logger_->debug("PPAdapter alpha={}, linear_val={}, dis={}", alpha, linear_val, dis);
+        
+        float angular_velocity = controller_->update(alpha, linear_val, dis);
+        logger_->debug("PP output angular={}", angular_velocity);
+        bool completed = isPathCompleted(current_pose);
+        
+        return ControlOutput(mission_speed_, angular_velocity, completed);
+    }
+    
+    void onReset() override {
+        controller_->reset();
+    }
+
+private:
+    std::shared_ptr<PurePursuit_Controller_C> controller_;
+    double adapter_look_ahead_dis_; //保存前视距离
+    std::shared_ptr<spdlog::logger> logger_;
+    
+    void computeControlParamsWithLookAhead(const PathPoint& current_pose, 
+                                          float& alpha, float& linear_val, float& dis) {
+        // 查找前视点
+        PathPoint lookahead_point;
+        if (!findLookAheadPoint(current_pose, lookahead_point)) {
+            // 如果找不到前视点，使用当前路径点
+            int idx = current_point_index_;
+            if (idx >= static_cast<int>(path_points_.size())) {
+                idx = path_points_.size() - 1;
+            }
+            lookahead_point = path_points_[idx];
+        }
+        
+        // 计算从当前点到前视点的角度误差
+        double dx = lookahead_point.x - current_pose.x;
+        double dy = lookahead_point.y - current_pose.y;
+        double target_yaw = atan2(dy, dx);
+        
+        // 角度误差
+        alpha = static_cast<float>(target_yaw - current_pose.yaw);
+        while (alpha > M_PI) alpha -= 2 * M_PI;
+        while (alpha < -M_PI) alpha += 2 * M_PI;
+        
+        // 线速度
+        linear_val = static_cast<float>(mission_speed_);
+        
+        // 横向误差（用于更新索引）
+        int idx = current_point_index_;
+        if (idx >= static_cast<int>(path_points_.size()) - 1) {
+            idx = path_points_.size() - 1;
+        }
+        double dx_path = current_pose.x - path_points_[idx].x;
+        double dy_path = current_pose.y - path_points_[idx].y;
+        double path_yaw = path_points_[idx].yaw;
+        dis = static_cast<float>(-dx_path * sin(path_yaw) + dy_path * cos(path_yaw));
+    }
+    
+    bool findLookAheadPoint(const PathPoint& current_pose, PathPoint& lookahead_point) {
+        if (path_points_.empty()) {
+            return false;
+        }
+        
+        // 获取前视距离（从控制器参数获取）
+        // 这里需要从controller_获取lookAheadDis，但由于接口限制，我们使用默认值
+        double look_ahead_distance = adapter_look_ahead_dis_; // 需要与初始化时一致
+        
+        // 从当前点开始沿路径向前查找
+        int start_idx = current_point_index_;
+        if (start_idx >= static_cast<int>(path_points_.size())) {
+            start_idx = path_points_.size() - 1;
+        }
+        
+        double accumulated_dist = 0.0;
+        for (int i = start_idx; i < static_cast<int>(path_points_.size()) - 1; ++i) {
+            double dx = path_points_[i + 1].x - path_points_[i].x;
+            double dy = path_points_[i + 1].y - path_points_[i].y;
+            double segment_length = sqrt(dx * dx + dy * dy);
+            
+            if (accumulated_dist + segment_length >= look_ahead_distance) {
+                // 在前视距离内插值
+                double ratio = (look_ahead_distance - accumulated_dist) / segment_length;
+                lookahead_point.x = path_points_[i].x + ratio * dx;
+                lookahead_point.y = path_points_[i].y + ratio * dy;
+                lookahead_point.yaw = path_points_[i].yaw;
+                return true;
+            }
+            accumulated_dist += segment_length;
+        }
+        
+        // 如果路径长度不够，使用最后一个点
+        lookahead_point = path_points_.back();
+        return true;
+    }
+};
+
+} // namespace auto_drive_misson_center
+
+#endif // AUTO_DRIVE_MISSION_CENTER_PATH_TRACKING_CONTROLLER_ADAPTER_HPP_

auto_drive_misson_center/include/auto_drive_misson_center/pid_control.hpp

@@ -0,0 +1,143 @@
+#ifndef _PID_CONTROL_H_
+#define _PID_CONTROL_H_
+
+#include <iostream>
+#include <vector>
+#include "rclcpp/rclcpp.hpp"
+#include "base_following_controller.hpp"
+/*
+#include "spdlog/logger.h"
+#include "spdlog/spdlog.h"
+#include "spdlog/sinks/rotating_file_sink.h"
+*/
+
+using namespace std;
+
+//#define PID_ISUM_MAX_SIZE 10
+//#define PID_ISUM_MAX_SIZE 60
+#define PID_ISUM_MAX_SIZE 60 //nns test
+
+class FixSizeVec_C
+{
+public:
+    FixSizeVec_C(){};
+    
+    void pushBack(const float data){
+        if(vecSize <= vecData.size())
+        {
+            vecData.erase(vecData.begin());
+            pushBack(data);
+        }
+        else
+        {
+            vecData.push_back(data);
+        }
+        
+    };
+    void resetVec(const unsigned int size){
+        vecSize = size;
+        vecData.clear();
+    };
+    float getSum(){
+        float sum_val = 0.0;
+        for(auto &val : vecData)
+            sum_val += val;
+        return sum_val;
+    };
+
+private:
+    unsigned int vecSize;
+    vector<float> vecData;
+};
+
+class PID_Controller_C : public BaseFollowingController
+{
+public:
+    
+    PID_Controller_C(std::shared_ptr<spdlog::logger> logger_recorder)
+    :BaseFollowingController(logger_recorder), logger_(logger_recorder)
+    {
+            target = 0.0;
+            lastErr = 0.0;
+            sampleTime = 0.01;
+            IsumUpGuard = 2.2;
+            lastTime = 0.0;
+        
+           IsumFixSizeVec.resetVec(PID_ISUM_MAX_SIZE);
+        
+    };
+    ~PID_Controller_C(){
+
+    };
+    void init(const float max_linear_vel, const float max_angular_vel, const float kp, const float ki, \
+        const float kd, const float look_ahead_dis, const float kv)
+    {
+        // std::cout<<"PID init!"<<std::endl;
+        logger_->info("PID init!");
+        Kp = kp;
+        Ki = ki;
+        Kd = kd;
+    };
+    void reset(){
+        target = lastErr = 0.0;
+        sampleTime = 0.01;
+        IsumUpGuard = 2.2;
+        lastTime = 0.0;
+        IsumFixSizeVec.resetVec(PID_ISUM_MAX_SIZE);
+        
+    };
+    void set_pid(const float p_val, const float i_val, const float d_val){
+        Kp = p_val;
+        Ki = i_val;
+        Kd = d_val;
+    };
+    float update(const float alpha, const float linear_val, const float dis){
+        // std::cout<<"PID update!"<<std::endl;
+        logger_->trace("PID update!");
+        double curTime = rclcpp::Clock().now().seconds();
+        double deltaTime;
+        if(lastTime - 0.0 < 1e-6) 
+            deltaTime = 0.0;
+        else
+            deltaTime = curTime - lastTime;
+        
+        float deltaErr = dis - lastErr;
+        float out_val = 0.0;
+
+        //if(deltaTime > sampleTime)
+        {
+            IsumFixSizeVec.pushBack(Ki * deltaTime * dis);
+            float iSum = IsumFixSizeVec.getSum();
+            if(-IsumUpGuard > iSum) iSum = -IsumUpGuard;
+            if(IsumUpGuard < iSum) iSum = IsumUpGuard;
+
+            float dTerm;
+            if(0.1 < fabs(dis) || 0.1 < fabs(lastErr)) dTerm = 0.0;
+            if(deltaTime > 0) dTerm = deltaErr / deltaTime;
+
+            //out_val = Kp * dis + iSum + Kd * dTerm;
+            out_val = Kp * dis + iSum + Kd * deltaErr;
+            //pid_logger->debug("pid update dis:{:5.3}    iSum:{:5.3}    dTerm:{:5.3}   out_val:{:5.3}   deltaTime:{:5.3}", \
+                dis, iSum, dTerm, out_val, deltaTime);
+            logger_->trace("pid update dis:{:5.3}    iSum:{:5.3}    dTerm:{:5.3}   out_val:{:5.3}   deltaTime:{:5.3}", \
+                dis, iSum, dTerm, out_val, deltaTime);
+            lastErr = dis;
+            lastTime = curTime;
+            return out_val;
+        }
+
+    };
+
+    void test(){std::cout<<"pid test!"<<std::endl; logger_->debug("pid test!");};
+private:
+    float Kp, Ki, Kd;
+    float target, lastErr;
+    FixSizeVec_C IsumFixSizeVec;
+    double lastTime;
+    double sampleTime;
+    float IsumUpGuard;
+    //std::shared_ptr<spdlog::logger> pid_logger;
+    std::shared_ptr<spdlog::logger> logger_;
+};
+
+#endif

auto_drive_misson_center/include/auto_drive_misson_center/pid_path_tracking_controller.hpp

@@ -0,0 +1,269 @@
+#ifndef AUTO_DRIVE_MISSION_CENTER_PID_PATH_TRACKING_CONTROLLER_HPP_
+#define AUTO_DRIVE_MISSION_CENTER_PID_PATH_TRACKING_CONTROLLER_HPP_
+
+#include "auto_drive_misson_center/path_tracking_controller.hpp"
+#include <cmath>
+#include <algorithm>
+
+namespace auto_drive_misson_center {
+
+// PID路径跟踪控制器类
+class PIDPathTrackingController : public PathTrackingController {
+public:
+    PIDPathTrackingController() = default;
+    virtual ~PIDPathTrackingController() = default;
+
+protected:
+    // 初始化控制器
+    bool onInitialize() override {
+        
+        // 检查路径点是否有效
+        if (path_points_.empty()) {
+            return false;
+        }
+        
+        // 初始化PID参数
+        kp_ = 2.0;   // 比例系数
+        ki_ = 0.01;  // 积分系数
+        kd_ = 0.5;   // 微分系数
+        
+        // 初始化控制变量
+        integral_error_ = 0.0;
+        previous_error_ = 0.0;
+        is_backward_ = false;  // 默认前进模式
+        
+        return true;
+    }
+    
+    // 运行一次控制计算
+    ControlOutput onRun(const PathPoint& current_pose) override {
+        // 检查路径是否完成
+        if (isPathCompleted(current_pose)) {
+            return ControlOutput(0.0, 0.0, true);
+        }
+        
+        // 更新当前点索引
+        updateCurrentIndex(current_pose);
+        
+        // 获取当前索引
+        int current_index = current_point_index_;
+        
+        // 检查是否到达终点
+        if (current_index >= static_cast<int>(path_points_.size()) - 1) {
+            return ControlOutput(0.0, 0.0, true);
+        }
+        
+        // 计算目标点
+        auto target_point = findTargetPoint(current_pose, current_index);
+        double target_x = target_point.first;
+        double target_y = target_point.second;
+        
+        // 计算角度误差
+        double angle_error = calculateAngleError(current_pose, target_x, target_y);
+        
+        // PID控制计算角速度
+        double angular_velocity = calculatePID(angle_error);
+        
+        // 计算线速度（根据角度误差调整）
+        double linear_velocity = calculateLinearVelocity(angle_error);
+        
+        return ControlOutput(linear_velocity, angular_velocity, isPathCompleted(current_pose));
+    }
+    
+    // 重置控制器状态
+    void onReset() override {
+        integral_error_ = 0.0;
+        previous_error_ = 0.0;
+        is_backward_ = false;
+        angle_was_small_ = true;
+    }
+
+    // 检查路径是否完成的新逻辑
+    bool isPathCompleted(const PathPoint& current_pose) const override {
+        if (current_point_index_ >= static_cast<int>(path_points_.size()) - 1) {
+            return true;
+        }
+        
+        const auto& target = path_points_[current_point_index_];
+        
+        double vehicle_yaw = current_pose.yaw;
+        if (is_backward_) {
+            vehicle_yaw += M_PI;
+        }
+        
+        double v1_x = std::cos(vehicle_yaw);
+        double v1_y = std::sin(vehicle_yaw);
+        
+        double v2_x = target.x - current_pose.x;
+        double v2_y = target.y - current_pose.y;
+        double v2_magnitude = std::sqrt(v2_x * v2_x + v2_y * v2_y);
+        
+        if (v2_magnitude < 0.02) {
+            angle_was_small_ = true;
+            return true;
+        }
+        
+        double cos_angle = (v1_x * v2_x + v1_y * v2_y) / v2_magnitude;
+        cos_angle = std::max(-1.0, std::min(1.0, cos_angle));
+        double angle = std::acos(cos_angle);
+        double abs_angle = std::abs(angle);
+        
+        bool current_angle_small = (abs_angle < M_PI / 2.0);
+        
+        if (angle_was_small_ && !current_angle_small) {
+            angle_was_small_ = current_angle_small;
+            return true;
+        }
+        
+        angle_was_small_ = current_angle_small;
+        return false;
+    }
+    
+private:
+    // 查找目标点，返回目标点的x,y坐标
+    std::pair<double, double> findTargetPoint(const PathPoint& current_pose, int current_index) {
+        double distance_goal = 0.0;
+        int target_index = current_index;
+        
+        // 计算当前位置到当前索引点的距离
+        double dx = path_points_[current_index].x - current_pose.x;
+        double dy = path_points_[current_index].y - current_pose.y;
+        double current_distance = std::sqrt(dx * dx + dy * dy);
+        
+        // 如果当前距离小于0.5，直接使用下一个点
+        if (current_distance < 0.5) {
+            target_index = current_index + 1;
+            if (target_index >= static_cast<int>(path_points_.size()) - 1) {
+                target_index = static_cast<int>(path_points_.size()) - 1;
+            }
+        }
+        
+        // 累加距离直到满足条件
+        for (int i = target_index; i < static_cast<int>(path_points_.size()) - 1; i++) {
+            // 计算当前段距离
+            double segment_dx = path_points_[i + 1].x - path_points_[i].x;
+            double segment_dy = path_points_[i + 1].y - path_points_[i].y;
+            double segment_distance = std::sqrt(segment_dx * segment_dx + segment_dy * segment_dy);
+            
+            distance_goal += segment_distance;
+            
+            // 计算当前位置到i+1点的距离
+            double dx_to_target = path_points_[i + 1].x - current_pose.x;
+            double dy_to_target = path_points_[i + 1].y - current_pose.y;
+            double distance_to_target = std::sqrt(dx_to_target * dx_to_target + dy_to_target * dy_to_target);
+            
+            // 判断条件：distance_goal < 0.5 或 当前点到i+1距离 < 0.2
+            if (distance_goal < 0.5 || distance_to_target < 0.2) {
+                target_index = i + 1;
+            } else {
+                break;
+            }
+        }
+        
+        // 确保不超过路径范围
+        target_index = std::min(target_index, static_cast<int>(path_points_.size()) - 1);
+        
+        // 返回目标点的x,y坐标
+        return {path_points_[target_index].x, path_points_[target_index].y};
+    }
+    
+    // 计算角度误差
+    double calculateAngleError(const PathPoint& current_pose, double target_x, double target_y) {
+        // 计算目标方向向量
+        double target_dx = target_x - current_pose.x;
+        double target_dy = target_y - current_pose.y;
+        
+        // 计算机器人当前方向向量
+        double robot_direction_x = std::cos(current_pose.yaw);
+        double robot_direction_y = std::sin(current_pose.yaw);
+        
+        // 如果是后退状态，方向反向
+        if (is_backward_) {
+            robot_direction_x = -robot_direction_x;
+            robot_direction_y = -robot_direction_y;
+        }
+        
+        // 计算两个向量的夹角
+        double dot_product = robot_direction_x * target_dx + robot_direction_y * target_dy;
+        double robot_magnitude = std::sqrt(robot_direction_x * robot_direction_x + robot_direction_y * robot_direction_y);
+        double target_magnitude = std::sqrt(target_dx * target_dx + target_dy * target_dy);
+        
+        // 避免除零
+        if (robot_magnitude < 1e-10 || target_magnitude < 1e-10) {
+            return 0.0;
+        }
+        
+        double cos_angle = dot_product / (robot_magnitude * target_magnitude);
+        
+        // 限制cos值在[-1, 1]范围内
+        cos_angle = std::max(-1.0, std::min(1.0, cos_angle));
+        
+        double angle = std::acos(cos_angle);
+        
+        // 计算叉积判断方向（逆时针为正）
+        double cross_product = robot_direction_x * target_dy - robot_direction_y * target_dx;
+        if (cross_product < 0) {
+            angle = -angle;
+        }
+        
+        // 归一化到[-π, π]范围
+        while (angle > M_PI) {
+            angle -= 2 * M_PI;
+        }
+        while (angle < -M_PI) {
+            angle += 2 * M_PI;
+        }
+        
+        return angle;
+    }
+    
+    // PID控制计算
+    double calculatePID(double error) {
+        // 积分项
+        integral_error_ += error;
+        
+        // 限制积分项防止积分饱和
+        double max_integral = 1.0 / ki_;  // 简单限制
+        integral_error_ = std::max(-max_integral, std::min(max_integral, integral_error_));
+        
+        // 微分项
+        double derivative = error - previous_error_;
+        previous_error_ = error;
+        
+        // PID输出
+        double output = kp_ * error + ki_ * integral_error_ + kd_ * derivative;
+        
+        // 限制输出范围
+        double max_angular_velocity = 1.0;
+        return std::max(-max_angular_velocity, std::min(max_angular_velocity, output));
+    }
+    
+    // 计算线速度
+    double calculateLinearVelocity(double angle_error) {
+        // 根据角度误差调整线速度，误差越大速度越小
+        double max_velocity = mission_speed_;
+        double min_velocity = mission_speed_ * 0.3;
+        
+        // 角度误差的绝对值
+        double abs_error = std::abs(angle_error);
+        
+        // 当角度误差较大时降低速度
+        if (abs_error > M_PI / 4) {  // 45度以上
+            return min_velocity;
+        } else if (abs_error > M_PI / 8) {  // 22.5度以上
+            return max_velocity * 0.6;
+        } else {
+            return max_velocity;
+        }
+    }
+    
+    // PID参数
+    double kp_, ki_, kd_;
+    double integral_error_;
+    double previous_error_;
+    bool is_backward_;
+};
+
+} // namespace auto_drive_misson_center
+
+#endif // AUTO_DRIVE_MISSION_CENTER_PID_PATH_TRACKING_CONTROLLER_HPP_

auto_drive_misson_center/include/auto_drive_misson_center/pure_pursuit_control.hpp

@@ -0,0 +1,58 @@
+#ifndef _PURE_PURSUIT_CONTROLL_H_
+#define _PURE_PURSUIT_CONTROLL_H_
+
+#include <iostream>
+#include <math.h>
+#include <Eigen/Dense>
+#include "base_following_controller.hpp"
+#include "pid_control.hpp"
+
+using namespace Eigen;
+
+class PurePursuit_Controller_C : public BaseFollowingController
+{
+public:
+    PurePursuit_Controller_C() = delete;
+    PurePursuit_Controller_C(std::shared_ptr<spdlog::logger> logger_recorder)
+    :BaseFollowingController(logger_recorder), logger_(logger_recorder){};
+    ~PurePursuit_Controller_C(){};
+
+    void init(const float max_linear_vel, const float max_angular_vel, const float kp, const float ki, \
+        const float kd, const float look_ahead_dis, const float kv)
+    {
+        // std::cout<<"PurePursuit init!"<<std::endl;
+        logger_->info("PurePursuit init! max_linear_vel:{}, max_angular_vel:{}", max_linear_vel, max_angular_vel);
+        maxLinearVel = max_linear_vel;
+        maxAngularVel = max_angular_vel;
+        lookAheadDis = look_ahead_dis;
+        k_v = kv;
+    }
+
+    float update(const float alpha, const float linear_val, const float dis)
+    {
+        // std::cout<<"PurePursuit update!"<<std::endl;
+        logger_->trace("PurePursuit update!");
+        float w_val = (2 * fabs(linear_val) * sin(alpha) / (k_v * fabs(linear_val) + lookAheadDis));  
+        w_val = fabs(w_val) > maxAngularVel ? (maxAngularVel * (w_val > 0.0 ? 1.0 : -1.0)) : w_val;
+        // std::cout << "linear_val: " << linear_val << ", w_val: " << w_val << std::endl;
+        logger_->trace("linear_val:{}, w_val:{}", linear_val, w_val);
+        return w_val;
+    };
+
+    void reset()
+    {
+        //pid_.reset();
+    }
+
+    void test(){std::cout<<"pp test!"<<std::endl; logger_->debug("pp test!");};
+
+private:
+    float maxLinearVel;
+    float maxAngularVel;
+    float lookAheadDis, k_v;
+    float maxSlowAngularVel, minToleranceAngle;
+    //PID_Controller_C pid_;
+    std::shared_ptr<spdlog::logger> logger_;
+};
+
+#endif

auto_drive_misson_center/include/auto_drive_misson_center/redis_utils.hpp

@@ -0,0 +1,41 @@
+#ifndef AUTO_DRIVE_MISSION_CENTER_REDIS_UTILS_HPP_
+#define AUTO_DRIVE_MISSION_CENTER_REDIS_UTILS_HPP_
+
+#include <string>
+#include <memory>
+#include <hiredis/hiredis.h>
+
+namespace auto_drive_misson_center {
+
+class RedisUtils {
+public:
+    RedisUtils();
+    ~RedisUtils();
+    
+    // 连接Redis服务器
+    bool connect(const std::string& host = "127.0.0.1", int port = 6379, int db = 0);
+    
+    // 断开连接
+    void disconnect();
+    
+    // 基本操作
+    bool set(const std::string& key, const std::string& value);
+    std::string get(const std::string& key);
+    bool exists(const std::string& key);
+    bool del(const std::string& key);
+    
+    // 检查连接状态
+    bool is_connected() const { return connected_; }
+
+private:
+    redisContext* context_{nullptr};
+    bool connected_{false};
+    int current_db_{0};
+    
+    // 选择数据库
+    bool select_db(int db);
+};
+
+} // namespace auto_drive_misson_center
+
+#endif // AUTO_DRIVE_MISSION_CENTER_REDIS_UTILS_HPP_

auto_drive_misson_center/include/auto_drive_misson_center/task_tracking_controller.hpp

@@ -0,0 +1,67 @@
+#ifndef AUTO_DRIVE_MISSION_CENTER_TASK_TRACKING_CONTROLLER_HPP_
+#define AUTO_DRIVE_MISSION_CENTER_TASK_TRACKING_CONTROLLER_HPP_
+
+#include "auto_drive_misson_center/path_tracking_controller.hpp"
+
+namespace auto_drive_misson_center {
+
+// 任务跟踪控制器类
+class TaskTrackingController : public PathTrackingController {
+public:
+    TaskTrackingController() = default;
+    virtual ~TaskTrackingController() = default;
+
+protected:
+    // 初始化控制器
+    bool onInitialize() override {
+        // 重置当前点索引
+        current_point_index_ = 0;
+        
+        // 检查路径点是否有效
+        if (path_points_.empty()) {
+            return false;
+        }
+        
+        // 初始化控制参数
+        max_linear_velocity_ = mission_speed_ * 0.5;  // 根据任务速度调整最大线速度
+        max_angular_velocity_ = 1.0;                 // 最大角速度
+        
+        return true;
+    }
+    
+    // 运行一次控制计算
+    ControlOutput onRun(const PathPoint& current_pose) override {
+        // 检查路径是否完成
+        if (isPathCompleted(current_pose)) {
+            return ControlOutput(0.0, 0.0, true);
+        }
+        
+        // 获取当前目标点
+        const PathPoint& target_point = path_points_[current_point_index_];
+        
+        // 计算到目标点的控制输出
+        ControlOutput output = ControlOutput(0, 0, false);
+        
+        // 每次运行后，current_point_index_加1
+        current_point_index_++;
+        
+        // 检查是否完成路径
+        output.is_completed = isPathCompleted(current_pose);
+        
+        return output;
+    }
+    
+    // 重置控制器状态
+    void onReset() override {
+        current_point_index_ = 0;
+    }
+    
+private:
+    double max_linear_velocity_{0.5};
+    double max_angular_velocity_{1.0};
+    
+};
+
+} // namespace auto_drive_misson_center
+
+#endif // AUTO_DRIVE_MISSION_CENTER_TASK_TRACKING_CONTROLLER_HPP_

auto_drive_misson_center/launch/all_nodes_launch.py

@@ -0,0 +1,155 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+
+def generate_launch_description():
+    ld = LaunchDescription()
+
+    # 1. 在顶层也声明参数，允许从命令行传入
+    # 默认值可以根据需求设定，这里设为 True 
+    declare_use_respawn = DeclareLaunchArgument(
+        'use_respawn',
+        default_value='True',
+        description='Pass respawn setting to sub-launch files'
+    )
+    ld.add_action(declare_use_respawn)
+
+    # 获取配置
+    use_respawn = LaunchConfiguration('use_respawn')
+
+    # 获取各个功能包的共享目录
+    auto_drive_mqtt_dir = get_package_share_directory('auto_drive_mqtt')
+    auto_drive_msg_center_dir = get_package_share_directory('auto_drive_msg_center')
+    auto_drive_misson_center_dir = get_package_share_directory('auto_drive_misson_center')
+    auto_drive_controller_driver_dir = get_package_share_directory('auto_drive_controller_driver')
+    auto_drive_can_controller_dir = get_package_share_directory('auto_drive_can_controller')
+    auto_drive_proj_bmi088_driver_dir = get_package_share_directory('auto_drive_proj_bmi088_driver')
+    auto_drive_proj_rtk_driver_dir = get_package_share_directory('auto_drive_proj_rtk_driver')
+    auto_drive_config_management_dir = get_package_share_directory('auto_drive_config_management')
+    auto_drive_log_management_dir = get_package_share_directory('auto_drive_log_management')
+    auto_drive_serial_bridge_dir = get_package_share_directory('auto_drive_serial_bridge')
+    
+    # 启动MQTT桥接节点
+    mqtt_bridge_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(auto_drive_mqtt_dir, 'launch', 'mqtt_bridge_launch.py')
+        ),
+        # 关键：将顶层的参数映射到底层 launch 文件的参数名
+        launch_arguments={
+            'use_respawn': use_respawn
+        }.items()
+    )
+    ld.add_action(mqtt_bridge_launch)
+
+    # 启动消息网关节点
+    msg_gateway_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(auto_drive_msg_center_dir, 'launch', 'msg_gateway_launch.py')
+        ),
+        launch_arguments={
+            'use_respawn': use_respawn
+        }.items()
+    )
+    ld.add_action(msg_gateway_launch)
+
+    # 启动控制器驱动节点
+    controller_driver_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(auto_drive_controller_driver_dir, 'launch', 'controller_drive_launch.py')
+        ),
+        launch_arguments={
+            'use_respawn': use_respawn
+        }.items()
+    )
+    ld.add_action(controller_driver_launch)
+
+    # 启动CAN控制器节点（使用Node方式）
+    can_controller_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(auto_drive_can_controller_dir, 'launch', 'can_controller_launch.py')
+        ),
+        launch_arguments={
+            'use_respawn': use_respawn
+        }.items()
+    )
+    ld.add_action(can_controller_launch)
+    
+    # 启动任务中心节点
+    mission_center_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(auto_drive_misson_center_dir, 'launch', 'mission_center_launch.py')
+        ),
+        launch_arguments={
+            'use_respawn': use_respawn
+        }.items()
+    )
+    ld.add_action(mission_center_launch)
+
+    # 启动BMI088驱动节点
+    bmi088_driver_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(auto_drive_proj_bmi088_driver_dir, 'launch', 'bmi088_driver_launch.py')
+        ),
+        launch_arguments={
+            'use_respawn': use_respawn
+        }.items()
+    )
+    ld.add_action(bmi088_driver_launch)
+
+    # 启动RTK驱动节点
+    rtk_driver_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(auto_drive_proj_rtk_driver_dir, 'launch', 'rtk_driver.launch.py')
+        ),
+        launch_arguments={
+            'use_respawn': use_respawn
+        }.items()
+    )
+    ld.add_action(rtk_driver_launch)
+
+    # 启动配置管理节点
+    config_management_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(auto_drive_config_management_dir, 'launch', 'config_management_launch.py')
+        ),
+        launch_arguments={
+            'use_respawn': use_respawn
+        }.items()
+    )
+    ld.add_action(config_management_launch)
+
+    # 启动日志管理节点
+    log_management_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(auto_drive_log_management_dir, 'launch', 'log_management_launch.py')
+        ),
+        launch_arguments={
+            'use_respawn': use_respawn
+        }.items()
+    )
+    ld.add_action(log_management_launch)
+
+    # 启动串口通信节点
+    serial_bridge_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(auto_drive_serial_bridge_dir, 'launch', 'serial_bridge_launch.py')
+        ),
+        launch_arguments={
+            'use_respawn': use_respawn
+        }.items()
+    )
+    # ld.add_action(serial_bridge_launch)
+
+
+    return ld
+
+
+# 启用所有节点的 respawn（默认）‌： ros2 launch auto_drive_misson_center all_nodes_launch.py
+# ‌禁用所有节点的 respawn‌： ros2 launch auto_drive_misson_center all_nodes_launch.py use_respawn:=False
+# 显式启用 Respawn (True): ros2 launch auto_drive_misson_center all_nodes_launch.py use_respawn:=True
+# 启用特定节点 respawn（单独调用子 launch）‌： ros2 launch auto_drive_proj_bmi088_driver bmi088_driver_launch.py use_respawn:=True

auto_drive_misson_center/launch/mission_center_launch.py

@@ -0,0 +1,118 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, ExecuteProcess, RegisterEventHandler, TimerAction
+from launch.event_handlers import OnProcessStart
+from launch.substitutions import LaunchConfiguration
+from launch.conditions import IfCondition, UnlessCondition
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+
+def generate_launch_description():
+    declare_use_respawn = DeclareLaunchArgument(
+        'use_respawn',
+        default_value='False',
+        description='Enable auto respawn for all mission center nodes'
+    )
+    use_respawn = LaunchConfiguration('use_respawn')
+    
+    config_file = os.path.join(
+        get_package_share_directory('auto_drive_misson_center'),
+        'config',
+        'misson_center_params.yaml'
+    )
+    
+    # 带respawn的节点组
+    auto_mission_handler_respawn = Node(
+        package='auto_drive_misson_center',
+        executable='auto_mission_handler_node',
+        name='auto_mission_handler_node',
+        parameters=[config_file],
+        output='screen',
+        respawn=True,
+        condition=IfCondition(use_respawn)
+    )
+    path_follow_handler_respawn = Node(
+        package='auto_drive_misson_center',
+        executable='path_follow_handler_node',
+        name='path_follow_handler_node',
+        parameters=[config_file],
+        output='screen',
+        respawn=True,
+        condition=IfCondition(use_respawn)
+    )
+    mission_center_respawn = Node(
+        package='auto_drive_misson_center',
+        executable='mission_center_node',
+        name='mission_center_node',
+        parameters=[config_file],
+        output='screen',
+        respawn=True,
+        condition=IfCondition(use_respawn)
+    )
+    
+    # 不带respawn的节点组
+    auto_mission_handler_normal = Node(
+        package='auto_drive_misson_center',
+        executable='auto_mission_handler_node',
+        name='auto_mission_handler_node',
+        parameters=[config_file],
+        output='screen',
+        respawn=False,
+        condition=UnlessCondition(use_respawn)
+    )
+    path_follow_handler_normal = Node(
+        package='auto_drive_misson_center',
+        executable='path_follow_handler_node',
+        name='path_follow_handler_node',
+        parameters=[config_file],
+        output='screen',
+        respawn=False,
+        condition=UnlessCondition(use_respawn)
+    )
+    mission_center_normal = Node(
+        package='auto_drive_misson_center',
+        executable='mission_center_node',
+        name='mission_center_node',
+        parameters=[config_file],
+        output='screen',
+        respawn=False,
+        condition=UnlessCondition(use_respawn)
+    )
+    
+    # 事件等待逻辑，根据参数选择对应节点组
+    wait_for_auto_mission_respawn = RegisterEventHandler(
+        OnProcessStart(
+            target_action=auto_mission_handler_respawn,
+            on_start=[
+                RegisterEventHandler(
+                    OnProcessStart(
+                        target_action=path_follow_handler_respawn,
+                        on_start=[mission_center_respawn]
+                    )
+                ),
+                path_follow_handler_respawn
+            ]
+        )
+    )
+    wait_for_auto_mission_normal = RegisterEventHandler(
+        OnProcessStart(
+            target_action=auto_mission_handler_normal,
+            on_start=[
+                RegisterEventHandler(
+                    OnProcessStart(
+                        target_action=path_follow_handler_normal,
+                        on_start=[mission_center_normal]
+                    )
+                ),
+                path_follow_handler_normal
+            ]
+        )
+    )
+    
+    return LaunchDescription([
+        declare_use_respawn,
+        auto_mission_handler_respawn,
+        auto_mission_handler_normal,
+        wait_for_auto_mission_respawn,
+        wait_for_auto_mission_normal
+    ])

auto_drive_misson_center/launch/test_nodes_launch.py

@@ -0,0 +1,163 @@
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, TimerAction
+from launch.substitutions import LaunchConfiguration
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from ament_index_python.packages import get_package_share_directory
+import os
+
+def generate_launch_description():
+    ld = LaunchDescription()
+
+    # 1. 在顶层声明参数，允许从命令行传入
+    declare_use_respawn = DeclareLaunchArgument(
+        'use_respawn',
+        default_value='True',
+        description='Pass respawn setting to sub-launch files'
+    )
+    ld.add_action(declare_use_respawn)
+
+    # 获取配置
+    use_respawn = LaunchConfiguration('use_respawn')
+
+    # 获取各个功能包的共享目录
+    auto_drive_mqtt_dir = get_package_share_directory('auto_drive_mqtt')
+    auto_drive_msg_center_dir = get_package_share_directory('auto_drive_msg_center')
+    auto_drive_misson_center_dir = get_package_share_directory('auto_drive_misson_center')
+    auto_drive_controller_driver_dir = get_package_share_directory('auto_drive_controller_driver')
+    auto_drive_can_controller_dir = get_package_share_directory('auto_drive_can_controller')
+    auto_drive_proj_bmi088_driver_dir = get_package_share_directory('auto_drive_proj_bmi088_driver')
+    auto_drive_proj_rtk_driver_dir = get_package_share_directory('auto_drive_proj_rtk_driver')
+    auto_drive_config_management_dir = get_package_share_directory('auto_drive_config_management')
+    auto_drive_log_management_dir = get_package_share_directory('auto_drive_log_management')
+    auto_drive_serial_bridge_dir = get_package_share_directory('auto_drive_serial_bridge')
+    
+    # 启动MQTT桥接节点 (立即启动)
+    mqtt_bridge_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(auto_drive_mqtt_dir, 'launch', 'mqtt_bridge_launch.py')
+        ),
+        launch_arguments={
+            'use_respawn': use_respawn
+        }.items()
+    )
+    ld.add_action(mqtt_bridge_launch)
+
+    # 定义延迟启动的动作列表 (延迟15秒后执行)
+    delayed_actions = []
+
+    # 启动消息网关节点
+    msg_gateway_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(auto_drive_msg_center_dir, 'launch', 'msg_gateway_launch.py')
+        ),
+        launch_arguments={
+            'use_respawn': use_respawn
+        }.items()
+    )
+    delayed_actions.append(msg_gateway_launch)
+
+    # 启动控制器驱动节点
+    controller_driver_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(auto_drive_controller_driver_dir, 'launch', 'controller_drive_launch.py')
+        ),
+        launch_arguments={
+            'use_respawn': use_respawn
+        }.items()
+    )
+    delayed_actions.append(controller_driver_launch)
+
+    # 启动CAN控制器节点
+    can_controller_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(auto_drive_can_controller_dir, 'launch', 'can_controller_launch.py')
+        ),
+        launch_arguments={
+            'use_respawn': use_respawn
+        }.items()
+    )
+    delayed_actions.append(can_controller_launch)
+    
+    # 启动任务中心节点
+    mission_center_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(auto_drive_misson_center_dir, 'launch', 'mission_center_launch.py')
+        ),
+        launch_arguments={
+            'use_respawn': use_respawn
+        }.items()
+    )
+    delayed_actions.append(mission_center_launch)
+
+    # 启动BMI088驱动节点
+    bmi088_driver_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(auto_drive_proj_bmi088_driver_dir, 'launch', 'bmi088_driver_launch.py')
+        ),
+        launch_arguments={
+            'use_respawn': use_respawn
+        }.items()
+    )
+    delayed_actions.append(bmi088_driver_launch)
+
+    # 启动RTK驱动节点
+    rtk_driver_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(auto_drive_proj_rtk_driver_dir, 'launch', 'rtk_driver.launch.py')
+        ),
+        launch_arguments={
+            'use_respawn': use_respawn
+        }.items()
+    )
+    delayed_actions.append(rtk_driver_launch)
+
+    # 启动配置管理节点
+    config_management_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(auto_drive_config_management_dir, 'launch', 'config_management_launch.py')
+        ),
+        launch_arguments={
+            'use_respawn': use_respawn
+        }.items()
+    )
+    delayed_actions.append(config_management_launch)
+
+    # 启动日志管理节点
+    log_management_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(auto_drive_log_management_dir, 'launch', 'log_management_launch.py')
+        ),
+        launch_arguments={
+            'use_respawn': use_respawn
+        }.items()
+    )
+    delayed_actions.append(log_management_launch)
+
+    # 启动串口通信节点
+    serial_bridge_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(auto_drive_serial_bridge_dir, 'launch', 'serial_bridge_launch.py')
+        ),
+        launch_arguments={
+            'use_respawn': use_respawn
+        }.items()
+    )
+    delayed_actions.append(serial_bridge_launch)
+
+    # 添加TimerAction，在MQTT启动15秒后执行其余所有节点
+    delay_timer = TimerAction(
+        period=15.0,
+        actions=delayed_actions
+    )
+    ld.add_action(delay_timer)
+
+    return ld
+
+
+
+# 启用所有节点的 respawn（默认）‌： ros2 launch auto_drive_misson_center test_nodes_launch.py
+# ‌禁用所有节点的 respawn‌： ros2 launch auto_drive_misson_center test_nodes_launch.py use_respawn:=False
+# 显式启用 Respawn (True): ros2 launch auto_drive_misson_center test_nodes_launch.py use_respawn:=True
+# 启用特定节点 respawn（单独调用子 launch）‌： ros2 launch auto_drive_proj_bmi088_driver bmi088_driver_launch.py use_respawn:=True

auto_drive_misson_center/package.xml

@@ -0,0 +1,25 @@
+<?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>auto_drive_misson_center</name>
+  <version>0.0.0</version>
+  <description>ROS2 C++ package for mission scheduling functionality using auto_drive_proj_utils and auto_drive_proj_msg</description>
+  <maintainer email="user@example.com">User Name</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
+  <depend>auto_drive_proj_utils</depend>
+  <depend>auto_drive_proj_msg</depend>
+  <depend>libcurl-dev</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

auto_drive_misson_center/scripts/auto_4G.sh

@@ -0,0 +1,97 @@
+#!/bin/bash
+
+# ===================== 全局配置变量 =====================
+LOG_DIR="/home/cat/sweet_ws/logs"
+LOG_FILE="${LOG_DIR}/auto_4G.log"
+LOG_MAX_SIZE=$((1 * 1024 * 1024))  # 日志上限1MB
+UNIT_NAME="ModemManager.service"
+MASK_LINK="/etc/systemd/system/${UNIT_NAME}"
+# =======================================================
+
+# 统一日志打印函数：同时输出控制台+日志文件
+log_print() {
+    local msg="$1"
+    local log_line="$(date '+%Y-%m-%d %H:%M:%S'): ${msg}"
+    echo "${log_line}"
+    echo "${log_line}" >> "${LOG_FILE}"
+}
+
+# ===================== 阶段1：初始化日志目录与文件 =====================
+mkdir -p "${LOG_DIR}"
+# 不存在则新建日志文件
+[ ! -f "${LOG_FILE}" ] && touch "${LOG_FILE}"
+# 固定权限
+chmod 755 "${LOG_DIR}"
+chmod 644 "${LOG_FILE}"
+# 设置目录、日志归属
+chown cat:cat "${LOG_DIR}"
+chown cat:cat "${LOG_FILE}"
+
+# 日志超容量自动清空
+file_size=$(du -b "${LOG_FILE}" | awk '{print $1}')
+if [ "${file_size}" -gt "${LOG_MAX_SIZE}" ]; then
+    log_print "日志文件超过1MB，执行清空操作"
+    > "${LOG_FILE}"
+fi
+# =====================================================================
+
+# ===================== 阶段2：屏蔽 ModemManager 服务 =====================
+log_print "========== 开始处理 ${UNIT_NAME} =========="
+if systemctl list-unit-files --type=service | grep -q "^${UNIT_NAME}"; then
+    log_print "检测到系统存在 ${UNIT_NAME}"
+
+    # 停止正在运行的服务
+    if systemctl is-active --quiet "${UNIT_NAME}"; then
+        systemctl stop "${UNIT_NAME}"
+        log_print "已停止运行中的 ${UNIT_NAME}"
+    else
+        log_print "${UNIT_NAME} 当前未运行，无需停止"
+    fi
+
+    # 关闭开机自启
+    if systemctl is-enabled --quiet "${UNIT_NAME}"; then
+        systemctl disable "${UNIT_NAME}"
+        log_print "已关闭 ${UNIT_NAME} 开机自启"
+    else
+        log_print "${UNIT_NAME} 已禁用开机自启，跳过"
+    fi
+
+    # 兼容旧systemd 判断是否已mask
+    if [ ! -L "${MASK_LINK}" ] || [ "$(readlink -f "${MASK_LINK}")" != "/dev/null" ]; then
+        systemctl mask "${UNIT_NAME}"
+        log_print "执行mask永久屏蔽 ${UNIT_NAME}"
+    else
+        log_print "${UNIT_NAME} 已完成屏蔽，无需重复操作"
+    fi
+
+else
+    log_print "系统未安装 ${UNIT_NAME}，跳过屏蔽流程"
+fi
+log_print "========== ${UNIT_NAME} 处理完成 =========="
+# =====================================================================
+
+# ===================== 阶段3：防重复启动 quectel-CM =====================
+if pidof quectel-CM > /dev/null; then
+    log_print "ERROR: quectel-CM 进程已在运行，脚本退出"
+    exit 1
+fi
+# =====================================================================
+
+# ============================ 阶段4：4G拨号 ============================
+log_print "========================================"
+log_print "4G拨号服务启动"
+log_print "========================================"
+
+# 等待USB 4G模组硬件初始化
+sleep 25
+
+log_print "开始前台执行 /usr/bin/quectel-CM -s cmnet(3gnet,ctnet)"
+# 前台运行 quectel-CM（不添加 &）
+# #APN:
+# 移动："cmnet"
+# 联通："3gnet"
+# 电信："ctnet"
+/usr/bin/quectel-CM -s cmnet >> "$LOG_FILE" 2>&1
+
+# 程序退出后执行日志
+log_print "quectel-CM 进程已退出, 脚本执行结束"

auto_drive_misson_center/scripts/auto_start.sh

@@ -0,0 +1,44 @@
+#!/bin/bash
+
+# Sweet ROS2 Foxy 启动脚本
+# 用于systemd服务
+
+# # 设置环境变量
+# export ROS_DISTRO=foxy
+# export ROS_VERSION=2
+# export ROS_LOCALHOST_ONLY=0
+
+# # 设置日志级别（可选）
+# export RCUTILS_CONSOLE_OUTPUT_FORMAT="[{severity}]: {message}"
+# export RCUTILS_LOGGING_USE_STDERR=0
+# export RCUTILS_LOGGING_BUFFERED_STREAM=1
+
+# 先启动升级程序（后台运行）
+if [ -f /home/cat/upgrade_ws/install/setup.bash ]; then
+    source /home/cat/upgrade_ws/install/setup.bash
+    
+    # 检查升级程序
+    if ! pgrep -f "ros2 run upgrade_driver upgrade_driver" > /dev/null; then
+        echo "Starting upgrade_driver..."
+        nohup ros2 run upgrade_driver upgrade_driver > /tmp/upgrade_driver.log 2>&1 &
+        sleep 2
+    fi
+fi
+
+# Source ROS2环境
+source /opt/ros/foxy/setup.bash
+
+# Source工作空间
+source /home/cat/sweet_ws/install/setup.bash
+
+# 等待网络和ROS2核心完全启动
+sleep 15
+
+# 启动所有节点
+exec ros2 launch auto_drive_misson_center all_nodes_launch.py use_respawn:=True
+
+# nohup ros2 launch auto_drive_misson_center all_nodes_launch.py >> /home/cat/sweet_ws/logs/start_nodes.log 2>&1 &
+
+
+
+

auto_drive_misson_center/scripts/deploy_4G_ros2_systemd_service.sh

@@ -0,0 +1,113 @@
+#!/bin/bash
+# systemd服务部署脚本
+
+set -e  # 遇到错误立即退出
+
+echo "========================================="
+echo "开始部署systemd服务..."
+echo "========================================="
+
+# 检查是否以root权限运行
+if [ "$EUID" -ne 0 ]; then 
+    echo "错误：请使用sudo或以root用户执行此脚本"
+    exit 1
+fi
+
+# 定义服务文件路径
+SERVICE_4G="/etc/systemd/system/auto_4G.service"
+SERVICE_ROS="/etc/systemd/system/sweet-ros-nodes-startup.service"
+
+# 备份已存在的服务文件
+if [ -f "$SERVICE_4G" ]; then
+    echo "备份 $SERVICE_4G 到 ${SERVICE_4G}.backup"
+    cp "$SERVICE_4G" "${SERVICE_4G}.backup"
+fi
+
+if [ -f "$SERVICE_ROS" ]; then
+    echo "备份 $SERVICE_ROS 到 ${SERVICE_ROS}.backup"
+    cp "$SERVICE_ROS" "${SERVICE_ROS}.backup"
+fi
+
+# 写入 auto_4G.service
+echo "创建 $SERVICE_4G ..."
+cat > "$SERVICE_4G" << 'EOF'
+[Unit]
+Description=auto_4G daemon
+After=network.target
+Before=multi-user.target
+
+[Service]
+Type=simple
+ExecStart=/bin/bash /opt/auto_4G.sh
+Restart=always
+RestartSec=10
+User=root
+StandardOutput=journal
+StandardError=journal
+
+[Install]
+WantedBy=multi-user.target
+EOF
+
+# 写入 sweet-ros-nodes-startup.service
+echo "创建 $SERVICE_ROS ..."
+cat > "$SERVICE_ROS" << 'EOF'
+[Unit]
+Description=Sweet ROS2 Foxy All Nodes Startup Service
+After=network.target
+Wants=network.target
+
+[Service]
+Type=simple
+User=cat
+Group=cat
+WorkingDirectory=/home/cat/sweet_ws
+ExecStart=/bin/bash /home/cat/sweet_ws/install/auto_drive_misson_center/share/auto_drive_misson_center/scripts/auto_start.sh
+#Restart=on-failure
+#RestartSec=10
+StandardOutput=journal
+StandardError=journal
+SyslogIdentifier=sweet-ros-nodes
+
+[Install]
+WantedBy=multi-user.target
+EOF
+
+# 重新加载systemd配置
+echo "重新加载systemd配置..."
+systemctl daemon-reload
+
+# 启用服务（开机自启动）
+echo "启用 auto_4G.service ..."
+systemctl enable auto_4G.service
+
+echo "启用 sweet-ros-nodes-startup.service ..."
+systemctl enable sweet-ros-nodes-startup.service
+
+# 显示服务状态
+echo "========================================="
+echo "服务状态："
+echo "========================================="
+echo ""
+echo "auto_4G.service 状态："
+systemctl status auto_4G.service --no-pager || true
+echo ""
+echo "sweet-ros-nodes-startup.service 状态："
+systemctl status sweet-ros-nodes-startup.service --no-pager || true
+
+echo ""
+echo "========================================="
+echo "部署完成！"
+echo "========================================="
+echo ""
+echo "可用命令："
+echo "  启动服务：systemctl start auto_4G.service"
+echo "  停止服务：systemctl stop auto_4G.service"
+echo "  重启服务：systemctl restart auto_4G.service"
+echo "  查看日志：journalctl -u auto_4G.service -f"
+echo ""
+echo "  启动服务：systemctl start sweet-ros-nodes-startup.service"
+echo "  停止服务：systemctl stop sweet-ros-nodes-startup.service"
+echo "  重启服务：systemctl restart sweet-ros-nodes-startup.service"
+echo "  查看日志：journalctl -u sweet-ros-nodes-startup.service -f"
+echo "========================================="

auto_drive_misson_center/scripts/mock_invoke_missionOn_rtk_node.py

@@ -0,0 +1,315 @@
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import PoseWithCovarianceStamped
+from auto_drive_proj_msg.srv import SetPose, ReqString
+import requests
+import json
+import argparse
+
+
+class MockPoseNode(Node):
+    def __init__(self, url: str, seg_points: int):
+        super().__init__("mock_invoke_missionOn_rtk_node")
+        self.target_url = url
+        self.SEGMENT_POINTS = seg_points  # 每两个拐点之间插值点数
+
+        # 1. /start_pose service
+        self.srv_start_pose = self.create_service(
+            SetPose,
+            "/start_pose",
+            self.handle_set_pose
+        )
+        self.get_logger().info("Service /start_pose created.")
+
+        # ========== 在这里新增 /invoke/xxx 服务名 ==========
+        invoke_service_names = [
+            "/motorOn",
+            "/borrow",
+            "/lift",
+        ]
+        self.invoke_services = []
+        for srv_name in invoke_service_names:
+            # lambda闭包捕获srv_name，传递给通用处理函数
+            srv = self.create_service(
+                ReqString,
+                srv_name,
+                lambda req, resp, name=srv_name: self._generic_invoke_handler(req, resp, name)
+            )
+            self.invoke_services.append(srv)
+            self.get_logger().info(f"Mock service created: {srv_name}")
+
+        # missionOn client
+        self.mission_on_client = self.create_client(ReqString, "/invoke/missionOn")
+
+        self.pub_rtk = None
+        self.coord_list = []       # geojson原始拐点
+        self.interp_points = []    # 插值之后全部点
+        self.index = 0             # 当前在interp_points的下标
+        self.timer = None
+        self.timer_period = 0.2
+        self.mission_on_done = False
+        self.trajectory_finished = False
+
+        # 一次性定时器，延迟100ms执行missionOn调用逻辑
+        self.init_timer = self.create_timer(0.1, self.do_mission_on_call)
+
+    def _generic_invoke_handler(self, request, response, srv_name: str):
+        """
+        :param request: ReqString.Request
+        :param response: ReqString.Response
+        :param srv_name: 当前被调用的服务名，例如 /invoke/borrow
+        """
+        self.get_logger().info(
+            f"[SERVICE CALL] service={srv_name}, request={request.request}, value={request.value}"
+        )
+        response.success = True
+        response.message = "ok"
+        response.properties = ""
+        return response
+
+    def do_mission_on_call(self):
+        """一次性执行：等待服务、调用 /invoke/missionOn"""
+        self.destroy_timer(self.init_timer)
+        if self.mission_on_done:
+            return
+        self.get_logger().info("Waiting for service /invoke/missionOn ...")
+        if not self.mission_on_client.wait_for_service(timeout_sec=20.0):
+            self.get_logger().error("/invoke/missionOn service not available after 20s, skip missionOn")
+            return
+        self.get_logger().info("/invoke/missionOn service ready, sending request")
+        req = ReqString.Request()
+        payload = [{"name": "value", "value": self.target_url}]
+        req.request = json.dumps(payload)
+        req.value = ""
+        future = self.mission_on_client.call_async(req)
+        future.add_done_callback(self.mission_on_future_callback)
+
+    def mission_on_future_callback(self, future):
+        self.mission_on_done = True
+        try:
+            resp = future.result()
+        except Exception as e:
+            self.get_logger().error(f"call /invoke/missionOn exception: {str(e)}")
+            return
+        if resp.success:
+            self.get_logger().info(f"/invoke/missionOn success, message={resp.message}")
+            self._post_missionon_ok()
+        else:
+            self.get_logger().error(
+                f"/invoke/missionOn return success=false. msg={resp.message}, properties={resp.properties}"
+            )
+
+    def _post_missionon_ok(self):
+        """missionOn调用成功后：初始化rtk发布、加载geojson、启动周期定时器"""
+        self.get_logger().info("Proceed load geojson and start /rtk_pose publish timer")
+        self.pub_rtk = self.create_publisher(
+            PoseWithCovarianceStamped,
+            "/rtk_pose",
+            10
+        )
+        self.load_geojson()
+        self.timer = self.create_timer(self.timer_period, self.publish_rtk_pose_cb)
+
+    def load_geojson(self):
+        try:
+            self.get_logger().info(f"Loading geojson from: {self.target_url}")
+            resp = requests.get(self.target_url, timeout=10)
+            resp.raise_for_status()
+            data = resp.json()
+            feature = data["features"][0]
+            geom = feature["geometry"]
+            self.coord_list = geom["coordinates"]
+            self.get_logger().info(f"Loaded raw waypoints count: {len(self.coord_list)}")
+
+            # 执行线性插值，每两个拐点之间生成 SEGMENT_POINTS 个点
+            self.interp_points = []
+            raw_cnt = len(self.coord_list)
+            if raw_cnt < 2:
+                self.interp_points = self.coord_list.copy()
+            else:
+                for i in range(raw_cnt - 1):
+                    p0 = self.coord_list[i]
+                    p1 = self.coord_list[i+1]
+                    lon0, lat0, alt0 = p0
+                    lon1, lat1, alt1 = p1
+                    for t_step in range(self.SEGMENT_POINTS):
+                        t = t_step / (self.SEGMENT_POINTS - 1)
+                        lon = lon0 + t * (lon1 - lon0)
+                        lat = lat0 + t * (lat1 - lat0)
+                        alt = alt0 + t * (alt1 - alt0)
+                        self.interp_points.append([lon, lat, alt])
+
+            self.get_logger().info(f"seg_points={self.SEGMENT_POINTS}, interpolated total points count: {len(self.interp_points)}")
+            self.index = 0
+
+        except Exception as e:
+            self.get_logger().error(f"Load geojson failed: {str(e)}")
+
+    def handle_set_pose(self, request, response):
+        self.get_logger().info(
+            f"Received SetPose request lat={request.lat:.6f}, lon={request.lon:.6f}, alt={request.alt:.2f}, yaw={request.yaw:.2f}"
+        )
+        response.success = True
+        return response
+
+    def publish_rtk_pose_cb(self):
+        if not self.interp_points or self.pub_rtk is None:
+            return
+
+        # 取当前插值点
+        lon, lat, alt = self.interp_points[self.index]
+
+        msg = PoseWithCovarianceStamped()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.header.frame_id = "enu"
+        msg.pose.pose.position.x = lon
+        msg.pose.pose.position.y = lat
+        msg.pose.pose.position.z = alt
+        msg.pose.pose.orientation.w = 1.0
+        msg.pose.pose.orientation.x = 0.0
+        msg.pose.pose.orientation.y = 0.0
+        msg.pose.pose.orientation.z = 0.0
+        for i in range(36):
+            msg.pose.covariance[i] = 0.0
+
+        self.pub_rtk.publish(msg)
+        # self.get_logger().info(
+        #     f"publish rtk data, lat={lat:.6f}, lon={lon:.6f}, alt={alt:.2f}, index={self.index}, interp_points={len(self.interp_points)}"
+        # )
+
+        if self.index < len(self.interp_points) - 1:
+            self.index += 1
+        else:
+            if not self.trajectory_finished:
+                self.trajectory_finished = True
+                self.get_logger().info("All interpolated trajectory points consumed, keep publishing last point.")
+
+
+def main(args=None):
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--url", type=str,
+                        default="https://test.sweetai.cn:4433/load/locationCmdPath/1701148411059699712.json",
+                        help="geojson path url for missionOn & rtk mock")
+    parser.add_argument("--seg-points", type=int, default=100,
+                        help="number of interpolated points per segment (default:100)")
+
+    parsed_args, ros_args = parser.parse_known_args(args)
+
+    rclpy.init(args=ros_args)
+    node = MockPoseNode(url=parsed_args.url, seg_points=parsed_args.seg_points)
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        node.get_logger().info("Shutting down mock node")
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()
+
+
+
+'''
+# mock_invoke_missionOn_rtk_node.py 脚本功能说明
+## 整体概述
+这是一个ROS2 Python模拟节点，用于模拟RTK定位输出，加载GeoJSON轨迹文件，对轨迹线段做线性插值，周期发布 `/rtk_pose` 定位话题；同时模拟一批`/invoke/*`系列服务，接收请求直接返回成功，并且自动调用外部服务 `/invoke/missionOn`。
+
+## 主要功能
+1. **模拟服务端**
+    - 提供 `/start_pose`（SetPose类型）服务；
+    - 模拟一批ReqString类型服务：`/motorOn`、`/borrow`、`/lift`；收到任意请求打印请求内容，固定返回`success=true`；
+    - 扩展方式：在代码内`invoke_service_names`列表直接追加服务名即可新增模拟服务。
+2. **自动调用外部服务**
+    - 节点启动0.1s后，作为客户端调用 `/invoke/missionOn` 服务，把传入的url放到request参数中；
+    - 如果 `/invoke/missionOn` 20秒内不可用，则跳过调用；只有调用成功后，才会加载GeoJSON并开始发布RTK话题。
+3. **GeoJSON轨迹加载 + 线性插值**
+    - 通过`--url`传入GeoJSON网络地址，读取`features[0].geometry.coordinates`原始拐点；
+    - 相邻两个拐点之间做直线插值，每段生成N个点，N由`--seg‑points`指定，默认100；插值包含线段起点与终点；
+    - 轨迹全部走完后，持续循环输出最后一个点位。
+4. **RTK话题发布**
+    - 话题：`/rtk_pose`，消息类型：`geometry_msgs/msg/PoseWithCovarianceStamped`；
+    - frame_id：`enu`，协方差全部填0；发布周期固定0.2s。
+5. **命令行参数**
+    | 参数 | 类型 | 默认值 | 说明 |
+    |------|------|--------|------|
+    | `--url` | str | 示例json地址 | GeoJSON轨迹文件http链接 |
+    | `--seg‑points` | int | 100 | 每两个拐点之间插值点数，建议≥2 |
+
+> 注意：`--seg‑points` 设置为1会触发除零异常，使用时数值不要小于2。
+
+## 使用示例
+### 1. 基础运行，使用默认参数
+```bash
+source ~/sweet_ws/install/setup.bash
+python3 mock_invoke_missionOn_rtk_node.py
+```
+
+### 2. 指定轨迹url，使用默认每段100插值点
+```bash
+python3 mock_invoke_missionOn_rtk_node.py --url "https://test.sweetai.cn:4433/load/locationCmdPath/2053755099087552512.json"
+```
+
+### 3. 指定url，每段插值50个点
+```bash
+python3 mock_invoke_missionOn_rtk_node.py \
+--url "https://test.sweetai.cn:4433/load/locationCmdPath/2053755099087552512.json" \
+--seg-points 50
+```
+
+### 4. 测试模拟invoke服务（新开终端）
+```bash
+# 测试 /motorOn
+ros2 service call /motorOn auto_drive_proj_msg/srv/ReqString "{request:'{}', value:''}"
+
+# 测试 /borrow
+ros2 service call /borrow auto_drive_proj_msg/srv/ReqString "{request:'{"cmd":1}', value:'test_val'}"
+```
+
+### 5. 查看输出rtk话题
+```bash
+ros2 topic echo /rtk_pose
+```
+
+## 关键日志示例
+```
+[INFO] Service /start_pose created.
+[INFO] Mock service created: /motorOn
+[INFO] Mock service created: /borrow
+[INFO] Mock service created: /lift
+[INFO] Waiting for service /invoke/missionOn ...
+[INFO] /invoke/missionOn service ready, sending request
+[INFO] /invoke/missionOn success, message=ok
+[INFO] Proceed load geojson and start /rtk_pose publish timer
+[INFO] Loaded raw waypoints count: 12
+[INFO] seg_points=100, interpolated total points count: 1100
+```
+
+轨迹走完日志：
+```
+[INFO] All interpolated trajectory points consumed, keep publishing last point.
+```
+
+## 扩展新模拟服务
+修改代码内列表，直接添加服务名字符串，无需新增回调函数：
+```python
+invoke_service_names = [
+    "/motorOn",
+    "/borrow",
+    "/lift",
+    "/invoke/xxxNew",  #新增此处即可
+]
+```
+
+## 退出程序
+终端按下 `Ctrl + C`。
+
+## 依赖
+- rclpy，ROS2 Foxy环境
+- requests：`pip3 install requests`
+- 自定义消息包：`auto_drive_proj_msg`，需要提前source工作空间。
+
+> 注意：节点启动后会等待`/invoke/missionOn`服务，若该服务不存在，会直接跳过加载轨迹，不会输出`/rtk_pose`话题。
+'''

auto_drive_misson_center/scripts/mock_start_pose_and_publish_rtk.py

@@ -0,0 +1,122 @@
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import PoseWithCovarianceStamped
+from auto_drive_proj_msg.srv import SetPose
+import requests
+import json
+import argparse
+
+
+class MockPoseNode(Node):
+    def __init__(self, url: str):
+        super().__init__("mock_start_pose_rtk_publisher")
+
+        # 1. 模拟 /start_pose service
+        self.srv = self.create_service(
+            SetPose,
+            "/start_pose",
+            self.handle_set_pose
+        )
+        self.get_logger().info("Service /start_pose ready, always return success=True")
+
+        # 2. rtk_pose publisher
+        self.pub_rtk = self.create_publisher(
+            PoseWithCovarianceStamped,
+            "/rtk_pose",
+            10
+        )
+
+        self.url = url
+        self.coord_list = []
+        self.load_geojson()
+
+        self.index = 0
+        self.timer_period = 0.2
+        self.timer = self.create_timer(self.timer_period, self.publish_rtk_pose_cb)
+
+    def load_geojson(self):
+        try:
+            self.get_logger().info(f"Loading geojson from: {self.url}")
+            resp = requests.get(self.url, timeout=10)
+            resp.raise_for_status()
+            data = resp.json()
+            feature = data["features"][0]
+            geom = feature["geometry"]
+            self.coord_list = geom["coordinates"]
+            self.get_logger().info(f"Loaded path points count: {len(self.coord_list)}")
+        except Exception as e:
+            self.get_logger().error(f"Load geojson failed: {str(e)}")
+
+    def handle_set_pose(self, request, response):
+        self.get_logger().info(f"Received SetPose request lat={request.lat:.6f}, lon={request.lon:.6f}, alt={request.alt:.2f}, yaw={request.yaw:.2f}")
+        response.success = True
+        return response
+
+    def publish_rtk_pose_cb(self):
+        if not self.coord_list:
+            return
+
+        lon, lat, alt = self.coord_list[self.index]
+
+        msg = PoseWithCovarianceStamped()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.header.frame_id = "enu"
+
+        msg.pose.pose.position.x = lon
+        msg.pose.pose.position.y = lat
+        msg.pose.pose.position.z = alt
+
+        msg.pose.pose.orientation.w = 1.0
+        msg.pose.pose.orientation.x = 0.0
+        msg.pose.pose.orientation.y = 0.0
+        msg.pose.pose.orientation.z = 0.0
+
+        for i in range(36):
+            msg.pose.covariance[i] = 0.0
+
+        self.pub_rtk.publish(msg)
+
+        self.index += 1
+        if self.index >= len(self.coord_list):
+            self.index = 0
+
+
+def main(args=None):
+    # 解析命令行参数，注意：分离ros args和自定义argparse参数
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--url", type=str,
+                        default="https://test.sweetai.cn:4433/load/locationCmdPath/1701148411059699712.json",
+                        help="geojson path url")
+    parsed_args, ros_args = parser.parse_known_args(args)
+
+    rclpy.init(args=ros_args)
+    node = MockPoseNode(url=parsed_args.url)
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        node.get_logger().info("Shutting down mock node")
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()
+
+# python3 mock_start_pose_and_publish_rtk.py --url  "https://test.sweetai.cn:4433/load/locationCmdPath/2053755099087552512.json"
+
+'''
+功能：
+1. 模拟 `auto_drive_proj_msg/srv/SetPose` 服务 `/start_pose`，任意请求直接返回 `success=True`
+2. HTTP 下载给定 GeoJSON 路径文件，解析 LineString 坐标序列，循环发布 `/rtk_pose` (`geometry_msgs/msg/PoseWithCovarianceStamped`)
+> 注意 GeoJSON coordinates 格式：`[lon, lat, alt]`，geoJSON 标准顺序是**经度在前，纬度在后**；
+> 脚本没有 yaw，pose 的 orientation 简单填 0 四元数；协方差全部填 0；时间戳使用 ROS 系统时间。
+
+关键点说明
+1. GeoJSON LineString 标准顺序：`[longitude, latitude, altitude]`，脚本做了对应映射；原始 json 没有 yaw，姿态固定 yaw=0。
+2. 路径点遍历完会循环从头重新播放，模拟 RTK 持续输出轨迹。
+3. `/start_pose`服务不解析请求参数，任何入参直接返回`success=True`，完全匹配你的需求。
+4. frame_id 写为`map`，你可以根据实际工程改成`enu`。
+5. 发布周期`timer_period = 0.2`，可按需调快 / 调慢。
+'''

auto_drive_misson_center/scripts/set_sweet_quick_commands.sh

@@ -0,0 +1,48 @@
+#!/bin/bash
+
+# 函数：sweetSetQuickCmdToBashrc
+# 功能：将 sweet_quick_commands.sh 的加载命令添加到 ~/.bashrc（若不存在则追加）
+# 要求：遇错即退，执行成功给出提示
+
+sweetSetQuickCmdToBashrc() {
+    # 定义要检查/添加的内容（注意使用单引号防止变量展开，此处使用heredoc或直接字符串）
+    local line_to_add='if [ -f /home/cat/sweet_ws/install/auto_drive_misson_center/share/auto_drive_misson_center/scripts/sweet_quick_commands.sh ]; then
+        . /home/cat/sweet_ws/install/auto_drive_misson_center/share/auto_drive_misson_center/scripts/sweet_quick_commands.sh
+    fi'
+
+    # 目标文件
+    local bashrc="$HOME/.bashrc"
+
+    # 错误处理：遇错即退
+    set -e
+    trap 'echo "错误：函数 sweetSetQuickCmdToBashrc 执行失败，退出码 $?" >&2; return 1' ERR
+
+    # 1. 检查 ~/.bashrc 是否存在，若不存在则创建（通常默认存在，但以防万一）
+    if [ ! -f "$bashrc" ]; then
+        echo "警告：$bashrc 不存在，将创建新文件。"
+        touch "$bashrc" || { echo "错误：无法创建 $bashrc" >&2; return 1; }
+    fi
+
+    # 2. 检查是否已包含该内容（使用 grep 匹配关键特征，避免重复添加）
+    # 这里以 "sweet_quick_commands.sh" 作为唯一标识
+    if grep -q "sweet_quick_commands.sh" "$bashrc"; then
+        echo "提示：~/.bashrc 中已存在 sweet_quick_commands.sh 的加载命令，无需重复添加。"
+        return 0
+    fi
+
+    # 3. 追加内容到 ~/.bashrc 末尾
+    echo "" >> "$bashrc"                      # 加一个空行，保持整洁
+    echo "# 以下内容由 sweetSetQuickCmdToBashrc 添加，用于加载 sweet_quick_commands.sh" >> "$bashrc"
+    echo "$line_to_add" >> "$bashrc"         # 添加实际命令（注意保留换行）
+    echo "提示：已成功将 sweet_quick_commands.sh 的加载命令添加到 ~/.bashrc 末尾。"
+
+    # 4. 可选：立即生效（source），但通常提示用户手动执行
+    # 为避免影响当前 shell 环境，此处不自动 source，仅提示
+    echo "提示：若要立即生效，请执行 source ~/.bashrc 或重新打开终端。"
+
+    # 取消错误陷阱（可选）
+    trap - ERR
+    return 0
+}
+
+sweetSetQuickCmdToBashrc

auto_drive_misson_center/src/auto_mission_handler.cpp

@@ -0,0 +1,509 @@
+#include "auto_drive_misson_center/auto_mission_handler.hpp"
+#include "auto_drive_misson_center/redis_utils.hpp"
+#include "auto_drive_misson_center/logger.hpp"
+#include <thread>
+#include <chrono>
+
+namespace auto_drive_misson_center {
+
+AutoMissionHandler::AutoMissionHandler(const std::string & node_name)
+    : Node(node_name)
+{
+    get_params();
+
+    initLogger(logger_logger_name_, 
+                logger_filename_,
+                logger_node_name_,
+                static_cast<size_t>(logger_max_size_),
+                static_cast<size_t>(logger_max_files_),
+                logger_set_level_);
+
+    // 创建回调组
+    // subscription_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+    // service_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+    // publisher_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+    // timer_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+    // rclcpp::SubscriptionOptions subscription_options;
+    // subscription_options.callback_group = subscription_cb_group_;
+    // rclcpp::PublisherOptions publisher_options;
+    // publisher_options.callback_group = publisher_cb_group_;
+    service_cached_client_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
+
+    path_id_sub_ = this->create_subscription<std_msgs::msg::Int32>(
+        "/auto_path_ind", 10,
+        std::bind(&AutoMissionHandler::path_id_callback, this, std::placeholders::_1)/*,
+        subscription_options*/);
+
+    auto_mission_service_ = this->create_service<auto_drive_proj_msg::srv::ReqString>(
+        "/auto_mission",
+        std::bind(&AutoMissionHandler::auto_mission_callback, this,
+                 std::placeholders::_1, std::placeholders::_2)/*,
+                 rmw_qos_profile_services_default,
+                 service_cb_group_*/);
+
+    event_info_pub_ = this->create_publisher<std_msgs::msg::String>("/event/info", 10/*, publisher_options*/);
+    event_warn_pub_ = this->create_publisher<std_msgs::msg::String>("/event/warn", 10/*, publisher_options*/);
+    event_error_pub_ = this->create_publisher<std_msgs::msg::String>("/event/error", 10/*, publisher_options*/);
+
+    main_timer_ = this->create_wall_timer(
+        std::chrono::milliseconds(10),
+        std::bind(&AutoMissionHandler::mainTimerCallback, this)/*,
+        timer_cb_group_*/);
+
+    mission_state_ = MissionState::IDLE;
+
+    LOG_INFO("Auto Mission Handler started, state: IDLE");
+}
+
+void AutoMissionHandler::get_params()
+{
+    // LOG_DEBUG("Auto Mission Handler get params");
+
+    // redis
+    this->declare_parameter("redis_host", "127.0.0.1");
+    redis_host_ = this->get_parameter("redis_host").get_parameter_value().get<std::string>();
+
+    this->declare_parameter("redis_port", 6379);
+    redis_port_ = this->get_parameter("redis_port").get_parameter_value().get<int>();
+
+    this->declare_parameter("redis_db", 0);
+    redis_db_ = this->get_parameter("redis_db").get_parameter_value().get<int>();
+
+    this->declare_parameter("device_id", "2072868304711761920");
+    device_id_ = this->get_parameter("device_id").get_parameter_value().get<std::string>();
+
+    // logger
+    this->declare_parameter("logger_logger_name", "auto_mission");
+    logger_logger_name_ = this->get_parameter("logger_logger_name").get_parameter_value().get<std::string>();
+
+    this->declare_parameter("logger_filename", "logs/mission_center_logger.txt");
+    logger_filename_ = this->get_parameter("logger_filename").get_parameter_value().get<std::string>();
+
+    this->declare_parameter("logger_node_name", "auto_mission");
+    logger_node_name_ = this->get_parameter("logger_node_name").get_parameter_value().get<std::string>();
+
+    this->declare_parameter("logger_max_size", 10485760);
+    logger_max_size_ = this->get_parameter("logger_max_size").get_parameter_value().get<int>();
+
+    this->declare_parameter("logger_max_files", 5);
+    logger_max_files_ = this->get_parameter("logger_max_files").get_parameter_value().get<int>();
+
+    this->declare_parameter("logger_set_level", 1);
+    logger_set_level_ = this->get_parameter("logger_set_level").get_parameter_value().get<int>();
+}
+
+void AutoMissionHandler::reportEvent(const std::string& level, const std::string& event_name, const std::string& content)
+{
+    json data_obj;
+    data_obj["eventName"] = event_name;
+    data_obj["content"] = content;
+    json root;
+    root["data"] = data_obj;
+
+    std_msgs::msg::String event_msg;
+    event_msg.data = root.dump();
+
+    if (level == "info") {
+        event_info_pub_->publish(event_msg);
+    } else if (level == "warn") {
+        event_warn_pub_->publish(event_msg);
+    } else if (level == "error") {
+        event_error_pub_->publish(event_msg);
+    }
+}
+
+void AutoMissionHandler::path_id_callback(const std_msgs::msg::Int32::SharedPtr msg)
+{
+    current_path_id_ = msg->data;
+}
+
+void AutoMissionHandler::mainTimerCallback()
+{
+    switch (mission_state_) {
+        case MissionState::RUNNING:
+            processRunningState();
+            break;
+        case MissionState::PAUSE:
+            processPauseState();
+            break;
+        case MissionState::CONTINUE:
+            processContinueState();
+            break;
+        case MissionState::IDLE:
+        default:
+            break;
+    }
+}
+
+void AutoMissionHandler::processRunningState()
+{
+    if (mission_json_.empty()) {
+        LOG_WARN("run state, mission json empty");
+        return;
+    }
+
+    // 检查数组边界
+    if (current_mission_index_ >= mission_json_.size()) {
+        LOG_WARN("Current mission index out of bounds: {} >= {}", 
+                 current_mission_index_, mission_json_.size());
+        return;
+    }
+
+    for (size_t i = current_mission_index_; i < mission_json_.size(); i++) {
+        const auto& item = mission_json_[i];
+
+        if (!item.contains("pathInd") || !item.contains("function")) {
+            LOG_WARN("Mission item {} missing required fields", i);
+            continue;
+        }
+
+        // 检查 function 字段是否为数组
+        if (!item["function"].is_array()) {
+            LOG_WARN("Function field is not an array in mission item {}", i);
+            continue;
+        }
+
+        if (current_path_id_ <= item["pathInd"].get<int>()) {
+            LOG_INFO("Executing mission item {} with path_id {}",
+                     i + 1, item["pathInd"].get<int>());
+
+            for (const auto& function : item["function"]) {
+                if (function.contains("functionId") && function.contains("inputs")) {
+                    std::string function_id = function["functionId"].get<std::string>();
+                    json inputs = function["inputs"];
+
+                    if (execute_function(function_id, inputs)) {
+                        LOG_INFO("Running State, {} executed successfully", function_id);
+                    } else {
+                        LOG_WARN("Running State, Failed to execute function: {},\n{}", function_id, inputs.dump());
+                    }
+                }
+            }
+
+            current_mission_index_++;
+            save_current_mission_index();
+            LOG_INFO("Mission item {} completed", i + 1);
+        } else {
+            break;
+        }
+    }
+
+    if (current_mission_index_ >= mission_json_.size()) {
+        reportEvent("info", "MISSION_COMPLETE", "All missions completed");
+        resetMissionState();
+        LOG_INFO("All missions completed");
+    }
+}
+
+void AutoMissionHandler::processPauseState()
+{
+}
+
+void AutoMissionHandler::processContinueState()
+{
+    if (continue_command_index_ >= continue_command_queue_.size()) {
+        LOG_INFO("Continue replay completed, mission state change:{} -> {}", missionStateToString(mission_state_), "RUNNING");
+        mission_state_ = MissionState::RUNNING;
+        reportEvent("info", "MISSION_RESUME", "Mission resumed");
+        return;
+    }
+
+    auto& cmd = continue_command_queue_[continue_command_index_];
+    LOG_INFO("Replaying command: {}", cmd.first);
+
+    if (execute_function(cmd.first, cmd.second)) {
+        continue_command_index_++;
+    } else {
+        LOG_WARN("Failed to replay command: {}", cmd.first);
+        continue_command_index_++;
+    }
+}
+
+void AutoMissionHandler::auto_mission_callback(
+    const auto_drive_proj_msg::srv::ReqString::Request::SharedPtr request,
+    auto_drive_proj_msg::srv::ReqString::Response::SharedPtr response)
+{
+    try {
+        LOG_INFO("Received auto mission request: {}, value:\n{}",
+                 request->request, request->value);
+
+        std::string cmd_value = request->request;
+        int cmd = -1;
+        try {
+            cmd = std::stoi(cmd_value);
+        } catch (...) {
+            response->success = false;
+            response->message = "Invalid command value";
+            LOG_WARN("auto mission cb, Invalid command value:{}", request->request);
+            return;
+        }
+
+        if (cmd == 3) {
+            if (!is_idle()) {
+                response->success = false;
+                response->message = "Cannot accept new mission in current state";
+                LOG_WARN("auto mission cb, cmd:{}, Cannot accept new mission in current state", cmd);
+                return;
+            }
+
+            std::string mission_content = request->value;
+            if (mission_content.empty()) {
+                response->success = false;
+                response->message = "Mission content is empty";
+                LOG_WARN("auto mission cb, cmd:{}, Mission content is empty", cmd);
+                return;
+            }
+
+            json mission_json;
+            try {
+                mission_json = json::parse(mission_content);
+            } catch (const json::parse_error& e) {
+                response->success = false;
+                response->message = "Failed to parse JSON: " + std::string(e.what());
+                LOG_WARN("auto mission cb, cmd:{}, Failed to parse JSON:{}", cmd, std::string(e.what()));
+                return;
+            }
+
+            if (!mission_json.is_array()) {
+                response->success = false;
+                response->message = "Mission data must be a JSON array";
+                LOG_WARN("auto mission cb, cmd:{}, Mission data must be a JSON array", cmd);
+                return;
+            }
+
+            if (!save_mission_content(mission_content)) {
+                response->success = false;
+                response->message = "Failed to save mission content to Redis";
+                LOG_WARN("auto mission cb, cmd:{}, Failed to save mission content to Redis", cmd);
+                return;
+            }
+
+            executed_commands_ = json::object();
+            mission_json_ = mission_json;
+            current_mission_index_ = 0;
+            LOG_INFO("mission state change(cmd-3):{} -> {}", missionStateToString(mission_state_), "RUNNING");
+            mission_state_ = MissionState::RUNNING;
+
+            if (!save_current_mission_index()) {
+                LOG_WARN("auto mission cb, cmd:{}, Failed to save mission index to Redis", cmd);
+            }
+
+            reportEvent("info", "MISSION_START", "Mission started");
+            response->success = true;
+            response->message = "Mission accepted";
+            LOG_INFO("auto mission cb, cmd:{}, Mission accepted, {} items", cmd, mission_json_.size());
+
+        } else if (cmd == 1) {
+            if (!is_running()) {
+                response->success = false;
+                response->message = "Cannot pause, mission is not running";
+                LOG_WARN("auto mission cb, cmd:{}, Cannot pause, mission is not running", cmd);
+                return;
+            }
+
+            LOG_INFO("mission state change(cmd-1):{} -> {}", missionStateToString(mission_state_), "PAUSE");
+            mission_state_ = MissionState::PAUSE;
+            reportEvent("warn", "MISSION_PAUSE", "任务暂停");
+            response->success = true;
+            response->message = "Mission paused";
+            LOG_INFO("auto mission cb, cmd:{}, Mission paused at index: {}", cmd, current_mission_index_);
+
+        } else if (cmd == 0) {
+            if (!is_running() && !is_pause()) {
+                response->success = false;
+                response->message = "Cannot stop, mission is not running or paused";
+                LOG_WARN("auto mission cb, cmd:{}, Cannot stop, mission is not running or paused", cmd);
+                return;
+            }
+
+            resetMissionState();
+            reportEvent("info", "MISSION_STOP", "Mission stopped");
+            response->success = true;
+            response->message = "Mission stopped";
+            LOG_INFO("auto mission cb, cmd:{}, Mission stopped", cmd);
+
+        } else if (cmd == 2) {
+            if (!is_pause()) {
+                response->success = false;
+                response->message = "Cannot resume, mission is not paused";
+                LOG_WARN("auto mission cb, cmd:{}, Cannot resume, mission is not paused", cmd);
+                return;
+            }
+
+            if (!load_executed_commands()) {
+                response->success = false;
+                response->message = "Failed to load executed commands from Redis";
+                LOG_WARN("auto mission cb, cmd:{}, Failed to load executed commands from Redis", cmd);
+                return;
+            }
+
+            continue_command_queue_.clear();
+            for (auto& item : executed_commands_.items()) {
+                continue_command_queue_.push_back({item.key(), item.value()});
+            }
+            continue_command_index_ = 0;
+
+            LOG_INFO("mission state change(cmd-2):{} -> {}", missionStateToString(mission_state_), "CONTINUE");
+            mission_state_ = MissionState::CONTINUE;
+
+            response->success = true;
+            response->message = "Mission resuming from index: " + std::to_string(current_mission_index_);
+            LOG_INFO("auto mission cb, cmd:{}, Mission resuming, {} commands to replay", cmd, continue_command_queue_.size());
+
+        } else {
+            response->success = false;
+            response->message = "Invalid command. Use: 0=stop, 1=pause, 2=resume, 3=new task";
+            LOG_WARN("auto mission cb, cmd:{}, Invalid command. Use: 0=stop, 1=pause, 2=resume, 3=new task", cmd);
+        }
+
+    } catch (const std::exception& e) {
+        response->success = false;
+        response->message = "Exception occurred: " + std::string(e.what());
+        LOG_ERROR("auto mission cb, cmd:{}, {}", request->request, response->message);
+    }
+}
+
+bool AutoMissionHandler::execute_function(const std::string& function_id, const json& inputs)
+{
+    auto client = get_cached_client(function_id);
+    if (!client) {
+        LOG_ERROR("No client available for function: {}", function_id);
+        return false;
+    }
+
+    auto request = std::make_shared<auto_drive_proj_msg::srv::ReqString::Request>();
+    request->request = inputs.dump();
+
+    auto result = client->async_send_request(request);
+
+    if (result.wait_for(std::chrono::seconds(5)) == std::future_status::ready) {
+        try {
+            auto response = result.get();
+            if (response->success) {
+                executed_commands_[function_id] = inputs;
+                save_executed_commands();
+                LOG_INFO("Function {} executed successfully", function_id);
+                return true;
+            } else {
+                LOG_WARN("Function {} execution failed: {}", function_id, response->message);
+                return false;
+            }
+        } catch (const std::exception& e) {
+            LOG_ERROR("Exception executing function {}: {}", function_id, e.what());
+            return false;
+        }
+    } else {
+        LOG_WARN("Function {} execution timeout", function_id);
+        return false;
+    }
+}
+
+rclcpp::Client<auto_drive_proj_msg::srv::ReqString>::SharedPtr AutoMissionHandler::get_cached_client(
+    const std::string& function_id)
+{
+    auto it = cached_clients_.find(function_id);
+    if (it != cached_clients_.end()) {
+        return it->second;
+    }
+
+    LOG_INFO("auto mission handler create new function: {}", function_id);
+    auto client = this->create_client<auto_drive_proj_msg::srv::ReqString>(function_id,
+                                                                            rmw_qos_profile_services_default,
+                                                                            service_cached_client_cb_group_
+                                                                        );
+    cached_clients_[function_id] = client;
+    return client;
+}
+
+void AutoMissionHandler::resetMissionState()
+{
+    LOG_INFO("reset mission state:{} -> {}", missionStateToString(mission_state_), "IDLE");
+    mission_state_ = MissionState::IDLE;
+    mission_json_.clear();
+    current_mission_index_ = 0;
+    executed_commands_.clear();
+    continue_command_queue_.clear();
+    continue_command_index_ = 0;
+}
+
+bool AutoMissionHandler::save_to_redis(const std::string& key, const std::string& value) {
+    RedisUtils redis;
+    if (!redis.connect(redis_host_, redis_port_, redis_db_)) {
+        LOG_ERROR("Failed to connect to Redis save ro redis {}:{}:{}", redis_host_, redis_port_, redis_db_);
+        return false;
+    }
+    return redis.set(key, value);
+}
+
+std::string AutoMissionHandler::load_from_redis(const std::string& key) {
+    RedisUtils redis;
+    if (!redis.connect(redis_host_, redis_port_, redis_db_)) {
+        LOG_ERROR("Failed to connect to Redis load from redis {}:{}:{}", redis_host_, redis_port_, redis_db_);
+        return "";
+    }
+    return redis.get(key);
+}
+
+bool AutoMissionHandler::save_executed_commands() {
+    return save_to_redis("auto_mission_cmd", executed_commands_.dump());
+}
+
+bool AutoMissionHandler::save_current_mission_index() {
+    return save_to_redis("current_mission_index_", std::to_string(current_mission_index_));
+}
+
+bool AutoMissionHandler::save_mission_content(const std::string& content) {
+    return save_to_redis("auto_mission_content", content);
+}
+
+bool AutoMissionHandler::load_executed_commands() {
+    std::string commands_str = load_from_redis("auto_mission_cmd");
+    if (commands_str.empty()) {
+        LOG_INFO("load executed commands from redis is empty");
+        executed_commands_ = json::object();
+        return true;
+    }
+    try {
+        executed_commands_ = json::parse(commands_str);
+        return true;
+    } catch (const json::parse_error& e) {
+        LOG_ERROR("Failed to parse executed commands: {}, redis load cmd:{}", e.what(), commands_str);
+        executed_commands_ = json::object();
+        return false;
+    }
+}
+
+bool AutoMissionHandler::load_current_mission_index() {
+    std::string index_str = load_from_redis("current_mission_index_");
+    if (index_str.empty()) {
+        LOG_INFO("load current mission index from redis is empty");
+        current_mission_index_ = 0;
+        return false;
+    }
+    try {
+        current_mission_index_ = std::stoul(index_str);
+        return true;
+    } catch (const std::exception& e) {
+        LOG_ERROR("Failed to parse mission index: {}, redis load current mission index:{}", e.what(), index_str);
+        current_mission_index_ = 0;
+        return false;
+    }
+}
+
+std::string AutoMissionHandler::load_mission_content() {
+    return load_from_redis("auto_mission_content");
+}
+
+} // namespace auto_drive_misson_center
+
+int main(int argc, char** argv) {
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<auto_drive_misson_center::AutoMissionHandler>();
+
+    rclcpp::executors::MultiThreadedExecutor executor;
+    executor.add_node(node);
+    executor.spin();
+
+    rclcpp::shutdown();
+    return 0;
+}

auto_drive_misson_center/src/logger.cpp

@@ -0,0 +1,34 @@
+#include "auto_drive_misson_center/logger.hpp"
+
+namespace auto_drive_misson_center {
+
+std::shared_ptr<spdlog::logger> logger;
+
+void initLogger(const std::string logger_logger_name, 
+                const std::string logger_filename, 
+                const std::string logger_node_name,
+                size_t logger_max_size, 
+                size_t logger_max_files, 
+                const int logger_set_level)
+{
+    size_t max_size = logger_max_size;
+    size_t max_files = logger_max_files;
+
+    logger = spdlog::rotating_logger_mt(logger_logger_name, logger_filename, max_size, max_files);
+    // # trace = 0, debug = 1, info = 2, warn = 3, err = 4, critical = 5, off = 6,
+    auto lvl = static_cast<spdlog::level::level_enum>(logger_set_level);
+    auto val = static_cast<int>(lvl);
+    if(val < 0 || val > 6)
+    {
+        lvl = spdlog::level::info;
+    }
+    logger->set_level(lvl);
+    logger->flush_on(lvl);
+    logger->set_pattern("[%Y-%m-%d %H:%M:%S.%e] [%l] [%n] [%v]");
+
+    logger->info("**************************************************");
+    logger->info("*    MISSION CENTER:{} LOG STARTED  *", logger_node_name);
+    logger->info("**************************************************");
+}
+
+}

auto_drive_misson_center/src/mission_center_node.cpp

@@ -0,0 +1,970 @@
+#include "auto_drive_misson_center/mission_center_node.hpp"
+#include "auto_drive_misson_center/redis_utils.hpp"
+#include "auto_drive_misson_center/logger.hpp"
+#include <sstream>
+#include <iostream>
+#include <thread>
+#include <chrono>
+#include <future>
+
+namespace auto_drive_misson_center {
+
+static size_t WriteCallback(void* contents, size_t size, size_t nmemb, void* userp)
+{
+    ((std::string*)userp)->append((char*)contents, size * nmemb);
+    return size * nmemb;
+}
+
+MissionCenterNode::MissionCenterNode(const std::string & node_name)
+    : Node(node_name)
+{
+    get_params();
+
+    initLogger(logger_logger_name_, 
+                logger_filename_,
+                logger_node_name_,
+                static_cast<size_t>(logger_max_size_),
+                static_cast<size_t>(logger_max_files_),
+                logger_set_level_);
+
+    curl_global_init(CURL_GLOBAL_DEFAULT);
+
+    // 创建回调组
+    service_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+    client_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+    publisher_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+    rclcpp::PublisherOptions publisher_options;
+    publisher_options.callback_group = publisher_cb_group_;
+
+    auto_service_ = this->create_service<auto_drive_proj_msg::srv::ReqString>(
+        "/invoke/auto",
+        std::bind(&MissionCenterNode::autoCallback, this,
+                 std::placeholders::_1, std::placeholders::_2)/*,
+                 rmw_qos_profile_services_default,
+                 service_cb_group_*/);
+
+    mission_on_service_ = this->create_service<auto_drive_proj_msg::srv::ReqString>(
+        "/invoke/missionOn",
+        std::bind(&MissionCenterNode::missionOnCallback, this,
+                 std::placeholders::_1, std::placeholders::_2)/*,
+                 rmw_qos_profile_services_default,
+                 service_cb_group_*/);
+
+    stop_mission_service_ = this->create_service<std_srvs::srv::SetBool>(
+        "/center/stop_mission",
+        std::bind(&MissionCenterNode::stopMissionCallback, this,
+                 std::placeholders::_1, std::placeholders::_2)/*,
+                 rmw_qos_profile_services_default,
+                 service_cb_group_*/);
+
+    stop_path_service_ = this->create_service<std_srvs::srv::SetBool>(
+        "/center/stop_path",
+        std::bind(&MissionCenterNode::stopPathCallback, this,
+                 std::placeholders::_1, std::placeholders::_2)/*,
+                 rmw_qos_profile_services_default,
+                 service_cb_group_*/);
+
+    auto_mission_client_ = this->create_client<auto_drive_proj_msg::srv::ReqString>("/auto_mission"/*, 
+                                                                rmw_qos_profile_services_default,
+                                                                client_cb_group_*/);
+    auto_follow_client_ = this->create_client<auto_drive_proj_msg::srv::ReqString>("/auto_follow"/*, 
+                                                                rmw_qos_profile_services_default,
+                                                                client_cb_group_*/);
+
+    event_info_pub_ = this->create_publisher<std_msgs::msg::String>("/event/info", 10/*, publisher_options*/);
+    event_warn_pub_ = this->create_publisher<std_msgs::msg::String>("/event/warn", 10/*, publisher_options*/);
+    event_error_pub_ = this->create_publisher<std_msgs::msg::String>("/event/error", 10/*, publisher_options*/);
+    properties_report_pub_ = this->create_publisher<std_msgs::msg::String>(
+        "/properties/report",
+        10
+    );
+
+    last_main_state_report_time_ = this->now();
+
+    // main_state_ = MainState::INIT;
+    changeMainState(MainState::INIT);
+    LOG_INFO("Mission Center Node started, state: INIT");
+
+    MainState loaded_state = loadStateFromRedis();
+    if (loaded_state == MainState::PAUSE || loaded_state == MainState::AUTO) {
+        // main_state_ = MainState::PAUSE_MIS;
+        changeMainState(MainState::PAUSE_MIS);
+        LOG_INFO("Restored state PAUSE/AUTO, transitioning to PAUSE_MIS");
+    } else {
+        // main_state_ = MainState::STOP_MIS;
+        changeMainState(MainState::STOP_MIS);
+        LOG_INFO("Restored state STOP/IDLE, transitioning to STOP_MIS");
+    }
+}
+
+void MissionCenterNode::get_params()
+{
+    // LOG_DEBUG("Mission Center Node get params");
+
+    // redis
+    this->declare_parameter("redis_host", "127.0.0.1");
+    redis_host_ = this->get_parameter("redis_host").get_parameter_value().get<std::string>();
+
+    this->declare_parameter("redis_port", 6379);
+    redis_port_ = this->get_parameter("redis_port").get_parameter_value().get<int>();
+
+    this->declare_parameter("redis_db", 0);
+    redis_db_ = this->get_parameter("redis_db").get_parameter_value().get<int>();
+
+    this->declare_parameter("device_id", "2072868304711761920");
+    device_id_ = this->get_parameter("device_id").get_parameter_value().get<std::string>();
+
+    // logger
+    this->declare_parameter("logger_logger_name", "center_node");
+    logger_logger_name_ = this->get_parameter("logger_logger_name").get_parameter_value().get<std::string>();
+
+    this->declare_parameter("logger_filename", "logs/mission_center_logger.txt");
+    logger_filename_ = this->get_parameter("logger_filename").get_parameter_value().get<std::string>();
+
+    this->declare_parameter("logger_node_name", "center_node");
+    logger_node_name_ = this->get_parameter("logger_node_name").get_parameter_value().get<std::string>();
+
+    this->declare_parameter("logger_max_size", 10485760);
+    logger_max_size_ = this->get_parameter("logger_max_size").get_parameter_value().get<int>();
+
+    this->declare_parameter("logger_max_files", 5);
+    logger_max_files_ = this->get_parameter("logger_max_files").get_parameter_value().get<int>();
+
+    this->declare_parameter("logger_set_level", 1);
+    logger_set_level_ = this->get_parameter("logger_set_level").get_parameter_value().get<int>();
+}
+
+void MissionCenterNode::run()
+{
+    LOG_INFO("Starting Mission Center Node main loop");
+
+    while (rclcpp::ok()) {
+        rclcpp::spin_some(this->shared_from_this());
+
+        handleAsyncResponses();
+        publishPropertyReport();
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+
+    LOG_WARN("Mission Center Node stopped");
+}
+
+void MissionCenterNode::reportEvent(const std::string& level, const std::string& event_name, const std::string& content)
+{
+    json data_obj;
+    data_obj["eventName"] = event_name;
+    data_obj["content"] = content;
+    json root;
+    root["data"] = data_obj;
+
+    std_msgs::msg::String event_msg;
+    event_msg.data = root.dump();
+
+    if (level == "info") {
+        event_info_pub_->publish(event_msg);
+    } else if (level == "warn") {
+        event_warn_pub_->publish(event_msg);
+    } else if (level == "error") {
+        event_error_pub_->publish(event_msg);
+    }
+}
+
+void MissionCenterNode::publishPropertyReport(bool reportImmediately)
+{
+    if (!reportImmediately) {
+        if ((this->now() - last_main_state_report_time_).seconds() < LAST_MAIN_STATE_REPORT_TIMEOUT_SECONDS) {
+            return;
+        }
+    }
+
+    if (!properties_report_pub_) return;
+
+    int mission_status_value = -1;
+    int control_mode_value = 0;
+    switch (main_state_) {
+        case MainState::IDLE:
+            mission_status_value = 0;
+            break;
+        case MainState::INIT:
+            mission_status_value = 1;
+            break;
+        case MainState::AUTO:
+            mission_status_value = 2;
+            control_mode_value = 1;
+            break;
+        case MainState::PAUSE_MIS:
+        case MainState::PAUSE_PATH:
+            mission_status_value = 3;
+            break;
+        default:
+            mission_status_value = -1;
+            break;
+    }
+    if (-1 == mission_status_value) {
+        return;
+    }
+
+    std_msgs::msg::String msg;
+    json j;
+    j["messageType"] = "PROPERTY_REPORT";
+    j["deviceId"] = device_id_;
+    j["timestamp"] = rclcpp::Clock().now().seconds() * 1000.0;  // ms
+
+    json props;
+    props["mission_status"] = mission_status_value;
+    props["control_mode"] = control_mode_value;
+    j["properties"] = props;
+
+    msg.data = j.dump();
+    properties_report_pub_->publish(msg);
+    last_main_state_report_time_ = this->now();
+
+    LOG_DEBUG("Properties report published: {}", j.dump());
+}
+
+void MissionCenterNode::changeMainState(MainState state)
+{
+    main_state_ = state;
+    publishPropertyReport(true);
+}
+
+void MissionCenterNode::autoCallback(
+    const auto_drive_proj_msg::srv::ReqString::Request::SharedPtr request,
+    auto_drive_proj_msg::srv::ReqString::Response::SharedPtr response)
+{
+    try {
+        LOG_INFO("Received /invoke/auto request : {}", request->request);
+
+        json request_json = json::parse(request->request);
+        std::string str_cmd;
+
+        if (request_json.is_array()) {
+            for (const auto& item : request_json) {
+                if (item.contains("name") && item["name"] == "value" && item.contains("value")) {
+                    str_cmd = item["value"].get<std::string>();
+                    break;
+                }
+            }
+        }
+
+        std::string cmd_value = str_cmd; //request->request;
+        int cmd = -1;
+        try {
+            cmd = std::stoi(cmd_value);
+        } catch (...) {
+            response->success = false;
+            response->message = "Invalid command value";
+            LOG_WARN("auto cb, Invalid command value:{}", request->request);
+            return;
+        }
+        LOG_DEBUG("auto cb, str_cmd: {}, cmd: {}, main state: {}", str_cmd, cmd, mainStateToString(main_state_));
+
+        if (isAuto()) {
+            if (cmd == 1) {
+                LOG_INFO("auto cb Command auto: pause, switching to PAUSE_MIS");
+                // main_state_ = MainState::PAUSE_MIS;
+                changeMainState(MainState::PAUSE_MIS);
+                response->success = true;
+                response->message = "Pause command accepted";
+            } else if (cmd == 0) {
+                LOG_INFO("auto cb Command auto: stop, switching to STOP_MIS");
+                // main_state_ = MainState::STOP_MIS;
+                changeMainState(MainState::STOP_MIS);
+                response->success = true;
+                response->message = "Stop command accepted";
+            } else {
+                response->success = false;
+                response->message = "Command not allowed in AUTO state";
+                LOG_WARN("auto cb Command auto: Command not allowed in AUTO state");
+            }
+        } else if (isPause()) {
+            if (cmd == 2) {
+                LOG_INFO("auto cb Command pause: resume, reloading from Redis and sending continue");
+
+                RedisUtils redis;
+                if (!redis.connect(redis_host_, redis_port_, redis_db_)) {
+                    response->success = false;
+                    response->message = "Failed to connect to Redis";
+                    LOG_ERROR("auto cb Command pause:Failed to connect to Redis misson auto cb {}:{}:{}", redis_host_, redis_port_, redis_db_);
+                    return;
+                }
+
+                std::string mission_data = redis.get("mission_content");
+                std::string follow_data = redis.get("follow_content");
+
+                if (mission_data.empty() || follow_data.empty()) {
+                    response->success = false;
+                    response->message = "No mission data found in Redis";
+                    LOG_WARN("auto cb Command pause:: No mission data found in Redis");
+                    return;
+                }
+
+                auto mission_request = std::make_shared<auto_drive_proj_msg::srv::ReqString::Request>();
+                mission_request->request = "2";
+                mission_request->value = mission_data;
+                mission_future_ = auto_mission_client_->async_send_request(mission_request);
+
+                auto follow_request = std::make_shared<auto_drive_proj_msg::srv::ReqString::Request>();
+                follow_request->request = "2";
+                follow_request->value = follow_data;
+                follow_future_ = auto_follow_client_->async_send_request(follow_request);
+
+                has_pending_request_.store(true);
+                request_start_time_ = this->now();
+                LOG_INFO("main state change(auto cb:cmd-2):{} -> {}", mainStateToString(main_state_), "WAIT_MIS_RSP");
+                // main_state_ = MainState::WAIT_MIS_RSP;
+                changeMainState(MainState::WAIT_MIS_RSP);
+
+                response->success = true;
+                response->message = "Resume command accepted";
+                LOG_DEBUG("auto cb Command pause: Resume command accepted");
+            } else if (cmd == 0) {
+                LOG_INFO("auto cb Command pause: stop, switching to STOP_MIS");
+                LOG_INFO("main state change(auto cb:cmd-0):{} -> {}", mainStateToString(main_state_), "STOP_MIS");
+                // main_state_ = MainState::STOP_MIS;
+                changeMainState(MainState::STOP_MIS);
+                response->success = true;
+                response->message = "Stop command accepted";
+                LOG_DEBUG("auto cb Command pause: Stop command accepted");
+            } else {
+                response->success = false;
+                response->message = "Command not allowed in PAUSE state";
+                LOG_WARN("auto cb Command pause: Command not allowed in PAUSE state");
+            }
+        } else {
+            response->success = false;
+            response->message = "Command not allowed in current state";
+            LOG_WARN("auto cb Command other: Command not allowed in current state : {}", static_cast<int>(main_state_));
+        }
+
+    } catch (const std::exception& e) {
+        response->success = false;
+        response->message = std::string("Exception: ") + e.what();
+        LOG_ERROR("auto cb Command execption:{}", response->message);
+    }
+}
+
+void MissionCenterNode::missionOnCallback(
+    const auto_drive_proj_msg::srv::ReqString::Request::SharedPtr request,
+    auto_drive_proj_msg::srv::ReqString::Response::SharedPtr response)
+{
+    try {
+        LOG_INFO("Received /invoke/missionOn request: {}", request->request);
+
+        if (!isIdle()) {
+            response->success = false;
+            response->message = "Cannot accept new mission in current state";
+            LOG_WARN("mission on cb, current not idle state: Cannot accept new mission in current state:{}", mainStateToString(main_state_));
+            return;
+        }
+
+        json request_json = json::parse(request->request);
+        std::string download_url;
+
+        if (request_json.is_array()) {
+            for (const auto& item : request_json) {
+                if (item.contains("name") && item["name"] == "value" && item.contains("value")) {
+                    download_url = item["value"].get<std::string>();
+                    break;
+                }
+            }
+        }
+
+        if (download_url.empty()) {
+            response->success = false;
+            response->message = "Invalid request format";
+            LOG_WARN("mission on cb, Invalid request format");
+            return;
+        }
+
+        std::string downloaded_data = downloadFile(download_url);
+        if (downloaded_data.empty()) {
+            response->success = false;
+            response->message = "Failed to download file";
+            LOG_WARN("mission on cb, Failed to download file");
+            return;
+        }
+        LOG_INFO("Downloading from URL:{}, \ndown load data:{}", download_url, downloaded_data);
+
+        std::string mission_data;
+        std::string follow_data;
+
+        if (!processDownloadedData(downloaded_data, mission_data, follow_data)) {
+            response->success = false;
+            response->message = "Failed to parse downloaded data";
+            LOG_WARN("mission on cb, Failed to parse downloaded data");
+            return;
+        }
+
+        RedisUtils redis;
+        if (!redis.connect(redis_host_, redis_port_, redis_db_)) {
+            response->success = false;
+            response->message = "Failed to connect to Redis";
+            LOG_ERROR("Failed to connect to Redis misson on cb {}:{}:{}", redis_host_, redis_port_, redis_db_);
+            return;
+        }
+
+        redis.set("mission_content", mission_data);
+        redis.set("follow_content", follow_data);
+
+        auto mission_request = std::make_shared<auto_drive_proj_msg::srv::ReqString::Request>();
+        mission_request->request = "3";
+        mission_request->value = mission_data;
+        mission_future_ = auto_mission_client_->async_send_request(mission_request);
+
+        auto follow_request = std::make_shared<auto_drive_proj_msg::srv::ReqString::Request>();
+        follow_request->request = "3";
+        follow_request->value = follow_data;
+        follow_future_ = auto_follow_client_->async_send_request(follow_request);
+
+        has_pending_request_.store(true);
+        request_start_time_ = this->now();
+        LOG_INFO("main state change(missionOn cb):{} -> {}", mainStateToString(main_state_), "WAIT_MIS_RSP");
+        // main_state_ = MainState::WAIT_MIS_RSP;
+        changeMainState(MainState::WAIT_MIS_RSP);
+
+        response->success = true;
+        response->message = "Mission request accepted, processing in background";
+        LOG_INFO("mission on cb, set main state to WAIT MIS RSP, Mission request accepted, processing in background");
+
+    } catch (const std::exception& e) {
+        response->success = false;
+        response->message = std::string("Exception: ") + e.what();
+        LOG_ERROR("mission on cb, exception:{}", e.what());
+    }
+}
+
+void MissionCenterNode::stopMissionCallback(
+    const std_srvs::srv::SetBool::Request::SharedPtr request,
+    std_srvs::srv::SetBool::Response::SharedPtr response)
+{
+    if (request->data) {
+        mission_finished_ = true;
+        LOG_INFO("stop mission cb, mission_finished set to TRUE");
+    }
+    response->success = true;
+    response->message = "mission_finished updated";
+    LOG_DEBUG("stop mission cb, mission_finished updated");
+}
+
+void MissionCenterNode::stopPathCallback(
+    const std_srvs::srv::SetBool::Request::SharedPtr request,
+    std_srvs::srv::SetBool::Response::SharedPtr response)
+{
+    if (request->data) {
+        path_finished_ = true;
+        LOG_INFO("stop path cb, path_finished set to TRUE");
+    }
+    response->success = true;
+    response->message = "path_finished updated";
+    LOG_DEBUG("stop path cb, path_finished updated");
+}
+
+void MissionCenterNode::handleAsyncResponses()
+{
+    switch (main_state_) {
+        case MainState::PAUSE_MIS:
+            syncSendPauseMission();
+            // main_state_ = MainState::PAUSE_PATH;
+            changeMainState(MainState::PAUSE_PATH);
+            saveState();
+            break;
+
+        case MainState::PAUSE_PATH:
+            syncSendPauseFollow();
+            // main_state_ = MainState::PAUSE;
+            changeMainState(MainState::PAUSE);
+            saveState();
+            reportEvent("warn", "MISSION_PAUSE", "Mission paused");
+            break;
+
+        case MainState::STOP_MIS:
+            syncSendStopMission();
+            // main_state_ = MainState::STOP_PATH;
+            changeMainState(MainState::STOP_PATH);
+            saveState();
+            break;
+
+        case MainState::STOP_PATH:
+            syncSendStopFollow();
+            // main_state_ = MainState::IDLE;
+            changeMainState(MainState::IDLE);
+            saveState();
+            reportEvent("info", "MISSION_STOP", "Mission stopped");
+            break;
+
+        case MainState::WAIT_MIS_RSP:
+            processWaitMisRspState();
+            break;
+
+        case MainState::WAIT_PATH_RSP:
+            processWaitPathRspState();
+            break;
+
+        default:
+            break;
+    }
+}
+
+void MissionCenterNode::processWaitMisRspState()
+{
+    if (!has_pending_request_.load()) {
+        LOG_INFO("main state change(Wait Mis Rsp State:has pending request is false):{} -> {}", mainStateToString(main_state_), "IDLE");
+        // main_state_ = MainState::IDLE;
+        changeMainState(MainState::IDLE);
+        return;
+    }
+
+    if (mission_future_.wait_for(std::chrono::milliseconds(0)) == std::future_status::ready) {
+        auto mission_response = mission_future_.get();
+        if (mission_response->success) {
+            LOG_INFO("Mission request successful");
+            reportEvent("info", "CENTER_MISSION_SEND_OK", "Mission sent successfully");
+            LOG_INFO("main state change(Wait Mis Rsp State:Mission request successful):{} -> {}", mainStateToString(main_state_), "WAIT_PATH_RSP");
+            // main_state_ = MainState::WAIT_PATH_RSP;
+            changeMainState(MainState::WAIT_PATH_RSP);
+        } else {
+            LOG_ERROR("Mission request failed: {}", mission_response->message);
+            reportEvent("error", "CENTER_MISSION_SEND_FAILED", mission_response->message);
+            has_pending_request_.store(false);
+            LOG_INFO("main state change(Wait Mis Rsp State:Mission request failed):{} -> {}", mainStateToString(main_state_), "STOP_MIS");
+            // main_state_ = MainState::STOP_MIS;
+            changeMainState(MainState::STOP_MIS);
+        }
+    } else if ((this->now() - request_start_time_).seconds() > REQUEST_TIMEOUT_SECONDS) {
+        LOG_ERROR("Mission request timeout");
+        reportEvent("error", "CENTER_MISSION_SEND_FAILED", "Mission request timeout");
+        has_pending_request_.store(false);
+        LOG_INFO("main state change(Wait Mis Rsp State:Mission request timeout):{} -> {}", mainStateToString(main_state_), "STOP_MIS");
+        // main_state_ = MainState::STOP_MIS;
+        changeMainState(MainState::STOP_MIS);
+    }
+}
+
+void MissionCenterNode::processWaitPathRspState()
+{
+    LOG_DEBUG("start  Wait Path Rsp State");
+    if (!has_pending_request_.load()) {
+        LOG_INFO("main state change(Wait Path Rsp State:has pending request is false):{} -> {}", mainStateToString(main_state_), "IDLE");
+        // main_state_ = MainState::IDLE;
+        changeMainState(MainState::IDLE);
+        return;
+    }
+
+    if (follow_future_.wait_for(std::chrono::milliseconds(0)) == std::future_status::ready) {
+        auto follow_response = follow_future_.get();
+        if (follow_response->success) {
+            LOG_INFO("Path request successful");
+            reportEvent("info", "CENTER_PATH_SEND_OK", "Path sent successfully");
+            mission_finished_ = false;
+            path_finished_ = false;
+            LOG_INFO("main state change(Wait Path Rsp State:Path sent successfully):{} -> {}", mainStateToString(main_state_), "AUTO");
+            // main_state_ = MainState::AUTO;
+            changeMainState(MainState::AUTO);
+        } else {
+            LOG_ERROR("Path request failed: {}", follow_response->message);
+            reportEvent("error", "CENTER_PATH_SEND_FAILED", follow_response->message);
+            has_pending_request_.store(false);
+            LOG_INFO("main state change(Wait Path Rsp State:Path request failed):{} -> {}", mainStateToString(main_state_), "STOP_MIS");
+            // main_state_ = MainState::STOP_MIS;
+            changeMainState(MainState::STOP_MIS);
+        }
+    } else if ((this->now() - request_start_time_).seconds() > REQUEST_TIMEOUT_SECONDS) {
+        LOG_ERROR("Path request timeout");
+        reportEvent("error", "CENTER_PATH_SEND_FAILED", "Path request timeout");
+        has_pending_request_.store(false);
+        LOG_INFO("main state change(Wait Path Rsp State:Path request timeout):{} -> {}", mainStateToString(main_state_), "STOP_MIS");
+        // main_state_ = MainState::STOP_MIS;
+        changeMainState(MainState::STOP_MIS);
+    }
+}
+
+void MissionCenterNode::syncSendPauseMission()
+{
+    if (!auto_mission_client_->service_is_ready())
+    {
+        LOG_ERROR("auto_mission service not ready");
+        return;
+    }
+
+    auto request = std::make_shared<auto_drive_proj_msg::srv::ReqString::Request>();
+    request->request = "1";
+    auto result = auto_mission_client_->async_send_request(request);
+
+    auto ret = rclcpp::spin_until_future_complete(
+        this->get_node_base_interface(), result, std::chrono::seconds(5));
+    
+    if (ret == rclcpp::FutureReturnCode::SUCCESS)
+    {
+        auto response = result.get();
+        if (response->success){
+            LOG_INFO("Pause mission sent successfully");
+        }
+        else
+        {
+            LOG_ERROR("Pause mission failed: {}", response->message);
+        }   
+    }
+    else if (ret == rclcpp::FutureReturnCode::TIMEOUT)
+    {
+        LOG_ERROR("Pause mission request timeout(5s)");
+    }
+    else if (ret == rclcpp::FutureReturnCode::INTERRUPTED)
+    {
+        LOG_ERROR("Node shutdown interrupted service request");
+    }
+}
+
+void MissionCenterNode::syncSendPauseFollow()
+{
+    if (!auto_follow_client_->service_is_ready())
+    {
+        LOG_ERROR("auto_follow service not ready, skip pause follow request");
+        return;
+    }
+
+    auto request = std::make_shared<auto_drive_proj_msg::srv::ReqString::Request>();
+    request->request = "1";
+    auto result = auto_follow_client_->async_send_request(request);
+
+    auto ret = rclcpp::spin_until_future_complete(
+        this->get_node_base_interface(), result, std::chrono::seconds(5));
+
+    if (ret == rclcpp::FutureReturnCode::SUCCESS)
+    {
+        auto response = result.get();
+        if (response->success)
+        {
+            LOG_INFO("Pause follow sent successfully");
+        }
+        else
+        {
+            LOG_ERROR("Pause follow failed: {}", response->message);
+        }
+    }
+    else if (ret == rclcpp::FutureReturnCode::TIMEOUT)
+    {
+        LOG_ERROR("Pause follow request timeout(5s)");
+    }
+    else if (ret == rclcpp::FutureReturnCode::INTERRUPTED)
+    {
+        LOG_ERROR("Pause follow request interrupted, node is shutting down");
+    }
+}
+
+void MissionCenterNode::syncSendStopMission()
+{
+    if (!auto_mission_client_->service_is_ready())
+    {
+        LOG_ERROR("auto_mission service not ready, skip stop mission request");
+        return;
+    }
+
+    auto request = std::make_shared<auto_drive_proj_msg::srv::ReqString::Request>();
+    request->request = "0";
+    auto result = auto_mission_client_->async_send_request(request);
+
+    auto ret = rclcpp::spin_until_future_complete(
+        this->get_node_base_interface(), result, std::chrono::seconds(5));
+
+    if (ret == rclcpp::FutureReturnCode::SUCCESS)
+    {
+        auto response = result.get();
+        if (response->success)
+        {
+            LOG_INFO("Stop mission sent successfully");
+        }
+        else
+        {
+            LOG_ERROR("Stop mission failed: {}", response->message);
+        }
+    }
+    else if (ret == rclcpp::FutureReturnCode::TIMEOUT)
+    {
+        LOG_ERROR("Stop mission request timeout(5s)");
+    }
+    else if (ret == rclcpp::FutureReturnCode::INTERRUPTED)
+    {
+        LOG_ERROR("Stop mission request interrupted, node is shutting down");
+    }
+}
+
+void MissionCenterNode::syncSendStopFollow()
+{
+    if (!auto_follow_client_->service_is_ready())
+    {
+        LOG_ERROR("auto_follow service not ready, skip stop follow request");
+        return;
+    }
+
+    auto request = std::make_shared<auto_drive_proj_msg::srv::ReqString::Request>();
+    request->request = "0";
+    auto result = auto_follow_client_->async_send_request(request);
+
+    auto ret = rclcpp::spin_until_future_complete(
+        this->get_node_base_interface(), result, std::chrono::seconds(5));
+
+    if (ret == rclcpp::FutureReturnCode::SUCCESS)
+    {
+        auto response = result.get();
+        if (response->success)
+        {
+            LOG_INFO("Stop follow sent successfully");
+        }
+        else
+        {
+            LOG_ERROR("Stop follow failed: {}", response->message);
+        }
+    }
+    else if (ret == rclcpp::FutureReturnCode::TIMEOUT)
+    {
+        LOG_ERROR("Stop follow request timeout(5s)");
+    }
+    else if (ret == rclcpp::FutureReturnCode::INTERRUPTED)
+    {
+        LOG_ERROR("Stop follow request interrupted, node is shutting down");
+    }
+}
+
+
+std::string MissionCenterNode::downloadFile(const std::string& url)
+{
+    CURL* curl = curl_easy_init();
+    std::string readBuffer;
+    if (curl) {
+        curl_easy_setopt(curl, CURLOPT_URL, url.c_str());
+        curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, WriteCallback);
+        curl_easy_setopt(curl, CURLOPT_WRITEDATA, &readBuffer);
+        curl_easy_setopt(curl, CURLOPT_FOLLOWLOCATION, 1L);
+        curl_easy_setopt(curl, CURLOPT_TIMEOUT, 30L);
+        curl_easy_perform(curl);
+        curl_easy_cleanup(curl);
+    }
+    return readBuffer;
+}
+
+bool MissionCenterNode::processDownloadedData(const std::string& downloaded_data,
+                                            std::string& mission_data, std::string& follow_data)
+{
+    try {
+        json data = json::parse(downloaded_data);
+
+        if (!data.contains("features") || !data["features"].is_array() || data["features"].empty()) {
+            LOG_ERROR("Invalid GeoJSON format");
+            return false;
+        }
+
+        auto& first_feature = data["features"][0];
+        if (!first_feature.contains("geometry") || !first_feature["geometry"].contains("coordinates")) {
+            LOG_ERROR("Feature missing geometry coordinates");
+            return false;
+        }
+
+        std::vector<std::vector<double>> coordinates;
+        std::unique_ptr<GeodeticConverter> geodetic_converter;
+        bool geo_init_flag = false;
+
+        for (const auto& coord : first_feature["geometry"]["coordinates"]) {
+            if (coord.is_array() && coord.size() >= 2) {
+                double east, north, up;
+                if (!geo_init_flag) {
+                    geodetic_converter = std::make_unique<GeodeticConverter>(coord[0], coord[1], 0.0);
+                    data["properties"]["start_lat"] = coord[0].get<double>();
+                    data["properties"]["start_lon"] = coord[1].get<double>();
+                    data["properties"]["start_yaw"] = coord.size() > 2 ? coord[2].get<double>() : 0.0;
+                    geo_init_flag = true;
+                }
+                geodetic_converter->geodetic2Enu(coord[0], coord[1], 0.0, &east, &north, &up);
+                coordinates.push_back({east, north, coord.size() > 2 ? coord[2].get<double>() : 0.0});
+            }
+        }
+
+        if (coordinates.empty()) {
+            LOG_ERROR("No valid coordinates found");
+            return false;
+        }
+
+        interpolation_map_.clear();
+        std::vector<std::vector<double>> interpolated_coords = interpolatePoints(coordinates, 0.5);
+
+        json processed_data = data;
+        json new_coordinates = json::array();
+        for (const auto& coord : interpolated_coords) {
+            new_coordinates.push_back(json::array({coord[0], coord[1], coord[2]}));
+        }
+        processed_data["features"][0]["geometry"]["coordinates"] = new_coordinates;
+
+        if (processed_data["features"][0].contains("properties")) {
+            if (processed_data["features"][0]["properties"].contains("mission") &&
+                processed_data["features"][0]["properties"]["mission"].is_array()) {
+                json& mission_array = processed_data["features"][0]["properties"]["mission"];
+                for (auto& mission_item : mission_array) {
+                    if (mission_item.contains("pathInd")) {
+                        int original_path_ind = mission_item["pathInd"].get<int>();
+                        mission_item["pathInd"] = updatePathIndices(original_path_ind, interpolation_map_);
+                    }
+                }
+            }
+
+            if (processed_data["features"][0]["properties"].contains("turnInds") &&
+                processed_data["features"][0]["properties"]["turnInds"].is_array()) {
+                json& turn_ind_array = processed_data["features"][0]["properties"]["turnInds"];
+                for (auto& turn_ind_item : turn_ind_array) {
+                    int new_turn_ind = updatePathIndices(turn_ind_item.get<int>(), interpolation_map_);
+                    turn_ind_item = new_turn_ind;
+                }
+            }
+        }
+
+        mission_data = processed_data["features"][0]["properties"]["mission"].dump();
+        follow_data = processed_data.dump();
+
+        LOG_INFO("Processed {} coordinates to {} interpolated points",
+                 coordinates.size(), interpolated_coords.size());
+        return true;
+
+    } catch (const std::exception& e) {
+        LOG_ERROR("Error processing downloaded data: {}", e.what());
+        return false;
+    }
+}
+
+std::vector<std::vector<double>> MissionCenterNode::interpolatePoints(
+    const std::vector<std::vector<double>>& coordinates, double max_distance)
+{
+    std::vector<std::vector<double>> result;
+
+    if (coordinates.empty()) {
+        return result;
+    }
+
+    result.push_back(coordinates[0]);
+    int inserted_points = 0;
+
+    for (size_t i = 0; i < coordinates.size() - 1; ++i) {
+        const auto& current = coordinates[i];
+        const auto& next = coordinates[i + 1];
+
+        double dx = next[0] - current[0];
+        double dy = next[1] - current[1];
+        double distance = std::sqrt(dx * dx + dy * dy);
+
+        if (distance > max_distance) {
+            int num_segments = static_cast<int>(std::ceil(distance / max_distance));
+
+            for (int j = 1; j < num_segments; ++j) {
+                double ratio = static_cast<double>(j) / num_segments;
+                double x = current[0] + dx * ratio;
+                double y = current[1] + dy * ratio;
+                result.push_back({x, y, current[2]});
+                inserted_points++;
+            }
+            interpolation_map_.push_back({i + 1, inserted_points});
+        }
+
+        result.push_back(next);
+    }
+
+    interpolation_map_.push_back({coordinates.size() - 1, inserted_points});
+    return result;
+}
+
+int MissionCenterNode::updatePathIndices(
+    const int original_ind, const std::vector<std::pair<int, int>>& interpolation_map)
+{
+    int new_index = original_ind;
+    for (const auto& mapping : interpolation_map) {
+        if (mapping.first >= original_ind) {
+            new_index += mapping.second;
+            break;
+        }
+    }
+    return new_index;
+}
+
+bool MissionCenterNode::saveState()
+{
+    try {
+        RedisUtils redis;
+        if (!redis.connect(redis_host_, redis_port_, redis_db_)) {
+            LOG_ERROR("Failed to connect to Redis for saving state {}:{}:{}", redis_host_, redis_port_, redis_db_);
+            reportEvent("warn", "CENTER_REDIS_STATE_SAVE_FAILED", "Failed to connect to Redis");
+            return false;
+        }
+
+        std::string state_str;
+        switch (main_state_) {
+            case MainState::INIT: state_str = "INIT"; break;
+            case MainState::PAUSE_MIS: state_str = "PAUSE_MIS"; break;
+            case MainState::PAUSE_PATH: state_str = "PAUSE_PATH"; break;
+            case MainState::PAUSE: state_str = "PAUSE"; break;
+            case MainState::STOP_MIS: state_str = "STOP_MIS"; break;
+            case MainState::STOP_PATH: state_str = "STOP_PATH"; break;
+            case MainState::IDLE: state_str = "IDLE"; break;
+            case MainState::WAIT_MIS_RSP: state_str = "WAIT_MIS_RSP"; break;
+            case MainState::WAIT_PATH_RSP: state_str = "WAIT_PATH_RSP"; break;
+            case MainState::AUTO: state_str = "AUTO"; break;
+        }
+
+        if (redis.set("mission_center_state", state_str)) {
+            LOG_INFO("State saved to Redis: {}", state_str);
+            reportEvent("info", "CENTER_REDIS_STATE_SAVE_OK", "State saved: " + state_str);
+            return true;
+        } else {
+            LOG_ERROR("Failed to save state to Redis");
+            reportEvent("warn", "CENTER_REDIS_STATE_SAVE_FAILED", "Failed to save state");
+            return false;
+        }
+    } catch (const std::exception& e) {
+        LOG_ERROR("Exception in saveState: {}", e.what());
+        return false;
+    }
+}
+
+MainState MissionCenterNode::loadStateFromRedis()
+{
+    try {
+        RedisUtils redis;
+        if (!redis.connect(redis_host_, redis_port_, redis_db_)) {
+            LOG_WARN("Failed to connect to Redis for loading state, defaulting to STOP {}:{}:{}", redis_host_, redis_port_, redis_db_);
+            return MainState::STOP_MIS;
+        }
+
+        std::string state_str = redis.get("mission_center_state");
+        if (state_str.empty()) {
+            LOG_INFO("No state found in Redis, defaulting to STOP");
+            return MainState::STOP_MIS;
+        }
+
+        LOG_INFO("Loaded state from Redis: {}", state_str);
+
+        if (state_str == "PAUSE") {
+            return MainState::PAUSE;
+        } else if (state_str == "AUTO") {
+            return MainState::AUTO;
+        } else if (state_str == "IDLE" || state_str == "STOP_MIS" || state_str == "STOP_PATH" || state_str == "INIT") {
+            return MainState::STOP_MIS;
+        }
+
+        return MainState::STOP_MIS;
+    } catch (const std::exception& e) {
+        LOG_ERROR("Exception in loadStateFromRedis: {}", e.what());
+        return MainState::STOP_MIS;
+    }
+}
+
+} // namespace auto_drive_misson_center
+
+#include <rclcpp/rclcpp.hpp>
+
+int main(int argc, char * argv[])
+{
+    rclcpp::init(argc, argv);
+
+    auto node = std::make_shared<auto_drive_misson_center::MissionCenterNode>();
+    node->run();
+
+    rclcpp::shutdown();
+    return 0;
+}

auto_drive_misson_center/src/path_follow_handler.cpp

@@ -0,0 +1,847 @@
+#include "auto_drive_misson_center/path_follow_handler.hpp"
+#include "auto_drive_misson_center/path_tracking_controller.hpp"
+#include "auto_drive_misson_center/pid_path_tracking_controller.hpp"
+#include "auto_drive_misson_center/path_tracking_controller_adapter.hpp"
+#include "auto_drive_misson_center/task_tracking_controller.hpp"
+#include "auto_drive_misson_center/redis_utils.hpp"
+#include "auto_drive_misson_center/logger.hpp"
+#include <thread>
+#include <chrono>
+#include <cmath>
+#include <limits>
+
+namespace auto_drive_misson_center {
+
+constexpr double PathFollowHandler::POS_LOST_TIMEOUT_SECONDS;
+constexpr int PathFollowHandler::POS_RECOVERY_COUNT;
+constexpr double PathFollowHandler::POS_RECOVERY_TIME_SECONDS;
+constexpr double PathFollowHandler::POS_GAP_THRESHOLD_METERS;
+constexpr int PathFollowHandler::ZERO_VELOCITY_COUNT;
+
+PathFollowHandler::PathFollowHandler(const std::string & node_name)
+    : Node(node_name)
+{
+    get_params();
+
+    initLogger(logger_logger_name_, 
+                logger_filename_,
+                logger_node_name_,
+                static_cast<size_t>(logger_max_size_),
+                static_cast<size_t>(logger_max_files_),
+                logger_set_level_);
+
+    // 创建回调组
+    subscription_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+    publisher_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+    service_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+    client_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+    rclcpp::SubscriptionOptions subscription_options;
+    subscription_options.callback_group = subscription_cb_group_;
+    rclcpp::PublisherOptions publisher_options;
+    publisher_options.callback_group = publisher_cb_group_;
+
+    redis_utils_ = std::make_unique<RedisUtils>();
+    if (!redis_utils_->connect(redis_host_, redis_port_, redis_db_)) {
+        LOG_ERROR("Failed to connect to Redis path {}:{}:{}", redis_host_, redis_port_, redis_db_);
+    } else {
+        LOG_INFO("Redis connected successfully");
+        // 任务完成 / STOP状态进入时调用
+        redis_utils_->del("seg_inner_point_index");
+    }
+
+    pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
+        "/local_pose", 10,
+        std::bind(&PathFollowHandler::poseCallback, this, std::placeholders::_1)/*,
+        subscription_options*/);
+
+    cmd_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/twist_cmd_vel", 10/*, publisher_options*/);
+    path_ind_pub_ = this->create_publisher<std_msgs::msg::Int32>("/auto_path_ind", 10/*, publisher_options*/);
+
+    auto_follow_service_ = this->create_service<auto_drive_proj_msg::srv::ReqString>(
+        "/auto_follow",
+        std::bind(&PathFollowHandler::autoFollowCallback, this,
+                 std::placeholders::_1, std::placeholders::_2)/*,
+                 rmw_qos_profile_services_default,
+                 service_cb_group_*/);
+
+    set_pose_client_ = this->create_client<auto_drive_proj_msg::srv::SetPose>("/start_pose"/*, 
+                                                                rmw_qos_profile_services_default,
+                                                                client_cb_group_*/);
+    stop_path_client_ = this->create_client<std_srvs::srv::SetBool>("/center/stop_path"/*, 
+                                                                rmw_qos_profile_services_default,
+                                                                client_cb_group_*/);
+
+    event_info_pub_ = this->create_publisher<std_msgs::msg::String>("/event/info", 10/*, publisher_options*/);
+    event_warn_pub_ = this->create_publisher<std_msgs::msg::String>("/event/warn", 10/*, publisher_options*/);
+    event_error_pub_ = this->create_publisher<std_msgs::msg::String>("/event/error", 10/*, publisher_options*/);
+    properties_report_pub_ = create_publisher<std_msgs::msg::String>(
+        "/properties/report",
+        10
+    );
+
+    this->declare_parameter<std::string>("controller_type", "pure_pursuit");
+    this->get_parameter("controller_type", controller_type_);
+    LOG_INFO("Controller type parameter: {}", controller_type_);
+
+    last_pose_time_ = this->now();
+
+    LOG_INFO("Path Follow Handler started, initial state: IDLE");
+}
+
+void PathFollowHandler::get_params()
+{
+    // LOG_DEBUG("Path Follow Handler get params");
+
+    // redis
+    this->declare_parameter("redis_host", "127.0.0.1");
+    redis_host_ = this->get_parameter("redis_host").get_parameter_value().get<std::string>();
+
+    this->declare_parameter("redis_port", 6379);
+    redis_port_ = this->get_parameter("redis_port").get_parameter_value().get<int>();
+
+    this->declare_parameter("redis_db", 0);
+    redis_db_ = this->get_parameter("redis_db").get_parameter_value().get<int>();
+
+    this->declare_parameter("device_id", "2072868304711761920");
+    device_id_ = this->get_parameter("device_id").get_parameter_value().get<std::string>();
+
+    // logger
+    this->declare_parameter("logger_logger_name", "path_follow");
+    logger_logger_name_ = this->get_parameter("logger_logger_name").get_parameter_value().get<std::string>();
+
+    this->declare_parameter("logger_filename", "logs/mission_center_logger.txt");
+    logger_filename_ = this->get_parameter("logger_filename").get_parameter_value().get<std::string>();
+
+    this->declare_parameter("logger_node_name", "path_follow");
+    logger_node_name_ = this->get_parameter("logger_node_name").get_parameter_value().get<std::string>();
+
+    this->declare_parameter("logger_max_size", 10485760);
+    logger_max_size_ = this->get_parameter("logger_max_size").get_parameter_value().get<int>();
+
+    this->declare_parameter("logger_max_files", 5);
+    logger_max_files_ = this->get_parameter("logger_max_files").get_parameter_value().get<int>();
+
+    this->declare_parameter("logger_set_level", 1);
+    logger_set_level_ = this->get_parameter("logger_set_level").get_parameter_value().get<int>();
+}
+
+void PathFollowHandler::reportEvent(const std::string& level, const std::string& event_name, const std::string& content)
+{
+    json data_obj;
+    data_obj["eventName"] = event_name;
+    data_obj["content"] = content;
+    json root;
+    root["data"] = data_obj;
+
+    std_msgs::msg::String event_msg;
+    event_msg.data = root.dump();
+
+    if (level == "info") {
+        event_info_pub_->publish(event_msg);
+    } else if (level == "warn") {
+        event_warn_pub_->publish(event_msg);
+    } else if (level == "error") {
+        event_error_pub_->publish(event_msg);
+    }
+}
+
+void PathFollowHandler::publishPropertyReport(const std::map<std::string, double>& properties)
+{
+    if (!properties_report_pub_) return;
+
+    std_msgs::msg::String msg;
+    json j;
+    j["messageType"] = "PROPERTY_REPORT";
+    j["deviceId"] = device_id_;
+    j["timestamp"] = rclcpp::Clock().now().seconds() * 1000.0;  // ms
+
+    json props;
+    for (const auto& [name, value] : properties) {
+        props[name] = value;
+    }
+    j["properties"] = props;
+
+    msg.data = j.dump();
+    properties_report_pub_->publish(msg);
+
+    LOG_TRACE("Properties report published: {}", j.dump());
+}
+
+void PathFollowHandler::poseCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
+{
+    current_pose_ = *msg;
+    pose_initialized_ = true;
+    last_pose_time_ = this->now();
+
+    if (main_state_ == MainState::POS_LOST) {
+        auto now = this->now();
+        double elapsed_since_lost = (now - pos_lost_start_time_).seconds();
+
+        if (elapsed_since_lost <= POS_RECOVERY_TIME_SECONDS) {
+            pos_lost_counter_++;
+            LOG_INFO("Pose received in POS_LOST state, count: {}/{}, elapsed: {:.2f}s",
+                     pos_lost_counter_, PathFollowHandler::POS_RECOVERY_COUNT, elapsed_since_lost);
+
+            if (pos_lost_counter_ >= PathFollowHandler::POS_RECOVERY_COUNT) {
+                LOG_INFO("Pose recovered, switching to RUNNING");
+                pos_lost_counter_ = 0;
+                LOG_INFO("main state change(pose cb):{} -> {}", mainStateToString(main_state_), "RUNNING");
+                main_state_ = MainState::RUNNING;
+                reportEvent("info", "PATH_RECOVERED", "定位恢复");
+            }
+        } else {
+            pos_lost_counter_ = 1;
+            pos_lost_start_time_ = now;
+            LOG_INFO("POS_LOST recovery timeout, resetting counter");
+        }
+    }
+}
+
+void PathFollowHandler::run()
+{
+    LOG_INFO("Starting Path Follow Handler main loop");
+
+    while (rclcpp::ok()) {
+        rclcpp::spin_some(this->shared_from_this());
+
+        processMainLoop();
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+
+    LOG_WARN("Path Follow Handler stopped");
+}
+
+void PathFollowHandler::processMainLoop()
+{
+    if (!pose_initialized_) {
+        return;
+    }
+
+    switch (main_state_) {
+        case MainState::NEW_TASK:
+            processNewTaskState();
+            break;
+        case MainState::READY_SET_START:
+            processReadySetStartState();
+            break;
+        case MainState::READY_TO_RUN:
+            processReadyToRunState();
+            break;
+        case MainState::RUNNING:
+            processRunningState();
+            break;
+        case MainState::POS_LOST:
+            processPosLostState();
+            break;
+        case MainState::PAUSE:
+            processPauseState();
+            break;
+        case MainState::AUTO_OFF:
+            processAutoOffState();
+            break;
+        case MainState::IDLE:
+        default:
+            break;
+    }
+}
+
+void PathFollowHandler::autoFollowCallback(
+    const auto_drive_proj_msg::srv::ReqString::Request::SharedPtr request,
+    auto_drive_proj_msg::srv::ReqString::Response::SharedPtr response)
+{
+    try {
+        LOG_INFO("Received auto follow request:{}, \nvalue:{}", request->request, request->value);
+
+        std::string cmd = request->request;
+        int cmd_val = -1;
+        try {
+            cmd_val = std::stoi(cmd);
+        } catch (const std::exception& e) {
+            LOG_ERROR("Failed to parse command value: {}", e.what());
+            response->success = false;
+            response->message = "Invalid command value";
+            LOG_WARN("auto follow cb, cmd:{}, Invalid command value", cmd);
+            return;
+        }
+
+        if (cmd_val == 3) {
+            // 任务完成 / STOP状态进入时调用
+            redis_utils_->del("seg_inner_point_index");
+            if (!isIdle()) {
+                response->success = false;
+                response->message = "Cannot accept new mission, not in IDLE state";
+                LOG_WARN("auto follow cb, cmd:{}, Cannot accept new mission, not in IDLE state", cmd_val);
+                return;
+            }
+
+            if (!parseMissionJson(request->value)) {
+                response->success = false;
+                response->message = "Failed to parse mission JSON";
+                LOG_WARN("auto follow cb, cmd:{}, Failed to parse mission JSON", cmd_val);
+                return;
+            }
+
+            if (!redis_utils_->set("auto_follow_content_", request->value)) {
+                LOG_WARN("auto follow cb, cmd:{}, Failed to save mission content to Redis", cmd_val);
+            }
+
+            is_resuming_ = false;
+            LOG_INFO("main state change(auto follow cb:cmd-3):{} -> {}", mainStateToString(main_state_), "NEW_TASK");
+            main_state_ = MainState::NEW_TASK;
+
+            response->success = true;
+            response->message = "Mission accepted";
+            LOG_INFO("auto follow cb, cmd:{}, Mission accepted", cmd_val);
+
+        } else if (cmd_val == 2) {
+            if (!isPause()) {
+                response->success = false;
+                response->message = "Cannot resume, not in PAUSE state";
+                LOG_WARN("auto follow cb, cmd:{}, Cannot resume, not in PAUSE state", cmd_val);
+                return;
+            }
+
+            std::string mission_content = redis_utils_->get("auto_follow_content_");
+            if (mission_content.empty()) {
+                response->success = false;
+                response->message = "No mission content found in Redis";
+                LOG_WARN("auto follow cb, cmd:{}, No mission content found in Redis", cmd_val);
+                return;
+            }
+
+            if (!parseMissionJson(mission_content)) {
+                response->success = false;
+                response->message = "Failed to parse mission JSON from Redis";
+                LOG_WARN("auto follow cb, cmd:{}, Failed to parse mission JSON from Redis", cmd_val);
+                return;
+            }
+
+            loadSavedStateFromRedis();
+            is_resuming_ = true;
+            LOG_INFO("main state change(auto follow cb:cmd-2):{} -> {}", mainStateToString(main_state_), "NEW_TASK");
+            main_state_ = MainState::NEW_TASK;
+
+            response->success = true;
+            response->message = "Mission resumed";
+            LOG_INFO("auto follow cb, cmd:{}, Mission resumed from pause", cmd_val);
+
+        } else if (cmd_val == 1) {
+            if (isIdle()) {
+                response->success = false;
+                response->message = "Cannot pause, in IDLE state";
+                LOG_WARN("auto follow cb, is idle, cmd:{}, Cannot pause, in IDLE state", cmd_val);
+                return;
+            }
+
+            if (isPause()) {
+                response->success = false;
+                response->message = "Already in PAUSE state";
+                LOG_WARN("auto follow cb, is pause, cmd:{}, Already in PAUSE state", cmd_val);
+                return;
+            }
+
+            pause_counter_ = 0;
+            LOG_INFO("main state change(auto follow cb:cmd-1):{} -> {}", mainStateToString(main_state_), "PAUSE");
+            main_state_ = MainState::PAUSE;
+
+            response->success = true;
+            response->message = "Pause command accepted";
+            LOG_INFO("auto follow cb, cmd:{}, Pause command accepted", cmd_val);
+
+        } else if (cmd_val == 0) {
+            // 任务完成 / STOP状态进入时调用
+            redis_utils_->del("seg_inner_point_index");
+            if (isIdle()) {
+                response->success = true;
+                response->message = "Already in IDLE state";
+                LOG_WARN("auto follow cb, is idle, cmd:{}, Already in IDLE state", cmd_val);
+                return;
+            }
+
+            pause_counter_ = 0;
+            LOG_INFO("main state change(auto follow cb:cmd-0):{} -> {}", mainStateToString(main_state_), "AUTO_OFF");
+            main_state_ = MainState::AUTO_OFF;
+
+            response->success = true;
+            response->message = "Stop command accepted";
+            LOG_INFO("auto follow cb, cmd:{}, Stop command accepted", cmd_val);
+        } else {
+            response->success = false;
+            response->message = "Invalid command, use 0=stop, 1=pause, 2=resume, 3=new task";
+            LOG_INFO("auto follow cb, cmd:{}, Invalid command, use 0=stop, 1=pause, 2=resume, 3=new task", cmd_val);
+        }
+
+    } catch (const std::exception& e) {
+        response->success = false;
+        response->message = "Exception occurred: " + std::string(e.what());
+        LOG_ERROR("auto follow cb, cmd:{}, Exception occurred: {}", request->request, std::string(e.what()));
+    }
+}
+
+void PathFollowHandler::processRunningState()
+{
+    double time_diff = (this->now() - last_pose_time_).seconds();
+    if (time_diff > PathFollowHandler::POS_LOST_TIMEOUT_SECONDS) {
+        LOG_WARN("Pose timeout, switching to POS_LOST");
+        pos_lost_counter_ = 0;
+        pos_lost_start_time_ = this->now();
+        LOG_INFO("main state change(Running State:POS_LOST_TIMEOUT_SECONDS):{} -> {}", mainStateToString(main_state_), "POS_LOST");
+        main_state_ = MainState::POS_LOST;
+        reportEvent("error", "PATH_LOST", "定位丢失");
+        return;
+    }
+
+    if (path_controller_) {
+        double qx = current_pose_.pose.pose.orientation.x;
+        double qy = current_pose_.pose.pose.orientation.y;
+        double qz = current_pose_.pose.pose.orientation.z;
+        double qw = current_pose_.pose.pose.orientation.w;
+        double yaw = std::atan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz));
+
+        PathPoint current_pose_point(current_pose_.pose.pose.position.x,
+                                    current_pose_.pose.pose.position.y,
+                                    yaw);
+        
+        ControlOutput output = path_controller_->run(current_pose_point);
+
+        auto twist_msg = geometry_msgs::msg::Twist();
+        twist_msg.linear.x = output.linear_velocity;
+        twist_msg.angular.z = output.angular_velocity;
+        cmd_vel_pub_->publish(twist_msg);
+        std::map<std::string, double> property_updates;
+        property_updates["linear_velocity"] = output.linear_velocity;
+        property_updates["angular_velocity"] = output.angular_velocity;
+        publishPropertyReport(property_updates);
+
+        auto path_ind_msg = std_msgs::msg::Int32();
+        path_ind_msg.data = path_controller_->getCurrentPointIndex() + path_ind_start_;
+        path_ind_pub_->publish(path_ind_msg);
+
+        saveCurrentStateToRedis();
+
+        LOG_DEBUG("path controller, current pose point:x:{}, y:{}, yaw:{}; twist_msg pub:linear x:{}, angular z:{}, run is completed:{}, Current Point Index:{}, Path Point Count:{}", 
+                current_pose_.pose.pose.position.x,
+                current_pose_.pose.pose.position.y,
+                yaw,
+                twist_msg.linear.x,
+                twist_msg.angular.z,
+                output.is_completed,
+                path_controller_->getCurrentPointIndex(),
+                path_controller_->getPathPointCount());
+
+        if (output.is_completed || path_controller_->isPathCompleted()) {
+            path_ind_start_ += path_controller_->getPathPointCount();
+            current_segment_index_++;
+
+            if (current_segment_index_ >= static_cast<int>(path_segments_.size())) {
+                LOG_INFO("Mission completed");
+
+                auto final_ind_msg = std_msgs::msg::Int32();
+                int total_points = 0;
+                for (const auto& seg : path_segments_) {
+                    total_points += static_cast<int>(seg.size());
+                }
+                final_ind_msg.data = total_points - 1;
+                path_ind_pub_->publish(final_ind_msg);
+
+                LOG_INFO("main state change(Running State:Mission completed):{} -> {}", mainStateToString(main_state_), "AUTO_OFF");
+                main_state_ = MainState::AUTO_OFF;
+            } else {
+                LOG_INFO("main state change(Running State:Segment completed):{} -> {}", mainStateToString(main_state_), "NEW_TASK");
+                main_state_ = MainState::NEW_TASK;
+                LOG_INFO("Segment completed, moving to next segment");
+            }
+        }
+    }
+}
+
+void PathFollowHandler::processPosLostState()
+{
+    if (pos_lost_counter_ < ZERO_VELOCITY_COUNT) {
+        LOG_INFO("Publishing zero velocity in POS_LOST ({}/{})", pos_lost_counter_ + 1, ZERO_VELOCITY_COUNT);
+        publishZeroVelocity();
+        pos_lost_counter_++;
+    }
+}
+
+void PathFollowHandler::processPauseState()
+{
+    if (pause_counter_ < ZERO_VELOCITY_COUNT) {
+        LOG_INFO("Publishing zero velocity in PAUSE ({}/{})", pause_counter_ + 1, ZERO_VELOCITY_COUNT);
+        publishZeroVelocity();
+        pause_counter_++;
+    } else {
+        LOG_INFO("PAUSE state reached, waiting for continue command");
+    }
+}
+
+void PathFollowHandler::processAutoOffState()
+{
+    if (pause_counter_ < ZERO_VELOCITY_COUNT) {
+        LOG_INFO("Publishing zero velocity in AUTO_OFF ({}/{})", pause_counter_ + 1, ZERO_VELOCITY_COUNT);
+        publishZeroVelocity();
+        pause_counter_++;
+    } else {
+        reportEvent("info", "PATH_TASK_STOP", "Path task stopped");
+        notifyCenterStopPath();
+        resetTaskState();
+    }
+}
+
+void PathFollowHandler::processNewTaskState()
+{
+    LOG_DEBUG("process New Task State");
+    reportEvent("info", "PATH_TASK_START", "Path task started");
+
+    if (!loadPathTrackingController(controller_type_)) {
+        LOG_ERROR("Failed to load path tracking controller");
+        reportEvent("error", "PATH_CONTROLLER_LOAD_FAILED", "控制器加载失败");
+        resetTaskState();
+        return;
+    }
+
+    path_segments_.clear();
+    std::vector<PathPoint> current_segment;
+
+    for (size_t i = 0; i < coordinates_.size(); ++i) {
+        if (coordinates_[i].size() >= 3) {
+            PathPoint point(coordinates_[i][0], coordinates_[i][1], coordinates_[i][2]);
+            current_segment.push_back(point);
+        }
+
+        bool is_turn_point = false;
+        for (int turn_ind : turn_inds_) {
+            if (static_cast<int>(i) == turn_ind) {
+                is_turn_point = true;
+                break;
+            }
+        }
+
+        if (is_turn_point && !current_segment.empty()) {
+            path_segments_.push_back(current_segment);
+            current_segment.clear();
+        }
+    }
+
+    if (!current_segment.empty()) {
+        path_segments_.push_back(current_segment);
+    }
+
+    if (is_resuming_) {
+        LOG_INFO("Resuming from saved segment index: {}", current_segment_index_);
+    } else {
+        current_segment_index_ = findNearestPathSegment(current_pose_.pose.pose);
+        LOG_INFO("Starting from nearest segment: {}", current_segment_index_);
+    }
+
+    path_ind_start_ = 0;
+    for (int i = 0; i < current_segment_index_; ++i) {
+        path_ind_start_ += static_cast<int>(path_segments_[i].size());
+    }
+
+    LOG_INFO("main state change(New Task State):{} -> {}", mainStateToString(main_state_), "READY_SET_START");
+    main_state_ = MainState::READY_SET_START;
+    LOG_INFO("NEW_TASK processed, segment count: {}, current segment: {}",
+             path_segments_.size(), current_segment_index_);
+}
+
+void PathFollowHandler::processReadySetStartState()
+{
+    if (!has_pending_set_pose_) {
+        if (set_pose_client_->service_is_ready()) {
+            auto request = std::make_shared<auto_drive_proj_msg::srv::SetPose::Request>();
+            request->lat = start_lat_;
+            request->lon = start_lon_;
+            request->yaw = start_yaw_;
+            request->alt = 0.0;
+
+            set_pose_future_ = set_pose_client_->async_send_request(request);
+            has_pending_set_pose_ = true;
+            set_pose_start_time_ = this->now();
+
+            LOG_INFO("Calling SetPose service");
+        } else {
+            LOG_WARN("SetPose service is not ready");
+        }
+    } else {
+        auto elapsed = (this->now() - set_pose_start_time_).seconds();
+        if (elapsed > SET_POSE_TIMEOUT_SECONDS) {
+            LOG_ERROR("SetPose service call timed out");
+            has_pending_set_pose_ = false;
+            LOG_INFO("main state change(Ready Set StartState:SetPose call timed out):{} -> {}", mainStateToString(main_state_), "AUTO_OFF");
+            main_state_ = MainState::AUTO_OFF;
+            reportEvent("error", "PATH_SET_POSE_FAILED", "初始位姿设置超时");
+        } else if (set_pose_future_.wait_for(std::chrono::milliseconds(0)) == std::future_status::ready) {
+            try {
+                auto response = set_pose_future_.get();
+                if (response->success) {
+                    LOG_INFO("SetPose succeeded, switching to READY_TO_RUN");
+                    LOG_INFO("main state change(Ready Set StartState:SetPose succeeded):{} -> {}", mainStateToString(main_state_), "READY_TO_RUN");
+                    main_state_ = MainState::READY_TO_RUN;
+                    reportEvent("info", "PATH_SET_POSE_OK", "初始位姿设置成功");
+                } else {
+                    LOG_ERROR("SetPose failed");
+                    LOG_INFO("main state change(Ready Set StartState:SetPose failed):{} -> {}", mainStateToString(main_state_), "AUTO_OFF");
+                    main_state_ = MainState::AUTO_OFF;
+                    reportEvent("error", "PATH_SET_POSE_FAILED", "初始位姿设置失败");
+                }
+                has_pending_set_pose_ = false;
+            } catch (const std::exception& e) {
+                LOG_ERROR("SetPose exception: {}", e.what());
+                has_pending_set_pose_ = false;
+                LOG_INFO("main state change(Ready Set StartState:SetPose exception):{} -> {}", mainStateToString(main_state_), "AUTO_OFF");
+                main_state_ = MainState::AUTO_OFF;
+                reportEvent("error", "PATH_SET_POSE_FAILED", "初始位姿设置异常");
+            }
+        }
+    }
+}
+
+void PathFollowHandler::processReadyToRunState()
+{
+    if (current_segment_index_ >= 0 && current_segment_index_ < static_cast<int>(path_segments_.size())) {
+        if (path_controller_->initialize(mission_speed_, turn_radius_,
+                                       path_segments_[current_segment_index_])) {
+            LOG_INFO("main state change(Ready To RunState:initialized):{} -> {}", mainStateToString(main_state_), "RUNNING");
+            main_state_ = MainState::RUNNING;
+            LOG_INFO("Path controller initialized, starting execution");
+            reportEvent("info", "PATH_CONTROLLER_LOAD_OK", "路径跟踪控制器加载成功");
+
+            {
+                // 从redis读取resume_idx恢复点位下标
+                auto total_points = path_controller_->getPathPointCount();
+                if(resume_inner_point_index_ < 0 || resume_inner_point_index_ >= total_points)
+                {
+                    LOG_WARN("resume index {} out of range[0,{}], reset to 0",
+                        resume_inner_point_index_, total_points);
+                    resume_inner_point_index_ = 0;
+                }
+                path_controller_->setCurrentPointIndex(resume_inner_point_index_);
+            }
+        } else {
+            LOG_ERROR("Failed to initialize path controller");
+            LOG_INFO("main state change(Ready To RunState:PATH_CONTROLLER_LOAD_FAILED):{} -> {}", mainStateToString(main_state_), "AUTO_OFF");
+            main_state_ = MainState::AUTO_OFF;
+            reportEvent("error", "PATH_CONTROLLER_LOAD_FAILED", "路径跟踪控制器初始化失败");
+        }
+    } else {
+        LOG_INFO("main state change(Ready To RunState):{} -> {}", mainStateToString(main_state_), "AUTO_OFF");
+        main_state_ = MainState::AUTO_OFF;
+    }
+}
+
+bool PathFollowHandler::parseMissionJson(const std::string& json_str)
+{
+    try {
+        json mission_data = json::parse(json_str);
+
+        if (mission_data.contains("properties")) {
+            auto& properties = mission_data["properties"];
+            turn_radius_ = properties.value("turnRadius", 1.0);
+            mission_speed_ = properties.value("misionSpeed", 0.0);
+            start_lat_ = properties.value("start_lat", 0.0);
+            start_lon_ = properties.value("start_lon", 0.0);
+            start_yaw_ = properties.value("start_yaw", 0.0);
+        }
+
+        if (mission_data.contains("features") && mission_data["features"].is_array() &&
+            !mission_data["features"].empty()) {
+            auto& feature = mission_data["features"][0];
+
+            if (feature.contains("properties") && feature["properties"].contains("turnInds")) {
+                turn_inds_ = feature["properties"]["turnInds"].get<std::vector<int>>();
+            }
+
+            if (feature.contains("geometry") && feature["geometry"].contains("coordinates")) {
+                coordinates_ = feature["geometry"]["coordinates"].get<std::vector<std::vector<double>>>();
+            }
+        }
+
+        if (coordinates_.empty()) {
+            LOG_ERROR("No coordinates found in mission data");
+            return false;
+        }
+
+        LOG_INFO("Mission JSON parsed: {} coordinates, {} turn indices",
+                 coordinates_.size(), turn_inds_.size());
+        return true;
+
+    } catch (const json::parse_error& e) {
+        LOG_ERROR("JSON parse error: {}", e.what());
+        return false;
+    } catch (const std::exception& e) {
+        LOG_ERROR("Exception in parseMissionJson: {}", e.what());
+        return false;
+    }
+}
+
+bool PathFollowHandler::loadPathTrackingController(const std::string& controller_type)
+{
+    if (controller_type == "lqr") {
+        path_controller_ = std::make_shared<LQRControllerAdapter>(logger);
+        LOG_INFO("LQR path tracking controller loaded");
+        reportEvent("info", "PATH_CONTROLLER_LOAD_OK", "LQR控制器加载成功");
+        return true;
+    } else if (controller_type == "pid") {
+        path_controller_ = std::make_shared<PIDControllerAdapter>(logger);
+        LOG_INFO("PID path tracking controller loaded");
+        reportEvent("info", "PATH_CONTROLLER_LOAD_OK", "PID控制器加载成功");
+        return true;
+    } else if (controller_type == "pure_pursuit") {
+        path_controller_ = std::make_shared<PurePursuitControllerAdapter>(logger);
+        LOG_INFO("Pure Pursuit path tracking controller loaded");
+        reportEvent("info", "PATH_CONTROLLER_LOAD_OK", "Pure Pursuit控制器加载成功");
+        return true;
+    } else {
+        LOG_ERROR("load Path Tracking Controller ERROR");
+    }
+
+    path_controller_ = std::make_shared<PIDPathTrackingController>();
+    LOG_INFO("PID path tracking controller loaded");
+    reportEvent("info", "PATH_CONTROLLER_LOAD_OK", "PID控制器加载成功");
+    return true;
+}
+
+int PathFollowHandler::findNearestPathSegment(const geometry_msgs::msg::Pose& pose)
+{
+    if (path_segments_.empty()) {
+        return 0;
+    }
+
+    int nearest_segment = 0;
+    double min_distance = std::numeric_limits<double>::max();
+
+    for (int i = 0; i < static_cast<int>(path_segments_.size()); ++i) {
+        if (!path_segments_[i].empty()) {
+            const auto& point = path_segments_[i][0];
+            double dx = pose.position.x - point.x;
+            double dy = pose.position.y - point.y;
+            double distance = std::sqrt(dx*dx + dy*dy);
+            
+            if (distance < min_distance) {
+                min_distance = distance;
+                nearest_segment = i;
+            }
+        }
+    }
+
+    return nearest_segment;
+}
+
+double PathFollowHandler::calculateDistance(const geometry_msgs::msg::Pose& pose1, const geometry_msgs::msg::Pose& pose2)
+{
+    double dx = pose1.position.x - pose2.position.x;
+    double dy = pose1.position.y - pose2.position.y;
+    return std::sqrt(dx * dx + dy * dy);
+}
+
+void PathFollowHandler::publishZeroVelocity()
+{
+    auto twist_msg = geometry_msgs::msg::Twist();
+    twist_msg.linear.x = 0.0;
+    twist_msg.angular.z = 0.0;
+    cmd_vel_pub_->publish(twist_msg);
+
+    std::map<std::string, double> property_updates;
+    property_updates["linear_velocity"] = 0.0;
+    property_updates["angular_velocity"] = 0.0;
+    publishPropertyReport(property_updates);
+}
+
+void PathFollowHandler::resetTaskState()
+{
+    main_state_ = MainState::IDLE;
+    turn_radius_ = 1.0;
+    mission_speed_ = 0.0;
+    turn_inds_.clear();
+    coordinates_.clear();
+    path_segments_.clear();
+    current_segment_index_ = -1;
+    path_ind_start_ = 0;
+    path_controller_.reset();
+    pause_counter_ = 0;
+    pos_lost_counter_ = 0;
+    is_resuming_ = false;
+    has_pending_set_pose_ = false;
+
+    LOG_INFO("Task state reset to IDLE");
+}
+
+void PathFollowHandler::notifyCenterStopPath()
+{
+    if (stop_path_client_->service_is_ready()) {
+        auto request = std::make_shared<std_srvs::srv::SetBool::Request>();
+        request->data = true;
+        auto result = stop_path_client_->async_send_request(request);
+        LOG_WARN("Notified center: path stopped");
+        reportEvent("info", "PATH_TASK_COMPLETE", "Path execution completed");
+    }
+}
+
+void PathFollowHandler::saveCurrentStateToRedis()
+{
+    redis_utils_->set("current_segment_index_", std::to_string(current_segment_index_));
+    redis_utils_->set("current_pose_x", std::to_string(current_pose_.pose.pose.position.x));
+    redis_utils_->set("current_pose_y", std::to_string(current_pose_.pose.pose.position.y));
+    redis_utils_->set("path_ind_start_", std::to_string(path_ind_start_));
+
+    int current_idx = path_controller_->getCurrentPointIndex();
+    redis_utils_->set("seg_inner_point_index", std::to_string(current_idx));
+}
+
+void PathFollowHandler::loadSavedStateFromRedis()
+{
+    is_resuming_ = true;
+
+    std::string seg_index_str = redis_utils_->get("current_segment_index_");
+    if (!seg_index_str.empty()) {
+        current_segment_index_ = std::stoi(seg_index_str);
+        LOG_INFO("Loaded saved segment index: {}", current_segment_index_);
+    }
+
+    std::string saved_x_str = redis_utils_->get("current_pose_x");
+    std::string saved_y_str = redis_utils_->get("current_pose_y");
+
+    if (!saved_x_str.empty() && !saved_y_str.empty()) {
+        double saved_x = std::stod(saved_x_str);
+        double saved_y = std::stod(saved_y_str);
+
+        double distance = std::sqrt(std::pow(current_pose_.pose.pose.position.x - saved_x, 2) +
+                                   std::pow(current_pose_.pose.pose.position.y - saved_y, 2));
+
+        if (distance > POS_GAP_THRESHOLD_METERS) {
+            LOG_ERROR("Position gap too large: {} meters, resuming denied", distance);
+            resetTaskState();
+        }
+    }
+
+    // 读取 seg_inner_point_index
+    std::string idx_str = redis_utils_->get("seg_inner_point_index");
+    if(!idx_str.empty()) {
+        try
+        {
+            resume_inner_point_index_ = std::stoi(idx_str);
+            LOG_INFO("resume load seg_inner_point_index = {}", resume_inner_point_index_);
+        } catch(const std::exception& e) {
+            LOG_WARN("parse seg_inner_point_index failed, use default 0, err:{}", e.what());
+            resume_inner_point_index_ = 0;
+        }
+    } else {
+        // redis无该key，回退从头0开始跑
+        resume_inner_point_index_ = 0;
+        LOG_INFO("no seg_inner_point_index in redis, resume from point index 0");
+    }
+}
+
+} // namespace auto_drive_misson_center
+
+#include <rclcpp/rclcpp.hpp>
+
+int main(int argc, char** argv) {
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<auto_drive_misson_center::PathFollowHandler>();
+    node->run();
+    rclcpp::shutdown();
+    return 0;
+}

auto_drive_misson_center/src/redis_utils.cpp

@@ -0,0 +1,123 @@
+#include "auto_drive_misson_center/redis_utils.hpp"
+#include <iostream>
+
+namespace auto_drive_misson_center {
+
+RedisUtils::RedisUtils() = default;
+
+RedisUtils::~RedisUtils() {
+    disconnect();
+}
+
+bool RedisUtils::connect(const std::string& host, int port, int db) {
+    disconnect();
+    
+    context_ = redisConnect(host.c_str(), port);
+    if (context_ == nullptr || context_->err) {
+        if (context_) {
+            std::cerr << "Redis connection error: " << context_->errstr << std::endl;
+            redisFree(context_);
+            context_ = nullptr;
+        } else {
+            std::cerr << "Redis connection error: can't allocate redis context" << std::endl;
+        }
+        return false;
+    }
+    
+    connected_ = true;
+    
+    // 选择数据库
+    if (!select_db(db)) {
+        disconnect();
+        return false;
+    }
+    
+    return true;
+}
+
+void RedisUtils::disconnect() {
+    if (context_) {
+        redisFree(context_);
+        context_ = nullptr;
+    }
+    connected_ = false;
+    current_db_ = 0;
+}
+
+bool RedisUtils::set(const std::string& key, const std::string& value) {
+    if (!connected_ || !context_) return false;
+    
+    redisReply* reply = (redisReply*)redisCommand(context_, "SET %s %s", key.c_str(), value.c_str());
+    if (!reply) return false;
+    
+    bool success = (reply->type != REDIS_REPLY_ERROR);
+    freeReplyObject(reply);
+    return success;
+}
+
+std::string RedisUtils::get(const std::string& key) {
+    if (!connected_ || !context_) return "";
+    
+    redisReply* reply = (redisReply*)redisCommand(context_, "GET %s", key.c_str());
+    if (!reply || reply->type == REDIS_REPLY_ERROR) {
+        if (reply) freeReplyObject(reply);
+        return "";
+    }
+    
+    std::string result;
+    if (reply->type == REDIS_REPLY_STRING) {
+        result = std::string(reply->str, reply->len);
+    }
+    
+    freeReplyObject(reply);
+    return result;
+}
+
+bool RedisUtils::exists(const std::string& key) {
+    if (!connected_ || !context_) return false;
+    
+    redisReply* reply = (redisReply*)redisCommand(context_, "EXISTS %s", key.c_str());
+    if (!reply || reply->type == REDIS_REPLY_ERROR) {
+        if (reply) freeReplyObject(reply);
+        return false;
+    }
+    
+    bool exists = (reply->type == REDIS_REPLY_INTEGER && reply->integer == 1);
+    freeReplyObject(reply);
+    return exists;
+}
+
+bool RedisUtils::del(const std::string& key) {
+    if (!connected_ || !context_) return false;
+    
+    redisReply* reply = (redisReply*)redisCommand(context_, "DEL %s", key.c_str());
+    if (!reply || reply->type == REDIS_REPLY_ERROR) {
+        if (reply) freeReplyObject(reply);
+        return false;
+    }
+    
+    bool success = (reply->type == REDIS_REPLY_INTEGER);
+    freeReplyObject(reply);
+    return success;
+}
+
+bool RedisUtils::select_db(int db) {
+    if (!connected_ || !context_) return false;
+    
+    redisReply* reply = (redisReply*)redisCommand(context_, "SELECT %d", db);
+    if (!reply || reply->type == REDIS_REPLY_ERROR) {
+        if (reply) freeReplyObject(reply);
+        return false;
+    }
+    
+    bool success = (reply->type == REDIS_REPLY_STATUS);
+    freeReplyObject(reply);
+    
+    if (success) {
+        current_db_ = db;
+    }
+    
+    return success;
+}
+
+} // namespace auto_drive_misson_center

auto_drive_proj_msg/CMakeLists.txt

@@ -0,0 +1,58 @@
+cmake_minimum_required(VERSION 3.8)
+project(auto_drive_proj_msg)
+
+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)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+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()
+
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(geometry_msgs REQUIRED)
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/Movebase.msg"
+  "msg/EventMsg.msg"
+  "msg/ObstacleAvoidance.msg"
+  "msg/StatusUpdate.msg"
+  "msg/MqttPubMsg.msg"
+  "srv/GetMultiRegs.srv"
+  "srv/GetReg.srv"
+  "srv/GetRegFloat32.srv"
+  "srv/GetRegInt32.srv"
+  "srv/SetMultiRegs.srv"
+  "srv/SetReg.srv"
+  "srv/SetRegFloat32.srv"
+  "srv/SetRegInt32.srv"
+  "srv/ReadProperties.srv"
+  "srv/ReqString.srv"
+  "srv/ReqAuto.srv"
+  "srv/ReqMission.srv"
+  "srv/VisualData.srv"
+  "srv/SpeedConvertParams.srv"
+  "srv/SetPose.srv"
+  "srv/GetCoil.srv"
+  "srv/GetMultiCoils.srv"
+  "srv/SetCoil.srv"
+  "srv/SetMultiCoils.srv"
+  DEPENDENCIES geometry_msgs
+)
+
+
+ament_export_dependencies(rosidl_default_runtime)
+ament_package()

auto_drive_proj_msg/msg/EventMsg.msg

@@ -0,0 +1,2 @@
+string event_name
+string content

auto_drive_proj_msg/msg/Movebase.msg

@@ -0,0 +1,3 @@
+float32 speed
+float32 angle
+bool finetuning

auto_drive_proj_msg/msg/MqttPubMsg.msg

@@ -0,0 +1,2 @@
+string topic
+string json_msg

auto_drive_proj_msg/msg/ObstacleAvoidance.msg

@@ -0,0 +1,2 @@
+#0:减速 1:停止 2:正常
+int32 speed_command

auto_drive_proj_msg/msg/StatusUpdate.msg

@@ -0,0 +1,50 @@
+int32 rtk_mode
+float32 battery_u
+float64 battery_i
+float64 battery_power
+float32 battery_capacity
+string battery_status
+float32 battery_single_max_vol
+float32 battery_single_min_vol
+float32 battery_max_tempreture
+float32 battery_min_tempreture
+float32 battery_mos_tempreture
+float32 r_motor_u
+float32 r_motor_i
+string r_status
+int32 r_motor_rpm
+float32 l_motor_u
+float32 l_motor_i
+string l_status
+int32 l_motor_rpm
+int32 lift_status
+bool rotavator_status
+int32 auto_status
+int32 max_auto_speed 
+bool slave_status 
+float64 pose_x 
+float64 pose_y 
+float64 pose_yaw
+float32 work_odom 
+float32 all_work_odom
+float32 odom
+float32 all_odom
+float32 battery_energy_consumption
+float32 all_battery_energy_consumption
+float32 cpu_usage
+float32 memory_usage
+string hardversion
+string softversion
+int32 statusel
+int32 statuser
+int32 statuseh
+int32 statusew
+int32 statusebms
+int32 statuse
+int32 status
+int32 bmsc
+int32 bmspower
+int32 energycomsumption
+float32 linear_speed
+float32 angular_speed
+float64 total_odom

auto_drive_proj_msg/package.xml

@@ -0,0 +1,29 @@
+<?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>auto_drive_proj_msg</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="root@todo.todo">root</maintainer>
+  <license>TODO: License declaration</license>
+
+  <depend>action_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>rclcpp</depend>
+  
+  <buildtool_depend>ament_cmake</buildtool_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>

auto_drive_proj_msg/srv/GetCoil.srv

@@ -0,0 +1,4 @@
+uint16 address
+---
+bool success
+uint8[] data

auto_drive_proj_msg/srv/GetMultiCoils.srv

@@ -0,0 +1,5 @@
+uint16 address
+int32 count
+---
+bool success
+uint8[] data

auto_drive_proj_msg/srv/GetMultiRegs.srv

@@ -0,0 +1,6 @@
+uint16 address
+uint8 count
+---
+bool success
+uint16[] data
+string message

auto_drive_proj_msg/srv/GetReg.srv

@@ -0,0 +1,5 @@
+uint16 address
+---
+bool success
+uint16 data
+string message

auto_drive_proj_msg/srv/GetRegFloat32.srv

@@ -0,0 +1,5 @@
+uint16 address
+---
+bool success
+float32 data
+string message

auto_drive_proj_msg/srv/GetRegInt32.srv

@@ -0,0 +1,5 @@
+uint16 address
+---
+bool success
+int32 data
+string message

auto_drive_proj_msg/srv/ReadProperties.srv

@@ -0,0 +1,5 @@
+string[] data
+---
+bool success
+string message
+string properties

auto_drive_proj_msg/srv/ReqAuto.srv

@@ -0,0 +1,4 @@
+string request
+---
+bool success
+string message

auto_drive_proj_msg/srv/ReqMission.srv

@@ -0,0 +1,4 @@
+string request
+---
+bool success
+string message

auto_drive_proj_msg/srv/ReqString.srv

@@ -0,0 +1,6 @@
+string request
+string value
+---
+bool success
+string message
+string properties

auto_drive_proj_msg/srv/SetCoil.srv

@@ -0,0 +1,4 @@
+uint16 address
+uint8 data
+---
+bool success

auto_drive_proj_msg/srv/SetMultiCoils.srv

@@ -0,0 +1,4 @@
+uint16 address
+uint8[] data
+---
+bool success

auto_drive_proj_msg/srv/SetMultiRegs.srv

@@ -0,0 +1,5 @@
+uint16 address
+uint16[] data
+---
+bool success
+string message

auto_drive_proj_msg/srv/SetPose.srv

@@ -0,0 +1,6 @@
+float64 lat
+float64 lon
+float64 alt
+float64 yaw
+---
+bool success

auto_drive_proj_msg/srv/SetReg.srv

@@ -0,0 +1,5 @@
+uint16 address
+uint16 data
+---
+bool success
+string message

auto_drive_proj_msg/srv/SetRegFloat32.srv

@@ -0,0 +1,5 @@
+uint16 address
+float32 data
+---
+bool success
+string message

auto_drive_proj_msg/srv/SetRegInt32.srv

@@ -0,0 +1,5 @@
+uint16 address
+int32 data
+---
+bool success
+string message

auto_drive_proj_msg/srv/SpeedConvertParams.srv

@@ -0,0 +1,7 @@
+string request
+---
+float32 max_speed
+float32 wheel_dis
+float32 wheel_diameter
+float32 rpm_ratio
+float32 reduction_ratio

auto_drive_proj_msg/srv/VisualData.srv

@@ -0,0 +1,5 @@
+#请求视觉端识别茶叶。0：左视觉端识别、1：右视觉端识别
+int64 context_visual
+---
+#视觉识别结果反馈。0：抓取失败、1：抓取成功
+geometry_msgs/Point[] target_visual

auto_drive_proj_utils/CMakeLists.txt

@@ -0,0 +1,104 @@
+cmake_minimum_required(VERSION 3.8)
+project(auto_drive_proj_utils)
+set(CMAKE_BUILD_TYPE "Debug")
+
+if(NOT CMAKE_C_STANDARD)
+    set(CMAKE_C_STANDARD 99)
+endif()
+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 -Wnon-virtual-dtor -Woverloaded-virtual)
+endif ()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+#find_package(backward_ros REQUIRED)
+
+include_directories(${EIGEN3_INCLUDE_DIR})
+add_definitions(${EIGEN_DEFINITIONS})
+
+
+#include_directories(include)
+
+include_directories(Modbus LibModbus SweetLog ThreadPool Json spdlog SerialCommunicaiton UTM_WGS84 Utils GeodeticConverter MD5)
+file(GLOB_RECURSE DIR_SRCS *.cpp *.cc *.h *.hpp)
+# 添加源文件，生成库
+add_library(auto_drive_proj_utils SHARED
+        ${DIR_SRCS}
+        )
+
+target_include_directories(auto_drive_proj_utils PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>
+    $<INSTALL_INTERFACE:include>
+)
+
+# 用于代替传统的target_link_libraries
+ament_target_dependencies(auto_drive_proj_utils
+        PUBLIC
+        rclcpp
+        )
+
+
+# 为了使下游文件可以访问
+ament_export_targets(auto_drive_proj_utils HAS_LIBRARY_TARGET)
+ament_export_dependencies(
+        rclcpp
+)
+
+install(
+        TARGETS auto_drive_proj_utils
+        EXPORT auto_drive_proj_utils
+        ARCHIVE DESTINATION lib
+        LIBRARY DESTINATION lib
+        RUNTIME DESTINATION bin
+        #INCLUDES DESTINATION include
+)
+
+install(
+    DIRECTORY Json/
+    DESTINATION include/auto_drive_proj_utils/Json
+)
+install(
+    DIRECTORY GeodeticConverter/
+    DESTINATION include/auto_drive_proj_utils/GeodeticConverter
+)
+install(
+    DIRECTORY spdlog/
+    DESTINATION include/auto_drive_proj_utils/spdlog
+)
+install(
+    DIRECTORY SerialCommunicaiton/
+    DESTINATION include/auto_drive_proj_utils/SerialCommunicaiton
+)
+install(
+    DIRECTORY UTM_WGS84/
+    DESTINATION include/auto_drive_proj_utils/UTM_WGS84
+)
+install(
+    DIRECTORY Utils/
+    DESTINATION include/auto_drive_proj_utils/Utils
+)
+install(
+    DIRECTORY LibModbus/
+    DESTINATION include/auto_drive_proj_utils/LibModbus
+)
+install(
+    DIRECTORY ThreadPool/
+    DESTINATION include/auto_drive_proj_utils/ThreadPool
+)
+install(
+    DIRECTORY MD5/
+    DESTINATION include/auto_drive_proj_utils/MD5
+)
+install(DIRECTORY
+  config
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+
+ament_package()

auto_drive_proj_utils/GeodeticConverter/GeodeticConverter.hpp

@@ -0,0 +1,211 @@
+//GeodeticConverter.hpp
+#ifndef air_GeodeticConverter_hpp
+#define air_GeodeticConverter_hpp
+
+#include <math.h>
+#include <eigen3/Eigen/Dense>
+
+static double kSemimajorAxis = 6378137;
+static double kSemiminorAxis = 6356752.3142;
+static double kFirstEccentricitySquared = 6.69437999014 * 0.001;
+static double kSecondEccentricitySquared = 6.73949674228 * 0.001;
+static double kFlattening = 1 / 298.257223563;
+
+
+class GeodeticConverter
+{
+ public:
+  GeodeticConverter(double home_latitude = 0, double home_longitude = 0, double home_altitude = 0)
+    : home_latitude_(home_latitude), home_longitude_(home_longitude)
+  {
+    // Save NED origin
+    home_latitude_rad_ = deg2Rad(home_latitude_);
+    home_longitude_rad_ = deg2Rad(home_longitude_);
+    home_altitude_ = home_altitude;
+
+    // Compute ECEF of NED origin
+    geodetic2Ecef(home_latitude_, home_longitude_, home_altitude, &home_ecef_x_, &home_ecef_y_, &home_ecef_z_);
+
+    // Compute ECEF to NED and NED to ECEF matrices
+    double phiP = atan2(home_ecef_z_, sqrt(pow(home_ecef_x_, 2) + pow(home_ecef_y_, 2)));
+
+    ecef_to_ned_matrix_ = nRe(phiP, home_longitude_rad_);
+    ned_to_ecef_matrix_ = nRe(home_latitude_rad_, home_longitude_rad_).transpose();    
+  }
+
+  void getHome(double* latitude, double* longitude, double* altitude)
+  {
+    *latitude = home_latitude_;
+    *longitude = home_longitude_;
+    *altitude = home_altitude_;
+  }
+
+  void geodetic2Ecef(const double latitude, const double longitude, const double altitude, double* x,
+                     double* y, double* z)
+  {
+    // Convert geodetic coordinates to ECEF.
+    // http://code.google.com/p/pysatel/source/browse/trunk/coord.py?r=22
+    double lat_rad = deg2Rad(latitude);
+    double lon_rad = deg2Rad(longitude);
+    double xi = sqrt(1 - kFirstEccentricitySquared * sin(lat_rad) * sin(lat_rad));
+    *x = (kSemimajorAxis / xi + altitude) * cos(lat_rad) * cos(lon_rad);
+    *y = (kSemimajorAxis / xi + altitude) * cos(lat_rad) * sin(lon_rad);
+    *z = (kSemimajorAxis / xi * (1 - kFirstEccentricitySquared) + altitude) * sin(lat_rad);
+  }
+
+  void ecef2Geodetic(const double x, const double y, const double z, double* latitude,
+                     double* longitude, double* altitude)
+  {
+    // Convert ECEF coordinates to geodetic coordinates.
+    // J. Zhu, "Conversion of Earth-centered Earth-fixed coordinates
+    // to geodetic coordinates," IEEE Transactions on Aerospace and
+    // Electronic Systems, vol. 30, pp. 957-961, 1994.
+
+    double r = sqrt(x * x + y * y);
+    double Esq = kSemimajorAxis * kSemimajorAxis - kSemiminorAxis * kSemiminorAxis;
+    double F = 54 * kSemiminorAxis * kSemiminorAxis * z * z;
+    double G = r * r + (1 - kFirstEccentricitySquared) * z * z - kFirstEccentricitySquared * Esq;
+    double C = (kFirstEccentricitySquared * kFirstEccentricitySquared * F * r * r) / pow(G, 3);
+    double S = cbrt(1 + C + sqrt(C * C + 2 * C));
+    double P = F / (3 * pow((S + 1 / S + 1), 2) * G * G);
+    double Q = sqrt(1 + 2 * kFirstEccentricitySquared * kFirstEccentricitySquared * P);
+    double r_0 = -(P * kFirstEccentricitySquared * r) / (1 + Q)
+        + sqrt(
+            0.5 * kSemimajorAxis * kSemimajorAxis * (1 + 1.0 / Q)
+                - P * (1 - kFirstEccentricitySquared) * z * z / (Q * (1 + Q)) - 0.5 * P * r * r);
+    double U = sqrt(pow((r - kFirstEccentricitySquared * r_0), 2) + z * z);
+    double V = sqrt(
+        pow((r - kFirstEccentricitySquared * r_0), 2) + (1 - kFirstEccentricitySquared) * z * z);
+    double Z_0 = kSemiminorAxis * kSemiminorAxis * z / (kSemimajorAxis * V);
+    *altitude = U * (1 - kSemiminorAxis * kSemiminorAxis / (kSemimajorAxis * V));
+    *latitude = rad2Deg(atan((z + kSecondEccentricitySquared * Z_0) / r));
+    *longitude = rad2Deg(atan2(y, x));
+  }
+
+  void ecef2Ned(const double x, const double y, const double z, double* north, double* east,
+                double* down)
+  {
+    // Converts ECEF coordinate position into local-tangent-plane NED.
+    // Coordinates relative to given ECEF coordinate frame.
+
+    Vector3d vect, ret;
+    vect(0) = x - home_ecef_x_;
+    vect(1) = y - home_ecef_y_;
+    vect(2) = z - home_ecef_z_;
+    ret = ecef_to_ned_matrix_ * vect;
+    *north = ret(0);
+    *east = ret(1);
+    *down = -ret(2);
+  }
+
+  void ned2Ecef(const double north, const double east, const double down, double* x, double* y,
+                double* z)
+  {
+    // NED (north/east/down) to ECEF coordinates
+    Vector3d ned, ret;
+    ned(0) = north;
+    ned(1) = east;
+    ned(2) = -down;
+    ret = ned_to_ecef_matrix_ * ned;
+    *x = ret(0) + home_ecef_x_;
+    *y = ret(1) + home_ecef_y_;
+    *z = ret(2) + home_ecef_z_;
+  }
+
+  void geodetic2Ned(const double latitude, const double longitude, const double altitude,
+                    double* north, double* east, double* down)
+  {
+    // Geodetic position to local NED frame
+    double x, y, z;
+    geodetic2Ecef(latitude, longitude, altitude, &x, &y, &z);
+    ecef2Ned(x, y, z, north, east, down);
+  }
+
+  void ned2Geodetic(const double north, const double east, const double down, double* latitude,
+                    double* longitude, double* altitude)
+  {
+    // Local NED position to geodetic coordinates
+    double x, y, z;
+    ned2Ecef(north, east, down, &x, &y, &z);
+    ecef2Geodetic(x, y, z, latitude, longitude, altitude);
+  }
+
+  void geodetic2Enu(const double latitude, const double longitude, const double altitude,
+                    double* east, double* north, double* up)
+  {
+    // Geodetic position to local ENU frame
+    double x, y, z;
+    geodetic2Ecef(latitude, longitude, altitude, &x, &y, &z);
+
+    double aux_north, aux_east, aux_down;
+    ecef2Ned(x, y, z, &aux_north, &aux_east, &aux_down);
+
+    *east = aux_east;
+    *north = aux_north;
+    *up = -aux_down;
+  }
+
+  void enu2Geodetic(const double east, const double north, const double up, double* latitude,
+                    double* longitude, double* altitude)
+  {
+    // Local ENU position to geodetic coordinates
+
+    const double aux_north = north;
+    const double aux_east = east;
+    const double aux_down = -up;
+    double x, y, z;
+    ned2Ecef(aux_north, aux_east, aux_down, &x, &y, &z);
+    ecef2Geodetic(x, y, z, latitude, longitude, altitude);
+  }
+
+private:
+  // Geodetic system parameters
+  
+  typedef Eigen::Vector3d Vector3d;
+  typedef Eigen::Matrix<double, 3, 3> Matrix3x3d;
+
+  inline Matrix3x3d nRe(const double lat_radians, const double lon_radians)
+  {
+    const double sLat = sin(lat_radians);
+    const double sLon = sin(lon_radians);
+    const double cLat = cos(lat_radians);
+    const double cLon = cos(lon_radians);
+
+    Matrix3x3d ret;
+    ret(0, 0) = -sLat * cLon;
+    ret(0, 1) = -sLat * sLon;
+    ret(0, 2) = cLat;
+    ret(1, 0) = -sLon;
+    ret(1, 1) = cLon;
+    ret(1, 2) = 0.0;
+    ret(2, 0) = cLat * cLon;
+    ret(2, 1) = cLat * sLon;
+    ret(2, 2) = sLat;
+
+    return ret;
+  }
+
+  inline double rad2Deg(const double radians)
+  {
+    return (radians / M_PI) * 180.0;
+  }
+
+  inline double deg2Rad(const double degrees)
+  {
+    return (degrees / 180.0) * M_PI;
+  }
+
+  double home_latitude_rad_, home_latitude_;
+  double home_longitude_rad_, home_longitude_;
+  double home_altitude_;
+
+  double home_ecef_x_;
+  double home_ecef_y_;
+  double home_ecef_z_;
+
+  Matrix3x3d ecef_to_ned_matrix_;
+  Matrix3x3d ned_to_ecef_matrix_;
+
+}; // class GeodeticConverter
+
+#endif

auto_drive_proj_utils/LibModbus/modbus-data.c

@@ -0,0 +1,292 @@
+/*
+ * Copyright © Stéphane Raimbault <stephane.raimbault@gmail.com>
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later
+ */
+
+#include <stdlib.h>
+
+// clang-format off
+#ifndef _MSC_VER
+#  include <stdint.h>
+#else
+#  include "stdint.h"
+#endif
+
+#include <string.h>
+#include <assert.h>
+
+#if defined(_WIN32)
+#  include <winsock2.h>
+#else
+#  include <arpa/inet.h>
+#endif
+
+#include <config.h>
+
+#include "modbus.h"
+
+#if defined(HAVE_BYTESWAP_H)
+#  include <byteswap.h>
+#endif
+
+#if defined(__APPLE__)
+#  include <libkern/OSByteOrder.h>
+#  define bswap_16 OSSwapInt16
+#  define bswap_32 OSSwapInt32
+#  define bswap_64 OSSwapInt64
+#endif
+
+#if defined(__GNUC__)
+#  define GCC_VERSION (__GNUC__ * 100 + __GNUC_MINOR__ * 10)
+#  if GCC_VERSION >= 430
+// Since GCC >= 4.30, GCC provides __builtin_bswapXX() alternatives so we switch to them
+#    undef bswap_32
+#    define bswap_32 __builtin_bswap32
+#  endif
+#  if GCC_VERSION >= 480
+#    undef bswap_16
+#    define bswap_16 __builtin_bswap16
+#  endif
+#endif
+
+#if defined(_MSC_VER) && (_MSC_VER >= 1400)
+#  define bswap_32 _byteswap_ulong
+#  define bswap_16 _byteswap_ushort
+#endif
+
+#if !defined(bswap_16)
+#  warning "Fallback on C functions for bswap_16"
+static inline uint16_t bswap_16(uint16_t x)
+{
+    return (x >> 8) | (x << 8);
+}
+#endif
+
+#if !defined(bswap_32)
+#  warning "Fallback on C functions for bswap_32"
+static inline uint32_t bswap_32(uint32_t x)
+{
+    return (bswap_16(x & 0xffff) << 16) | (bswap_16(x >> 16));
+}
+#endif
+// clang-format on
+
+/* Sets many bits from a single byte value (all 8 bits of the byte value are
+   set) */
+void modbus_set_bits_from_byte(uint8_t *dest, int idx, const uint8_t value)
+{
+    int i;
+
+    for (i = 0; i < 8; i++) {
+        dest[idx + i] = (value & (1 << i)) ? 1 : 0;
+    }
+}
+
+/* Sets many bits from a table of bytes (only the bits between idx and
+   idx + nb_bits are set) */
+void modbus_set_bits_from_bytes(uint8_t *dest,
+                                int idx,
+                                unsigned int nb_bits,
+                                const uint8_t *tab_byte)
+{
+    unsigned int i;
+    int shift = 0;
+
+    for (i = idx; i < idx + nb_bits; i++) {
+        dest[i] = tab_byte[(i - idx) / 8] & (1 << shift) ? 1 : 0;
+        /* gcc doesn't like: shift = (++shift) % 8; */
+        shift++;
+        shift %= 8;
+    }
+}
+
+/* Gets the byte value from many bits.
+   To obtain a full byte, set nb_bits to 8. */
+uint8_t modbus_get_byte_from_bits(const uint8_t *src, int idx, unsigned int nb_bits)
+{
+    unsigned int i;
+    uint8_t value = 0;
+
+    if (nb_bits > 8) {
+        /* Assert is ignored if NDEBUG is set */
+        assert(nb_bits < 8);
+        nb_bits = 8;
+    }
+
+    for (i = 0; i < nb_bits; i++) {
+        value |= (src[idx + i] << i);
+    }
+
+    return value;
+}
+
+/* Get a float from 4 bytes (Modbus) without any conversion (ABCD) */
+float modbus_get_float_abcd(const uint16_t *src)
+{
+    float f;
+    uint32_t i;
+    uint8_t a, b, c, d;
+
+    a = (src[0] >> 8) & 0xFF;
+    b = (src[0] >> 0) & 0xFF;
+    c = (src[1] >> 8) & 0xFF;
+    d = (src[1] >> 0) & 0xFF;
+
+    i = (a << 24) | (b << 16) | (c << 8) | (d << 0);
+    memcpy(&f, &i, 4);
+
+    return f;
+}
+
+/* Get a float from 4 bytes (Modbus) in inversed format (DCBA) */
+float modbus_get_float_dcba(const uint16_t *src)
+{
+    float f;
+    uint32_t i;
+    uint8_t a, b, c, d;
+
+    a = (src[0] >> 8) & 0xFF;
+    b = (src[0] >> 0) & 0xFF;
+    c = (src[1] >> 8) & 0xFF;
+    d = (src[1] >> 0) & 0xFF;
+
+    i = (d << 24) | (c << 16) | (b << 8) | (a << 0);
+    memcpy(&f, &i, 4);
+
+    return f;
+}
+
+/* Get a float from 4 bytes (Modbus) with swapped bytes (BADC) */
+float modbus_get_float_badc(const uint16_t *src)
+{
+    float f;
+    uint32_t i;
+    uint8_t a, b, c, d;
+
+    a = (src[0] >> 8) & 0xFF;
+    b = (src[0] >> 0) & 0xFF;
+    c = (src[1] >> 8) & 0xFF;
+    d = (src[1] >> 0) & 0xFF;
+
+    i = (b << 24) | (a << 16) | (d << 8) | (c << 0);
+    memcpy(&f, &i, 4);
+
+    return f;
+}
+
+/* Get a float from 4 bytes (Modbus) with swapped words (CDAB) */
+float modbus_get_float_cdab(const uint16_t *src)
+{
+    float f;
+    uint32_t i;
+    uint8_t a, b, c, d;
+
+    a = (src[0] >> 8) & 0xFF;
+    b = (src[0] >> 0) & 0xFF;
+    c = (src[1] >> 8) & 0xFF;
+    d = (src[1] >> 0) & 0xFF;
+
+    i = (c << 24) | (d << 16) | (a << 8) | (b << 0);
+    memcpy(&f, &i, 4);
+
+    return f;
+}
+
+/* DEPRECATED - Get a float from 4 bytes in sort of Modbus format */
+float modbus_get_float(const uint16_t *src)
+{
+    float f;
+    uint32_t i;
+
+    i = (((uint32_t) src[1]) << 16) + src[0];
+    memcpy(&f, &i, sizeof(float));
+
+    return f;
+}
+
+/* Set a float to 4 bytes for Modbus w/o any conversion (ABCD) */
+void modbus_set_float_abcd(float f, uint16_t *dest)
+{
+    uint32_t i;
+    uint8_t *out = (uint8_t *) dest;
+    uint8_t a, b, c, d;
+
+    memcpy(&i, &f, sizeof(uint32_t));
+    a = (i >> 24) & 0xFF;
+    b = (i >> 16) & 0xFF;
+    c = (i >> 8) & 0xFF;
+    d = (i >> 0) & 0xFF;
+
+    out[0] = a;
+    out[1] = b;
+    out[2] = c;
+    out[3] = d;
+}
+
+/* Set a float to 4 bytes for Modbus with byte and word swap conversion (DCBA) */
+void modbus_set_float_dcba(float f, uint16_t *dest)
+{
+    uint32_t i;
+    uint8_t *out = (uint8_t *) dest;
+    uint8_t a, b, c, d;
+
+    memcpy(&i, &f, sizeof(uint32_t));
+    a = (i >> 24) & 0xFF;
+    b = (i >> 16) & 0xFF;
+    c = (i >> 8) & 0xFF;
+    d = (i >> 0) & 0xFF;
+
+    out[0] = d;
+    out[1] = c;
+    out[2] = b;
+    out[3] = a;
+}
+
+/* Set a float to 4 bytes for Modbus with byte swap conversion (BADC) */
+void modbus_set_float_badc(float f, uint16_t *dest)
+{
+    uint32_t i;
+    uint8_t *out = (uint8_t *) dest;
+    uint8_t a, b, c, d;
+
+    memcpy(&i, &f, sizeof(uint32_t));
+    a = (i >> 24) & 0xFF;
+    b = (i >> 16) & 0xFF;
+    c = (i >> 8) & 0xFF;
+    d = (i >> 0) & 0xFF;
+
+    out[0] = b;
+    out[1] = a;
+    out[2] = d;
+    out[3] = c;
+}
+
+/* Set a float to 4 bytes for Modbus with word swap conversion (CDAB) */
+void modbus_set_float_cdab(float f, uint16_t *dest)
+{
+    uint32_t i;
+    uint8_t *out = (uint8_t *) dest;
+    uint8_t a, b, c, d;
+
+    memcpy(&i, &f, sizeof(uint32_t));
+    a = (i >> 24) & 0xFF;
+    b = (i >> 16) & 0xFF;
+    c = (i >> 8) & 0xFF;
+    d = (i >> 0) & 0xFF;
+
+    out[0] = c;
+    out[1] = d;
+    out[2] = a;
+    out[3] = b;
+}
+
+/* DEPRECATED - Set a float to 4 bytes in a sort of Modbus format! */
+void modbus_set_float(float f, uint16_t *dest)
+{
+    uint32_t i;
+
+    memcpy(&i, &f, sizeof(uint32_t));
+    dest[0] = (uint16_t) i;
+    dest[1] = (uint16_t) (i >> 16);
+}

auto_drive_proj_utils/LibModbus/modbus-private.h

@@ -0,0 +1,121 @@
+/*
+ * Copyright © Stéphane Raimbault <stephane.raimbault@gmail.com>
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later
+ */
+
+#ifndef MODBUS_PRIVATE_H
+#define MODBUS_PRIVATE_H
+
+// clang-format off
+#ifndef _MSC_VER
+# include <stdint.h>
+# include <sys/time.h>
+#else
+# include "stdint.h"
+# include <time.h>
+typedef int ssize_t;
+#endif
+// clang-format on
+#include <config.h>
+#include <sys/types.h>
+
+#include "modbus.h"
+
+MODBUS_BEGIN_DECLS
+
+/* It's not really the minimal length (the real one is report slave ID
+ * in RTU (4 bytes)) but it's a convenient size to use in RTU or TCP
+ * communications to read many values or write a single one.
+ * Maximum between :
+ * - HEADER_LENGTH_TCP (7) + function (1) + address (2) + number (2)
+ * - HEADER_LENGTH_RTU (1) + function (1) + address (2) + number (2) + CRC (2)
+ */
+#define _MIN_REQ_LENGTH 12
+
+#define _REPORT_SLAVE_ID 180
+
+#define _MODBUS_EXCEPTION_RSP_LENGTH 5
+
+/* Timeouts in microsecond (0.5 s) */
+#define _RESPONSE_TIMEOUT 500000
+#define _BYTE_TIMEOUT     500000
+
+typedef enum {
+    _MODBUS_BACKEND_TYPE_RTU = 0,
+    _MODBUS_BACKEND_TYPE_TCP
+} modbus_backend_type_t;
+
+/*
+ *  ---------- Request     Indication ----------
+ *  | Client | ---------------------->| Server |
+ *  ---------- Confirmation  Response ----------
+ */
+typedef enum {
+    /* Request message on the server side */
+    MSG_INDICATION,
+    /* Request message on the client side */
+    MSG_CONFIRMATION
+} msg_type_t;
+
+/* This structure reduces the number of params in functions and so
+ * optimizes the speed of execution (~ 37%). */
+typedef struct _sft {
+    int slave;
+    int function;
+    int t_id;
+} sft_t;
+
+typedef struct _modbus_backend {
+    unsigned int backend_type;
+    unsigned int header_length;
+    unsigned int checksum_length;
+    unsigned int max_adu_length;
+    int (*set_slave)(modbus_t *ctx, int slave);
+    int (*build_request_basis)(
+        modbus_t *ctx, int function, int addr, int nb, uint8_t *req);
+    int (*build_response_basis)(sft_t *sft, uint8_t *rsp);
+    int (*prepare_response_tid)(const uint8_t *req, int *req_length);
+    int (*send_msg_pre)(uint8_t *req, int req_length);
+    ssize_t (*send)(modbus_t *ctx, const uint8_t *req, int req_length);
+    int (*receive)(modbus_t *ctx, uint8_t *req);
+    ssize_t (*recv)(modbus_t *ctx, uint8_t *rsp, int rsp_length);
+    int (*check_integrity)(modbus_t *ctx, uint8_t *msg, const int msg_length);
+    int (*pre_check_confirmation)(modbus_t *ctx,
+                                  const uint8_t *req,
+                                  const uint8_t *rsp,
+                                  int rsp_length);
+    int (*connect)(modbus_t *ctx);
+    unsigned int (*is_connected)(modbus_t *ctx);
+    void (*close)(modbus_t *ctx);
+    int (*flush)(modbus_t *ctx);
+    int (*select)(modbus_t *ctx, fd_set *rset, struct timeval *tv, int msg_length);
+    void (*free)(modbus_t *ctx);
+} modbus_backend_t;
+
+struct _modbus {
+    /* Slave address */
+    int slave;
+    /* Socket or file descriptor */
+    int s;
+    int debug;
+    int error_recovery;
+    int quirks;
+    struct timeval response_timeout;
+    struct timeval byte_timeout;
+    struct timeval indication_timeout;
+    const modbus_backend_t *backend;
+    void *backend_data;
+};
+
+void _modbus_init_common(modbus_t *ctx);
+void _error_print(modbus_t *ctx, const char *context);
+int _modbus_receive_msg(modbus_t *ctx, uint8_t *msg, msg_type_t msg_type);
+
+#ifndef HAVE_STRLCPY
+size_t strlcpy(char *dest, const char *src, size_t dest_size);
+#endif
+
+MODBUS_END_DECLS
+
+#endif /* MODBUS_PRIVATE_H */

auto_drive_proj_utils/LibModbus/modbus-rtu-private.h

@@ -0,0 +1,77 @@
+/*
+ * Copyright © Stéphane Raimbault <stephane.raimbault@gmail.com>
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later
+ */
+
+#ifndef MODBUS_RTU_PRIVATE_H
+#define MODBUS_RTU_PRIVATE_H
+
+#ifndef _MSC_VER
+#include <stdint.h>
+#else
+#include "stdint.h"
+#endif
+
+#if defined(_WIN32)
+#include <windows.h>
+#else
+#include <termios.h>
+#endif
+
+#define _MODBUS_RTU_HEADER_LENGTH     1
+#define _MODBUS_RTU_PRESET_REQ_LENGTH 6
+#define _MODBUS_RTU_PRESET_RSP_LENGTH 2
+
+#define _MODBUS_RTU_CHECKSUM_LENGTH 2
+
+#if defined(_WIN32)
+#if !defined(ENOTSUP)
+#define ENOTSUP WSAEOPNOTSUPP
+#endif
+
+/* WIN32: struct containing serial handle and a receive buffer */
+#define PY_BUF_SIZE 512
+
+struct win32_ser {
+    /* File handle */
+    HANDLE fd;
+    /* Receive buffer */
+    uint8_t buf[PY_BUF_SIZE];
+    /* Received chars */
+    DWORD n_bytes;
+};
+#endif /* _WIN32 */
+
+typedef struct _modbus_rtu {
+    /* Device: "/dev/ttyS0", "/dev/ttyUSB0" or "/dev/tty.USA19*" on Mac OS X. */
+    char *device;
+    /* Bauds: 9600, 19200, 57600, 115200, etc */
+    int baud;
+    /* Data bit */
+    uint8_t data_bit;
+    /* Stop bit */
+    uint8_t stop_bit;
+    /* Parity: 'N', 'O', 'E' */
+    char parity;
+#if defined(_WIN32)
+    struct win32_ser w_ser;
+    DCB old_dcb;
+#else
+    /* Save old termios settings */
+    struct termios old_tios;
+#endif
+#if HAVE_DECL_TIOCSRS485
+    int serial_mode;
+#endif
+#if HAVE_DECL_TIOCM_RTS
+    int rts;
+    int rts_delay;
+    int onebyte_time;
+    void (*set_rts)(modbus_t *ctx, int on);
+#endif
+    /* To handle many slaves on the same link */
+    int confirmation_to_ignore;
+} modbus_rtu_t;
+
+#endif /* MODBUS_RTU_PRIVATE_H */

auto_drive_proj_utils/LibModbus/modbus-rtu.h

@@ -0,0 +1,43 @@
+/*
+ * Copyright © Stéphane Raimbault <stephane.raimbault@gmail.com>
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later
+ */
+
+#ifndef MODBUS_RTU_H
+#define MODBUS_RTU_H
+
+#include "modbus.h"
+
+MODBUS_BEGIN_DECLS
+
+/* Modbus_Application_Protocol_V1_1b.pdf Chapter 4 Section 1 Page 5
+ * RS232 / RS485 ADU = 253 bytes + slave (1 byte) + CRC (2 bytes) = 256 bytes
+ */
+#define MODBUS_RTU_MAX_ADU_LENGTH 256
+
+MODBUS_API modbus_t *
+modbus_new_rtu(const char *device, int baud, char parity, int data_bit, int stop_bit);
+
+#define MODBUS_RTU_RS232 0
+#define MODBUS_RTU_RS485 1
+
+MODBUS_API int modbus_rtu_set_serial_mode(modbus_t *ctx, int mode);
+MODBUS_API int modbus_rtu_get_serial_mode(modbus_t *ctx);
+
+#define MODBUS_RTU_RTS_NONE 0
+#define MODBUS_RTU_RTS_UP   1
+#define MODBUS_RTU_RTS_DOWN 2
+
+MODBUS_API int modbus_rtu_set_rts(modbus_t *ctx, int mode);
+MODBUS_API int modbus_rtu_get_rts(modbus_t *ctx);
+
+MODBUS_API int modbus_rtu_set_custom_rts(modbus_t *ctx,
+                                         void (*set_rts)(modbus_t *ctx, int on));
+
+MODBUS_API int modbus_rtu_set_rts_delay(modbus_t *ctx, int us);
+MODBUS_API int modbus_rtu_get_rts_delay(modbus_t *ctx);
+
+MODBUS_END_DECLS
+
+#endif /* MODBUS_RTU_H */

auto_drive_proj_utils/LibModbus/modbus-tcp-private.h

@@ -0,0 +1,41 @@
+/*
+ * Copyright © Stéphane Raimbault <stephane.raimbault@gmail.com>
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later
+ */
+
+#ifndef MODBUS_TCP_PRIVATE_H
+#define MODBUS_TCP_PRIVATE_H
+
+#define _MODBUS_TCP_HEADER_LENGTH     7
+#define _MODBUS_TCP_PRESET_REQ_LENGTH 12
+#define _MODBUS_TCP_PRESET_RSP_LENGTH 8
+
+#define _MODBUS_TCP_CHECKSUM_LENGTH 0
+
+/* In both structures, the transaction ID must be placed on first position
+   to have a quick access not dependent of the TCP backend */
+typedef struct _modbus_tcp {
+    /* Extract from MODBUS Messaging on TCP/IP Implementation Guide V1.0b
+       (page 23/46):
+       The transaction identifier is used to associate the future response
+       with the request. This identifier is unique on each TCP connection. */
+    uint16_t t_id;
+    /* TCP port */
+    int port;
+    /* IP address */
+    char ip[16];
+} modbus_tcp_t;
+
+typedef struct _modbus_tcp_pi {
+    /* Transaction ID */
+    uint16_t t_id;
+    /* TCP port */
+    int port;
+    /* Node */
+    char *node;
+    /* Service */
+    char *service;
+} modbus_tcp_pi_t;
+
+#endif /* MODBUS_TCP_PRIVATE_H */

auto_drive_proj_utils/LibModbus/modbus-tcp.c

@@ -0,0 +1,974 @@
+/*
+ * Copyright © Stéphane Raimbault <stephane.raimbault@gmail.com>
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later
+ */
+
+// clang-format off
+#if defined(_WIN32)
+# define OS_WIN32
+/* ws2_32.dll has getaddrinfo and freeaddrinfo on Windows XP and later.
+ * minwg32 headers check WINVER before allowing the use of these */
+# ifndef WINVER
+#   define WINVER 0x0501
+# endif
+#endif
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <errno.h>
+#ifndef _MSC_VER
+#include <unistd.h>
+#endif
+#include <signal.h>
+#include <sys/types.h>
+
+#if defined(_WIN32)
+/* Already set in modbus-tcp.h but it seems order matters in VS2005 */
+# include <winsock2.h>
+# include <ws2tcpip.h>
+# define SHUT_RDWR 2
+# define close closesocket
+# define strdup _strdup
+#else
+# include <sys/socket.h>
+# include <sys/ioctl.h>
+
+#if defined(__OpenBSD__) || (defined(__FreeBSD__) && __FreeBSD__ < 5)
+# define OS_BSD
+# include <netinet/in_systm.h>
+#endif
+
+# include <netinet/in.h>
+# include <netinet/ip.h>
+# include <netinet/tcp.h>
+# include <arpa/inet.h>
+# include <netdb.h>
+#endif
+
+#if !defined(MSG_NOSIGNAL)
+#define MSG_NOSIGNAL 0
+#endif
+
+#if defined(_AIX) && !defined(MSG_DONTWAIT)
+#define MSG_DONTWAIT MSG_NONBLOCK
+#endif
+// clang-format on
+
+#include "modbus-private.h"
+
+#include "modbus-tcp-private.h"
+#include "modbus-tcp.h"
+
+#ifdef OS_WIN32
+static int _modbus_tcp_init_win32(void)
+{
+    /* Initialise Windows Socket API */
+    WSADATA wsaData;
+
+    if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0) {
+        fprintf(stderr,
+                "WSAStartup() returned error code %d\n",
+                (unsigned int) GetLastError());
+        errno = EIO;
+        return -1;
+    }
+    return 0;
+}
+#endif
+
+static int _modbus_set_slave(modbus_t *ctx, int slave)
+{
+    int max_slave = (ctx->quirks & MODBUS_QUIRK_MAX_SLAVE) ? 255 : 247;
+
+    /* Broadcast address is 0 (MODBUS_BROADCAST_ADDRESS) */
+    if (slave >= 0 && slave <= max_slave) {
+        ctx->slave = slave;
+    } else if (slave == MODBUS_TCP_SLAVE) {
+        /* The special value MODBUS_TCP_SLAVE (0xFF) can be used in TCP mode to
+         * restore the default value. */
+        ctx->slave = slave;
+    } else {
+        errno = EINVAL;
+        return -1;
+    }
+
+    return 0;
+}
+
+/* Builds a TCP request header */
+static int _modbus_tcp_build_request_basis(
+    modbus_t *ctx, int function, int addr, int nb, uint8_t *req)
+{
+    modbus_tcp_t *ctx_tcp = ctx->backend_data;
+
+    /* Increase transaction ID */
+    if (ctx_tcp->t_id < UINT16_MAX)
+        ctx_tcp->t_id++;
+    else
+        ctx_tcp->t_id = 0;
+    req[0] = ctx_tcp->t_id >> 8;
+    req[1] = ctx_tcp->t_id & 0x00ff;
+
+    /* Protocol Modbus */
+    req[2] = 0;
+    req[3] = 0;
+
+    /* Length will be defined later by set_req_length_tcp at offsets 4
+       and 5 */
+
+    req[6] = ctx->slave;
+    req[7] = function;
+    req[8] = addr >> 8;
+    req[9] = addr & 0x00ff;
+    req[10] = nb >> 8;
+    req[11] = nb & 0x00ff;
+
+    return _MODBUS_TCP_PRESET_REQ_LENGTH;
+}
+
+/* Builds a TCP response header */
+static int _modbus_tcp_build_response_basis(sft_t *sft, uint8_t *rsp)
+{
+    /* Extract from MODBUS Messaging on TCP/IP Implementation
+       Guide V1.0b (page 23/46):
+       The transaction identifier is used to associate the future
+       response with the request. */
+    rsp[0] = sft->t_id >> 8;
+    rsp[1] = sft->t_id & 0x00ff;
+
+    /* Protocol Modbus */
+    rsp[2] = 0;
+    rsp[3] = 0;
+
+    /* Length will be set later by send_msg (4 and 5) */
+
+    /* The slave ID is copied from the indication */
+    rsp[6] = sft->slave;
+    rsp[7] = sft->function;
+
+    return _MODBUS_TCP_PRESET_RSP_LENGTH;
+}
+
+static int _modbus_tcp_prepare_response_tid(const uint8_t *req, int *req_length)
+{
+    return (req[0] << 8) + req[1];
+}
+
+static int _modbus_tcp_send_msg_pre(uint8_t *req, int req_length)
+{
+    /* Subtract the header length to the message length */
+    int mbap_length = req_length - 6;
+
+    req[4] = mbap_length >> 8;
+    req[5] = mbap_length & 0x00FF;
+
+    return req_length;
+}
+
+static ssize_t _modbus_tcp_send(modbus_t *ctx, const uint8_t *req, int req_length)
+{
+    /* MSG_NOSIGNAL
+       Requests not to send SIGPIPE on errors on stream oriented
+       sockets when the other end breaks the connection.  The EPIPE
+       error is still returned. */
+    return send(ctx->s, (const char *) req, req_length, MSG_NOSIGNAL);
+}
+
+static int _modbus_tcp_receive(modbus_t *ctx, uint8_t *req)
+{
+    return _modbus_receive_msg(ctx, req, MSG_INDICATION);
+}
+
+static ssize_t _modbus_tcp_recv(modbus_t *ctx, uint8_t *rsp, int rsp_length)
+{
+    return recv(ctx->s, (char *) rsp, rsp_length, 0);
+}
+
+static int _modbus_tcp_check_integrity(modbus_t *ctx, uint8_t *msg, const int msg_length)
+{
+    return msg_length;
+}
+
+static int _modbus_tcp_pre_check_confirmation(modbus_t *ctx,
+                                              const uint8_t *req,
+                                              const uint8_t *rsp,
+                                              int rsp_length)
+{
+    unsigned int protocol_id;
+    /* Check transaction ID */
+    if (req[0] != rsp[0] || req[1] != rsp[1]) {
+        if (ctx->debug) {
+            fprintf(stderr,
+                    "Invalid transaction ID received 0x%X (not 0x%X)\n",
+                    (rsp[0] << 8) + rsp[1],
+                    (req[0] << 8) + req[1]);
+        }
+        errno = EMBBADDATA;
+        return -1;
+    }
+
+    /* Check protocol ID */
+    protocol_id = (rsp[2] << 8) + rsp[3];
+    if (protocol_id != 0x0) {
+        if (ctx->debug) {
+            fprintf(stderr, "Invalid protocol ID received 0x%X (not 0x0)\n", protocol_id);
+        }
+        errno = EMBBADDATA;
+        return -1;
+    }
+
+    return 0;
+}
+
+static int _modbus_tcp_set_ipv4_options(int s)
+{
+    int rc;
+    int option;
+
+    /* Set the TCP no delay flag */
+    /* SOL_TCP = IPPROTO_TCP */
+    option = 1;
+    rc = setsockopt(s, IPPROTO_TCP, TCP_NODELAY, (const void *) &option, sizeof(int));
+    if (rc == -1) {
+        return -1;
+    }
+
+    /* If the OS does not offer SOCK_NONBLOCK, fall back to setting FIONBIO to
+     * make sockets non-blocking */
+    /* Do not care about the return value, this is optional */
+#if !defined(SOCK_NONBLOCK) && defined(FIONBIO)
+#ifdef OS_WIN32
+    {
+        /* Setting FIONBIO expects an unsigned long according to MSDN */
+        u_long loption = 1;
+        ioctlsocket(s, FIONBIO, &loption);
+    }
+#else
+    option = 1;
+    ioctl(s, FIONBIO, &option);
+#endif
+#endif
+
+#ifndef OS_WIN32
+    /**
+     * Cygwin defines IPTOS_LOWDELAY but can't handle that flag so it's
+     * necessary to workaround that problem.
+     **/
+    /* Set the IP low delay option */
+    option = IPTOS_LOWDELAY;
+    rc = setsockopt(s, IPPROTO_IP, IP_TOS, (const void *) &option, sizeof(int));
+    if (rc == -1) {
+        return -1;
+    }
+#endif
+
+    return 0;
+}
+
+static int _connect(int sockfd,
+                    const struct sockaddr *addr,
+                    socklen_t addrlen,
+                    const struct timeval *ro_tv)
+{
+    int rc = connect(sockfd, addr, addrlen);
+
+#ifdef OS_WIN32
+    int wsaError = 0;
+    if (rc == -1) {
+        wsaError = WSAGetLastError();
+    }
+
+    if (wsaError == WSAEWOULDBLOCK || wsaError == WSAEINPROGRESS) {
+#else
+    if (rc == -1 && errno == EINPROGRESS) {
+#endif
+        fd_set wset;
+        int optval;
+        socklen_t optlen = sizeof(optval);
+        struct timeval tv = *ro_tv;
+
+        /* Wait to be available in writing */
+        FD_ZERO(&wset);
+        FD_SET(sockfd, &wset);
+        rc = select(sockfd + 1, NULL, &wset, NULL, &tv);
+        if (rc <= 0) {
+            /* Timeout or fail */
+            return -1;
+        }
+
+        /* The connection is established if SO_ERROR and optval are set to 0 */
+        rc = getsockopt(sockfd, SOL_SOCKET, SO_ERROR, (void *) &optval, &optlen);
+        if (rc == 0 && optval == 0) {
+            return 0;
+        } else {
+            errno = ECONNREFUSED;
+            return -1;
+        }
+    }
+    return rc;
+}
+
+/* Establishes a modbus TCP connection with a Modbus server. */
+static int _modbus_tcp_connect(modbus_t *ctx)
+{
+    int rc;
+    /* Specialized version of sockaddr for Internet socket address (same size) */
+    struct sockaddr_in addr;
+    modbus_tcp_t *ctx_tcp = ctx->backend_data;
+    int flags = SOCK_STREAM;
+
+#ifdef OS_WIN32
+    if (_modbus_tcp_init_win32() == -1) {
+        return -1;
+    }
+#endif
+
+#ifdef SOCK_CLOEXEC
+    flags |= SOCK_CLOEXEC;
+#endif
+
+#ifdef SOCK_NONBLOCK
+    flags |= SOCK_NONBLOCK;
+#endif
+
+    ctx->s = socket(PF_INET, flags, 0);
+    if (ctx->s < 0) {
+        return -1;
+    }
+
+    rc = _modbus_tcp_set_ipv4_options(ctx->s);
+    if (rc == -1) {
+        close(ctx->s);
+        ctx->s = -1;
+        return -1;
+    }
+
+    if (ctx->debug) {
+        printf("Connecting to %s:%d\n", ctx_tcp->ip, ctx_tcp->port);
+    }
+
+    addr.sin_family = AF_INET;
+    addr.sin_port = htons(ctx_tcp->port);
+    rc = inet_pton(addr.sin_family, ctx_tcp->ip, &(addr.sin_addr));
+    if (rc <= 0) {
+        if (ctx->debug) {
+            fprintf(stderr, "Invalid IP address: %s\n", ctx_tcp->ip);
+        }
+        close(ctx->s);
+        ctx->s = -1;
+        return -1;
+    }
+
+    rc =
+        _connect(ctx->s, (struct sockaddr *) &addr, sizeof(addr), &ctx->response_timeout);
+    if (rc == -1) {
+        close(ctx->s);
+        ctx->s = -1;
+        return -1;
+    }
+
+    return 0;
+}
+
+/* Establishes a modbus TCP PI connection with a Modbus server. */
+static int _modbus_tcp_pi_connect(modbus_t *ctx)
+{
+    int rc;
+    struct addrinfo *ai_list;
+    struct addrinfo *ai_ptr;
+    struct addrinfo ai_hints;
+    modbus_tcp_pi_t *ctx_tcp_pi = ctx->backend_data;
+
+#ifdef OS_WIN32
+    if (_modbus_tcp_init_win32() == -1) {
+        return -1;
+    }
+#endif
+
+    memset(&ai_hints, 0, sizeof(ai_hints));
+#ifdef AI_ADDRCONFIG
+    ai_hints.ai_flags |= AI_ADDRCONFIG;
+#endif
+    ai_hints.ai_family = AF_UNSPEC;
+    ai_hints.ai_socktype = SOCK_STREAM;
+    ai_hints.ai_addr = NULL;
+    ai_hints.ai_canonname = NULL;
+    ai_hints.ai_next = NULL;
+
+    ai_list = NULL;
+    rc = getaddrinfo(ctx_tcp_pi->node, ctx_tcp_pi->service, &ai_hints, &ai_list);
+    if (rc != 0) {
+        if (ctx->debug) {
+            fprintf(stderr, "Error returned by getaddrinfo: %s\n", gai_strerror(rc));
+        }
+        errno = ECONNREFUSED;
+        return -1;
+    }
+
+    for (ai_ptr = ai_list; ai_ptr != NULL; ai_ptr = ai_ptr->ai_next) {
+        int flags = ai_ptr->ai_socktype;
+        int s;
+
+#ifdef SOCK_CLOEXEC
+        flags |= SOCK_CLOEXEC;
+#endif
+
+#ifdef SOCK_NONBLOCK
+        flags |= SOCK_NONBLOCK;
+#endif
+
+        s = socket(ai_ptr->ai_family, flags, ai_ptr->ai_protocol);
+        if (s < 0)
+            continue;
+
+        if (ai_ptr->ai_family == AF_INET)
+            _modbus_tcp_set_ipv4_options(s);
+
+        if (ctx->debug) {
+            printf("Connecting to [%s]:%s\n", ctx_tcp_pi->node, ctx_tcp_pi->service);
+        }
+
+        rc = _connect(s, ai_ptr->ai_addr, ai_ptr->ai_addrlen, &ctx->response_timeout);
+        if (rc == -1) {
+            close(s);
+            continue;
+        }
+
+        ctx->s = s;
+        break;
+    }
+
+    freeaddrinfo(ai_list);
+
+    if (ctx->s < 0) {
+        return -1;
+    }
+
+    return 0;
+}
+
+static unsigned int _modbus_tcp_is_connected(modbus_t *ctx)
+{
+    return ctx->s >= 0;
+}
+
+/* Closes the network connection and socket in TCP mode */
+static void _modbus_tcp_close(modbus_t *ctx)
+{
+    if (ctx->s >= 0) {
+        shutdown(ctx->s, SHUT_RDWR);
+        close(ctx->s);
+        ctx->s = -1;
+    }
+}
+
+static int _modbus_tcp_flush(modbus_t *ctx)
+{
+    int rc;
+    int rc_sum = 0;
+
+    do {
+        /* Extract the garbage from the socket */
+        char devnull[MODBUS_TCP_MAX_ADU_LENGTH];
+#ifndef OS_WIN32
+        rc = recv(ctx->s, devnull, MODBUS_TCP_MAX_ADU_LENGTH, MSG_DONTWAIT);
+#else
+        /* On Win32, it's a bit more complicated to not wait */
+        fd_set rset;
+        struct timeval tv;
+
+        tv.tv_sec = 0;
+        tv.tv_usec = 0;
+        FD_ZERO(&rset);
+        FD_SET(ctx->s, &rset);
+        rc = select(ctx->s + 1, &rset, NULL, NULL, &tv);
+        if (rc == -1) {
+            return -1;
+        }
+
+        if (rc == 1) {
+            /* There is data to flush */
+            rc = recv(ctx->s, devnull, MODBUS_TCP_MAX_ADU_LENGTH, 0);
+        }
+#endif
+        if (rc > 0) {
+            rc_sum += rc;
+        }
+    } while (rc == MODBUS_TCP_MAX_ADU_LENGTH);
+
+    return rc_sum;
+}
+
+/* Listens for any request from one or many modbus masters in TCP */
+int modbus_tcp_listen(modbus_t *ctx, int nb_connection)
+{
+    int new_s;
+    int enable;
+    int flags;
+    struct sockaddr_in addr;
+    modbus_tcp_t *ctx_tcp;
+    int rc;
+
+    if (ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    ctx_tcp = ctx->backend_data;
+
+#ifdef OS_WIN32
+    if (_modbus_tcp_init_win32() == -1) {
+        return -1;
+    }
+#endif
+
+    flags = SOCK_STREAM;
+
+#ifdef SOCK_CLOEXEC
+    flags |= SOCK_CLOEXEC;
+#endif
+
+    new_s = socket(PF_INET, flags, IPPROTO_TCP);
+    if (new_s == -1) {
+        return -1;
+    }
+
+    enable = 1;
+    if (setsockopt(new_s, SOL_SOCKET, SO_REUSEADDR, (char *) &enable, sizeof(enable)) ==
+        -1) {
+        close(new_s);
+        return -1;
+    }
+
+    memset(&addr, 0, sizeof(addr));
+    addr.sin_family = AF_INET;
+    /* If the modbus port is < to 1024, we need the setuid root. */
+    addr.sin_port = htons(ctx_tcp->port);
+    if (ctx_tcp->ip[0] == '0') {
+        /* Listen any addresses */
+        addr.sin_addr.s_addr = htonl(INADDR_ANY);
+    } else {
+        /* Listen only specified IP address */
+        rc = inet_pton(addr.sin_family, ctx_tcp->ip, &(addr.sin_addr));
+        if (rc <= 0) {
+            if (ctx->debug) {
+                fprintf(stderr, "Invalid IP address: %s\n", ctx_tcp->ip);
+            }
+            close(new_s);
+            return -1;
+        }
+    }
+
+    if (bind(new_s, (struct sockaddr *) &addr, sizeof(addr)) == -1) {
+        close(new_s);
+        return -1;
+    }
+
+    if (listen(new_s, nb_connection) == -1) {
+        close(new_s);
+        return -1;
+    }
+
+    return new_s;
+}
+
+int modbus_tcp_pi_listen(modbus_t *ctx, int nb_connection)
+{
+    int rc;
+    struct addrinfo *ai_list;
+    struct addrinfo *ai_ptr;
+    struct addrinfo ai_hints;
+    const char *node;
+    const char *service;
+    int new_s;
+    modbus_tcp_pi_t *ctx_tcp_pi;
+
+    if (ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    ctx_tcp_pi = ctx->backend_data;
+
+#ifdef OS_WIN32
+    if (_modbus_tcp_init_win32() == -1) {
+        return -1;
+    }
+#endif
+
+    if (ctx_tcp_pi->node[0] == 0) {
+        node = NULL; /* == any */
+    } else {
+        node = ctx_tcp_pi->node;
+    }
+
+    if (ctx_tcp_pi->service[0] == 0) {
+        service = "502";
+    } else {
+        service = ctx_tcp_pi->service;
+    }
+
+    memset(&ai_hints, 0, sizeof(ai_hints));
+    /* If node is not NULL, than the AI_PASSIVE flag is ignored. */
+    ai_hints.ai_flags |= AI_PASSIVE;
+#ifdef AI_ADDRCONFIG
+    ai_hints.ai_flags |= AI_ADDRCONFIG;
+#endif
+    ai_hints.ai_family = AF_UNSPEC;
+    ai_hints.ai_socktype = SOCK_STREAM;
+    ai_hints.ai_addr = NULL;
+    ai_hints.ai_canonname = NULL;
+    ai_hints.ai_next = NULL;
+
+    ai_list = NULL;
+    rc = getaddrinfo(node, service, &ai_hints, &ai_list);
+    if (rc != 0) {
+        if (ctx->debug) {
+            fprintf(stderr, "Error returned by getaddrinfo: %s\n", gai_strerror(rc));
+        }
+        errno = ECONNREFUSED;
+        return -1;
+    }
+
+    new_s = -1;
+    for (ai_ptr = ai_list; ai_ptr != NULL; ai_ptr = ai_ptr->ai_next) {
+        int flags = ai_ptr->ai_socktype;
+        int s;
+
+#ifdef SOCK_CLOEXEC
+        flags |= SOCK_CLOEXEC;
+#endif
+
+        s = socket(ai_ptr->ai_family, flags, ai_ptr->ai_protocol);
+        if (s < 0) {
+            if (ctx->debug) {
+                perror("socket");
+            }
+            continue;
+        } else {
+            int enable = 1;
+            rc =
+                setsockopt(s, SOL_SOCKET, SO_REUSEADDR, (void *) &enable, sizeof(enable));
+            if (rc != 0) {
+                close(s);
+                if (ctx->debug) {
+                    perror("setsockopt");
+                }
+                continue;
+            }
+        }
+
+        rc = bind(s, ai_ptr->ai_addr, ai_ptr->ai_addrlen);
+        if (rc != 0) {
+            close(s);
+            if (ctx->debug) {
+                perror("bind");
+            }
+            continue;
+        }
+
+        rc = listen(s, nb_connection);
+        if (rc != 0) {
+            close(s);
+            if (ctx->debug) {
+                perror("listen");
+            }
+            continue;
+        }
+
+        new_s = s;
+        break;
+    }
+    freeaddrinfo(ai_list);
+
+    if (new_s < 0) {
+        return -1;
+    }
+
+    return new_s;
+}
+
+int modbus_tcp_accept(modbus_t *ctx, int *s)
+{
+    struct sockaddr_in addr;
+    socklen_t addrlen;
+
+    if (ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    addrlen = sizeof(addr);
+#ifdef HAVE_ACCEPT4
+    /* Inherit socket flags and use accept4 call */
+    ctx->s = accept4(*s, (struct sockaddr *) &addr, &addrlen, SOCK_CLOEXEC);
+#else
+    ctx->s = accept(*s, (struct sockaddr *) &addr, &addrlen);
+#endif
+
+    if (ctx->s < 0) {
+        return -1;
+    }
+
+    if (ctx->debug) {
+        char buf[INET_ADDRSTRLEN];
+        if (inet_ntop(AF_INET, &(addr.sin_addr), buf, INET_ADDRSTRLEN) == NULL) {
+            fprintf(stderr, "Client connection accepted from unparsable IP.\n");
+        } else {
+            printf("Client connection accepted from %s.\n", buf);
+        }
+    }
+
+    return ctx->s;
+}
+
+int modbus_tcp_pi_accept(modbus_t *ctx, int *s)
+{
+    struct sockaddr_in6 addr;
+    socklen_t addrlen;
+
+    if (ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    addrlen = sizeof(addr);
+#ifdef HAVE_ACCEPT4
+    /* Inherit socket flags and use accept4 call */
+    ctx->s = accept4(*s, (struct sockaddr *) &addr, &addrlen, SOCK_CLOEXEC);
+#else
+    ctx->s = accept(*s, (struct sockaddr *) &addr, &addrlen);
+#endif
+
+    if (ctx->s < 0) {
+        return -1;
+    }
+
+    if (ctx->debug) {
+        char buf[INET6_ADDRSTRLEN];
+        if (inet_ntop(AF_INET6, &(addr.sin6_addr), buf, INET6_ADDRSTRLEN) == NULL) {
+            fprintf(stderr, "Client connection accepted from unparsable IP.\n");
+        } else {
+            printf("Client connection accepted from %s.\n", buf);
+        }
+    }
+
+    return ctx->s;
+}
+
+static int
+_modbus_tcp_select(modbus_t *ctx, fd_set *rset, struct timeval *tv, int length_to_read)
+{
+    int s_rc;
+    while ((s_rc = select(ctx->s + 1, rset, NULL, NULL, tv)) == -1) {
+        if (errno == EINTR) {
+            if (ctx->debug) {
+                fprintf(stderr, "A non blocked signal was caught\n");
+            }
+            /* Necessary after an error */
+            FD_ZERO(rset);
+            FD_SET(ctx->s, rset);
+        } else {
+            return -1;
+        }
+    }
+
+    if (s_rc == 0) {
+        errno = ETIMEDOUT;
+        return -1;
+    }
+
+    return s_rc;
+}
+
+static void _modbus_tcp_free(modbus_t *ctx)
+{
+    if (ctx->backend_data) {
+        free(ctx->backend_data);
+    }
+    free(ctx);
+}
+
+static void _modbus_tcp_pi_free(modbus_t *ctx)
+{
+    if (ctx->backend_data) {
+        modbus_tcp_pi_t *ctx_tcp_pi = ctx->backend_data;
+        free(ctx_tcp_pi->node);
+        free(ctx_tcp_pi->service);
+        free(ctx->backend_data);
+    }
+
+    free(ctx);
+}
+
+// clang-format off
+const modbus_backend_t _modbus_tcp_backend = {
+    _MODBUS_BACKEND_TYPE_TCP,
+    _MODBUS_TCP_HEADER_LENGTH,
+    _MODBUS_TCP_CHECKSUM_LENGTH,
+    MODBUS_TCP_MAX_ADU_LENGTH,
+    _modbus_set_slave,
+    _modbus_tcp_build_request_basis,
+    _modbus_tcp_build_response_basis,
+    _modbus_tcp_prepare_response_tid,
+    _modbus_tcp_send_msg_pre,
+    _modbus_tcp_send,
+    _modbus_tcp_receive,
+    _modbus_tcp_recv,
+    _modbus_tcp_check_integrity,
+    _modbus_tcp_pre_check_confirmation,
+    _modbus_tcp_connect,
+    _modbus_tcp_is_connected,
+    _modbus_tcp_close,
+    _modbus_tcp_flush,
+    _modbus_tcp_select,
+    _modbus_tcp_free
+};
+
+const modbus_backend_t _modbus_tcp_pi_backend = {
+    _MODBUS_BACKEND_TYPE_TCP,
+    _MODBUS_TCP_HEADER_LENGTH,
+    _MODBUS_TCP_CHECKSUM_LENGTH,
+    MODBUS_TCP_MAX_ADU_LENGTH,
+    _modbus_set_slave,
+    _modbus_tcp_build_request_basis,
+    _modbus_tcp_build_response_basis,
+    _modbus_tcp_prepare_response_tid,
+    _modbus_tcp_send_msg_pre,
+    _modbus_tcp_send,
+    _modbus_tcp_receive,
+    _modbus_tcp_recv,
+    _modbus_tcp_check_integrity,
+    _modbus_tcp_pre_check_confirmation,
+    _modbus_tcp_pi_connect,
+    _modbus_tcp_is_connected,
+    _modbus_tcp_close,
+    _modbus_tcp_flush,
+    _modbus_tcp_select,
+    _modbus_tcp_pi_free
+};
+
+// clang-format on
+
+modbus_t *modbus_new_tcp(const char *ip, int port)
+{
+    modbus_t *ctx;
+    modbus_tcp_t *ctx_tcp;
+    size_t dest_size;
+    size_t ret_size;
+
+#if defined(OS_BSD)
+    /* MSG_NOSIGNAL is unsupported on *BSD so we install an ignore
+       handler for SIGPIPE. */
+    struct sigaction sa;
+
+    sa.sa_handler = SIG_IGN;
+    if (sigaction(SIGPIPE, &sa, NULL) < 0) {
+        /* The debug flag can't be set here... */
+        fprintf(stderr, "Could not install SIGPIPE handler.\n");
+        return NULL;
+    }
+#endif
+
+    ctx = (modbus_t *) malloc(sizeof(modbus_t));
+    if (ctx == NULL) {
+        return NULL;
+    }
+    _modbus_init_common(ctx);
+
+    /* Could be changed after to reach a remote serial Modbus device */
+    ctx->slave = MODBUS_TCP_SLAVE;
+
+    ctx->backend = &_modbus_tcp_backend;
+
+    ctx->backend_data = (modbus_tcp_t *) malloc(sizeof(modbus_tcp_t));
+    if (ctx->backend_data == NULL) {
+        modbus_free(ctx);
+        errno = ENOMEM;
+        return NULL;
+    }
+    ctx_tcp = (modbus_tcp_t *) ctx->backend_data;
+
+    if (ip != NULL) {
+        dest_size = sizeof(char) * 16;
+        ret_size = strlcpy(ctx_tcp->ip, ip, dest_size);
+        if (ret_size == 0) {
+            fprintf(stderr, "The IP string is empty\n");
+            modbus_free(ctx);
+            errno = EINVAL;
+            return NULL;
+        }
+
+        if (ret_size >= dest_size) {
+            fprintf(stderr, "The IP string has been truncated\n");
+            modbus_free(ctx);
+            errno = EINVAL;
+            return NULL;
+        }
+    } else {
+        ctx_tcp->ip[0] = '0';
+    }
+    ctx_tcp->port = port;
+    ctx_tcp->t_id = 0;
+
+    return ctx;
+}
+
+modbus_t *modbus_new_tcp_pi(const char *node, const char *service)
+{
+    modbus_t *ctx;
+    modbus_tcp_pi_t *ctx_tcp_pi;
+
+    ctx = (modbus_t *) malloc(sizeof(modbus_t));
+    if (ctx == NULL) {
+        return NULL;
+    }
+    _modbus_init_common(ctx);
+
+    /* Could be changed after to reach a remote serial Modbus device */
+    ctx->slave = MODBUS_TCP_SLAVE;
+
+    ctx->backend = &_modbus_tcp_pi_backend;
+
+    ctx->backend_data = (modbus_tcp_pi_t *) malloc(sizeof(modbus_tcp_pi_t));
+    if (ctx->backend_data == NULL) {
+        modbus_free(ctx);
+        errno = ENOMEM;
+        return NULL;
+    }
+    ctx_tcp_pi = (modbus_tcp_pi_t *) ctx->backend_data;
+    ctx_tcp_pi->node = NULL;
+    ctx_tcp_pi->service = NULL;
+
+    if (node != NULL) {
+        ctx_tcp_pi->node = strdup(node);
+    } else {
+        /* The node argument can be empty to indicate any hosts */
+        ctx_tcp_pi->node = strdup("");
+    }
+
+    if (ctx_tcp_pi->node == NULL) {
+        modbus_free(ctx);
+        errno = ENOMEM;
+        return NULL;
+    }
+
+    if (service != NULL && service[0] != '\0') {
+        ctx_tcp_pi->service = strdup(service);
+    } else {
+        /* Default Modbus port number */
+        ctx_tcp_pi->service = strdup("502");
+    }
+
+    if (ctx_tcp_pi->service == NULL) {
+        modbus_free(ctx);
+        errno = ENOMEM;
+        return NULL;
+    }
+
+    ctx_tcp_pi->t_id = 0;
+
+    return ctx;
+}

auto_drive_proj_utils/LibModbus/modbus-tcp.h

@@ -0,0 +1,52 @@
+/*
+ * Copyright © Stéphane Raimbault <stephane.raimbault@gmail.com>
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later
+ */
+
+#ifndef MODBUS_TCP_H
+#define MODBUS_TCP_H
+
+#include "modbus.h"
+
+MODBUS_BEGIN_DECLS
+
+#if defined(_WIN32) && !defined(__CYGWIN__)
+/* Win32 with MinGW, supplement to <errno.h> */
+#include <winsock2.h>
+#if !defined(ECONNRESET)
+#define ECONNRESET WSAECONNRESET
+#endif
+#if !defined(ECONNREFUSED)
+#define ECONNREFUSED WSAECONNREFUSED
+#endif
+#if !defined(ETIMEDOUT)
+#define ETIMEDOUT WSAETIMEDOUT
+#endif
+#if !defined(ENOPROTOOPT)
+#define ENOPROTOOPT WSAENOPROTOOPT
+#endif
+#if !defined(EINPROGRESS)
+#define EINPROGRESS WSAEINPROGRESS
+#endif
+#endif
+
+#define MODBUS_TCP_DEFAULT_PORT 502
+#define MODBUS_TCP_SLAVE        0xFF
+
+/* Modbus_Application_Protocol_V1_1b.pdf Chapter 4 Section 1 Page 5
+ * TCP MODBUS ADU = 253 bytes + MBAP (7 bytes) = 260 bytes
+ */
+#define MODBUS_TCP_MAX_ADU_LENGTH 260
+
+MODBUS_API modbus_t *modbus_new_tcp(const char *ip_address, int port);
+MODBUS_API int modbus_tcp_listen(modbus_t *ctx, int nb_connection);
+MODBUS_API int modbus_tcp_accept(modbus_t *ctx, int *s);
+
+MODBUS_API modbus_t *modbus_new_tcp_pi(const char *node, const char *service);
+MODBUS_API int modbus_tcp_pi_listen(modbus_t *ctx, int nb_connection);
+MODBUS_API int modbus_tcp_pi_accept(modbus_t *ctx, int *s);
+
+MODBUS_END_DECLS
+
+#endif /* MODBUS_TCP_H */

auto_drive_proj_utils/LibModbus/modbus-version.h

@@ -0,0 +1,51 @@
+/*
+ * Copyright © Stéphane Raimbault <stephane.raimbault@gmail.com>
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+ */
+
+#ifndef MODBUS_VERSION_H
+#define MODBUS_VERSION_H
+
+/* The major version, (1, if %LIBMODBUS_VERSION is 1.2.3) */
+#define LIBMODBUS_VERSION_MAJOR (@LIBMODBUS_VERSION_MAJOR@)
+
+/* The minor version (2, if %LIBMODBUS_VERSION is 1.2.3) */
+#define LIBMODBUS_VERSION_MINOR (@LIBMODBUS_VERSION_MINOR@)
+
+/* The micro version (3, if %LIBMODBUS_VERSION is 1.2.3) */
+#define LIBMODBUS_VERSION_MICRO (@LIBMODBUS_VERSION_MICRO@)
+
+/* The full version, like 1.2.3 */
+#define LIBMODBUS_VERSION @LIBMODBUS_VERSION@
+
+/* The full version, in string form (suited for string concatenation)
+ */
+#define LIBMODBUS_VERSION_STRING "@LIBMODBUS_VERSION@"
+
+/* Numerically encoded version, eg. v1.2.3 is 0x010203 */
+#define LIBMODBUS_VERSION_HEX                                                                      \
+    ((LIBMODBUS_VERSION_MAJOR << 16) | (LIBMODBUS_VERSION_MINOR << 8) |                            \
+     (LIBMODBUS_VERSION_MICRO << 0))
+
+/* Evaluates to True if the version is greater than @major, @minor and @micro
+ */
+#define LIBMODBUS_VERSION_CHECK(major, minor, micro)                                               \
+    (LIBMODBUS_VERSION_MAJOR > (major) ||                                                          \
+     (LIBMODBUS_VERSION_MAJOR == (major) && LIBMODBUS_VERSION_MINOR > (minor)) ||                  \
+     (LIBMODBUS_VERSION_MAJOR == (major) && LIBMODBUS_VERSION_MINOR == (minor) &&                  \
+      LIBMODBUS_VERSION_MICRO >= (micro)))
+
+#endif /* MODBUS_VERSION_H */

auto_drive_proj_utils/LibModbus/modbus.h

@@ -0,0 +1,329 @@
+/*
+ * Copyright © Stéphane Raimbault <stephane.raimbault@gmail.com>
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later
+ */
+
+#ifndef MODBUS_H
+#define MODBUS_H
+
+// clang-format off
+/* Add this for macros that defined unix flavor */
+#if (defined(__unix__) || defined(unix)) && !defined(USG)
+# include <sys/param.h>
+#endif
+
+#ifndef _MSC_VER
+# include <stdint.h>
+#else
+# include "stdint.h"
+#endif
+
+#include "modbus-version.h"
+
+#if defined(_MSC_VER)
+# if defined(DLLBUILD)
+/* define DLLBUILD when building the DLL */
+#  define MODBUS_API __declspec(dllexport)
+# else
+#  define MODBUS_API __declspec(dllimport)
+# endif
+#else
+# define MODBUS_API
+#endif
+
+#ifdef  __cplusplus
+# define MODBUS_BEGIN_DECLS  extern "C" {
+# define MODBUS_END_DECLS    }
+#else
+# define MODBUS_BEGIN_DECLS
+# define MODBUS_END_DECLS
+#endif
+// clang-format on
+
+MODBUS_BEGIN_DECLS
+
+#ifndef FALSE
+#define FALSE 0
+#endif
+
+#ifndef TRUE
+#define TRUE 1
+#endif
+
+#ifndef OFF
+#define OFF 0
+#endif
+
+#ifndef ON
+#define ON 1
+#endif
+
+/* Modbus function codes */
+#define MODBUS_FC_READ_COILS               0x01
+#define MODBUS_FC_READ_DISCRETE_INPUTS     0x02
+#define MODBUS_FC_READ_HOLDING_REGISTERS   0x03
+#define MODBUS_FC_READ_INPUT_REGISTERS     0x04
+#define MODBUS_FC_WRITE_SINGLE_COIL        0x05
+#define MODBUS_FC_WRITE_SINGLE_REGISTER    0x06
+#define MODBUS_FC_READ_EXCEPTION_STATUS    0x07
+#define MODBUS_FC_WRITE_MULTIPLE_COILS     0x0F
+#define MODBUS_FC_WRITE_MULTIPLE_REGISTERS 0x10
+#define MODBUS_FC_REPORT_SLAVE_ID          0x11
+#define MODBUS_FC_MASK_WRITE_REGISTER      0x16
+#define MODBUS_FC_WRITE_AND_READ_REGISTERS 0x17
+
+#define MODBUS_BROADCAST_ADDRESS 0
+
+/* Modbus_Application_Protocol_V1_1b.pdf (chapter 6 section 1 page 12)
+ * Quantity of Coils to read (2 bytes): 1 to 2000 (0x7D0)
+ * (chapter 6 section 11 page 29)
+ * Quantity of Coils to write (2 bytes): 1 to 1968 (0x7B0)
+ */
+#define MODBUS_MAX_READ_BITS  2000
+#define MODBUS_MAX_WRITE_BITS 1968
+
+/* Modbus_Application_Protocol_V1_1b.pdf (chapter 6 section 3 page 15)
+ * Quantity of Registers to read (2 bytes): 1 to 125 (0x7D)
+ * (chapter 6 section 12 page 31)
+ * Quantity of Registers to write (2 bytes) 1 to 123 (0x7B)
+ * (chapter 6 section 17 page 38)
+ * Quantity of Registers to write in R/W registers (2 bytes) 1 to 121 (0x79)
+ */
+#define MODBUS_MAX_READ_REGISTERS     125
+#define MODBUS_MAX_WRITE_REGISTERS    123
+#define MODBUS_MAX_WR_WRITE_REGISTERS 121
+#define MODBUS_MAX_WR_READ_REGISTERS  125
+
+/* The size of the MODBUS PDU is limited by the size constraint inherited from
+ * the first MODBUS implementation on Serial Line network (max. RS485 ADU = 256
+ * bytes). Therefore, MODBUS PDU for serial line communication = 256 - Server
+ * address (1 byte) - CRC (2 bytes) = 253 bytes.
+ */
+#define MODBUS_MAX_PDU_LENGTH 253
+
+/* Consequently:
+ * - RTU MODBUS ADU = 253 bytes + Server address (1 byte) + CRC (2 bytes) = 256
+ *   bytes.
+ * - TCP MODBUS ADU = 253 bytes + MBAP (7 bytes) = 260 bytes.
+ * so the maximum of both backend in 260 bytes. This size can used to allocate
+ * an array of bytes to store responses and it will be compatible with the two
+ * backends.
+ */
+#define MODBUS_MAX_ADU_LENGTH 260
+
+/* Random number to avoid errno conflicts */
+#define MODBUS_ENOBASE 112345678
+
+/* Protocol exceptions */
+enum {
+    MODBUS_EXCEPTION_ILLEGAL_FUNCTION = 0x01,
+    MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS,
+    MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE,
+    MODBUS_EXCEPTION_SLAVE_OR_SERVER_FAILURE,
+    MODBUS_EXCEPTION_ACKNOWLEDGE,
+    MODBUS_EXCEPTION_SLAVE_OR_SERVER_BUSY,
+    MODBUS_EXCEPTION_NEGATIVE_ACKNOWLEDGE,
+    MODBUS_EXCEPTION_MEMORY_PARITY,
+    MODBUS_EXCEPTION_NOT_DEFINED,
+    MODBUS_EXCEPTION_GATEWAY_PATH,
+    MODBUS_EXCEPTION_GATEWAY_TARGET,
+    MODBUS_EXCEPTION_MAX
+};
+
+#define EMBXILFUN  (MODBUS_ENOBASE + MODBUS_EXCEPTION_ILLEGAL_FUNCTION)
+#define EMBXILADD  (MODBUS_ENOBASE + MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS)
+#define EMBXILVAL  (MODBUS_ENOBASE + MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE)
+#define EMBXSFAIL  (MODBUS_ENOBASE + MODBUS_EXCEPTION_SLAVE_OR_SERVER_FAILURE)
+#define EMBXACK    (MODBUS_ENOBASE + MODBUS_EXCEPTION_ACKNOWLEDGE)
+#define EMBXSBUSY  (MODBUS_ENOBASE + MODBUS_EXCEPTION_SLAVE_OR_SERVER_BUSY)
+#define EMBXNACK   (MODBUS_ENOBASE + MODBUS_EXCEPTION_NEGATIVE_ACKNOWLEDGE)
+#define EMBXMEMPAR (MODBUS_ENOBASE + MODBUS_EXCEPTION_MEMORY_PARITY)
+#define EMBXGPATH  (MODBUS_ENOBASE + MODBUS_EXCEPTION_GATEWAY_PATH)
+#define EMBXGTAR   (MODBUS_ENOBASE + MODBUS_EXCEPTION_GATEWAY_TARGET)
+
+/* Native libmodbus error codes */
+#define EMBBADCRC   (EMBXGTAR + 1)
+#define EMBBADDATA  (EMBXGTAR + 2)
+#define EMBBADEXC   (EMBXGTAR + 3)
+#define EMBUNKEXC   (EMBXGTAR + 4)
+#define EMBMDATA    (EMBXGTAR + 5)
+#define EMBBADSLAVE (EMBXGTAR + 6)
+
+extern const unsigned int libmodbus_version_major;
+extern const unsigned int libmodbus_version_minor;
+extern const unsigned int libmodbus_version_micro;
+
+typedef struct _modbus modbus_t;
+
+typedef struct _modbus_mapping_t {
+    int nb_bits;
+    int start_bits;
+    int nb_input_bits;
+    int start_input_bits;
+    int nb_input_registers;
+    int start_input_registers;
+    int nb_registers;
+    int start_registers;
+    uint8_t *tab_bits;
+    uint8_t *tab_input_bits;
+    uint16_t *tab_input_registers;
+    uint16_t *tab_registers;
+} modbus_mapping_t;
+
+typedef enum {
+    MODBUS_ERROR_RECOVERY_NONE = 0,
+    MODBUS_ERROR_RECOVERY_LINK = (1 << 1),
+    MODBUS_ERROR_RECOVERY_PROTOCOL = (1 << 2)
+} modbus_error_recovery_mode;
+
+typedef enum {
+    MODBUS_QUIRK_NONE = 0,
+    MODBUS_QUIRK_MAX_SLAVE = (1 << 1),
+    MODBUS_QUIRK_REPLY_TO_BROADCAST = (1 << 2),
+    MODBUS_QUIRK_ALL = 0xFF
+} modbus_quirks;
+
+MODBUS_API int modbus_set_slave(modbus_t *ctx, int slave);
+MODBUS_API int modbus_get_slave(modbus_t *ctx);
+MODBUS_API int modbus_set_error_recovery(modbus_t *ctx,
+                                         modbus_error_recovery_mode error_recovery);
+MODBUS_API int modbus_set_socket(modbus_t *ctx, int s);
+MODBUS_API int modbus_get_socket(modbus_t *ctx);
+
+MODBUS_API int
+modbus_get_response_timeout(modbus_t *ctx, uint32_t *to_sec, uint32_t *to_usec);
+MODBUS_API int
+modbus_set_response_timeout(modbus_t *ctx, uint32_t to_sec, uint32_t to_usec);
+
+MODBUS_API int
+modbus_get_byte_timeout(modbus_t *ctx, uint32_t *to_sec, uint32_t *to_usec);
+MODBUS_API int modbus_set_byte_timeout(modbus_t *ctx, uint32_t to_sec, uint32_t to_usec);
+
+MODBUS_API int
+modbus_get_indication_timeout(modbus_t *ctx, uint32_t *to_sec, uint32_t *to_usec);
+MODBUS_API int
+modbus_set_indication_timeout(modbus_t *ctx, uint32_t to_sec, uint32_t to_usec);
+
+MODBUS_API int modbus_get_header_length(modbus_t *ctx);
+
+MODBUS_API int modbus_connect(modbus_t *ctx);
+MODBUS_API void modbus_close(modbus_t *ctx);
+
+MODBUS_API void modbus_free(modbus_t *ctx);
+
+MODBUS_API int modbus_flush(modbus_t *ctx);
+MODBUS_API int modbus_set_debug(modbus_t *ctx, int flag);
+
+MODBUS_API const char *modbus_strerror(int errnum);
+
+MODBUS_API int modbus_read_bits(modbus_t *ctx, int addr, int nb, uint8_t *dest);
+MODBUS_API int modbus_read_input_bits(modbus_t *ctx, int addr, int nb, uint8_t *dest);
+MODBUS_API int modbus_read_registers(modbus_t *ctx, int addr, int nb, uint16_t *dest);
+MODBUS_API int
+modbus_read_input_registers(modbus_t *ctx, int addr, int nb, uint16_t *dest);
+MODBUS_API int modbus_write_bit(modbus_t *ctx, int coil_addr, int status);
+MODBUS_API int modbus_write_register(modbus_t *ctx, int reg_addr, const uint16_t value);
+MODBUS_API int modbus_write_bits(modbus_t *ctx, int addr, int nb, const uint8_t *data);
+MODBUS_API int
+modbus_write_registers(modbus_t *ctx, int addr, int nb, const uint16_t *data);
+MODBUS_API int
+modbus_mask_write_register(modbus_t *ctx, int addr, uint16_t and_mask, uint16_t or_mask);
+MODBUS_API int modbus_write_and_read_registers(modbus_t *ctx,
+                                               int write_addr,
+                                               int write_nb,
+                                               const uint16_t *src,
+                                               int read_addr,
+                                               int read_nb,
+                                               uint16_t *dest);
+MODBUS_API int modbus_report_slave_id(modbus_t *ctx, int max_dest, uint8_t *dest);
+
+MODBUS_API modbus_mapping_t *
+modbus_mapping_new_start_address(unsigned int start_bits,
+                                 unsigned int nb_bits,
+                                 unsigned int start_input_bits,
+                                 unsigned int nb_input_bits,
+                                 unsigned int start_registers,
+                                 unsigned int nb_registers,
+                                 unsigned int start_input_registers,
+                                 unsigned int nb_input_registers);
+
+MODBUS_API modbus_mapping_t *modbus_mapping_new(int nb_bits,
+                                                int nb_input_bits,
+                                                int nb_registers,
+                                                int nb_input_registers);
+MODBUS_API void modbus_mapping_free(modbus_mapping_t *mb_mapping);
+
+MODBUS_API int
+modbus_send_raw_request(modbus_t *ctx, const uint8_t *raw_req, int raw_req_length);
+
+MODBUS_API int modbus_receive(modbus_t *ctx, uint8_t *req);
+
+MODBUS_API int modbus_receive_confirmation(modbus_t *ctx, uint8_t *rsp);
+
+MODBUS_API int modbus_reply(modbus_t *ctx,
+                            const uint8_t *req,
+                            int req_length,
+                            modbus_mapping_t *mb_mapping);
+MODBUS_API int
+modbus_reply_exception(modbus_t *ctx, const uint8_t *req, unsigned int exception_code);
+MODBUS_API int modbus_enable_quirks(modbus_t *ctx, unsigned int quirks_mask);
+MODBUS_API int modbus_disable_quirks(modbus_t *ctx, unsigned int quirks_mask);
+
+/**
+ * UTILS FUNCTIONS
+ **/
+
+#define MODBUS_GET_HIGH_BYTE(data) (((data) >> 8) & 0xFF)
+#define MODBUS_GET_LOW_BYTE(data)  ((data) &0xFF)
+#define MODBUS_GET_INT64_FROM_INT16(tab_int16, index)                                \
+  (((int64_t) tab_int16[(index)] << 48) | ((int64_t) tab_int16[(index) + 1] << 32) | \
+   ((int64_t) tab_int16[(index) + 2] << 16) | (int64_t) tab_int16[(index) + 3])
+#define MODBUS_GET_INT32_FROM_INT16(tab_int16, index) \
+  (((int32_t) tab_int16[(index)] << 16) | (int32_t) tab_int16[(index) + 1])
+#define MODBUS_GET_INT16_FROM_INT8(tab_int8, index) \
+  (((int16_t) tab_int8[(index)] << 8) | (int16_t) tab_int8[(index) + 1])
+#define MODBUS_SET_INT16_TO_INT8(tab_int8, index, value)        \
+  do {                                                          \
+    ((int8_t *) (tab_int8))[(index)] = (int8_t) ((value) >> 8); \
+    ((int8_t *) (tab_int8))[(index) + 1] = (int8_t) (value);    \
+  } while (0)
+#define MODBUS_SET_INT32_TO_INT16(tab_int16, index, value)          \
+  do {                                                              \
+    ((int16_t *) (tab_int16))[(index)] = (int16_t) ((value) >> 16); \
+    ((int16_t *) (tab_int16))[(index) + 1] = (int16_t) (value);     \
+  } while (0)
+#define MODBUS_SET_INT64_TO_INT16(tab_int16, index, value)              \
+  do {                                                                  \
+    ((int16_t *) (tab_int16))[(index)] = (int16_t) ((value) >> 48);     \
+    ((int16_t *) (tab_int16))[(index) + 1] = (int16_t) ((value) >> 32); \
+    ((int16_t *) (tab_int16))[(index) + 2] = (int16_t) ((value) >> 16); \
+    ((int16_t *) (tab_int16))[(index) + 3] = (int16_t) (value);         \
+  } while (0)
+
+MODBUS_API void modbus_set_bits_from_byte(uint8_t *dest, int idx, const uint8_t value);
+MODBUS_API void modbus_set_bits_from_bytes(uint8_t *dest,
+                                           int idx,
+                                           unsigned int nb_bits,
+                                           const uint8_t *tab_byte);
+MODBUS_API uint8_t modbus_get_byte_from_bits(const uint8_t *src,
+                                             int idx,
+                                             unsigned int nb_bits);
+MODBUS_API float modbus_get_float(const uint16_t *src);
+MODBUS_API float modbus_get_float_abcd(const uint16_t *src);
+MODBUS_API float modbus_get_float_dcba(const uint16_t *src);
+MODBUS_API float modbus_get_float_badc(const uint16_t *src);
+MODBUS_API float modbus_get_float_cdab(const uint16_t *src);
+
+MODBUS_API void modbus_set_float(float f, uint16_t *dest);
+MODBUS_API void modbus_set_float_abcd(float f, uint16_t *dest);
+MODBUS_API void modbus_set_float_dcba(float f, uint16_t *dest);
+MODBUS_API void modbus_set_float_badc(float f, uint16_t *dest);
+MODBUS_API void modbus_set_float_cdab(float f, uint16_t *dest);
+
+#include "modbus-rtu.h"
+#include "modbus-tcp.h"
+
+MODBUS_END_DECLS
+
+#endif /* MODBUS_H */

auto_drive_proj_utils/MD5/md5.cpp

@@ -0,0 +1,344 @@
+#include "md5.hpp"
+ 
+//using namespace std;
+ 
+/* Constants for MD5Transform routine. */
+#define S11 7
+#define S12 12
+#define S13 17
+#define S14 22
+#define S21 5
+#define S22 9
+#define S23 14
+#define S24 20
+#define S31 4
+#define S32 11
+#define S33 16
+#define S34 23
+#define S41 6
+#define S42 10
+#define S43 15
+#define S44 21
+ 
+ 
+/* F, G, H and I are basic MD5 functions.
+*/
+#define F(x, y, z) (((x) & (y)) | ((~x) & (z)))
+#define G(x, y, z) (((x) & (z)) | ((y) & (~z)))
+#define H(x, y, z) ((x) ^ (y) ^ (z))
+#define I(x, y, z) ((y) ^ ((x) | (~z)))
+ 
+/* ROTATE_LEFT rotates x left n bits.
+*/
+#define ROTATE_LEFT(x, n) (((x) << (n)) | ((x) >> (32-(n))))
+ 
+/* FF, GG, HH, and II transformations for rounds 1, 2, 3, and 4.
+Rotation is separate from addition to prevent recomputation.
+*/
+#define FF(a, b, c, d, x, s, ac) { \
+(a) += F ((b), (c), (d)) + (x) + ac; \
+(a) = ROTATE_LEFT ((a), (s)); \
+(a) += (b); \
+}
+#define GG(a, b, c, d, x, s, ac) { \
+(a) += G ((b), (c), (d)) + (x) + ac; \
+(a) = ROTATE_LEFT ((a), (s)); \
+(a) += (b); \
+}
+#define HH(a, b, c, d, x, s, ac) { \
+(a) += H ((b), (c), (d)) + (x) + ac; \
+(a) = ROTATE_LEFT ((a), (s)); \
+(a) += (b); \
+}
+#define II(a, b, c, d, x, s, ac) { \
+(a) += I ((b), (c), (d)) + (x) + ac; \
+(a) = ROTATE_LEFT ((a), (s)); \
+(a) += (b); \
+}
+ 
+ 
+const byte MD5::PADDING[64] = { 0x80 };
+const char MD5::HEX[16] = {
+'0', '1', '2', '3',
+'4', '5', '6', '7',
+'8', '9', 'a', 'b',
+'c', 'd', 'e', 'f'
+};
+ 
+ 
+/* Default construct. */
+MD5::MD5() {
+reset();
+}
+ 
+/* Construct a MD5 object with a input buffer. */
+MD5::MD5(const void* input, size_t length) {
+reset();
+update(input, length);
+}
+ 
+/* Construct a MD5 object with a string. */
+MD5::MD5(const string& str) {
+reset();
+update(str);
+}
+ 
+/* Construct a MD5 object with a file. */
+MD5::MD5(ifstream& in) {
+reset();
+update(in);
+}
+ 
+/* Return the message-digest */
+const byte* MD5::digest() {
+ 
+if (!_finished) {
+   _finished = true;
+   final();
+}
+return _digest;
+}
+ 
+/* Reset the calculate state */
+void MD5::reset() {
+ 
+_finished = false;
+/* reset number of bits. */
+_count[0] = _count[1] = 0;
+/* Load magic initialization constants. */
+_state[0] = 0x67452301;
+_state[1] = 0xefcdab89;
+_state[2] = 0x98badcfe;
+_state[3] = 0x10325476;
+}
+ 
+/* Updating the context with a input buffer. */
+void MD5::update(const void* input, size_t length) {
+update((const byte*)input, length);
+}
+ 
+/* Updating the context with a string. */
+void MD5::update(const string& str) {
+update((const byte*)str.c_str(), str.length());
+}
+ 
+/* Updating the context with a file. */
+void MD5::update(ifstream& in) {
+ 
+if (!in) {
+   return;
+}
+ 
+std::streamsize length;
+char buffer[BUFFER_SIZE];
+while (!in.eof()) {
+   in.read(buffer, BUFFER_SIZE);
+   length = in.gcount();
+   if (length > 0) {
+    update(buffer, length);
+   }
+}
+in.close();
+}
+ 
+/* MD5 block update operation. Continues an MD5 message-digest
+operation, processing another message block, and updating the
+context.
+*/
+void MD5::update(const byte* input, size_t length) {
+ 
+uint32 i, index, partLen;
+ 
+_finished = false;
+ 
+/* Compute number of bytes mod 64 */
+index = (uint32)((_count[0] >> 3) & 0x3f);
+ 
+/* update number of bits */
+if ((_count[0] += ((uint32)length << 3)) < ((uint32)length << 3)) {
+   ++_count[1];
+}
+_count[1] += ((uint32)length >> 29);
+ 
+partLen = 64 - index;
+ 
+/* transform as many times as possible. */
+if (length >= partLen) {
+ 
+   memcpy(&_buffer[index], input, partLen);
+   transform(_buffer);
+ 
+   for (i = partLen; i + 63 < length; i += 64) {
+    transform(&input[i]);
+   }
+   index = 0;
+ 
+} else {
+   i = 0;
+}
+ 
+/* Buffer remaining input */
+memcpy(&_buffer[index], &input[i], length - i);
+}
+ 
+/* MD5 finalization. Ends an MD5 message-_digest operation, writing the
+the message _digest and zeroizing the context.
+*/
+void MD5::final() {
+ 
+byte bits[8];
+uint32 oldState[4];
+uint32 oldCount[2];
+uint32 index, padLen;
+ 
+/* Save current state and count. */
+memcpy(oldState, _state, 16);
+memcpy(oldCount, _count, 8);
+ 
+/* Save number of bits */
+encode(_count, bits, 8);
+ 
+/* Pad out to 56 mod 64. */
+index = (uint32)((_count[0] >> 3) & 0x3f);
+padLen = (index < 56) ? (56 - index) : (120 - index);
+update(PADDING, padLen);
+ 
+/* Append length (before padding) */
+update(bits, 8);
+ 
+/* Store state in digest */
+encode(_state, _digest, 16);
+ 
+/* Restore current state and count. */
+memcpy(_state, oldState, 16);
+memcpy(_count, oldCount, 8);
+}
+ 
+/* MD5 basic transformation. Transforms _state based on block. */
+void MD5::transform(const byte block[64]) {
+ 
+uint32 a = _state[0], b = _state[1], c = _state[2], d = _state[3], x[16];
+ 
+decode(block, x, 64);
+ 
+/* Round 1 */
+FF (a, b, c, d, x[ 0], S11, 0xd76aa478); /* 1 */
+FF (d, a, b, c, x[ 1], S12, 0xe8c7b756); /* 2 */
+FF (c, d, a, b, x[ 2], S13, 0x242070db); /* 3 */
+FF (b, c, d, a, x[ 3], S14, 0xc1bdceee); /* 4 */
+FF (a, b, c, d, x[ 4], S11, 0xf57c0faf); /* 5 */
+FF (d, a, b, c, x[ 5], S12, 0x4787c62a); /* 6 */
+FF (c, d, a, b, x[ 6], S13, 0xa8304613); /* 7 */
+FF (b, c, d, a, x[ 7], S14, 0xfd469501); /* 8 */
+FF (a, b, c, d, x[ 8], S11, 0x698098d8); /* 9 */
+FF (d, a, b, c, x[ 9], S12, 0x8b44f7af); /* 10 */
+FF (c, d, a, b, x[10], S13, 0xffff5bb1); /* 11 */
+FF (b, c, d, a, x[11], S14, 0x895cd7be); /* 12 */
+FF (a, b, c, d, x[12], S11, 0x6b901122); /* 13 */
+FF (d, a, b, c, x[13], S12, 0xfd987193); /* 14 */
+FF (c, d, a, b, x[14], S13, 0xa679438e); /* 15 */
+FF (b, c, d, a, x[15], S14, 0x49b40821); /* 16 */
+ 
+/* Round 2 */
+GG (a, b, c, d, x[ 1], S21, 0xf61e2562); /* 17 */
+GG (d, a, b, c, x[ 6], S22, 0xc040b340); /* 18 */
+GG (c, d, a, b, x[11], S23, 0x265e5a51); /* 19 */
+GG (b, c, d, a, x[ 0], S24, 0xe9b6c7aa); /* 20 */
+GG (a, b, c, d, x[ 5], S21, 0xd62f105d); /* 21 */
+GG (d, a, b, c, x[10], S22, 0x2441453); /* 22 */
+GG (c, d, a, b, x[15], S23, 0xd8a1e681); /* 23 */
+GG (b, c, d, a, x[ 4], S24, 0xe7d3fbc8); /* 24 */
+GG (a, b, c, d, x[ 9], S21, 0x21e1cde6); /* 25 */
+GG (d, a, b, c, x[14], S22, 0xc33707d6); /* 26 */
+GG (c, d, a, b, x[ 3], S23, 0xf4d50d87); /* 27 */
+GG (b, c, d, a, x[ 8], S24, 0x455a14ed); /* 28 */
+GG (a, b, c, d, x[13], S21, 0xa9e3e905); /* 29 */
+GG (d, a, b, c, x[ 2], S22, 0xfcefa3f8); /* 30 */
+GG (c, d, a, b, x[ 7], S23, 0x676f02d9); /* 31 */
+GG (b, c, d, a, x[12], S24, 0x8d2a4c8a); /* 32 */
+ 
+/* Round 3 */
+HH (a, b, c, d, x[ 5], S31, 0xfffa3942); /* 33 */
+HH (d, a, b, c, x[ 8], S32, 0x8771f681); /* 34 */
+HH (c, d, a, b, x[11], S33, 0x6d9d6122); /* 35 */
+HH (b, c, d, a, x[14], S34, 0xfde5380c); /* 36 */
+HH (a, b, c, d, x[ 1], S31, 0xa4beea44); /* 37 */
+HH (d, a, b, c, x[ 4], S32, 0x4bdecfa9); /* 38 */
+HH (c, d, a, b, x[ 7], S33, 0xf6bb4b60); /* 39 */
+HH (b, c, d, a, x[10], S34, 0xbebfbc70); /* 40 */
+HH (a, b, c, d, x[13], S31, 0x289b7ec6); /* 41 */
+HH (d, a, b, c, x[ 0], S32, 0xeaa127fa); /* 42 */
+HH (c, d, a, b, x[ 3], S33, 0xd4ef3085); /* 43 */
+HH (b, c, d, a, x[ 6], S34, 0x4881d05); /* 44 */
+HH (a, b, c, d, x[ 9], S31, 0xd9d4d039); /* 45 */
+HH (d, a, b, c, x[12], S32, 0xe6db99e5); /* 46 */
+HH (c, d, a, b, x[15], S33, 0x1fa27cf8); /* 47 */
+HH (b, c, d, a, x[ 2], S34, 0xc4ac5665); /* 48 */
+ 
+/* Round 4 */
+II (a, b, c, d, x[ 0], S41, 0xf4292244); /* 49 */
+II (d, a, b, c, x[ 7], S42, 0x432aff97); /* 50 */
+II (c, d, a, b, x[14], S43, 0xab9423a7); /* 51 */
+II (b, c, d, a, x[ 5], S44, 0xfc93a039); /* 52 */
+II (a, b, c, d, x[12], S41, 0x655b59c3); /* 53 */
+II (d, a, b, c, x[ 3], S42, 0x8f0ccc92); /* 54 */
+II (c, d, a, b, x[10], S43, 0xffeff47d); /* 55 */
+II (b, c, d, a, x[ 1], S44, 0x85845dd1); /* 56 */
+II (a, b, c, d, x[ 8], S41, 0x6fa87e4f); /* 57 */
+II (d, a, b, c, x[15], S42, 0xfe2ce6e0); /* 58 */
+II (c, d, a, b, x[ 6], S43, 0xa3014314); /* 59 */
+II (b, c, d, a, x[13], S44, 0x4e0811a1); /* 60 */
+II (a, b, c, d, x[ 4], S41, 0xf7537e82); /* 61 */
+II (d, a, b, c, x[11], S42, 0xbd3af235); /* 62 */
+II (c, d, a, b, x[ 2], S43, 0x2ad7d2bb); /* 63 */
+II (b, c, d, a, x[ 9], S44, 0xeb86d391); /* 64 */
+ 
+_state[0] += a;
+_state[1] += b;
+_state[2] += c;
+_state[3] += d;
+}
+ 
+/* Encodes input (ulong) into output (byte). Assumes length is
+a multiple of 4.
+*/
+void MD5::encode(const uint32* input, byte* output, size_t length) {
+ 
+for (size_t i = 0, j = 0; j < length; ++i, j += 4) {
+   output[j]= (byte)(input[i] & 0xff);
+   output[j + 1] = (byte)((input[i] >> 8) & 0xff);
+   output[j + 2] = (byte)((input[i] >> 16) & 0xff);
+   output[j + 3] = (byte)((input[i] >> 24) & 0xff);
+}
+}
+ 
+/* Decodes input (byte) into output (ulong). Assumes length is
+a multiple of 4.
+*/
+void MD5::decode(const byte* input, uint32* output, size_t length) {
+ 
+for (size_t i = 0, j = 0; j < length; ++i, j += 4) {
+   output[i] = ((uint32)input[j]) | (((uint32)input[j + 1]) << 8) |
+   (((uint32)input[j + 2]) << 16) | (((uint32)input[j + 3]) << 24);
+}
+}
+ 
+/* Convert byte array to hex string. */
+string MD5::bytesToHexString(const byte* input, size_t length) {
+ 
+string str;
+str.reserve(length << 1);
+for (size_t i = 0; i < length; ++i) {
+   int t = input[i];
+   int a = t / 16;
+   int b = t % 16;
+   str.append(1, HEX[a]);
+   str.append(1, HEX[b]);
+}
+return str;
+}
+ 
+/* Convert digest to string value */
+string MD5::toString() {
+return bytesToHexString(digest(), 16);
+}

auto_drive_proj_utils/MD5/md5.hpp

@@ -0,0 +1,58 @@
+#ifndef MD5_HPP
+#define MD5_HPP
+ 
+#include <string>
+#include <cstring>
+#include <fstream>
+ 
+/* Type define */
+//typedef unsigned char byte;
+//typedef unsigned int uint32;
+
+using byte = unsigned char;
+using uint32 = unsigned int;
+
+ 
+using std::string;
+using std::ifstream;
+
+
+/* MD5 declaration. */
+class MD5 {
+public:
+MD5();
+MD5(const void* input, size_t length);
+MD5(const string& str);
+MD5(ifstream& in);
+void update(const void* input, size_t length);
+void update(const string& str);
+void update(ifstream& in);
+const byte* digest();
+string toString();
+void reset();
+ 
+private:
+void update(const byte* input, size_t length);
+void final();
+void transform(const byte block[64]);
+void encode(const uint32* input, byte* output, size_t length);
+void decode(const byte* input, uint32* output, size_t length);
+string bytesToHexString(const byte* input, size_t length);
+ 
+/* class uncopyable */
+MD5(const MD5&);
+MD5& operator=(const MD5&);
+ 
+private:
+uint32 _state[4]; /* state (ABCD) */
+uint32 _count[2]; /* number of bits, modulo 2^64 (low-order word first) */
+byte _buffer[64]; /* input buffer */
+byte _digest[16]; /* message digest */
+bool _finished;   /* calculate finished ? */
+ 
+static const byte PADDING[64]; /* padding for calculate */
+static const char HEX[16];
+enum { BUFFER_SIZE = 1024 };
+};
+ 
+#endif /*MD5_H*/

auto_drive_proj_utils/SerialCommunicaiton/list_ports_linux.cc

@@ -0,0 +1,336 @@
+#if defined(__linux__)
+
+/*
+ * Copyright (c) 2014 Craig Lilley <cralilley@gmail.com>
+ * This software is made available under the terms of the MIT licence.
+ * A copy of the licence can be obtained from:
+ * http://opensource.org/licenses/MIT
+ */
+
+#include <vector>
+#include <string>
+#include <sstream>
+#include <stdexcept>
+#include <iostream>
+#include <fstream>
+#include <cstdio>
+#include <cstdarg>
+#include <cstdlib>
+
+#include <glob.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <unistd.h>
+
+#include "serial.h"
+
+using serial::PortInfo;
+using std::istringstream;
+using std::ifstream;
+using std::getline;
+using std::vector;
+using std::string;
+using std::cout;
+using std::endl;
+
+static vector<string> glob(const vector<string>& patterns);
+static string basename(const string& path);
+static string dirname(const string& path);
+static bool path_exists(const string& path);
+static string realpath(const string& path);
+static string usb_sysfs_friendly_name(const string& sys_usb_path);
+static vector<string> get_sysfs_info(const string& device_path);
+static string read_line(const string& file);
+static string usb_sysfs_hw_string(const string& sysfs_path);
+static string format(const char* format, ...);
+
+vector<string>
+glob(const vector<string>& patterns)
+{
+    vector<string> paths_found;
+
+	if(patterns.size() == 0)
+	    return paths_found;
+
+    glob_t glob_results;
+
+    int glob_retval = glob(patterns[0].c_str(), 0, NULL, &glob_results);
+
+    vector<string>::const_iterator iter = patterns.begin();
+
+    while(++iter != patterns.end())
+    {
+        glob_retval = glob(iter->c_str(), GLOB_APPEND, NULL, &glob_results);
+    }
+
+    for(int path_index = 0; path_index < glob_results.gl_pathc; path_index++)
+    {
+        paths_found.push_back(glob_results.gl_pathv[path_index]);
+    }
+
+    globfree(&glob_results);
+
+    return paths_found;
+}
+
+string
+basename(const string& path)
+{
+    size_t pos = path.rfind("/");
+
+    if(pos == std::string::npos)
+        return path;
+
+    return string(path, pos+1, string::npos);
+}
+
+string
+dirname(const string& path)
+{
+    size_t pos = path.rfind("/");
+
+    if(pos == std::string::npos)
+        return path;
+    else if(pos == 0)
+        return "/";
+
+    return string(path, 0, pos);
+}
+
+bool
+path_exists(const string& path)
+{
+    struct stat sb;
+
+    if( stat(path.c_str(), &sb ) == 0 )
+        return true;
+
+    return false;
+}
+
+string
+realpath(const string& path)
+{
+    char* real_path = realpath(path.c_str(), NULL);
+
+    string result;
+
+    if(real_path != NULL)
+    {
+        result = real_path;
+
+        free(real_path);
+    }
+
+    return result;
+}
+
+string
+usb_sysfs_friendly_name(const string& sys_usb_path)
+{
+    unsigned int device_number = 0;
+
+    istringstream( read_line(sys_usb_path + "/devnum") ) >> device_number;
+
+    string manufacturer = read_line( sys_usb_path + "/manufacturer" );
+
+    string product = read_line( sys_usb_path + "/product" );
+
+    string serial = read_line( sys_usb_path + "/serial" );
+
+    if( manufacturer.empty() && product.empty() && serial.empty() )
+        return "";
+
+    return format("%s %s %s", manufacturer.c_str(), product.c_str(), serial.c_str() );
+}
+
+vector<string>
+get_sysfs_info(const string& device_path)
+{
+    string device_name = basename( device_path );
+
+    string friendly_name;
+
+    string hardware_id;
+
+    string sys_device_path = format( "/sys/class/tty/%s/device", device_name.c_str() );
+
+    if( device_name.compare(0,6,"ttyUSB") == 0 )
+    {
+        sys_device_path = dirname( dirname( realpath( sys_device_path ) ) );
+
+        if( path_exists( sys_device_path ) )
+        {
+            friendly_name = usb_sysfs_friendly_name( sys_device_path );
+
+            hardware_id = usb_sysfs_hw_string( sys_device_path );
+        }
+    }
+    else if( device_name.compare(0,6,"ttyACM") == 0 )
+    {
+        sys_device_path = dirname( realpath( sys_device_path ) );
+
+        if( path_exists( sys_device_path ) )
+        {
+            friendly_name = usb_sysfs_friendly_name( sys_device_path );
+
+            hardware_id = usb_sysfs_hw_string( sys_device_path );
+        }
+    }
+    else
+    {
+        // Try to read ID string of PCI device
+
+        string sys_id_path = sys_device_path + "/id";
+
+        if( path_exists( sys_id_path ) )
+            hardware_id = read_line( sys_id_path );
+    }
+
+    if( friendly_name.empty() )
+        friendly_name = device_name;
+
+    if( hardware_id.empty() )
+        hardware_id = "n/a";
+
+    vector<string> result;
+    result.push_back(friendly_name);
+    result.push_back(hardware_id);
+
+    return result;
+}
+
+string
+read_line(const string& file)
+{
+    ifstream ifs(file.c_str(), ifstream::in);
+
+    string line;
+
+    if(ifs)
+    {
+        getline(ifs, line);
+    }
+
+    return line;
+}
+
+string
+format(const char* format, ...)
+{
+    va_list ap;
+
+    size_t buffer_size_bytes = 256;
+
+    string result;
+
+    char* buffer = (char*)malloc(buffer_size_bytes);
+
+    if( buffer == NULL )
+        return result;
+
+    bool done = false;
+
+    unsigned int loop_count = 0;
+
+    while(!done)
+    {
+        va_start(ap, format);
+
+        int return_value = vsnprintf(buffer, buffer_size_bytes, format, ap);
+
+        if( return_value < 0 )
+        {
+            done = true;
+        }
+        else if( return_value >= buffer_size_bytes )
+        {
+            // Realloc and try again.
+
+            buffer_size_bytes = return_value + 1;
+
+            char* new_buffer_ptr = (char*)realloc(buffer, buffer_size_bytes);
+
+            if( new_buffer_ptr == NULL )
+            {
+                done = true;
+            }
+            else
+            {
+                buffer = new_buffer_ptr;
+            }
+        }
+        else
+        {
+            result = buffer;
+            done = true;
+        }
+
+        va_end(ap);
+
+        if( ++loop_count > 5 )
+            done = true;
+    }
+
+    free(buffer);
+
+    return result;
+}
+
+string
+usb_sysfs_hw_string(const string& sysfs_path)
+{
+    string serial_number = read_line( sysfs_path + "/serial" );
+
+    if( serial_number.length() > 0 )
+    {
+        serial_number = format( "SNR=%s", serial_number.c_str() );
+    }
+
+    string vid = read_line( sysfs_path + "/idVendor" );
+
+    string pid = read_line( sysfs_path + "/idProduct" );
+
+    return format("USB VID:PID=%s:%s %s", vid.c_str(), pid.c_str(), serial_number.c_str() );
+}
+
+vector<PortInfo>
+serial::list_ports()
+{
+    vector<PortInfo> results;
+
+    vector<string> search_globs;
+    search_globs.push_back("/dev/ttyACM*");
+    search_globs.push_back("/dev/ttyS*");
+    search_globs.push_back("/dev/ttyUSB*");
+    search_globs.push_back("/dev/tty.*");
+    search_globs.push_back("/dev/cu.*");
+    search_globs.push_back("/dev/rfcomm*");
+
+    vector<string> devices_found = glob( search_globs );
+
+    vector<string>::iterator iter = devices_found.begin();
+
+    while( iter != devices_found.end() )
+    {
+        string device = *iter++;
+
+        vector<string> sysfs_info = get_sysfs_info( device );
+
+        string friendly_name = sysfs_info[0];
+
+        string hardware_id = sysfs_info[1];
+
+        PortInfo device_entry;
+        device_entry.port = device;
+        device_entry.description = friendly_name;
+        device_entry.hardware_id = hardware_id;
+
+        results.push_back( device_entry );
+
+    }
+
+    return results;
+}
+
+#endif // defined(__linux__)

auto_drive_proj_utils/SerialCommunicaiton/list_ports_osx.cc

@@ -0,0 +1,286 @@
+#if defined(__APPLE__)
+
+#include <sys/param.h>
+#include <stdint.h>
+
+#include <CoreFoundation/CoreFoundation.h>
+#include <IOKit/IOKitLib.h>
+#include <IOKit/serial/IOSerialKeys.h>
+#include <IOKit/IOBSD.h>
+
+#include <iostream>
+#include <string>
+#include <vector>
+
+#include "serial/serial.h"
+
+using serial::PortInfo;
+using std::string;
+using std::vector;
+
+#define HARDWARE_ID_STRING_LENGTH 128
+
+string cfstring_to_string( CFStringRef cfstring );
+string get_device_path( io_object_t& serial_port );
+string get_class_name( io_object_t& obj );
+io_registry_entry_t get_parent_iousb_device( io_object_t& serial_port );
+string get_string_property( io_object_t& device, const char* property );
+uint16_t get_int_property( io_object_t& device, const char* property );
+string rtrim(const string& str);
+
+string
+cfstring_to_string( CFStringRef cfstring )
+{
+    char cstring[MAXPATHLEN];
+    string result;
+
+    if( cfstring )
+    {
+        Boolean success = CFStringGetCString( cfstring,
+            cstring,
+            sizeof(cstring),
+            kCFStringEncodingASCII );
+
+        if( success )
+            result = cstring;
+    }
+
+    return result;
+}
+
+string
+get_device_path( io_object_t& serial_port )
+{
+    CFTypeRef callout_path;
+    string device_path;
+
+    callout_path = IORegistryEntryCreateCFProperty( serial_port,
+        CFSTR(kIOCalloutDeviceKey),
+        kCFAllocatorDefault,
+        0 );
+
+    if (callout_path)
+    {
+        if( CFGetTypeID(callout_path) == CFStringGetTypeID() )
+            device_path = cfstring_to_string( static_cast<CFStringRef>(callout_path) );
+
+        CFRelease(callout_path);
+    }
+
+    return device_path;
+}
+
+string
+get_class_name( io_object_t& obj )
+{
+    string result;
+    io_name_t class_name;
+    kern_return_t kern_result;
+
+    kern_result = IOObjectGetClass( obj, class_name );
+
+    if( kern_result == KERN_SUCCESS )
+        result = class_name;
+
+    return result;
+}
+
+io_registry_entry_t
+get_parent_iousb_device( io_object_t& serial_port )
+{
+    io_object_t device = serial_port;
+    io_registry_entry_t parent = 0;
+    io_registry_entry_t result = 0;
+    kern_return_t kern_result = KERN_FAILURE;
+    string name = get_class_name(device);
+
+    // Walk the IO Registry tree looking for this devices parent IOUSBDevice.
+    while( name != "IOUSBDevice" )
+    {
+        kern_result = IORegistryEntryGetParentEntry( device,
+        kIOServicePlane,
+        &parent );
+
+        if(kern_result != KERN_SUCCESS)
+        {
+            result = 0;
+            break;
+        }
+
+        device = parent;
+
+        name = get_class_name(device);
+    }
+
+    if(kern_result == KERN_SUCCESS)
+        result = device;
+
+    return result;
+}
+
+string
+get_string_property( io_object_t& device, const char* property )
+{
+    string property_name;
+
+    if( device )
+    {
+        CFStringRef property_as_cfstring = CFStringCreateWithCString (
+            kCFAllocatorDefault,
+            property,
+            kCFStringEncodingASCII );
+
+        CFTypeRef name_as_cfstring = IORegistryEntryCreateCFProperty(
+            device,
+            property_as_cfstring,
+            kCFAllocatorDefault,
+            0 );
+
+        if( name_as_cfstring )
+        {
+            if( CFGetTypeID(name_as_cfstring) == CFStringGetTypeID() )
+                property_name = cfstring_to_string( static_cast<CFStringRef>(name_as_cfstring) );
+
+            CFRelease(name_as_cfstring);
+        }
+
+        if(property_as_cfstring)
+            CFRelease(property_as_cfstring);
+    }
+
+    return property_name;
+}
+
+uint16_t
+get_int_property( io_object_t& device, const char* property )
+{
+    uint16_t result = 0;
+
+    if( device )
+    {
+        CFStringRef property_as_cfstring = CFStringCreateWithCString (
+            kCFAllocatorDefault,
+            property,
+            kCFStringEncodingASCII );
+
+        CFTypeRef number = IORegistryEntryCreateCFProperty( device,
+            property_as_cfstring,
+            kCFAllocatorDefault,
+            0 );
+
+        if(property_as_cfstring)
+            CFRelease(property_as_cfstring);
+
+        if( number )
+        {
+            if( CFGetTypeID(number) == CFNumberGetTypeID() )
+            {
+                bool success = CFNumberGetValue( static_cast<CFNumberRef>(number),
+                    kCFNumberSInt16Type,
+                    &result );
+
+                if( !success )
+                    result = 0;
+            }
+
+            CFRelease(number);
+        }
+
+    }
+
+    return result;
+}
+
+string rtrim(const string& str)
+{
+    string result = str;
+
+    string whitespace = " \t\f\v\n\r";
+
+    std::size_t found = result.find_last_not_of(whitespace);
+
+    if (found != std::string::npos)
+        result.erase(found+1);
+    else
+        result.clear();
+
+    return result;
+}
+
+vector<PortInfo>
+serial::list_ports(void)
+{
+    vector<PortInfo> devices_found;
+    CFMutableDictionaryRef classes_to_match;
+    io_iterator_t serial_port_iterator;
+    io_object_t serial_port;
+    mach_port_t master_port;
+    kern_return_t kern_result;
+
+    kern_result = IOMasterPort(MACH_PORT_NULL, &master_port);
+
+    if(kern_result != KERN_SUCCESS)
+        return devices_found;
+
+    classes_to_match = IOServiceMatching(kIOSerialBSDServiceValue);
+
+    if (classes_to_match == NULL)
+        return devices_found;
+
+    CFDictionarySetValue( classes_to_match,
+        CFSTR(kIOSerialBSDTypeKey),
+        CFSTR(kIOSerialBSDAllTypes) );
+
+    kern_result = IOServiceGetMatchingServices(master_port, classes_to_match, &serial_port_iterator);
+
+    if (KERN_SUCCESS != kern_result)
+        return devices_found;
+
+    while ( (serial_port = IOIteratorNext(serial_port_iterator)) )
+    {
+        string device_path = get_device_path( serial_port );
+        io_registry_entry_t parent = get_parent_iousb_device( serial_port );
+        IOObjectRelease(serial_port);
+
+        if( device_path.empty() )
+            continue;
+
+        PortInfo port_info;
+        port_info.port = device_path;
+        port_info.description = "n/a";
+        port_info.hardware_id = "n/a";
+
+        string device_name = rtrim( get_string_property( parent, "USB Product Name" ) );
+        string vendor_name = rtrim( get_string_property( parent, "USB Vendor Name") );
+        string description = rtrim( vendor_name + " " + device_name );
+        if( !description.empty() )
+            port_info.description = description;
+
+        string serial_number = rtrim(get_string_property( parent, "USB Serial Number" ) );
+        uint16_t vendor_id = get_int_property( parent, "idVendor" );
+        uint16_t product_id = get_int_property( parent, "idProduct" );
+
+        if( vendor_id && product_id )
+        {
+            char cstring[HARDWARE_ID_STRING_LENGTH];
+
+            if(serial_number.empty())
+                serial_number = "None";
+
+            int ret = snprintf( cstring, HARDWARE_ID_STRING_LENGTH, "USB VID:PID=%04x:%04x SNR=%s",
+                vendor_id,
+                product_id,
+                serial_number.c_str() );
+
+            if( (ret >= 0) && (ret < HARDWARE_ID_STRING_LENGTH) )
+                port_info.hardware_id = cstring;
+        }
+
+        devices_found.push_back(port_info);
+    }
+
+    IOObjectRelease(serial_port_iterator);
+    return devices_found;
+}
+
+#endif // defined(__APPLE__)

auto_drive_proj_utils/SerialCommunicaiton/list_ports_win.cc

@@ -0,0 +1,152 @@
+#if defined(_WIN32)
+
+/*
+ * Copyright (c) 2014 Craig Lilley <cralilley@gmail.com>
+ * This software is made available under the terms of the MIT licence.
+ * A copy of the licence can be obtained from:
+ * http://opensource.org/licenses/MIT
+ */
+
+#include "serial/serial.h"
+#include <tchar.h>
+#include <windows.h>
+#include <setupapi.h>
+#include <initguid.h>
+#include <devguid.h>
+#include <cstring>
+
+using serial::PortInfo;
+using std::vector;
+using std::string;
+
+static const DWORD port_name_max_length = 256;
+static const DWORD friendly_name_max_length = 256;
+static const DWORD hardware_id_max_length = 256;
+
+// Convert a wide Unicode string to an UTF8 string
+std::string utf8_encode(const std::wstring &wstr)
+{
+	int size_needed = WideCharToMultiByte(CP_UTF8, 0, &wstr[0], (int)wstr.size(), NULL, 0, NULL, NULL);
+	std::string strTo( size_needed, 0 );
+	WideCharToMultiByte                  (CP_UTF8, 0, &wstr[0], (int)wstr.size(), &strTo[0], size_needed, NULL, NULL);
+	return strTo;
+}
+
+vector<PortInfo>
+serial::list_ports()
+{
+	vector<PortInfo> devices_found;
+
+	HDEVINFO device_info_set = SetupDiGetClassDevs(
+		(const GUID *) &GUID_DEVCLASS_PORTS,
+		NULL,
+		NULL,
+		DIGCF_PRESENT);
+
+	unsigned int device_info_set_index = 0;
+	SP_DEVINFO_DATA device_info_data;
+
+	device_info_data.cbSize = sizeof(SP_DEVINFO_DATA);
+
+	while(SetupDiEnumDeviceInfo(device_info_set, device_info_set_index, &device_info_data))
+	{
+		device_info_set_index++;
+
+		// Get port name
+
+		HKEY hkey = SetupDiOpenDevRegKey(
+			device_info_set,
+			&device_info_data,
+			DICS_FLAG_GLOBAL,
+			0,
+			DIREG_DEV,
+			KEY_READ);
+
+		TCHAR port_name[port_name_max_length];
+		DWORD port_name_length = port_name_max_length;
+
+		LONG return_code = RegQueryValueEx(
+					hkey,
+					_T("PortName"),
+					NULL,
+					NULL,
+					(LPBYTE)port_name,
+					&port_name_length);
+
+		RegCloseKey(hkey);
+
+		if(return_code != EXIT_SUCCESS)
+			continue;
+
+		if(port_name_length > 0 && port_name_length <= port_name_max_length)
+			port_name[port_name_length-1] = '\0';
+		else
+			port_name[0] = '\0';
+
+		// Ignore parallel ports
+
+		if(_tcsstr(port_name, _T("LPT")) != NULL)
+			continue;
+
+		// Get port friendly name
+
+		TCHAR friendly_name[friendly_name_max_length];
+		DWORD friendly_name_actual_length = 0;
+
+		BOOL got_friendly_name = SetupDiGetDeviceRegistryProperty(
+					device_info_set,
+					&device_info_data,
+					SPDRP_FRIENDLYNAME,
+					NULL,
+					(PBYTE)friendly_name,
+					friendly_name_max_length,
+					&friendly_name_actual_length);
+
+		if(got_friendly_name == TRUE && friendly_name_actual_length > 0)
+			friendly_name[friendly_name_actual_length-1] = '\0';
+		else
+			friendly_name[0] = '\0';
+
+		// Get hardware ID
+
+		TCHAR hardware_id[hardware_id_max_length];
+		DWORD hardware_id_actual_length = 0;
+
+		BOOL got_hardware_id = SetupDiGetDeviceRegistryProperty(
+					device_info_set,
+					&device_info_data,
+					SPDRP_HARDWAREID,
+					NULL,
+					(PBYTE)hardware_id,
+					hardware_id_max_length,
+					&hardware_id_actual_length);
+
+		if(got_hardware_id == TRUE && hardware_id_actual_length > 0)
+			hardware_id[hardware_id_actual_length-1] = '\0';
+		else
+			hardware_id[0] = '\0';
+
+		#ifdef UNICODE
+			std::string portName = utf8_encode(port_name);
+			std::string friendlyName = utf8_encode(friendly_name);
+			std::string hardwareId = utf8_encode(hardware_id);
+		#else
+			std::string portName = port_name;
+			std::string friendlyName = friendly_name;
+			std::string hardwareId = hardware_id;
+		#endif
+
+		PortInfo port_entry;
+		port_entry.port = portName;
+		port_entry.description = friendlyName;
+		port_entry.hardware_id = hardwareId;
+
+		devices_found.push_back(port_entry);
+	}
+
+	SetupDiDestroyDeviceInfoList(device_info_set);
+
+	return devices_found;
+}
+
+#endif // #if defined(_WIN32)

auto_drive_proj_utils/SerialCommunicaiton/serial.cc

@@ -0,0 +1,432 @@
+/* Copyright 2012 William Woodall and John Harrison */
+#include <algorithm>
+
+#if !defined(_WIN32) && !defined(__OpenBSD__) && !defined(__FreeBSD__)
+# include <alloca.h>
+#endif
+
+#if defined (__MINGW32__)
+# define alloca __builtin_alloca
+#endif
+
+#include "serial.h"
+
+#ifdef _WIN32
+#include "win.h"
+#else
+#include "unix.h"
+#endif
+
+using std::invalid_argument;
+using std::min;
+using std::numeric_limits;
+using std::vector;
+using std::size_t;
+using std::string;
+
+using serial::Serial;
+using serial::SerialException;
+using serial::IOException;
+using serial::bytesize_t;
+using serial::parity_t;
+using serial::stopbits_t;
+using serial::flowcontrol_t;
+
+class Serial::ScopedReadLock {
+public:
+  ScopedReadLock(SerialImpl *pimpl) : pimpl_(pimpl) {
+    this->pimpl_->readLock();
+  }
+  ~ScopedReadLock() {
+    this->pimpl_->readUnlock();
+  }
+private:
+  // Disable copy constructors
+  ScopedReadLock(const ScopedReadLock&);
+  const ScopedReadLock& operator=(ScopedReadLock);
+
+  SerialImpl *pimpl_;
+};
+
+class Serial::ScopedWriteLock {
+public:
+  ScopedWriteLock(SerialImpl *pimpl) : pimpl_(pimpl) {
+    this->pimpl_->writeLock();
+  }
+  ~ScopedWriteLock() {
+    this->pimpl_->writeUnlock();
+  }
+private:
+  // Disable copy constructors
+  ScopedWriteLock(const ScopedWriteLock&);
+  const ScopedWriteLock& operator=(ScopedWriteLock);
+  SerialImpl *pimpl_;
+};
+
+Serial::Serial (const string &port, uint32_t baudrate, serial::Timeout timeout,
+                bytesize_t bytesize, parity_t parity, stopbits_t stopbits,
+                flowcontrol_t flowcontrol)
+ : pimpl_(new SerialImpl (port, baudrate, bytesize, parity,
+                                           stopbits, flowcontrol))
+{
+  pimpl_->setTimeout(timeout);
+}
+
+Serial::~Serial ()
+{
+  delete pimpl_;
+}
+
+void
+Serial::open ()
+{
+  pimpl_->open ();
+}
+
+void
+Serial::close ()
+{
+  pimpl_->close ();
+}
+
+bool
+Serial::isOpen () const
+{
+  return pimpl_->isOpen ();
+}
+
+size_t
+Serial::available ()
+{
+  return pimpl_->available ();
+}
+
+bool
+Serial::waitReadable ()
+{
+  serial::Timeout timeout(pimpl_->getTimeout ());
+  return pimpl_->waitReadable(timeout.read_timeout_constant);
+}
+
+void
+Serial::waitByteTimes (size_t count)
+{
+  pimpl_->waitByteTimes(count);
+}
+
+size_t
+Serial::read_ (uint8_t *buffer, size_t size)
+{
+  return this->pimpl_->read (buffer, size);
+}
+
+size_t
+Serial::read (uint8_t *buffer, size_t size)
+{
+  ScopedReadLock lock(this->pimpl_);
+  return this->pimpl_->read (buffer, size);
+}
+
+size_t
+Serial::read (std::vector<uint8_t> &buffer, size_t size)
+{
+  ScopedReadLock lock(this->pimpl_);
+  uint8_t *buffer_ = new uint8_t[size];
+  size_t bytes_read = 0;
+
+  try {
+    bytes_read = this->pimpl_->read (buffer_, size);
+  }
+  catch (const std::exception &e) {
+    delete[] buffer_;
+    throw;
+  }
+
+  buffer.insert (buffer.end (), buffer_, buffer_+bytes_read);
+  delete[] buffer_;
+  return bytes_read;
+}
+
+size_t
+Serial::read (std::string &buffer, size_t size)
+{
+  ScopedReadLock lock(this->pimpl_);
+  uint8_t *buffer_ = new uint8_t[size];
+  size_t bytes_read = 0;
+  try {
+    bytes_read = this->pimpl_->read (buffer_, size);
+  }
+  catch (const std::exception &e) {
+    delete[] buffer_;
+    throw;
+  }
+  buffer.append (reinterpret_cast<const char*>(buffer_), bytes_read);
+  delete[] buffer_;
+  return bytes_read;
+}
+
+string
+Serial::read (size_t size)
+{
+  std::string buffer;
+  this->read (buffer, size);
+  return buffer;
+}
+
+size_t
+Serial::readline (string &buffer, size_t size, string eol)
+{
+  ScopedReadLock lock(this->pimpl_);
+  size_t eol_len = eol.length ();
+  uint8_t *buffer_ = static_cast<uint8_t*>
+                              (alloca (size * sizeof (uint8_t)));
+  size_t read_so_far = 0;
+  while (true)
+  {
+    size_t bytes_read = this->read_ (buffer_ + read_so_far, 1);
+    read_so_far += bytes_read;
+    if (bytes_read == 0) {
+      break; // Timeout occured on reading 1 byte
+    }
+    if(read_so_far < eol_len) continue;
+    if (string (reinterpret_cast<const char*>
+         (buffer_ + read_so_far - eol_len), eol_len) == eol) {
+      break; // EOL found
+    }
+    if (read_so_far == size) {
+      break; // Reached the maximum read length
+    }
+  }
+  buffer.append(reinterpret_cast<const char*> (buffer_), read_so_far);
+  return read_so_far;
+}
+
+string
+Serial::readline (size_t size, string eol)
+{
+  std::string buffer;
+  this->readline (buffer, size, eol);
+  return buffer;
+}
+
+vector<string>
+Serial::readlines (size_t size, string eol)
+{
+  ScopedReadLock lock(this->pimpl_);
+  std::vector<std::string> lines;
+  size_t eol_len = eol.length ();
+  uint8_t *buffer_ = static_cast<uint8_t*>
+    (alloca (size * sizeof (uint8_t)));
+  size_t read_so_far = 0;
+  size_t start_of_line = 0;
+  while (read_so_far < size) {
+    size_t bytes_read = this->read_ (buffer_+read_so_far, 1);
+    read_so_far += bytes_read;
+    if (bytes_read == 0) {
+      if (start_of_line != read_so_far) {
+        lines.push_back (
+          string (reinterpret_cast<const char*> (buffer_ + start_of_line),
+            read_so_far - start_of_line));
+      }
+      break; // Timeout occured on reading 1 byte
+    }
+    if(read_so_far < eol_len) continue;
+    if (string (reinterpret_cast<const char*>
+         (buffer_ + read_so_far - eol_len), eol_len) == eol) {
+      // EOL found
+      lines.push_back(
+        string(reinterpret_cast<const char*> (buffer_ + start_of_line),
+          read_so_far - start_of_line));
+      start_of_line = read_so_far;
+    }
+    if (read_so_far == size) {
+      if (start_of_line != read_so_far) {
+        lines.push_back(
+          string(reinterpret_cast<const char*> (buffer_ + start_of_line),
+            read_so_far - start_of_line));
+      }
+      break; // Reached the maximum read length
+    }
+  }
+  return lines;
+}
+
+size_t
+Serial::write (const string &data)
+{
+  ScopedWriteLock lock(this->pimpl_);
+  return this->write_ (reinterpret_cast<const uint8_t*>(data.c_str()),
+                       data.length());
+}
+
+size_t
+Serial::write (const std::vector<uint8_t> &data)
+{
+  ScopedWriteLock lock(this->pimpl_);
+  return this->write_ (&data[0], data.size());
+}
+
+size_t
+Serial::write (const uint8_t *data, size_t size)
+{
+  ScopedWriteLock lock(this->pimpl_);
+  return this->write_(data, size);
+}
+
+size_t
+Serial::write_ (const uint8_t *data, size_t length)
+{
+  return pimpl_->write (data, length);
+}
+
+void
+Serial::setPort (const string &port)
+{
+  ScopedReadLock rlock(this->pimpl_);
+  ScopedWriteLock wlock(this->pimpl_);
+  bool was_open = pimpl_->isOpen ();
+  if (was_open) close();
+  pimpl_->setPort (port);
+  if (was_open) open ();
+}
+
+string
+Serial::getPort () const
+{
+  return pimpl_->getPort ();
+}
+
+void
+Serial::setTimeout (serial::Timeout &timeout)
+{
+  pimpl_->setTimeout (timeout);
+}
+
+serial::Timeout
+Serial::getTimeout () const {
+  return pimpl_->getTimeout ();
+}
+
+void
+Serial::setBaudrate (uint32_t baudrate)
+{
+  pimpl_->setBaudrate (baudrate);
+}
+
+uint32_t
+Serial::getBaudrate () const
+{
+  return uint32_t(pimpl_->getBaudrate ());
+}
+
+void
+Serial::setBytesize (bytesize_t bytesize)
+{
+  pimpl_->setBytesize (bytesize);
+}
+
+bytesize_t
+Serial::getBytesize () const
+{
+  return pimpl_->getBytesize ();
+}
+
+void
+Serial::setParity (parity_t parity)
+{
+  pimpl_->setParity (parity);
+}
+
+parity_t
+Serial::getParity () const
+{
+  return pimpl_->getParity ();
+}
+
+void
+Serial::setStopbits (stopbits_t stopbits)
+{
+  pimpl_->setStopbits (stopbits);
+}
+
+stopbits_t
+Serial::getStopbits () const
+{
+  return pimpl_->getStopbits ();
+}
+
+void
+Serial::setFlowcontrol (flowcontrol_t flowcontrol)
+{
+  pimpl_->setFlowcontrol (flowcontrol);
+}
+
+flowcontrol_t
+Serial::getFlowcontrol () const
+{
+  return pimpl_->getFlowcontrol ();
+}
+
+void Serial::flush ()
+{
+  ScopedReadLock rlock(this->pimpl_);
+  ScopedWriteLock wlock(this->pimpl_);
+  pimpl_->flush ();
+}
+
+void Serial::flushInput ()
+{
+  ScopedReadLock lock(this->pimpl_);
+  pimpl_->flushInput ();
+}
+
+void Serial::flushOutput ()
+{
+  ScopedWriteLock lock(this->pimpl_);
+  pimpl_->flushOutput ();
+}
+
+void Serial::sendBreak (int duration)
+{
+  pimpl_->sendBreak (duration);
+}
+
+void Serial::setBreak (bool level)
+{
+  pimpl_->setBreak (level);
+}
+
+void Serial::setRTS (bool level)
+{
+  pimpl_->setRTS (level);
+}
+
+void Serial::setDTR (bool level)
+{
+  pimpl_->setDTR (level);
+}
+
+bool Serial::waitForChange()
+{
+  return pimpl_->waitForChange();
+}
+
+bool Serial::getCTS ()
+{
+  return pimpl_->getCTS ();
+}
+
+bool Serial::getDSR ()
+{
+  return pimpl_->getDSR ();
+}
+
+bool Serial::getRI ()
+{
+  return pimpl_->getRI ();
+}
+
+bool Serial::getCD ()
+{
+  return pimpl_->getCD ();
+}

auto_drive_proj_utils/SerialCommunicaiton/serial.h

@@ -0,0 +1,779 @@
+/*!
+ * \file serial/serial.h
+ * \author  William Woodall <wjwwood@gmail.com>
+ * \author  John Harrison   <ash.gti@gmail.com>
+ * \version 0.1
+ *
+ * \section LICENSE
+ *
+ * The MIT License
+ *
+ * Copyright (c) 2012 William Woodall
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ *
+ * \section DESCRIPTION
+ *
+ * This provides a cross platform interface for interacting with Serial Ports.
+ */
+
+#ifndef SERIAL_H
+#define SERIAL_H
+
+#include <limits>
+#include <vector>
+#include <string>
+#include <cstring>
+#include <sstream>
+#include <exception>
+#include <stdexcept>
+//#include <v8stdint.h>
+
+#include <stddef.h>
+#include <stdio.h>
+#include <stdint.h>
+
+#define THROW(exceptionClass, message) throw exceptionClass(__FILE__, \
+__LINE__, (message) )
+
+namespace serial {
+
+/*!
+ * Enumeration defines the possible bytesizes for the serial port.
+ */
+typedef enum {
+  fivebits = 5,
+  sixbits = 6,
+  sevenbits = 7,
+  eightbits = 8
+} bytesize_t;
+
+/*!
+ * Enumeration defines the possible parity types for the serial port.
+ */
+typedef enum {
+  parity_none = 0,
+  parity_odd = 1,
+  parity_even = 2,
+  parity_mark = 3,
+  parity_space = 4
+} parity_t;
+
+/*!
+ * Enumeration defines the possible stopbit types for the serial port.
+ */
+typedef enum {
+  stopbits_one = 1,
+  stopbits_two = 2,
+  stopbits_one_point_five
+} stopbits_t;
+
+/*!
+ * Enumeration defines the possible flowcontrol types for the serial port.
+ */
+typedef enum {
+  flowcontrol_none = 0,
+  flowcontrol_software,
+  flowcontrol_hardware
+} flowcontrol_t;
+
+/*!
+ * Structure for setting the timeout of the serial port, times are
+ * in milliseconds.
+ *
+ * In order to disable the interbyte timeout, set it to Timeout::max().
+ */
+struct Timeout {
+#ifdef max
+# undef max
+#endif
+  static uint32_t max() {return std::numeric_limits<uint32_t>::max();}
+  /*!
+   * Convenience function to generate Timeout structs using a
+   * single absolute timeout.
+   *
+   * \param timeout A long that defines the time in milliseconds until a
+   * timeout occurs after a call to read or write is made.
+   *
+   * \return Timeout struct that represents this simple timeout provided.
+   */
+  static Timeout simpleTimeout(uint32_t timeout) {
+    return Timeout(max(), timeout, 0, timeout, 0);
+  }
+
+  /*! Number of milliseconds between bytes received to timeout on. */
+  uint32_t inter_byte_timeout;
+  /*! A constant number of milliseconds to wait after calling read. */
+  uint32_t read_timeout_constant;
+  /*! A multiplier against the number of requested bytes to wait after
+   *  calling read.
+   */
+  uint32_t read_timeout_multiplier;
+  /*! A constant number of milliseconds to wait after calling write. */
+  uint32_t write_timeout_constant;
+  /*! A multiplier against the number of requested bytes to wait after
+   *  calling write.
+   */
+  uint32_t write_timeout_multiplier;
+
+  explicit Timeout (uint32_t inter_byte_timeout_=0,
+                    uint32_t read_timeout_constant_=0,
+                    uint32_t read_timeout_multiplier_=0,
+                    uint32_t write_timeout_constant_=0,
+                    uint32_t write_timeout_multiplier_=0)
+  : inter_byte_timeout(inter_byte_timeout_),
+    read_timeout_constant(read_timeout_constant_),
+    read_timeout_multiplier(read_timeout_multiplier_),
+    write_timeout_constant(write_timeout_constant_),
+    write_timeout_multiplier(write_timeout_multiplier_)
+  {}
+};
+
+/*!
+ * Class that provides a portable serial port interface.
+ */
+class Serial {
+public:
+  /*!
+   * Creates a Serial object and opens the port if a port is specified,
+   * otherwise it remains closed until serial::Serial::open is called.
+   *
+   * \param port A std::string containing the address of the serial port,
+   *        which would be something like 'COM1' on Windows and '/dev/ttyS0'
+   *        on Linux.
+   *
+   * \param baudrate An unsigned 32-bit integer that represents the baudrate
+   *
+   * \param timeout A serial::Timeout struct that defines the timeout
+   * conditions for the serial port. \see serial::Timeout
+   *
+   * \param bytesize Size of each byte in the serial transmission of data,
+   * default is eightbits, possible values are: fivebits, sixbits, sevenbits,
+   * eightbits
+   *
+   * \param parity Method of parity, default is parity_none, possible values
+   * are: parity_none, parity_odd, parity_even
+   *
+   * \param stopbits Number of stop bits used, default is stopbits_one,
+   * possible values are: stopbits_one, stopbits_one_point_five, stopbits_two
+   *
+   * \param flowcontrol Type of flowcontrol used, default is
+   * flowcontrol_none, possible values are: flowcontrol_none,
+   * flowcontrol_software, flowcontrol_hardware
+   *
+   * \throw serial::PortNotOpenedException
+   * \throw serial::IOException
+   * \throw std::invalid_argument
+   */
+  Serial (const std::string &port = "",
+          uint32_t baudrate = 9600,
+          Timeout timeout = Timeout(),
+          bytesize_t bytesize = eightbits,
+          parity_t parity = parity_none,
+          stopbits_t stopbits = stopbits_one,
+          flowcontrol_t flowcontrol = flowcontrol_none);
+
+  /*! Destructor */
+  virtual ~Serial ();
+
+  /*!
+   * Opens the serial port as long as the port is set and the port isn't
+   * already open.
+   *
+   * If the port is provided to the constructor then an explicit call to open
+   * is not needed.
+   *
+   * \see Serial::Serial
+   *
+   * \throw std::invalid_argument
+   * \throw serial::SerialException
+   * \throw serial::IOException
+   */
+  void
+  open ();
+
+  /*! Gets the open status of the serial port.
+   *
+   * \return Returns true if the port is open, false otherwise.
+   */
+  bool
+  isOpen () const;
+
+  /*! Closes the serial port. */
+  void
+  close ();
+
+  /*! Return the number of characters in the buffer. */
+  size_t
+  available ();
+
+  /*! Block until there is serial data to read or read_timeout_constant
+   * number of milliseconds have elapsed. The return value is true when
+   * the function exits with the port in a readable state, false otherwise
+   * (due to timeout or select interruption). */
+  bool
+  waitReadable ();
+
+  /*! Block for a period of time corresponding to the transmission time of
+   * count characters at present serial settings. This may be used in con-
+   * junction with waitReadable to read larger blocks of data from the
+   * port. */
+  void
+  waitByteTimes (size_t count);
+
+  /*! Read a given amount of bytes from the serial port into a given buffer.
+   *
+   * The read function will return in one of three cases:
+   *  * The number of requested bytes was read.
+   *    * In this case the number of bytes requested will match the size_t
+   *      returned by read.
+   *  * A timeout occurred, in this case the number of bytes read will not
+   *    match the amount requested, but no exception will be thrown.  One of
+   *    two possible timeouts occurred:
+   *    * The inter byte timeout expired, this means that number of
+   *      milliseconds elapsed between receiving bytes from the serial port
+   *      exceeded the inter byte timeout.
+   *    * The total timeout expired, which is calculated by multiplying the
+   *      read timeout multiplier by the number of requested bytes and then
+   *      added to the read timeout constant.  If that total number of
+   *      milliseconds elapses after the initial call to read a timeout will
+   *      occur.
+   *  * An exception occurred, in this case an actual exception will be thrown.
+   *
+   * \param buffer An uint8_t array of at least the requested size.
+   * \param size A size_t defining how many bytes to be read.
+   *
+   * \return A size_t representing the number of bytes read as a result of the
+   *         call to read.
+   *
+   * \throw serial::PortNotOpenedException
+   * \throw serial::SerialException
+   */
+  size_t
+  read (uint8_t *buffer, size_t size);
+
+  /*! Read a given amount of bytes from the serial port into a give buffer.
+   *
+   * \param buffer A reference to a std::vector of uint8_t.
+   * \param size A size_t defining how many bytes to be read.
+   *
+   * \return A size_t representing the number of bytes read as a result of the
+   *         call to read.
+   *
+   * \throw serial::PortNotOpenedException
+   * \throw serial::SerialException
+   */
+  size_t
+  read (std::vector<uint8_t> &buffer, size_t size = 1);
+
+  /*! Read a given amount of bytes from the serial port into a give buffer.
+   *
+   * \param buffer A reference to a std::string.
+   * \param size A size_t defining how many bytes to be read.
+   *
+   * \return A size_t representing the number of bytes read as a result of the
+   *         call to read.
+   *
+   * \throw serial::PortNotOpenedException
+   * \throw serial::SerialException
+   */
+  size_t
+  read (std::string &buffer, size_t size = 1);
+
+  /*! Read a given amount of bytes from the serial port and return a string
+   *  containing the data.
+   *
+   * \param size A size_t defining how many bytes to be read.
+   *
+   * \return A std::string containing the data read from the port.
+   *
+   * \throw serial::PortNotOpenedException
+   * \throw serial::SerialException
+   */
+  std::string
+  read (size_t size = 1);
+
+  /*! Reads in a line or until a given delimiter has been processed.
+   *
+   * Reads from the serial port until a single line has been read.
+   *
+   * \param buffer A std::string reference used to store the data.
+   * \param size A maximum length of a line, defaults to 65536 (2^16)
+   * \param eol A string to match against for the EOL.
+   *
+   * \return A size_t representing the number of bytes read.
+   *
+   * \throw serial::PortNotOpenedException
+   * \throw serial::SerialException
+   */
+  size_t
+  readline (std::string &buffer, size_t size = 65536, std::string eol = "\n");
+
+  /*! Reads in a line or until a given delimiter has been processed.
+   *
+   * Reads from the serial port until a single line has been read.
+   *
+   * \param size A maximum length of a line, defaults to 65536 (2^16)
+   * \param eol A string to match against for the EOL.
+   *
+   * \return A std::string containing the line.
+   *
+   * \throw serial::PortNotOpenedException
+   * \throw serial::SerialException
+   */
+  std::string
+  readline (size_t size = 65536, std::string eol = "\n");
+
+  /*! Reads in multiple lines until the serial port times out.
+   *
+   * This requires a timeout > 0 before it can be run. It will read until a
+   * timeout occurs and return a list of strings.
+   *
+   * \param size A maximum length of combined lines, defaults to 65536 (2^16)
+   *
+   * \param eol A string to match against for the EOL.
+   *
+   * \return A vector<string> containing the lines.
+   *
+   * \throw serial::PortNotOpenedException
+   * \throw serial::SerialException
+   */
+  std::vector<std::string>
+  readlines (size_t size = 65536, std::string eol = "\n");
+
+  /*! Write a string to the serial port.
+   *
+   * \param data A const reference containing the data to be written
+   * to the serial port.
+   *
+   * \param size A size_t that indicates how many bytes should be written from
+   * the given data buffer.
+   *
+   * \return A size_t representing the number of bytes actually written to
+   * the serial port.
+   *
+   * \throw serial::PortNotOpenedException
+   * \throw serial::SerialException
+   * \throw serial::IOException
+   */
+  size_t
+  write (const uint8_t *data, size_t size);
+
+  /*! Write a string to the serial port.
+   *
+   * \param data A const reference containing the data to be written
+   * to the serial port.
+   *
+   * \return A size_t representing the number of bytes actually written to
+   * the serial port.
+   *
+   * \throw serial::PortNotOpenedException
+   * \throw serial::SerialException
+   * \throw serial::IOException
+   */
+  size_t
+  write (const std::vector<uint8_t> &data);
+
+  /*! Write a string to the serial port.
+   *
+   * \param data A const reference containing the data to be written
+   * to the serial port.
+   *
+   * \return A size_t representing the number of bytes actually written to
+   * the serial port.
+   *
+   * \throw serial::PortNotOpenedException
+   * \throw serial::SerialException
+   * \throw serial::IOException
+   */
+  size_t
+  write (const std::string &data);
+
+  /*! Sets the serial port identifier.
+   *
+   * \param port A const std::string reference containing the address of the
+   * serial port, which would be something like 'COM1' on Windows and
+   * '/dev/ttyS0' on Linux.
+   *
+   * \throw std::invalid_argument
+   */
+  void
+  setPort (const std::string &port);
+
+  /*! Gets the serial port identifier.
+   *
+   * \see Serial::setPort
+   *
+   * \throw std::invalid_argument
+   */
+  std::string
+  getPort () const;
+
+  /*! Sets the timeout for reads and writes using the Timeout struct.
+   *
+   * There are two timeout conditions described here:
+   *  * The inter byte timeout:
+   *    * The inter_byte_timeout component of serial::Timeout defines the
+   *      maximum amount of time, in milliseconds, between receiving bytes on
+   *      the serial port that can pass before a timeout occurs.  Setting this
+   *      to zero will prevent inter byte timeouts from occurring.
+   *  * Total time timeout:
+   *    * The constant and multiplier component of this timeout condition,
+   *      for both read and write, are defined in serial::Timeout.  This
+   *      timeout occurs if the total time since the read or write call was
+   *      made exceeds the specified time in milliseconds.
+   *    * The limit is defined by multiplying the multiplier component by the
+   *      number of requested bytes and adding that product to the constant
+   *      component.  In this way if you want a read call, for example, to
+   *      timeout after exactly one second regardless of the number of bytes
+   *      you asked for then set the read_timeout_constant component of
+   *      serial::Timeout to 1000 and the read_timeout_multiplier to zero.
+   *      This timeout condition can be used in conjunction with the inter
+   *      byte timeout condition with out any problems, timeout will simply
+   *      occur when one of the two timeout conditions is met.  This allows
+   *      users to have maximum control over the trade-off between
+   *      responsiveness and efficiency.
+   *
+   * Read and write functions will return in one of three cases.  When the
+   * reading or writing is complete, when a timeout occurs, or when an
+   * exception occurs.
+   *
+   * A timeout of 0 enables non-blocking mode.
+   *
+   * \param timeout A serial::Timeout struct containing the inter byte
+   * timeout, and the read and write timeout constants and multipliers.
+   *
+   * \see serial::Timeout
+   */
+  void
+  setTimeout (Timeout &timeout);
+
+  /*! Sets the timeout for reads and writes. */
+  void
+  setTimeout (uint32_t inter_byte_timeout, uint32_t read_timeout_constant,
+              uint32_t read_timeout_multiplier, uint32_t write_timeout_constant,
+              uint32_t write_timeout_multiplier)
+  {
+    Timeout timeout(inter_byte_timeout, read_timeout_constant,
+                    read_timeout_multiplier, write_timeout_constant,
+                    write_timeout_multiplier);
+    return setTimeout(timeout);
+  }
+
+  /*! Gets the timeout for reads in seconds.
+   *
+   * \return A Timeout struct containing the inter_byte_timeout, and read
+   * and write timeout constants and multipliers.
+   *
+   * \see Serial::setTimeout
+   */
+  Timeout
+  getTimeout () const;
+
+  /*! Sets the baudrate for the serial port.
+   *
+   * Possible baudrates depends on the system but some safe baudrates include:
+   * 110, 300, 600, 1200, 2400, 4800, 9600, 14400, 19200, 28800, 38400, 56000,
+   * 57600, 115200
+   * Some other baudrates that are supported by some comports:
+   * 128000, 153600, 230400, 256000, 460800, 500000, 921600
+   *
+   * \param baudrate An integer that sets the baud rate for the serial port.
+   *
+   * \throw std::invalid_argument
+   */
+  void
+  setBaudrate (uint32_t baudrate);
+
+  /*! Gets the baudrate for the serial port.
+   *
+   * \return An integer that sets the baud rate for the serial port.
+   *
+   * \see Serial::setBaudrate
+   *
+   * \throw std::invalid_argument
+   */
+  uint32_t
+  getBaudrate () const;
+
+  /*! Sets the bytesize for the serial port.
+   *
+   * \param bytesize Size of each byte in the serial transmission of data,
+   * default is eightbits, possible values are: fivebits, sixbits, sevenbits,
+   * eightbits
+   *
+   * \throw std::invalid_argument
+   */
+  void
+  setBytesize (bytesize_t bytesize);
+
+  /*! Gets the bytesize for the serial port.
+   *
+   * \see Serial::setBytesize
+   *
+   * \throw std::invalid_argument
+   */
+  bytesize_t
+  getBytesize () const;
+
+  /*! Sets the parity for the serial port.
+   *
+   * \param parity Method of parity, default is parity_none, possible values
+   * are: parity_none, parity_odd, parity_even
+   *
+   * \throw std::invalid_argument
+   */
+  void
+  setParity (parity_t parity);
+
+  /*! Gets the parity for the serial port.
+   *
+   * \see Serial::setParity
+   *
+   * \throw std::invalid_argument
+   */
+  parity_t
+  getParity () const;
+
+  /*! Sets the stopbits for the serial port.
+   *
+   * \param stopbits Number of stop bits used, default is stopbits_one,
+   * possible values are: stopbits_one, stopbits_one_point_five, stopbits_two
+   *
+   * \throw std::invalid_argument
+   */
+  void
+  setStopbits (stopbits_t stopbits);
+
+  /*! Gets the stopbits for the serial port.
+   *
+   * \see Serial::setStopbits
+   *
+   * \throw std::invalid_argument
+   */
+  stopbits_t
+  getStopbits () const;
+
+  /*! Sets the flow control for the serial port.
+   *
+   * \param flowcontrol Type of flowcontrol used, default is flowcontrol_none,
+   * possible values are: flowcontrol_none, flowcontrol_software,
+   * flowcontrol_hardware
+   *
+   * \throw std::invalid_argument
+   */
+  void
+  setFlowcontrol (flowcontrol_t flowcontrol);
+
+  /*! Gets the flow control for the serial port.
+   *
+   * \see Serial::setFlowcontrol
+   *
+   * \throw std::invalid_argument
+   */
+  flowcontrol_t
+  getFlowcontrol () const;
+
+  /*! Flush the input and output buffers */
+  void
+  flush ();
+
+  /*! Flush only the input buffer */
+  void
+  flushInput ();
+
+  /*! Flush only the output buffer */
+  void
+  flushOutput ();
+
+  /*! Sends the RS-232 break signal.  See tcsendbreak(3). */
+  void
+  sendBreak (int duration);
+
+  /*! Set the break condition to a given level.  Defaults to true. */
+  void
+  setBreak (bool level = true);
+
+  /*! Set the RTS handshaking line to the given level.  Defaults to true. */
+  void
+  setRTS (bool level = true);
+
+  /*! Set the DTR handshaking line to the given level.  Defaults to true. */
+  void
+  setDTR (bool level = true);
+
+  /*!
+   * Blocks until CTS, DSR, RI, CD changes or something interrupts it.
+   *
+   * Can throw an exception if an error occurs while waiting.
+   * You can check the status of CTS, DSR, RI, and CD once this returns.
+   * Uses TIOCMIWAIT via ioctl if available (mostly only on Linux) with a
+   * resolution of less than +-1ms and as good as +-0.2ms.  Otherwise a
+   * polling method is used which can give +-2ms.
+   *
+   * \return Returns true if one of the lines changed, false if something else
+   * occurred.
+   *
+   * \throw SerialException
+   */
+  bool
+  waitForChange ();
+
+  /*! Returns the current status of the CTS line. */
+  bool
+  getCTS ();
+
+  /*! Returns the current status of the DSR line. */
+  bool
+  getDSR ();
+
+  /*! Returns the current status of the RI line. */
+  bool
+  getRI ();
+
+  /*! Returns the current status of the CD line. */
+  bool
+  getCD ();
+
+private:
+  // Disable copy constructors
+  Serial(const Serial&);
+  Serial& operator=(const Serial&);
+
+  // Pimpl idiom, d_pointer
+  class SerialImpl;
+  SerialImpl *pimpl_;
+
+  // Scoped Lock Classes
+  class ScopedReadLock;
+  class ScopedWriteLock;
+
+  // Read common function
+  size_t
+  read_ (uint8_t *buffer, size_t size);
+  // Write common function
+  size_t
+  write_ (const uint8_t *data, size_t length);
+
+};
+
+class SerialException : public std::exception
+{
+  // Disable copy constructors
+  SerialException& operator=(const SerialException&);
+  std::string e_what_;
+public:
+  SerialException (const char *description) {
+      std::stringstream ss;
+      ss << "SerialException " << description << " failed.";
+      e_what_ = ss.str();
+  }
+  SerialException (const SerialException& other) : e_what_(other.e_what_) {}
+  virtual ~SerialException() throw() {}
+  virtual const char* what () const throw () {
+    return e_what_.c_str();
+  }
+};
+
+class IOException : public std::exception
+{
+  // Disable copy constructors
+  IOException& operator=(const IOException&);
+  std::string file_;
+  int line_;
+  std::string e_what_;
+  int errno_;
+public:
+  explicit IOException (std::string file, int line, int errnum)
+    : file_(file), line_(line), errno_(errnum) {
+      std::stringstream ss;
+#if defined(_WIN32) && !defined(__MINGW32__)
+      char error_str [1024];
+      strerror_s(error_str, 1024, errnum);
+#else
+      char * error_str = strerror(errnum);
+#endif
+      ss << "IO Exception (" << errno_ << "): " << error_str;
+      ss << ", file " << file_ << ", line " << line_ << ".";
+      e_what_ = ss.str();
+  }
+  explicit IOException (std::string file, int line, const char * description)
+    : file_(file), line_(line), errno_(0) {
+      std::stringstream ss;
+      ss << "IO Exception: " << description;
+      ss << ", file " << file_ << ", line " << line_ << ".";
+      e_what_ = ss.str();
+  }
+  virtual ~IOException() throw() {}
+  IOException (const IOException& other) : line_(other.line_), e_what_(other.e_what_), errno_(other.errno_) {}
+
+  int getErrorNumber () const { return errno_; }
+
+  virtual const char* what () const throw () {
+    return e_what_.c_str();
+  }
+};
+
+class PortNotOpenedException : public std::exception
+{
+  // Disable copy constructors
+  const PortNotOpenedException& operator=(PortNotOpenedException);
+  std::string e_what_;
+public:
+  PortNotOpenedException (const char * description)  {
+      std::stringstream ss;
+      ss << "PortNotOpenedException " << description << " failed.";
+      e_what_ = ss.str();
+  }
+  PortNotOpenedException (const PortNotOpenedException& other) : e_what_(other.e_what_) {}
+  virtual ~PortNotOpenedException() throw() {}
+  virtual const char* what () const throw () {
+    return e_what_.c_str();
+  }
+};
+
+/*!
+ * Structure that describes a serial device.
+ */
+struct PortInfo {
+
+  /*! Address of the serial port (this can be passed to the constructor of Serial). */
+  std::string port;
+
+  /*! Human readable description of serial device if available. */
+  std::string description;
+
+  /*! Hardware ID (e.g. VID:PID of USB serial devices) or "n/a" if not available. */
+  std::string hardware_id;
+
+};
+
+/* Lists the serial ports available on the system
+ *
+ * Returns a vector of available serial ports, each represented
+ * by a serial::PortInfo data structure:
+ *
+ * \return vector of serial::PortInfo.
+ */
+std::vector<PortInfo>
+list_ports();
+
+} // namespace serial
+
+#endif

auto_drive_proj_utils/SerialCommunicaiton/unix.h

@@ -0,0 +1,221 @@
+/*!
+ * \file serial/impl/unix.h
+ * \author  William Woodall <wjwwood@gmail.com>
+ * \author  John Harrison <ash@greaterthaninfinity.com>
+ * \version 0.1
+ *
+ * \section LICENSE
+ *
+ * The MIT License
+ *
+ * Copyright (c) 2012 William Woodall, John Harrison
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ *
+ * \section DESCRIPTION
+ *
+ * This provides a unix based pimpl for the Serial class. This implementation is
+ * based off termios.h and uses select for multiplexing the IO ports.
+ *
+ */
+
+#if !defined(_WIN32)
+
+#ifndef SERIAL_IMPL_UNIX_H
+#define SERIAL_IMPL_UNIX_H
+
+#include "serial.h"
+
+#include <pthread.h>
+
+namespace serial {
+
+using std::size_t;
+using std::string;
+using std::invalid_argument;
+
+using serial::SerialException;
+using serial::IOException;
+
+class MillisecondTimer {
+public:
+  MillisecondTimer(const uint32_t millis);         
+  int64_t remaining();
+
+private:
+  static timespec timespec_now();
+  timespec expiry;
+};
+
+class serial::Serial::SerialImpl {
+public:
+  SerialImpl (const string &port,
+              unsigned long baudrate,
+              bytesize_t bytesize,
+              parity_t parity,
+              stopbits_t stopbits,
+              flowcontrol_t flowcontrol);
+
+  virtual ~SerialImpl ();
+
+  void
+  open ();
+
+  void
+  close ();
+
+  bool
+  isOpen () const;
+
+  size_t
+  available ();
+
+  bool
+  waitReadable (uint32_t timeout);
+
+  void
+  waitByteTimes (size_t count);
+
+  size_t
+  read (uint8_t *buf, size_t size = 1);
+
+  size_t
+  write (const uint8_t *data, size_t length);
+
+  void
+  flush ();
+
+  void
+  flushInput ();
+
+  void
+  flushOutput ();
+
+  void
+  sendBreak (int duration);
+
+  void
+  setBreak (bool level);
+
+  void
+  setRTS (bool level);
+
+  void
+  setDTR (bool level);
+
+  bool
+  waitForChange ();
+
+  bool
+  getCTS ();
+
+  bool
+  getDSR ();
+
+  bool
+  getRI ();
+
+  bool
+  getCD ();
+
+  void
+  setPort (const string &port);
+
+  string
+  getPort () const;
+
+  void
+  setTimeout (Timeout &timeout);
+
+  Timeout
+  getTimeout () const;
+
+  void
+  setBaudrate (unsigned long baudrate);
+
+  unsigned long
+  getBaudrate () const;
+
+  void
+  setBytesize (bytesize_t bytesize);
+
+  bytesize_t
+  getBytesize () const;
+
+  void
+  setParity (parity_t parity);
+
+  parity_t
+  getParity () const;
+
+  void
+  setStopbits (stopbits_t stopbits);
+
+  stopbits_t
+  getStopbits () const;
+
+  void
+  setFlowcontrol (flowcontrol_t flowcontrol);
+
+  flowcontrol_t
+  getFlowcontrol () const;
+
+  void
+  readLock ();
+
+  void
+  readUnlock ();
+
+  void
+  writeLock ();
+
+  void
+  writeUnlock ();
+
+protected:
+  void reconfigurePort ();
+
+private:
+  string port_;               // Path to the file descriptor
+  int fd_;                    // The current file descriptor
+
+  bool is_open_;
+  bool xonxoff_;
+  bool rtscts_;
+
+  Timeout timeout_;           // Timeout for read operations
+  unsigned long baudrate_;    // Baudrate
+  uint32_t byte_time_ns_;     // Nanoseconds to transmit/receive a single byte
+
+  parity_t parity_;           // Parity
+  bytesize_t bytesize_;       // Size of the bytes
+  stopbits_t stopbits_;       // Stop Bits
+  flowcontrol_t flowcontrol_; // Flow Control
+
+  // Mutex used to lock the read functions
+  pthread_mutex_t read_mutex;
+  // Mutex used to lock the write functions
+  pthread_mutex_t write_mutex;
+};
+
+}
+
+#endif // SERIAL_IMPL_UNIX_H
+
+#endif // !defined(_WIN32)

auto_drive_proj_utils/SerialCommunicaiton/v8stdint.h

@@ -0,0 +1,57 @@
+// This header is from the v8 google project:
+// http://code.google.com/p/v8/source/browse/trunk/include/v8stdint.h
+
+// Copyright 2012 the V8 project authors. All rights reserved.
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above
+//       copyright notice, this list of conditions and the following
+//       disclaimer in the documentation and/or other materials provided
+//       with the distribution.
+//     * Neither the name of Google Inc. nor the names of its
+//       contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+// Load definitions of standard types.
+
+#ifndef V8STDINT_H_
+#define V8STDINT_H_
+
+#include <stddef.h>
+#include <stdio.h>
+
+#if defined(_WIN32) && !defined(__MINGW32__)
+
+typedef signed char int8_t;
+typedef unsigned char uint8_t;
+typedef short int16_t;  // NOLINT
+typedef unsigned short uint16_t;  // NOLINT
+typedef int int32_t;
+typedef unsigned int uint32_t;
+typedef __int64 int64_t;
+typedef unsigned __int64 uint64_t;
+// intptr_t and friends are defined in crtdefs.h through stdio.h.
+
+#else
+
+#include <stdint.h>
+
+#endif
+
+#endif  // V8STDINT_H_

auto_drive_proj_utils/SerialCommunicaiton/win.cc

@@ -0,0 +1,646 @@
+#if defined(_WIN32)
+
+/* Copyright 2012 William Woodall and John Harrison */
+
+#include <sstream>
+
+#include "serial/impl/win.h"
+
+using std::string;
+using std::wstring;
+using std::stringstream;
+using std::invalid_argument;
+using serial::Serial;
+using serial::Timeout;
+using serial::bytesize_t;
+using serial::parity_t;
+using serial::stopbits_t;
+using serial::flowcontrol_t;
+using serial::SerialException;
+using serial::PortNotOpenedException;
+using serial::IOException;
+
+inline wstring
+_prefix_port_if_needed(const wstring &input)
+{
+  static wstring windows_com_port_prefix = L"\\\\.\\";
+  if (input.compare(0, windows_com_port_prefix.size(), windows_com_port_prefix) != 0)
+  {
+    return windows_com_port_prefix + input;
+  }
+  return input;
+}
+
+Serial::SerialImpl::SerialImpl (const string &port, unsigned long baudrate,
+                                bytesize_t bytesize,
+                                parity_t parity, stopbits_t stopbits,
+                                flowcontrol_t flowcontrol)
+  : port_ (port.begin(), port.end()), fd_ (INVALID_HANDLE_VALUE), is_open_ (false),
+    baudrate_ (baudrate), parity_ (parity),
+    bytesize_ (bytesize), stopbits_ (stopbits), flowcontrol_ (flowcontrol)
+{
+  if (port_.empty () == false)
+    open ();
+  read_mutex = CreateMutex(NULL, false, NULL);
+  write_mutex = CreateMutex(NULL, false, NULL);
+}
+
+Serial::SerialImpl::~SerialImpl ()
+{
+  this->close();
+  CloseHandle(read_mutex);
+  CloseHandle(write_mutex);
+}
+
+void
+Serial::SerialImpl::open ()
+{
+  if (port_.empty ()) {
+    throw invalid_argument ("Empty port is invalid.");
+  }
+  if (is_open_ == true) {
+    throw SerialException ("Serial port already open.");
+  }
+
+  // See: https://github.com/wjwwood/serial/issues/84
+  wstring port_with_prefix = _prefix_port_if_needed(port_);
+  LPCWSTR lp_port = port_with_prefix.c_str();
+  fd_ = CreateFileW(lp_port,
+                    GENERIC_READ | GENERIC_WRITE,
+                    0,
+                    0,
+                    OPEN_EXISTING,
+                    FILE_ATTRIBUTE_NORMAL,
+                    0);
+
+  if (fd_ == INVALID_HANDLE_VALUE) {
+    DWORD create_file_err = GetLastError();
+	stringstream ss;
+    switch (create_file_err) {
+    case ERROR_FILE_NOT_FOUND:
+      // Use this->getPort to convert to a std::string
+      ss << "Specified port, " << this->getPort() << ", does not exist.";
+      THROW (IOException, ss.str().c_str());
+    default:
+      ss << "Unknown error opening the serial port: " << create_file_err;
+      THROW (IOException, ss.str().c_str());
+    }
+  }
+
+  reconfigurePort();
+  is_open_ = true;
+}
+
+void
+Serial::SerialImpl::reconfigurePort ()
+{
+  if (fd_ == INVALID_HANDLE_VALUE) {
+    // Can only operate on a valid file descriptor
+    THROW (IOException, "Invalid file descriptor, is the serial port open?");
+  }
+
+  DCB dcbSerialParams = {0};
+
+  dcbSerialParams.DCBlength=sizeof(dcbSerialParams);
+
+  if (!GetCommState(fd_, &dcbSerialParams)) {
+    //error getting state
+    THROW (IOException, "Error getting the serial port state.");
+  }
+
+  // setup baud rate
+  switch (baudrate_) {
+#ifdef CBR_0
+  case 0: dcbSerialParams.BaudRate = CBR_0; break;
+#endif
+#ifdef CBR_50
+  case 50: dcbSerialParams.BaudRate = CBR_50; break;
+#endif
+#ifdef CBR_75
+  case 75: dcbSerialParams.BaudRate = CBR_75; break;
+#endif
+#ifdef CBR_110
+  case 110: dcbSerialParams.BaudRate = CBR_110; break;
+#endif
+#ifdef CBR_134
+  case 134: dcbSerialParams.BaudRate = CBR_134; break;
+#endif
+#ifdef CBR_150
+  case 150: dcbSerialParams.BaudRate = CBR_150; break;
+#endif
+#ifdef CBR_200
+  case 200: dcbSerialParams.BaudRate = CBR_200; break;
+#endif
+#ifdef CBR_300
+  case 300: dcbSerialParams.BaudRate = CBR_300; break;
+#endif
+#ifdef CBR_600
+  case 600: dcbSerialParams.BaudRate = CBR_600; break;
+#endif
+#ifdef CBR_1200
+  case 1200: dcbSerialParams.BaudRate = CBR_1200; break;
+#endif
+#ifdef CBR_1800
+  case 1800: dcbSerialParams.BaudRate = CBR_1800; break;
+#endif
+#ifdef CBR_2400
+  case 2400: dcbSerialParams.BaudRate = CBR_2400; break;
+#endif
+#ifdef CBR_4800
+  case 4800: dcbSerialParams.BaudRate = CBR_4800; break;
+#endif
+#ifdef CBR_7200
+  case 7200: dcbSerialParams.BaudRate = CBR_7200; break;
+#endif
+#ifdef CBR_9600
+  case 9600: dcbSerialParams.BaudRate = CBR_9600; break;
+#endif
+#ifdef CBR_14400
+  case 14400: dcbSerialParams.BaudRate = CBR_14400; break;
+#endif
+#ifdef CBR_19200
+  case 19200: dcbSerialParams.BaudRate = CBR_19200; break;
+#endif
+#ifdef CBR_28800
+  case 28800: dcbSerialParams.BaudRate = CBR_28800; break;
+#endif
+#ifdef CBR_57600
+  case 57600: dcbSerialParams.BaudRate = CBR_57600; break;
+#endif
+#ifdef CBR_76800
+  case 76800: dcbSerialParams.BaudRate = CBR_76800; break;
+#endif
+#ifdef CBR_38400
+  case 38400: dcbSerialParams.BaudRate = CBR_38400; break;
+#endif
+#ifdef CBR_115200
+  case 115200: dcbSerialParams.BaudRate = CBR_115200; break;
+#endif
+#ifdef CBR_128000
+  case 128000: dcbSerialParams.BaudRate = CBR_128000; break;
+#endif
+#ifdef CBR_153600
+  case 153600: dcbSerialParams.BaudRate = CBR_153600; break;
+#endif
+#ifdef CBR_230400
+  case 230400: dcbSerialParams.BaudRate = CBR_230400; break;
+#endif
+#ifdef CBR_256000
+  case 256000: dcbSerialParams.BaudRate = CBR_256000; break;
+#endif
+#ifdef CBR_460800
+  case 460800: dcbSerialParams.BaudRate = CBR_460800; break;
+#endif
+#ifdef CBR_921600
+  case 921600: dcbSerialParams.BaudRate = CBR_921600; break;
+#endif
+  default:
+    // Try to blindly assign it
+    dcbSerialParams.BaudRate = baudrate_;
+  }
+
+  // setup char len
+  if (bytesize_ == eightbits)
+    dcbSerialParams.ByteSize = 8;
+  else if (bytesize_ == sevenbits)
+    dcbSerialParams.ByteSize = 7;
+  else if (bytesize_ == sixbits)
+    dcbSerialParams.ByteSize = 6;
+  else if (bytesize_ == fivebits)
+    dcbSerialParams.ByteSize = 5;
+  else
+    throw invalid_argument ("invalid char len");
+
+  // setup stopbits
+  if (stopbits_ == stopbits_one)
+    dcbSerialParams.StopBits = ONESTOPBIT;
+  else if (stopbits_ == stopbits_one_point_five)
+    dcbSerialParams.StopBits = ONE5STOPBITS;
+  else if (stopbits_ == stopbits_two)
+    dcbSerialParams.StopBits = TWOSTOPBITS;
+  else
+    throw invalid_argument ("invalid stop bit");
+
+  // setup parity
+  if (parity_ == parity_none) {
+    dcbSerialParams.Parity = NOPARITY;
+  } else if (parity_ == parity_even) {
+    dcbSerialParams.Parity = EVENPARITY;
+  } else if (parity_ == parity_odd) {
+    dcbSerialParams.Parity = ODDPARITY;
+  } else if (parity_ == parity_mark) {
+    dcbSerialParams.Parity = MARKPARITY;
+  } else if (parity_ == parity_space) {
+    dcbSerialParams.Parity = SPACEPARITY;
+  } else {
+    throw invalid_argument ("invalid parity");
+  }
+
+  // setup flowcontrol
+  if (flowcontrol_ == flowcontrol_none) {
+    dcbSerialParams.fOutxCtsFlow = false;
+    dcbSerialParams.fRtsControl = RTS_CONTROL_DISABLE;
+    dcbSerialParams.fOutX = false;
+    dcbSerialParams.fInX = false;
+  }
+  if (flowcontrol_ == flowcontrol_software) {
+    dcbSerialParams.fOutxCtsFlow = false;
+    dcbSerialParams.fRtsControl = RTS_CONTROL_DISABLE;
+    dcbSerialParams.fOutX = true;
+    dcbSerialParams.fInX = true;
+  }
+  if (flowcontrol_ == flowcontrol_hardware) {
+    dcbSerialParams.fOutxCtsFlow = true;
+    dcbSerialParams.fRtsControl = RTS_CONTROL_HANDSHAKE;
+    dcbSerialParams.fOutX = false;
+    dcbSerialParams.fInX = false;
+  }
+
+  // activate settings
+  if (!SetCommState(fd_, &dcbSerialParams)){
+    CloseHandle(fd_);
+    THROW (IOException, "Error setting serial port settings.");
+  }
+
+  // Setup timeouts
+  COMMTIMEOUTS timeouts = {0};
+  timeouts.ReadIntervalTimeout = timeout_.inter_byte_timeout;
+  timeouts.ReadTotalTimeoutConstant = timeout_.read_timeout_constant;
+  timeouts.ReadTotalTimeoutMultiplier = timeout_.read_timeout_multiplier;
+  timeouts.WriteTotalTimeoutConstant = timeout_.write_timeout_constant;
+  timeouts.WriteTotalTimeoutMultiplier = timeout_.write_timeout_multiplier;
+  if (!SetCommTimeouts(fd_, &timeouts)) {
+    THROW (IOException, "Error setting timeouts.");
+  }
+}
+
+void
+Serial::SerialImpl::close ()
+{
+  if (is_open_ == true) {
+    if (fd_ != INVALID_HANDLE_VALUE) {
+      int ret;
+      ret = CloseHandle(fd_);
+      if (ret == 0) {
+        stringstream ss;
+        ss << "Error while closing serial port: " << GetLastError();
+        THROW (IOException, ss.str().c_str());
+      } else {
+        fd_ = INVALID_HANDLE_VALUE;
+      }
+    }
+    is_open_ = false;
+  }
+}
+
+bool
+Serial::SerialImpl::isOpen () const
+{
+  return is_open_;
+}
+
+size_t
+Serial::SerialImpl::available ()
+{
+  if (!is_open_) {
+    return 0;
+  }
+  COMSTAT cs;
+  if (!ClearCommError(fd_, NULL, &cs)) {
+    stringstream ss;
+    ss << "Error while checking status of the serial port: " << GetLastError();
+    THROW (IOException, ss.str().c_str());
+  }
+  return static_cast<size_t>(cs.cbInQue);
+}
+
+bool
+Serial::SerialImpl::waitReadable (uint32_t /*timeout*/)
+{
+  THROW (IOException, "waitReadable is not implemented on Windows.");
+  return false;
+}
+
+void
+Serial::SerialImpl::waitByteTimes (size_t /*count*/)
+{
+  THROW (IOException, "waitByteTimes is not implemented on Windows.");
+}
+
+size_t
+Serial::SerialImpl::read (uint8_t *buf, size_t size)
+{
+  if (!is_open_) {
+    throw PortNotOpenedException ("Serial::read");
+  }
+  DWORD bytes_read;
+  if (!ReadFile(fd_, buf, static_cast<DWORD>(size), &bytes_read, NULL)) {
+    stringstream ss;
+    ss << "Error while reading from the serial port: " << GetLastError();
+    THROW (IOException, ss.str().c_str());
+  }
+  return (size_t) (bytes_read);
+}
+
+size_t
+Serial::SerialImpl::write (const uint8_t *data, size_t length)
+{
+  if (is_open_ == false) {
+    throw PortNotOpenedException ("Serial::write");
+  }
+  DWORD bytes_written;
+  if (!WriteFile(fd_, data, static_cast<DWORD>(length), &bytes_written, NULL)) {
+    stringstream ss;
+    ss << "Error while writing to the serial port: " << GetLastError();
+    THROW (IOException, ss.str().c_str());
+  }
+  return (size_t) (bytes_written);
+}
+
+void
+Serial::SerialImpl::setPort (const string &port)
+{
+  port_ = wstring(port.begin(), port.end());
+}
+
+string
+Serial::SerialImpl::getPort () const
+{
+  return string(port_.begin(), port_.end());
+}
+
+void
+Serial::SerialImpl::setTimeout (serial::Timeout &timeout)
+{
+  timeout_ = timeout;
+  if (is_open_) {
+    reconfigurePort ();
+  }
+}
+
+serial::Timeout
+Serial::SerialImpl::getTimeout () const
+{
+  return timeout_;
+}
+
+void
+Serial::SerialImpl::setBaudrate (unsigned long baudrate)
+{
+  baudrate_ = baudrate;
+  if (is_open_) {
+    reconfigurePort ();
+  }
+}
+
+unsigned long
+Serial::SerialImpl::getBaudrate () const
+{
+  return baudrate_;
+}
+
+void
+Serial::SerialImpl::setBytesize (serial::bytesize_t bytesize)
+{
+  bytesize_ = bytesize;
+  if (is_open_) {
+    reconfigurePort ();
+  }
+}
+
+serial::bytesize_t
+Serial::SerialImpl::getBytesize () const
+{
+  return bytesize_;
+}
+
+void
+Serial::SerialImpl::setParity (serial::parity_t parity)
+{
+  parity_ = parity;
+  if (is_open_) {
+    reconfigurePort ();
+  }
+}
+
+serial::parity_t
+Serial::SerialImpl::getParity () const
+{
+  return parity_;
+}
+
+void
+Serial::SerialImpl::setStopbits (serial::stopbits_t stopbits)
+{
+  stopbits_ = stopbits;
+  if (is_open_) {
+    reconfigurePort ();
+  }
+}
+
+serial::stopbits_t
+Serial::SerialImpl::getStopbits () const
+{
+  return stopbits_;
+}
+
+void
+Serial::SerialImpl::setFlowcontrol (serial::flowcontrol_t flowcontrol)
+{
+  flowcontrol_ = flowcontrol;
+  if (is_open_) {
+    reconfigurePort ();
+  }
+}
+
+serial::flowcontrol_t
+Serial::SerialImpl::getFlowcontrol () const
+{
+  return flowcontrol_;
+}
+
+void
+Serial::SerialImpl::flush ()
+{
+  if (is_open_ == false) {
+    throw PortNotOpenedException ("Serial::flush");
+  }
+  FlushFileBuffers (fd_);
+}
+
+void
+Serial::SerialImpl::flushInput ()
+{
+  if (is_open_ == false) {
+    throw PortNotOpenedException("Serial::flushInput");
+  }
+  PurgeComm(fd_, PURGE_RXCLEAR);
+}
+
+void
+Serial::SerialImpl::flushOutput ()
+{
+  if (is_open_ == false) {
+    throw PortNotOpenedException("Serial::flushOutput");
+  }
+  PurgeComm(fd_, PURGE_TXCLEAR);
+}
+
+void
+Serial::SerialImpl::sendBreak (int /*duration*/)
+{
+  THROW (IOException, "sendBreak is not supported on Windows.");
+}
+
+void
+Serial::SerialImpl::setBreak (bool level)
+{
+  if (is_open_ == false) {
+    throw PortNotOpenedException ("Serial::setBreak");
+  }
+  if (level) {
+    EscapeCommFunction (fd_, SETBREAK);
+  } else {
+    EscapeCommFunction (fd_, CLRBREAK);
+  }
+}
+
+void
+Serial::SerialImpl::setRTS (bool level)
+{
+  if (is_open_ == false) {
+    throw PortNotOpenedException ("Serial::setRTS");
+  }
+  if (level) {
+    EscapeCommFunction (fd_, SETRTS);
+  } else {
+    EscapeCommFunction (fd_, CLRRTS);
+  }
+}
+
+void
+Serial::SerialImpl::setDTR (bool level)
+{
+  if (is_open_ == false) {
+    throw PortNotOpenedException ("Serial::setDTR");
+  }
+  if (level) {
+    EscapeCommFunction (fd_, SETDTR);
+  } else {
+    EscapeCommFunction (fd_, CLRDTR);
+  }
+}
+
+bool
+Serial::SerialImpl::waitForChange ()
+{
+  if (is_open_ == false) {
+    throw PortNotOpenedException ("Serial::waitForChange");
+  }
+  DWORD dwCommEvent;
+
+  if (!SetCommMask(fd_, EV_CTS | EV_DSR | EV_RING | EV_RLSD)) {
+    // Error setting communications mask
+    return false;
+  }
+
+  if (!WaitCommEvent(fd_, &dwCommEvent, NULL)) {
+    // An error occurred waiting for the event.
+    return false;
+  } else {
+    // Event has occurred.
+    return true;
+  }
+}
+
+bool
+Serial::SerialImpl::getCTS ()
+{
+  if (is_open_ == false) {
+    throw PortNotOpenedException ("Serial::getCTS");
+  }
+  DWORD dwModemStatus;
+  if (!GetCommModemStatus(fd_, &dwModemStatus)) {
+    THROW (IOException, "Error getting the status of the CTS line.");
+  }
+
+  return (MS_CTS_ON & dwModemStatus) != 0;
+}
+
+bool
+Serial::SerialImpl::getDSR ()
+{
+  if (is_open_ == false) {
+    throw PortNotOpenedException ("Serial::getDSR");
+  }
+  DWORD dwModemStatus;
+  if (!GetCommModemStatus(fd_, &dwModemStatus)) {
+    THROW (IOException, "Error getting the status of the DSR line.");
+  }
+
+  return (MS_DSR_ON & dwModemStatus) != 0;
+}
+
+bool
+Serial::SerialImpl::getRI()
+{
+  if (is_open_ == false) {
+    throw PortNotOpenedException ("Serial::getRI");
+  }
+  DWORD dwModemStatus;
+  if (!GetCommModemStatus(fd_, &dwModemStatus)) {
+    THROW (IOException, "Error getting the status of the RI line.");
+  }
+
+  return (MS_RING_ON & dwModemStatus) != 0;
+}
+
+bool
+Serial::SerialImpl::getCD()
+{
+  if (is_open_ == false) {
+    throw PortNotOpenedException ("Serial::getCD");
+  }
+  DWORD dwModemStatus;
+  if (!GetCommModemStatus(fd_, &dwModemStatus)) {
+    // Error in GetCommModemStatus;
+    THROW (IOException, "Error getting the status of the CD line.");
+  }
+
+  return (MS_RLSD_ON & dwModemStatus) != 0;
+}
+
+void
+Serial::SerialImpl::readLock()
+{
+  if (WaitForSingleObject(read_mutex, INFINITE) != WAIT_OBJECT_0) {
+    THROW (IOException, "Error claiming read mutex.");
+  }
+}
+
+void
+Serial::SerialImpl::readUnlock()
+{
+  if (!ReleaseMutex(read_mutex)) {
+    THROW (IOException, "Error releasing read mutex.");
+  }
+}
+
+void
+Serial::SerialImpl::writeLock()
+{
+  if (WaitForSingleObject(write_mutex, INFINITE) != WAIT_OBJECT_0) {
+    THROW (IOException, "Error claiming write mutex.");
+  }
+}
+
+void
+Serial::SerialImpl::writeUnlock()
+{
+  if (!ReleaseMutex(write_mutex)) {
+    THROW (IOException, "Error releasing write mutex.");
+  }
+}
+
+#endif // #if defined(_WIN32)
+

auto_drive_proj_utils/SerialCommunicaiton/win.h

@@ -0,0 +1,207 @@
+/*!
+ * \file serial/impl/windows.h
+ * \author  William Woodall <wjwwood@gmail.com>
+ * \author  John Harrison <ash@greaterthaninfinity.com>
+ * \version 0.1
+ *
+ * \section LICENSE
+ *
+ * The MIT License
+ *
+ * Copyright (c) 2012 William Woodall, John Harrison
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ *
+ * \section DESCRIPTION
+ *
+ * This provides a windows implementation of the Serial class interface.
+ *
+ */
+
+#if defined(_WIN32)
+
+#ifndef SERIAL_IMPL_WINDOWS_H
+#define SERIAL_IMPL_WINDOWS_H
+
+#include "serial/serial.h"
+
+#include "windows.h"
+
+namespace serial {
+
+using std::string;
+using std::wstring;
+using std::invalid_argument;
+
+using serial::SerialException;
+using serial::IOException;
+
+class serial::Serial::SerialImpl {
+public:
+  SerialImpl (const string &port,
+              unsigned long baudrate,
+              bytesize_t bytesize,
+              parity_t parity,
+              stopbits_t stopbits,
+              flowcontrol_t flowcontrol);
+
+  virtual ~SerialImpl ();
+
+  void
+  open ();
+
+  void
+  close ();
+
+  bool
+  isOpen () const;
+
+  size_t
+  available ();
+  
+  bool
+  waitReadable (uint32_t timeout);
+
+  void
+  waitByteTimes (size_t count);
+
+  size_t
+  read (uint8_t *buf, size_t size = 1);
+
+  size_t
+  write (const uint8_t *data, size_t length);
+
+  void
+  flush ();
+
+  void
+  flushInput ();
+
+  void
+  flushOutput ();
+
+  void
+  sendBreak (int duration);
+
+  void
+  setBreak (bool level);
+
+  void
+  setRTS (bool level);
+
+  void
+  setDTR (bool level);
+
+  bool
+  waitForChange ();
+
+  bool
+  getCTS ();
+
+  bool
+  getDSR ();
+
+  bool
+  getRI ();
+
+  bool
+  getCD ();
+
+  void
+  setPort (const string &port);
+
+  string
+  getPort () const;
+
+  void
+  setTimeout (Timeout &timeout);
+
+  Timeout
+  getTimeout () const;
+
+  void
+  setBaudrate (unsigned long baudrate);
+
+  unsigned long
+  getBaudrate () const;
+
+  void
+  setBytesize (bytesize_t bytesize);
+
+  bytesize_t
+  getBytesize () const;
+
+  void
+  setParity (parity_t parity);
+
+  parity_t
+  getParity () const;
+
+  void
+  setStopbits (stopbits_t stopbits);
+
+  stopbits_t
+  getStopbits () const;
+
+  void
+  setFlowcontrol (flowcontrol_t flowcontrol);
+
+  flowcontrol_t
+  getFlowcontrol () const;
+
+  void
+  readLock ();
+
+  void
+  readUnlock ();
+
+  void
+  writeLock ();
+
+  void
+  writeUnlock ();
+
+protected:
+  void reconfigurePort ();
+
+private:
+  wstring port_;               // Path to the file descriptor
+  HANDLE fd_;
+
+  bool is_open_;
+
+  Timeout timeout_;           // Timeout for read operations
+  unsigned long baudrate_;    // Baudrate
+
+  parity_t parity_;           // Parity
+  bytesize_t bytesize_;       // Size of the bytes
+  stopbits_t stopbits_;       // Stop Bits
+  flowcontrol_t flowcontrol_; // Flow Control
+
+  // Mutex used to lock the read functions
+  HANDLE read_mutex;
+  // Mutex used to lock the write functions
+  HANDLE write_mutex;
+};
+
+}
+
+#endif // SERIAL_IMPL_WINDOWS_H
+
+#endif // if defined(_WIN32)

auto_drive_proj_utils/ThreadPool/BS_thread_pool.hpp

@@ -0,0 +1,763 @@
+#pragma once
+
+/**
+ * @file BS_thread_pool.hpp
+ * @author Barak Shoshany (baraksh@gmail.com) (http://baraksh.com)
+ * @version 3.3.0
+ * @date 2022-08-03
+ * @copyright Copyright (c) 2022 Barak Shoshany. Licensed under the MIT license. If you found this project useful, please consider starring it on GitHub! If you use this library in software of any kind, please provide a link to the GitHub repository https://github.com/bshoshany/thread-pool in the source code and documentation. If you use this library in published research, please cite it as follows: Barak Shoshany, "A C++17 Thread Pool for High-Performance Scientific Computing", doi:10.5281/zenodo.4742687, arXiv:2105.00613 (May 2021)
+ *
+ * @brief BS::thread_pool: a fast, lightweight, and easy-to-use C++17 thread pool library. This header file contains the entire library, including the main BS::thread_pool class and the helper classes BS::multi_future, BS::blocks, BS:synced_stream, and BS::timer.
+ */
+
+#define BS_THREAD_POOL_VERSION "v3.3.0 (2022-08-03)"
+
+#include <atomic>             // std::atomic
+#include <chrono>             // std::chrono
+#include <condition_variable> // std::condition_variable
+#include <exception>          // std::current_exception
+#include <functional>         // std::bind, std::function, std::invoke
+#include <future>             // std::future, std::promise
+#include <iostream>           // std::cout, std::endl, std::flush, std::ostream
+#include <memory>             // std::make_shared, std::make_unique, std::shared_ptr, std::unique_ptr
+#include <mutex>              // std::mutex, std::scoped_lock, std::unique_lock
+#include <queue>              // std::queue
+#include <thread>             // std::thread
+#include <type_traits>        // std::common_type_t, std::conditional_t, std::decay_t, std::invoke_result_t, std::is_void_v
+#include <utility>            // std::forward, std::move, std::swap
+#include <vector>             // std::vector
+
+namespace BS
+{
+/**
+ * @brief A convenient shorthand for the type of std::thread::hardware_concurrency(). Should evaluate to unsigned int.
+ */
+using concurrency_t = std::invoke_result_t<decltype(std::thread::hardware_concurrency)>;
+
+// ============================================================================================= //
+//                                    Begin class multi_future                                   //
+
+/**
+ * @brief A helper class to facilitate waiting for and/or getting the results of multiple futures at once.
+ *
+ * @tparam T The return type of the futures.
+ */
+template <typename T>
+class [[nodiscard]] multi_future
+{
+public:
+    /**
+     * @brief Construct a multi_future object with the given number of futures.
+     *
+     * @param num_futures_ The desired number of futures to store.
+     */
+    multi_future(const size_t num_futures_ = 0) : futures(num_futures_) {}
+
+    /**
+     * @brief Get the results from all the futures stored in this multi_future object, rethrowing any stored exceptions.
+     *
+     * @return If the futures return void, this function returns void as well. Otherwise, it returns a vector containing the results.
+     */
+    [[nodiscard]] std::conditional_t<std::is_void_v<T>, void, std::vector<T>> get()
+    {
+        if constexpr (std::is_void_v<T>)
+        {
+            for (size_t i = 0; i < futures.size(); ++i)
+                futures[i].get();
+            return;
+        }
+        else
+        {
+            std::vector<T> results(futures.size());
+            for (size_t i = 0; i < futures.size(); ++i)
+                results[i] = futures[i].get();
+            return results;
+        }
+    }
+
+    /**
+     * @brief Get a reference to one of the futures stored in this multi_future object.
+     *
+     * @param i The index of the desired future.
+     * @return The future.
+     */
+    [[nodiscard]] std::future<T>& operator[](const size_t i)
+    {
+        return futures[i];
+    }
+
+    /**
+     * @brief Append a future to this multi_future object.
+     *
+     * @param future The future to append.
+     */
+    void push_back(std::future<T> future)
+    {
+        futures.push_back(std::move(future));
+    }
+
+    /**
+     * @brief Get the number of futures stored in this multi_future object.
+     *
+     * @return The number of futures.
+     */
+    [[nodiscard]] size_t size() const
+    {
+        return futures.size();
+    }
+
+    /**
+     * @brief Wait for all the futures stored in this multi_future object.
+     */
+    void wait() const
+    {
+        for (size_t i = 0; i < futures.size(); ++i)
+            futures[i].wait();
+    }
+
+private:
+    /**
+     * @brief A vector to store the futures.
+     */
+    std::vector<std::future<T>> futures;
+};
+
+//                                     End class multi_future                                    //
+// ============================================================================================= //
+
+// ============================================================================================= //
+//                                       Begin class blocks                                      //
+
+/**
+ * @brief A helper class to divide a range into blocks. Used by parallelize_loop() and push_loop().
+ *
+ * @tparam T1 The type of the first index in the range. Should be a signed or unsigned integer.
+ * @tparam T2 The type of the index after the last index in the range. Should be a signed or unsigned integer. If T1 is not the same as T2, a common type will be automatically inferred.
+ * @tparam T The common type of T1 and T2.
+ */
+template <typename T1, typename T2, typename T = std::common_type_t<T1, T2>>
+class [[nodiscard]] blocks
+{
+public:
+    /**
+     * @brief Construct a blocks object with the given specifications.
+     *
+     * @param first_index_ The first index in the range.
+     * @param index_after_last_ The index after the last index in the range.
+     * @param num_blocks_ The desired number of blocks to divide the range into.
+     */
+    blocks(const T1 first_index_, const T2 index_after_last_, const size_t num_blocks_) : first_index(static_cast<T>(first_index_)), index_after_last(static_cast<T>(index_after_last_)), num_blocks(num_blocks_)
+    {
+        if (index_after_last < first_index)
+            std::swap(index_after_last, first_index);
+        total_size = static_cast<size_t>(index_after_last - first_index);
+        block_size = static_cast<size_t>(total_size / num_blocks);
+        if (block_size == 0)
+        {
+            block_size = 1;
+            num_blocks = (total_size > 1) ? total_size : 1;
+        }
+    }
+
+    /**
+     * @brief Get the first index of a block.
+     *
+     * @param i The block number.
+     * @return The first index.
+     */
+    [[nodiscard]] T start(const size_t i) const
+    {
+        return static_cast<T>(i * block_size) + first_index;
+    }
+
+    /**
+     * @brief Get the index after the last index of a block.
+     *
+     * @param i The block number.
+     * @return The index after the last index.
+     */
+    [[nodiscard]] T end(const size_t i) const
+    {
+        return (i == num_blocks - 1) ? index_after_last : (static_cast<T>((i + 1) * block_size) + first_index);
+    }
+
+    /**
+     * @brief Get the number of blocks. Note that this may be different than the desired number of blocks that was passed to the constructor.
+     *
+     * @return The number of blocks.
+     */
+    [[nodiscard]] size_t get_num_blocks() const
+    {
+        return num_blocks;
+    }
+
+    /**
+     * @brief Get the total number of indices in the range.
+     *
+     * @return The total number of indices.
+     */
+    [[nodiscard]] size_t get_total_size() const
+    {
+        return total_size;
+    }
+
+private:
+    /**
+     * @brief The size of each block (except possibly the last block).
+     */
+    size_t block_size = 0;
+
+    /**
+     * @brief The first index in the range.
+     */
+    T first_index = 0;
+
+    /**
+     * @brief The index after the last index in the range.
+     */
+    T index_after_last = 0;
+
+    /**
+     * @brief The number of blocks.
+     */
+    size_t num_blocks = 0;
+
+    /**
+     * @brief The total number of indices in the range.
+     */
+    size_t total_size = 0;
+};
+
+//                                        End class blocks                                       //
+// ============================================================================================= //
+
+// ============================================================================================= //
+//                                    Begin class thread_pool                                    //
+
+/**
+ * @brief A fast, lightweight, and easy-to-use C++17 thread pool class.
+ */
+class [[nodiscard]] thread_pool
+{
+public:
+    // ============================
+    // Constructors and destructors
+    // ============================
+
+    /**
+     * @brief Construct a new thread pool.
+     *
+     * @param thread_count_ The number of threads to use. The default value is the total number of hardware threads available, as reported by the implementation. This is usually determined by the number of cores in the CPU. If a core is hyperthreaded, it will count as two threads.
+     */
+    thread_pool(const concurrency_t thread_count_ = 0) : thread_count(determine_thread_count(thread_count_)), threads(std::make_unique<std::thread[]>(determine_thread_count(thread_count_)))
+    {
+        create_threads();
+    }
+
+    /**
+     * @brief Destruct the thread pool. Waits for all tasks to complete, then destroys all threads. Note that if the pool is paused, then any tasks still in the queue will never be executed.
+     */
+    ~thread_pool()
+    {
+        wait_for_tasks();
+        destroy_threads();
+    }
+
+    // =======================
+    // Public member functions
+    // =======================
+
+    /**
+     * @brief Get the number of tasks currently waiting in the queue to be executed by the threads.
+     *
+     * @return The number of queued tasks.
+     */
+    [[nodiscard]] size_t get_tasks_queued() const
+    {
+        const std::scoped_lock tasks_lock(tasks_mutex);
+        return tasks.size();
+    }
+
+    /**
+     * @brief Get the number of tasks currently being executed by the threads.
+     *
+     * @return The number of running tasks.
+     */
+    [[nodiscard]] size_t get_tasks_running() const
+    {
+        const std::scoped_lock tasks_lock(tasks_mutex);
+        return tasks_total - tasks.size();
+    }
+
+    /**
+     * @brief Get the total number of unfinished tasks: either still in the queue, or running in a thread. Note that get_tasks_total() == get_tasks_queued() + get_tasks_running().
+     *
+     * @return The total number of tasks.
+     */
+    [[nodiscard]] size_t get_tasks_total() const
+    {
+        return tasks_total;
+    }
+
+    /**
+     * @brief Get the number of threads in the pool.
+     *
+     * @return The number of threads.
+     */
+    [[nodiscard]] concurrency_t get_thread_count() const
+    {
+        return thread_count;
+    }
+
+    /**
+     * @brief Check whether the pool is currently paused.
+     *
+     * @return true if the pool is paused, false if it is not paused.
+     */
+    [[nodiscard]] bool is_paused() const
+    {
+        return paused;
+    }
+
+    /**
+     * @brief Parallelize a loop by automatically splitting it into blocks and submitting each block separately to the queue. Returns a multi_future object that contains the futures for all of the blocks.
+     *
+     * @tparam F The type of the function to loop through.
+     * @tparam T1 The type of the first index in the loop. Should be a signed or unsigned integer.
+     * @tparam T2 The type of the index after the last index in the loop. Should be a signed or unsigned integer. If T1 is not the same as T2, a common type will be automatically inferred.
+     * @tparam T The common type of T1 and T2.
+     * @tparam R The return value of the loop function F (can be void).
+     * @param first_index The first index in the loop.
+     * @param index_after_last The index after the last index in the loop. The loop will iterate from first_index to (index_after_last - 1) inclusive. In other words, it will be equivalent to "for (T i = first_index; i < index_after_last; ++i)". Note that if index_after_last == first_index, no blocks will be submitted.
+     * @param loop The function to loop through. Will be called once per block. Should take exactly two arguments: the first index in the block and the index after the last index in the block. loop(start, end) should typically involve a loop of the form "for (T i = start; i < end; ++i)".
+     * @param num_blocks The maximum number of blocks to split the loop into. The default is to use the number of threads in the pool.
+     * @return A multi_future object that can be used to wait for all the blocks to finish. If the loop function returns a value, the multi_future object can also be used to obtain the values returned by each block.
+     */
+    template <typename F, typename T1, typename T2, typename T = std::common_type_t<T1, T2>, typename R = std::invoke_result_t<std::decay_t<F>, T, T>>
+    [[nodiscard]] multi_future<R> parallelize_loop(const T1 first_index, const T2 index_after_last, F&& loop, const size_t num_blocks = 0)
+    {
+        blocks blks(first_index, index_after_last, num_blocks ? num_blocks : thread_count);
+        if (blks.get_total_size() > 0)
+        {
+            multi_future<R> mf(blks.get_num_blocks());
+            for (size_t i = 0; i < blks.get_num_blocks(); ++i)
+                mf[i] = submit(std::forward<F>(loop), blks.start(i), blks.end(i));
+            return mf;
+        }
+        else
+        {
+            return multi_future<R>();
+        }
+    }
+
+    /**
+     * @brief Parallelize a loop by automatically splitting it into blocks and submitting each block separately to the queue. Returns a multi_future object that contains the futures for all of the blocks. This overload is used for the special case where the first index is 0.
+     *
+     * @tparam F The type of the function to loop through.
+     * @tparam T The type of the loop indices. Should be a signed or unsigned integer.
+     * @tparam R The return value of the loop function F (can be void).
+     * @param index_after_last The index after the last index in the loop. The loop will iterate from 0 to (index_after_last - 1) inclusive. In other words, it will be equivalent to "for (T i = 0; i < index_after_last; ++i)". Note that if index_after_last == 0, no blocks will be submitted.
+     * @param loop The function to loop through. Will be called once per block. Should take exactly two arguments: the first index in the block and the index after the last index in the block. loop(start, end) should typically involve a loop of the form "for (T i = start; i < end; ++i)".
+     * @param num_blocks The maximum number of blocks to split the loop into. The default is to use the number of threads in the pool.
+     * @return A multi_future object that can be used to wait for all the blocks to finish. If the loop function returns a value, the multi_future object can also be used to obtain the values returned by each block.
+     */
+    template <typename F, typename T, typename R = std::invoke_result_t<std::decay_t<F>, T, T>>
+    [[nodiscard]] multi_future<R> parallelize_loop(const T index_after_last, F&& loop, const size_t num_blocks = 0)
+    {
+        return parallelize_loop(0, index_after_last, std::forward<F>(loop), num_blocks);
+    }
+
+    /**
+     * @brief Pause the pool. The workers will temporarily stop retrieving new tasks out of the queue, although any tasks already executed will keep running until they are finished.
+     */
+    void pause()
+    {
+        paused = true;
+    }
+
+    /**
+     * @brief Parallelize a loop by automatically splitting it into blocks and submitting each block separately to the queue. Does not return a multi_future, so the user must use wait_for_tasks() or some other method to ensure that the loop finishes executing, otherwise bad things will happen.
+     *
+     * @tparam F The type of the function to loop through.
+     * @tparam T1 The type of the first index in the loop. Should be a signed or unsigned integer.
+     * @tparam T2 The type of the index after the last index in the loop. Should be a signed or unsigned integer. If T1 is not the same as T2, a common type will be automatically inferred.
+     * @tparam T The common type of T1 and T2.
+     * @param first_index The first index in the loop.
+     * @param index_after_last The index after the last index in the loop. The loop will iterate from first_index to (index_after_last - 1) inclusive. In other words, it will be equivalent to "for (T i = first_index; i < index_after_last; ++i)". Note that if index_after_last == first_index, no blocks will be submitted.
+     * @param loop The function to loop through. Will be called once per block. Should take exactly two arguments: the first index in the block and the index after the last index in the block. loop(start, end) should typically involve a loop of the form "for (T i = start; i < end; ++i)".
+     * @param num_blocks The maximum number of blocks to split the loop into. The default is to use the number of threads in the pool.
+     */
+    template <typename F, typename T1, typename T2, typename T = std::common_type_t<T1, T2>>
+    void push_loop(const T1 first_index, const T2 index_after_last, F&& loop, const size_t num_blocks = 0)
+    {
+        blocks blks(first_index, index_after_last, num_blocks ? num_blocks : thread_count);
+        if (blks.get_total_size() > 0)
+        {
+            for (size_t i = 0; i < blks.get_num_blocks(); ++i)
+                push_task(std::forward<F>(loop), blks.start(i), blks.end(i));
+        }
+    }
+
+    /**
+     * @brief Parallelize a loop by automatically splitting it into blocks and submitting each block separately to the queue. Does not return a multi_future, so the user must use wait_for_tasks() or some other method to ensure that the loop finishes executing, otherwise bad things will happen. This overload is used for the special case where the first index is 0.
+     *
+     * @tparam F The type of the function to loop through.
+     * @tparam T The type of the loop indices. Should be a signed or unsigned integer.
+     * @param index_after_last The index after the last index in the loop. The loop will iterate from 0 to (index_after_last - 1) inclusive. In other words, it will be equivalent to "for (T i = 0; i < index_after_last; ++i)". Note that if index_after_last == 0, no blocks will be submitted.
+     * @param loop The function to loop through. Will be called once per block. Should take exactly two arguments: the first index in the block and the index after the last index in the block. loop(start, end) should typically involve a loop of the form "for (T i = start; i < end; ++i)".
+     * @param num_blocks The maximum number of blocks to split the loop into. The default is to use the number of threads in the pool.
+     */
+    template <typename F, typename T>
+    void push_loop(const T index_after_last, F&& loop, const size_t num_blocks = 0)
+    {
+        push_loop(0, index_after_last, std::forward<F>(loop), num_blocks);
+    }
+
+    /**
+     * @brief Push a function with zero or more arguments, but no return value, into the task queue. Does not return a future, so the user must use wait_for_tasks() or some other method to ensure that the task finishes executing, otherwise bad things will happen.
+     *
+     * @tparam F The type of the function.
+     * @tparam A The types of the arguments.
+     * @param task The function to push.
+     * @param args The zero or more arguments to pass to the function. Note that if the task is a class member function, the first argument must be a pointer to the object, i.e. &object (or this), followed by the actual arguments.
+     */
+    template <typename F, typename... A>
+    void push_task(F&& task, A&&... args)
+    {
+        std::function<void()> task_function = std::bind(std::forward<F>(task), std::forward<A>(args)...);
+        {
+            const std::scoped_lock tasks_lock(tasks_mutex);
+            tasks.push(task_function);
+        }
+        ++tasks_total;
+        task_available_cv.notify_one();
+    }
+
+    /**
+     * @brief Reset the number of threads in the pool. Waits for all currently running tasks to be completed, then destroys all threads in the pool and creates a new thread pool with the new number of threads. Any tasks that were waiting in the queue before the pool was reset will then be executed by the new threads. If the pool was paused before resetting it, the new pool will be paused as well.
+     *
+     * @param thread_count_ The number of threads to use. The default value is the total number of hardware threads available, as reported by the implementation. This is usually determined by the number of cores in the CPU. If a core is hyperthreaded, it will count as two threads.
+     */
+    void reset(const concurrency_t thread_count_ = 0)
+    {
+        const bool was_paused = paused;
+        paused = true;
+        wait_for_tasks();
+        destroy_threads();
+        thread_count = determine_thread_count(thread_count_);
+        threads = std::make_unique<std::thread[]>(thread_count);
+        paused = was_paused;
+        create_threads();
+    }
+
+    /**
+     * @brief Submit a function with zero or more arguments into the task queue. If the function has a return value, get a future for the eventual returned value. If the function has no return value, get an std::future<void> which can be used to wait until the task finishes.
+     *
+     * @tparam F The type of the function.
+     * @tparam A The types of the zero or more arguments to pass to the function.
+     * @tparam R The return type of the function (can be void).
+     * @param task The function to submit.
+     * @param args The zero or more arguments to pass to the function. Note that if the task is a class member function, the first argument must be a pointer to the object, i.e. &object (or this), followed by the actual arguments.
+     * @return A future to be used later to wait for the function to finish executing and/or obtain its returned value if it has one.
+     */
+    template <typename F, typename... A, typename R = std::invoke_result_t<std::decay_t<F>, std::decay_t<A>...>>
+    [[nodiscard]] std::future<R> submit(F&& task, A&&... args)
+    {
+        std::function<R()> task_function = std::bind(std::forward<F>(task), std::forward<A>(args)...);
+        std::shared_ptr<std::promise<R>> task_promise = std::make_shared<std::promise<R>>();
+        push_task(
+            [task_function, task_promise]
+            {
+                try
+                {
+                    if constexpr (std::is_void_v<R>)
+                    {
+                        std::invoke(task_function);
+                        task_promise->set_value();
+                    }
+                    else
+                    {
+                        task_promise->set_value(std::invoke(task_function));
+                    }
+                }
+                catch (...)
+                {
+                    try
+                    {
+                        task_promise->set_exception(std::current_exception());
+                    }
+                    catch (...)
+                    {
+                    }
+                }
+            });
+        return task_promise->get_future();
+    }
+
+    /**
+     * @brief Unpause the pool. The workers will resume retrieving new tasks out of the queue.
+     */
+    void unpause()
+    {
+        paused = false;
+    }
+
+    /**
+     * @brief Wait for tasks to be completed. Normally, this function waits for all tasks, both those that are currently running in the threads and those that are still waiting in the queue. However, if the pool is paused, this function only waits for the currently running tasks (otherwise it would wait forever). Note: To wait for just one specific task, use submit() instead, and call the wait() member function of the generated future.
+     */
+    void wait_for_tasks()
+    {
+        waiting = true;
+        std::unique_lock<std::mutex> tasks_lock(tasks_mutex);
+        task_done_cv.wait(tasks_lock, [this] { return (tasks_total == (paused ? tasks.size() : 0)); });
+        waiting = false;
+    }
+
+private:
+    // ========================
+    // Private member functions
+    // ========================
+
+    /**
+     * @brief Create the threads in the pool and assign a worker to each thread.
+     */
+    void create_threads()
+    {
+        running = true;
+        for (concurrency_t i = 0; i < thread_count; ++i)
+        {
+            threads[i] = std::thread(&thread_pool::worker, this);
+        }
+    }
+
+    /**
+     * @brief Destroy the threads in the pool.
+     */
+    void destroy_threads()
+    {
+        running = false;
+        task_available_cv.notify_all();
+        for (concurrency_t i = 0; i < thread_count; ++i)
+        {
+            threads[i].join();
+        }
+    }
+
+    /**
+     * @brief Determine how many threads the pool should have, based on the parameter passed to the constructor or reset().
+     *
+     * @param thread_count_ The parameter passed to the constructor or reset(). If the parameter is a positive number, then the pool will be created with this number of threads. If the parameter is non-positive, or a parameter was not supplied (in which case it will have the default value of 0), then the pool will be created with the total number of hardware threads available, as obtained from std::thread::hardware_concurrency(). If the latter returns a non-positive number for some reason, then the pool will be created with just one thread.
+     * @return The number of threads to use for constructing the pool.
+     */
+    [[nodiscard]] concurrency_t determine_thread_count(const concurrency_t thread_count_)
+    {
+        if (thread_count_ > 0)
+            return thread_count_;
+        else
+        {
+            if (std::thread::hardware_concurrency() > 0)
+                return std::thread::hardware_concurrency();
+            else
+                return 1;
+        }
+    }
+
+    /**
+     * @brief A worker function to be assigned to each thread in the pool. Waits until it is notified by push_task() that a task is available, and then retrieves the task from the queue and executes it. Once the task finishes, the worker notifies wait_for_tasks() in case it is waiting.
+     */
+    void worker()
+    {
+        while (running)
+        {
+            std::function<void()> task;
+            std::unique_lock<std::mutex> tasks_lock(tasks_mutex);
+            task_available_cv.wait(tasks_lock, [this] { return !tasks.empty() || !running; });
+            if (running && !paused)
+            {
+                task = std::move(tasks.front());
+                tasks.pop();
+                tasks_lock.unlock();
+                task();
+                tasks_lock.lock();
+                --tasks_total;
+                if (waiting)
+                    task_done_cv.notify_one();
+            }
+        }
+    }
+
+    // ============
+    // Private data
+    // ============
+
+    /**
+     * @brief An atomic variable indicating whether the workers should pause. When set to true, the workers temporarily stop retrieving new tasks out of the queue, although any tasks already executed will keep running until they are finished. When set to false again, the workers resume retrieving tasks.
+     */
+    std::atomic<bool> paused = false;
+
+    /**
+     * @brief An atomic variable indicating to the workers to keep running. When set to false, the workers permanently stop working.
+     */
+    std::atomic<bool> running = false;
+
+    /**
+     * @brief A condition variable used to notify worker() that a new task has become available.
+     */
+    std::condition_variable task_available_cv = {};
+
+    /**
+     * @brief A condition variable used to notify wait_for_tasks() that a tasks is done.
+     */
+    std::condition_variable task_done_cv = {};
+
+    /**
+     * @brief A queue of tasks to be executed by the threads.
+     */
+    std::queue<std::function<void()>> tasks = {};
+
+    /**
+     * @brief An atomic variable to keep track of the total number of unfinished tasks - either still in the queue, or running in a thread.
+     */
+    std::atomic<size_t> tasks_total = 0;
+
+    /**
+     * @brief A mutex to synchronize access to the task queue by different threads.
+     */
+    mutable std::mutex tasks_mutex = {};
+
+    /**
+     * @brief The number of threads in the pool.
+     */
+    concurrency_t thread_count = 0;
+
+    /**
+     * @brief A smart pointer to manage the memory allocated for the threads.
+     */
+    std::unique_ptr<std::thread[]> threads = nullptr;
+
+    /**
+     * @brief An atomic variable indicating that wait_for_tasks() is active and expects to be notified whenever a task is done.
+     */
+    std::atomic<bool> waiting = false;
+};
+
+//                                     End class thread_pool                                     //
+// ============================================================================================= //
+
+// ============================================================================================= //
+//                                   Begin class synced_stream                                   //
+
+/**
+ * @brief A helper class to synchronize printing to an output stream by different threads.
+ */
+class [[nodiscard]] synced_stream
+{
+public:
+    /**
+     * @brief Construct a new synced stream.
+     *
+     * @param out_stream_ The output stream to print to. The default value is std::cout.
+     */
+    synced_stream(std::ostream& out_stream_ = std::cout) : out_stream(out_stream_) {}
+
+    /**
+     * @brief Print any number of items into the output stream. Ensures that no other threads print to this stream simultaneously, as long as they all exclusively use the same synced_stream object to print.
+     *
+     * @tparam T The types of the items
+     * @param items The items to print.
+     */
+    template <typename... T>
+    void print(T&&... items)
+    {
+        const std::scoped_lock lock(stream_mutex);
+        (out_stream << ... << std::forward<T>(items));
+    }
+
+    /**
+     * @brief Print any number of items into the output stream, followed by a newline character. Ensures that no other threads print to this stream simultaneously, as long as they all exclusively use the same synced_stream object to print.
+     *
+     * @tparam T The types of the items
+     * @param items The items to print.
+     */
+    template <typename... T>
+    void println(T&&... items)
+    {
+        print(std::forward<T>(items)..., '\n');
+    }
+
+    /**
+     * @brief A stream manipulator to pass to a synced_stream (an explicit cast of std::endl). Prints a newline character to the stream, and then flushes it. Should only be used if flushing is desired, otherwise '\n' should be used instead.
+     */
+    inline static std::ostream& (&endl)(std::ostream&) = static_cast<std::ostream& (&)(std::ostream&)>(std::endl);
+
+    /**
+     * @brief A stream manipulator to pass to a synced_stream (an explicit cast of std::flush). Used to flush the stream.
+     */
+    inline static std::ostream& (&flush)(std::ostream&) = static_cast<std::ostream& (&)(std::ostream&)>(std::flush);
+
+private:
+    /**
+     * @brief The output stream to print to.
+     */
+    std::ostream& out_stream;
+
+    /**
+     * @brief A mutex to synchronize printing.
+     */
+    mutable std::mutex stream_mutex = {};
+};
+
+//                                    End class synced_stream                                    //
+// ============================================================================================= //
+
+// ============================================================================================= //
+//                                       Begin class timer                                       //
+
+/**
+ * @brief A helper class to measure execution time for benchmarking purposes.
+ */
+class [[nodiscard]] timer
+{
+public:
+    /**
+     * @brief Start (or restart) measuring time.
+     */
+    void start()
+    {
+        start_time = std::chrono::steady_clock::now();
+    }
+
+    /**
+     * @brief Stop measuring time and store the elapsed time since start().
+     */
+    void stop()
+    {
+        elapsed_time = std::chrono::steady_clock::now() - start_time;
+    }
+
+    /**
+     * @brief Get the number of milliseconds that have elapsed between start() and stop().
+     *
+     * @return The number of milliseconds.
+     */
+    [[nodiscard]] std::chrono::milliseconds::rep ms() const
+    {
+        return (std::chrono::duration_cast<std::chrono::milliseconds>(elapsed_time)).count();
+    }
+
+private:
+    /**
+     * @brief The time point when measuring started.
+     */
+    std::chrono::time_point<std::chrono::steady_clock> start_time = std::chrono::steady_clock::now();
+
+    /**
+     * @brief The duration that has elapsed between start() and stop().
+     */
+    std::chrono::duration<double> elapsed_time = std::chrono::duration<double>::zero();
+};
+
+//                                        End class timer                                        //
+// ============================================================================================= //
+
+} // namespace BS

auto_drive_proj_utils/UTM_WGS84/CoorConv.hpp

@@ -0,0 +1,380 @@
+// LatLong-UTM.c++
+// Conversions:  LatLong to UTM;  and UTM to LatLong;
+// by Eugene Reimer, ereimer@shaw.ca, 2002-December;
+// with LLtoUTM & UTMtoLL routines based on those by Chuck Gantz chuck.gantz@globalstar.com;
+// with ellipsoid & datum constants from Peter H Dana website (http://www.colorado.edu/geography/gcraft/notes/datum/edlist.html);
+//
+// Usage:  see the Usage() routine below;
+//
+// Copyright © 1995,2002,2010 Eugene Reimer, Peter Dana, Chuck Gantz.  Released under the GPL;  see http://www.gnu.org/licenses/gpl.html
+// (Peter Dana's non-commercial clause precludes using the LGPL)
+
+
+#include <cmath>			//2010-08-11: was <math.h>	  
+#include <cstdio>			//2010-08-11: was <stdio.h>	  
+#include <cstdlib>			//2010-08-11: was <stdlib.h>  
+#include <cstring>			//2010-08-11: was <string.h>  
+#include <cctype>			//2010-08-11: was <ctype.h>  
+#include <iostream>			//2010-08-11: was <iostream.h>
+#include <iomanip>			//2010-08-11: was <iomanip.h> 
+using namespace std;			//2010-08-11: added
+const double PI       =	4*atan(1);	//Gantz used: PI=3.14159265;
+const double deg2rad  =	PI/180;
+const double rad2deg  =	180/PI;
+const double k0       =	0.9996;
+
+class Ellipsoid{
+public:
+	Ellipsoid(){};
+	Ellipsoid(int id, char* name, double radius, double fr){
+		Name=name;  EquatorialRadius=radius;  eccSquared=2/fr-1/(fr*fr);
+	}
+	char* Name;
+	double EquatorialRadius; 
+	double eccSquared;
+};
+static Ellipsoid ellip[] = {		//converted from PeterDana website, by Eugene Reimer 2002dec
+//		 eId,  Name,		   EquatorialRadius,    1/flattening;
+	Ellipsoid( 0, "Airy1830",		6377563.396,	299.3249646),
+	Ellipsoid( 1, "AiryModified",		6377340.189,	299.3249646),
+	Ellipsoid( 2, "AustralianNational",	6378160,	298.25),
+	Ellipsoid( 3, "Bessel1841Namibia",	6377483.865,	299.1528128),
+	Ellipsoid( 4, "Bessel1841",		6377397.155,	299.1528128),
+	Ellipsoid( 5, "Clarke1866",		6378206.4,	294.9786982),
+	Ellipsoid( 6, "Clarke1880",		6378249.145,	293.465),
+	Ellipsoid( 7, "EverestIndia1830",	6377276.345,	300.8017),
+	Ellipsoid( 8, "EverestSabahSarawak",	6377298.556,	300.8017),
+	Ellipsoid( 9, "EverestIndia1956",	6377301.243,	300.8017),
+	Ellipsoid(10, "EverestMalaysia1969",	6377295.664,	300.8017),	//Dana has no datum that uses this ellipsoid!
+	Ellipsoid(11, "EverestMalay_Sing",	6377304.063,	300.8017),
+	Ellipsoid(12, "EverestPakistan",	6377309.613,	300.8017),
+	Ellipsoid(13, "Fischer1960Modified",	6378155,	298.3),
+	Ellipsoid(14, "Helmert1906",		6378200,	298.3),
+	Ellipsoid(15, "Hough1960",		6378270,	297),
+	Ellipsoid(16, "Indonesian1974",		6378160,	298.247),
+	Ellipsoid(17, "International1924",	6378388,	297),
+	Ellipsoid(18, "Krassovsky1940",		6378245,	298.3),
+	Ellipsoid(19, "GRS80",			6378137,	298.257222101),
+	Ellipsoid(20, "SouthAmerican1969",	6378160,	298.25),
+	Ellipsoid(21, "WGS72",			6378135,	298.26),
+	Ellipsoid(22, "WGS84",			6378137,	298.257223563)
+};
+#define	eClarke1866	5		//names for ellipsoidId's
+#define	eGRS80		19
+#define	eWGS72		21
+#define	eWGS84		22
+
+class Datum{
+public:
+	Datum(){};
+	Datum(int id, char* name, int eid, double dx, double dy, double dz){
+		Name=name;  eId=eid;  dX=dx;  dY=dy;  dZ=dz;
+	}
+	char* Name;
+	int   eId;
+	double dX;
+	double dY;
+	double dZ;
+};
+static Datum datum[] = {		//converted from PeterDana website, by Eugene Reimer 2002dec
+//	      Id,  Name,			eId,		dX,	dY,	dZ;	//when & where this datum is applicable
+	Datum( 0, "NAD27_AK",			eClarke1866,	-5,	135,	172),	//NAD27 for Alaska Excluding Aleutians
+	Datum( 1, "NAD27_AK_AleutiansE",	eClarke1866,	-2,	152,	149),	//NAD27 for Aleutians East of 180W
+	Datum( 2, "NAD27_AK_AleutiansW",	eClarke1866,	2,	204,	105),	//NAD27 for Aleutians West of 180W
+	Datum( 3, "NAD27_Bahamas",		eClarke1866,	-4,	154,	178),	//NAD27 for Bahamas Except SanSalvadorIsland
+	Datum( 4, "NAD27_Bahamas_SanSalv",	eClarke1866,	1,	140,	165),	//NAD27 for Bahamas SanSalvadorIsland
+	Datum( 5, "NAD27_AB_BC",		eClarke1866,	-7,	162,	188),	//NAD27 for Canada Alberta BritishColumbia
+	Datum( 6, "NAD27_MB_ON",		eClarke1866,	-9,	157,	184),	//NAD27 for Canada Manitoba Ontario
+	Datum( 7, "NAD27_NB_NL_NS_QC",		eClarke1866,	-22,	160,	190),	//NAD27 for Canada NewBrunswick Newfoundland NovaScotia Quebec
+	Datum( 8, "NAD27_NT_SK",		eClarke1866,	4,	159,	188),	//NAD27 for Canada NorthwestTerritories Saskatchewan
+	Datum( 9, "NAD27_YT",			eClarke1866,	-7,	139,	181),	//NAD27 for Canada Yukon
+	Datum(10, "NAD27_CanalZone",		eClarke1866,	0,	125,	201),	//NAD27 for CanalZone (ER: is that Panama??)
+	Datum(11, "NAD27_Cuba",			eClarke1866,	-9,	152,	178),	//NAD27 for Cuba
+	Datum(12, "NAD27_Greenland",		eClarke1866,	11,	114,	195),	//NAD27 for Greenland (HayesPeninsula)
+	Datum(13, "NAD27_Carribean",		eClarke1866,	-3,	142,	183),	//NAD27 for Antigua Barbados Barbuda Caicos Cuba DominicanRep GrandCayman Jamaica Turks
+	Datum(14, "NAD27_CtrlAmerica",		eClarke1866,	0,	125,	194),	//NAD27 for Belize CostaRica ElSalvador Guatemala Honduras Nicaragua
+	Datum(15, "NAD27_Canada",		eClarke1866,	-10,	158,	187),	//NAD27 for Canada
+	Datum(16, "NAD27_ConUS",		eClarke1866,	-8,	160,	176),	//NAD27 for CONUS
+	Datum(17, "NAD27_ConUS_East",		eClarke1866,	-9,	161,	179),	//NAD27 for CONUS East of Mississippi Including Louisiana Missouri Minnesota
+	Datum(18, "NAD27_ConUS_West",		eClarke1866,	-8,	159,	175),	//NAD27 for CONUS West of Mississippi Excluding Louisiana Missouri Minnesota
+	Datum(19, "NAD27_Mexico",		eClarke1866,	-12,	130,	190),	//NAD27 for Mexico
+	Datum(20, "NAD83_AK",			eGRS80,		0,	0,	0),	//NAD83 for Alaska Excluding Aleutians
+	Datum(21, "NAD83_AK_Aleutians",		eGRS80,		-2,	0,	4),	//NAD83 for Aleutians
+	Datum(22, "NAD83_Canada",		eGRS80,		0,	0,	0),	//NAD83 for Canada
+	Datum(23, "NAD83_ConUS",		eGRS80,		0,	0,	0),	//NAD83 for CONUS
+	Datum(24, "NAD83_Hawaii",		eGRS80,		1,	1,	-1),	//NAD83 for Hawaii
+	Datum(25, "NAD83_Mexico_CtrlAmerica",	eGRS80,		0,	0,	0),	//NAD83 for Mexico CentralAmerica
+	Datum(26, "WGS72",			eWGS72,		0,	0,	0),	//WGS72 for world
+	Datum(27, "WGS84",			eWGS84,		0,	0,	0)	//WGS84 for world
+};
+#define	dNAD27_MB_ON	6		//names for datumId's
+#define	dNAD27_Canada	15
+#define	dNAD83_Canada	22
+#define	dNAD83_ConUS	23
+#define	dWGS84		27
+
+
+void DatumConvert(int dIn, double LatIn, double LongIn, double HtIn, int dTo,  double& LatTo, double& LongTo, double& HtTo){
+   // converts LatLongHt in datum dIn, to LatLongHt in datum dTo;  2002dec: by Eugene Reimer, from PeterDana equations.
+   // Lat and Long params are in degrees;  North latitudes and East longitudes are positive;  Height is in meters;
+   // ==This approach to Datum-conversion is a waste of time;  to get acceptable accuracy a large table is needed -- see NADCON, NTv2...
+   double a,ee,N,X,Y,Z,EE,p,b,t;
+   
+   //--transform to XYZ, using the "In" ellipsoid
+   //LongIn += 180;
+   a = ellip[datum[dIn].eId].EquatorialRadius;
+   ee= ellip[datum[dIn].eId].eccSquared;
+   N = a / sqrt(1 - ee*sin(LatIn*deg2rad)*sin(LatIn*deg2rad));
+   X = (N + HtIn) * cos(LatIn*deg2rad) * cos(LongIn*deg2rad);
+   Y = (N + HtIn) * cos(LatIn*deg2rad) * sin(LongIn*deg2rad);
+   Z = (N*(1-ee) + HtIn) * sin(LatIn*deg2rad);
+
+   //--apply delta-terms dX dY dZ
+   //cout<<"\tX:" <<X <<" Y:" <<Y <<" Z:" <<Z;		//==DEBUG
+   X+= datum[dIn].dX - datum[dTo].dX;
+   Y+= datum[dIn].dY - datum[dTo].dY;
+   Z+= datum[dIn].dZ - datum[dTo].dZ;
+   //cout<<"\tX:" <<X <<" Y:" <<Y <<" Z:" <<Z;		//==DEBUG
+   
+   //--transform back to LatLongHeight, using the "To" ellipsoid
+   a = ellip[datum[dTo].eId].EquatorialRadius;
+   ee= ellip[datum[dTo].eId].eccSquared;
+   EE= ee/(1-ee);
+   p = sqrt(X*X + Y*Y);
+   b = a*sqrt(1-ee);
+   t = atan(Z*a/(p*b));
+   LatTo = atan((Z+EE*b*sin(t)*sin(t)*sin(t)) / (p-ee*a*cos(t)*cos(t)*cos(t)));
+   LongTo= atan(Y/X);
+   HtTo  = p/cos(LatTo) - a/sqrt(1-ee*sin(LatTo)*sin(LatTo));
+   LatTo  *= rad2deg;
+   LongTo *= rad2deg;  LongTo -= 180;
+}
+
+
+void LLtoUTM(int eId, double Lat, double Long,  double& Northing, double& Easting, int& Zone){
+   // converts LatLong to UTM coords;  3/22/95: by ChuckGantz chuck.gantz@globalstar.com, from USGS Bulletin 1532.
+   // Lat and Long are in degrees;  North latitudes and East Longitudes are positive.
+   double a = ellip[eId].EquatorialRadius;
+   double ee= ellip[eId].eccSquared;
+   Long -= int((Long+180)/360)*360;			//ensure longitude within -180.00..179.9
+   double N, T, C, A, M;
+   double LatRad = Lat*deg2rad;
+   double LongRad = Long*deg2rad;
+
+   Zone = int((Long + 186)/6);
+   if( Lat >= 56.0 && Lat < 64.0 && Long >= 3.0 && Long < 12.0 )  Zone = 32;
+   if( Lat >= 72.0 && Lat < 84.0 ){			//Special zones for Svalbard
+      if(      Long >= 0.0  && Long <  9.0 )  Zone = 31;
+      else if( Long >= 9.0  && Long < 21.0 )  Zone = 33;
+      else if( Long >= 21.0 && Long < 33.0 )  Zone = 35;
+      else if( Long >= 33.0 && Long < 42.0 )  Zone = 37;
+   }
+   double LongOrigin = Zone*6 - 183;			//origin in middle of zone
+   double LongOriginRad = LongOrigin * deg2rad;
+
+   double EE = ee/(1-ee);
+
+   N = a/sqrt(1-ee*sin(LatRad)*sin(LatRad));
+   T = tan(LatRad)*tan(LatRad);
+   C = EE*cos(LatRad)*cos(LatRad);
+   A = cos(LatRad)*(LongRad-LongOriginRad);
+
+   M= a*((1 - ee/4    - 3*ee*ee/64 - 5*ee*ee*ee/256  ) *LatRad 
+	    - (3*ee/8 + 3*ee*ee/32 + 45*ee*ee*ee/1024) *sin(2*LatRad)
+	    + (15*ee*ee/256 + 45*ee*ee*ee/1024	  ) *sin(4*LatRad)
+	    - (35*ee*ee*ee/3072			  ) *sin(6*LatRad));
+   
+   Easting = k0*N*(A+(1-T+C)*A*A*A/6+(5-18*T+T*T+72*C-58*EE)*A*A*A*A*A/120) + 500000.0;
+
+   Northing = k0*(M+N*tan(LatRad)*(A*A/2+(5-T+9*C+4*C*C)*A*A*A*A/24
+			    + (61-58*T+T*T+600*C-330*EE)*A*A*A*A*A*A/720));
+}
+
+
+void UTMtoLL(int eId, double Northing, double Easting, int Zone,  double& Lat, double& Long){
+   // converts UTM coords to LatLong;  3/22/95: by ChuckGantz chuck.gantz@globalstar.com, from USGS Bulletin 1532.
+   // Lat and Long are in degrees;  North latitudes and East Longitudes are positive.
+   double a = ellip[eId].EquatorialRadius;
+   double ee = ellip[eId].eccSquared;
+   double EE = ee/(1-ee);
+   double e1 = (1-sqrt(1-ee))/(1+sqrt(1-ee));
+   double N1, T1, C1, R1, D, M, mu, phi1Rad;
+   double x = Easting - 500000.0;			//remove 500,000 meter offset for longitude
+   double y = Northing;
+   double LongOrigin = Zone*6 - 183;			//origin in middle of zone
+
+   M = y / k0;
+   mu = M/(a*(1-ee/4-3*ee*ee/64-5*ee*ee*ee/256));
+
+   phi1Rad = mu + (3*e1/2-27*e1*e1*e1/32) *sin(2*mu) 
+		+ (21*e1*e1/16-55*e1*e1*e1*e1/32) *sin(4*mu)
+		+ (151*e1*e1*e1/96) *sin(6*mu);
+   N1 = a/sqrt(1-ee*sin(phi1Rad)*sin(phi1Rad));
+   T1 = tan(phi1Rad)*tan(phi1Rad);
+   C1 = EE*cos(phi1Rad)*cos(phi1Rad);
+   R1 = a*(1-ee)/pow(1-ee*sin(phi1Rad)*sin(phi1Rad), 1.5);
+   D = x/(N1*k0);
+
+   Lat = phi1Rad - (N1*tan(phi1Rad)/R1)*(D*D/2-(5+3*T1+10*C1-4*C1*C1-9*EE)*D*D*D*D/24
+		   +(61+90*T1+298*C1+45*T1*T1-252*EE-3*C1*C1)*D*D*D*D*D*D/720);
+   Lat *= rad2deg;
+   Long = (D-(1+2*T1+C1)*D*D*D/6+(5-2*C1+28*T1-3*C1*C1+8*EE+24*T1*T1)*D*D*D*D*D/120) / cos(phi1Rad);
+   Long = LongOrigin + Long*rad2deg;
+}
+
+
+//==procedures to use and test the LatLong-to-UTM and UTM-to-LatLong conversion routines
+
+int Z,Fmt,K; double E,N,Lat,Long;		//global vars for Prt routine
+#define fUT4	1				//for Fmt: UTM-4wd-style
+#define fLLD	2				//for Fmt: LL-degrees-N|S-E|W
+#define fLLDM	4				//for Fmt: LL-degrees:minutes-N|S-E|W
+#define fLLDMS	8				//for Fmt: LL-degrees:minutes:seconds-N|S-E|W
+#define fUTs	16				//for Fmt: UTM-signed
+#define fLLs	32				//for Fmt: LL-degrees-signed
+#define fUT3	64				//for Fmt: UTM-3wd-style
+#define nbr	atof														//atof: ascii-to-number
+#define Nbr	strtod
+double  nbN(char*a){return strchr("Ss",*a) ?atof(a+1)-10000000 :strchr("Nn",*a) ?atof(a+1) :atof(a);}				//Northing
+double  nbL(char*a){														//lat|long: [N|E|S|W]d[:m[:s]][N|E|S|W]
+   char*p; double R=Nbr(a+!!isalpha(*a),&p); if(*p==':')R+=Nbr(p+1,&p)/60; if(*p==':')R+=Nbr(p+1,&p)/3600;
+   return strchr("SsWw",*a)||(*p&&strchr("SsWw",*p)) ?-R :R;
+}
+void prtDM (double D){double M=(D-int(D))*60; cout<<setprecision(3)<<int(D)<<":"<<M;}
+void prtDMS(double D){double M=(D-int(D))*60; cout<<setprecision(1)<<int(D)<<":"<<int(M)<<":"<<(M-int(M))*60;}
+void ps() {if(K++)cout<<" == ";}
+void Prt(int Fmt){														//Prt: print according to Fmt bits
+   K=0;
+   if(Fmt&fLLs)  {ps(); cout<<setprecision(9) <<Lat <<" " <<Long;								}	//LL degrees signed  
+   if(Fmt&fLLD)  {ps(); char NS=(Lat<0?'S':'N'), EW=(Long<0?'W':'E'); cout<<setprecision(5)<<abs(Lat)<<NS<<" "<<abs(Long)<<EW;	}	//LL degrees N|S E|W 
+   if(Fmt&fLLDM) {ps(); char NS=(Lat<0?'S':'N'), EW=(Long<0?'W':'E'); prtDM (abs(Lat));cout<<NS<<" ";prtDM (abs(Long));cout<<EW;}	//LL  d:m    N|S E|W 
+   if(Fmt&fLLDMS){ps(); char NS=(Lat<0?'S':'N'), EW=(Long<0?'W':'E'); prtDMS(abs(Lat));cout<<NS<<" ";prtDMS(abs(Long));cout<<EW;}	//LL  d:m:s  N|S E|W 
+   if(Fmt&fUTs)  {ps(); cout<<setprecision(3)<<Z<<" "<<E<<" "<<N;								}	//UTM signed          
+   if(Fmt&fUT3)  {ps(); cout<<setprecision(0)<<Z<<" "<<E<<" ";                  if(N<0)cout<<'S'<<N+10000000;else cout<<N;	}	//UTM std 3-word S-style
+   if(Fmt&fUT4)  {ps(); cout<<setprecision(0)<<Z<<" "<<(N<0?"S ":"N ")<<E<<" "; if(N<0)cout     <<N+10000000;else cout<<N;	}	//UTM std 4-word N|S-style
+}
+void CvtLine(int C, char**V, char*p){
+   if     (C==2){Lat=nbL(V[0]); Long=nbL(V[1]);                                               LLtoUTM(eWGS84,Lat,Long,N,E,Z);}	//2 words, convert from LatLong
+   else if(C==3){Z=atoi(V[0]); E=nbr(V[1]); N=nbN(V[2]);                                      UTMtoLL(eWGS84,N,E,Z,Lat,Long);}	//3 words, convert from UTM
+   else         {Z=atoi(V[0]); E=nbr(V[2]); N=nbN(V[3]); N=strchr("Ss",V[1][0])?N-10000000:N; UTMtoLL(eWGS84,N,E,Z,Lat,Long);}	//4 words, convert from UTM 2nd wd is N|S
+   Prt(Fmt); if(*p)cout<<"\t"<<p; cout<<"\n";											//print as per Fmt, add comment if any
+   //==Prt(Fmt) replaced by  Prt(fXXX);cout<<" == ";Prt(Fmt&~fXXX)  to show in approximately input-format first, then in the other formats?
+}
+
+void test(int eId, double pLat, double pLong){
+   Lat=pLat; Long=pLong;
+   cout<<"\nEllipsoid:" <<ellip[eId].Name;	cout<<"\nStarting Lat,Long position:              "; Prt(fLLs);
+   LLtoUTM(eId, Lat, Long, N, E, Z);		cout<<"\nConverted to UTM-Zone,Easting,Northing:  "; Prt(fUTs);
+   UTMtoLL(eId, N, E, Z, Lat, Long);		cout<<"\nConverted back to Lat,Long:              "; Prt(fLLs);
+   cout<<"\n";
+}
+void testD(int dIn, double LatIn, double LongIn, double HtIn, int dTo){
+   cout<<"\nDatum:" <<datum[dIn].Name <<" Ellipsoid:" <<ellip[datum[dIn].eId].Name  <<" To-Datum:" <<datum[dTo].Name <<" To-Ellipsoid:" <<ellip[datum[dTo].eId].Name;
+   cout<<setprecision(9);							//note: Ht (in metres) needs 5-digits less precision than Lat,Long do
+   double LatTo, LongTo, HtTo;					cout<<"\nIN: datum:" <<datum[dIn].Name <<"\tLat:" <<LatIn <<" Long:" <<LongIn <<" Ht:" <<HtIn;
+   DatumConvert(dIn,LatIn,LongIn,HtIn, dTo,LatTo,LongTo,HtTo);	cout<<"\nTO: datum:" <<datum[dTo].Name <<"\tLat:" <<LatTo <<" Long:" <<LongTo <<" Ht:" <<HtTo;
+   DatumConvert(dTo,LatTo,LongTo,HtTo, dIn,LatIn,LongIn,HtIn);	cout<<"\nTO: datum:" <<datum[dIn].Name <<"\tLat:" <<LatIn <<" Long:" <<LongIn <<" Ht:" <<HtIn;
+   cout<<"\n";
+}
+void Testcases(){
+   test(eWGS84,	  47+22.690/60, 8+13.950/60);					//WGS-84,     47:22.690N   8:13.950E    == ChuckGantz-eg  (Swiss Grid: 659879/247637)
+   test(eWGS84,	  30+16./60+28.82/3600, -(97+44./60+25.19/3600));		//WGS-84,     30:16:28.82N 97:44:25.19W == PeterDana-eg1
+   test(eClarke1866, 30+16./60+28.03/3600, -(97+44./60+24.09/3600));		//Clarke1866, 30:16:28.03N 97:44:24.09W == PeterDana-eg2;  recall NAD27 uses Clarke1866
+   test(eClarke1866, 51.5, -101.5);						//Clarke1866, 51:30N 101:30W == SE-corner of topo-map:62N12
+   testD(dWGS84, 30+16./60+28.82/3600, -(97+44./60+25.19/3600), 0, dWGS84);	//test DatumConvert on Dana-eg1 to itself (to see X,Y,Z)
+   testD(dNAD27_MB_ON,  51.5, -101.5, 0, dNAD83_Canada);			//test DatumConvert on SE-corner of topo-map:62N12;  FROM: dNAD27_Canada | dNAD27_MB_ON
+}
+void Usage(){
+   cout<<"Usage:";
+   cout<<"\n	LatLong-UTM  [--Outputformat]  Latitude[N|S]  Longitude[E|W]";
+   cout<<"\n	LatLong-UTM  [--Outputformat]  Zone  Easting  Northing";
+   cout<<"\n	LatLong-UTM  [--Outputformat]  Zone  N|S  Easting  Northing";
+   cout<<"\n	LatLong-UTM  [--Outputformat]";
+   cout<<"\nreads from standard-input when no coordinates given on cmdline, where each line is one of:";
+   cout<<"\n	Latitude[N|S]  Longitude[E|W]";
+   cout<<"\n	Zone  Easting  Northing";
+   cout<<"\n	Zone  N|S  Easting  Northing";
+   cout<<"\nLatitude and Longitude are in degrees or degrees:minutes or degrees:minutes:seconds with north and east positive, S and W are alternate minus-signs;";
+   cout<<"\nZone is an integer 1..60;  Easting in metres with origin for zone#N at 500,000m west of longitude N*6-183°;  northing is metres from equator with north positive;";
+   cout<<"\nin the 3-word style, Northing is northing or the letter S followed by northing-plus-ten-million;";
+   cout<<"\nin the 4-word style when 2nd word is S, Northing is northing-plus-ten-million;";
+   cout<<"\nin the signed style, Northing is northing (and sanity prevails, unfortunately that style is not widely used);";
+   cout<<"\nOutputformat is an integer, the sum of one or more of: 1 for UTM-4-word, 2 for LL-degrees, 4 for LL-degrees:minutes, 8 for LL-degrees:minutes:seconds,";
+   cout<<"\n	16 for UTM-signed, 32 for LL-signed, 64 for UTM-3-word;  the default for Outputformat is 7 (which is UTM + LL-degrees + LL-degrees:minutes);";
+   cout<<"\nan input-file can have comments, from '#' to end-of-line, and such comments as well as empty-lines will be echoed to the output;";
+   cout<<"\nno datum conversion is provided, the WGS84 datum is used throughout (WGS84 is identical to GRS80 aka NAD83 to 8 significant digits);";
+   cout<<"\nEXAMPLES:";
+   cout<<"\n	LatLong-UTM  50.26     -96		-- convert Latitude:50.26 Longitude:-96 to UTM";
+   cout<<"\n	LatLong-UTM  50:15.6   -96		-- same as preceding";
+   cout<<"\n	LatLong-UTM  50:15:36  -96		-- same as preceding";
+   cout<<"\n	LatLong-UTM  50:15:36N  96W		-- same as preceding";
+   cout<<"\n	LatLong-UTM  N50:15:36  W96		-- same as preceding";
+   cout<<"\n	LatLong-UTM  -25.0  18.13		-- convert Latitude:-25.0 Longitude:18.13 to UTM";
+   cout<<"\n	LatLong-UTM  14 501000 5678901		-- convert Zone:14 Easting:501000 Northing:5678901 to LatLong";
+   cout<<"\n	LatLong-UTM  14 501000 -5666777		-- convert Zone:14 Easting:501000 Northing:-5666777 to LatLong";
+   cout<<"\n	LatLong-UTM  14 501000 S4333223		-- same as preceding;  Northing as S followed by Northing-plus-ten-million";
+   cout<<"\n	LatLong-UTM  14 S 501000 4333223	-- same as preceding, in the 4-word style";
+   cout<<"\n	LatLong-UTM  --48  <trailpoints-utm.txt	-- convert each line of trailpoints-utm.txt, with Outputformat: UTM-signed + LL-signed";
+   cout<<"\n";
+}
+
+
+// 2002-December:  by Eugene Reimer, ereimer@shaw.ca
+// started with 2 routines (LLtoUTM, UTMtoLL) by Chuck Gantz chuck.gantz@globalstar.com;
+//	Gantz cites his source as:  USGS Bulletin#1532;
+// dropped the "LatZone" letters (aka Latitude-Bands);
+// reworked handling "origin 10million" for northings in Southern-hemisphere -- his method needed "LatZone" letters which are even sillier than ten-million convention;
+// better value for PI:
+//	made 6mm difference in calculated UTM coords (in Gantz example);
+// ellipsoid constants:  replaced Gantz's constants, which he said were from PeterDana website, with data from PeterDana website (to my surprise they differ):
+//	http://www.colorado.edu/geography/gcraft/notes/datum/edlist.html
+//	Dana cites his sources as:  NIMA-8350.2-1977july4  and  MADTRAN-1996october1
+//	-- better Clarke1866 ellipsoid made 360mm difference in calculated UTM coords in MB;  NAD-27 example in Manitoba now within 1mm of NR-Canada online-converter!!
+//	-- computed UTM-coords for AustinDomeStar differ from Dana's by roughly 100mm; but agree to the mm with NR-Canada online-converter (nrcan.gc.ca)!!
+// datum constants & datum conversion routine:  using formulae from PeterDana website;  the dX,dY,dZ constants are for the conversion formulae;
+//	have defined only those datums that apply to NorthAmerica (28 out of several hundred);
+//	Dana cites:  Bowring,B. 1976. Transformations from spatial to geographical coordinates. Survey Review XXIII; pg323-327.
+// added support for input from a file;  am tempted to scrap cmdline-args in which case the Usage-Examples would become:  echo "..." | LatLong-UTM
+// considered scrapping Testcases routine and using a bash-script of testcases instead;  ==keeping it for non-WGS84 testcases since my UI has no datum-conversion...
+
+// 2010-08-11:  in 2002 this compiled just fine,  but the current g++ insists main must return int;  and warns about deprecated header-files;
+//	fixed by revising void main() --> int main();  adding return 0;  revising #include's as shown above eg: <math.h> --> <cmath>;  adding: using namespace std;
+
+// 2010-08-12:  debugging DatumConvert:
+//	my 1st testD eg, Dana-eg1, going TO-XYZ;
+//		according to Dana it is:	X:-742507.1    Y:-5462738.5    Z:3196706.5		(Dana calls it ECEF XYZ)
+//		I was getting:			X:-743130.2424 Y:-5467322.9359 Z:3199389.2544
+//		I'm now getting (sin-sqred):	X:-742507.1145 Y:-5462738.4892 Z:3196706.5101		<==AGREES after sin->sin-sqred fix!!
+//		GEOTRANS (to Geocentric) gets:	X:-742507      Y:-5462738      Z:3196707		(Geotrans calls it Geocentric)
+//		ergo: GEOTRANS agrees with Dana, my code based on Dana-equations does too after fixing;
+//	my 2nd testD eg, converting Lat:51.5 Long:101.5 from NAD27 to NAD83:
+//		NTv2 (online):		Lat:51:29:59.96845  Long:101:30:1.65354W			<==using http://www.geod.nrcan.gc.ca/apps/ntv2/ntv2_geo_e.php
+//					Lat:51.49999124     Long:101.5004593W				<--converted to degrees on my calculator (10-digit precision)
+//		NTv2 (exe under Wine):  Lat:51.499991236    Long:101.500459317W				<==AUTHORITATIVE ANSWER
+//		NADCON gets:		Lat:==NO LUCK, NORTH-AMERICA==USA==!!==				<==http://www.ngs.noaa.gov/cgi-bin/nadcon.prl
+//		GEOTRANS gets:		Lat:51.50004        Long:-101.50059      Ht:-25			<--inaccurate;  its UI truncates Datum-name => unsure which??
+//		I'm getting:		Lat:51.500036202    Long:-101.500594748  Ht:-24.919516822	<--From:NAD27_Canada;  same as GEOTRANS but not good enough
+//		I'm getting:		Lat:51.500013926    Long:-101.500577765  Ht:-26.781441874    	<--From:NAD27_MB_ON;   slightly closer but still not good enough
+//		Paper http://www.h-e.com/pdfs/de_sbe94.pdf:  shows MB has approx no change in Lat,  from -1.0 in E-MB to -1.6seconds in W-MB change in Long;
+//			same info in degrees:  Longitude from -0.00027 in Eastern-MB to -0.00044degrees in Western-MB;  <--my eg in W-MB changes by -0.00046degrees;
+//			Error on that map-corner-eg to 5-place-fractional-degrees:  Lat-error: 0.00001  Long-error: 00012;
+//	==Conclusions after some reading:  (1) NADCON & NTv2(NRCAN) are the reasonably accurate methods;  (2) my (and GEOTRANS's) formula approach is a waste of time;
+//	PROJ (http://trac.osgeo.org/proj/) -- it needs grid shift file, eg: NTV2_0.GSB from http://www.geod.nrcan.gc.ca/online_data_e.php
+//	NTv2, DOS-program that comes with the "grid shift file" NTV2_0.GSB from NRCAN, runs under Wine;  http://www.geod.nrcan.gc.ca/online_data_e.php
+//	note: NTv2 is Canadian;  Australia, NewZealand, Germany, Switzerland, Spain also use its file-format;  and unofficially GreatBritain, France, Portugal;
+//	NADCON is yank;  they call it the NORTH-AMERICAN converter but it only supports locations within-USA:-)
+//	TRANSDAT: Windows program installed via Wine, into c:\Program Files\transdat;  	needs MFC42.DLL, installed into /home/ereimer/.wine/drive_c/windows/system32
+// NTv2:  have found OpenSource programs with NTv2 support;  also found freely downloadable NTv2 Data-files;  and the NTv2 Developer's Guide;
+// Notes on NTv2 doc & tools are now in my Datum-Conversion rant:  http://ereimer.net/rants/datum-conversion.htm
+//	[For me: copies of NTv2 doc & tools are in:  /pix/er-GIS-DatumConversion-NAD83-WGS84-NTv2-etc-data;  see 00-NOTES... in that dir]
+// those tools offer UTM<->LatLong as well as Datum-shift conversions;  may abandon this program as hardly needed, except for more flexible outputformat==??==
+// one thing that is needed:  a NAD27->NAD83 conversion-table (in NTv2 format) that handles all of North America==??==
+
+// 2010-08-30:
+// put up a web-interface:  http://ereimer.net/cgi-bin/coordinatecvt;
+// nbL:  accept N|S E|W at either end of a  Lat/Long coordinate;  (letter-up-front style is not offered for output);
+// CvtLine:  accept UTM-4-word input eg: 14 N 501000 5678901 (same as 14 501000 N5678901);  Prt: fUT4 is Fmt bit for UTM-4-word output, replaces fUT3 as on-by-default;
+// Usage: added Examples and reworded Instructions for the 4-word UTM style now supported on input & output;
+// Consider:
+//	doing space->colon fixups to get Lat Long pair might be possible, since valid UTM-Easting is never less than 60...
+//	adding some checking for better errmsgs;  eg: each "word" must start with digit|minus-sign|N|S|E|W|n|s|e|w;

auto_drive_proj_utils/Utils/common_utils.hpp

@@ -0,0 +1,258 @@
+#ifndef _COMMON_UTILS_HPP_
+#define _COMMON_UTILS_HPP_
+
+#include <iostream>
+#include <string>
+#include <map>
+
+#define DEG2RAD(deg) (M_PI / 180.0 * (deg))
+#define EARTH_RADIUS (6378137.0)    //radius of the earth
+#define WGS84_B (6356752.3142)
+#define ECC 0.081819190842965
+
+// 定义每个位的报警含义
+typedef struct
+{
+    std::vector<std::string> status_warn_vec = {
+        "左电机报警",     // bit 0
+        "右电机报警",     // bit 1
+        "液压电机报警",   // bit 2
+        "作业电机报警",   // bit 3
+        "BMS报警",        // bit 4
+        "未知报警 5",     // bit 5
+        "未知报警 6",     // bit 6
+        "未知报警 7",     // bit 7
+        "急停按下",       // bit 8
+        "下位机升级失败", // bit 9
+        "上位机心跳超时", // bit 10
+        "举升上升异常",   // bit 11
+        "举升下降异常",   // bit 12
+        "未知报警 13",    // bit 13
+        "未知报警 14",    // bit 14
+        "未知报警 15",    // bit 15
+        "未知报警 16",    // bit 16
+        "未知报警 17",    // bit 17
+        "未知报警 18",    // bit 18
+        "未知报警 19",    // bit 19
+        "未知报警 20",    // bit 20
+        "未知报警 21",    // bit 21
+        "未知报警 22",    // bit 22
+        "未知报警 23",    // bit 23
+        "未知报警 24",    // bit 24
+        "未知报警 25",    // bit 25
+        "未知报警 26",    // bit 26
+        "未知报警 27",    // bit 27
+        "未知报警 28",    // bit 28
+        "未知报警 29",    // bit 29
+        "未知报警 30",    // bit 30
+        "未知报警 31"     // bit 31
+    };
+}LowerPcStatus;
+
+enum class LIFT_FUNC
+{
+    LIFT_HOLD = 0,
+    LIFT_UP,
+    LIFT_DOWN
+};
+
+enum class REAP_FUNC
+{
+    REAP_OFF = 0,
+    REAP_ON
+};
+
+enum class AUTO_STATUS
+{
+    AUTO_STOP = 0,
+    AUTO_PAUSE,
+    AUTO_DRIVING
+};
+
+enum class MODBUSFUNCODE
+{
+    WRITE_SINGLE_COIL_E = 5,
+    WRITE_MULTIPLE_COILS_E = 15,
+    WRITE_MULTIPLE_REGISTERS_E = 16,
+    WRITE_SINGLE_REGISTER_E = 6,
+    WRITE_INT32_E = 7,
+    READ_COILS_E = 1,
+    READ_DISCRETE_INPUTS_E = 2,
+    READ_HOLDING_REGISTERS_E = 3
+};
+
+enum class RTK_MODE_TYPE
+{
+    INVALID = 0,
+    SINGLE_POINT,
+    DIFF,
+    GPS_PPS,
+    RTK_INT,
+    RTK_FLOAT,
+    INS_MODE,
+    MANUAL_MODE,
+    SIM_MODE
+};
+
+enum class RTK_QUAL_TYPE
+{
+    INVALID = 0,
+    SIGAL_LOCATION,
+    RTK_FLOAT,
+    RTK_FIX
+};
+
+enum class LASER_OBSTACLE_AVOID_TYPE
+{
+    SLOW = 0,
+    STOP,
+    NONE
+};
+
+typedef struct Pose_T
+{
+    double x;
+    double y;
+    double z;
+    double yaw;
+
+    Pose_T(double x_val, double y_val, double z_val, double yaw_val)
+        : x(x_val), y(y_val), z(z_val), yaw(yaw_val)
+    {
+
+    }
+    Pose_T()
+    {
+        
+    }
+    bool operator == (Pose_T& pose)
+    {
+        return ((pose.x - x < 1e-6) && (pose.y - y < 1e-6)); 
+    }
+} ;
+
+typedef struct Point_T
+{
+    double x;
+    double y;
+
+    bool operator == (Point_T& point)
+    {
+        return ((point.x - x < 1e-6) && (point.y - y < 1e-6)); 
+    }
+} ;
+
+typedef struct 
+{
+    /* data */
+    float speed;
+    float angle;
+}MoveBaseData;
+
+typedef struct
+{
+    double lon;
+    double lat;
+    double enu_x;
+    double enu_y;
+    double enu_z;
+    float roll;
+    float pitch;
+    float yaw;
+    double height;
+    int rtk_mode;
+    int rtk_qual;
+    int pos_qual;
+    int heading_qual;
+    double gps_vel;
+    double gps_vel_x;
+    double gps_vel_y;
+    int visual_satellites_num;
+    int pdop_val;
+    double slave_lon;
+    double slave_lat;
+    double gps_vel_ts;
+    double pos_ts;
+
+}RtkData;
+
+static void WGS84ToENU_Without_Height(const double lati, const double longt, \
+    const double start_lati, const double start_longt, double &x_east, double &y_north){
+    x_east = (DEG2RAD(longt) - DEG2RAD(start_longt)) * EARTH_RADIUS * cos(DEG2RAD(start_lati));
+    y_north = (DEG2RAD(lati) - DEG2RAD(start_lati)) * EARTH_RADIUS;
+	
+}
+
+static void pos2ecef(const double lon0, const double lat0, const double alt0, double &ecef_x, double &ecef_y, double &ecef_z)
+{
+    double lon = DEG2RAD(lon0);
+    double lat = DEG2RAD(lat0);
+    double alt = alt0;
+
+    double cosLon = cos(lon);
+    double cosLat = cos(lat);
+    double sinLon = sin(lon);
+    double sinLat = sin(lat);
+    double wgs84_f = (EARTH_RADIUS - WGS84_B) / EARTH_RADIUS;
+    double pow_e_2 = wgs84_f * (2.0 - wgs84_f);
+
+    double N = EARTH_RADIUS / sqrt(1.0 - pow_e_2 * sinLat * sinLat);
+
+    ecef_x = (N + alt) * cosLat * cosLon;
+    ecef_y = (N + alt) * cosLat * sinLon;
+    ecef_z = (N * (1.0 - pow_e_2) + alt) * sinLat;
+}
+
+static void Wgs84ToEnu(const double lon0, const double lat0, const double alt0,
+                    const double lon1, const double lat1, const double alt1,
+                    double &x_e, double &y_n)
+{
+    double ecef_x1, ecef_y1, ecef_z1;
+    double ecef_x0, ecef_y0, ecef_z0;
+    pos2ecef(lon1, lat1, alt1, ecef_x1, ecef_y1, ecef_z1);
+    pos2ecef(lon0, lat0, alt0, ecef_x0, ecef_y0, ecef_z0);
+
+    double offset_x = ecef_x0 - ecef_x1;
+    double offset_y = ecef_y0 - ecef_y1;
+    double offset_z = ecef_z0 - ecef_z1;
+    double lon_tmp = DEG2RAD(lon1);
+    double lat_tmp = DEG2RAD(lat1);
+
+    double cosLon = cos(lon_tmp);
+    double cosLat = cos(lat_tmp);
+    double sinLon = sin(lon_tmp);
+    double sinLat = sin(lat_tmp);
+
+    x_e = -1 * sinLon * offset_x + cosLon * offset_y;
+    y_n = -1 * sinLat * cosLon * offset_x - 1 * sinLat * sinLon * offset_y + cosLat * offset_z;
+}
+
+static float CorrectAngle(float angle)
+{
+    if(std::isnan(angle))
+    {
+        throw "CorrectAngle error, input angle is nan!";
+        return 0;
+    }
+    while(angle > M_PI)
+        angle -= 2 * M_PI;
+    while(angle < -M_PI)
+        angle += 2 * M_PI;
+
+    return angle;
+}
+
+static double CalculateDisByLatLon(const double lon_a, const double lat_a, const double lon_b, const double lat_b)
+{
+    double radLat1 = DEG2RAD(lat_a);
+    double radLat2 = DEG2RAD(lat_b);
+    double a = radLat1 - radLat2;
+    double b = DEG2RAD(lon_a) - DEG2RAD(lon_b);
+    double s = 2 * asin(std::sqrt(std::pow(sin(a / 2), 2) +
+                                       cos(radLat1) * cos(radLat2) * std::pow(sin(b / 2), 2)));
+    s = s * EARTH_RADIUS;
+    s = round(s * 10000000) / 10000000.0;
+    return s;
+}
+
+#endif

auto_drive_proj_utils/config/can.yaml

@@ -0,0 +1,42 @@
+devices:
+  microComputer:
+    read_address: 0x10040405
+    upgrade_address: 0x10EF0401
+    ymodem_address: 0x10F10001
+    registers:
+      - name: 软件版本号
+        address: 0x01
+        type: UINT16
+        value: 0x104
+      - name: 参数保存
+        address: 0xB0
+        type: UINT16
+        value: 0x1
+      - name: 左电机配置
+        address: 0xB1
+        type: UINT32
+        value: 0x5DC0002
+      - name: 右电机配置
+        address: 0xB3
+        type: UINT32
+        value: 0x5DC0002
+      - name: 液压电机配置
+        address: 0xB5
+        type: UINT32
+        value: 0x5DC0024
+      - name: 工作电机配置
+        address: 0xB7
+        type: UINT32
+        value: 0x5DC0005
+      - name: 工作电机配置1
+        address: 0xB9
+        type: UINT32
+        value: 0x0
+      - name: OUT口配置
+        address: 0xBB
+        type: UINT16
+        value: 0x0
+      - name: IN口配置
+        address: 0xBC
+        type: UINT16
+        value: 0x0

auto_drive_proj_utils/config/params.yaml

@@ -0,0 +1,230 @@
+auto_drive:
+    # parameters
+    ros__parameters:
+      delta_goal: 0.6
+      max_speed: 1.0 
+      speed_scale: 1.0
+      center2header: 1.03               #前方rtk蘑菇头至履带着地中心点的水平距离
+      rtkAntennaHeight: 0.96            #前方rtk天线至地面的高度
+      is_rtk_antenna_horizontal: false  # true为rtk左右安装，false为前后安装
+      rtk_antenna_horizontal_dis: 0.95  #左右rtk天线的距离（rtk天线左右安装时需要配置此参数，前后安装时无需配置此参数）
+      use_imu: true                     #是否使用imu
+      use_imu_prediction: false         #当rtk精度不够时是否使用imu预测
+      cross_track_x_err: 0.0
+      cross_track_y_err: 0.0
+      
+      kp: 1.76
+      ki: 0.00
+      kd: 0.8
+      is_peak_tea_leaves_machine: false
+      max_linear_vel: 2.0
+      max_angular_vel: 0.7   
+      max_slow_angular_vel: 0.7
+      min_turn_linear_vel: 0.2   
+      min_tolerance_angle: 0.0348
+      kv: 1.0
+      look_ahead_dis: 0.4
+      ahead_goal_dis: 0.4
+      use_pp_control: true
+      line_path_following_type: 1 # 1 for lqr, 2 for pid, 3 for pp
+      curve_path_following_type: 3 # 1 for lqr, 2 for pid, 3 for pp
+
+      rtk_intensity_detected: true
+      laser_scan_detected: false
+
+controller_driver:
+  # parameters
+  ros__parameters:
+    slave_ip: "192.168.1.111"
+    slave_port: 500
+    ser_port: "/dev/ttyS0"           #串口号
+    ser_baud_rate: 921600
+    use_modbus_rtu: true             #使用modbus设置true，使用网口设置false 
+    reduction_ratio: 20              #减速比
+    wheel_dis: 0.95                  #左右驱动轮之间的距离
+    wheel_diameter: 0.253361812718   #驱动轮的半径加上履带厚度 * 2
+    leftDir: 1                       #左电机方向
+    rightDir: 1                      #右电机方向
+    angleDir: 1                    #角速度方向
+    maxMotorSpeed: 20000             #电机最高转速20000对应2000转每分
+    heart_beat_addr: 0x2f            #与下位机心跳寄存器地址
+    mqtt_time_out: 1.6               #mqtt超时时间 1.6
+
+    use_odom: false
+    rpm_ratio: 10
+    odom_vel_scale: 1.00791935247
+    battery_capacity: 200
+    left_speed_addr: 0x22  #0x12
+    right_speed_addr: 0x24 #0x14
+     
+    lift_addr: 0x28 #0x20
+    work_addr: 0x29 #0x21
+    bms_addr: 0x54
+    bms_power_addr: 0x55
+    bms_consumption_addr: 0x57
+    bms_compuse_addr: 0x51
+    motor_properties_addr: 0x59
+    hdy_speed_addr: 0x5d
+    work_speed_addr: 0x5f
+    left_drive_moto_enable_addr: 0x21 #0x10
+    right_drive_moto_enable_addr: 0x11
+
+    h_s_version_addr: 0x00
+    lower_status_addr: 0x02
+    left_motor_status_addr: 0x06
+    right_motor_status_addr: 0x07
+    hdy_motor_status_addr : 0x08
+    work_motor_status_addr: 0x09
+    bms_status_addr: 0x0a
+
+    set_slave_status_addr: 0x0e
+    clear_warn_status_addr: 0x0f
+
+    left_encode_addr: 0x50
+    right_encode_addr: 0x52
+    
+    heartbeat_time: 1.0
+    battery_capacity_write_addr: 0x3d
+    energy_consumption_read_addr: 0x51
+    max_linear_vel: 2.0
+    max_angular_vel: 1.571
+    use_lat_lon_calculate_dis: true
+    json_file_path: "/install/controller_driver/share/controller_driver/config/all_odom.json"
+
+    #nannongsuo
+    is_peak_tea_leaves_machine: false
+    move_straight_addr: 0x27
+    move_around_addr: 0x28
+    direction_fixed_addr: 0x5e
+    manual_connnect_addr: 0x10
+    lock_an_unlock_addr: 0x11
+    emergency_stop_addr: 0x12
+    reap_knife_addr: 0x13
+    fans_addr: 0x14
+    left_assistive_device_addr: 0x15
+    right_assistive_device_addr: 0x16
+    cancle_cruise_addr: 0x17
+    cruise_addr: 0x18
+    internal_derailment_addr: 0x19
+    outer_derailment_addr: 0x1a
+    stop_engine_addr: 0x1d
+    start_engine_addr: 0x1e
+    tailgate_down_addr: 0x1f
+    tailgate_up_addr: 0x20
+    reap_knife_down_addr: 0x21
+    reap_knife_up_addr: 0x22
+    gas_decrease_addr: 0x25
+    gas_increase_addr: 0x26
+
+app_control_driver:
+    # parameters
+    ros__parameters:
+      rtk_mqtt_topic: "rtkbase/sweet/all"   #rtkbase/JZRTK240416003/all
+      mqtt_server_ip: "robot.sweetai.cn"
+      server_port: "1883"
+      device_key: "1731842529342955520"              #设备ID号
+      #device_key: "1727217057199988746"
+      secure_id: "secureRobot"
+      secure_key: "secureRobot"
+
+      #mqtt_server_ip: "test.sweetai.cn"        #服务器ip
+      #device_key: "1970417648118595584"              #设备ID号
+      #secure_id: "secureId"
+      #secure_key: "secureKey"
+  
+      #下面为RTK运输小车所使用的参数
+      coordinateDistanceThreshold: 0.2  
+      coordinateYawThreshold: 0.174      
+      mapId: "1676848562602491904"   
+
+      isFollowingSlaver: false
+      isFollowingMaster: false
+      position_server_ip: "117.185.46.195" 
+      position_device_key: "send/123456789"
+      
+      is_use_ntrip: true
+
+      #ntrip_server_ip: "robot.sweetai.cn"   #117.185.46.195 120.253.239.161[备用] sdk.pnt.10086.cn[备用] 120.253.226.97 115.236.153.172
+      #ntrip_server_user: "1731842529342955520" # cpef4809  qa
+      #ntrip_server_password: "secureRobot"  # tje050j3  12345678
+      ntrip_server_port: 7210   #8001:CGCS2000坐标系 8002:WGS84坐标系  58050
+      ntrip_server_mountpoint: "SWEET33_GRCE"  #RTCM33_GRCE  SWEET33_GRCE
+      ntrip_server_agent: "NTRIP GNSSInternetRadio/1.0.0"
+
+remote_msg_driver:
+    # parameters
+    ros__parameters:
+      lift_serial_address: 0x28
+      work_serial_address: 0x29
+      motor_on_serial_address: 0x21
+      device_name: "张三"
+
+rtk_driver:
+    # parameters
+    ros__parameters:
+      ser_port: "/dev/ttyS7"                       #rtk串口号
+      is_rtk_antenna_horizontal: false             # true为rtk左右安装，false为前后安装
+      ser_baudrate: 115200
+      use_imu_prediction: false
+      
+imu_driver:
+    ros__parameters:  
+      imu_port: "/dev/ttyS3"
+      imu_baudrate: 9600
+
+wireless_driver:
+    # parameters
+    ros__parameters:
+      ser_baudrate: 115200
+      ser_port: "/dev/ttyS5"              #自研大工控机为ttyS9 新板子为ttyS5
+      #device_key: "1731842529342955520"   #修改为相应的设备ID，与remote_control节点设备号一致
+
+ObstacleAvoidance:
+    # parameters
+    ros__parameters:
+      collisionThreshold: 2.0  #减速阈值
+      stopThreshold: 0.91       #停止阈值
+      lidarDetectionThreshold: 15.0   #激光雷达探测阈值，默认15m
+      carRadius: 0.75              #小车半径
+      radarDetectionModeSelect: 0     #小车激光数据获取范围模式（0：前面部分、1：后面部分、2：前后两部分）
+      angularResolution: 0.12 #角度分辨率 单位：度
+
+LaserControl:
+  ros__parameters:
+    galvanometerSerialPort: "/dev/ttyUSB2"
+    serBaudrate: 115200
+
+bmi088_driver:
+    # parameters
+    ros__parameters:
+      use_i2c: true
+      i2c_port: "/dev/i2c-5"
+      serial_port: "/dev/ttyUSB0"
+      ser_baudrate: 115200
+      imu_extrinsic_roll: 0.0 #3.1415926
+      imu_extrinsic_pitch: 0.0
+      imu_extrinsic_yaw: 0.0
+
+      is_imu_reverse: false
+      use_initial_offset: false
+
+auto_following:
+    # parameters
+    ros__parameters:
+      left_dis: -0.2
+      hinder_dis: 2.5
+      max_linear_vel: 0.5
+      max_angular_vel: 0.2
+      max_slow_angular_vel: 0.1 #0.174
+      min_tolerance_angle: 0.0348
+      kv: 0.0
+      look_ahead_dis: 0.3
+      master_rtk_to_center_dis: 0.58
+      master_rtk_to_car_center_dis: 1.415
+      kp: 0.15 #0.25
+      ki: 0.1 #0.1
+      kd: 0.05 #0.05
+      point_cluster_size: 5
+      slope_tolerance: 0.087 # 3 degrees
+      line_path_following_type: 1 # 1 for lqr, 2 for pid, 3 for pp
+      curve_path_following_type: 3 # 1 for lqr, 2 for pid, 3 for pp

auto_drive_proj_utils/config/version.yaml

@@ -0,0 +1 @@
+version: "VERSION-2.1.0_4" #当前版本号

auto_drive_proj_utils/package.xml

@@ -0,0 +1,20 @@
+<?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>auto_drive_proj_utils</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="ahui@todo.todo">ahui</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>backward_ros</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

auto_drive_proj_utils/spdlog/async.h

@@ -0,0 +1,99 @@
+// Copyright(c) 2015-present, Gabi Melman & spdlog contributors.
+// Distributed under the MIT License (http://opensource.org/licenses/MIT)
+
+#pragma once
+
+//
+// Async logging using global thread pool
+// All loggers created here share same global thread pool.
+// Each log message is pushed to a queue along with a shared pointer to the
+// logger.
+// If a logger deleted while having pending messages in the queue, it's actual
+// destruction will defer
+// until all its messages are processed by the thread pool.
+// This is because each message in the queue holds a shared_ptr to the
+// originating logger.
+
+#include <spdlog/async_logger.h>
+#include <spdlog/details/registry.h>
+#include <spdlog/details/thread_pool.h>
+
+#include <memory>
+#include <mutex>
+#include <functional>
+
+namespace spdlog {
+
+namespace details {
+static const size_t default_async_q_size = 8192;
+}
+
+// async logger factory - creates async loggers backed with thread pool.
+// if a global thread pool doesn't already exist, create it with default queue
+// size of 8192 items and single thread.
+template<async_overflow_policy OverflowPolicy = async_overflow_policy::block>
+struct async_factory_impl
+{
+    template<typename Sink, typename... SinkArgs>
+    static std::shared_ptr<async_logger> create(std::string logger_name, SinkArgs &&... args)
+    {
+        auto &registry_inst = details::registry::instance();
+
+        // create global thread pool if not already exists..
+
+        auto &mutex = registry_inst.tp_mutex();
+        std::lock_guard<std::recursive_mutex> tp_lock(mutex);
+        auto tp = registry_inst.get_tp();
+        if (tp == nullptr)
+        {
+            tp = std::make_shared<details::thread_pool>(details::default_async_q_size, 1U);
+            registry_inst.set_tp(tp);
+        }
+
+        auto sink = std::make_shared<Sink>(std::forward<SinkArgs>(args)...);
+        auto new_logger = std::make_shared<async_logger>(std::move(logger_name), std::move(sink), std::move(tp), OverflowPolicy);
+        registry_inst.initialize_logger(new_logger);
+        return new_logger;
+    }
+};
+
+using async_factory = async_factory_impl<async_overflow_policy::block>;
+using async_factory_nonblock = async_factory_impl<async_overflow_policy::overrun_oldest>;
+
+template<typename Sink, typename... SinkArgs>
+inline std::shared_ptr<spdlog::logger> create_async(std::string logger_name, SinkArgs &&... sink_args)
+{
+    return async_factory::create<Sink>(std::move(logger_name), std::forward<SinkArgs>(sink_args)...);
+}
+
+template<typename Sink, typename... SinkArgs>
+inline std::shared_ptr<spdlog::logger> create_async_nb(std::string logger_name, SinkArgs &&... sink_args)
+{
+    return async_factory_nonblock::create<Sink>(std::move(logger_name), std::forward<SinkArgs>(sink_args)...);
+}
+
+// set global thread pool.
+inline void init_thread_pool(
+    size_t q_size, size_t thread_count, std::function<void()> on_thread_start, std::function<void()> on_thread_stop)
+{
+    auto tp = std::make_shared<details::thread_pool>(q_size, thread_count, on_thread_start, on_thread_stop);
+    details::registry::instance().set_tp(std::move(tp));
+}
+
+inline void init_thread_pool(size_t q_size, size_t thread_count, std::function<void()> on_thread_start)
+{
+    init_thread_pool(q_size, thread_count, on_thread_start, [] {});
+}
+
+inline void init_thread_pool(size_t q_size, size_t thread_count)
+{
+    init_thread_pool(
+        q_size, thread_count, [] {}, [] {});
+}
+
+// get the global thread pool.
+inline std::shared_ptr<spdlog::details::thread_pool> thread_pool()
+{
+    return details::registry::instance().get_tp();
+}
+} // namespace spdlog

auto_drive_proj_utils/spdlog/async_logger-inl.h

@@ -0,0 +1,92 @@
+// Copyright(c) 2015-present, Gabi Melman & spdlog contributors.
+// Distributed under the MIT License (http://opensource.org/licenses/MIT)
+
+#pragma once
+
+#ifndef SPDLOG_HEADER_ONLY
+#    include <spdlog/async_logger.h>
+#endif
+
+#include <spdlog/sinks/sink.h>
+#include <spdlog/details/thread_pool.h>
+
+#include <memory>
+#include <string>
+
+SPDLOG_INLINE spdlog::async_logger::async_logger(
+    std::string logger_name, sinks_init_list sinks_list, std::weak_ptr<details::thread_pool> tp, async_overflow_policy overflow_policy)
+    : async_logger(std::move(logger_name), sinks_list.begin(), sinks_list.end(), std::move(tp), overflow_policy)
+{}
+
+SPDLOG_INLINE spdlog::async_logger::async_logger(
+    std::string logger_name, sink_ptr single_sink, std::weak_ptr<details::thread_pool> tp, async_overflow_policy overflow_policy)
+    : async_logger(std::move(logger_name), {std::move(single_sink)}, std::move(tp), overflow_policy)
+{}
+
+// send the log message to the thread pool
+SPDLOG_INLINE void spdlog::async_logger::sink_it_(const details::log_msg &msg)
+{
+    if (auto pool_ptr = thread_pool_.lock())
+    {
+        pool_ptr->post_log(shared_from_this(), msg, overflow_policy_);
+    }
+    else
+    {
+        throw_spdlog_ex("async log: thread pool doesn't exist anymore");
+    }
+}
+
+// send flush request to the thread pool
+SPDLOG_INLINE void spdlog::async_logger::flush_()
+{
+    if (auto pool_ptr = thread_pool_.lock())
+    {
+        pool_ptr->post_flush(shared_from_this(), overflow_policy_);
+    }
+    else
+    {
+        throw_spdlog_ex("async flush: thread pool doesn't exist anymore");
+    }
+}
+
+//
+// backend functions - called from the thread pool to do the actual job
+//
+SPDLOG_INLINE void spdlog::async_logger::backend_sink_it_(const details::log_msg &msg)
+{
+    for (auto &sink : sinks_)
+    {
+        if (sink->should_log(msg.level))
+        {
+            SPDLOG_TRY
+            {
+                sink->log(msg);
+            }
+            SPDLOG_LOGGER_CATCH(msg.source)
+        }
+    }
+
+    if (should_flush_(msg))
+    {
+        backend_flush_();
+    }
+}
+
+SPDLOG_INLINE void spdlog::async_logger::backend_flush_()
+{
+    for (auto &sink : sinks_)
+    {
+        SPDLOG_TRY
+        {
+            sink->flush();
+        }
+        SPDLOG_LOGGER_CATCH(source_loc())
+    }
+}
+
+SPDLOG_INLINE std::shared_ptr<spdlog::logger> spdlog::async_logger::clone(std::string new_name)
+{
+    auto cloned = std::make_shared<spdlog::async_logger>(*this);
+    cloned->name_ = std::move(new_name);
+    return cloned;
+}

auto_drive_proj_utils/spdlog/async_logger.h

@@ -0,0 +1,68 @@
+// Copyright(c) 2015-present, Gabi Melman & spdlog contributors.
+// Distributed under the MIT License (http://opensource.org/licenses/MIT)
+
+#pragma once
+
+// Fast asynchronous logger.
+// Uses pre allocated queue.
+// Creates a single back thread to pop messages from the queue and log them.
+//
+// Upon each log write the logger:
+//    1. Checks if its log level is enough to log the message
+//    2. Push a new copy of the message to a queue (or block the caller until
+//    space is available in the queue)
+// Upon destruction, logs all remaining messages in the queue before
+// destructing..
+
+#include <spdlog/logger.h>
+
+namespace spdlog {
+
+// Async overflow policy - block by default.
+enum class async_overflow_policy
+{
+    block,         // Block until message can be enqueued
+    overrun_oldest // Discard oldest message in the queue if full when trying to
+                   // add new item.
+};
+
+namespace details {
+class thread_pool;
+}
+
+class SPDLOG_API async_logger final : public std::enable_shared_from_this<async_logger>, public logger
+{
+    friend class details::thread_pool;
+
+public:
+    template<typename It>
+    async_logger(std::string logger_name, It begin, It end, std::weak_ptr<details::thread_pool> tp,
+        async_overflow_policy overflow_policy = async_overflow_policy::block)
+        : logger(std::move(logger_name), begin, end)
+        , thread_pool_(std::move(tp))
+        , overflow_policy_(overflow_policy)
+    {}
+
+    async_logger(std::string logger_name, sinks_init_list sinks_list, std::weak_ptr<details::thread_pool> tp,
+        async_overflow_policy overflow_policy = async_overflow_policy::block);
+
+    async_logger(std::string logger_name, sink_ptr single_sink, std::weak_ptr<details::thread_pool> tp,
+        async_overflow_policy overflow_policy = async_overflow_policy::block);
+
+    std::shared_ptr<logger> clone(std::string new_name) override;
+
+protected:
+    void sink_it_(const details::log_msg &msg) override;
+    void flush_() override;
+    void backend_sink_it_(const details::log_msg &incoming_log_msg);
+    void backend_flush_();
+
+private:
+    std::weak_ptr<details::thread_pool> thread_pool_;
+    async_overflow_policy overflow_policy_;
+};
+} // namespace spdlog
+
+#ifdef SPDLOG_HEADER_ONLY
+#    include "async_logger-inl.h"
+#endif

auto_drive_proj_utils/spdlog/cfg/argv.h

@@ -0,0 +1,44 @@
+// Copyright(c) 2015-present, Gabi Melman & spdlog contributors.
+// Distributed under the MIT License (http://opensource.org/licenses/MIT)
+
+#pragma once
+#include <spdlog/cfg/helpers.h>
+#include <spdlog/details/registry.h>
+
+//
+// Init log levels using each argv entry that starts with "SPDLOG_LEVEL="
+//
+// set all loggers to debug level:
+// example.exe "SPDLOG_LEVEL=debug"
+
+// set logger1 to trace level
+// example.exe "SPDLOG_LEVEL=logger1=trace"
+
+// turn off all logging except for logger1 and logger2:
+// example.exe "SPDLOG_LEVEL=off,logger1=debug,logger2=info"
+
+namespace spdlog {
+namespace cfg {
+
+// search for SPDLOG_LEVEL= in the args and use it to init the levels
+inline void load_argv_levels(int argc, const char **argv)
+{
+    const std::string spdlog_level_prefix = "SPDLOG_LEVEL=";
+    for (int i = 1; i < argc; i++)
+    {
+        std::string arg = argv[i];
+        if (arg.find(spdlog_level_prefix) == 0)
+        {
+            auto levels_string = arg.substr(spdlog_level_prefix.size());
+            helpers::load_levels(levels_string);
+        }
+    }
+}
+
+inline void load_argv_levels(int argc, char **argv)
+{
+    load_argv_levels(argc, const_cast<const char **>(argv));
+}
+
+} // namespace cfg
+} // namespace spdlog

auto_drive_proj_utils/spdlog/cfg/env.h

@@ -0,0 +1,38 @@
+// Copyright(c) 2015-present, Gabi Melman & spdlog contributors.
+// Distributed under the MIT License (http://opensource.org/licenses/MIT)
+
+#pragma once
+#include <spdlog/cfg/helpers.h>
+#include <spdlog/details/registry.h>
+#include <spdlog/details/os.h>
+
+//
+// Init levels and patterns from env variables SPDLOG_LEVEL
+// Inspired from Rust's "env_logger" crate (https://crates.io/crates/env_logger).
+// Note - fallback to "info" level on unrecognized levels
+//
+// Examples:
+//
+// set global level to debug:
+// export SPDLOG_LEVEL=debug
+//
+// turn off all logging except for logger1:
+// export SPDLOG_LEVEL="*=off,logger1=debug"
+//
+
+// turn off all logging except for logger1 and logger2:
+// export SPDLOG_LEVEL="off,logger1=debug,logger2=info"
+
+namespace spdlog {
+namespace cfg {
+inline void load_env_levels()
+{
+    auto env_val = details::os::getenv("SPDLOG_LEVEL");
+    if (!env_val.empty())
+    {
+        helpers::load_levels(env_val);
+    }
+}
+
+} // namespace cfg
+} // namespace spdlog

auto_drive_proj_utils/spdlog/cfg/helpers-inl.h

@@ -0,0 +1,120 @@
+// Copyright(c) 2015-present, Gabi Melman & spdlog contributors.
+// Distributed under the MIT License (http://opensource.org/licenses/MIT)
+
+#pragma once
+
+#ifndef SPDLOG_HEADER_ONLY
+#    include <spdlog/cfg/helpers.h>
+#endif
+
+#include <spdlog/spdlog.h>
+#include <spdlog/details/os.h>
+#include <spdlog/details/registry.h>
+
+#include <algorithm>
+#include <string>
+#include <utility>
+#include <sstream>
+
+namespace spdlog {
+namespace cfg {
+namespace helpers {
+
+// inplace convert to lowercase
+inline std::string &to_lower_(std::string &str)
+{
+    std::transform(
+        str.begin(), str.end(), str.begin(), [](char ch) { return static_cast<char>((ch >= 'A' && ch <= 'Z') ? ch + ('a' - 'A') : ch); });
+    return str;
+}
+
+// inplace trim spaces
+inline std::string &trim_(std::string &str)
+{
+    const char *spaces = " \n\r\t";
+    str.erase(str.find_last_not_of(spaces) + 1);
+    str.erase(0, str.find_first_not_of(spaces));
+    return str;
+}
+
+// return (name,value) trimmed pair from given "name=value" string.
+// return empty string on missing parts
+// "key=val" => ("key", "val")
+// " key  =  val " => ("key", "val")
+// "key=" => ("key", "")
+// "val" => ("", "val")
+
+inline std::pair<std::string, std::string> extract_kv_(char sep, const std::string &str)
+{
+    auto n = str.find(sep);
+    std::string k, v;
+    if (n == std::string::npos)
+    {
+        v = str;
+    }
+    else
+    {
+        k = str.substr(0, n);
+        v = str.substr(n + 1);
+    }
+    return std::make_pair(trim_(k), trim_(v));
+}
+
+// return vector of key/value pairs from sequence of "K1=V1,K2=V2,.."
+// "a=AAA,b=BBB,c=CCC,.." => {("a","AAA"),("b","BBB"),("c", "CCC"),...}
+inline std::unordered_map<std::string, std::string> extract_key_vals_(const std::string &str)
+{
+    std::string token;
+    std::istringstream token_stream(str);
+    std::unordered_map<std::string, std::string> rv{};
+    while (std::getline(token_stream, token, ','))
+    {
+        if (token.empty())
+        {
+            continue;
+        }
+        auto kv = extract_kv_('=', token);
+        rv[kv.first] = kv.second;
+    }
+    return rv;
+}
+
+SPDLOG_INLINE void load_levels(const std::string &input)
+{
+    if (input.empty() || input.size() > 512)
+    {
+        return;
+    }
+
+    auto key_vals = extract_key_vals_(input);
+    std::unordered_map<std::string, level::level_enum> levels;
+    level::level_enum global_level = level::info;
+    bool global_level_found = false;
+
+    for (auto &name_level : key_vals)
+    {
+        auto &logger_name = name_level.first;
+        auto level_name = to_lower_(name_level.second);
+        auto level = level::from_str(level_name);
+        // ignore unrecognized level names
+        if (level == level::off && level_name != "off")
+        {
+            continue;
+        }
+        if (logger_name.empty()) // no logger name indicate global level
+        {
+            global_level_found = true;
+            global_level = level;
+        }
+        else
+        {
+            levels[logger_name] = level;
+        }
+    }
+
+    details::registry::instance().set_levels(std::move(levels), global_level_found ? &global_level : nullptr);
+}
+
+} // namespace helpers
+} // namespace cfg
+} // namespace spdlog