init

src/rm_hardware_driver/rm_serial_driver/CMakeLists.txt

@@ -0,0 +1,66 @@
+cmake_minimum_required(VERSION 3.10)
+project(rm_serial_driver)
+
+## Use C++17
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+## By adding -Wall and -Werror, the compiler does not ignore warnings anymore,
+## enforcing cleaner code.
+add_definitions(-Wall -Werror -O3)
+
+## Export compile commands for clangd
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+#######################
+## Find dependencies ##
+#######################
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+###########
+## Build ##
+###########
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+DIRECTORY src
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN fyt::serial_driver::SerialDriverNode
+  EXECUTABLE ${PROJECT_NAME}_node
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN fyt::serial_driver::VirtualSerialNode
+  EXECUTABLE virtual_serial_node
+)
+
+target_include_directories(${PROJECT_NAME}_node PUBLIC include)
+
+#############
+## Testing ##
+#############
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  list(APPEND AMENT_LINT_AUTO_EXCLUDE
+    ament_cmake_copyright
+    ament_cmake_uncrustify
+    ament_cmake_cpplint
+  )
+  ament_lint_auto_find_test_dependencies()
+  find_package(ament_cmake_gtest)
+
+  ament_add_gtest(test_fixed_packet_tool test/test_fixed_packet_tool.cpp)
+  target_link_libraries(test_fixed_packet_tool ${PROJECT_NAME})
+endif()
+
+#############
+## Install ##
+#############
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+)

src/rm_hardware_driver/rm_serial_driver/README.md

@@ -0,0 +1,40 @@
+# rm_serial_driver
+
+FYT视觉24赛季串口通信模块
+
+## fyt::SerialDriverNode
+
+串口驱动节点
+
+### 发布话题
+
+*  `serial/receive` (`rm_interfaces/msg/SerialReceiveData`) - 下位机发送到上位机的数据
+*  `tf` (`geometry_msgs/msg/TransformStamped`) - 云台的tf变换
+  
+### Subscribed Topics
+
+* `cmd_gimbal` (`rm_interfaces/msg/GimbalCmd`) - 云台控制信息
+* `cmd_chassis` (`rm_interfaces/msg/ChassisCmd`) - 底盘控制信息
+
+### 参数
+
+* `target_frame` (string, default: "odom") - 下位机欧拉角的相对坐标系
+* `timestamp_offset` (double, default: 0.0) - tf数据的时间戳补偿
+* `port_name` (string, default: "/dev/ttyUART") - 串口设备对应的文件名
+* `protocol` (string, default: "infantry") - 协议类型
+* `enable_data_print` (bool, default: false) - 是否打印串口读出的原始数据
+
+## fyt::VirtualSerial
+
+仿真串口驱动节点
+
+### 发布话题
+
+*  `serial/receive` (`rm_interfaces/msg/SerialReceiveData`) - 下位机发送到上位机的数据（固定数据）
+*  `tf` (`geometry_msgs/msg/TransformStamped`) - 云台的tf变换（固定数据）
+  
+### 参数
+
+* `pitch` (double, default: 0.0) - 固定的pitch角度 
+* `yaw` (double, default: 0.0) - 固定的yaw角度 
+* `vision_mode` (int, default: 0) - 视觉模式 

src/rm_hardware_driver/rm_serial_driver/include/rm_serial_driver/crc8.hpp

@@ -0,0 +1,19 @@
+// Copyright (C) FYT Vision Group. All rights reserved.
+
+#ifndef RM_SERIAL_DRIVER_CRC8_HPP_
+#define RM_SERIAL_DRIVER_CRC8_HPP_
+
+#include <cstddef>
+#include <cstdint>
+
+namespace fyt::serial_driver {
+
+constexpr uint8_t kCrc8Init = 0xff;
+
+uint8_t GetCrc8(const uint8_t *data, size_t len, uint8_t init = kCrc8Init);
+bool VerifyCrc8(const uint8_t *data, size_t len);
+void AppendCrc8(uint8_t *data, size_t len);
+
+}  // namespace fyt::serial_driver
+
+#endif  // RM_SERIAL_DRIVER_CRC8_HPP_

src/rm_hardware_driver/rm_serial_driver/include/rm_serial_driver/fixed_packet.hpp

@@ -0,0 +1,85 @@
+// Copyright (C) 2021 RoboMaster-OSS
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Additional modifications and features by Chengfu Zou, 2023.
+//
+// Copyright (C) FYT Vision Group. All rights reserved.
+
+#ifndef SERIAL_DRIVER_FIXED_PACKET_HPP_
+#define SERIAL_DRIVER_FIXED_PACKET_HPP_
+
+// std
+#include <cstdint>
+#include <cstring>
+#include <memory>
+
+namespace fyt::serial_driver {
+
+// Fixed-length data packet:
+// [head_byte(0xff), ...(data_bytes)..., crc8_byte]
+template <int capacity = 16>
+class FixedPacket {
+ public:
+  using SharedPtr = std::shared_ptr<FixedPacket>;
+  FixedPacket() {
+    memset(buffer_, 0, capacity);
+    buffer_[0] = 0xff;  // frame head
+  }
+
+ public:
+  // Clear payload and crc.
+  void clear() { memset(buffer_ + 1, 0, capacity - 1); }
+  // Set crc8 byte (last byte).
+  void setCrc8(uint8_t crc) { buffer_[capacity - 1] = crc; }
+  // Backward-compatible alias.
+  void setCheckByte(uint8_t check_byte) { setCrc8(check_byte); }
+  // Copy data to internal buffer.
+  void copyFrom(const void* src) { memcpy(buffer_, src, capacity); }
+  // Get internal buffer (read-only).
+  const uint8_t* buffer() const { return buffer_; }
+
+  // Load custom data to buffer.
+  template <typename T, int data_len = sizeof(T)>
+  bool loadData(T const &data, int index) {
+    // Bounds check
+    if (index > 0 && ((index + data_len) < (capacity))) {
+      memcpy(buffer_ + index, &data, data_len);
+      return true;
+    }
+    return false;
+  }
+
+  // Unload data from buffer.
+  template <typename T, int data_len = sizeof(T)>
+  bool unloadData(T &data, int index) {
+    // Bounds check
+    if (index > 0 && ((index + data_len) < (capacity))) {
+      memcpy(&data, buffer_ + index, data_len);
+      return true;
+    }
+    return false;
+  }
+
+ private:
+  // Packet buffer
+  uint8_t buffer_[capacity];  // NOLINT
+};
+
+using FixedPacket16 = FixedPacket<16>;
+using FixedPacket32 = FixedPacket<32>;
+using FixedPacket64 = FixedPacket<64>;
+
+}  // namespace fyt::serial_driver
+
+#endif  // SERIAL_DRIVER_FIXED_PACKET_HPP_

src/rm_hardware_driver/rm_serial_driver/include/rm_serial_driver/fixed_packet_tool.hpp

@@ -0,0 +1,214 @@
+// Copyright (C) 2021 RoboMaster-OSS
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Additional modifications and features by Chengfu Zou, 2023.
+//
+// Copyright (C) FYT Vision Group. All rights reserved.
+
+#ifndef SERIAL_DRIVER_FIXED_PACKET_TOOL_HPP_
+#define SERIAL_DRIVER_FIXED_PACKET_TOOL_HPP_
+
+// std
+#include <chrono>
+#include <cstddef>
+#include <cstring>
+#include <memory>
+#include <mutex>
+#include <queue>
+#include <stdexcept>
+#include <string>
+#include <thread>
+// project
+#include "rm_serial_driver/crc8.hpp"
+#include "rm_serial_driver/fixed_packet.hpp"
+#include "rm_serial_driver/transporter_interface.hpp"
+#include "rm_utils/logger/log.hpp"
+
+namespace fyt::serial_driver {
+
+template <int capacity = 16>
+class FixedPacketTool {
+public:
+  using SharedPtr = std::shared_ptr<FixedPacketTool>;
+  FixedPacketTool() = delete;
+  explicit FixedPacketTool(std::shared_ptr<TransporterInterface> transporter)
+  : transporter_(transporter) {
+    if (!transporter) {
+      throw std::invalid_argument("transporter is nullptr");
+    }
+    FYT_REGISTER_LOGGER("serial_driver", "~/fyt2024-log", INFO);
+  }
+
+  ~FixedPacketTool() { enbaleRealtimeSend(false); }
+
+  bool isOpen() { return transporter_->isOpen(); }
+  void enbaleRealtimeSend(bool enable);
+  void setRequiredLength(int required_len) { required_len_ = required_len; }
+  bool sendPacket(const FixedPacket<capacity> &packet);
+  bool recvPacket(FixedPacket<capacity> &packet);
+
+  std::string getErrorMessage() { return transporter_->errorMessage(); }
+
+private:
+  bool checkPacket(uint8_t *tmp_buffer, int recv_len);
+  bool simpleSendPacket(const FixedPacket<capacity> &packet);
+
+private:
+  std::shared_ptr<TransporterInterface> transporter_;
+  // data
+  uint8_t tmp_buffer_[capacity];       // NOLINT
+  uint8_t recv_buffer_[capacity * 2];  // NOLINT
+  int recv_buf_len_{0};
+  int required_len_{0};
+  // for realtime sending
+  bool use_realtime_send_{false};
+  std::mutex realtime_send_mut_;
+  std::unique_ptr<std::thread> realtime_send_thread_;
+  std::queue<FixedPacket<capacity>> realtime_packets_;
+};
+
+template <int capacity>
+bool FixedPacketTool<capacity>::checkPacket(uint8_t *buffer, int recv_len) {
+  // Check length
+  if (required_len_ > 0 && recv_len != required_len_) {
+    return false;
+  }
+  if (required_len_ <= 0 && recv_len != capacity) {
+    return false;
+  }
+  // Check frame head
+  if (buffer[0] != 0xff) {
+    return false;
+  }
+  // Check CRC8 (last byte)
+  return VerifyCrc8(buffer, static_cast<size_t>(recv_len));
+}
+
+template <int capacity>
+bool FixedPacketTool<capacity>::simpleSendPacket(const FixedPacket<capacity> &packet) {
+  uint8_t send_buffer[capacity];
+  memcpy(send_buffer, packet.buffer(), capacity);
+  AppendCrc8(send_buffer, capacity);
+  if (transporter_->write(send_buffer, capacity) == capacity) {
+    return true;
+  } else {
+    // reconnect
+    FYT_ERROR("serial_driver", "transporter_->write() failed");
+    transporter_->close();
+    transporter_->open();
+    return false;
+  }
+}
+
+template <int capacity>
+void FixedPacketTool<capacity>::enbaleRealtimeSend(bool enable) {
+  if (enable == use_realtime_send_) {
+    return;
+  }
+  if (enable) {
+    use_realtime_send_ = true;
+    realtime_send_thread_ = std::make_unique<std::thread>([&]() {
+      FixedPacket<capacity> packet;
+      while (use_realtime_send_) {
+        bool empty = true;
+        {
+          std::lock_guard<std::mutex> lock(realtime_send_mut_);
+          empty = realtime_packets_.empty();
+          if (!empty) {
+            packet = realtime_packets_.front();
+            realtime_packets_.pop();
+          }
+        }
+        if (!empty) {
+          simpleSendPacket(packet);
+        } else {
+          std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+      }
+    });
+  } else {
+    use_realtime_send_ = false;
+    realtime_send_thread_->join();
+    realtime_send_thread_.reset();
+  }
+}
+
+template <int capacity>
+bool FixedPacketTool<capacity>::sendPacket(const FixedPacket<capacity> &packet) {
+  if (use_realtime_send_) {
+    std::lock_guard<std::mutex> lock(realtime_send_mut_);
+    realtime_packets_.push(packet);
+    return true;
+  } else {
+    return simpleSendPacket(packet);
+  }
+}
+
+template <int capacity>
+bool FixedPacketTool<capacity>::recvPacket(FixedPacket<capacity> &packet) {
+  memset(tmp_buffer_, 0, capacity);
+  int recv_len = transporter_->read(tmp_buffer_, capacity);
+  if (recv_len > 0) {
+    // check packet
+    if (checkPacket(tmp_buffer_, recv_len)) {
+      packet.copyFrom(tmp_buffer_);
+      return true;
+    } else {
+      // Try to recover broken frames by concatenating and scanning.
+      FYT_INFO("serial_driver", "checkPacket() failed, check if it is a broken frame");
+      if (recv_buf_len_ + recv_len > capacity * 2) {
+        recv_buf_len_ = 0;
+      }
+      // Concatenate buffer
+      memcpy(recv_buffer_ + recv_buf_len_, tmp_buffer_, recv_len);
+      recv_buf_len_ = recv_buf_len_ + recv_len;
+      // Scan for valid frame
+      int primary_len = required_len_ > 0 ? required_len_ : capacity;
+      for (int i = 0; (i + primary_len) <= recv_buf_len_; i++) {
+        if (checkPacket(recv_buffer_ + i, primary_len)) {
+          packet.copyFrom(recv_buffer_ + i);
+          // Update receive buffer after extracting one frame
+          int k = 0;
+          for (int j = i + primary_len; j < recv_buf_len_; j++, k++) {
+            recv_buffer_[k] = recv_buffer_[j];
+          }
+          recv_buf_len_ = k;
+          return true;
+        }
+      }
+      // Broken frame or invalid data
+      FYT_WARN("serial_driver",
+               "checkPacket() failed with recv_len:{}, frame head:{}, crc:{}",
+               recv_len,
+               tmp_buffer_[0],
+               tmp_buffer_[recv_len - 1]);
+      return false;
+    }
+  } else {
+    FYT_ERROR("serial_driver", "transporter_->read() failed");
+    // reconnect
+    transporter_->close();
+    transporter_->open();
+    // serial port error
+    return false;
+  }
+}
+
+using FixedPacketTool16 = FixedPacketTool<16>;
+using FixedPacketTool32 = FixedPacketTool<32>;
+using FixedPacketTool64 = FixedPacketTool<64>;
+
+}  // namespace fyt::serial_driver
+
+#endif  // SERIAL_DRIVER_FIXED_PACKET_TOOL_HPP_

src/rm_hardware_driver/rm_serial_driver/include/rm_serial_driver/protocol.hpp

@@ -0,0 +1,68 @@
+// Created by Chengfu Zou on 2023.7.6
+// Copyright (C) FYT Vision Group. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SERIAL_DRIVER_PROTOCOL_HPP_
+#define SERIAL_DRIVER_PROTOCOL_HPP_
+
+// std
+#include <memory>
+#include <string>
+#include <string_view>
+// ros2
+#include <geometry_msgs/msg/twist.hpp>
+#include <rclcpp/rclcpp.hpp>
+// project
+#include "rm_interfaces/msg/chassis_cmd.hpp"
+#include "rm_interfaces/msg/gimbal_cmd.hpp"
+#include "rm_interfaces/msg/serial_receive_data.hpp"
+#include "rm_interfaces/srv/set_mode.hpp"
+#include "rm_serial_driver/fixed_packet.hpp"
+#include "rm_serial_driver/fixed_packet_tool.hpp"
+#include "rm_serial_driver/uart_transporter.hpp"
+
+namespace fyt::serial_driver {
+namespace protocol {
+typedef enum : unsigned char { Fire = 0x01, NotFire = 0x00 } FireState;
+
+// Protocol interface
+class Protocol {
+public:
+  virtual ~Protocol() = default;
+
+  // Send gimbal command
+  virtual void send(const rm_interfaces::msg::GimbalCmd &data) = 0;
+
+  // Receive data from serial port
+  virtual bool receive(rm_interfaces::msg::SerialReceiveData &data) = 0;
+
+  // Optional: force RX packet length (e.g., 16 or 20)
+  virtual void setRxPacketLength(int required_len) { (void)required_len; }
+
+  // Create subscriptions for SerialDriverNode
+  virtual std::vector<rclcpp::SubscriptionBase::SharedPtr> getSubscriptions(
+    rclcpp::Node::SharedPtr node) = 0;
+
+  // Cretate setMode client for SerialDriverNode
+  virtual std::vector<rclcpp::Client<rm_interfaces::srv::SetMode>::SharedPtr> getClients(
+    rclcpp::Node::SharedPtr node) const = 0;
+
+  virtual std::string getErrorMessage() = 0;
+
+private:
+};
+
+}  // namespace protocol
+}  // namespace fyt::serial_driver
+#endif  // SERIAL_DRIVER_PROTOCOLS_HPP_

src/rm_hardware_driver/rm_serial_driver/include/rm_serial_driver/protocol/default_protocol.hpp

@@ -0,0 +1,52 @@
+// Created by Chengfu Zou
+// Copyright (C) FYT Vision Group. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SERIAL_DRIVER_DEFAULT_PROTOCOL_HPP_
+#define SERIAL_DRIVER_DEFAULT_PROTOCOL_HPP_
+
+#include "rm_serial_driver/protocol.hpp"
+
+namespace fyt::serial_driver::protocol {
+// 默认
+class DefaultProtocol : public Protocol {
+public:
+  explicit DefaultProtocol(std::string_view port_name,
+                           bool enable_data_print,
+                           bool enable_send_print);
+
+  ~DefaultProtocol() = default;
+
+  void send(const rm_interfaces::msg::GimbalCmd &data) override;
+
+  bool receive(rm_interfaces::msg::SerialReceiveData &data) override;
+  void setRxPacketLength(int required_len) override;
+
+  std::vector<rclcpp::SubscriptionBase::SharedPtr> getSubscriptions(rclcpp::Node::SharedPtr node) override;
+
+  std::vector<rclcpp::Client<rm_interfaces::srv::SetMode>::SharedPtr> getClients(
+    rclcpp::Node::SharedPtr node) const override;
+
+  std::string getErrorMessage() override { return rx_packet_tool_->getErrorMessage(); }
+
+private:
+  int rx_packet_length_{20};
+  FixedPacketTool<20>::SharedPtr rx_packet_tool_;
+  FixedPacketTool<16>::SharedPtr tx_packet_tool_;
+  bool enable_data_print_{false};
+  bool enable_send_print_{false};
+};
+}  // namespace fyt::serial_driver::protocol
+
+#endif  // SERIAL_DRIVER_DEFAULT_PROTOCOL_HPP_

src/rm_hardware_driver/rm_serial_driver/include/rm_serial_driver/protocol/infantry_protocol.hpp

@@ -0,0 +1,53 @@
+// Created by Chengfu Zou
+// Copyright (C) FYT Vision Group. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SERIAL_DRIVER_INFANTRY_PROTOCOL_HPP_
+#define SERIAL_DRIVER_INFANTRY_PROTOCOL_HPP_
+
+#include "rm_serial_driver/protocol.hpp"
+
+namespace fyt::serial_driver::protocol {
+// 步兵通信协议
+class ProtocolInfantry : public Protocol {
+public:
+  explicit ProtocolInfantry(std::string_view port_name,
+                            bool enable_data_print,
+                            bool enable_send_print);
+
+  ~ProtocolInfantry() = default;
+
+  void send(const rm_interfaces::msg::GimbalCmd &data) override;
+
+  bool receive(rm_interfaces::msg::SerialReceiveData &data) override;
+  void setRxPacketLength(int required_len) override;
+
+  std::vector<rclcpp::SubscriptionBase::SharedPtr> getSubscriptions(
+    rclcpp::Node::SharedPtr node) override;
+
+  std::vector<rclcpp::Client<rm_interfaces::srv::SetMode>::SharedPtr> getClients(
+    rclcpp::Node::SharedPtr node) const override;
+
+  std::string getErrorMessage() override { return rx_packet_tool_->getErrorMessage(); }
+
+private:
+  int rx_packet_length_{20};
+  FixedPacketTool<20>::SharedPtr rx_packet_tool_;
+  FixedPacketTool<16>::SharedPtr tx_packet_tool_;
+  bool enable_data_print_{false};
+  bool enable_send_print_{false};
+};
+}  // namespace fyt::serial_driver::protocol
+
+#endif  // SERIAL_DRIVER_INFANTRY_PROTOCOL_HPP_

src/rm_hardware_driver/rm_serial_driver/include/rm_serial_driver/protocol/sentry_protocol.hpp

@@ -0,0 +1,58 @@
+// Created by Chengfu Zou
+// Copyright (C) FYT Vision Group. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SERIAL_DRIVER__SENTRY_PROTOCOL_HPP_
+#define SERIAL_DRIVER__SENTRY_PROTOCOL_HPP_
+
+// project
+#include "rm_interfaces/msg/chassis_cmd.hpp"
+#include "rm_serial_driver/protocol.hpp"
+
+namespace fyt::serial_driver::protocol {
+// 哨兵通信协议
+class ProtocolSentry : public Protocol {
+public:
+  explicit ProtocolSentry(std::string_view port_name,
+                          bool enable_data_print,
+                          bool enable_send_print);
+
+  ~ProtocolSentry() = default;
+
+  void send(const rm_interfaces::msg::GimbalCmd &data) override;
+
+  void send(const rm_interfaces::msg::ChassisCmd &data);
+
+  bool receive(rm_interfaces::msg::SerialReceiveData &data) override;
+  void setRxPacketLength(int required_len) override;
+
+  std::vector<rclcpp::SubscriptionBase::SharedPtr> getSubscriptions(rclcpp::Node::SharedPtr node) override;
+
+  std::vector<rclcpp::Client<rm_interfaces::srv::SetMode>::SharedPtr> getClients(
+    rclcpp::Node::SharedPtr node) const override;
+
+  std::string getErrorMessage() override { return packet_tool_->getErrorMessage(); }
+
+private:
+  enum GameStatus { NOT_START = 0x00, ENEMY_RED = 0x01, ENEMY_BLUE = 0x02 };
+  int rx_packet_length_{32};
+  FixedPacketTool<32>::SharedPtr packet_tool_;
+  FixedPacket<32> packet_;
+  rm_interfaces::msg::ChassisCmd chassis_cmd_;
+  bool enable_data_print_{false};
+  bool enable_send_print_{false};
+};
+}  // namespace fyt::serial_driver::protocol
+
+#endif  // SERIAL_DRIVER_SENTRY_PROTOCOL_HPP_

src/rm_hardware_driver/rm_serial_driver/include/rm_serial_driver/protocol_factory.hpp

@@ -0,0 +1,59 @@
+// Created by Chengfu Zou on 2023.7.6
+// Copyright (C) FYT Vision Group. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SERIAL_DRIVER_PROTOCOL_FACTORY_HPP_
+#define SERIAL_DRIVER_PROTOCOL_FACTORY_HPP_
+
+#include <memory>
+#include <string_view>
+
+#include "rm_serial_driver/protocol.hpp"
+#include "rm_serial_driver/protocol/default_protocol.hpp"
+#include "rm_serial_driver/protocol/infantry_protocol.hpp"
+#include "rm_serial_driver/protocol/sentry_protocol.hpp"
+
+namespace fyt::serial_driver {
+
+class ProtocolFactory {
+public:
+  ProtocolFactory() = delete;
+  // Factory method to create a protocol
+  static std::unique_ptr<protocol::Protocol> createProtocol(std::string_view protocol_type,
+                                                            std::string_view port_name,
+                                                            bool enable_data_print,
+                                                            bool enable_send_print) {
+    if (protocol_type == "infantry") {
+      return std::make_unique<protocol::ProtocolInfantry>(
+        port_name, enable_data_print, enable_send_print);
+    }
+    if (protocol_type == "hero") {
+      return std::make_unique<protocol::DefaultProtocol>(
+        port_name, enable_data_print, enable_send_print);
+    }
+    if (protocol_type == "air") {
+      return std::make_unique<protocol::DefaultProtocol>(
+        port_name, enable_data_print, enable_send_print);
+    }
+    if (protocol_type == "sentry") {
+      return std::make_unique<protocol::ProtocolSentry>(
+        port_name, enable_data_print, enable_send_print);
+    }
+
+    return nullptr;
+  }
+};
+
+};      // namespace fyt::serial_driver
+#endif  // SERIAL_DRIVER_PROTOCOL_FACTORY_HPP_

src/rm_hardware_driver/rm_serial_driver/include/rm_serial_driver/serial_driver_node.hpp

@@ -0,0 +1,90 @@
+// Created by Chengfu Zou on 2023.7.1
+// Copyright (C) FYT Vision Group. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SERIAL_DRIVER_SERIAL_DRIVER_NODE_HPP_
+#define SERIAL_DRIVER_SERIAL_DRIVER_NODE_HPP_
+
+// std
+#include <memory>
+#include <thread>
+// ros2
+#include <tf2_ros/transform_broadcaster.h>
+
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+// project
+#include "rm_utils/heartbeat.hpp"
+#include "rm_interfaces/msg/gimbal_cmd.hpp"
+#include "rm_interfaces/msg/serial_receive_data.hpp"
+#include "rm_interfaces/srv/set_mode.hpp"
+#include "rm_serial_driver/fixed_packet_tool.hpp"
+#include "rm_serial_driver/protocol.hpp"
+#include "rm_serial_driver/protocol_factory.hpp"
+#include "rm_serial_driver/transporter_interface.hpp"
+
+namespace fyt::serial_driver {
+
+// Node wrapper for SerialDriver
+// Implementing secondary development through the Protocol class
+class SerialDriverNode : public rclcpp::Node {
+public:
+  explicit SerialDriverNode(const rclcpp::NodeOptions &options);
+
+  ~SerialDriverNode();
+
+  void listenLoop();
+
+  void init();
+
+  // Param client to set detect_color
+  struct SetModeClient {
+    SetModeClient(rclcpp::Client<rm_interfaces::srv::SetMode>::SharedPtr p) : ptr(p) {}
+    std::atomic<bool> on_waiting = false;
+    std::atomic<int> mode = 0;
+    rclcpp::Client<rm_interfaces::srv::SetMode>::SharedPtr ptr;
+  };
+  std::unordered_map<std::string, SetModeClient> set_mode_clients_;
+  void setMode(SetModeClient &client, const uint8_t mode);
+
+private:
+  // Heartbeat
+  HeartBeatPublisher::SharedPtr heartbeat_;
+
+  std::unique_ptr<std::thread> listen_thread_;
+  // Protocol
+  std::unique_ptr<protocol::Protocol> protocol_;
+
+  std::string target_frame_;
+
+  // Subscriptions
+  std::vector<rclcpp::SubscriptionBase::SharedPtr> subscriptions_;
+  // Publisher
+  rclcpp::Publisher<rm_interfaces::msg::SerialReceiveData>::SharedPtr serial_receive_data_pub_;
+
+  // Broadcast tf from odom to pitch_link
+  double timestamp_offset_ = 0;
+  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+
+  std::vector<double> yaw_to_pitch_offset_ = {0.0, 0.0, 0.0};
+  double yaw_to_pitch_radius_ = 0.0;
+  bool use_yaw_to_pitch_angle_ = true;
+};
+
+}  // namespace fyt::serial_driver
+
+#endif  // SERIAL_DRIVER_SERIAL_DRIVER_NODE_HPP_
+

src/rm_hardware_driver/rm_serial_driver/include/rm_serial_driver/transporter_interface.hpp

@@ -0,0 +1,47 @@
+// Copyright (C) 2021 RoboMaster-OSS
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Additional modifications and features by Chengfu Zou, 2023.
+//
+// Copyright (C) FYT Vision Group. All rights reserved.
+
+#ifndef SERIAL_DRIVER_TRANSPORTER_INTERFACE_HPP_
+#define SERIAL_DRIVER_TRANSPORTER_INTERFACE_HPP_
+
+// std
+#include <memory>
+#include <string>
+
+namespace fyt::serial_driver {
+
+// Transporter device interface to transport data between embedded systems
+// (stm32,c51) and PC
+class TransporterInterface {
+public:
+  using SharedPtr = std::shared_ptr<TransporterInterface>;
+  virtual ~TransporterInterface() = default;
+  virtual bool open() = 0;
+  virtual void close() = 0;
+  virtual bool isOpen() = 0;
+  // return recv len>0, return <0 if error
+  virtual int read(void *buffer, size_t len) = 0;
+  // return send len>0, return <0 if error
+  virtual int write(const void *buffer, size_t len) = 0;
+  // get error message when open() return false.
+  virtual std::string errorMessage() = 0;
+};
+
+}  // namespace fyt::serial_driver
+
+#endif  // SERIAL_DRIVER_TRANSPORTER_INTERFACE_HPP_

src/rm_hardware_driver/rm_serial_driver/include/rm_serial_driver/uart_transporter.hpp

@@ -0,0 +1,73 @@
+// Copyright (C) 2021 RoboMaster-OSS
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Additional modifications and features by Chengfu Zou, 2023.
+//
+// Copyright (C) FYT Vision Group. All rights reserved.
+
+#ifndef SERIAL_DRIVER_UART_TRANSPORTER_HPP_
+#define SERIAL_DRIVER_UART_TRANSPORTER_HPP_
+
+// std
+#include <string>
+// project
+#include "rm_serial_driver/transporter_interface.hpp"
+
+namespace fyt::serial_driver {
+
+// 串口数据传输设备，符合通用传输接口。
+class UartTransporter : public TransporterInterface {
+public:
+  UartTransporter(const std::string &device_path = "/dev/ttyUSB0",
+                  int speed = 115200,
+                  int flow_ctrl = 0,
+                  int databits = 8,
+                  int stopbits = 1,
+                  int parity = 'N')
+  : device_path_(device_path)
+  , speed_(speed)
+  , flow_ctrl_(flow_ctrl)
+  , databits_(databits)
+  , stopbits_(stopbits)
+  , parity_(parity) {}
+
+  bool open() override;
+  void close() override;
+  bool isOpen() override;
+  int read(void *buffer, size_t len) override;
+  int write(const void *buffer, size_t len) override;
+  std::string errorMessage() override { return error_message_; }
+
+private:
+  bool setParam(
+    int speed = 115200, int flow_ctrl = 0, int databits = 0, int stopbits = 1, int parity = 'N');
+
+private:
+  // 设备文件描述符
+  int fd_{-1};
+  // 设备状态
+  bool is_open_{false};
+  std::string error_message_;
+  // 设备参数
+  std::string device_path_;
+  int speed_;
+  int flow_ctrl_;
+  int databits_;
+  int stopbits_;
+  int parity_;
+};
+
+}  // namespace fyt::serial_driver
+
+#endif  // SERIAL_DRIVER_UART_TRANSPORTER_HPP_

src/rm_hardware_driver/rm_serial_driver/package.xml

@@ -0,0 +1,37 @@
+<?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>rm_serial_driver</name>
+  <version>0.1.0</version>
+  <description>A template for ROS packages.</description>
+  <maintainer email="chen.junn@outlook.com">Chen Jun</maintainer>
+  <license>MIT</license>
+  <url type="website">https://github.com/chenjunnn/ros2_mindvision_camera</url>
+  <url type="bugtracker">https://github.com/chenjunnn/ros2_mindvision_camera/issues</url>
+  <author email="chen.junn@outlook.com">Chen Jun</author>
+
+  <!-- buildtool_depend: dependencies of the build process -->
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <!-- depend: build, export, and execution dependency -->
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>serial_driver</depend>
+  <depend>geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>visualization_msgs</depend>
+  <depend>rm_interfaces</depend>
+  <depend>rm_utils</depend>
+  <depend>std_srvs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_clang_format</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/rm_hardware_driver/rm_serial_driver/src/crc8.cpp

@@ -0,0 +1,52 @@
+// Copyright (C) FYT Vision Group. All rights reserved.
+
+#include "rm_serial_driver/crc8.hpp"
+
+namespace fyt::serial_driver {
+namespace {
+const uint8_t kCrc8Table[256] = {
+  0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83, 0xc2, 0x9c, 0x7e, 0x20, 0xa3, 0xfd, 0x1f, 0x41,
+  0x9d, 0xc3, 0x21, 0x7f, 0xfc, 0xa2, 0x40, 0x1e, 0x5f, 0x01, 0xe3, 0xbd, 0x3e, 0x60, 0x82, 0xdc,
+  0x23, 0x7d, 0x9f, 0xc1, 0x42, 0x1c, 0xfe, 0xa0, 0xe1, 0xbf, 0x5d, 0x03, 0x80, 0xde, 0x3c, 0x62,
+  0xbe, 0xe0, 0x02, 0x5c, 0xdf, 0x81, 0x63, 0x3d, 0x7c, 0x22, 0xc0, 0x9e, 0x1d, 0x43, 0xa1, 0xff,
+  0x46, 0x18, 0xfa, 0xa4, 0x27, 0x79, 0x9b, 0xc5, 0x84, 0xda, 0x38, 0x66, 0xe5, 0xbb, 0x59, 0x07,
+  0xdb, 0x85, 0x67, 0x39, 0xba, 0xe4, 0x06, 0x58, 0x19, 0x47, 0xa5, 0xfb, 0x78, 0x26, 0xc4, 0x9a,
+  0x65, 0x3b, 0xd9, 0x87, 0x04, 0x5a, 0xb8, 0xe6, 0xa7, 0xf9, 0x1b, 0x45, 0xc6, 0x98, 0x7a, 0x24,
+  0xf8, 0xa6, 0x44, 0x1a, 0x99, 0xc7, 0x25, 0x7b, 0x3a, 0x64, 0x86, 0xd8, 0x5b, 0x05, 0xe7, 0xb9,
+  0x8c, 0xd2, 0x30, 0x6e, 0xed, 0xb3, 0x51, 0x0f, 0x4e, 0x10, 0xf2, 0xac, 0x2f, 0x71, 0x93, 0xcd,
+  0x11, 0x4f, 0xad, 0xf3, 0x70, 0x2e, 0xcc, 0x92, 0xd3, 0x8d, 0x6f, 0x31, 0xb2, 0xec, 0x0e, 0x50,
+  0xaf, 0xf1, 0x13, 0x4d, 0xce, 0x90, 0x72, 0x2c, 0x6d, 0x33, 0xd1, 0x8f, 0x0c, 0x52, 0xb0, 0xee,
+  0x32, 0x6c, 0x8e, 0xd0, 0x53, 0x0d, 0xef, 0xb1, 0xf0, 0xae, 0x4c, 0x12, 0x91, 0xcf, 0x2d, 0x73,
+  0xca, 0x94, 0x76, 0x28, 0xab, 0xf5, 0x17, 0x49, 0x08, 0x56, 0xb4, 0xea, 0x69, 0x37, 0xd5, 0x8b,
+  0x57, 0x09, 0xeb, 0xb5, 0x36, 0x68, 0x8a, 0xd4, 0x95, 0xcb, 0x29, 0x77, 0xf4, 0xaa, 0x48, 0x16,
+  0xe9, 0xb7, 0x55, 0x0b, 0x88, 0xd6, 0x34, 0x6a, 0x2b, 0x75, 0x97, 0xc9, 0x4a, 0x14, 0xf6, 0xa8,
+  0x74, 0x2a, 0xc8, 0x96, 0x15, 0x4b, 0xa9, 0xf7, 0xb6, 0xe8, 0x0a, 0x54, 0xd7, 0x89, 0x6b, 0x35,
+};
+}  // namespace
+
+uint8_t GetCrc8(const uint8_t *data, size_t len, uint8_t init) {
+  if (!data) {
+    return init;
+  }
+  uint8_t crc = init;
+  while (len--) {
+    crc = kCrc8Table[crc ^ (*data++)];
+  }
+  return crc;
+}
+
+bool VerifyCrc8(const uint8_t *data, size_t len) {
+  if (!data || len < 2) {
+    return false;
+  }
+  return GetCrc8(data, len - 1, kCrc8Init) == data[len - 1];
+}
+
+void AppendCrc8(uint8_t *data, size_t len) {
+  if (!data || len < 2) {
+    return;
+  }
+  data[len - 1] = GetCrc8(data, len - 1, kCrc8Init);
+}
+
+}  // namespace fyt::serial_driver

src/rm_hardware_driver/rm_serial_driver/src/protocol/default_protocol.cpp

@@ -0,0 +1,107 @@
+// Created by Chengfu Zou
+// Copyright (C) FYT Vision Group. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rm_serial_driver/protocol/default_protocol.hpp"
+
+#include "rm_utils/logger/log.hpp"
+
+namespace fyt::serial_driver::protocol {
+
+DefaultProtocol::DefaultProtocol(std::string_view port_name,
+                                 bool enable_data_print,
+                                 bool enable_send_print) {
+  auto uart_transporter = std::make_shared<UartTransporter>(std::string(port_name));
+  rx_packet_tool_ = std::make_shared<FixedPacketTool<20>>(uart_transporter);
+  tx_packet_tool_ = std::make_shared<FixedPacketTool<16>>(uart_transporter);
+  enable_data_print_ = enable_data_print;
+  enable_send_print_ = enable_send_print;
+}
+
+std::vector<rclcpp::SubscriptionBase::SharedPtr> DefaultProtocol::getSubscriptions(
+  rclcpp::Node::SharedPtr node) {
+  return {node->create_subscription<rm_interfaces::msg::GimbalCmd>(
+    "armor_solver/cmd_gimbal",
+    rclcpp::SensorDataQoS(),
+    [this](const rm_interfaces::msg::GimbalCmd::SharedPtr msg) { this->send(*msg); })
+
+  };
+}
+
+std::vector<rclcpp::Client<rm_interfaces::srv::SetMode>::SharedPtr> DefaultProtocol::getClients(
+  rclcpp::Node::SharedPtr node) const {
+  return {node->create_client<rm_interfaces::srv::SetMode>("armor_detector/set_mode",
+                                                           rmw_qos_profile_services_default)};
+}
+
+void DefaultProtocol::send(const rm_interfaces::msg::GimbalCmd &data) {
+  if (enable_send_print_) {
+    FYT_INFO("serial_driver",
+             "tx gimbal fire_advice:{} pitch:{} yaw:{} distance:{} shoot_rate:{}",
+             data.fire_advice,
+             data.pitch,
+             data.yaw,
+             data.distance,
+             data.shoot_rate);
+  }
+  FixedPacket<16> packet;
+  packet.loadData<unsigned char>(data.fire_advice ? FireState::Fire : FireState::NotFire, 1);
+  packet.loadData<float>(static_cast<float>(data.pitch), 2);
+  packet.loadData<float>(static_cast<float>(data.yaw), 6);
+  packet.loadData<float>(static_cast<float>(data.distance), 10);
+  packet.loadData<uint8_t>(data.shoot_rate, 14);
+  tx_packet_tool_->sendPacket(packet);
+}
+
+bool DefaultProtocol::receive(rm_interfaces::msg::SerialReceiveData &data) {
+  FixedPacket<20> packet;
+  if (rx_packet_tool_->recvPacket(packet)) {
+    packet.unloadData(data.mode, 1);
+    packet.unloadData(data.roll, 2);
+    packet.unloadData(data.pitch, 6);
+    packet.unloadData(data.yaw, 10);
+    if (rx_packet_length_ >= 20) {
+      packet.unloadData(data.yaw_to_pitch_angle, 14);
+    } else {
+      data.yaw_to_pitch_angle = 0.0f;
+    }
+    int speed_index = (rx_packet_length_ > 0 ? rx_packet_length_ : 20) - 2;
+    uint8_t speed_raw = 0;
+    if (speed_index > 0 && speed_index < 20 && packet.unloadData(speed_raw, speed_index)) {
+      data.bullet_speed = static_cast<float>(speed_raw) / 10.0f;
+    } else {
+      data.bullet_speed = 0.0f;
+    }
+    if (enable_data_print_) {
+      FYT_INFO("serial_driver",
+               "rx mode:{} roll:{} pitch:{} yaw:{} yaw_to_pitch_angle:{} bullet_speed:{}",
+               data.mode,
+               data.roll,
+               data.pitch,
+               data.yaw,
+               data.yaw_to_pitch_angle,
+               data.bullet_speed);
+    }
+    return true;
+  } else {
+    return false;
+  }
+}
+
+void DefaultProtocol::setRxPacketLength(int required_len) {
+  rx_packet_length_ = required_len > 0 ? required_len : 20;
+  rx_packet_tool_->setRequiredLength(required_len);
+}
+
+}  // namespace fyt::serial_driver::protocol

src/rm_hardware_driver/rm_serial_driver/src/protocol/infantry_protocol.cpp

@@ -0,0 +1,114 @@
+// Created by Chengfu Zou
+// Copyright (C) FYT Vision Group. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rm_serial_driver/protocol/infantry_protocol.hpp"
+
+#include "rm_utils/logger/log.hpp"
+
+namespace fyt::serial_driver::protocol {
+ProtocolInfantry::ProtocolInfantry(std::string_view port_name,
+                                   bool enable_data_print,
+                                   bool enable_send_print) {
+  auto uart_transporter = std::make_shared<UartTransporter>(std::string(port_name));
+  rx_packet_tool_ = std::make_shared<FixedPacketTool<20>>(uart_transporter);
+  tx_packet_tool_ = std::make_shared<FixedPacketTool<16>>(uart_transporter);
+  enable_data_print_ = enable_data_print;
+  enable_send_print_ = enable_send_print;
+}
+
+void ProtocolInfantry::send(const rm_interfaces::msg::GimbalCmd &data) {
+  if (enable_send_print_) {
+    FYT_INFO("serial_driver",
+             "tx gimbal fire_advice:{} pitch:{} yaw:{} distance:{}",
+             data.fire_advice,
+             data.pitch,
+             data.yaw,
+             data.distance);
+  }
+  FixedPacket<16> packet;
+  packet.loadData<unsigned char>(data.fire_advice ? FireState::Fire : FireState::NotFire, 1);
+  packet.loadData<float>(static_cast<float>(data.pitch), 2);
+  packet.loadData<float>(static_cast<float>(data.yaw), 6);
+  packet.loadData<float>(static_cast<float>(data.distance), 10);
+  tx_packet_tool_->sendPacket(packet);
+}
+
+bool ProtocolInfantry::receive(rm_interfaces::msg::SerialReceiveData &data) {
+  FixedPacket<20> packet;
+  if (rx_packet_tool_->recvPacket(packet)) {
+    packet.unloadData(data.mode, 1);
+    packet.unloadData(data.roll, 2);
+    packet.unloadData(data.pitch, 6);
+    packet.unloadData(data.yaw, 10);
+    if (rx_packet_length_ >= 20) {
+      packet.unloadData(data.yaw_to_pitch_angle, 14);
+    } else {
+      data.yaw_to_pitch_angle = 0.0f;
+    }
+    int speed_index = (rx_packet_length_ > 0 ? rx_packet_length_ : 20) - 2;
+    uint8_t speed_raw = 0;
+    if (speed_index > 0 && speed_index < 20 && packet.unloadData(speed_raw, speed_index)) {
+      data.bullet_speed = static_cast<float>(speed_raw) / 10.0f;
+    } else {
+      data.bullet_speed = 0.0f;
+    }
+    if (enable_data_print_) {
+      FYT_INFO("serial_driver",
+               "rx mode:{} roll:{} pitch:{} yaw:{} yaw_to_pitch_angle:{} bullet_speed:{}",
+               data.mode,
+               data.roll,
+               data.pitch,
+               data.yaw,
+               data.yaw_to_pitch_angle,
+               data.bullet_speed);
+    }
+    return true;
+  } else {
+    return false;
+  }
+}
+
+void ProtocolInfantry::setRxPacketLength(int required_len) {
+  rx_packet_length_ = required_len > 0 ? required_len : 20;
+  rx_packet_tool_->setRequiredLength(required_len);
+}
+
+std::vector<rclcpp::SubscriptionBase::SharedPtr> ProtocolInfantry::getSubscriptions(
+  rclcpp::Node::SharedPtr node) {
+  auto sub1 = node->create_subscription<rm_interfaces::msg::GimbalCmd>(
+    "armor_solver/cmd_gimbal",
+    rclcpp::SensorDataQoS(),
+    [this](const rm_interfaces::msg::GimbalCmd::SharedPtr msg) { this->send(*msg); });
+  auto sub2 = node->create_subscription<rm_interfaces::msg::GimbalCmd>(
+    "rune_solver/cmd_gimbal",
+    rclcpp::SensorDataQoS(),
+    [this](const rm_interfaces::msg::GimbalCmd::SharedPtr msg) { this->send(*msg); });
+  return {sub1, sub2};
+}
+
+std::vector<rclcpp::Client<rm_interfaces::srv::SetMode>::SharedPtr> ProtocolInfantry::getClients(
+  rclcpp::Node::SharedPtr node) const {
+  auto client1 = node->create_client<rm_interfaces::srv::SetMode>("armor_detector/set_mode",
+                                                                  rmw_qos_profile_services_default);
+  auto client2 = node->create_client<rm_interfaces::srv::SetMode>("armor_solver/set_mode",
+                                                                  rmw_qos_profile_services_default);
+  auto client3 = node->create_client<rm_interfaces::srv::SetMode>("rune_detector/set_mode",
+                                                                  rmw_qos_profile_services_default);
+  auto client4 = node->create_client<rm_interfaces::srv::SetMode>("rune_solver/set_mode",
+                                                                  rmw_qos_profile_services_default);
+  return {client1, client2, client3, client4};
+}
+
+}  // namespace fyt::serial_driver::protocol

src/rm_hardware_driver/rm_serial_driver/src/protocol/sentry_protocol.cpp

@@ -0,0 +1,177 @@
+// Created by Chengfu Zou
+// Copyright (C) FYT Vision Group. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rm_serial_driver/protocol/sentry_protocol.hpp"
+// ros2
+#include <geometry_msgs/msg/twist.hpp>
+
+#include "rm_utils/logger/log.hpp"
+
+namespace fyt::serial_driver::protocol {
+ProtocolSentry::ProtocolSentry(std::string_view port_name,
+                               bool enable_data_print,
+                               bool enable_send_print) {
+  auto uart_transporter = std::make_shared<UartTransporter>(std::string(port_name));
+  packet_tool_ = std::make_shared<FixedPacketTool<32>>(uart_transporter);
+  enable_data_print_ = enable_data_print;
+  enable_send_print_ = enable_send_print;
+}
+
+void ProtocolSentry::send(const rm_interfaces::msg::GimbalCmd &data) {
+  if (enable_send_print_) {
+    FYT_INFO("serial_driver",
+             "tx gimbal fire_advice:{} pitch:{} yaw:{} distance:{}",
+             data.fire_advice,
+             data.pitch,
+             data.yaw,
+             data.distance);
+  }
+  packet_.loadData<unsigned char>(data.fire_advice ? FireState::Fire : FireState::NotFire, 1);
+  // is_spin
+  // packet_.loadData<unsigned char>(0x00, 2);
+  // gimbal control
+  packet_.loadData<float>(static_cast<float>(data.pitch), 4);
+  packet_.loadData<float>(static_cast<float>(data.yaw), 8);
+  packet_.loadData<float>(static_cast<float>(data.distance), 12);
+  // // chassis control
+  // // linear x
+  // packet_.loadData<float>(0, 16);
+  // // linear y
+  // packet_.loadData<float>(0, 20);
+  // // angular z
+  // packet_.loadData<float>(0, 24);
+  // // useless data
+  // packet_.loadData<float>(0, 28);
+  packet_tool_->sendPacket(packet_);
+}
+
+void ProtocolSentry::send(const rm_interfaces::msg::ChassisCmd &data) {
+  if (enable_send_print_) {
+    FYT_INFO("serial_driver",
+             "tx chassis spin:{} navigating:{} linear_x:{} linear_y:{} angular_z:{}",
+             data.is_spining,
+             data.is_navigating,
+             data.twist.linear.x,
+             data.twist.linear.y,
+             data.twist.angular.z);
+  }
+  // packet_.loadData<unsigned char>(0x00, 1);
+  // is_spin
+  packet_.loadData<unsigned char>(data.is_spining ? 0x01 : 0x00, 2);
+  packet_.loadData<unsigned char>(data.is_navigating ? 0x01 : 0x00, 3);
+  // gimbal control
+  // packet_.loadData<float>(0, 4);
+  // packet_.loadData<float>(0, 8);
+  // packet_.loadData<float>(0, 12);
+  // chassis control
+  // linear x
+  packet_.loadData<float>(data.twist.linear.x, 16);
+  // linear y
+  packet_.loadData<float>(data.twist.linear.y, 20);
+  // angular z
+  packet_.loadData<float>(data.twist.angular.z, 24);
+  // useless data
+  // packet_.loadData<float>(0, 28);
+  packet_tool_->sendPacket(packet_);
+}
+
+bool ProtocolSentry::receive(rm_interfaces::msg::SerialReceiveData &data) {
+  FixedPacket<32> packet;
+  if (packet_tool_->recvPacket(packet)) {
+     // game status
+    uint8_t enemy_color;
+    packet.unloadData(enemy_color, 1);
+    data.mode = (enemy_color == ENEMY_BLUE ? 1 : 0);
+
+    packet.unloadData(data.pitch, 2);
+    packet.unloadData(data.yaw, 6);
+    packet.unloadData(data.yaw_to_pitch_angle, 10);
+    // 实际上是底盘角度
+    // packet.unloadData(data.chassis_yaw, 10);
+    // blood
+    packet.unloadData(data.judge_system_data.blood, 14);
+    // remaining time
+    packet.unloadData(data.judge_system_data.remaining_time, 16);
+    // outpost hp
+    packet.unloadData(data.judge_system_data.outpost_hp, 20);
+    // operator control message
+    packet.unloadData(data.judge_system_data.operator_command.is_outpost_attacking, 22);
+    packet.unloadData(data.judge_system_data.operator_command.is_retreating, 23);
+    packet.unloadData(data.judge_system_data.operator_command.is_drone_avoiding, 24);
+
+    packet.unloadData(data.judge_system_data.game_status, 25);
+
+    int speed_index = (rx_packet_length_ > 0 ? rx_packet_length_ : 32) - 2;
+    uint8_t speed_raw = 0;
+    if (speed_index > 0 && speed_index < 32 && packet.unloadData(speed_raw, speed_index)) {
+      data.bullet_speed = static_cast<float>(speed_raw) / 10.0f;
+    } else {
+      data.bullet_speed = 0.0f;
+    }
+    if (enable_data_print_) {
+      FYT_INFO("serial_driver",
+               "rx mode:{} pitch:{} yaw:{} yaw_to_pitch_angle:{} blood:{} remaining:{} outpost_hp:{} "
+               "operator(outpost_attacking:{} retreating:{} drone_avoiding:{}) game_status:{} "
+               "bullet_speed:{}",
+               data.mode,
+               data.pitch,
+               data.yaw,
+               data.yaw_to_pitch_angle,
+               data.judge_system_data.blood,
+               data.judge_system_data.remaining_time,
+               data.judge_system_data.outpost_hp,
+               data.judge_system_data.operator_command.is_outpost_attacking,
+               data.judge_system_data.operator_command.is_retreating,
+               data.judge_system_data.operator_command.is_drone_avoiding,
+               data.judge_system_data.game_status,
+               data.bullet_speed);
+    }
+    return true;
+  } else {
+    return false;
+  }
+}
+
+void ProtocolSentry::setRxPacketLength(int required_len) {
+  rx_packet_length_ = required_len > 0 ? required_len : 32;
+  packet_tool_->setRequiredLength(required_len);
+}
+
+std::vector<rclcpp::SubscriptionBase::SharedPtr> ProtocolSentry::getSubscriptions(
+  rclcpp::Node::SharedPtr node) {
+  auto sub1 = node->create_subscription<rm_interfaces::msg::GimbalCmd>(
+    "armor_solver/cmd_gimbal",
+    rclcpp::SensorDataQoS(),
+    [this](const rm_interfaces::msg::GimbalCmd::SharedPtr msg) { this->send(*msg); });
+  auto sub2 = node->create_subscription<rm_interfaces::msg::GimbalCmd>(
+    "rune_solver/cmd_gimbal",
+    rclcpp::SensorDataQoS(),
+    [this](const rm_interfaces::msg::GimbalCmd::SharedPtr msg) { this->send(*msg); });
+  auto sub3 = node->create_subscription<rm_interfaces::msg::ChassisCmd>(
+    "/cmd_chassis",
+    rclcpp::SensorDataQoS(),
+    [this](const rm_interfaces::msg::ChassisCmd::SharedPtr msg) { this->send(*msg); });
+  return {sub1, sub2, sub3};
+}
+
+std::vector<rclcpp::Client<rm_interfaces::srv::SetMode>::SharedPtr> ProtocolSentry::getClients(
+  rclcpp::Node::SharedPtr node) const {
+  auto client1 = node->create_client<rm_interfaces::srv::SetMode>("armor_detector/set_mode",
+                                                                  rmw_qos_profile_services_default);
+  auto client2 = node->create_client<rm_interfaces::srv::SetMode>("armor_solver/set_mode",
+                                                                  rmw_qos_profile_services_default);
+  return {client1, client2};
+}
+}  // namespace fyt::serial_driver::protocol

src/rm_hardware_driver/rm_serial_driver/src/serial_driver_node.cpp

@@ -0,0 +1,213 @@
+// Created by Chengfu Zou on 2023.7.1
+// Copyright (C) FYT Vision Group. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rm_serial_driver/serial_driver_node.hpp"
+
+#include <tf2/LinearMath/Matrix3x3.h>
+// std
+#include <chrono>
+#include <cmath>
+#include <cstdint>
+#include <memory>
+#include <thread>
+// ros2
+#include <Eigen/Geometry>
+#include <rclcpp/rclcpp.hpp>
+// project
+#include "rm_serial_driver/uart_transporter.hpp"
+#include "rm_utils/logger/log.hpp"
+#include "rm_utils/math/utils.hpp"
+
+namespace fyt::serial_driver {
+SerialDriverNode::SerialDriverNode(const rclcpp::NodeOptions &options)
+: Node("serial_driver", options) {
+  FYT_REGISTER_LOGGER("serial_driver", "~/fyt2024-log", INFO);
+
+  // Task thread
+  listen_thread_ = std::make_unique<std::thread>(&SerialDriverNode::listenLoop, this);
+}
+
+void SerialDriverNode::init() {
+  FYT_INFO("serial_driver", "Initializing SerialDriverNode!");
+  // Init
+  target_frame_ = this->declare_parameter("target_frame", "odom");
+  std::string port_name = this->declare_parameter("port_name", "/dev/ttyUSB0");
+  std::string protocol_type = this->declare_parameter("protocol", "infantry");
+  bool enable_data_print = this->declare_parameter("enable_data_print", false);
+  bool enable_send_print = this->declare_parameter("enable_send_print", false);
+  // Create Protocol
+  protocol_ =
+    ProtocolFactory::createProtocol(protocol_type, port_name, enable_data_print, enable_send_print);
+  if (protocol_ == nullptr) {
+    FYT_FATAL("serial_driver", "Failed to create protocol with type: {}", protocol_type);
+    rclcpp::shutdown();
+    return;
+  }
+  FYT_INFO(
+    "serial_driver", "Protocol has been created with type: {}, port: {}", protocol_type, port_name);
+
+  // Subscriptions
+  subscriptions_ = protocol_->getSubscriptions(this->shared_from_this());
+  for (auto sub : subscriptions_) {
+    FYT_INFO("serial_driver", "Subscribe to topic: {}", sub->get_topic_name());
+  }
+  // Publisher
+  serial_receive_data_pub_ = this->create_publisher<rm_interfaces::msg::SerialReceiveData>(
+    "serial/receive", rclcpp::SensorDataQoS());
+
+  // TF broadcaster
+  timestamp_offset_ = this->declare_parameter("timestamp_offset", 0.0);
+  tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
+  
+  // yaw轴到pitch轴的偏移量 [x, y, z] (单位: 米)
+  // x: 前后方向 (正值表示pitch轴在yaw轴前方)
+  // y: 左右方向 (正值表示pitch轴在yaw轴左方)
+  // z: 上下方向 (正值表示pitch轴在yaw轴上方)
+  yaw_to_pitch_offset_ =
+    this->declare_parameter<std::vector<double>>("yaw_to_pitch_offset", {0.0, 0.0, 0.0});
+  yaw_to_pitch_radius_ = this->declare_parameter(
+    "yaw_to_pitch_radius", std::hypot(yaw_to_pitch_offset_[0], yaw_to_pitch_offset_[2]));
+  use_yaw_to_pitch_angle_ = this->declare_parameter("use_yaw_to_pitch_angle", true);
+  protocol_->setRxPacketLength(use_yaw_to_pitch_angle_ ? 20 : 16);
+
+  // Param client
+  for (auto client : protocol_->getClients(this->shared_from_this())) {
+    std::string name = client->get_service_name();
+    set_mode_clients_.emplace(name, client);
+    FYT_INFO("serial_driver", "Create client for service: {}", name);
+  }
+
+  // Heartbeat
+  heartbeat_ = HeartBeatPublisher::create(this);
+
+  FYT_INFO("serial_driver", "SerialDriverNode has been initialized!");
+}
+
+SerialDriverNode::~SerialDriverNode() {
+  FYT_INFO("serial_driver", "Destroy SerialDriverNode!");
+  rclcpp::shutdown();
+  if (listen_thread_ != nullptr) {
+    listen_thread_->join();
+  }
+}
+
+void SerialDriverNode::listenLoop() {
+  if (protocol_ == nullptr) {
+    // Lazy init because shared_from_this() is not available in constructor
+    init();
+  }
+
+  rm_interfaces::msg::SerialReceiveData receive_data;
+  while (rclcpp::ok()) {
+    if (protocol_->receive(receive_data)) {
+      receive_data.header.stamp = this->now() + rclcpp::Duration::from_seconds(timestamp_offset_);
+      receive_data.header.frame_id = target_frame_;
+      serial_receive_data_pub_->publish(receive_data);
+
+      for (auto &[service_name, client] : set_mode_clients_) {
+        if (client.mode.load() != receive_data.mode && !client.on_waiting.load()) {
+          setMode(client, receive_data.mode);
+        }
+      }
+
+      geometry_msgs::msg::TransformStamped t;
+      timestamp_offset_ = this->get_parameter("timestamp_offset").as_double();
+      auto stamp = this->now() + rclcpp::Duration::from_seconds(timestamp_offset_);
+      
+      auto pitch = -receive_data.pitch * M_PI / 180.0;
+      auto yaw = receive_data.yaw * M_PI / 180.0;
+      
+      // 发布 odom -> yaw_link 变换 (只有yaw旋转)
+      t.header.stamp = stamp;
+      t.header.frame_id = target_frame_;
+      t.child_frame_id = "yaw_link";
+      tf2::Quaternion q_yaw;
+      q_yaw.setRPY(0, 0, yaw);
+      t.transform.rotation = tf2::toMsg(q_yaw);
+      t.transform.translation.x = 0;
+      t.transform.translation.y = 0;
+      t.transform.translation.z = 0;
+      tf_broadcaster_->sendTransform(t);
+      
+      // 发布 yaw_link -> gimbal_link 变换 (只有pitch旋转 + yaw-pitch轴偏移)
+      // yaw_to_pitch_offset: yaw轴到pitch轴的偏移量 [x, y, z]
+      // 需要在参数文件中配置: yaw_to_pitch_offset: [0.0, 0.0, 0.05]
+      t.header.stamp = stamp;
+      t.header.frame_id = "yaw_link";
+      t.child_frame_id = "pitch_link";
+      tf2::Quaternion q_pitch;
+      q_pitch.setRPY(0, pitch, 0);  // 只有pitch
+      t.transform.rotation = tf2::toMsg(q_pitch);
+      // 从参数获取yaw到pitch轴的偏移
+      if (use_yaw_to_pitch_angle_ && yaw_to_pitch_radius_ > 0.0 &&
+          std::isfinite(receive_data.yaw_to_pitch_angle)) {
+        double yaw_to_pitch_angle = receive_data.yaw_to_pitch_angle * M_PI / 180.0;
+        t.transform.translation.x = -yaw_to_pitch_radius_ * std::sin(yaw_to_pitch_angle);
+        t.transform.translation.y = yaw_to_pitch_offset_[1];
+        t.transform.translation.z = yaw_to_pitch_radius_ * std::cos(yaw_to_pitch_angle);
+      } else {
+        // Fallback to fixed offset when angle data is unavailable.
+        t.transform.translation.x = yaw_to_pitch_offset_[0];
+        t.transform.translation.y = yaw_to_pitch_offset_[1];
+        t.transform.translation.z = yaw_to_pitch_offset_[2];
+      }
+      tf_broadcaster_->sendTransform(t);
+    } else {
+      auto error_message = protocol_->getErrorMessage();
+      error_message = error_message.empty() ? "unknown" : error_message;
+      FYT_WARN("serial_driver", "Failed to reveive packet! error message :{}", error_message);
+      std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    }
+  }
+}
+
+void SerialDriverNode::setMode(SetModeClient &client, const uint8_t mode) {
+  using namespace std::chrono_literals;
+
+  std::string service_name = client.ptr->get_service_name();
+  // Wait for service
+  while (!client.ptr->wait_for_service(1s)) {
+    if (!rclcpp::ok()) {
+      FYT_ERROR(
+        "serial_driver", "Interrupted while waiting for the service {}. Exiting.", service_name);
+      return;
+    }
+    FYT_INFO("serial_driver", "Service {} not available, waiting again...", service_name);
+  }
+  if (!client.ptr->service_is_ready()) {
+    FYT_WARN("serial_driver", "Service: {} is not available!", service_name);
+    return;
+  }
+  // Send request
+  auto req = std::make_shared<rm_interfaces::srv::SetMode::Request>();
+  req->mode = mode;
+
+  client.on_waiting.store(true);
+  auto result = client.ptr->async_send_request(
+    req, [mode, &client](rclcpp::Client<rm_interfaces::srv::SetMode>::SharedFuture result) {
+      client.on_waiting.store(false);
+      if (result.get()->success) {
+        client.mode.store(mode);
+      }
+    });
+}
+
+}  // namespace fyt::serial_driver
+
+#include "rclcpp_components/register_node_macro.hpp"
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(fyt::serial_driver::SerialDriverNode)

src/rm_hardware_driver/rm_serial_driver/src/transporter_driver/uart_transporter.cpp

@@ -0,0 +1,196 @@
+// Copyright (C) 2021 RoboMaster-OSS
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Additional modifications and features by Chengfu Zou, 2023.
+//
+// Copyright (C) FYT Vision Group. All rights reserved.
+
+#include "rm_serial_driver/uart_transporter.hpp"
+// System
+#include <errno.h>  /*错误号定义*/
+#include <fcntl.h>  /*文件控制定义*/
+#include <stdio.h>  /*标准输入输出定义*/
+#include <stdlib.h> /*标准函数库定义*/
+#include <string.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <termios.h> /*PPSIX 终端控制定义*/
+#include <unistd.h>  /*Unix 标准函数定义*/
+
+namespace fyt::serial_driver {
+
+bool UartTransporter::setParam(int speed, int flow_ctrl, int databits, int stopbits, int parity) {
+  // 设置串口数据帧格式
+  int speed_arr[] = {B115200, B19200, B9600, B4800, B2400, B1200, B300};
+  int name_arr[] = {115200, 19200, 9600, 4800, 2400, 1200, 300};
+  struct termios options;
+  // tcgetattr(fd,&options)得到与fd指向对象的相关参数，并将它们保存于options,该函数还可以测试配置是否正确，
+  // 该串口是否可用等。若调用成功，函数返回值为0，若调用失败，函数返回值为1.
+  if (tcgetattr(fd_, &options) != 0) {
+    error_message_ = "Setup Serial err";
+    return false;
+  }
+  // 设置串口输入波特率和输出波特率
+  for (size_t i = 0; i < sizeof(speed_arr) / sizeof(int); i++) {
+    if (speed == name_arr[i]) {
+      cfsetispeed(&options, speed_arr[i]);
+      cfsetospeed(&options, speed_arr[i]);
+    }
+  }
+  // 修改控制模式，保证程序不会占用串口
+  options.c_cflag |= CLOCAL;
+  // 修改控制模式，使得能够从串口中读取输入数据
+  options.c_cflag |= CREAD;
+  // 设置数据流控制
+  switch (flow_ctrl) {
+    case 0:  // 不使用流控制
+      options.c_cflag &= ~CRTSCTS;
+      break;
+    case 1:  // 使用硬件流控制
+      options.c_cflag |= CRTSCTS;
+      break;
+    case 2:  // 使用软件流控制
+      options.c_cflag |= IXON | IXOFF | IXANY;
+      break;
+  }
+  // 设置数据位
+  // 屏蔽其他标志位
+  options.c_cflag &= ~CSIZE;
+  switch (databits) {
+    case 5:
+      options.c_cflag |= CS5;
+      break;
+    case 6:
+      options.c_cflag |= CS6;
+      break;
+    case 7:
+      options.c_cflag |= CS7;
+      break;
+    case 8:
+      options.c_cflag |= CS8;
+      break;
+    default:
+      error_message_ = "Unsupported data size";
+      return false;
+  }
+  // 设置校验位
+  switch (parity) {
+    case 'n':
+    case 'N':  // 无奇偶校验位。
+      options.c_cflag &= ~PARENB;
+      options.c_iflag &= ~INPCK;
+      break;
+    case 'o':
+    case 'O':  // 设置为奇校验
+      options.c_cflag |= (PARODD | PARENB);
+      options.c_iflag |= INPCK;
+      break;
+    case 'e':
+    case 'E':  // 设置为偶校验
+      options.c_cflag |= PARENB;
+      options.c_cflag &= ~PARODD;
+      options.c_iflag |= INPCK;
+      break;
+    case 's':
+    case 'S':  // 设置为空格
+      options.c_cflag &= ~PARENB;
+      options.c_cflag &= ~CSTOPB;
+      break;
+    default:
+      error_message_ = "Unsupported parity";
+      return false;
+  }
+  // 设置停止位
+  switch (stopbits) {
+    case 1:
+      options.c_cflag &= ~CSTOPB;
+      break;
+    case 2:
+      options.c_cflag |= CSTOPB;
+      break;
+    default:
+      error_message_ = "Unsupported stop bits";
+      return false;
+  }
+
+  // 修改输出模式，原始数据输出
+  options.c_oflag &= ~OPOST;
+  options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
+  // 传输特殊字符，否则特殊字符0x0d,0x11,0x13会被屏蔽或映射。
+  options.c_iflag &= ~(BRKINT | ICRNL | INPCK | ISTRIP | IXON);
+
+  // 设置等待时间和最小接收字符
+  options.c_cc[VTIME] = 1;  // 读取一个字符等待1*(1/10)s
+  options.c_cc[VMIN] = 1;   // 读取字符的最少个数为1
+  tcflush(fd_, TCIFLUSH);
+
+  // 激活配置 (将修改后的termios数据设置到串口中）
+  if (tcsetattr(fd_, TCSANOW, &options) != 0) {
+    error_message_ = "com set error";
+    return false;
+  }
+  return true;
+}
+
+bool UartTransporter::open() {
+  if (is_open_) {
+    return true;
+  }
+  fd_ = ::open(device_path_.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
+  if (-1 == fd_) {
+    error_message_ = "can't open uart device: " + device_path_;
+    return false;
+  }
+  // 恢复串口为阻塞状态
+  if (fcntl(fd_, F_SETFL, 0) < 0) {
+    error_message_ = "fcntl failed";
+    return false;
+  }
+  // 测试是否为终端设备
+  // 避免自启动无法读取数据
+  // if (0 == isatty(STDIN_FILENO)) {
+  //   error_message_ = "standard input is not a terminal device";
+  //   return false;
+  // }
+  // 设置串口数据帧格式
+  if (!setParam(speed_, flow_ctrl_, databits_, stopbits_, parity_)) {
+    return false;
+  }
+  is_open_ = true;
+  return true;
+}
+
+void UartTransporter::close() {
+  if (!is_open_) {
+    return;
+  }
+  ::close(fd_);
+  fd_ = -1;
+  is_open_ = false;
+}
+
+bool UartTransporter::isOpen() { return is_open_; }
+
+int UartTransporter::read(void *buffer, size_t len) {
+  int ret = ::read(fd_, buffer, len);
+  // tcflush(fd_, TCIFLUSH);
+  return ret;
+}
+
+int UartTransporter::write(const void *buffer, size_t len) {
+  int ret = ::write(fd_, buffer, len);
+  return ret;
+}
+
+}  // namespace fyt::serial_driver

src/rm_hardware_driver/rm_serial_driver/src/virtual_serial_node.cpp

@@ -0,0 +1,210 @@
+// Created by Chengfu Zou
+// Copyright (C) FYT Vision Group. All rights reserved.
+
+// std
+#include <chrono>
+#include <cmath>
+#include <condition_variable>
+#include <future>
+#include <memory>
+#include <opencv2/calib3d.hpp>
+#include <rclcpp/executors.hpp>
+#include <thread>
+// ros2
+#include <tf2_ros/transform_broadcaster.h>
+
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <rclcpp/logging.hpp>
+#include <rclcpp/node_options.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+// project
+#include "rm_interfaces/msg/serial_receive_data.hpp"
+#include "rm_interfaces/srv/set_mode.hpp"
+#include "rm_utils/heartbeat.hpp"
+#include "rm_utils/logger/log.hpp"
+#include "rm_utils/math/utils.hpp"
+
+namespace fyt::serial_driver {
+class VirtualSerialNode : public rclcpp::Node {
+  struct SetModeClient {
+    SetModeClient(rclcpp::Client<rm_interfaces::srv::SetMode>::SharedPtr p) : ptr(p) {}
+    std::atomic<bool> on_waiting = false;
+    std::atomic<int> mode = 0;
+    rclcpp::Client<rm_interfaces::srv::SetMode>::SharedPtr ptr;
+  };
+
+public:
+  explicit VirtualSerialNode(const rclcpp::NodeOptions &options) : Node("serial_driver", options) {
+    FYT_REGISTER_LOGGER("serial_driver", "~/fyt2024-log", INFO);
+    FYT_INFO("serial_driver", "Starting VirtualSerialNode!");
+
+    // 设置关闭标志
+    is_running_.store(true);
+
+    serial_receive_data_pub_ =
+      this->create_publisher<rm_interfaces::msg::SerialReceiveData>("serial/receive", 10);
+
+    tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
+
+    this->declare_parameter("vision_mode", static_cast<int>(0));
+
+    has_rune_ = this->declare_parameter("has_rune", true);
+
+    serial_receive_data_msg_.header.frame_id = "odom";
+    serial_receive_data_msg_.bullet_speed = this->declare_parameter("bullet_speed", 25.0);
+    transform_stamped_.header.frame_id = "odom";
+    transform_stamped_.child_frame_id = "yaw_link";
+    serial_receive_data_msg_.mode = 0;
+    serial_receive_data_msg_.roll = this->declare_parameter("roll", 0.0);
+    serial_receive_data_msg_.pitch = this->declare_parameter("pitch", 0.0);
+    serial_receive_data_msg_.yaw = this->declare_parameter("yaw", 0.0);
+    serial_receive_data_msg_.yaw_to_pitch_angle =
+      this->declare_parameter("yaw_to_pitch_angle", 0.0);
+
+    // yaw轴到pitch轴的偏移量 [x, y, z] (单位: 米)
+    yaw_to_pitch_offset_ =
+      this->declare_parameter<std::vector<double>>("yaw_to_pitch_offset", {0.0, 0.0, 0.0});
+    yaw_to_pitch_radius_ = this->declare_parameter(
+      "yaw_to_pitch_radius", std::hypot(yaw_to_pitch_offset_[0], yaw_to_pitch_offset_[2]));
+    use_yaw_to_pitch_angle_ = this->declare_parameter("use_yaw_to_pitch_angle", true);
+
+    // Heartbeat
+    heartbeat_ = HeartBeatPublisher::create(this);
+
+    // Param client
+    auto autoaim_set_mode_client_1 =
+      this->create_client<rm_interfaces::srv::SetMode>("armor_detector/set_mode");
+    set_mode_clients_.emplace(autoaim_set_mode_client_1->get_service_name(),
+                              autoaim_set_mode_client_1);
+    auto autoaim_set_mode_client_2 =
+      this->create_client<rm_interfaces::srv::SetMode>("armor_solver/set_mode");
+    set_mode_clients_.emplace(autoaim_set_mode_client_2->get_service_name(),
+                              autoaim_set_mode_client_2);
+    if (has_rune_) {
+      auto client1 = this->create_client<rm_interfaces::srv::SetMode>("rune_detector/set_mode");
+      set_mode_clients_.emplace(client1->get_service_name(), client1);
+      auto client2 = this->create_client<rm_interfaces::srv::SetMode>("rune_solver/set_mode");
+      set_mode_clients_.emplace(client2->get_service_name(), client2);
+    }
+
+    timer_ = this->create_wall_timer(std::chrono::milliseconds(1), [this]() {
+      serial_receive_data_msg_.header.stamp = this->now();
+      int mode = this->get_parameter("vision_mode").as_int();
+      [[maybe_unused]] double roll = this->get_parameter("roll").as_double();
+      double pitch = this->get_parameter("pitch").as_double();
+      double yaw = this->get_parameter("yaw").as_double();
+      double yaw_to_pitch_angle = this->get_parameter("yaw_to_pitch_angle").as_double();
+      double bullet_speed = this->get_parameter("bullet_speed").as_double();
+      serial_receive_data_msg_.mode = mode;
+      serial_receive_data_msg_.pitch = pitch;
+      serial_receive_data_msg_.yaw = yaw;
+      serial_receive_data_msg_.yaw_to_pitch_angle = yaw_to_pitch_angle;
+      serial_receive_data_msg_.bullet_speed = bullet_speed;
+
+      auto stamp = this->now();
+
+      // 发布 odom -> yaw_link 变换 (只有yaw旋转)
+      tf2::Quaternion q_yaw;
+      q_yaw.setRPY(0, 0, yaw * M_PI / 180.0);
+      transform_stamped_.transform.rotation = tf2::toMsg(q_yaw);
+      transform_stamped_.transform.translation.x = 0;
+      transform_stamped_.transform.translation.y = 0;
+      transform_stamped_.transform.translation.z = 0;
+      transform_stamped_.header.frame_id = "odom";
+      transform_stamped_.child_frame_id = "yaw_link";
+      transform_stamped_.header.stamp = stamp;
+      tf_broadcaster_->sendTransform(transform_stamped_);
+
+      // 发布 yaw_link -> gimbal_link 变换 (只有pitch旋转 + yaw-pitch轴偏移)
+      tf2::Quaternion q_pitch;
+      q_pitch.setRPY(0, -pitch * M_PI / 180.0, 0);  // 只有pitch
+      transform_stamped_.transform.rotation = tf2::toMsg(q_pitch);
+      if (use_yaw_to_pitch_angle_ && yaw_to_pitch_radius_ > 0.0) {
+        double yaw_to_pitch_angle_rad = yaw_to_pitch_angle * M_PI / 180.0;
+        transform_stamped_.transform.translation.x =
+          -yaw_to_pitch_radius_ * std::sin(yaw_to_pitch_angle_rad);
+        transform_stamped_.transform.translation.y = yaw_to_pitch_offset_[1];
+        transform_stamped_.transform.translation.z =
+          yaw_to_pitch_radius_ * std::cos(yaw_to_pitch_angle_rad);
+      } else {
+        transform_stamped_.transform.translation.x = yaw_to_pitch_offset_[0];
+        transform_stamped_.transform.translation.y = yaw_to_pitch_offset_[1];
+        transform_stamped_.transform.translation.z = yaw_to_pitch_offset_[2];
+      }
+      transform_stamped_.header.frame_id = "yaw_link";
+      transform_stamped_.child_frame_id = "pitch_link";
+      transform_stamped_.header.stamp = stamp;
+      tf_broadcaster_->sendTransform(transform_stamped_);
+
+      // serial_receive_data_msg.mode = mode;
+      serial_receive_data_pub_->publish(serial_receive_data_msg_);
+
+      for (auto &[service_name, client] : set_mode_clients_) {
+        if (client.mode.load() != mode && !client.on_waiting.load()) {
+          setMode(client, mode);
+        }
+      }
+    });
+  }
+
+  ~VirtualSerialNode() {
+    // 设置关闭标志，让所有等待循环退出
+    is_running_.store(false);
+    // 取消定时器
+    if (timer_) {
+      timer_->cancel();
+    }
+    FYT_INFO("serial_driver", "VirtualSerialNode destroyed.");
+  }
+
+  void setMode(SetModeClient &client, const uint8_t mode) {
+    std::string service_name = client.ptr->get_service_name();
+
+    // Check if service is ready without blocking
+    if (!client.ptr->service_is_ready()) {
+      return;
+    }
+
+    // Send request
+    auto req = std::make_shared<rm_interfaces::srv::SetMode::Request>();
+    req->mode = mode;
+    client.on_waiting.store(true);
+    auto result = client.ptr->async_send_request(
+      req, [mode, &client](rclcpp::Client<rm_interfaces::srv::SetMode>::SharedFuture result) {
+        client.on_waiting.store(false);
+        if (result.get()->success) {
+          client.mode.store(mode);
+        }
+      });
+  }
+
+private:
+  HeartBeatPublisher::SharedPtr heartbeat_;
+  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+  rclcpp::Publisher<rm_interfaces::msg::SerialReceiveData>::SharedPtr serial_receive_data_pub_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  rm_interfaces::msg::SerialReceiveData serial_receive_data_msg_;
+  geometry_msgs::msg::TransformStamped transform_stamped_;
+
+  bool has_rune_;
+
+  // 运行状态标志，用于优雅关闭
+  std::atomic<bool> is_running_{true};
+
+  // yaw轴到pitch轴的偏移量 [x, y, z] (单位: 米)
+  std::vector<double> yaw_to_pitch_offset_ = {0.0, 0.0, 0.0};
+  double yaw_to_pitch_radius_ = 0.0;
+  bool use_yaw_to_pitch_angle_ = true;
+
+  std::unordered_map<std::string, SetModeClient> set_mode_clients_;
+};
+}  // namespace fyt::serial_driver
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(fyt::serial_driver::VirtualSerialNode)
+

src/rm_hardware_driver/rm_serial_driver/test/dummy_transporter.hpp

@@ -0,0 +1,94 @@
+// Copyright 2021 RoboMaster-OSS
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef DUMMY_TRANSPORTER_HPP_
+#define DUMMY_TRANSPORTER_HPP_
+
+#include <unistd.h>
+
+#include <memory>
+#include <rm_utils/assert.hpp>
+#include <string>
+
+#include "rm_serial_driver/transporter_interface.hpp"
+
+using namespace fyt::serial_driver;
+// FIFO传输设备，用于测试。
+class FifoTransporter : public TransporterInterface
+{
+public:
+  FifoTransporter(int fifo_rd_fd, int fifo_wr_fd)
+  : fifo_rd_fd_(fifo_rd_fd), fifo_wr_fd_(fifo_wr_fd)
+  {}
+
+  bool open() override
+  {
+    return true;
+  }
+  void close() override
+  {
+  }
+  bool isOpen() override
+  {
+    return true;
+  }
+  int read(void * buffer, size_t len) override
+  {
+    return ::read(fifo_rd_fd_, buffer, len);
+  }
+  int write(const void * buffer, size_t len) override
+  {
+    return ::write(fifo_wr_fd_, buffer, len);
+  }
+  std::string errorMessage() override
+  {
+    return error_message_;
+  }
+
+private:
+  int fifo_rd_fd_;
+  int fifo_wr_fd_;
+  std::string error_message_;
+};
+
+class TransporterFactory
+{
+public:
+  TransporterFactory()
+  {
+    int ret = pipe(fds1);
+    FYT_ASSERT(ret == 0);
+    ret = pipe(fds2);
+    FYT_ASSERT(ret == 0);
+
+    transporter1_ = std::make_shared<FifoTransporter>(fds1[0], fds2[1]);
+    transporter2_ = std::make_shared<FifoTransporter>(fds2[0], fds1[1]);
+  }
+  TransporterInterface::SharedPtr get_transporter1()
+  {
+    return transporter1_;
+  }
+  TransporterInterface::SharedPtr get_transporter2()
+  {
+    return transporter2_;
+  }
+
+private:
+  int fds1[2];
+  int fds2[2];
+  TransporterInterface::SharedPtr transporter1_;
+  TransporterInterface::SharedPtr transporter2_;
+};
+
+#endif  // DUMMY_TRANSPORTER_HPP_

src/rm_hardware_driver/rm_serial_driver/test/test_fixed_packet_tool.cpp

@@ -0,0 +1,74 @@
+// Copyright 2021 RoboMaster-OSS
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "dummy_transporter.hpp"
+#include "gtest/gtest.h"
+#include "rm_serial_driver/fixed_packet.hpp"
+#include "rm_serial_driver/fixed_packet_tool.hpp"
+
+using namespace fyt;
+TEST(FixedPacketTool, construct_with_nullptr) {
+  EXPECT_THROW(serial_driver::FixedPacketTool<32>(nullptr), std::invalid_argument);
+}
+
+TEST(FixedPacketTool, send_and_recv) {
+  auto factory = std::make_shared<TransporterFactory>();
+  auto transporter1 = factory->get_transporter1();
+  auto transporter2 = factory->get_transporter2();
+  auto packet_tool1 = std::make_shared<serial_driver::FixedPacketTool<32>>(transporter1);
+  auto packet_tool2 = std::make_shared<serial_driver::FixedPacketTool<32>>(transporter2);
+  serial_driver::FixedPacket<32> packet1, packet2;
+  // send
+  int a = 10;
+  packet1.loadData(a, 10);
+  bool send_ret = packet_tool1->sendPacket(packet1);
+  ASSERT_TRUE(send_ret);
+  // recv
+  int b;
+  bool recv_ret = packet_tool2->recvPacket(packet2);
+  ASSERT_TRUE(recv_ret);
+  packet2.unloadData<int>(b, 10);
+  EXPECT_EQ(a, b);
+}
+
+TEST(FixedPacketTool, realtime_send) {
+  auto factory = std::make_shared<TransporterFactory>();
+  auto transporter1 = factory->get_transporter1();
+  auto transporter2 = factory->get_transporter2();
+  auto packet_tool1 = std::make_shared<serial_driver::FixedPacketTool<32>>(transporter1);
+  auto packet_tool2 = std::make_shared<serial_driver::FixedPacketTool<32>>(transporter2);
+  packet_tool1->enbaleRealtimeSend(true);
+  serial_driver::FixedPacket<32> packet1, packet2;
+  // recv
+  auto t = std::thread([&]() {
+    int b;
+    for (int i = 0; i < 10; i++) {
+      bool recv_ret = packet_tool2->recvPacket(packet2);
+      ASSERT_TRUE(recv_ret);
+      packet2.unloadData<int>(b, 10);
+      EXPECT_EQ(i, b);
+    }
+  });
+  // send
+  for (int i = 0; i < 10; i++) {
+    packet1.loadData(i, 10);
+    bool send_ret = packet_tool1->sendPacket(packet1);
+    ASSERT_TRUE(send_ret);
+  }
+  t.join();
+}

src/rm_hardware_driver/ros2_hik_camera/.clang-format

@@ -0,0 +1,18 @@
+---
+Language: Cpp
+BasedOnStyle: Google
+
+AccessModifierOffset: -2
+AlignAfterOpenBracket: AlwaysBreak
+BraceWrapping:
+  AfterClass: true
+  AfterFunction: true
+  AfterNamespace: true
+  AfterStruct: true
+BreakBeforeBraces: Custom
+ColumnLimit: 100
+ConstructorInitializerIndentWidth: 0
+ContinuationIndentWidth: 2
+DerivePointerAlignment: false
+PointerAlignment: Middle
+ReflowComments: false

src/rm_hardware_driver/ros2_hik_camera/.clang-tidy

@@ -0,0 +1,55 @@
+---
+Checks:          '-*,
+                  performance-*,
+                  -performance-unnecessary-value-param,
+                  llvm-namespace-comment,
+                  modernize-redundant-void-arg,
+                  modernize-use-nullptr,
+                  modernize-use-default,
+                  modernize-use-override,
+                  modernize-loop-convert,
+                  modernize-make-shared,
+                  modernize-make-unique,
+                  misc-unused-parameters,
+                  readability-named-parameter,
+                  readability-redundant-smartptr-get,
+                  readability-redundant-string-cstr,
+                  readability-simplify-boolean-expr,
+                  readability-container-size-empty,
+                  readability-identifier-naming,
+                  '
+HeaderFilterRegex: ''
+AnalyzeTemporaryDtors: false
+CheckOptions:
+  - key:             llvm-namespace-comment.ShortNamespaceLines
+    value:           '10'
+  - key:             llvm-namespace-comment.SpacesBeforeComments
+    value:           '2'
+  - key:             misc-unused-parameters.StrictMode
+    value:           '1'
+  - key:             readability-braces-around-statements.ShortStatementLines
+    value:           '2'
+  # type names
+  - key:             readability-identifier-naming.ClassCase
+    value:           CamelCase
+  - key:             readability-identifier-naming.EnumCase
+    value:           CamelCase
+  - key:             readability-identifier-naming.UnionCase
+    value:           CamelCase
+  # method names
+  - key:             readability-identifier-naming.MethodCase
+    value:           camelBack
+  # variable names
+  - key:             readability-identifier-naming.VariableCase
+    value:           lower_case
+  - key:             readability-identifier-naming.ClassMemberSuffix
+    value:           '_'
+  # const static or global variables are UPPER_CASE
+  - key:             readability-identifier-naming.EnumConstantCase
+    value:           UPPER_CASE
+  - key:             readability-identifier-naming.StaticConstantCase
+    value:           UPPER_CASE
+  - key:             readability-identifier-naming.ClassConstantCase
+    value:           UPPER_CASE
+  - key:             readability-identifier-naming.GlobalVariableCase
+    value:           UPPER_CASE

src/rm_hardware_driver/ros2_hik_camera/CMakeLists.txt

@@ -0,0 +1,78 @@
+cmake_minimum_required(VERSION 3.8)
+project(hik_camera)
+
+## Use C++14
+set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+## By adding -Wall and -Werror, the compiler does not ignore warnings anymore,
+## enforcing cleaner code.
+add_definitions(-Wall -Werror)
+
+## Export compile commands for clangd
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+find_package(OpenCV REQUIRED)
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/hik_camera_node.cpp
+  src/recorder.cpp
+  src/video_player_node.cpp
+)
+
+target_include_directories(${PROJECT_NAME} PUBLIC hikSDK/include)
+target_include_directories(${PROJECT_NAME} PUBLIC include ${OpenCV_INCLUDE_DIRS})
+
+if(CMAKE_SYSTEM_PROCESSOR MATCHES "x86_64")
+  target_link_directories(${PROJECT_NAME} PUBLIC hikSDK/lib/amd64)
+  install(
+    DIRECTORY hikSDK/lib/amd64/
+    DESTINATION lib
+  )
+elseif(CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64")
+  target_link_directories(${PROJECT_NAME} PUBLIC hikSDK/lib/arm64)
+  install(
+    DIRECTORY hikSDK/lib/arm64/
+    DESTINATION lib
+  )
+else()
+  message(FATAL_ERROR "Unsupport host system architecture: ${CMAKE_HOST_SYSTEM_PROCESSOR}!")
+endif()
+
+target_link_libraries(${PROJECT_NAME}
+  FormatConversion
+  MediaProcess
+  MvCameraControl
+  MVRender
+  MvUsb3vTL
+  ${OpenCV_LIBS}
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN fyt::camera_driver::HikCameraNode
+  EXECUTABLE ${PROJECT_NAME}_node
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN fyt::camera_driver::VideoPlayerNode
+  EXECUTABLE video_player_node
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  list(APPEND AMENT_LINT_AUTO_EXCLUDE
+    ament_cmake_copyright
+    ament_cmake_cpplint
+    ament_cmake_uncrustify
+  )
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  config
+  launch
+)

src/rm_hardware_driver/ros2_hik_camera/README.md

@@ -0,0 +1,100 @@
+# ros2_hik_camera
+
+A ROS2 packge for Hikvision USB3.0 industrial camera
+
+## Usage
+
+```
+ros2 launch hik_camera hik_camera.launch.py
+```
+
+离线视频回放（VideoPlayerNode，用于没有相机时跑全链路/复现）：
+
+```bash
+ros2 run hik_camera video_player_node --ros-args --params-file <your_video_player_params.yaml>
+```
+
+也可以在整套工程里通过 rm_bringup 启动（推荐），此时相机参数通常来自：
+
+- rm_bringup/config/node_params/camera_driver_params.yaml
+
+注意：rm_bringup 里加载的是自己的参数文件，hik_camera 包内的 config 主要用于“单独启动 hik_camera.launch.py”时测试。
+
+## Params
+
+以下参数均为 `camera_driver` 节点的 ROS 参数（YAML 中写在 `/**/ros__parameters` 下面即可）：
+
+- `camera_info_url`：标定文件 URL（如 `package://rm_bringup/config/camera_info.yaml`）
+- `camera_name`：相机名字标签（主要给 CameraInfo/工具识别用，不影响取图）
+- `camera_frame_id`：发布图像的 frame_id，默认 `camera_optical_frame`
+- `use_sensor_data_qos`：是否使用 sensor_data QoS
+
+成像控制：
+
+- `exposure_time`：曝光时间（单位：us），支持运行时 `ros2 param set`
+- `gain`：增益，支持运行时 `ros2 param set`
+- `auto_white_balance`：自动白平衡（0=Off，非0=Continuous）
+- `frame_rate`：期望帧率（会尝试下发到相机的 `AcquisitionFrameRate*` 节点；不同型号可能不支持）
+
+ROI / 裁剪（已接入海康 SDK 的 Width/Height/OffsetX/OffsetY，且会按相机的 min/max/inc 自动对齐）：
+
+- `resolution_width`
+- `resolution_height`
+- `offsetX`
+- `offsetY`
+
+像素格式（发布端输出格式，驱动会用 SDK 转码后发布）：
+
+- `pixel_format`：`rgb8`（默认）/ `bgr8` / `mono8`
+
+录像：
+
+- `recording`：是否录制，`true` 时保存到 `~/fyt2024-log/video/<timestamp>.avi`
+
+VideoPlayerNode 参数（与 rm_bringup/config/node_params/video_player_params.yaml 对齐）：
+
+- `path`：视频文件路径
+- `frame_rate`：发布帧率
+- `start_frame`：从第几帧开始发布（前面只读不发）
+- `keep_looping`：播到结尾是否循环
+- `frame_id`：发布 frame_id
+
+示例：
+/**:
+	ros__parameters:
+		camera_info_url: package://rm_bringup/config/camera_info.yaml
+		camera_name: narrow_stereo
+		camera_frame_id: camera_optical_frame
+		pixel_format: rgb8
+
+		exposure_time: 2500
+		gain: 15.0
+		auto_white_balance: 1
+		frame_rate: 210
+
+		resolution_width: 1280
+		resolution_height: 1024
+		offsetX: 0
+		offsetY: 0
+
+		recording: false
+
+```
+
+## Topics
+
+- `image_raw`：图像（Image）
+- `image_raw/camera_info`：相机标定信息（CameraInfo）
+- `camera_driver/heartbeat`：心跳（Int64，自增，1Hz）
+
+## Health / Reconnect
+
+- 驱动包含看门狗：超过 5 秒没有收到新帧会触发 `close()` 并自动重连 `open()`。
+
+## Daheng 风格参数说明（为什么有些需要重启）
+
+在本工程里，类似 `resolution_width/height`、`offsetX/Y`、`auto_white_balance`、`frame_rate` 这类参数属于“相机配置”，通常是在相机 `open()` 时一次性下发到硬件。
+
+- 对 Daheng 原实现：运行时只支持动态改 `exposure_time/gain`；其它参数改 YAML 后需要重启节点生效。
+- 对当前 Hik 实现：这些参数已支持运行时动态改（`ros2 param set` 会立即尝试下发到相机），但具体是否成功取决于相机型号是否暴露对应 GenICam 节点（失败会 WARN）。
+

src/rm_hardware_driver/ros2_hik_camera/config/camera_info.yaml

@@ -0,0 +1,26 @@
+image_width: 1280
+image_height: 1024
+camera_name: narrow_stereo
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [1385.639315,   0.     , 597.199071,
+           0.     , 1387.157400 , 452.204594,
+           0.     ,   0.     ,   1.     ]
+distortion_model: plumb_bob
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [-0.057934, 0.141815, -0.003889, -0.006984, 0.000000]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [1., 0., 0.,
+         0., 1., 0.,
+         0., 0., 1.]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [1380.831055,   0.     , 590.255495,   0.     ,
+           0.     , 1387.520996, 449.174676,   0.     ,
+           0.     ,   0.     ,   1.     ,   0.     ]

src/rm_hardware_driver/ros2_hik_camera/config/camera_params.yaml

@@ -0,0 +1,7 @@
+/camera_driver:
+  ros__parameters:
+    camera_name: narrow_stereo
+    camera_info_url: package://hik_camera/config/camera_info.yaml
+    exposure_time: 3000
+    gain: 16.0
+    use_sensor_data_qos: true

src/rm_hardware_driver/ros2_hik_camera/hikSDK/include/MvErrorDefine.h

@@ -0,0 +1,104 @@
+
+#ifndef _MV_ERROR_DEFINE_H_
+#define _MV_ERROR_DEFINE_H_
+
+/********************************************************************/
+///  \~chinese
+///  \name 正确码定义
+///  @{
+///  \~english
+///  \name Definition of correct code
+///  @{
+#define MV_OK                   0x00000000  ///< \~chinese 成功，无错误             \~english Successed, no error
+/// @}
+
+/********************************************************************/
+///  \~chinese
+///  \name 通用错误码定义:范围0x80000000-0x800000FF
+///  @{
+///  \~english
+///  \name Definition of General error code
+///  @{
+#define MV_E_HANDLE             0x80000000  ///< \~chinese 错误或无效的句柄         \~english Error or invalid handle
+#define MV_E_SUPPORT            0x80000001  ///< \~chinese 不支持的功能             \~english Not supported function
+#define MV_E_BUFOVER            0x80000002  ///< \~chinese 缓存已满                 \~english Buffer overflow
+#define MV_E_CALLORDER          0x80000003  ///< \~chinese 函数调用顺序错误         \~english Function calling order error
+#define MV_E_PARAMETER          0x80000004  ///< \~chinese 错误的参数               \~english Incorrect parameter
+#define MV_E_RESOURCE           0x80000006  ///< \~chinese 资源申请失败             \~english Applying resource failed
+#define MV_E_NODATA             0x80000007  ///< \~chinese 无数据                   \~english No data
+#define MV_E_PRECONDITION       0x80000008  ///< \~chinese 前置条件有误，或运行环境已发生变化       \~english Precondition error, or running environment changed
+#define MV_E_VERSION            0x80000009  ///< \~chinese 版本不匹配               \~english Version mismatches
+#define MV_E_NOENOUGH_BUF       0x8000000A  ///< \~chinese 传入的内存空间不足       \~english Insufficient memory
+#define MV_E_ABNORMAL_IMAGE     0x8000000B  ///< \~chinese 异常图像，可能是丢包导致图像不完整       \~english Abnormal image, maybe incomplete image because of lost packet
+#define MV_E_LOAD_LIBRARY       0x8000000C  ///< \~chinese 动态导入DLL失败          \~english Load library failed
+#define MV_E_NOOUTBUF           0x8000000D  ///< \~chinese 没有可输出的缓存         \~english No Avaliable Buffer
+#define MV_E_UNKNOW             0x800000FF  ///< \~chinese 未知的错误               \~english Unknown error
+/// @}
+
+/********************************************************************/
+///  \~chinese
+///  \name GenICam系列错误:范围0x80000100-0x800001FF
+///  @{
+///  \~english
+///  \name GenICam Series Error Codes: Range from 0x80000100 to 0x800001FF
+///  @{
+#define MV_E_GC_GENERIC         0x80000100  ///< \~chinese 通用错误                 \~english General error
+#define MV_E_GC_ARGUMENT        0x80000101  ///< \~chinese 参数非法                 \~english Illegal parameters
+#define MV_E_GC_RANGE           0x80000102  ///< \~chinese 值超出范围               \~english The value is out of range
+#define MV_E_GC_PROPERTY        0x80000103  ///< \~chinese 属性                     \~english Property
+#define MV_E_GC_RUNTIME         0x80000104  ///< \~chinese 运行环境有问题           \~english Running environment error
+#define MV_E_GC_LOGICAL         0x80000105  ///< \~chinese 逻辑错误                 \~english Logical error
+#define MV_E_GC_ACCESS          0x80000106  ///< \~chinese 节点访问条件有误         \~english Node accessing condition error
+#define MV_E_GC_TIMEOUT         0x80000107  ///< \~chinese 超时                     \~english Timeout
+#define MV_E_GC_DYNAMICCAST     0x80000108  ///< \~chinese 转换异常                 \~english Transformation exception
+#define MV_E_GC_UNKNOW          0x800001FF  ///< \~chinese GenICam未知错误          \~english GenICam unknown error
+/// @}
+
+/********************************************************************/
+///  \~chinese
+///  \name GigE_STATUS对应的错误码:范围0x80000200-0x800002FF
+///  @{
+///  \~english
+///  \name GigE_STATUS Error Codes: Range from 0x80000200 to 0x800002FF
+///  @{
+#define MV_E_NOT_IMPLEMENTED    0x80000200  ///< \~chinese 命令不被设备支持         \~english The command is not supported by device
+#define MV_E_INVALID_ADDRESS    0x80000201  ///< \~chinese 访问的目标地址不存在     \~english The target address being accessed does not exist
+#define MV_E_WRITE_PROTECT      0x80000202  ///< \~chinese 目标地址不可写           \~english The target address is not writable
+#define MV_E_ACCESS_DENIED      0x80000203  ///< \~chinese 设备无访问权限           \~english No permission
+#define MV_E_BUSY               0x80000204  ///< \~chinese 设备忙，或网络断开       \~english Device is busy, or network disconnected
+#define MV_E_PACKET             0x80000205  ///< \~chinese 网络包数据错误           \~english Network data packet error
+#define MV_E_NETER              0x80000206  ///< \~chinese 网络相关错误             \~english Network error
+#define MV_E_IP_CONFLICT        0x80000221  ///< \~chinese 设备IP冲突               \~english Device IP conflict
+/// @}
+
+/********************************************************************/
+///  \~chinese
+///  \name USB_STATUS对应的错误码:范围0x80000300-0x800003FF
+///  @{
+///  \~english
+///  \name USB_STATUS Error Codes: Range from 0x80000300 to 0x800003FF
+///  @{
+#define MV_E_USB_READ           0x80000300      ///< \~chinese 读usb出错            \~english Reading USB error
+#define MV_E_USB_WRITE          0x80000301      ///< \~chinese 写usb出错            \~english Writing USB error
+#define MV_E_USB_DEVICE         0x80000302      ///< \~chinese 设备异常             \~english Device exception
+#define MV_E_USB_GENICAM        0x80000303      ///< \~chinese GenICam相关错误      \~english GenICam error
+#define MV_E_USB_BANDWIDTH      0x80000304      ///< \~chinese 带宽不足  该错误码新增   \~english Insufficient bandwidth, this error code is newly added
+#define MV_E_USB_DRIVER         0x80000305      ///< \~chinese 驱动不匹配或者未装驱动   \~english Driver mismatch or unmounted drive
+#define MV_E_USB_UNKNOW         0x800003FF      ///< \~chinese USB未知的错误        \~english USB unknown error
+/// @}
+
+/********************************************************************/
+///  \~chinese
+///  \name 升级时对应的错误码:范围0x80000400-0x800004FF
+///  @{
+///  \~english
+///  \name Upgrade Error Codes: Range from 0x80000400 to 0x800004FF
+///  @{
+#define MV_E_UPG_FILE_MISMATCH     0x80000400 ///< \~chinese 升级固件不匹配         \~english Firmware mismatches
+#define MV_E_UPG_LANGUSGE_MISMATCH 0x80000401 ///< \~chinese 升级固件语言不匹配     \~english Firmware language mismatches
+#define MV_E_UPG_CONFLICT          0x80000402 ///< \~chinese 升级冲突（设备已经在升级了再次请求升级即返回此错误）   \~english Upgrading conflicted (repeated upgrading requests during device upgrade)
+#define MV_E_UPG_INNER_ERR         0x80000403 ///< \~chinese 升级时相机内部出现错误     \~english Camera internal error during upgrade
+#define MV_E_UPG_UNKNOW            0x800004FF ///< \~chinese 升级时未知错误         \~english Unknown error during upgrade
+/// @}
+
+#endif //_MV_ERROR_DEFINE_H_

src/rm_hardware_driver/ros2_hik_camera/hikSDK/include/MvISPErrorDefine.h

@@ -0,0 +1,93 @@
+
+#ifndef _MV_ISP_ERROR_DEFINE_H_
+#define _MV_ISP_ERROR_DEFINE_H_
+
+/************************************************************************
+* 来自ISP算法库的错误码
+************************************************************************/
+// 通用类型
+#define MV_ALG_OK                   0x00000000  //处理正确
+#define MV_ALG_ERR                  0x10000000  //不确定类型错误
+
+// 能力检查
+#define MV_ALG_E_ABILITY_ARG        0x10000001  //能力集中存在无效参数
+
+// 内存检查
+#define MV_ALG_E_MEM_NULL           0x10000002  //内存地址为空
+#define MV_ALG_E_MEM_ALIGN          0x10000003  //内存对齐不满足要求
+#define MV_ALG_E_MEM_LACK           0x10000004  //内存空间大小不够
+#define MV_ALG_E_MEM_SIZE_ALIGN     0x10000005  //内存空间大小不满足对齐要求
+#define MV_ALG_E_MEM_ADDR_ALIGN     0x10000006  //内存地址不满足对齐要求
+
+// 图像检查
+#define MV_ALG_E_IMG_FORMAT         0x10000007  //图像格式不正确或者不支持
+#define MV_ALG_E_IMG_SIZE           0x10000008  //图像宽高不正确或者超出范围
+#define MV_ALG_E_IMG_STEP           0x10000009  //图像宽高与step参数不匹配
+#define MV_ALG_E_IMG_DATA_NULL      0x1000000A  //图像数据存储地址为空
+
+// 输入输出参数检查
+#define MV_ALG_E_CFG_TYPE           0x1000000B  //设置或者获取参数类型不正确
+#define MV_ALG_E_CFG_SIZE           0x1000000C  //设置或者获取参数的输入、输出结构体大小不正确
+#define MV_ALG_E_PRC_TYPE           0x1000000D  //处理类型不正确
+#define MV_ALG_E_PRC_SIZE           0x1000000E  //处理时输入、输出参数大小不正确
+#define MV_ALG_E_FUNC_TYPE          0x1000000F  //子处理类型不正确
+#define MV_ALG_E_FUNC_SIZE          0x10000010  //子处理时输入、输出参数大小不正确
+
+// 运行参数检查
+#define MV_ALG_E_PARAM_INDEX        0x10000011  //index参数不正确
+#define MV_ALG_E_PARAM_VALUE        0x10000012  //value参数不正确或者超出范围
+#define MV_ALG_E_PARAM_NUM          0x10000013  //param_num参数不正确
+
+// 接口调用检查
+#define MV_ALG_E_NULL_PTR           0x10000014  //函数参数指针为空
+#define MV_ALG_E_OVER_MAX_MEM       0x10000015  //超过限定的最大内存
+#define MV_ALG_E_CALL_BACK          0x10000016  //回调函数出错
+
+// 算法库加密相关检查
+#define MV_ALG_E_ENCRYPT            0x10000017  //加密错误
+#define MV_ALG_E_EXPIRE             0x10000018  //算法库使用期限错误
+
+// 内部模块返回的基本错误类型
+#define MV_ALG_E_BAD_ARG            0x10000019  //参数范围不正确
+#define MV_ALG_E_DATA_SIZE          0x1000001A  //数据大小不正确
+#define MV_ALG_E_STEP               0x1000001B  //数据step不正确
+
+// cpu指令集支持错误码
+#define MV_ALG_E_CPUID              0x1000001C  //cpu不支持优化代码中的指令集
+
+#define MV_ALG_WARNING              0x1000001D  //警告
+
+#define MV_ALG_E_TIME_OUT           0x1000001E  //算法库超时
+#define MV_ALG_E_LIB_VERSION        0x1000001F  //算法版本号出错
+#define MV_ALG_E_MODEL_VERSION      0x10000020  //模型版本号出错
+#define MV_ALG_E_GPU_MEM_ALLOC      0x10000021  //GPU内存分配错误
+#define MV_ALG_E_FILE_NON_EXIST     0x10000022  //文件不存在
+#define MV_ALG_E_NONE_STRING        0x10000023  //字符串为空
+#define MV_ALG_E_IMAGE_CODEC        0x10000024  //图像解码器错误
+#define MV_ALG_E_FILE_OPEN          0x10000025  //打开文件错误
+#define MV_ALG_E_FILE_READ          0x10000026  //文件读取错误
+#define MV_ALG_E_FILE_WRITE         0x10000027  //文件写错误
+#define MV_ALG_E_FILE_READ_SIZE     0x10000028  //文件读取大小错误
+#define MV_ALG_E_FILE_TYPE          0x10000029  //文件类型错误
+#define MV_ALG_E_MODEL_TYPE         0x1000002A  //模型类型错误
+#define MV_ALG_E_MALLOC_MEM         0x1000002B  //分配内存错误
+#define MV_ALG_E_BIND_CORE_FAILED   0x1000002C  //线程绑核失败
+
+// 降噪特有错误码
+#define MV_ALG_E_DENOISE_NE_IMG_FORMAT       0x10402001  //噪声特性图像格式错误
+#define MV_ALG_E_DENOISE_NE_FEATURE_TYPE     0x10402002  //噪声特性类型错误
+#define MV_ALG_E_DENOISE_NE_PROFILE_NUM      0x10402003  //噪声特性个数错误
+#define MV_ALG_E_DENOISE_NE_GAIN_NUM         0x10402004  //噪声特性增益个数错误
+#define MV_ALG_E_DENOISE_NE_GAIN_VAL         0x10402005  //噪声曲线增益值输入错误
+#define MV_ALG_E_DENOISE_NE_BIN_NUM          0x10402006  //噪声曲线柱数错误
+#define MV_ALG_E_DENOISE_NE_INIT_GAIN        0x10402007  //噪声估计初始化增益设置错误
+#define MV_ALG_E_DENOISE_NE_NOT_INIT         0x10402008  //噪声估计未初始化
+#define MV_ALG_E_DENOISE_COLOR_MODE          0x10402009  //颜色空间模式错误
+#define MV_ALG_E_DENOISE_ROI_NUM             0x1040200a  //图像ROI个数错误
+#define MV_ALG_E_DENOISE_ROI_ORI_PT          0x1040200b  //图像ROI原点错误
+#define MV_ALG_E_DENOISE_ROI_SIZE            0x1040200c  //图像ROI大小错误
+#define MV_ALG_E_DENOISE_GAIN_NOT_EXIST      0x1040200d  //输入的相机增益不存在(增益个数已达上限)
+#define MV_ALG_E_DENOISE_GAIN_BEYOND_RANGE   0x1040200e  //输入的相机增益不在范围内
+#define MV_ALG_E_DENOISE_NP_BUF_SIZE         0x1040200f  //输入的噪声特性内存大小错误
+
+#endif //_MV_ISP_ERROR_DEFINE_H_

src/rm_hardware_driver/ros2_hik_camera/hikSDK/include/PixelType.h

@@ -0,0 +1,202 @@
+
+#ifndef _MV_PIXEL_TYPE_H_
+#define _MV_PIXEL_TYPE_H_
+
+//#include "Base/GCTypes.h"
+
+
+/************************************************************************/
+/*     GigE Vision (2.0.03) PIXEL FORMATS                               */
+/************************************************************************/
+
+// Indicate if pixel is monochrome or RGB
+#define MV_GVSP_PIX_MONO                                0x01000000
+#define MV_GVSP_PIX_RGB                                 0x02000000 // deprecated in version 1.1
+#define MV_GVSP_PIX_COLOR                               0x02000000
+#define MV_GVSP_PIX_CUSTOM                              0x80000000
+#define MV_GVSP_PIX_COLOR_MASK                          0xFF000000
+
+// Indicate effective number of bits occupied by the pixel (including padding).
+// This can be used to compute amount of memory required to store an image.
+#define MV_PIXEL_BIT_COUNT(n)                           ((n) << 16)
+
+#define MV_GVSP_PIX_EFFECTIVE_PIXEL_SIZE_MASK           0x00FF0000
+#define MV_GVSP_PIX_EFFECTIVE_PIXEL_SIZE_SHIFT          16
+
+// Pixel ID: lower 16-bit of the pixel formats
+#define MV_GVSP_PIX_ID_MASK                             0x0000FFFF
+#define MV_GVSP_PIX_COUNT                               0x46 // next Pixel ID available
+
+
+enum MvGvspPixelType
+{
+    // Undefined pixel type
+#ifdef WIN32
+    PixelType_Gvsp_Undefined                =   0xFFFFFFFF, 
+#else
+    PixelType_Gvsp_Undefined                =   -1, 
+#endif
+    // Mono buffer format defines
+    PixelType_Gvsp_Mono1p                   =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(1) | 0x0037),
+    PixelType_Gvsp_Mono2p                   =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(2) | 0x0038),
+    PixelType_Gvsp_Mono4p                   =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(4) | 0x0039),
+    PixelType_Gvsp_Mono8                    =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8) | 0x0001),
+    PixelType_Gvsp_Mono8_Signed             =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8)  | 0x0002),
+    PixelType_Gvsp_Mono10                   =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0003),
+    PixelType_Gvsp_Mono10_Packed            =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x0004),
+    PixelType_Gvsp_Mono12                   =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0005),
+    PixelType_Gvsp_Mono12_Packed            =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x0006),
+    PixelType_Gvsp_Mono14                   =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0025),
+    PixelType_Gvsp_Mono16                   =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0007),
+
+    // Bayer buffer format defines 
+    PixelType_Gvsp_BayerGR8                 =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8) | 0x0008),
+    PixelType_Gvsp_BayerRG8                 =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8) | 0x0009),
+    PixelType_Gvsp_BayerGB8                 =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8) | 0x000A),
+    PixelType_Gvsp_BayerBG8                 =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8) | 0x000B),
+    PixelType_Gvsp_BayerGR10                =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x000C),
+    PixelType_Gvsp_BayerRG10                =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x000D),
+    PixelType_Gvsp_BayerGB10                =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x000E),
+    PixelType_Gvsp_BayerBG10                =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x000F),
+    PixelType_Gvsp_BayerGR12                =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0010),
+    PixelType_Gvsp_BayerRG12                =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0011),
+    PixelType_Gvsp_BayerGB12                =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0012),
+    PixelType_Gvsp_BayerBG12                =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0013),
+    PixelType_Gvsp_BayerGR10_Packed         =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x0026),
+    PixelType_Gvsp_BayerRG10_Packed         =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x0027),
+    PixelType_Gvsp_BayerGB10_Packed         =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x0028),
+    PixelType_Gvsp_BayerBG10_Packed         =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x0029),
+    PixelType_Gvsp_BayerGR12_Packed         =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x002A),
+    PixelType_Gvsp_BayerRG12_Packed         =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x002B),
+    PixelType_Gvsp_BayerGB12_Packed         =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x002C),
+    PixelType_Gvsp_BayerBG12_Packed         =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x002D),
+    PixelType_Gvsp_BayerGR16                =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x002E),
+    PixelType_Gvsp_BayerRG16                =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x002F),
+    PixelType_Gvsp_BayerGB16                =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0030),
+    PixelType_Gvsp_BayerBG16                =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0031),
+
+    // RGB Packed buffer format defines 
+    PixelType_Gvsp_RGB8_Packed              =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(24) | 0x0014),
+    PixelType_Gvsp_BGR8_Packed              =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(24) | 0x0015),
+    PixelType_Gvsp_RGBA8_Packed             =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(32) | 0x0016),
+    PixelType_Gvsp_BGRA8_Packed             =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(32) | 0x0017),
+    PixelType_Gvsp_RGB10_Packed             =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(48) | 0x0018),
+    PixelType_Gvsp_BGR10_Packed             =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(48) | 0x0019),
+    PixelType_Gvsp_RGB12_Packed             =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(48) | 0x001A),
+    PixelType_Gvsp_BGR12_Packed             =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(48) | 0x001B),
+    PixelType_Gvsp_RGB16_Packed             =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(48) | 0x0033),
+    PixelType_Gvsp_BGR16_Packed             =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(48) | 0x004B),
+    PixelType_Gvsp_RGBA16_Packed            =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(64) | 0x0064),
+    PixelType_Gvsp_BGRA16_Packed            =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(64) | 0x0051),
+    PixelType_Gvsp_RGB10V1_Packed           =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(32) | 0x001C),
+    PixelType_Gvsp_RGB10V2_Packed           =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(32) | 0x001D),
+    PixelType_Gvsp_RGB12V1_Packed           =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(36) | 0X0034),
+    PixelType_Gvsp_RGB565_Packed            =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(16) | 0x0035),
+    PixelType_Gvsp_BGR565_Packed            =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(16) | 0X0036),
+
+    // YUV Packed buffer format defines 
+    PixelType_Gvsp_YUV411_Packed            =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(12) | 0x001E),
+    PixelType_Gvsp_YUV422_Packed            =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(16) | 0x001F),
+    PixelType_Gvsp_YUV422_YUYV_Packed       =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(16) | 0x0032),
+    PixelType_Gvsp_YUV444_Packed            =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(24) | 0x0020),
+    PixelType_Gvsp_YCBCR8_CBYCR             =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(24) | 0x003A),
+    PixelType_Gvsp_YCBCR422_8               =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(16) | 0x003B),
+    PixelType_Gvsp_YCBCR422_8_CBYCRY        =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(16) | 0x0043),
+    PixelType_Gvsp_YCBCR411_8_CBYYCRYY      =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(12) | 0x003C),
+    PixelType_Gvsp_YCBCR601_8_CBYCR         =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(24) | 0x003D),
+    PixelType_Gvsp_YCBCR601_422_8           =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(16) | 0x003E),
+    PixelType_Gvsp_YCBCR601_422_8_CBYCRY    =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(16) | 0x0044),
+    PixelType_Gvsp_YCBCR601_411_8_CBYYCRYY  =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(12) | 0x003F),
+    PixelType_Gvsp_YCBCR709_8_CBYCR         =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(24) | 0x0040),
+    PixelType_Gvsp_YCBCR709_422_8           =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(16) | 0x0041),
+    PixelType_Gvsp_YCBCR709_422_8_CBYCRY    =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(16) | 0x0045),
+    PixelType_Gvsp_YCBCR709_411_8_CBYYCRYY  =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(12) | 0x0042),
+
+    // RGB Planar buffer format defines 
+    PixelType_Gvsp_RGB8_Planar              =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(24) | 0x0021),
+    PixelType_Gvsp_RGB10_Planar             =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(48) | 0x0022),
+    PixelType_Gvsp_RGB12_Planar             =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(48) | 0x0023),
+    PixelType_Gvsp_RGB16_Planar             =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(48) | 0x0024),
+
+    // 自定义的图片格式
+    PixelType_Gvsp_Jpeg                     =   (MV_GVSP_PIX_CUSTOM | MV_PIXEL_BIT_COUNT(24) | 0x0001),
+
+    PixelType_Gvsp_Coord3D_ABC32f           =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(96) | 0x00C0),//0x026000C0
+    PixelType_Gvsp_Coord3D_ABC32f_Planar    =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(96) | 0x00C1),//0x026000C1
+
+    // 该值被废弃，请参考PixelType_Gvsp_Coord3D_AC32f_64; the value is discarded
+    PixelType_Gvsp_Coord3D_AC32f            =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(40) | 0x00C2),
+    // 该值被废弃; the value is discarded    (已放入Chunkdata)
+    PixelType_Gvsp_COORD3D_DEPTH_PLUS_MASK  =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(28) | 0x0001),
+
+    PixelType_Gvsp_Coord3D_ABC32            =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(96) | 0x3001),//0x82603001
+    PixelType_Gvsp_Coord3D_AB32f            =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(64) | 0x3002),//0x82403002
+    PixelType_Gvsp_Coord3D_AB32             =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(64) | 0x3003),//0x82403003
+    PixelType_Gvsp_Coord3D_AC32f_64         =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(64) | 0x00C2),//0x024000C2
+    PixelType_Gvsp_Coord3D_AC32f_Planar     =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(64) | 0x00C3),//0x024000C3
+    PixelType_Gvsp_Coord3D_AC32             =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(64) | 0x3004),//0x82403004
+    PixelType_Gvsp_Coord3D_A32f             =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(32) | 0x00BD),//0x012000BD
+    PixelType_Gvsp_Coord3D_A32              =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(32) | 0x3005),//0x81203005
+    PixelType_Gvsp_Coord3D_C32f             =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(32) | 0x00BF),//0x012000BF
+    PixelType_Gvsp_Coord3D_C32              =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(32) | 0x3006),//0x81203006
+
+    PixelType_Gvsp_Coord3D_ABC16            =   (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(48) | 0x00B9),//0x023000B9
+    PixelType_Gvsp_Coord3D_C16              =   (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x00B8),//0x011000B8
+
+    //无损压缩像素格式定义
+    PixelType_Gvsp_HB_Mono8                    =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8) | 0x0001),
+    PixelType_Gvsp_HB_Mono10                   =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0003),
+    PixelType_Gvsp_HB_Mono10_Packed            =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x0004),
+    PixelType_Gvsp_HB_Mono12                   =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0005),
+    PixelType_Gvsp_HB_Mono12_Packed            =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x0006),
+    PixelType_Gvsp_HB_Mono16                   =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0007),
+    PixelType_Gvsp_HB_BayerGR8                 =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8) | 0x0008),
+    PixelType_Gvsp_HB_BayerRG8                 =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8) | 0x0009),
+    PixelType_Gvsp_HB_BayerGB8                 =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8) | 0x000A),
+    PixelType_Gvsp_HB_BayerBG8                 =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8) | 0x000B),
+	PixelType_Gvsp_HB_BayerRBGG8               =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8) | 0x0046),
+    PixelType_Gvsp_HB_BayerGR10                =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x000C),
+    PixelType_Gvsp_HB_BayerRG10                =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x000D),
+    PixelType_Gvsp_HB_BayerGB10                =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x000E),
+    PixelType_Gvsp_HB_BayerBG10                =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x000F),
+    PixelType_Gvsp_HB_BayerGR12                =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0010),
+    PixelType_Gvsp_HB_BayerRG12                =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0011),
+    PixelType_Gvsp_HB_BayerGB12                =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0012),
+    PixelType_Gvsp_HB_BayerBG12                =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0013),
+    PixelType_Gvsp_HB_BayerGR10_Packed         =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x0026),
+    PixelType_Gvsp_HB_BayerRG10_Packed         =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x0027),
+    PixelType_Gvsp_HB_BayerGB10_Packed         =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x0028),
+    PixelType_Gvsp_HB_BayerBG10_Packed         =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x0029),
+    PixelType_Gvsp_HB_BayerGR12_Packed         =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x002A),
+    PixelType_Gvsp_HB_BayerRG12_Packed         =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x002B),
+    PixelType_Gvsp_HB_BayerGB12_Packed         =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x002C),
+    PixelType_Gvsp_HB_BayerBG12_Packed         =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x002D),
+    PixelType_Gvsp_HB_YUV422_Packed            =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(16) | 0x001F),
+    PixelType_Gvsp_HB_YUV422_YUYV_Packed       =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(16) | 0x0032),
+    PixelType_Gvsp_HB_RGB8_Packed              =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(24) | 0x0014),
+    PixelType_Gvsp_HB_BGR8_Packed              =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(24) | 0x0015),
+    PixelType_Gvsp_HB_RGBA8_Packed             =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(32) | 0x0016),
+    PixelType_Gvsp_HB_BGRA8_Packed             =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(32) | 0x0017),
+    PixelType_Gvsp_HB_RGB16_Packed             =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(48) | 0x0033),
+    PixelType_Gvsp_HB_BGR16_Packed             =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(48) | 0x004B),
+    PixelType_Gvsp_HB_RGBA16_Packed            =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(64) | 0x0064),
+    PixelType_Gvsp_HB_BGRA16_Packed            =   (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(64) | 0x0051),
+
+};
+
+//enum MvUsbPixelType
+//{
+//
+//};
+
+//跨平台定义
+//Cross Platform Definition
+#ifdef WIN32
+typedef __int64 int64_t;
+typedef unsigned __int64 uint64_t;
+#else
+#include <stdint.h>
+#endif
+
+
+#endif /* _MV_PIXEL_TYPE_H_ */

src/rm_hardware_driver/ros2_hik_camera/include/hik_camera/recorder.hpp

@@ -0,0 +1,46 @@
+#ifndef HIK_CAMERA_RECORDER_HPP_
+#define HIK_CAMERA_RECORDER_HPP_
+
+// std
+#include <atomic>
+#include <condition_variable>
+#include <deque>
+#include <filesystem>
+#include <mutex>
+#include <thread>
+#include <vector>
+
+// OpenCV
+#include <opencv2/videoio.hpp>
+
+namespace fyt::camera_driver {
+class Recorder {
+public:
+  using Frame = std::vector<unsigned char>;
+
+  Recorder(const std::filesystem::path &file, int fps, cv::Size size);
+  ~Recorder();
+
+  void addFrame(const Frame &frame);
+  void start();
+  void stop();
+
+  std::filesystem::path path;
+
+private:
+  void recorderThread();
+
+  cv::Size size_;
+  int fps_;
+  cv::VideoWriter writer_;
+
+  std::deque<Frame> frame_queue_;
+
+  std::mutex mutex_;
+  std::atomic<bool> recording_{false};
+  std::condition_variable cv_;
+  std::thread recorder_thread_;
+};
+}  // namespace fyt::camera_driver
+
+#endif  // HIK_CAMERA_RECORDER_HPP_

src/rm_hardware_driver/ros2_hik_camera/launch/hik_camera.launch.py

@@ -0,0 +1,34 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory, get_package_prefix
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    params_file = os.path.join(
+        get_package_share_directory('hik_camera'), 'config', 'camera_params.yaml')
+
+    camera_info_url = 'package://hik_camera/config/camera_info.yaml'
+
+    return LaunchDescription([
+        DeclareLaunchArgument(name='params_file',
+                              default_value=params_file),
+        DeclareLaunchArgument(name='camera_info_url',
+                              default_value=camera_info_url),
+        DeclareLaunchArgument(name='use_sensor_data_qos',
+                              default_value='false'),
+
+        Node(
+            package='hik_camera',
+            executable='hik_camera_node',
+            output='screen',
+            emulate_tty=True,
+            parameters=[LaunchConfiguration('params_file'), {
+                'camera_info_url': LaunchConfiguration('camera_info_url'),
+                'use_sensor_data_qos': LaunchConfiguration('use_sensor_data_qos'),
+            }],
+        )
+    ])

src/rm_hardware_driver/ros2_hik_camera/package.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>hik_camera</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="1258124282@qq.com">nolem</maintainer>
+  <license>TODO: License declaration</license>
+
+  <!-- buildtool_depend: dependencies of the build process -->
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <!-- depend: build, export, and execution dependency -->
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</depend>
+  <depend>image_transport</depend>
+  <depend>image_transport_plugins</depend>
+  <depend>camera_info_manager</depend>
+  <depend>rm_utils</depend>
+  <depend>opencv</depend>
+
+  <exec_depend>camera_calibration</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/rm_hardware_driver/ros2_hik_camera/src/hik_camera_node.cpp

@@ -0,0 +1,514 @@
+#include <camera_info_manager/camera_info_manager.hpp>
+#include <chrono>
+#include <cstdlib>
+#include <ctime>
+#include <filesystem>
+#include <image_transport/image_transport.hpp>
+#include <memory>
+#include <mutex>
+#include <rcl_interfaces/msg/set_parameters_result.hpp>
+#include <rclcpp/logging.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/camera_info.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <string>
+#include <thread>
+
+#include "MvCameraControl.h"
+#include "hik_camera/recorder.hpp"
+#include "rm_utils/heartbeat.hpp"
+#include "rm_utils/logger/log.hpp"
+
+namespace fyt::camera_driver
+{
+class HikCameraNode : public rclcpp::Node
+{
+public:
+  explicit HikCameraNode(const rclcpp::NodeOptions & options) : Node("camera_driver", options)
+  {
+    FYT_REGISTER_LOGGER("camera_driver", "~/fyt2024-log", INFO);
+    FYT_INFO("camera_driver", "Starting HikCameraNode!");
+
+    camera_name_ = this->declare_parameter("camera_name", "narrow_stereo");
+    camera_info_url_ =
+      this->declare_parameter("camera_info_url", "package://hik_camera/config/camera_info.yaml");
+    frame_id_ = this->declare_parameter("camera_frame_id", "camera_optical_frame");
+    pixel_format_ = this->declare_parameter("pixel_format", "rgb8");
+    frame_rate_ = this->declare_parameter("frame_rate", 210);
+    exposure_time_ = this->declare_parameter("exposure_time", 5000);
+    gain_ = this->declare_parameter("gain", 12.0);
+
+    resolution_width_ = this->declare_parameter("resolution_width", 1280);
+    resolution_height_ = this->declare_parameter("resolution_height", 1024);
+    offset_x_ = this->declare_parameter("offsetX", 0);
+    offset_y_ = this->declare_parameter("offsetY", 0);
+
+    image_msg_.header.frame_id = frame_id_;
+    image_msg_.encoding = pixel_format_;
+
+    bool use_sensor_data_qos = this->declare_parameter("use_sensor_data_qos", true);
+    auto qos = use_sensor_data_qos ? rmw_qos_profile_sensor_data : rmw_qos_profile_default;
+    camera_pub_ = image_transport::create_camera_publisher(this, "image_raw", qos);
+
+    camera_info_manager_ =
+      std::make_unique<camera_info_manager::CameraInfoManager>(this, camera_name_);
+    if (camera_info_manager_->validateURL(camera_info_url_)) {
+      camera_info_manager_->loadCameraInfo(camera_info_url_);
+      camera_info_msg_ = camera_info_manager_->getCameraInfo();
+    } else {
+      camera_info_manager_->setCameraName(camera_name_);
+      sensor_msgs::msg::CameraInfo camera_info;
+      camera_info_manager_->setCameraInfo(camera_info);
+      FYT_WARN("camera_driver", "Invalid camera info URL: {}", camera_info_url_);
+    }
+
+    // Heartbeat
+    heartbeat_ = fyt::HeartBeatPublisher::create(this);
+
+    // Param set callback
+    params_callback_handle_ = this->add_on_set_parameters_callback(
+      std::bind(&HikCameraNode::parametersCallback, this, std::placeholders::_1));
+
+    // Recorder
+    bool enable_recorder = this->declare_parameter("recording", false);
+    if (enable_recorder) {
+      std::string home = std::getenv("HOME") ? std::getenv("HOME") : std::string(".");
+      namespace fs = std::filesystem;
+      std::filesystem::path video_path = fs::path(home) / "fyt2024-log/video/" /
+                                         std::string(std::to_string(std::time(nullptr)) + ".avi");
+      recorder_ = std::make_unique<Recorder>(
+        video_path, frame_rate_, cv::Size(resolution_width_, resolution_height_));
+      recorder_->start();
+      FYT_INFO("camera_driver", "Recorder started! Video file: {}", video_path.string());
+    }
+
+    // Watch dog timer (also handles initial open)
+    timer_ = this->create_wall_timer(
+      std::chrono::milliseconds(1000), std::bind(&HikCameraNode::timerCallback, this));
+
+    // Start capture thread
+    capture_thread_ = std::thread([this]() { this->captureLoop(); });
+
+    FYT_INFO("camera_driver", "HikCameraNode has been initialized!");
+  }
+
+  ~HikCameraNode() override
+  {
+    close();
+    if (capture_thread_.joinable()) {
+      capture_thread_.join();
+    }
+    if (recorder_ != nullptr) {
+      recorder_->stop();
+      FYT_INFO(
+        "camera_driver", "Recorder stopped! Video file {} has been saved",
+        recorder_->path.string());
+    }
+    FYT_INFO("camera_driver", "HikCameraNode has been destroyed!");
+  }
+
+private:
+  static int64_t clampToInc(int64_t value, int64_t min, int64_t max, int64_t inc)
+  {
+    if (inc <= 0) {
+      inc = 1;
+    }
+    if (value < min) {
+      value = min;
+    }
+    if (value > max) {
+      value = max;
+    }
+    const int64_t delta = value - min;
+    return min + (delta / inc) * inc;
+  }
+
+  bool getIntNodeLocked(const char * name, MVCC_INTVALUE & out)
+  {
+    if (camera_handle_ == nullptr) {
+      return false;
+    }
+    return MV_CC_GetIntValue(camera_handle_, name, &out) == MV_OK;
+  }
+
+  bool setIntNodeLocked(const char * name, int64_t value)
+  {
+    if (camera_handle_ == nullptr) {
+      return false;
+    }
+    return MV_CC_SetIntValue(camera_handle_, name, value) == MV_OK;
+  }
+
+  void applyRoiLocked()
+  {
+    // Best-effort: not all cameras support all nodes
+    MVCC_INTVALUE width_info;
+    MVCC_INTVALUE height_info;
+    MVCC_INTVALUE offx_info;
+    MVCC_INTVALUE offy_info;
+
+    const bool has_width = getIntNodeLocked("Width", width_info);
+    const bool has_height = getIntNodeLocked("Height", height_info);
+    const bool has_offx = getIntNodeLocked("OffsetX", offx_info);
+    const bool has_offy = getIntNodeLocked("OffsetY", offy_info);
+
+    if (!has_width || !has_height) {
+      FYT_WARN("camera_driver", "Camera does not expose Width/Height nodes; ROI params ignored");
+      return;
+    }
+
+    int64_t target_w =
+      clampToInc(resolution_width_, width_info.nMin, width_info.nMax, width_info.nInc);
+    int64_t target_h =
+      clampToInc(resolution_height_, height_info.nMin, height_info.nMax, height_info.nInc);
+
+    int64_t target_x = 0;
+    int64_t target_y = 0;
+    if (has_offx) {
+      target_x = clampToInc(offset_x_, offx_info.nMin, offx_info.nMax, offx_info.nInc);
+    }
+    if (has_offy) {
+      target_y = clampToInc(offset_y_, offy_info.nMin, offy_info.nMax, offy_info.nInc);
+    }
+
+    // Some cameras constrain width/height based on offset; apply in safe order
+    if (has_offx) {
+      (void)setIntNodeLocked("OffsetX", target_x);
+    }
+    if (has_offy) {
+      (void)setIntNodeLocked("OffsetY", target_y);
+    }
+    (void)setIntNodeLocked("Width", target_w);
+    (void)setIntNodeLocked("Height", target_h);
+
+    resolution_width_ = static_cast<int>(target_w);
+    resolution_height_ = static_cast<int>(target_h);
+    offset_x_ = static_cast<int>(target_x);
+    offset_y_ = static_cast<int>(target_y);
+
+    FYT_INFO(
+      "camera_driver", "ROI applied: {}x{} offset({},{})", resolution_width_, resolution_height_,
+      offset_x_, offset_y_);
+  }
+
+  void timerCallback()
+  {
+    // Open camera
+    while (!is_open_ && rclcpp::ok()) {
+      if (!open()) {
+        FYT_ERROR("camera_driver", "open() failed");
+        close();
+        return;
+      }
+    }
+
+    // Watchdog: no frames for too long => reconnect
+    double dt = (this->now() - latest_frame_stamp_).seconds();
+    if (dt > 5.0) {
+      FYT_WARN("camera_driver", "Camera is not alive! lost frame for {:.2f} seconds", dt);
+      close();
+    }
+  }
+
+  bool open()
+  {
+    if (is_open_) {
+      close();
+    }
+
+    FYT_INFO("camera_driver", "Opening Hik Camera Device!");
+
+    MV_CC_DEVICE_INFO_LIST device_list;
+    int ret = MV_CC_EnumDevices(MV_GIGE_DEVICE | MV_USB_DEVICE, &device_list);
+    if (ret != MV_OK || device_list.nDeviceNum == 0) {
+      FYT_WARN("camera_driver", "No camera found! Enum state: [0x{:x}]", ret);
+      return false;
+    }
+    FYT_INFO("camera_driver", "Found {} devices", device_list.nDeviceNum);
+
+    ret = MV_CC_CreateHandle(&camera_handle_, device_list.pDeviceInfo[0]);
+    if (ret != MV_OK) {
+      FYT_ERROR("camera_driver", "CreateHandle failed: [0x{:x}]", ret);
+      camera_handle_ = nullptr;
+      return false;
+    }
+
+    ret = MV_CC_OpenDevice(camera_handle_);
+    if (ret != MV_OK) {
+      FYT_ERROR("camera_driver", "OpenDevice failed: [0x{:x}]", ret);
+      MV_CC_DestroyHandle(&camera_handle_);
+      camera_handle_ = nullptr;
+      return false;
+    }
+
+    {
+      std::lock_guard<std::mutex> lock(camera_mutex_);
+      // Trigger off (continuous)
+      (void)MV_CC_SetTriggerMode(camera_handle_, MV_TRIGGER_MODE_OFF);
+
+      // Apply ROI/offset before querying image info
+      applyRoiLocked();
+
+      // Set exposure / gain
+      (void)MV_CC_SetFloatValue(camera_handle_, "ExposureTime", static_cast<float>(exposure_time_));
+      (void)MV_CC_SetFloatValue(camera_handle_, "Gain", static_cast<float>(gain_));
+    }
+
+    // Get image info
+    {
+      std::lock_guard<std::mutex> lock(camera_mutex_);
+      ret = MV_CC_GetImageInfo(camera_handle_, &img_info_);
+    }
+    if (ret != MV_OK) {
+      FYT_ERROR("camera_driver", "GetImageInfo failed: [0x{:x}]", ret);
+      close();
+      return false;
+    }
+
+    // Init convert param
+    convert_param_ = {};
+    convert_param_.nWidth = img_info_.nWidthValue;
+    convert_param_.nHeight = img_info_.nHeightValue;
+    convert_param_.enDstPixelType = PixelType_Gvsp_RGB8_Packed;
+
+    // Use actual output size from camera
+    resolution_width_ = static_cast<int>(img_info_.nWidthValue);
+    resolution_height_ = static_cast<int>(img_info_.nHeightValue);
+
+    image_msg_.height = resolution_height_;
+    image_msg_.width = resolution_width_;
+    image_msg_.step = resolution_width_ * 3;
+    image_msg_.data.resize(static_cast<size_t>(image_msg_.height) * image_msg_.step);
+
+    camera_info_msg_.header.frame_id = frame_id_;
+    camera_info_msg_.width = image_msg_.width;
+    camera_info_msg_.height = image_msg_.height;
+    latest_frame_stamp_ = this->now();
+
+    {
+      std::lock_guard<std::mutex> lock(camera_mutex_);
+      ret = MV_CC_StartGrabbing(camera_handle_);
+    }
+    if (ret != MV_OK) {
+      FYT_ERROR("camera_driver", "StartGrabbing failed: [0x{:x}]", ret);
+      close();
+      return false;
+    }
+
+    is_open_ = true;
+    FYT_INFO("camera_driver", "Hik Camera Device Open Success!");
+    return true;
+  }
+
+  void close()
+  {
+    if (!is_open_) {
+      return;
+    }
+    FYT_INFO("camera_driver", "Closing Hik Camera Device!");
+    is_open_ = false;
+
+    std::lock_guard<std::mutex> lock(camera_mutex_);
+    if (camera_handle_) {
+      (void)MV_CC_StopGrabbing(camera_handle_);
+      (void)MV_CC_CloseDevice(camera_handle_);
+      (void)MV_CC_DestroyHandle(&camera_handle_);
+      camera_handle_ = nullptr;
+    }
+  }
+
+  void captureLoop()
+  {
+    MV_FRAME_OUT out_frame;
+    while (rclcpp::ok()) {
+      if (!is_open_ || camera_handle_ == nullptr) {
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+        continue;
+      }
+
+      int ret;
+      {
+        std::lock_guard<std::mutex> lock(camera_mutex_);
+        if (!is_open_ || camera_handle_ == nullptr) {
+          continue;
+        }
+        ret = MV_CC_GetImageBuffer(camera_handle_, &out_frame, 1000);
+      }
+      if (ret != MV_OK) {
+        FYT_WARN("camera_driver", "GetImageBuffer failed: [0x{:x}]", ret);
+        fail_count_++;
+        if (fail_count_ > 5) {
+          close();
+        }
+        continue;
+      }
+
+      // Ensure output buffer
+      const size_t need_size =
+        static_cast<size_t>(out_frame.stFrameInfo.nWidth) * out_frame.stFrameInfo.nHeight * 3;
+      if (image_msg_.data.size() != need_size) {
+        image_msg_.data.resize(need_size);
+      }
+
+      convert_param_.pDstBuffer = image_msg_.data.data();
+      convert_param_.nDstBufferSize = image_msg_.data.size();
+      convert_param_.pSrcData = out_frame.pBufAddr;
+      convert_param_.nSrcDataLen = out_frame.stFrameInfo.nFrameLen;
+      convert_param_.enSrcPixelType = out_frame.stFrameInfo.enPixelType;
+
+      {
+        std::lock_guard<std::mutex> lock(camera_mutex_);
+        if (!is_open_ || camera_handle_ == nullptr) {
+          continue;
+        }
+        ret = MV_CC_ConvertPixelType(camera_handle_, &convert_param_);
+      }
+      if (ret != MV_OK) {
+        FYT_WARN("camera_driver", "ConvertPixelType failed: [0x{:x}]", ret);
+        {
+          std::lock_guard<std::mutex> lock(camera_mutex_);
+          if (camera_handle_ != nullptr) {
+            (void)MV_CC_FreeImageBuffer(camera_handle_, &out_frame);
+          }
+        }
+        fail_count_++;
+        continue;
+      }
+
+      auto stamp = this->now();
+      image_msg_.header.stamp = stamp;
+      image_msg_.header.frame_id = frame_id_;
+      image_msg_.height = out_frame.stFrameInfo.nHeight;
+      image_msg_.width = out_frame.stFrameInfo.nWidth;
+      image_msg_.step = out_frame.stFrameInfo.nWidth * 3;
+
+      camera_info_msg_.header = image_msg_.header;
+      camera_pub_.publish(image_msg_, camera_info_msg_);
+
+      latest_frame_stamp_ = stamp;
+      fail_count_ = 0;
+
+      if (recorder_ != nullptr) {
+        recorder_->addFrame(image_msg_.data);
+      }
+
+      {
+        std::lock_guard<std::mutex> lock(camera_mutex_);
+        if (camera_handle_ != nullptr) {
+          (void)MV_CC_FreeImageBuffer(camera_handle_, &out_frame);
+        }
+      }
+    }
+  }
+
+  rcl_interfaces::msg::SetParametersResult parametersCallback(
+    const std::vector<rclcpp::Parameter> & parameters)
+  {
+    rcl_interfaces::msg::SetParametersResult result;
+    result.successful = true;
+
+    for (const auto & param : parameters) {
+      if (param.get_name() == "exposure_time") {
+        exposure_time_ = param.as_int();
+        std::lock_guard<std::mutex> lock(camera_mutex_);
+        if (camera_handle_ != nullptr) {
+          int status =
+            MV_CC_SetFloatValue(camera_handle_, "ExposureTime", static_cast<float>(exposure_time_));
+          if (status != MV_OK) {
+            result.successful = false;
+            result.reason = "Failed to set exposure time, status = " + std::to_string(status);
+          } else {
+            FYT_INFO("camera_driver", "Set exposure_time: {}", exposure_time_);
+          }
+        }
+      } else if (param.get_name() == "gain") {
+        gain_ = param.as_double();
+        std::lock_guard<std::mutex> lock(camera_mutex_);
+        if (camera_handle_ != nullptr) {
+          int status = MV_CC_SetFloatValue(camera_handle_, "Gain", static_cast<float>(gain_));
+          if (status != MV_OK) {
+            result.successful = false;
+            result.reason = "Failed to set gain, status = " + std::to_string(status);
+          } else {
+            FYT_INFO("camera_driver", "Set gain: {}", gain_);
+          }
+        }
+      } else if (param.get_name() == "resolution_width") {
+        resolution_width_ = param.as_int();
+        std::lock_guard<std::mutex> lock(camera_mutex_);
+        if (camera_handle_ != nullptr) {
+          applyRoiLocked();
+        }
+      } else if (param.get_name() == "resolution_height") {
+        resolution_height_ = param.as_int();
+        std::lock_guard<std::mutex> lock(camera_mutex_);
+        if (camera_handle_ != nullptr) {
+          applyRoiLocked();
+        }
+      } else if (param.get_name() == "offsetX") {
+        offset_x_ = param.as_int();
+        std::lock_guard<std::mutex> lock(camera_mutex_);
+        if (camera_handle_ != nullptr) {
+          applyRoiLocked();
+        }
+      } else if (param.get_name() == "offsetY") {
+        offset_y_ = param.as_int();
+        std::lock_guard<std::mutex> lock(camera_mutex_);
+        if (camera_handle_ != nullptr) {
+          applyRoiLocked();
+        }
+      }
+    }
+
+    return result;
+  }
+
+  // ROS pub
+  sensor_msgs::msg::Image image_msg_;
+  image_transport::CameraPublisher camera_pub_;
+
+  // camera info
+  std::string camera_name_;
+  std::string camera_info_url_;
+  std::string frame_id_;
+  std::string pixel_format_;
+  std::unique_ptr<camera_info_manager::CameraInfoManager> camera_info_manager_;
+  sensor_msgs::msg::CameraInfo camera_info_msg_;
+
+  // Watch dog
+  rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::Time latest_frame_stamp_{0, 0, RCL_ROS_TIME};
+
+  // Heartbeat
+  fyt::HeartBeatPublisher::SharedPtr heartbeat_;
+
+  // Params
+  int frame_rate_{210};
+  int exposure_time_{5000};
+  double gain_{12.0};
+  int resolution_width_{1280};
+  int resolution_height_{1024};
+  int offset_x_{0};
+  int offset_y_{0};
+
+  // Recorder
+  std::unique_ptr<Recorder> recorder_;
+
+  // Camera SDK
+  void * camera_handle_{nullptr};
+  MV_IMAGE_BASIC_INFO img_info_{};
+  MV_CC_PIXEL_CONVERT_PARAM convert_param_{};
+
+  std::mutex camera_mutex_;
+
+  // State
+  std::atomic<bool> is_open_{false};
+  int fail_count_{0};
+  std::thread capture_thread_;
+  rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr params_callback_handle_;
+};
+}  // namespace fyt::camera_driver
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+RCLCPP_COMPONENTS_REGISTER_NODE(fyt::camera_driver::HikCameraNode)

src/rm_hardware_driver/ros2_hik_camera/src/recorder.cpp

@@ -0,0 +1,82 @@
+#include "hik_camera/recorder.hpp"
+
+#include <chrono>
+
+namespace fyt::camera_driver {
+Recorder::Recorder(const std::filesystem::path &file, int fps, cv::Size size)
+: path(file), size_(size), fps_(fps) {}
+
+Recorder::~Recorder() { stop(); }
+
+void Recorder::start() {
+  if (recording_.exchange(true)) {
+    return;
+  }
+
+  writer_.open(
+    path.string(), cv::VideoWriter::fourcc('M', 'J', 'P', 'G'), fps_, size_, true);
+
+  recorder_thread_ = std::thread(&Recorder::recorderThread, this);
+}
+
+void Recorder::stop() {
+  if (!recording_.exchange(false)) {
+    return;
+  }
+
+  cv_.notify_all();
+  if (recorder_thread_.joinable()) {
+    recorder_thread_.join();
+  }
+
+  if (writer_.isOpened()) {
+    writer_.release();
+  }
+}
+
+void Recorder::addFrame(const Frame &frame) {
+  if (!recording_.load()) {
+    return;
+  }
+
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    frame_queue_.push_back(frame);
+  }
+  cv_.notify_one();
+}
+
+void Recorder::recorderThread() {
+  while (recording_.load()) {
+    Frame frame;
+    {
+      std::unique_lock<std::mutex> lock(mutex_);
+      cv_.wait_for(lock, std::chrono::milliseconds(100), [this]() {
+        return !recording_.load() || !frame_queue_.empty();
+      });
+
+      if (!recording_.load()) {
+        break;
+      }
+      if (frame_queue_.empty()) {
+        continue;
+      }
+
+      frame = std::move(frame_queue_.front());
+      frame_queue_.pop_front();
+    }
+
+    if (!writer_.isOpened()) {
+      continue;
+    }
+
+    if (frame.size() != static_cast<size_t>(size_.width * size_.height * 3)) {
+      continue;
+    }
+
+    cv::Mat img(size_.height, size_.width, CV_8UC3, frame.data());
+    writer_.write(img);
+  }
+}
+
+}  // namespace fyt::camera_driver

src/rm_hardware_driver/ros2_hik_camera/src/video_player_node.cpp

@@ -0,0 +1,171 @@
+// Video player node for offline debugging
+
+#include <camera_info_manager/camera_info_manager.hpp>
+#include <image_transport/image_transport.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/image_encodings.hpp>
+#include <sensor_msgs/msg/camera_info.hpp>
+#include <sensor_msgs/msg/image.hpp>
+
+#include <chrono>
+#include <filesystem>
+#include <memory>
+#include <string>
+
+#include <opencv2/opencv.hpp>
+
+#include "rm_utils/heartbeat.hpp"
+#include "rm_utils/logger/log.hpp"
+
+namespace fyt::camera_driver {
+class VideoPlayerNode : public rclcpp::Node {
+public:
+  explicit VideoPlayerNode(const rclcpp::NodeOptions &options) : Node("camera_driver", options) {
+    FYT_REGISTER_LOGGER("camera_driver", "~/fyt2024-log", INFO);
+    FYT_INFO("camera_driver", "Starting VideoPlayerNode!");
+
+    video_path_ = this->declare_parameter("path", "/tmp/video.avi");
+    camera_info_url_ =
+      this->declare_parameter("camera_info_url", "package://rm_bringup/config/camera_info.yaml");
+    frame_id_ = this->declare_parameter("frame_id", "camera_optical_frame");
+    frame_rate_ = this->declare_parameter("frame_rate", 30);
+    start_frame_ = this->declare_parameter("start_frame", 0);
+    keep_looping_ = this->declare_parameter("keep_looping", true);
+
+    heartbeat_ = fyt::HeartBeatPublisher::create(this);
+
+    const std::filesystem::path video_file(video_path_);
+    if (!std::filesystem::exists(video_file)) {
+      FYT_ERROR("camera_driver", "Video file {} does not exist!", video_file.string());
+      rclcpp::shutdown();
+      return;
+    }
+
+    cap_.open(video_path_);
+    if (!cap_.isOpened()) {
+      FYT_ERROR("camera_driver", "Video file open failed: {}", video_path_);
+      rclcpp::shutdown();
+      return;
+    }
+
+    image_msg_.header.frame_id = frame_id_;
+    image_msg_.encoding = sensor_msgs::image_encodings::BGR8;
+
+    image_msg_.width = static_cast<uint32_t>(cap_.get(cv::CAP_PROP_FRAME_WIDTH));
+    image_msg_.height = static_cast<uint32_t>(cap_.get(cv::CAP_PROP_FRAME_HEIGHT));
+    image_msg_.step = image_msg_.width * 3;
+    image_msg_.data.resize(static_cast<size_t>(image_msg_.step) * image_msg_.height);
+
+    camera_info_manager_ = std::make_shared<camera_info_manager::CameraInfoManager>(
+      this, "camera_driver", "file://" + video_path_);
+
+    if (camera_info_manager_->validateURL(camera_info_url_)) {
+      camera_info_manager_->loadCameraInfo(camera_info_url_);
+      camera_info_ = camera_info_manager_->getCameraInfo();
+    } else {
+      camera_info_manager_->setCameraName(video_path_);
+      sensor_msgs::msg::CameraInfo camera_info;
+      camera_info.header.frame_id = frame_id_;
+      camera_info.header.stamp = this->now();
+      camera_info.width = image_msg_.width;
+      camera_info.height = image_msg_.height;
+      camera_info_manager_->setCameraInfo(camera_info);
+      FYT_WARN("camera_driver", "Invalid camera info URL: {}", camera_info_url_);
+    }
+
+    camera_info_.header.frame_id = frame_id_;
+    camera_info_.width = image_msg_.width;
+    camera_info_.height = image_msg_.height;
+
+    bool use_sensor_data_qos = this->declare_parameter("use_sensor_data_qos", true);
+    auto qos = use_sensor_data_qos ? rmw_qos_profile_sensor_data : rmw_qos_profile_default;
+    camera_pub_ = image_transport::create_camera_publisher(this, "image_raw", qos);
+
+    const int publish_fps = (frame_rate_ > 0) ? frame_rate_ : 30;
+    const auto period = std::chrono::duration_cast<std::chrono::nanoseconds>(
+      std::chrono::duration<double>(1.0 / static_cast<double>(publish_fps)));
+
+    timer_ = this->create_wall_timer(period, std::bind(&VideoPlayerNode::onTimer, this));
+
+    FYT_INFO(
+      "camera_driver", "Video player ready: {} ({}x{}, {} fps)", video_path_, image_msg_.width,
+      image_msg_.height, publish_fps);
+  }
+
+private:
+  void onTimer() {
+    if (!cap_.isOpened()) {
+      return;
+    }
+
+    cv::Mat frame;
+    cap_ >> frame;
+
+    if (frame.empty()) {
+      FYT_INFO("camera_driver", "Video file ends!");
+      if (!keep_looping_) {
+        rclcpp::shutdown();
+        return;
+      }
+
+      cap_.release();
+      cap_.open(video_path_);
+      frame_cnt_ = 0;
+      return;
+    }
+
+    frame_cnt_++;
+    if (frame_cnt_ < start_frame_) {
+      return;
+    }
+
+    if (frame.cols != static_cast<int>(image_msg_.width) ||
+        frame.rows != static_cast<int>(image_msg_.height) || frame.type() != CV_8UC3) {
+      cv::Mat bgr;
+      if (frame.channels() == 3) {
+        bgr = frame;
+      } else if (frame.channels() == 1) {
+        cv::cvtColor(frame, bgr, cv::COLOR_GRAY2BGR);
+      } else {
+        frame.convertTo(bgr, CV_8UC3);
+      }
+      if (bgr.cols != static_cast<int>(image_msg_.width) || bgr.rows != static_cast<int>(image_msg_.height)) {
+        cv::resize(bgr, bgr, cv::Size(static_cast<int>(image_msg_.width), static_cast<int>(image_msg_.height)));
+      }
+      std::memcpy(image_msg_.data.data(), bgr.data, image_msg_.data.size());
+    } else {
+      std::memcpy(image_msg_.data.data(), frame.data, image_msg_.data.size());
+    }
+
+    auto stamp = this->now();
+    image_msg_.header.stamp = stamp;
+    camera_info_.header.stamp = stamp;
+    camera_info_.header.frame_id = frame_id_;
+    image_msg_.header.frame_id = frame_id_;
+
+    camera_pub_.publish(image_msg_, camera_info_);
+  }
+
+  fyt::HeartBeatPublisher::SharedPtr heartbeat_;
+
+  std::string video_path_;
+  std::string camera_info_url_;
+  std::string frame_id_;
+  int frame_rate_{30};
+  int start_frame_{0};
+  bool keep_looping_{true};
+  int frame_cnt_{0};
+
+  cv::VideoCapture cap_;
+
+  image_transport::CameraPublisher camera_pub_;
+  sensor_msgs::msg::Image image_msg_;
+  sensor_msgs::msg::CameraInfo camera_info_;
+  std::shared_ptr<camera_info_manager::CameraInfoManager> camera_info_manager_;
+
+  rclcpp::TimerBase::SharedPtr timer_;
+};
+}  // namespace fyt::camera_driver
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(fyt::camera_driver::VideoPlayerNode)