gc更改前的推送

src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver.cc

@@ -1009,8 +1009,8 @@ namespace lslidar_driver
 					scan->ranges.assign(scan_num, std::numeric_limits<float>::infinity());
 					scan->intensities.reserve(scan_num);
 					scan->intensities.assign(scan_num, std::numeric_limits<float>::infinity());
-					// scan->scan_time = scan_time;
+					scan->scan_time = scan_time;
-					// scan->time_increment = scan_time / (double)(count_num);
+					scan->time_increment = scan_time / static_cast<double>(scan_num);
 
 					for (int k = 0; k < scan_num; k++)
 					{
@@ -1168,8 +1168,8 @@ namespace lslidar_driver
 					scan->ranges.assign(scan_num, std::numeric_limits<float>::infinity());
 					scan->intensities.reserve(scan_num);
 					scan->intensities.assign(scan_num, std::numeric_limits<float>::infinity());
-					scan->scan_time = 0.1;
+					scan->scan_time = scan_time;
-					scan->time_increment = 0.1 / (double)(scan_num - 1);
+					scan->time_increment = scan_time / static_cast<double>(scan_num - 1);
 
 					int start_num = floor(angle_able_min * count_num / 360);
 					int end_num = floor(angle_able_max * count_num / 360);

src/map/nav2_costmap_map.yaml

@@ -0,0 +1,7 @@
+image: nav2_costmap_binary.png
+mode: trinary
+resolution: 0.01
+origin: [0.0, 0.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.25

src/navigation/obstacle_nav2/config/nav2_params.yaml

@@ -170,7 +170,7 @@ controller_server_rclcpp_node:
 local_costmap:
   local_costmap:
     ros__parameters:
-      update_frequency: 8.0
+      update_frequency: 10.0
       publish_frequency: 4.0
       transform_tolerance: 0.5
       global_frame: odom
@@ -189,7 +189,7 @@ local_costmap:
         enabled: true
         topic: /obstacles
         obstacle_timeout: 1.0
-        transform_tolerance: 0.2
+        transform_tolerance: 0.02
         default_obstacle_radius: 0.05
         minimum_obstacle_radius: 0.02
         maximum_obstacle_radius: 0.06
@@ -229,7 +229,7 @@ global_costmap:
         enabled: true
         topic: /obstacles
         obstacle_timeout: 1.0
-        transform_tolerance: 0.2
+        transform_tolerance: 0.02
         default_obstacle_radius: 0.05
         minimum_obstacle_radius: 0.02
         maximum_obstacle_radius: 0.50

src/navigation/obstacle_nav2/src/obstacle_array_layer.cpp

@@ -54,7 +54,7 @@ void ObstacleArrayLayer::onInitialize()
   node->declare_parameter(name_ + ".enabled", true);
   node->declare_parameter(name_ + ".topic", std::string("/obstacles"));
   node->declare_parameter(name_ + ".obstacle_timeout", 0.5);
-  node->declare_parameter(name_ + ".transform_tolerance", 0.2);
+  node->declare_parameter(name_ + ".transform_tolerance", 0.02);
   node->declare_parameter(name_ + ".default_obstacle_radius", 0.05);
   node->declare_parameter(name_ + ".minimum_obstacle_radius", 0.02);
   node->declare_parameter(name_ + ".maximum_obstacle_radius", 0.50);
@@ -82,7 +82,7 @@ void ObstacleArrayLayer::onInitialize()
   rclcpp::SubscriptionOptions subscription_options;
   subscription_options.callback_group = callback_group_;
   obstacle_sub_ = node->create_subscription<obstacle_scanner::msg::ObstacleArray>(
-    topic_, rclcpp::QoS(10).reliable(),
+    topic_, rclcpp::QoS(rclcpp::KeepLast(1)).reliable(),
     std::bind(&ObstacleArrayLayer::obstacleCallback, this, std::placeholders::_1),
     subscription_options);
   callback_executor_ = std::make_unique<rclcpp::executors::SingleThreadedExecutor>();
@@ -91,9 +91,7 @@ void ObstacleArrayLayer::onInitialize()
   callback_stop_.store(false, std::memory_order_release);
   callback_thread_ = std::thread(
     [this]() {
-      while (!callback_stop_.load(std::memory_order_acquire)) {
+      callback_executor_->spin();
-        callback_executor_->spin_once(std::chrono::milliseconds(100));
-      }
     });
 
   current_ = true;

src/obstacle_scanner/src/obstacle_scanner_node.cpp

@@ -63,7 +63,7 @@ public:
       1, static_cast<int>(declare_parameter<int>("debug_info_stride", 1)));
 
     obstacles_pub_ = create_publisher<obstacle_scanner::msg::ObstacleArray>(
-      "/obstacles", 10);
+      "/obstacles", rclcpp::QoS(rclcpp::KeepLast(1)).reliable());
 
     if (debug_) {
       debug_pub_ = create_publisher<sensor_msgs::msg::Image>(

src/origincar_base/CMakeLists.txt

@@ -46,6 +46,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/Position.msg"
  )
 if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
   find_package(ament_lint_auto REQUIRED)
   # the following line skips the linter which checks for copyrights
   # uncomment the line when a copyright and license is not present in all source files
@@ -112,6 +113,15 @@ if(BUILD_TESTING)
   add_executable(scan_odom_timing_logger_test test/scan_odom_timing_logger_test.cpp)
   target_link_libraries(scan_odom_timing_logger_test origincar_base_log)
   add_test(NAME scan_odom_timing_logger_test COMMAND scan_odom_timing_logger_test)
+
+  ament_add_gtest(command_gate_test test/command_gate_test.cpp)
+  target_include_directories(command_gate_test PRIVATE include)
+
+  ament_add_gtest(command_frame_test test/command_frame_test.cpp)
+  target_include_directories(command_frame_test PRIVATE include)
+
+  ament_add_gtest(serial_frame_parser_test test/serial_frame_parser_test.cpp)
+  target_include_directories(serial_frame_parser_test PRIVATE include)
 endif()
 
 #add_executable(testNode src/test.cpp src/Quaternion_Solution.cpp)

src/origincar_base/include/origincar_base/command_frame.hpp

@@ -0,0 +1,43 @@
+#ifndef ORIGINCAR_BASE__COMMAND_FRAME_HPP_
+#define ORIGINCAR_BASE__COMMAND_FRAME_HPP_
+
+#include <array>
+#include <cstddef>
+#include <cstdint>
+
+#include "origincar_base/command_gate.hpp"
+
+namespace origincar_base_core
+{
+
+inline void writeInt16(std::array<uint8_t, 11> * frame, std::size_t high_index, int16_t value)
+{
+  const auto bits = static_cast<uint16_t>(value);
+  (*frame)[high_index] = static_cast<uint8_t>(bits >> 8);
+  (*frame)[high_index + 1] = static_cast<uint8_t>(bits & 0xFF);
+}
+
+inline std::array<uint8_t, 11> encodeCommandFrame(const Command & command)
+{
+  std::array<uint8_t, 11> frame{};
+  frame[0] = 0x7B;
+
+  writeInt16(&frame, 3, static_cast<int16_t>(command.linear_x * 1000.0));
+  if (command.ackermann) {
+    writeInt16(&frame, 5, 0);
+    writeInt16(&frame, 7, static_cast<int16_t>(command.steering_angle * 500.0));
+  } else {
+    writeInt16(&frame, 5, static_cast<int16_t>(command.linear_y * 1000.0));
+    writeInt16(&frame, 7, static_cast<int16_t>(command.angular_z * 1000.0));
+  }
+
+  for (std::size_t index = 0; index < 9; ++index) {
+    frame[9] ^= frame[index];
+  }
+  frame[10] = 0x7D;
+  return frame;
+}
+
+}  // namespace origincar_base_core
+
+#endif  // ORIGINCAR_BASE__COMMAND_FRAME_HPP_

src/origincar_base/include/origincar_base/command_gate.hpp

@@ -0,0 +1,63 @@
+#ifndef ORIGINCAR_BASE__COMMAND_GATE_HPP_
+#define ORIGINCAR_BASE__COMMAND_GATE_HPP_
+
+#include <chrono>
+#include <mutex>
+
+namespace origincar_base_core
+{
+
+struct Command
+{
+  double linear_x{0.0};
+  double linear_y{0.0};
+  double angular_z{0.0};
+  double steering_angle{0.0};
+  bool ackermann{false};
+};
+
+inline bool operator==(const Command & lhs, const Command & rhs)
+{
+  return lhs.linear_x == rhs.linear_x &&
+         lhs.linear_y == rhs.linear_y &&
+         lhs.angular_z == rhs.angular_z &&
+         lhs.steering_angle == rhs.steering_angle &&
+         lhs.ackermann == rhs.ackermann;
+}
+
+class CommandGate
+{
+public:
+  explicit CommandGate(std::chrono::milliseconds timeout)
+  : timeout_(timeout)
+  {
+  }
+
+  void update(const Command & command, std::chrono::steady_clock::time_point now)
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    latest_command_ = command;
+    last_command_time_ = now;
+    has_command_ = true;
+  }
+
+  Command commandAt(std::chrono::steady_clock::time_point now) const
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (!has_command_ || now - last_command_time_ >= timeout_) {
+      return {};
+    }
+    return latest_command_;
+  }
+
+private:
+  const std::chrono::milliseconds timeout_;
+  mutable std::mutex mutex_;
+  Command latest_command_{};
+  std::chrono::steady_clock::time_point last_command_time_{};
+  bool has_command_{false};
+};
+
+}  // namespace origincar_base_core
+
+#endif  // ORIGINCAR_BASE__COMMAND_GATE_HPP_

src/origincar_base/include/origincar_base/origincar_base.h

@@ -1,6 +1,8 @@
 #ifndef _ORIGINCAR_BASE_H_
 #define _ORIGINCAR_BASE_H_
 
+#include <atomic>
+#include <chrono>
 #include <memory>
 #include <inttypes.h>
 #include <array>
@@ -10,6 +12,8 @@
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/string.hpp"
 #include "origincar_base/log.hpp"
+#include "origincar_base/command_gate.hpp"
+#include "origincar_base/serial_frame_parser.hpp"
 #include "origincar_base/wall_fit_core.hpp"
 #include "origincar_base/wall_kalman_filter.hpp"
 #include <csignal>
@@ -154,6 +158,10 @@ private:
 	void Scan_Callback(const sensor_msgs::msg::LaserScan::SharedPtr scan);
 	void Apply_Wall_Update();
 	void Print_Timing_Log_If_Due();
+	void Sensor_Receive_Loop();
+	void Control_Timer_Callback();
+	void Tx_Timer_Callback();
+	void Send_Command(const origincar_base_core::Command & command);
 
 	bool Get_Sensor_Data();
 	unsigned char Check_Sum(unsigned char Count_Number, unsigned char mode);
@@ -215,6 +223,7 @@ private:
 	rclcpp::TimerBase::SharedPtr test_timer;
 
 	rclcpp::TimerBase::SharedPtr odom_timer;
+	rclcpp::TimerBase::SharedPtr tx_timer;
 	rclcpp::TimerBase::SharedPtr imu_timer;
 	rclcpp::TimerBase::SharedPtr voltage_timer;
 
@@ -232,6 +241,10 @@ private:
 	int wall_scan_stride_;
 	std::string cmd_vel;
 	int serial_baud_rate;
+	int cmd_watchdog_timeout_ms_;
+	int tx_period_ms_;
+	int serial_read_timeout_ms_;
+	int control_period_ms_;
 	RECEIVE_DATA Receive_Data;
 	SEND_DATA Send_Data;
 
@@ -264,6 +277,14 @@ private:
 	bool latest_scan_consumed_;
 	bool has_latest_scan_timing_frame_;
 	bool has_pending_odom_timing_frame_;
+	std::unique_ptr<origincar_base_core::CommandGate> command_gate_;
+	origincar_base_core::SerialFrameParser serial_frame_parser_;
+	std::mutex serial_mutex_;
+	std::mutex sensor_data_mutex_;
+	std::thread sensor_thread_;
+	std::atomic<bool> sensor_thread_stop_{true};
+	std::atomic<uint64_t> sensor_sequence_{0};
+	uint64_t last_processed_sensor_sequence_{0};
 };
 
 #endif //_ORIGINCAR_BASE_H_

src/origincar_base/include/origincar_base/serial_frame_parser.hpp

@@ -0,0 +1,56 @@
+#ifndef ORIGINCAR_BASE__SERIAL_FRAME_PARSER_HPP_
+#define ORIGINCAR_BASE__SERIAL_FRAME_PARSER_HPP_
+
+#include <algorithm>
+#include <array>
+#include <cstddef>
+#include <cstdint>
+#include <vector>
+
+namespace origincar_base_core
+{
+
+class SerialFrameParser
+{
+public:
+  static constexpr std::size_t kFrameSize = 24;
+
+  static constexpr uint8_t frameHeader() {return 0x7B;}
+  static constexpr uint8_t frameTail() {return 0x7D;}
+
+  void append(const uint8_t * data, std::size_t size)
+  {
+    buffer_.insert(buffer_.end(), data, data + size);
+  }
+
+  bool popFrame(std::array<uint8_t, kFrameSize> * frame)
+  {
+    while (!buffer_.empty()) {
+      const auto header = std::find(buffer_.begin(), buffer_.end(), frameHeader());
+      if (header == buffer_.end()) {
+        buffer_.clear();
+        return false;
+      }
+      buffer_.erase(buffer_.begin(), header);
+      if (buffer_.size() < kFrameSize) {
+        return false;
+      }
+      if (buffer_[kFrameSize - 1] != frameTail()) {
+        buffer_.erase(buffer_.begin());
+        continue;
+      }
+
+      std::copy_n(buffer_.begin(), kFrameSize, frame->begin());
+      buffer_.erase(buffer_.begin(), buffer_.begin() + kFrameSize);
+      return true;
+    }
+    return false;
+  }
+
+private:
+  std::vector<uint8_t> buffer_;
+};
+
+}  // namespace origincar_base_core
+
+#endif  // ORIGINCAR_BASE__SERIAL_FRAME_PARSER_HPP_

src/origincar_base/launch/base_serial.launch.py

@@ -6,6 +6,10 @@ def generate_launch_description():
     robot_parameters = [
         {'usart_port_name': '/dev/ttyACM0',
          'serial_baud_rate': 921600,
+         'serial_read_timeout_ms': 20,
+         'tx_period_ms': 20,
+         'cmd_watchdog_timeout_ms': 150,
+         'control_period_ms': 50,
          'robot_frame_id': 'base_footprint',
          'odom_frame_id': 'odom',
          'combined_odom_topic': 'odom_combined',

src/origincar_base/package.xml

@@ -11,6 +11,7 @@
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
   <build_export_depend>tf2_geometry_msgs</build_export_depend>
 
 

src/origincar_base/src/origincar_base.cpp

@@ -1,4 +1,5 @@
 #include "origincar_base/origincar_base.h"
+#include "origincar_base/command_frame.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "ackermann_msgs/msg/ackermann_drive_stamped.hpp"
 #include "origincar_msg/msg/data.hpp"
@@ -94,78 +95,26 @@ float origincar_base::Odom_Trans(uint8_t Data_High, uint8_t Data_Low)
 
 void origincar_base::Akm_Cmd_Vel_Callback(const ackermann_msgs::msg::AckermannDriveStamped::SharedPtr akm_ctl)
 {
-  short transition;
+  origincar_base_core::Command command;
-  std::cout << "linerx" << akm_ctl->drive.speed << std::endl;
+  command.linear_x = akm_ctl->drive.speed;
-  std::cout << "angular" << akm_ctl->drive.steering_angle << std::endl;
+  command.steering_angle = akm_ctl->drive.steering_angle;
-
+  command.ackermann = true;
-  Send_Data.tx[0] = FRAME_HEADER;
+  command_gate_->update(command, std::chrono::steady_clock::now());
-  Send_Data.tx[1] = 0;
-  Send_Data.tx[2] = 0;
-
-  transition = 0;
-  transition = akm_ctl->drive.speed * 1000;
-  Send_Data.tx[4] = transition;
-  Send_Data.tx[3] = transition >> 8;
-
-  transition = 0;
-  transition = akm_ctl->drive.steering_angle * 1000 / 2;
-  Send_Data.tx[8] = transition;
-  Send_Data.tx[7] = transition >> 8;
-
-  Send_Data.tx[9] = Check_Sum(9, SEND_DATA_CHECK);
-  Send_Data.tx[10] = FRAME_TAIL;
-
-  try
-  {
-    Stm32_Serial.write(Send_Data.tx, sizeof(Send_Data.tx));
-  }
-  catch (serial::IOException &e)
-  {
-    RCLCPP_ERROR(this->get_logger(), ("Unable to send data through serial port"));
-  }
 }
 
 void origincar_base::Cmd_Vel_Callback(const geometry_msgs::msg::Twist::SharedPtr twist_aux)
 {
-//   RCLCPP_INFO(this->get_logger(), "linarx: %.2f, angularz: %.2f ", twist_aux->linear.x, twist_aux->angular.z);
+  origincar_base_core::Command command;
-  std::cout << "linerx" << twist_aux->linear.x << std::endl;
+  command.linear_x = twist_aux->linear.x;
-  std::cout << "angular" << twist_aux->angular.z << std::endl;
+  command.linear_y = twist_aux->linear.y;
-  short transition;
+  command.angular_z = twist_aux->angular.z;
-  Send_Data.tx[0] = FRAME_HEADER;
+  command_gate_->update(command, std::chrono::steady_clock::now());
-  Send_Data.tx[1] = 0;
-  Send_Data.tx[2] = 0;
-
-  transition = 0;
-  transition = twist_aux->linear.x * 1000;
-  Send_Data.tx[4] = transition;
-  Send_Data.tx[3] = transition >> 8;
-
-  transition = 0;
-  transition = twist_aux->linear.y * 1000;
-  Send_Data.tx[6] = transition;
-  Send_Data.tx[5] = transition >> 8;
-
-  transition = 0;
-  transition = (twist_aux->angular.z) * 1000;
-  Send_Data.tx[8] = transition;
-  Send_Data.tx[7] = transition >> 8;
-
-  Send_Data.tx[9] = Check_Sum(9, SEND_DATA_CHECK);
-  Send_Data.tx[10] = FRAME_TAIL;
-
-  try
-  {
-    Stm32_Serial.write(Send_Data.tx, sizeof(Send_Data.tx));
-  }
-  catch (serial::IOException &e)
-  {
-    RCLCPP_ERROR(this->get_logger(), ("Unable to send data through serial port"));
-  }
 }
 
 void origincar_base::Sign_Switch_Callback(const std_msgs::msg::Int32::SharedPtr sign_switch)
 {
   (void)sign_switch;
+  std::lock_guard<std::mutex> lock(sensor_data_mutex_);
   if (sign_switch->data == -1)
   {
     memset(&Robot_Pos, 0, sizeof(Robot_Pos));
@@ -202,6 +151,7 @@ void origincar_base::Sign_Switch_Callback(const std_msgs::msg::Int32::SharedPtr
 
 void origincar_base::Publish_ImuSensor()
 {
+  std::lock_guard<std::mutex> lock(sensor_data_mutex_);
   tf2::Quaternion q;
   q.setRPY(0.0, 0.0, Robot_Pos.Z);
 
@@ -227,6 +177,7 @@ void origincar_base::Publish_ImuSensor()
 
 void origincar_base::Publish_Odom()
 {
+  std::lock_guard<std::mutex> lock(sensor_data_mutex_);
   tf2::Quaternion q;
   q.setRPY(0, 0, Robot_Pos.Z);
   geometry_msgs::msg::Quaternion odom_quat = tf2::toMsg(q);
@@ -365,6 +316,7 @@ void origincar_base::Print_Timing_Log_If_Due()
 
 void origincar_base::Publish_Voltage()
 {
+  std::lock_guard<std::mutex> lock(sensor_data_mutex_);
   std_msgs::msg::Float32 voltage_msgs;
   static float Count_Voltage_Pub = 0;
 
@@ -378,6 +330,7 @@ void origincar_base::Publish_Voltage()
 
 void origincar_base::Publish_GyroDebug()
 {
+  std::lock_guard<std::mutex> lock(sensor_data_mutex_);
   origincar_msg::msg::Data gyro_debug;
   gyro_debug.x = gyro_z_filtered_pre_bias_;
   gyro_debug.y = gyro_z_bias_model_;
@@ -411,14 +364,37 @@ bool origincar_base::Get_Sensor_Data()
 {
   short transition_16 = 0, j = 0, Header_Pos = 0, Tail_Pos = 0;
   uint8_t Receive_Data_Pr[RECEIVE_DATA_SIZE] = {0};
+  std::array<uint8_t, RECEIVE_DATA_SIZE> frame{};
+  if (!serial_frame_parser_.popFrame(&frame)) {
     try
     {
-    Stm32_Serial.read(Receive_Data_Pr, sizeof(Receive_Data_Pr));
+      std::string incoming;
-  }
-  catch (const serial::SerialException &e)
       {
+        std::lock_guard<std::mutex> serial_lock(serial_mutex_);
+        const auto available = Stm32_Serial.available();
+        if (available == 0) {
           return false;
         }
+        incoming = Stm32_Serial.read(std::min<std::size_t>(available, 256));
+      }
+      if (incoming.empty()) {
+        return false;
+      }
+      serial_frame_parser_.append(
+          reinterpret_cast<const uint8_t *>(incoming.data()), incoming.size());
+    }
+    catch (const std::exception &e)
+    {
+      RCLCPP_ERROR_THROTTLE(
+          this->get_logger(), *this->get_clock(), 2000,
+          "Unable to read STM32 serial data: %s", e.what());
+      return false;
+    }
+    if (!serial_frame_parser_.popFrame(&frame)) {
+      return false;
+    }
+  }
+  std::copy(frame.begin(), frame.end(), Receive_Data_Pr);
   for (j = 0; j < 24; j++)
   {
     if (Receive_Data_Pr[j] == FRAME_HEADER)
@@ -441,6 +417,7 @@ bool origincar_base::Get_Sensor_Data()
     return false;
   }
 
+  std::lock_guard<std::mutex> sensor_lock(sensor_data_mutex_);
   Receive_Data.Frame_Header = Receive_Data.rx[0];
   Receive_Data.Frame_Tail = Receive_Data.rx[23];
   if (Receive_Data.Frame_Header == FRAME_HEADER)
@@ -529,6 +506,7 @@ bool origincar_base::Get_Sensor_Data()
         transition_16 |= Receive_Data.rx[21];
         Power_voltage = transition_16 / 1000 + (transition_16 % 1000) * 0.001;
 
+        sensor_sequence_.fetch_add(1, std::memory_order_release);
         return true;
       }
     }
@@ -539,31 +517,85 @@ bool origincar_base::Get_Sensor_Data()
 
 void origincar_base::Control()
 {
-  rclcpp::Time current_time, last_time;
+  sensor_thread_stop_.store(false, std::memory_order_release);
-  current_time = rclcpp::Node::now();
+  sensor_thread_ = std::thread(&origincar_base::Sensor_Receive_Loop, this);
-  last_time = rclcpp::Node::now();
+  rclcpp::spin(this->get_node_base_interface());
-  while (rclcpp::ok())
+  sensor_thread_stop_.store(true, std::memory_order_release);
+  if (sensor_thread_.joinable()) {
+    sensor_thread_.join();
+  }
+}
+
+void origincar_base::Sensor_Receive_Loop()
+{
+  while (rclcpp::ok() && !sensor_thread_stop_.load(std::memory_order_acquire)) {
+    if (!Get_Sensor_Data()) {
+      std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    }
+  }
+}
+
+void origincar_base::Control_Timer_Callback()
+{
+  const auto sequence = sensor_sequence_.load(std::memory_order_acquire);
+  if (sequence == 0 || sequence == last_processed_sensor_sequence_) {
+    return;
+  }
+
+  const auto current_time = rclcpp::Node::now();
+  Sampling_Time = (current_time - _Last_Time).seconds();
+  if (Sampling_Time <= 0.0f || Sampling_Time > 0.5f) {
+    Sampling_Time = static_cast<float>(control_period_ms_) / 1000.0f;
+  }
+  _Last_Time = current_time;
+
+  Vel_Pos_Data velocity;
   {
-    current_time = rclcpp::Node::now();
+    std::lock_guard<std::mutex> lock(sensor_data_mutex_);
-    Sampling_Time = (current_time - last_time).seconds();
+    velocity = Robot_Vel;
-    if (true == Get_Sensor_Data())
+  }
-    {
-      rclcpp::spin_some(this->get_node_base_interface());
   if (wall_filter_)
   {
-        wall_filter_->predict(1.03 * Robot_Vel.X, 1.01 * Robot_Vel.Y, Robot_Vel.Z, Sampling_Time);
+    wall_filter_->predict(
+        1.03 * velocity.X, 1.01 * velocity.Y, velocity.Z, Sampling_Time);
     Apply_Wall_Update();
     const auto fused_pose = wall_filter_->pose();
+    std::lock_guard<std::mutex> lock(sensor_data_mutex_);
     Robot_Pos.X = static_cast<float>(fused_pose.x);
     Robot_Pos.Y = static_cast<float>(fused_pose.y);
     Robot_Pos.Z = static_cast<float>(fused_pose.theta);
   }
+
   Publish_ImuSensor();
   Publish_GyroDebug();
   Publish_Voltage();
   Publish_Odom();
+  last_processed_sensor_sequence_ = sequence;
+}
+
+void origincar_base::Tx_Timer_Callback()
+{
+  if (!command_gate_) {
+    return;
   }
-    last_time = current_time;
+  Send_Command(command_gate_->commandAt(std::chrono::steady_clock::now()));
+}
+
+void origincar_base::Send_Command(const origincar_base_core::Command & command)
+{
+  const auto frame = origincar_base_core::encodeCommandFrame(command);
+  try
+  {
+    std::lock_guard<std::mutex> lock(serial_mutex_);
+    if (Stm32_Serial.isOpen()) {
+      Stm32_Serial.write(frame.data(), frame.size());
+    }
+  }
+  catch (const std::exception &e)
+  {
+    RCLCPP_ERROR_THROTTLE(
+        this->get_logger(), *this->get_clock(), 2000,
+        "Unable to send STM32 command: %s", e.what());
   }
 }
 
@@ -612,6 +644,10 @@ origincar_base::origincar_base()
   this->declare_parameter<double>("laser_y", 0.0);
   this->declare_parameter<double>("laser_yaw", 0.0);
   this->declare_parameter<double>("gyro_z_low_pass_alpha", kDefaultGyroZLowPassAlpha);
+  this->declare_parameter<int>("cmd_watchdog_timeout_ms", 150);
+  this->declare_parameter<int>("tx_period_ms", 20);
+  this->declare_parameter<int>("serial_read_timeout_ms", 20);
+  this->declare_parameter<int>("control_period_ms", 50);
 
   // Odom covariance parameters (tunable via YAML)
   this->declare_parameter<double>("odom_pose_cov_x", 0.01);
@@ -648,6 +684,16 @@ origincar_base::origincar_base()
   this->get_parameter("odom_pose_cov_x", odom_pose_cov_x_);
   this->get_parameter("odom_pose_cov_y", odom_pose_cov_y_);
   this->get_parameter("odom_pose_cov_yaw", odom_pose_cov_yaw_);
+  this->get_parameter("cmd_watchdog_timeout_ms", cmd_watchdog_timeout_ms_);
+  this->get_parameter("tx_period_ms", tx_period_ms_);
+  this->get_parameter("serial_read_timeout_ms", serial_read_timeout_ms_);
+  this->get_parameter("control_period_ms", control_period_ms_);
+  cmd_watchdog_timeout_ms_ = std::max(1, cmd_watchdog_timeout_ms_);
+  tx_period_ms_ = std::max(1, tx_period_ms_);
+  serial_read_timeout_ms_ = std::max(1, serial_read_timeout_ms_);
+  control_period_ms_ = std::max(1, control_period_ms_);
+  command_gate_ = std::make_unique<origincar_base_core::CommandGate>(
+      std::chrono::milliseconds(cmd_watchdog_timeout_ms_));
 
   wall_fit_config_ = ::origincar_wall::loadWallFitConfig(wall_config_path_);
   const auto initial_pose = ::origincar_wall::Pose2D{
@@ -691,7 +737,7 @@ origincar_base::origincar_base()
   {
     Stm32_Serial.setPort(usart_port_name);
     Stm32_Serial.setBaudrate(serial_baud_rate);
-    serial::Timeout _time = serial::Timeout::simpleTimeout(2000);
+    serial::Timeout _time = serial::Timeout::simpleTimeout(serial_read_timeout_ms_);
     Stm32_Serial.setTimeout(_time);
     Stm32_Serial.open();
   }
@@ -711,55 +757,27 @@ origincar_base::origincar_base()
   {
     RCLCPP_WARN(this->get_logger(), "scan_to_odom timing log file could not be opened: %s", scan_odom_timing_logger_.logPath().c_str());
   }
+  _Last_Time = rclcpp::Node::now();
+  tx_timer = create_wall_timer(
+      std::chrono::milliseconds(tx_period_ms_),
+      std::bind(&origincar_base::Tx_Timer_Callback, this));
+  odom_timer = create_wall_timer(
+      std::chrono::milliseconds(control_period_ms_),
+      std::bind(&origincar_base::Control_Timer_Callback, this));
 }
 
 void sigintHandler(int sig)
 {
-  sig = sig;
+  (void)sig;
-  printf("OriginBot shutdown...\n");
-  serial::Serial Stm32_Serial;
-  Stm32_Serial.setPort("/dev/ttyACM0");
-  Stm32_Serial.setBaudrate(921600);
-  serial::Timeout _time = serial::Timeout::simpleTimeout(2000);
-  Stm32_Serial.setTimeout(_time);
-  Stm32_Serial.open();
-  SEND_DATA Send_Data;
-  if (Stm32_Serial.isOpen())
-  {
-    Send_Data.tx[0] = FRAME_HEADER;
-    Send_Data.tx[1] = 0;
-    Send_Data.tx[2] = 0;
-
-    Send_Data.tx[4] = 0;
-    Send_Data.tx[3] = 0;
-
-    Send_Data.tx[6] = 0;
-    Send_Data.tx[5] = 0;
-
-    Send_Data.tx[7] = 0;
-    Send_Data.tx[8] = 0;
-    int check_sum = 0;
-    for (int k = 0; k < 9; k++)
-    {
-      check_sum = check_sum ^ Send_Data.tx[k];
-    }
-    Send_Data.tx[9] = check_sum;
-    Send_Data.tx[10] = FRAME_TAIL;
-
-    try
-    {
-      Stm32_Serial.write(Send_Data.tx, sizeof(Send_Data.tx));
-    }
-    catch (serial::IOException &e)
-    {
-    }
-  }
-  // Shutdown ROS2 and release resources.
   rclcpp::shutdown();
 }
 
 origincar_base::~origincar_base()
 {
+  sensor_thread_stop_.store(true, std::memory_order_release);
+  if (sensor_thread_.joinable()) {
+    sensor_thread_.join();
+  }
+  Send_Command(origincar_base_core::Command{});
   RCLCPP_INFO(this->get_logger(), "Shutting down");
 }
-

src/origincar_base/test/command_frame_test.cpp

@@ -0,0 +1,64 @@
+#include <array>
+#include <cstdint>
+
+#include <gtest/gtest.h>
+
+#include "origincar_base/command_frame.hpp"
+
+using origincar_base_core::Command;
+using origincar_base_core::encodeCommandFrame;
+
+namespace
+{
+uint8_t checksum(const std::array<uint8_t, 11> & frame)
+{
+  uint8_t value = 0;
+  for (std::size_t index = 0; index < 9; ++index) {
+    value ^= frame[index];
+  }
+  return value;
+}
+}  // namespace
+
+TEST(CommandFrameTest, EncodesTwistCommandUsingExistingProtocol)
+{
+  const auto frame = encodeCommandFrame(Command{0.25, -0.1, 0.4});
+
+  EXPECT_EQ(frame[0], 0x7B);
+  EXPECT_EQ(frame[3], 0x00);
+  EXPECT_EQ(frame[4], 0xFA);
+  EXPECT_EQ(frame[5], 0xFF);
+  EXPECT_EQ(frame[6], 0x9C);
+  EXPECT_EQ(frame[7], 0x01);
+  EXPECT_EQ(frame[8], 0x90);
+  EXPECT_EQ(frame[9], checksum(frame));
+  EXPECT_EQ(frame[10], 0x7D);
+}
+
+TEST(CommandFrameTest, EncodesAckermannSteeringWithFirmwareScale)
+{
+  Command command;
+  command.linear_x = 0.25;
+  command.steering_angle = 0.4;
+  command.ackermann = true;
+
+  const auto frame = encodeCommandFrame(command);
+
+  EXPECT_EQ(frame[3], 0x00);
+  EXPECT_EQ(frame[4], 0xFA);
+  EXPECT_EQ(frame[5], 0x00);
+  EXPECT_EQ(frame[6], 0x00);
+  EXPECT_EQ(frame[7], 0x00);
+  EXPECT_EQ(frame[8], 0xC8);
+  EXPECT_EQ(frame[9], checksum(frame));
+}
+
+TEST(CommandFrameTest, ZeroCommandEncodesZeroPayload)
+{
+  const auto frame = encodeCommandFrame(Command{});
+
+  for (std::size_t index = 1; index <= 8; ++index) {
+    EXPECT_EQ(frame[index], 0x00);
+  }
+  EXPECT_EQ(frame[9], checksum(frame));
+}

src/origincar_base/test/command_gate_test.cpp

@@ -0,0 +1,42 @@
+#include <chrono>
+
+#include <gtest/gtest.h>
+
+#include "origincar_base/command_gate.hpp"
+
+using origincar_base_core::Command;
+using origincar_base_core::CommandGate;
+
+TEST(CommandGateTest, ReturnsZeroBeforeFirstCommand)
+{
+  const auto t0 = std::chrono::steady_clock::time_point{};
+  CommandGate gate(std::chrono::milliseconds(150));
+
+  EXPECT_EQ(gate.commandAt(t0), (Command{}));
+}
+
+TEST(CommandGateTest, KeepsLatestCommandUntilWatchdogExpires)
+{
+  const auto t0 = std::chrono::steady_clock::time_point{};
+  CommandGate gate(std::chrono::milliseconds(150));
+  const Command command{0.25, 0.0, 0.4};
+
+  gate.update(command, t0);
+
+  EXPECT_EQ(gate.commandAt(t0 + std::chrono::milliseconds(149)), command);
+  EXPECT_EQ(gate.commandAt(t0 + std::chrono::milliseconds(150)), (Command{}));
+}
+
+TEST(CommandGateTest, NewCommandRefreshesWatchdog)
+{
+  const auto t0 = std::chrono::steady_clock::time_point{};
+  CommandGate gate(std::chrono::milliseconds(150));
+  const Command first{0.1, 0.0, 0.1};
+  const Command second{0.3, 0.0, -0.2};
+
+  gate.update(first, t0);
+  gate.update(second, t0 + std::chrono::milliseconds(100));
+
+  EXPECT_EQ(gate.commandAt(t0 + std::chrono::milliseconds(249)), second);
+  EXPECT_EQ(gate.commandAt(t0 + std::chrono::milliseconds(250)), (Command{}));
+}

src/origincar_base/test/serial_frame_parser_test.cpp

@@ -0,0 +1,66 @@
+#include <array>
+#include <cstdint>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include "origincar_base/serial_frame_parser.hpp"
+
+using origincar_base_core::SerialFrameParser;
+
+namespace
+{
+std::array<uint8_t, SerialFrameParser::kFrameSize> makeFrame(uint8_t seed)
+{
+  std::array<uint8_t, SerialFrameParser::kFrameSize> frame{};
+  frame[0] = 0x7B;
+  frame[23] = 0x7D;
+  for (std::size_t index = 1; index < 23; ++index) {
+    frame[index] = static_cast<uint8_t>(seed + index);
+  }
+  return frame;
+}
+}  // namespace
+
+TEST(SerialFrameParserTest, ReassemblesFrameAcrossPartialReads)
+{
+  SerialFrameParser parser;
+  const auto expected = makeFrame(10);
+  std::array<uint8_t, SerialFrameParser::kFrameSize> actual{};
+
+  parser.append(expected.data(), 7);
+  EXPECT_FALSE(parser.popFrame(&actual));
+
+  parser.append(expected.data() + 7, expected.size() - 7);
+  ASSERT_TRUE(parser.popFrame(&actual));
+  EXPECT_EQ(actual, expected);
+}
+
+TEST(SerialFrameParserTest, DiscardsNoiseBeforeValidFrame)
+{
+  SerialFrameParser parser;
+  const auto expected = makeFrame(20);
+  const std::vector<uint8_t> noise{0x00, 0x01, 0x7D, 0x55};
+  std::array<uint8_t, SerialFrameParser::kFrameSize> actual{};
+
+  parser.append(noise.data(), noise.size());
+  parser.append(expected.data(), expected.size());
+
+  ASSERT_TRUE(parser.popFrame(&actual));
+  EXPECT_EQ(actual, expected);
+}
+
+TEST(SerialFrameParserTest, RejectsInvalidTailAndContinuesSearching)
+{
+  SerialFrameParser parser;
+  auto invalid = makeFrame(30);
+  invalid[23] = 0x00;
+  const auto expected = makeFrame(40);
+  std::array<uint8_t, SerialFrameParser::kFrameSize> actual{};
+
+  parser.append(invalid.data(), invalid.size());
+  parser.append(expected.data(), expected.size());
+
+  ASSERT_TRUE(parser.popFrame(&actual));
+  EXPECT_EQ(actual, expected);
+}