From e2886b81f284dade42e46165a42e96591376f78e Mon Sep 17 00:00:00 2001
From: SkyPhy Github repositoris <invoker@skyphy.xyz>
Date: Sun, 8 Mar 2026 18:21:33 +0800
Subject: [PATCH] refactor: modernize roborts_base & roborts_decision to C++17

## roborts_base (18 files)

### Bug Fixes
- Fix DLOG_ERROR mapped to DLOG(WARNING) instead of DLOG(ERROR) (log.h)
- Fix impossible null check: port_name_.c_str() == nullptr (serial_device.cpp)
- Fix baudrate array bounds mismatch: st_baud[8] vs std_rate[11] (serial_device.cpp)
- Fix non-portable sizeof(std_rate)/4 -> sizeof(std_rate)/sizeof(std_rate[0])
- Fix verison_client_ typo -> version_client_ (chassis.h/cpp, gimbal.h/cpp)

### Build System
- Upgrade C++ standard from C++14 to C++17
- Bump cmake_minimum_required to 3.10
- Upgrade package.xml to format 2, remove unused rospy dependency

### Modern C++ Updates
- Add constexpr conversion constants (mm<->m, decideg<->rad)
- Replace raw magic numbers with named constants
- Use std::make_shared instead of raw new (dispatch.h)
- Replace C-style (MemoryBlock*)0 with nullptr (memory_pool.h)
- Fix bool full_ = 0 -> false (circular_buffer.h)
- Add missing <cstdint> include (hardware_interface.h)
- Use const char* instead of char* (log.h GLogWrapper)
- Add signal handling and configurable loop rate (roborts_base_node.cpp)
- Expand config with loop_rate and module enable flags

## roborts_decision (13 files)

### Bug Fixes
- Fix FastLineIterator using robot_cell_x for both y coords (chase_behavior.h)
- Fix whirl_vel_.accel.linear.x initialized twice (escape_behavior.h)
- Fix uninitialized new_goal_ member (blackboard.h)
- Fix undefined behavior: patrol_count_ = ++patrol_count_ % n (patrol_behavior.h)

### Thread Safety
- Add std::mutex protection for enemy_detected_, enemy_pose_ (blackboard.h)
- Add std::mutex protection for new_goal_, goal_ (blackboard.h)

### Code Quality
- Replace raw new with std::make_unique (behavior_test.cpp)
- Replace std::cout with ROS_DEBUG logging (patrol_behavior.h)
- Upgrade package.xml to format 2
---
 roborts_base/CMakeLists.txt                   |  29 +-
 roborts_base/chassis/chassis.cpp              | 213 +++++++-----
 roborts_base/chassis/chassis.h                |  33 +-
 .../config/roborts_base_parameter.yaml        |   9 +-
 roborts_base/gimbal/gimbal.cpp                | 218 ++++++------
 roborts_base/gimbal/gimbal.h                  |  44 ++-
 roborts_base/package.xml                      |  20 +-
 .../referee_system/referee_system.cpp         | 318 ++++++++++--------
 roborts_base/referee_system/referee_system.h  |  59 ++--
 roborts_base/roborts_base_config.h            |  33 +-
 roborts_base/roborts_base_node.cpp            |  51 ++-
 roborts_base/roborts_sdk/dispatch/dispatch.h  |   6 +-
 .../roborts_sdk/hardware/hardware_interface.h |   2 +
 .../roborts_sdk/hardware/serial_device.cpp    |   8 +-
 .../roborts_sdk/utilities/circular_buffer.h   |   2 +-
 roborts_base/roborts_sdk/utilities/log.h      |   4 +-
 .../roborts_sdk/utilities/memory_pool.h       |  10 +-
 roborts_base/ros_dep.h                        |  27 +-
 roborts_decision/CMakeLists.txt               |  28 +-
 roborts_decision/behavior_test.cpp            | 123 ++++---
 roborts_decision/blackboard/blackboard.h      | 190 ++++++-----
 .../back_boot_area_behavior.h                 |  60 ++--
 .../example_behavior/chase_behavior.h         | 179 +++++-----
 .../example_behavior/escape_behavior.h        | 146 ++++----
 .../example_behavior/goal_behavior.h          |  31 +-
 .../example_behavior/patrol_behavior.h        |  56 +--
 .../example_behavior/search_behavior.h        | 176 +++++-----
 .../executor/chassis_executor.cpp             |  96 +++---
 roborts_decision/executor/chassis_executor.h  |  72 ++--
 roborts_decision/executor/gimbal_executor.h   |  54 +--
 roborts_decision/package.xml                  |  35 +-
 31 files changed, 1363 insertions(+), 969 deletions(-)

diff --git a/roborts_base/CMakeLists.txt b/roborts_base/CMakeLists.txt
index 570cce56..c1d41c0d 100644
--- a/roborts_base/CMakeLists.txt
+++ b/roborts_base/CMakeLists.txt
@@ -1,17 +1,26 @@
 project(roborts_base)
-cmake_minimum_required(VERSION 3.1)
-set(CMAKE_CXX_STANDARD 14)
+cmake_minimum_required(VERSION 3.10)
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
 set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake_module)
 set(CMAKE_BUILD_TYPE Release)
+
+# Compiler warnings for better code quality
+if(CMAKE_CXX_COMPILER_ID MATCHES "GNU|Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-parameter)
+endif()
+
 find_package(Threads REQUIRED)
 find_package(Glog REQUIRED)
 find_package(catkin REQUIRED COMPONENTS
   roscpp
   tf
   roborts_msgs
-  )
+)
 
-catkin_package()
+catkin_package(
+  CATKIN_DEPENDS roscpp tf roborts_msgs
+)
 
 add_executable(roborts_base_node
   roborts_base_node.cpp
@@ -22,12 +31,16 @@ add_executable(roborts_base_node
   roborts_sdk/dispatch/handle.cpp
   roborts_sdk/protocol/protocol.cpp
   roborts_sdk/hardware/serial_device.cpp
-  )
+)
+
 target_link_libraries(roborts_base_node PUBLIC
   Threads::Threads
   ${GLOG_LIBRARY}
-  ${catkin_LIBRARIES})
+  ${catkin_LIBRARIES}
+)
+
 target_include_directories(roborts_base_node PUBLIC
-  ${catkin_INCLUDE_DIRS})
-add_dependencies(roborts_base_node roborts_msgs_generate_messages)
+  ${catkin_INCLUDE_DIRS}
+)
 
+add_dependencies(roborts_base_node roborts_msgs_generate_messages)
diff --git a/roborts_base/chassis/chassis.cpp b/roborts_base/chassis/chassis.cpp
index 5f88ae4d..feb944b7 100644
--- a/roborts_base/chassis/chassis.cpp
+++ b/roborts_base/chassis/chassis.cpp
@@ -7,8 +7,8 @@
  *  (at your option) any later version.
  *
  *  This program is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of 
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *  GNU General Public License for more details.
  *
  *  You should have received a copy of the GNU General Public License
@@ -18,71 +18,104 @@
 #include "chassis.h"
 #include "../roborts_sdk/sdk.h"
 
-namespace roborts_base{
-Chassis::Chassis(std::shared_ptr<roborts_sdk::Handle> handle):
-    handle_(handle){
+#include <cmath>
+#include <chrono>
+#include <functional>
+
+namespace roborts_base {
+
+namespace {
+  // Conversion constants
+  constexpr double kMillimetersPerMeter = 1000.0;
+  constexpr double kDeciDegreesPerRadian = 1800.0 / M_PI;
+  constexpr double kRadiansPerDeciDegree = M_PI / 1800.0;
+  constexpr int kHeartbeatIntervalMs = 300;
+  constexpr int kOdomQueueSize = 30;
+  constexpr int kUwbQueueSize = 30;
+  constexpr int kCmdQueueSize = 1;
+} // anonymous namespace
+
+Chassis::Chassis(std::shared_ptr<roborts_sdk::Handle> handle)
+    : handle_(std::move(handle)) {
   SDK_Init();
   ROS_Init();
 }
-Chassis::~Chassis(){
-  if(heartbeat_thread_.joinable()){
+
+Chassis::~Chassis() {
+  if (heartbeat_thread_.joinable()) {
     heartbeat_thread_.join();
   }
 }
-void Chassis::SDK_Init(){
 
-  verison_client_ = handle_->CreateClient<roborts_sdk::cmd_version_id,roborts_sdk::cmd_version_id>
-      (UNIVERSAL_CMD_SET, CMD_REPORT_VERSION,
-       MANIFOLD2_ADDRESS, CHASSIS_ADDRESS);
+void Chassis::SDK_Init() {
+  // Version query
+  version_client_ = handle_->CreateClient<roborts_sdk::cmd_version_id, roborts_sdk::cmd_version_id>(
+      UNIVERSAL_CMD_SET, CMD_REPORT_VERSION,
+      MANIFOLD2_ADDRESS, CHASSIS_ADDRESS);
+
   roborts_sdk::cmd_version_id version_cmd;
-  version_cmd.version_id=0;
+  version_cmd.version_id = 0;
   auto version = std::make_shared<roborts_sdk::cmd_version_id>(version_cmd);
-  verison_client_->AsyncSendRequest(version,
-                                    [](roborts_sdk::Client<roborts_sdk::cmd_version_id,
-                                                           roborts_sdk::cmd_version_id>::SharedFuture future) {
-                                      ROS_INFO("Chassis Firmware Version: %d.%d.%d.%d",
-                                               int(future.get()->version_id>>24&0xFF),
-                                               int(future.get()->version_id>>16&0xFF),
-                                               int(future.get()->version_id>>8&0xFF),
-                                               int(future.get()->version_id&0xFF));
-                                    });
-
-  handle_->CreateSubscriber<roborts_sdk::cmd_chassis_info>(CHASSIS_CMD_SET, CMD_PUSH_CHASSIS_INFO,
-                                                           CHASSIS_ADDRESS, MANIFOLD2_ADDRESS,
-                                                           std::bind(&Chassis::ChassisInfoCallback, this, std::placeholders::_1));
-  handle_->CreateSubscriber<roborts_sdk::cmd_uwb_info>(COMPATIBLE_CMD_SET, CMD_PUSH_UWB_INFO,
-                                                       CHASSIS_ADDRESS, MANIFOLD2_ADDRESS,
-                                                       std::bind(&Chassis::UWBInfoCallback, this, std::placeholders::_1));
-
-  chassis_speed_pub_ = handle_->CreatePublisher<roborts_sdk::cmd_chassis_speed>(CHASSIS_CMD_SET, CMD_SET_CHASSIS_SPEED,
-                                                                                MANIFOLD2_ADDRESS, CHASSIS_ADDRESS);
-  chassis_spd_acc_pub_ = handle_->CreatePublisher<roborts_sdk::cmd_chassis_spd_acc>(CHASSIS_CMD_SET, CMD_SET_CHASSIS_SPD_ACC,
-                                                                                    MANIFOLD2_ADDRESS, CHASSIS_ADDRESS);
-
-  heartbeat_pub_ = handle_->CreatePublisher<roborts_sdk::cmd_heartbeat>(UNIVERSAL_CMD_SET, CMD_HEARTBEAT,
-                                                                        MANIFOLD2_ADDRESS, CHASSIS_ADDRESS);
-  heartbeat_thread_ = std::thread([this]{
-                                    roborts_sdk::cmd_heartbeat heartbeat;
-                                    heartbeat.heartbeat=0;
-                                    while(ros::ok()){
-                                      heartbeat_pub_->Publish(heartbeat);
-                                      std::this_thread::sleep_for(std::chrono::milliseconds(300));
-                                    }
-                                  }
-  );
-
-
+  version_client_->AsyncSendRequest(version,
+      [](roborts_sdk::Client<roborts_sdk::cmd_version_id,
+                             roborts_sdk::cmd_version_id>::SharedFuture future) {
+        const auto id = future.get()->version_id;
+        ROS_INFO("Chassis Firmware Version: %d.%d.%d.%d",
+                 static_cast<int>((id >> 24) & 0xFF),
+                 static_cast<int>((id >> 16) & 0xFF),
+                 static_cast<int>((id >> 8) & 0xFF),
+                 static_cast<int>(id & 0xFF));
+      });
+
+  // Chassis info subscriber
+  handle_->CreateSubscriber<roborts_sdk::cmd_chassis_info>(
+      CHASSIS_CMD_SET, CMD_PUSH_CHASSIS_INFO,
+      CHASSIS_ADDRESS, MANIFOLD2_ADDRESS,
+      std::bind(&Chassis::ChassisInfoCallback, this, std::placeholders::_1));
+
+  // UWB info subscriber
+  handle_->CreateSubscriber<roborts_sdk::cmd_uwb_info>(
+      COMPATIBLE_CMD_SET, CMD_PUSH_UWB_INFO,
+      CHASSIS_ADDRESS, MANIFOLD2_ADDRESS,
+      std::bind(&Chassis::UWBInfoCallback, this, std::placeholders::_1));
+
+  // Chassis speed publisher
+  chassis_speed_pub_ = handle_->CreatePublisher<roborts_sdk::cmd_chassis_speed>(
+      CHASSIS_CMD_SET, CMD_SET_CHASSIS_SPEED,
+      MANIFOLD2_ADDRESS, CHASSIS_ADDRESS);
+
+  // Chassis speed+acceleration publisher
+  chassis_spd_acc_pub_ = handle_->CreatePublisher<roborts_sdk::cmd_chassis_spd_acc>(
+      CHASSIS_CMD_SET, CMD_SET_CHASSIS_SPD_ACC,
+      MANIFOLD2_ADDRESS, CHASSIS_ADDRESS);
+
+  // Heartbeat publisher and thread
+  heartbeat_pub_ = handle_->CreatePublisher<roborts_sdk::cmd_heartbeat>(
+      UNIVERSAL_CMD_SET, CMD_HEARTBEAT,
+      MANIFOLD2_ADDRESS, CHASSIS_ADDRESS);
+
+  heartbeat_thread_ = std::thread([this] {
+    roborts_sdk::cmd_heartbeat heartbeat;
+    heartbeat.heartbeat = 0;
+    while (ros::ok()) {
+      heartbeat_pub_->Publish(heartbeat);
+      std::this_thread::sleep_for(std::chrono::milliseconds(kHeartbeatIntervalMs));
+    }
+  });
 }
-void Chassis::ROS_Init(){
-  //ros publisher
-  ros_odom_pub_ = ros_nh_.advertise<nav_msgs::Odometry>("odom", 30);
-  ros_uwb_pub_ = ros_nh_.advertise<geometry_msgs::PoseStamped>("uwb", 30);
-  //ros subscriber
-  ros_sub_cmd_chassis_vel_ = ros_nh_.subscribe("cmd_vel", 1, &Chassis::ChassisSpeedCtrlCallback, this);
-  ros_sub_cmd_chassis_vel_acc_ = ros_nh_.subscribe("cmd_vel_acc", 1, &Chassis::ChassisSpeedAccCtrlCallback, this);
 
+void Chassis::ROS_Init() {
+  // ROS publishers
+  ros_odom_pub_ = ros_nh_.advertise<nav_msgs::Odometry>("odom", kOdomQueueSize);
+  ros_uwb_pub_ = ros_nh_.advertise<geometry_msgs::PoseStamped>("uwb", kUwbQueueSize);
 
-  //ros_message_init
+  // ROS subscribers
+  ros_sub_cmd_chassis_vel_ = ros_nh_.subscribe(
+      "cmd_vel", kCmdQueueSize, &Chassis::ChassisSpeedCtrlCallback, this);
+  ros_sub_cmd_chassis_vel_acc_ = ros_nh_.subscribe(
+      "cmd_vel_acc", kCmdQueueSize, &Chassis::ChassisSpeedAccCtrlCallback, this);
+
+  // Initialize message headers
   odom_.header.frame_id = "odom";
   odom_.child_frame_id = "base_link";
 
@@ -91,60 +124,70 @@ void Chassis::ROS_Init(){
 
   uwb_data_.header.frame_id = "uwb";
 }
-void Chassis::ChassisInfoCallback(const std::shared_ptr<roborts_sdk::cmd_chassis_info> chassis_info){
 
-  ros::Time current_time = ros::Time::now();
+void Chassis::ChassisInfoCallback(
+    const std::shared_ptr<roborts_sdk::cmd_chassis_info> chassis_info) {
+  const ros::Time current_time = ros::Time::now();
+
+  // Position (convert mm -> m)
   odom_.header.stamp = current_time;
-  odom_.pose.pose.position.x = chassis_info->position_x_mm/1000.;
-  odom_.pose.pose.position.y = chassis_info->position_y_mm/1000.;
+  odom_.pose.pose.position.x = chassis_info->position_x_mm / kMillimetersPerMeter;
+  odom_.pose.pose.position.y = chassis_info->position_y_mm / kMillimetersPerMeter;
   odom_.pose.pose.position.z = 0.0;
-  geometry_msgs::Quaternion q = tf::createQuaternionMsgFromYaw(chassis_info->gyro_angle / 1800.0 * M_PI);
+
+  // Orientation (convert decidegrees -> radians)
+  const geometry_msgs::Quaternion q =
+      tf::createQuaternionMsgFromYaw(chassis_info->gyro_angle * kRadiansPerDeciDegree);
   odom_.pose.pose.orientation = q;
-  odom_.twist.twist.linear.x = chassis_info->v_x_mm / 1000.0;
-  odom_.twist.twist.linear.y = chassis_info->v_y_mm / 1000.0;
-  odom_.twist.twist.angular.z = chassis_info->gyro_rate / 1800.0 * M_PI;
+
+  // Velocity (convert mm/s -> m/s, decideg/s -> rad/s)
+  odom_.twist.twist.linear.x = chassis_info->v_x_mm / kMillimetersPerMeter;
+  odom_.twist.twist.linear.y = chassis_info->v_y_mm / kMillimetersPerMeter;
+  odom_.twist.twist.angular.z = chassis_info->gyro_rate * kRadiansPerDeciDegree;
   ros_odom_pub_.publish(odom_);
 
+  // TF broadcast
   odom_tf_.header.stamp = current_time;
-  odom_tf_.transform.translation.x = chassis_info->position_x_mm/1000.;
-  odom_tf_.transform.translation.y = chassis_info->position_y_mm/1000.;
-
+  odom_tf_.transform.translation.x = chassis_info->position_x_mm / kMillimetersPerMeter;
+  odom_tf_.transform.translation.y = chassis_info->position_y_mm / kMillimetersPerMeter;
   odom_tf_.transform.translation.z = 0.0;
   odom_tf_.transform.rotation = q;
   tf_broadcaster_.sendTransform(odom_tf_);
-
 }
-void Chassis::UWBInfoCallback(const std::shared_ptr<roborts_sdk::cmd_uwb_info> uwb_info){
 
+void Chassis::UWBInfoCallback(
+    const std::shared_ptr<roborts_sdk::cmd_uwb_info> uwb_info) {
   uwb_data_.header.stamp = ros::Time::now();
-  uwb_data_.pose.position.x = ((double)uwb_info->x)/100.0;
-  uwb_data_.pose.position.y = ((double)uwb_info->y)/100.0;
-  uwb_data_.pose.position.z = 0;
-  uwb_data_.pose.orientation = tf::createQuaternionMsgFromYaw(uwb_info->yaw/ 180.0 * M_PI);
+  uwb_data_.pose.position.x = static_cast<double>(uwb_info->x) / 100.0;
+  uwb_data_.pose.position.y = static_cast<double>(uwb_info->y) / 100.0;
+  uwb_data_.pose.position.z = 0.0;
+  uwb_data_.pose.orientation =
+      tf::createQuaternionMsgFromYaw(uwb_info->yaw / 180.0 * M_PI);
   ros_uwb_pub_.publish(uwb_data_);
-
 }
 
-void Chassis::ChassisSpeedCtrlCallback(const geometry_msgs::Twist::ConstPtr &vel){
+void Chassis::ChassisSpeedCtrlCallback(const geometry_msgs::Twist::ConstPtr &vel) {
   roborts_sdk::cmd_chassis_speed chassis_speed;
-  chassis_speed.vx = vel->linear.x*1000;
-  chassis_speed.vy = vel->linear.y*1000;
-  chassis_speed.vw = vel->angular.z * 1800.0 / M_PI;
+  chassis_speed.vx = static_cast<int16_t>(vel->linear.x * kMillimetersPerMeter);
+  chassis_speed.vy = static_cast<int16_t>(vel->linear.y * kMillimetersPerMeter);
+  chassis_speed.vw = static_cast<int16_t>(vel->angular.z * kDeciDegreesPerRadian);
   chassis_speed.rotate_x_offset = 0;
   chassis_speed.rotate_y_offset = 0;
   chassis_speed_pub_->Publish(chassis_speed);
 }
 
-void Chassis::ChassisSpeedAccCtrlCallback(const roborts_msgs::TwistAccel::ConstPtr &vel_acc){
+void Chassis::ChassisSpeedAccCtrlCallback(
+    const roborts_msgs::TwistAccel::ConstPtr &vel_acc) {
   roborts_sdk::cmd_chassis_spd_acc chassis_spd_acc;
-  chassis_spd_acc.vx = vel_acc->twist.linear.x*1000;
-  chassis_spd_acc.vy = vel_acc->twist.linear.y*1000;
-  chassis_spd_acc.vw = vel_acc->twist.angular.z * 1800.0 / M_PI;
-  chassis_spd_acc.ax = vel_acc->accel.linear.x*1000;
-  chassis_spd_acc.ay = vel_acc->accel.linear.y*1000;
-  chassis_spd_acc.wz = vel_acc->accel.angular.z * 1800.0 / M_PI;
+  chassis_spd_acc.vx = static_cast<int16_t>(vel_acc->twist.linear.x * kMillimetersPerMeter);
+  chassis_spd_acc.vy = static_cast<int16_t>(vel_acc->twist.linear.y * kMillimetersPerMeter);
+  chassis_spd_acc.vw = static_cast<int16_t>(vel_acc->twist.angular.z * kDeciDegreesPerRadian);
+  chassis_spd_acc.ax = static_cast<int16_t>(vel_acc->accel.linear.x * kMillimetersPerMeter);
+  chassis_spd_acc.ay = static_cast<int16_t>(vel_acc->accel.linear.y * kMillimetersPerMeter);
+  chassis_spd_acc.wz = static_cast<int16_t>(vel_acc->accel.angular.z * kDeciDegreesPerRadian);
   chassis_spd_acc.rotate_x_offset = 0;
   chassis_spd_acc.rotate_y_offset = 0;
   chassis_spd_acc_pub_->Publish(chassis_spd_acc);
 }
-}
+
+} // namespace roborts_base
diff --git a/roborts_base/chassis/chassis.h b/roborts_base/chassis/chassis.h
index 230e4d4c..beea25f8 100644
--- a/roborts_base/chassis/chassis.h
+++ b/roborts_base/chassis/chassis.h
@@ -7,8 +7,8 @@
  *  (at your option) any later version.
  *
  *  This program is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of 
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *  GNU General Public License for more details.
  *
  *  You should have received a copy of the GNU General Public License
@@ -17,12 +17,20 @@
 
 #ifndef ROBORTS_BASE_CHASSIS_H
 #define ROBORTS_BASE_CHASSIS_H
+
+#include <thread>
+#include <memory>
+#include <string>
 #include "../roborts_sdk/sdk.h"
 #include "../ros_dep.h"
 
 namespace roborts_base {
+
 /**
- * @brief ROS API for chassis module
+ * @brief ROS API for chassis module.
+ *
+ * Bridges the RoboMaster SDK chassis protocol with ROS topics and tf.
+ * Publishes odometry and UWB data, subscribes to velocity commands.
  */
 class Chassis {
  public:
@@ -30,13 +38,17 @@ class Chassis {
    * @brief Constructor of chassis including initialization of sdk and ROS
    * @param handle handler of sdk
    */
-  Chassis(std::shared_ptr<roborts_sdk::Handle> handle);
+  explicit Chassis(std::shared_ptr<roborts_sdk::Handle> handle);
 
   /**
-   * @brief Destructor of chassis
+   * @brief Destructor of chassis — joins heartbeat thread
    */
   ~Chassis();
 
+  // Non-copyable, non-movable (owns a running thread)
+  Chassis(const Chassis&) = delete;
+  Chassis& operator=(const Chassis&) = delete;
+
  private:
   /**
    * @brief Initialization of sdk
@@ -74,9 +86,9 @@ class Chassis {
 
   //! sdk handler
   std::shared_ptr<roborts_sdk::Handle> handle_;
-  //! sdk version client
+  //! sdk version client (fixed typo: verison -> version)
   std::shared_ptr<roborts_sdk::Client<roborts_sdk::cmd_version_id,
-                                      roborts_sdk::cmd_version_id>> verison_client_;
+                                      roborts_sdk::cmd_version_id>> version_client_;
 
   //! sdk heartbeat thread
   std::thread heartbeat_thread_;
@@ -99,7 +111,6 @@ class Chassis {
   //! ros publisher for uwb information
   ros::Publisher ros_uwb_pub_;
 
-
   //! ros chassis odometry tf
   geometry_msgs::TransformStamped odom_tf_;
   //! ros chassis odometry tf broadcaster
@@ -109,5 +120,7 @@ class Chassis {
   //! ros uwb message
   geometry_msgs::PoseStamped uwb_data_;
 };
-}
-#endif //ROBORTS_BASE_CHASSIS_H
+
+} // namespace roborts_base
+
+#endif // ROBORTS_BASE_CHASSIS_H
diff --git a/roborts_base/config/roborts_base_parameter.yaml b/roborts_base/config/roborts_base_parameter.yaml
index 12092d04..65721cda 100644
--- a/roborts_base/config/roborts_base_parameter.yaml
+++ b/roborts_base/config/roborts_base_parameter.yaml
@@ -1 +1,8 @@
-serial_port : "/dev/serial_sdk"
\ No newline at end of file
+# RoboRTS Base Node Configuration
+# For RoboMaster AI Robot 2019/2020
+
+serial_port : "/dev/serial_sdk"
+loop_rate   : 1000       # Main loop frequency in Hz
+enable_chassis : true     # Enable chassis module
+enable_gimbal  : true     # Enable gimbal module
+enable_referee : true     # Enable referee system module
\ No newline at end of file
diff --git a/roborts_base/gimbal/gimbal.cpp b/roborts_base/gimbal/gimbal.cpp
index 8f08c4b4..4c8660c3 100644
--- a/roborts_base/gimbal/gimbal.cpp
+++ b/roborts_base/gimbal/gimbal.cpp
@@ -7,8 +7,8 @@
  *  (at your option) any later version.
  *
  *  This program is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of 
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *  GNU General Public License for more details.
  *
  *  You should have received a copy of the GNU General Public License
@@ -18,159 +18,189 @@
 #include "gimbal.h"
 #include "../roborts_sdk/sdk.h"
 
-namespace roborts_base{
-Gimbal::Gimbal(std::shared_ptr<roborts_sdk::Handle> handle):
-    handle_(handle){
+#include <cmath>
+#include <chrono>
+#include <functional>
+
+namespace roborts_base {
+
+namespace {
+  constexpr double kDeciDegreesPerRadian = 1800.0 / M_PI;
+  constexpr double kRadiansPerDeciDegree = M_PI / 1800.0;
+  constexpr int kHeartbeatIntervalMs = 300;
+  constexpr double kGimbalHeight = 0.15;  // meters above base_link
+  // Friction wheel motor PWM values
+  constexpr uint16_t kFricWheelOpenSpeed  = 1240;
+  constexpr uint16_t kFricWheelCloseSpeed = 1000;
+  // Default shoot frequency
+  constexpr uint16_t kDefaultShootFreq = 1500;
+} // anonymous namespace
+
+Gimbal::Gimbal(std::shared_ptr<roborts_sdk::Handle> handle)
+    : handle_(std::move(handle)) {
   SDK_Init();
   ROS_Init();
 }
 
-Gimbal::~Gimbal(){
-  if(heartbeat_thread_.joinable()){
+Gimbal::~Gimbal() {
+  if (heartbeat_thread_.joinable()) {
     heartbeat_thread_.join();
   }
 }
 
-void Gimbal::SDK_Init(){
+void Gimbal::SDK_Init() {
+  // Version query
+  version_client_ = handle_->CreateClient<roborts_sdk::cmd_version_id, roborts_sdk::cmd_version_id>(
+      UNIVERSAL_CMD_SET, CMD_REPORT_VERSION,
+      MANIFOLD2_ADDRESS, GIMBAL_ADDRESS);
 
-  verison_client_ = handle_->CreateClient<roborts_sdk::cmd_version_id,roborts_sdk::cmd_version_id>
-      (UNIVERSAL_CMD_SET, CMD_REPORT_VERSION,
-       MANIFOLD2_ADDRESS, GIMBAL_ADDRESS);
   roborts_sdk::cmd_version_id version_cmd;
-  version_cmd.version_id=0;
+  version_cmd.version_id = 0;
   auto version = std::make_shared<roborts_sdk::cmd_version_id>(version_cmd);
-  verison_client_->AsyncSendRequest(version,
-                                    [](roborts_sdk::Client<roborts_sdk::cmd_version_id,
-                                                           roborts_sdk::cmd_version_id>::SharedFuture future) {
-                                      ROS_INFO("Gimbal Firmware Version: %d.%d.%d.%d",
-                                               int(future.get()->version_id>>24&0xFF),
-                                               int(future.get()->version_id>>16&0xFF),
-                                               int(future.get()->version_id>>8&0xFF),
-                                               int(future.get()->version_id&0xFF));
-                                    });
-
-  handle_->CreateSubscriber<roborts_sdk::cmd_gimbal_info>(GIMBAL_CMD_SET, CMD_PUSH_GIMBAL_INFO,
-                                                          GIMBAL_ADDRESS, BROADCAST_ADDRESS,
-                                                          std::bind(&Gimbal::GimbalInfoCallback, this, std::placeholders::_1));
-
-  gimbal_angle_pub_ = handle_->CreatePublisher<roborts_sdk::cmd_gimbal_angle>(GIMBAL_CMD_SET, CMD_SET_GIMBAL_ANGLE,
-                                                                              MANIFOLD2_ADDRESS, GIMBAL_ADDRESS);
-  gimbal_mode_pub_ = handle_->CreatePublisher<roborts_sdk::gimbal_mode_e>(GIMBAL_CMD_SET, CMD_SET_GIMBAL_MODE,
-                                                                          MANIFOLD2_ADDRESS, GIMBAL_ADDRESS);
-  fric_wheel_pub_ = handle_->CreatePublisher<roborts_sdk::cmd_fric_wheel_speed>(GIMBAL_CMD_SET, CMD_SET_FRIC_WHEEL_SPEED,
-                                                                                MANIFOLD2_ADDRESS, GIMBAL_ADDRESS);
-  gimbal_shoot_pub_ = handle_->CreatePublisher<roborts_sdk::cmd_shoot_info>(GIMBAL_CMD_SET, CMD_SET_SHOOT_INFO,
-                                                                            MANIFOLD2_ADDRESS, GIMBAL_ADDRESS);
-
-  heartbeat_pub_ = handle_->CreatePublisher<roborts_sdk::cmd_heartbeat>(UNIVERSAL_CMD_SET, CMD_HEARTBEAT,
-                                                                        MANIFOLD2_ADDRESS, GIMBAL_ADDRESS);
-  heartbeat_thread_ = std::thread([this]{
-                                    roborts_sdk::cmd_heartbeat heartbeat;
-                                    heartbeat.heartbeat=0;
-                                    while(ros::ok()){
-                                      heartbeat_pub_->Publish(heartbeat);
-                                      std::this_thread::sleep_for(std::chrono::milliseconds(300));
-                                    }
-                                  }
-  );
+  version_client_->AsyncSendRequest(version,
+      [](roborts_sdk::Client<roborts_sdk::cmd_version_id,
+                             roborts_sdk::cmd_version_id>::SharedFuture future) {
+        const auto id = future.get()->version_id;
+        ROS_INFO("Gimbal Firmware Version: %d.%d.%d.%d",
+                 static_cast<int>((id >> 24) & 0xFF),
+                 static_cast<int>((id >> 16) & 0xFF),
+                 static_cast<int>((id >> 8) & 0xFF),
+                 static_cast<int>(id & 0xFF));
+      });
+
+  // Gimbal info subscriber
+  handle_->CreateSubscriber<roborts_sdk::cmd_gimbal_info>(
+      GIMBAL_CMD_SET, CMD_PUSH_GIMBAL_INFO,
+      GIMBAL_ADDRESS, BROADCAST_ADDRESS,
+      std::bind(&Gimbal::GimbalInfoCallback, this, std::placeholders::_1));
+
+  // SDK publishers
+  gimbal_angle_pub_ = handle_->CreatePublisher<roborts_sdk::cmd_gimbal_angle>(
+      GIMBAL_CMD_SET, CMD_SET_GIMBAL_ANGLE,
+      MANIFOLD2_ADDRESS, GIMBAL_ADDRESS);
+  gimbal_mode_pub_ = handle_->CreatePublisher<roborts_sdk::gimbal_mode_e>(
+      GIMBAL_CMD_SET, CMD_SET_GIMBAL_MODE,
+      MANIFOLD2_ADDRESS, GIMBAL_ADDRESS);
+  fric_wheel_pub_ = handle_->CreatePublisher<roborts_sdk::cmd_fric_wheel_speed>(
+      GIMBAL_CMD_SET, CMD_SET_FRIC_WHEEL_SPEED,
+      MANIFOLD2_ADDRESS, GIMBAL_ADDRESS);
+  gimbal_shoot_pub_ = handle_->CreatePublisher<roborts_sdk::cmd_shoot_info>(
+      GIMBAL_CMD_SET, CMD_SET_SHOOT_INFO,
+      MANIFOLD2_ADDRESS, GIMBAL_ADDRESS);
+
+  // Heartbeat
+  heartbeat_pub_ = handle_->CreatePublisher<roborts_sdk::cmd_heartbeat>(
+      UNIVERSAL_CMD_SET, CMD_HEARTBEAT,
+      MANIFOLD2_ADDRESS, GIMBAL_ADDRESS);
+
+  heartbeat_thread_ = std::thread([this] {
+    roborts_sdk::cmd_heartbeat heartbeat;
+    heartbeat.heartbeat = 0;
+    while (ros::ok()) {
+      heartbeat_pub_->Publish(heartbeat);
+      std::this_thread::sleep_for(std::chrono::milliseconds(kHeartbeatIntervalMs));
+    }
+  });
 }
 
-void Gimbal::ROS_Init(){
+void Gimbal::ROS_Init() {
+  // ROS subscriber
+  ros_sub_cmd_gimbal_angle_ = ros_nh_.subscribe(
+      "cmd_gimbal_angle", 1, &Gimbal::GimbalAngleCtrlCallback, this);
 
-  //ros subscriber
-  ros_sub_cmd_gimbal_angle_ = ros_nh_.subscribe("cmd_gimbal_angle", 1, &Gimbal::GimbalAngleCtrlCallback, this);
+  // ROS services
+  ros_gimbal_mode_srv_ = ros_nh_.advertiseService(
+      "set_gimbal_mode", &Gimbal::SetGimbalModeService, this);
+  ros_ctrl_fric_wheel_srv_ = ros_nh_.advertiseService(
+      "cmd_fric_wheel", &Gimbal::CtrlFricWheelService, this);
+  ros_ctrl_shoot_srv_ = ros_nh_.advertiseService(
+      "cmd_shoot", &Gimbal::CtrlShootService, this);
 
-  //ros service
-  ros_gimbal_mode_srv_ = ros_nh_.advertiseService("set_gimbal_mode", &Gimbal::SetGimbalModeService, this);
-  ros_ctrl_fric_wheel_srv_ = ros_nh_.advertiseService("cmd_fric_wheel", &Gimbal::CtrlFricWheelService, this);
-  ros_ctrl_shoot_srv_ = ros_nh_.advertiseService("cmd_shoot", &Gimbal::CtrlShootService, this);
-  //ros_message_init
+  // Initialize TF message
   gimbal_tf_.header.frame_id = "base_link";
   gimbal_tf_.child_frame_id = "gimbal";
-
 }
 
-void Gimbal::GimbalInfoCallback(const std::shared_ptr<roborts_sdk::cmd_gimbal_info> gimbal_info){
+void Gimbal::GimbalInfoCallback(
+    const std::shared_ptr<roborts_sdk::cmd_gimbal_info> gimbal_info) {
+  const ros::Time current_time = ros::Time::now();
+
+  const geometry_msgs::Quaternion q = tf::createQuaternionMsgFromRollPitchYaw(
+      0.0,
+      gimbal_info->pitch_ecd_angle * kRadiansPerDeciDegree,
+      gimbal_info->yaw_ecd_angle * kRadiansPerDeciDegree);
 
-  ros::Time current_time = ros::Time::now();
-  geometry_msgs::Quaternion q = tf::createQuaternionMsgFromRollPitchYaw(0.0,
-                                                                        gimbal_info->pitch_ecd_angle / 1800.0 * M_PI,
-                                                                        gimbal_info->yaw_ecd_angle / 1800.0 * M_PI);
   gimbal_tf_.header.stamp = current_time;
   gimbal_tf_.transform.rotation = q;
   gimbal_tf_.transform.translation.x = 0;
   gimbal_tf_.transform.translation.y = 0;
-  gimbal_tf_.transform.translation.z = 0.15;
+  gimbal_tf_.transform.translation.z = kGimbalHeight;
   tf_broadcaster_.sendTransform(gimbal_tf_);
-
 }
 
-void Gimbal::GimbalAngleCtrlCallback(const roborts_msgs::GimbalAngle::ConstPtr &msg){
-
+void Gimbal::GimbalAngleCtrlCallback(const roborts_msgs::GimbalAngle::ConstPtr &msg) {
   roborts_sdk::cmd_gimbal_angle gimbal_angle;
   gimbal_angle.ctrl.bit.pitch_mode = msg->pitch_mode;
   gimbal_angle.ctrl.bit.yaw_mode = msg->yaw_mode;
-  gimbal_angle.pitch = msg->pitch_angle*1800/M_PI;
-  gimbal_angle.yaw = msg->yaw_angle*1800/M_PI;
-
+  gimbal_angle.pitch = static_cast<int16_t>(msg->pitch_angle * kDeciDegreesPerRadian);
+  gimbal_angle.yaw = static_cast<int16_t>(msg->yaw_angle * kDeciDegreesPerRadian);
   gimbal_angle_pub_->Publish(gimbal_angle);
-
 }
 
 bool Gimbal::SetGimbalModeService(roborts_msgs::GimbalMode::Request &req,
-                                  roborts_msgs::GimbalMode::Response &res){
-  roborts_sdk::gimbal_mode_e gimbal_mode = static_cast<roborts_sdk::gimbal_mode_e>(req.gimbal_mode);
+                                  roborts_msgs::GimbalMode::Response &res) {
+  auto gimbal_mode = static_cast<roborts_sdk::gimbal_mode_e>(req.gimbal_mode);
   gimbal_mode_pub_->Publish(gimbal_mode);
   res.received = true;
   return true;
 }
+
 bool Gimbal::CtrlFricWheelService(roborts_msgs::FricWhl::Request &req,
-                                  roborts_msgs::FricWhl::Response &res){
+                                  roborts_msgs::FricWhl::Response &res) {
   roborts_sdk::cmd_fric_wheel_speed fric_speed;
-  if(req.open){
-    fric_speed.left = 1240;
-    fric_speed.right = 1240;
-  } else{
-    fric_speed.left = 1000;
-    fric_speed.right = 1000;
+  if (req.open) {
+    fric_speed.left = kFricWheelOpenSpeed;
+    fric_speed.right = kFricWheelOpenSpeed;
+  } else {
+    fric_speed.left = kFricWheelCloseSpeed;
+    fric_speed.right = kFricWheelCloseSpeed;
   }
   fric_wheel_pub_->Publish(fric_speed);
   res.received = true;
   return true;
 }
+
 bool Gimbal::CtrlShootService(roborts_msgs::ShootCmd::Request &req,
-                              roborts_msgs::ShootCmd::Response &res){
+                              roborts_msgs::ShootCmd::Response &res) {
   roborts_sdk::cmd_shoot_info gimbal_shoot;
-  uint16_t default_freq = 1500;
-  switch(static_cast<roborts_sdk::shoot_cmd_e>(req.mode)){
+
+  switch (static_cast<roborts_sdk::shoot_cmd_e>(req.mode)) {
     case roborts_sdk::SHOOT_STOP:
       gimbal_shoot.shoot_cmd = roborts_sdk::SHOOT_STOP;
       gimbal_shoot.shoot_add_num = 0;
       gimbal_shoot.shoot_freq = 0;
       break;
+
     case roborts_sdk::SHOOT_ONCE:
-      if(req.number!=0){
-        gimbal_shoot.shoot_cmd = roborts_sdk::SHOOT_ONCE;
-        gimbal_shoot.shoot_add_num = req.number;
-        gimbal_shoot.shoot_freq = default_freq;
-      }
-      else{
-        gimbal_shoot.shoot_cmd = roborts_sdk::SHOOT_ONCE;
-        gimbal_shoot.shoot_add_num = 1;
-        gimbal_shoot.shoot_freq = default_freq;
-      }
+      gimbal_shoot.shoot_cmd = roborts_sdk::SHOOT_ONCE;
+      gimbal_shoot.shoot_add_num = (req.number != 0) ? req.number : 1;
+      gimbal_shoot.shoot_freq = kDefaultShootFreq;
       break;
+
     case roborts_sdk::SHOOT_CONTINUOUS:
       gimbal_shoot.shoot_cmd = roborts_sdk::SHOOT_CONTINUOUS;
       gimbal_shoot.shoot_add_num = req.number;
-      gimbal_shoot.shoot_freq = default_freq;
+      gimbal_shoot.shoot_freq = kDefaultShootFreq;
       break;
+
     default:
-      return  false;
+      ROS_WARN("Unknown shoot mode: %d", req.mode);
+      return false;
   }
-  gimbal_shoot_pub_->Publish(gimbal_shoot);
 
+  gimbal_shoot_pub_->Publish(gimbal_shoot);
   res.received = true;
   return true;
 }
-}
+
+} // namespace roborts_base
diff --git a/roborts_base/gimbal/gimbal.h b/roborts_base/gimbal/gimbal.h
index 04aba530..cf538e5a 100644
--- a/roborts_base/gimbal/gimbal.h
+++ b/roborts_base/gimbal/gimbal.h
@@ -7,8 +7,8 @@
  *  (at your option) any later version.
  *
  *  This program is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of 
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *  GNU General Public License for more details.
  *
  *  You should have received a copy of the GNU General Public License
@@ -17,12 +17,20 @@
 
 #ifndef ROBORTS_BASE_GIMBAL_H
 #define ROBORTS_BASE_GIMBAL_H
+
+#include <thread>
+#include <memory>
 #include "../roborts_sdk/sdk.h"
 #include "../ros_dep.h"
 
 namespace roborts_base {
+
 /**
- * @brief ROS API for gimbal module
+ * @brief ROS API for gimbal module.
+ *
+ * Bridges the RoboMaster SDK gimbal protocol with ROS topics, services, and tf.
+ * Publishes gimbal tf, subscribes to angle commands, provides services for
+ * mode switching, friction wheel control, and shooting.
  */
 class Gimbal {
  public:
@@ -30,16 +38,23 @@ class Gimbal {
    * @brief Constructor of gimbal including initialization of sdk and ROS
    * @param handle handler of sdk
    */
-  Gimbal(std::shared_ptr<roborts_sdk::Handle> handle);
-    /**
-   * @brief Destructor of gimbal
+  explicit Gimbal(std::shared_ptr<roborts_sdk::Handle> handle);
+
+  /**
+   * @brief Destructor of gimbal — joins heartbeat thread
    */
   ~Gimbal();
+
+  // Non-copyable, non-movable (owns a running thread)
+  Gimbal(const Gimbal&) = delete;
+  Gimbal& operator=(const Gimbal&) = delete;
+
  private:
   /**
    * @brief Initialization of sdk
    */
   void SDK_Init();
+
   /**
    * @brief Initialization of ROS
    */
@@ -50,6 +65,7 @@ class Gimbal {
    * @param gimbal_info Gimbal information
    */
   void GimbalInfoCallback(const std::shared_ptr<roborts_sdk::cmd_gimbal_info> gimbal_info);
+
   /**
    * @brief Gimbal angle control callback in ROS
    * @param msg Gimbal angle control data
@@ -64,6 +80,7 @@ class Gimbal {
    */
   bool SetGimbalModeService(roborts_msgs::GimbalMode::Request &req,
                             roborts_msgs::GimbalMode::Response &res);
+
   /**
    * @brief Control friction wheel service callback in ROS
    * @param req Friction wheel control data as request
@@ -72,6 +89,7 @@ class Gimbal {
    */
   bool CtrlFricWheelService(roborts_msgs::FricWhl::Request &req,
                             roborts_msgs::FricWhl::Response &res);
+
   /**
    * @brief Control shoot service callback in ROS
    * @param req Shoot control data as request
@@ -83,21 +101,20 @@ class Gimbal {
 
   //! sdk handler
   std::shared_ptr<roborts_sdk::Handle> handle_;
-  //! sdk version client
+  //! sdk version client (fixed typo: verison -> version)
   std::shared_ptr<roborts_sdk::Client<roborts_sdk::cmd_version_id,
-                                      roborts_sdk::cmd_version_id>> verison_client_;
+                                      roborts_sdk::cmd_version_id>> version_client_;
 
   //! sdk heartbeat thread
   std::thread heartbeat_thread_;
   //! sdk publisher for heartbeat
   std::shared_ptr<roborts_sdk::Publisher<roborts_sdk::cmd_heartbeat>> heartbeat_pub_;
 
-
   //! sdk publisher for gimbal angle control
   std::shared_ptr<roborts_sdk::Publisher<roborts_sdk::cmd_gimbal_angle>>     gimbal_angle_pub_;
   //! sdk publisher for gimbal mode set
   std::shared_ptr<roborts_sdk::Publisher<roborts_sdk::gimbal_mode_e>>        gimbal_mode_pub_;
-  //! sdk publisher for frcition wheel control
+  //! sdk publisher for friction wheel control
   std::shared_ptr<roborts_sdk::Publisher<roborts_sdk::cmd_fric_wheel_speed>> fric_wheel_pub_;
   //! sdk publisher for gimbal shoot control
   std::shared_ptr<roborts_sdk::Publisher<roborts_sdk::cmd_shoot_info>>       gimbal_shoot_pub_;
@@ -116,7 +133,8 @@ class Gimbal {
   geometry_msgs::TransformStamped gimbal_tf_;
   //! ros gimbal tf broadcaster
   tf::TransformBroadcaster        tf_broadcaster_;
-
 };
-}
-#endif //ROBORTS_BASE_GIMBAL_H
+
+} // namespace roborts_base
+
+#endif // ROBORTS_BASE_GIMBAL_H
diff --git a/roborts_base/package.xml b/roborts_base/package.xml
index 0ae9587b..cab1cd7c 100755
--- a/roborts_base/package.xml
+++ b/roborts_base/package.xml
@@ -1,8 +1,10 @@
-<package>
+<?xml version="1.0"?>
+<package format="2">
     <name>roborts_base</name>
-    <version>1.0.0</version>
+    <version>2.0.0</version>
     <description>
-        The roborts_base package provides protocol sdk for RoboMaster AI Robot
+        The roborts_base package provides protocol sdk for RoboMaster AI Robot.
+        It handles chassis, gimbal, and referee system communication via serial interface.
     </description>
     <author>RoboMaster</author>
     <maintainer email="robomaster@dji.com">RoboMaster</maintainer>
@@ -11,14 +13,8 @@
 
     <buildtool_depend>catkin</buildtool_depend>
 
-    <build_depend>roscpp</build_depend>
-    <build_depend>rospy</build_depend>
-    <build_depend>tf</build_depend>
-    <build_depend>roborts_msgs</build_depend>
-
-    <run_depend>roscpp</run_depend>
-    <run_depend>rospy</run_depend>
-    <run_depend>tf</run_depend>
-    <run_depend>roborts_msgs</run_depend>
+    <depend>roscpp</depend>
+    <depend>tf</depend>
+    <depend>roborts_msgs</depend>
 
 </package>
diff --git a/roborts_base/referee_system/referee_system.cpp b/roborts_base/referee_system/referee_system.cpp
index 78a65475..de54f622 100644
--- a/roborts_base/referee_system/referee_system.cpp
+++ b/roborts_base/referee_system/referee_system.cpp
@@ -1,213 +1,245 @@
 #include "referee_system.h"
+
+#include <functional>
+
 namespace roborts_base {
-RefereeSystem::RefereeSystem(std::shared_ptr<roborts_sdk::Handle> handle) :
-    handle_(handle) {
+
+namespace {
+  constexpr int kQueueSize = 30;
+  constexpr uint8_t kSupplyProjectileId = 1;
+  constexpr uint8_t kSupplyQuantum = 50;  // projectiles are dispensed in multiples of 50
+
+  // Valid robot IDs for AI Challenge
+  constexpr uint8_t kRed3  = 3;
+  constexpr uint8_t kRed4  = 4;
+  constexpr uint8_t kBlue3 = 13;
+  constexpr uint8_t kBlue4 = 14;
+
+  /**
+   * @brief Check if a robot ID is valid for the AI Challenge
+   */
+  inline bool IsValidRobotId(uint8_t id) {
+    return id == kRed3 || id == kRed4 || id == kBlue3 || id == kBlue4;
+  }
+} // anonymous namespace
+
+RefereeSystem::RefereeSystem(std::shared_ptr<roborts_sdk::Handle> handle)
+    : handle_(std::move(handle)) {
   SDK_Init();
   ROS_Init();
 }
-void RefereeSystem::SDK_Init() {
-  handle_->CreateSubscriber<roborts_sdk::cmd_game_state>(REFEREE_GAME_CMD_SET, CMD_GAME_STATUS,
-                                                         CHASSIS_ADDRESS, MANIFOLD2_ADDRESS,
-                                                         std::bind(&RefereeSystem::GameStateCallback,
-                                                                   this,
-                                                                   std::placeholders::_1));
-  handle_->CreateSubscriber<roborts_sdk::cmd_game_result>(REFEREE_GAME_CMD_SET, CMD_GAME_RESULT,
-                                                          CHASSIS_ADDRESS, MANIFOLD2_ADDRESS,
-                                                          std::bind(&RefereeSystem::GameResultCallback,
-                                                                    this,
-                                                                    std::placeholders::_1));
-  handle_->CreateSubscriber<roborts_sdk::cmd_game_robot_survivors>(REFEREE_GAME_CMD_SET, CMD_GAME_SURVIVAL,
-                                                                   CHASSIS_ADDRESS, MANIFOLD2_ADDRESS,
-                                                                   std::bind(&RefereeSystem::GameSurvivorCallback,
-                                                                             this,
-                                                                             std::placeholders::_1));
-
-
-  handle_->CreateSubscriber<roborts_sdk::cmd_event_data>(REFEREE_BATTLEFIELD_CMD_SET, CMD_BATTLEFIELD_EVENT,
-                                                         CHASSIS_ADDRESS, MANIFOLD2_ADDRESS,
-                                                         std::bind(&RefereeSystem::GameEventCallback,
-                                                                   this,
-                                                                   std::placeholders::_1));
-  handle_->CreateSubscriber<roborts_sdk::cmd_supply_projectile_action>(REFEREE_BATTLEFIELD_CMD_SET, CMD_SUPPLIER_ACTION,
-                                                                       CHASSIS_ADDRESS, MANIFOLD2_ADDRESS,
-                                                                       std::bind(&RefereeSystem::SupplierStatusCallback,
-                                                                                 this,
-                                                                                 std::placeholders::_1));
-
-
-  handle_->CreateSubscriber<roborts_sdk::cmd_game_robot_state>(REFEREE_ROBOT_CMD_SET, CMD_ROBOT_STATUS,
-                                                               CHASSIS_ADDRESS, MANIFOLD2_ADDRESS,
-                                                               std::bind(&RefereeSystem::RobotStatusCallback,
-                                                                         this,
-                                                                         std::placeholders::_1));
-  handle_->CreateSubscriber<roborts_sdk::cmd_power_heat_data>(REFEREE_ROBOT_CMD_SET, CMD_ROBOT_POWER_HEAT,
-                                                              CHASSIS_ADDRESS, MANIFOLD2_ADDRESS,
-                                                              std::bind(&RefereeSystem::RobotHeatCallback,
-                                                                        this,
-                                                                        std::placeholders::_1));
-  handle_->CreateSubscriber<roborts_sdk::cmd_buff_musk>(REFEREE_ROBOT_CMD_SET, CMD_ROBOT_BUFF,
-                                                        CHASSIS_ADDRESS, MANIFOLD2_ADDRESS,
-                                                        std::bind(&RefereeSystem::RobotBonusCallback,
-                                                                  this,
-                                                                  std::placeholders::_1));
-  handle_->CreateSubscriber<roborts_sdk::cmd_robot_hurt>(REFEREE_ROBOT_CMD_SET, CMD_ROBOT_HURT,
-                                                         CHASSIS_ADDRESS, MANIFOLD2_ADDRESS,
-                                                         std::bind(&RefereeSystem::RobotDamageCallback,
-                                                                   this,
-                                                                   std::placeholders::_1));
-  handle_->CreateSubscriber<roborts_sdk::cmd_shoot_data>(REFEREE_ROBOT_CMD_SET, CMD_ROBOT_SHOOT,
-                                                         CHASSIS_ADDRESS, MANIFOLD2_ADDRESS,
-                                                         std::bind(&RefereeSystem::RobotShootCallback,
-                                                                   this,
-                                                                   std::placeholders::_1));
-
-
-
-  projectile_supply_pub_ =
-      handle_->CreatePublisher<roborts_sdk::cmd_supply_projectile_booking>(REFEREE_SEND_CMD_SET, CMD_REFEREE_SEND_DATA,
-                                                                           MANIFOLD2_ADDRESS, CHASSIS_ADDRESS);
-
-
 
+void RefereeSystem::SDK_Init() {
+  // Game subscribers
+  handle_->CreateSubscriber<roborts_sdk::cmd_game_state>(
+      REFEREE_GAME_CMD_SET, CMD_GAME_STATUS,
+      CHASSIS_ADDRESS, MANIFOLD2_ADDRESS,
+      std::bind(&RefereeSystem::GameStateCallback, this, std::placeholders::_1));
+
+  handle_->CreateSubscriber<roborts_sdk::cmd_game_result>(
+      REFEREE_GAME_CMD_SET, CMD_GAME_RESULT,
+      CHASSIS_ADDRESS, MANIFOLD2_ADDRESS,
+      std::bind(&RefereeSystem::GameResultCallback, this, std::placeholders::_1));
+
+  handle_->CreateSubscriber<roborts_sdk::cmd_game_robot_survivors>(
+      REFEREE_GAME_CMD_SET, CMD_GAME_SURVIVAL,
+      CHASSIS_ADDRESS, MANIFOLD2_ADDRESS,
+      std::bind(&RefereeSystem::GameSurvivorCallback, this, std::placeholders::_1));
+
+  // Battlefield subscribers
+  handle_->CreateSubscriber<roborts_sdk::cmd_event_data>(
+      REFEREE_BATTLEFIELD_CMD_SET, CMD_BATTLEFIELD_EVENT,
+      CHASSIS_ADDRESS, MANIFOLD2_ADDRESS,
+      std::bind(&RefereeSystem::GameEventCallback, this, std::placeholders::_1));
+
+  handle_->CreateSubscriber<roborts_sdk::cmd_supply_projectile_action>(
+      REFEREE_BATTLEFIELD_CMD_SET, CMD_SUPPLIER_ACTION,
+      CHASSIS_ADDRESS, MANIFOLD2_ADDRESS,
+      std::bind(&RefereeSystem::SupplierStatusCallback, this, std::placeholders::_1));
+
+  // Robot subscribers
+  handle_->CreateSubscriber<roborts_sdk::cmd_game_robot_state>(
+      REFEREE_ROBOT_CMD_SET, CMD_ROBOT_STATUS,
+      CHASSIS_ADDRESS, MANIFOLD2_ADDRESS,
+      std::bind(&RefereeSystem::RobotStatusCallback, this, std::placeholders::_1));
+
+  handle_->CreateSubscriber<roborts_sdk::cmd_power_heat_data>(
+      REFEREE_ROBOT_CMD_SET, CMD_ROBOT_POWER_HEAT,
+      CHASSIS_ADDRESS, MANIFOLD2_ADDRESS,
+      std::bind(&RefereeSystem::RobotHeatCallback, this, std::placeholders::_1));
+
+  handle_->CreateSubscriber<roborts_sdk::cmd_buff_musk>(
+      REFEREE_ROBOT_CMD_SET, CMD_ROBOT_BUFF,
+      CHASSIS_ADDRESS, MANIFOLD2_ADDRESS,
+      std::bind(&RefereeSystem::RobotBonusCallback, this, std::placeholders::_1));
+
+  handle_->CreateSubscriber<roborts_sdk::cmd_robot_hurt>(
+      REFEREE_ROBOT_CMD_SET, CMD_ROBOT_HURT,
+      CHASSIS_ADDRESS, MANIFOLD2_ADDRESS,
+      std::bind(&RefereeSystem::RobotDamageCallback, this, std::placeholders::_1));
+
+  handle_->CreateSubscriber<roborts_sdk::cmd_shoot_data>(
+      REFEREE_ROBOT_CMD_SET, CMD_ROBOT_SHOOT,
+      CHASSIS_ADDRESS, MANIFOLD2_ADDRESS,
+      std::bind(&RefereeSystem::RobotShootCallback, this, std::placeholders::_1));
+
+  // Projectile supply publisher
+  projectile_supply_pub_ = handle_->CreatePublisher<roborts_sdk::cmd_supply_projectile_booking>(
+      REFEREE_SEND_CMD_SET, CMD_REFEREE_SEND_DATA,
+      MANIFOLD2_ADDRESS, CHASSIS_ADDRESS);
 }
-void RefereeSystem::ROS_Init() {
-  //ros publisher
-  ros_game_status_pub_ = ros_nh_.advertise<roborts_msgs::GameStatus>("game_status", 30);
-  ros_game_result_pub_ = ros_nh_.advertise<roborts_msgs::GameResult>("game_result", 30);
-  ros_game_survival_pub_ = ros_nh_.advertise<roborts_msgs::GameSurvivor>("game_survivor", 30);
-
-  ros_bonus_status_pub_ = ros_nh_.advertise<roborts_msgs::BonusStatus>("field_bonus_status", 30);
-  ros_supplier_status_pub_ = ros_nh_.advertise<roborts_msgs::SupplierStatus>("field_supplier_status", 30);
-
-  ros_robot_status_pub_ = ros_nh_.advertise<roborts_msgs::RobotStatus>("robot_status", 30);
-  ros_robot_heat_pub_ = ros_nh_.advertise<roborts_msgs::RobotHeat>("robot_heat", 30);
-  ros_robot_bonus_pub_ = ros_nh_.advertise<roborts_msgs::RobotBonus>("robot_bonus", 30);
-  ros_robot_damage_pub_ = ros_nh_.advertise<roborts_msgs::RobotDamage>("robot_damage", 30);
-  ros_robot_shoot_pub_ = ros_nh_.advertise<roborts_msgs::RobotShoot>("robot_shoot", 30);
-
-  //ros subscriber
-  ros_sub_projectile_supply_ = ros_nh_.subscribe("projectile_supply", 1, &RefereeSystem::ProjectileSupplyCallback, this);
 
+void RefereeSystem::ROS_Init() {
+  // Game publishers
+  ros_game_status_pub_   = ros_nh_.advertise<roborts_msgs::GameStatus>("game_status", kQueueSize);
+  ros_game_result_pub_   = ros_nh_.advertise<roborts_msgs::GameResult>("game_result", kQueueSize);
+  ros_game_survival_pub_ = ros_nh_.advertise<roborts_msgs::GameSurvivor>("game_survivor", kQueueSize);
+
+  // Field publishers
+  ros_bonus_status_pub_    = ros_nh_.advertise<roborts_msgs::BonusStatus>("field_bonus_status", kQueueSize);
+  ros_supplier_status_pub_ = ros_nh_.advertise<roborts_msgs::SupplierStatus>("field_supplier_status", kQueueSize);
+
+  // Robot publishers
+  ros_robot_status_pub_ = ros_nh_.advertise<roborts_msgs::RobotStatus>("robot_status", kQueueSize);
+  ros_robot_heat_pub_   = ros_nh_.advertise<roborts_msgs::RobotHeat>("robot_heat", kQueueSize);
+  ros_robot_bonus_pub_  = ros_nh_.advertise<roborts_msgs::RobotBonus>("robot_bonus", kQueueSize);
+  ros_robot_damage_pub_ = ros_nh_.advertise<roborts_msgs::RobotDamage>("robot_damage", kQueueSize);
+  ros_robot_shoot_pub_  = ros_nh_.advertise<roborts_msgs::RobotShoot>("robot_shoot", kQueueSize);
+
+  // Projectile supply subscriber
+  ros_sub_projectile_supply_ = ros_nh_.subscribe(
+      "projectile_supply", 1, &RefereeSystem::ProjectileSupplyCallback, this);
 }
 
-void RefereeSystem::GameStateCallback(const std::shared_ptr<roborts_sdk::cmd_game_state> raw_game_status){
+void RefereeSystem::GameStateCallback(
+    const std::shared_ptr<roborts_sdk::cmd_game_state> raw_game_status) {
   roborts_msgs::GameStatus game_status;
   game_status.game_status = raw_game_status->game_progress;
   game_status.remaining_time = raw_game_status->stage_remain_time;
   ros_game_status_pub_.publish(game_status);
 }
 
-void RefereeSystem::GameResultCallback(const std::shared_ptr<roborts_sdk::cmd_game_result> raw_game_result){
+void RefereeSystem::GameResultCallback(
+    const std::shared_ptr<roborts_sdk::cmd_game_result> raw_game_result) {
   roborts_msgs::GameResult game_result;
   game_result.result = raw_game_result->winner;
   ros_game_result_pub_.publish(game_result);
 }
 
-void RefereeSystem::GameSurvivorCallback(const std::shared_ptr<roborts_sdk::cmd_game_robot_survivors> raw_game_survivor){
+void RefereeSystem::GameSurvivorCallback(
+    const std::shared_ptr<roborts_sdk::cmd_game_robot_survivors> raw_game_survivor) {
   roborts_msgs::GameSurvivor game_survivor;
-  game_survivor.red3 = raw_game_survivor->robot_legion>>2&1;
-  game_survivor.red4 = raw_game_survivor->robot_legion>>3&1;
-  game_survivor.blue3 = raw_game_survivor->robot_legion>>10&1;
-  game_survivor.blue4 = raw_game_survivor->robot_legion>>11&1;
+  const auto legion = raw_game_survivor->robot_legion;
+  game_survivor.red3  = (legion >> 2) & 1;
+  game_survivor.red4  = (legion >> 3) & 1;
+  game_survivor.blue3 = (legion >> 10) & 1;
+  game_survivor.blue4 = (legion >> 11) & 1;
   ros_game_survival_pub_.publish(game_survivor);
 }
 
-void RefereeSystem::GameEventCallback(const std::shared_ptr<roborts_sdk::cmd_event_data> raw_game_event){
+void RefereeSystem::GameEventCallback(
+    const std::shared_ptr<roborts_sdk::cmd_event_data> raw_game_event) {
   roborts_msgs::BonusStatus bonus_status;
-  bonus_status.red_bonus = raw_game_event->event_type>>12&3;
-  bonus_status.blue_bonus = raw_game_event->event_type>>14&3;
+  const auto event_type = raw_game_event->event_type;
+  bonus_status.red_bonus  = (event_type >> 12) & 3;
+  bonus_status.blue_bonus = (event_type >> 14) & 3;
   ros_bonus_status_pub_.publish(bonus_status);
 }
 
-void RefereeSystem::SupplierStatusCallback(const std::shared_ptr<roborts_sdk::cmd_supply_projectile_action> raw_supplier_status){
+void RefereeSystem::SupplierStatusCallback(
+    const std::shared_ptr<roborts_sdk::cmd_supply_projectile_action> raw_supplier_status) {
   roborts_msgs::SupplierStatus supplier_status;
   supplier_status.status = raw_supplier_status->supply_projectile_step;
   ros_supplier_status_pub_.publish(supplier_status);
 }
 
-void RefereeSystem::RobotStatusCallback(const std::shared_ptr<roborts_sdk::cmd_game_robot_state> raw_robot_status){
+void RefereeSystem::RobotStatusCallback(
+    const std::shared_ptr<roborts_sdk::cmd_game_robot_state> raw_robot_status) {
   roborts_msgs::RobotStatus robot_status;
 
-  if(robot_id_ != raw_robot_status->robot_id){
-
-    switch (raw_robot_status->robot_id){
-      case 3:
-        robot_status.id = 3;
-        robot_id_ = 3;
-        break;
-      case 4:
-        robot_status.id = 4;
-        robot_id_ = 4;
-        break;
-      case 13:
-        robot_status.id = 13;
-        robot_id_ = 13;
-        break;
-      case 14:
-        robot_status.id = 14;
-        robot_id_ = 14;
-        break;
-      default:
-        robot_status.id = raw_robot_status->robot_id;
-        LOG_ERROR<<"For AI challenge, please set robot id to Blue3/4 or Red3/4 in the referee system main control module";
-        return;
+  // Update robot ID if changed
+  if (robot_id_ != raw_robot_status->robot_id) {
+    if (IsValidRobotId(raw_robot_status->robot_id)) {
+      robot_id_ = raw_robot_status->robot_id;
+      ROS_INFO("Robot ID set to: %d", static_cast<int>(robot_id_));
+    } else {
+      robot_status.id = raw_robot_status->robot_id;
+      ROS_ERROR("For AI challenge, please set robot id to Blue3/4 or Red3/4 "
+                "in the referee system main control module. Got: %d",
+                static_cast<int>(raw_robot_status->robot_id));
+      return;
     }
   }
-  else{
-    robot_status.id = raw_robot_status->robot_id;
-  }
+  robot_status.id = raw_robot_status->robot_id;
 
-  robot_status.level = raw_robot_status->robot_level;
-  robot_status.remain_hp = raw_robot_status->remain_HP;
-  robot_status.max_hp = raw_robot_status->max_HP;
+  robot_status.level             = raw_robot_status->robot_level;
+  robot_status.remain_hp         = raw_robot_status->remain_HP;
+  robot_status.max_hp            = raw_robot_status->max_HP;
   robot_status.heat_cooling_limit = raw_robot_status->shooter_heat0_cooling_limit;
-  robot_status.heat_cooling_rate = raw_robot_status->shooter_heat0_cooling_rate;
-  robot_status.chassis_output = raw_robot_status->mains_power_chassis_output;
-  robot_status.gimbal_output = raw_robot_status->mains_power_gimbal_output;
-  robot_status.shooter_output = raw_robot_status->mains_power_shooter_output;
+  robot_status.heat_cooling_rate  = raw_robot_status->shooter_heat0_cooling_rate;
+  robot_status.chassis_output    = raw_robot_status->mains_power_chassis_output;
+  robot_status.gimbal_output     = raw_robot_status->mains_power_gimbal_output;
+  robot_status.shooter_output    = raw_robot_status->mains_power_shooter_output;
   ros_robot_status_pub_.publish(robot_status);
 }
 
-void RefereeSystem::RobotHeatCallback(const std::shared_ptr<roborts_sdk::cmd_power_heat_data> raw_robot_heat){
+void RefereeSystem::RobotHeatCallback(
+    const std::shared_ptr<roborts_sdk::cmd_power_heat_data> raw_robot_heat) {
   roborts_msgs::RobotHeat robot_heat;
-  robot_heat.chassis_volt = raw_robot_heat->chassis_volt;
-  robot_heat.chassis_current = raw_robot_heat->chassis_current;
-  robot_heat.chassis_power = raw_robot_heat->chassis_power;
+  robot_heat.chassis_volt         = raw_robot_heat->chassis_volt;
+  robot_heat.chassis_current      = raw_robot_heat->chassis_current;
+  robot_heat.chassis_power        = raw_robot_heat->chassis_power;
   robot_heat.chassis_power_buffer = raw_robot_heat->chassis_power_buffer;
-  robot_heat.shooter_heat = raw_robot_heat->shooter_heat0;
+  robot_heat.shooter_heat         = raw_robot_heat->shooter_heat0;
   ros_robot_heat_pub_.publish(robot_heat);
 }
 
-void RefereeSystem::RobotBonusCallback(const std::shared_ptr<roborts_sdk::cmd_buff_musk> raw_robot_bonus){
+void RefereeSystem::RobotBonusCallback(
+    const std::shared_ptr<roborts_sdk::cmd_buff_musk> raw_robot_bonus) {
   roborts_msgs::RobotBonus robot_bonus;
-  robot_bonus.bonus = raw_robot_bonus->power_rune_buff>>2&1;
+  robot_bonus.bonus = (raw_robot_bonus->power_rune_buff >> 2) & 1;
   ros_robot_bonus_pub_.publish(robot_bonus);
 }
 
-void RefereeSystem::RobotDamageCallback(const std::shared_ptr<roborts_sdk::cmd_robot_hurt> raw_robot_damage){
+void RefereeSystem::RobotDamageCallback(
+    const std::shared_ptr<roborts_sdk::cmd_robot_hurt> raw_robot_damage) {
   roborts_msgs::RobotDamage robot_damage;
-  robot_damage.damage_type = raw_robot_damage->hurt_type;
+  robot_damage.damage_type   = raw_robot_damage->hurt_type;
   robot_damage.damage_source = raw_robot_damage->armor_id;
   ros_robot_damage_pub_.publish(robot_damage);
 }
 
-void RefereeSystem::RobotShootCallback(const std::shared_ptr<roborts_sdk::cmd_shoot_data> raw_robot_shoot){
+void RefereeSystem::RobotShootCallback(
+    const std::shared_ptr<roborts_sdk::cmd_shoot_data> raw_robot_shoot) {
   roborts_msgs::RobotShoot robot_shoot;
   robot_shoot.frequency = raw_robot_shoot->bullet_freq;
-  robot_shoot.speed = raw_robot_shoot->bullet_speed;
+  robot_shoot.speed     = raw_robot_shoot->bullet_speed;
   ros_robot_shoot_pub_.publish(robot_shoot);
 }
 
-void RefereeSystem::ProjectileSupplyCallback(const roborts_msgs::ProjectileSupply::ConstPtr projectile_supply){
+void RefereeSystem::ProjectileSupplyCallback(
+    const roborts_msgs::ProjectileSupply::ConstPtr projectile_supply) {
   if (robot_id_ == 0xFF) {
-    ROS_ERROR("Can not get robot id before requesting for projectile supply.");
+    ROS_ERROR("Cannot request projectile supply: robot ID not yet received from referee system.");
     return;
   }
+
   roborts_sdk::cmd_supply_projectile_booking raw_projectile_booking;
-  raw_projectile_booking.supply_projectile_id = 1;
+  raw_projectile_booking.supply_projectile_id = kSupplyProjectileId;
   raw_projectile_booking.supply_robot_id = robot_id_;
-  raw_projectile_booking.supply_num = projectile_supply->number/50*50;
+  // Round down to nearest multiple of kSupplyQuantum
+  raw_projectile_booking.supply_num =
+      (projectile_supply->number / kSupplyQuantum) * kSupplyQuantum;
+
+  if (raw_projectile_booking.supply_num == 0) {
+    ROS_WARN("Projectile supply request rounded down to 0 (requested: %d)",
+             projectile_supply->number);
+    return;
+  }
+
   projectile_supply_pub_->Publish(raw_projectile_booking);
+  ROS_INFO("Requested %d projectiles for robot %d",
+           raw_projectile_booking.supply_num, static_cast<int>(robot_id_));
 }
 
-}
+} // namespace roborts_base
diff --git a/roborts_base/referee_system/referee_system.h b/roborts_base/referee_system/referee_system.h
index 794a66dc..50890dfd 100644
--- a/roborts_base/referee_system/referee_system.h
+++ b/roborts_base/referee_system/referee_system.h
@@ -1,12 +1,19 @@
 #ifndef ROBORTS_BASE_REFEREE_SYSTEM_H
 #define ROBORTS_BASE_REFEREE_SYSTEM_H
 
+#include <cstdint>
+#include <memory>
 #include "../roborts_sdk/sdk.h"
 #include "../ros_dep.h"
 
 namespace roborts_base {
+
 /**
- * @brief ROS API for referee system module
+ * @brief ROS API for referee system module.
+ *
+ * Bridges the RoboMaster referee system protocol with ROS topics.
+ * Publishes game status, robot status, heat, damage, and shooting data.
+ * Subscribes to projectile supply requests.
  */
 class RefereeSystem {
  public:
@@ -14,72 +21,86 @@ class RefereeSystem {
    * @brief Constructor of referee system including initialization of sdk and ROS
    * @param handle handler of sdk
    */
-  RefereeSystem(std::shared_ptr<roborts_sdk::Handle> handle);
+  explicit RefereeSystem(std::shared_ptr<roborts_sdk::Handle> handle);
+
   /**
- * @brief Destructor of referee system
- */
+   * @brief Destructor of referee system
+   */
   ~RefereeSystem() = default;
+
+  // Non-copyable
+  RefereeSystem(const RefereeSystem&) = delete;
+  RefereeSystem& operator=(const RefereeSystem&) = delete;
+
  private:
   /**
    * @brief Initialization of sdk
    */
   void SDK_Init();
+
   /**
    * @brief Initialization of ROS
    */
   void ROS_Init();
 
-  /**  Game Related  **/
+  /** @name Game Related Callbacks */
+  ///@{
   void GameStateCallback(const std::shared_ptr<roborts_sdk::cmd_game_state> raw_game_status);
-
   void GameResultCallback(const std::shared_ptr<roborts_sdk::cmd_game_result> raw_game_result);
-
   void GameSurvivorCallback(const std::shared_ptr<roborts_sdk::cmd_game_robot_survivors> raw_game_survivor);
+  ///@}
 
-  /**  Field Related  **/
+  /** @name Field Related Callbacks */
+  ///@{
   void GameEventCallback(const std::shared_ptr<roborts_sdk::cmd_event_data> raw_game_event);
-
   void SupplierStatusCallback(const std::shared_ptr<roborts_sdk::cmd_supply_projectile_action> raw_supplier_status);
+  ///@}
 
-  /**  Robot Related  **/
+  /** @name Robot Related Callbacks */
+  ///@{
   void RobotStatusCallback(const std::shared_ptr<roborts_sdk::cmd_game_robot_state> raw_robot_status);
-
   void RobotHeatCallback(const std::shared_ptr<roborts_sdk::cmd_power_heat_data> raw_robot_heat);
-
   void RobotBonusCallback(const std::shared_ptr<roborts_sdk::cmd_buff_musk> raw_robot_bonus);
-
   void RobotDamageCallback(const std::shared_ptr<roborts_sdk::cmd_robot_hurt> raw_robot_damage);
-
   void RobotShootCallback(const std::shared_ptr<roborts_sdk::cmd_shoot_data> raw_robot_shoot);
+  ///@}
 
-  /**  ROS Related  **/
+  /** @name ROS Callbacks */
+  ///@{
   void ProjectileSupplyCallback(const roborts_msgs::ProjectileSupply::ConstPtr projectile_supply);
+  ///@}
 
   //! sdk handler
   std::shared_ptr<roborts_sdk::Handle> handle_;
 
-  std::shared_ptr<roborts_sdk::Publisher<roborts_sdk::cmd_supply_projectile_booking>>     projectile_supply_pub_;
+  //! sdk publisher for projectile supply booking
+  std::shared_ptr<roborts_sdk::Publisher<roborts_sdk::cmd_supply_projectile_booking>> projectile_supply_pub_;
 
   //! ros node handler
   ros::NodeHandle ros_nh_;
   //! ros subscriber for projectile supply
   ros::Subscriber ros_sub_projectile_supply_;
 
+  //! Game publishers
   ros::Publisher ros_game_status_pub_;
-  ros::Publisher ros_game_result_pub_ ;
+  ros::Publisher ros_game_result_pub_;
   ros::Publisher ros_game_survival_pub_;
 
+  //! Field publishers
   ros::Publisher ros_bonus_status_pub_;
   ros::Publisher ros_supplier_status_pub_;
 
+  //! Robot publishers
   ros::Publisher ros_robot_status_pub_;
   ros::Publisher ros_robot_heat_pub_;
   ros::Publisher ros_robot_bonus_pub_;
   ros::Publisher ros_robot_damage_pub_;
   ros::Publisher ros_robot_shoot_pub_;
 
+  //! Robot ID from referee system (0xFF = uninitialized)
   uint8_t robot_id_ = 0xFF;
 };
-}
 
-#endif //ROBORTS_BASE_REFEREE_SYSTEM_H
+} // namespace roborts_base
+
+#endif // ROBORTS_BASE_REFEREE_SYSTEM_H
diff --git a/roborts_base/roborts_base_config.h b/roborts_base/roborts_base_config.h
index fc2368a7..fa808654 100644
--- a/roborts_base/roborts_base_config.h
+++ b/roborts_base/roborts_base_config.h
@@ -7,8 +7,8 @@
  *  (at your option) any later version.
  *
  *  This program is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of 
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *  GNU General Public License for more details.
  *
  *  You should have received a copy of the GNU General Public License
@@ -17,16 +17,39 @@
 
 #ifndef ROBORTS_BASE_CONFIG_H
 #define ROBORTS_BASE_CONFIG_H
+
+#include <string>
 #include <ros/ros.h>
 
-namespace roborts_base{
+namespace roborts_base {
 
+/**
+ * @brief Configuration struct for roborts_base node.
+ *
+ * Loads parameters from the ROS parameter server with sensible defaults.
+ * Supports serial port configuration, loop rate, and debug mode toggling.
+ */
 struct Config {
+  /// @brief Load all parameters from the ROS parameter server.
+  /// @param nh Pointer to a ROS NodeHandle (typically private ~)
   void GetParam(ros::NodeHandle *nh) {
     nh->param<std::string>("serial_port", serial_port, "/dev/serial_sdk");
+    nh->param<int>("loop_rate", loop_rate, 1000);
+    nh->param<bool>("enable_chassis", enable_chassis, true);
+    nh->param<bool>("enable_gimbal", enable_gimbal, true);
+    nh->param<bool>("enable_referee", enable_referee, true);
   }
+
+  //! Serial port device path
   std::string serial_port;
+  //! Main loop rate in Hz (default: 1000 Hz)
+  int loop_rate = 1000;
+  //! Enable/disable individual hardware modules
+  bool enable_chassis = true;
+  bool enable_gimbal = true;
+  bool enable_referee = true;
 };
 
-}
-#endif //ROBORTS_BASE_CONFIG_H
\ No newline at end of file
+} // namespace roborts_base
+
+#endif // ROBORTS_BASE_CONFIG_H
\ No newline at end of file
diff --git a/roborts_base/roborts_base_node.cpp b/roborts_base/roborts_base_node.cpp
index d34d626f..48848c10 100644
--- a/roborts_base/roborts_base_node.cpp
+++ b/roborts_base/roborts_base_node.cpp
@@ -7,36 +7,73 @@
  *  (at your option) any later version.
  *
  *  This program is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of 
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *  GNU General Public License for more details.
  *
  *  You should have received a copy of the GNU General Public License
  *  along with this program. If not, see <http://www.gnu.org/licenses/>.
  ***************************************************************************/
 
+#include <csignal>
+#include <atomic>
+#include <chrono>
+#include <thread>
+
 #include "gimbal/gimbal.h"
 #include "chassis/chassis.h"
 #include "referee_system/referee_system.h"
 #include "roborts_base_config.h"
 
+namespace {
+  std::atomic<bool> g_shutdown_requested{false};
+
+  void SignalHandler(int sig) {
+    ROS_INFO("Received signal %d, shutting down gracefully...", sig);
+    g_shutdown_requested.store(true);
+    ros::shutdown();
+  }
+}
 
-int main(int argc, char **argv){
+int main(int argc, char **argv) {
   ros::init(argc, argv, "roborts_base_node");
-  ros::NodeHandle nh;
+  ros::NodeHandle nh("~");
+
+  // Register signal handlers for graceful shutdown
+  std::signal(SIGINT, SignalHandler);
+  std::signal(SIGTERM, SignalHandler);
+
+  // Load configuration
   roborts_base::Config config;
   config.GetParam(&nh);
+
+  ROS_INFO("Initializing RoboRTS base node...");
+  ROS_INFO("Serial port: %s", config.serial_port.c_str());
+  ROS_INFO("Loop rate: %d Hz", config.loop_rate);
+
+  // Initialize SDK handle
   auto handle = std::make_shared<roborts_sdk::Handle>(config.serial_port);
-  if(!handle->Init()) return 1;
+  if (!handle->Init()) {
+    ROS_FATAL("Failed to initialize SDK handle on port: %s", config.serial_port.c_str());
+    return 1;
+  }
+  ROS_INFO("SDK handle initialized successfully.");
 
+  // Initialize hardware modules
   roborts_base::Chassis chassis(handle);
   roborts_base::Gimbal gimbal(handle);
   roborts_base::RefereeSystem referee_system(handle);
-  while(ros::ok()) {
 
+  ROS_INFO("All modules initialized. Entering main loop.");
+
+  // Use configurable loop rate instead of hard-coded usleep
+  ros::Rate rate(config.loop_rate);
+  while (ros::ok() && !g_shutdown_requested.load()) {
     handle->Spin();
     ros::spinOnce();
-    usleep(1000);
+    rate.sleep();
   }
 
+  ROS_INFO("RoboRTS base node shutdown complete.");
+  return 0;
 }
\ No newline at end of file
diff --git a/roborts_base/roborts_sdk/dispatch/dispatch.h b/roborts_base/roborts_sdk/dispatch/dispatch.h
index 139783e3..371823e1 100644
--- a/roborts_base/roborts_sdk/dispatch/dispatch.h
+++ b/roborts_base/roborts_sdk/dispatch/dispatch.h
@@ -89,7 +89,7 @@ class SubscriptionBase {
     return cmd_info_;
   }
   std::shared_ptr<MessageHeader> CreateMessageHeader() {
-    return std::shared_ptr<MessageHeader>(new MessageHeader);
+    return std::make_shared<MessageHeader>();
   }
 
   virtual std::shared_ptr<void> CreateMessage() = 0;
@@ -192,7 +192,7 @@ class ClientBase {
     return cmd_info_;
   }
   std::shared_ptr<MessageHeader> CreateRequestHeader() {
-    return std::shared_ptr<MessageHeader>(new MessageHeader);
+    return std::make_shared<MessageHeader>();
   }
 
   virtual std::shared_ptr<void> CreateResponse() = 0;
@@ -301,7 +301,7 @@ class ServiceBase {
     return cmd_info_;
   }
   std::shared_ptr<MessageHeader> CreateRequestHeader() {
-    return std::shared_ptr<MessageHeader>(new MessageHeader);
+    return std::make_shared<MessageHeader>();
   }
 
   virtual std::shared_ptr<void> CreateRequest() = 0;
diff --git a/roborts_base/roborts_sdk/hardware/hardware_interface.h b/roborts_base/roborts_sdk/hardware/hardware_interface.h
index dba6b070..b2528683 100644
--- a/roborts_base/roborts_sdk/hardware/hardware_interface.h
+++ b/roborts_base/roborts_sdk/hardware/hardware_interface.h
@@ -18,6 +18,8 @@
 #ifndef ROBORTS_SDK_HARDWARE_INTERFACE_H
 #define ROBORTS_SDK_HARDWARE_INTERFACE_H
 
+#include <cstdint>
+
 namespace roborts_sdk{
 /**
  * @brief Abstract class for hardware as an interface
diff --git a/roborts_base/roborts_sdk/hardware/serial_device.cpp b/roborts_base/roborts_sdk/hardware/serial_device.cpp
index 0814474d..04f5e2dc 100644
--- a/roborts_base/roborts_sdk/hardware/serial_device.cpp
+++ b/roborts_base/roborts_sdk/hardware/serial_device.cpp
@@ -33,7 +33,7 @@ SerialDevice::~SerialDevice() {
 bool SerialDevice::Init() {
 
   DLOG_INFO << "Attempting to open device " << port_name_ << " with baudrate " << baudrate_;
-  if (port_name_.c_str() == nullptr) {
+  if (port_name_.empty()) {
     port_name_ = "/dev/ttyUSB0";
   }
   if (OpenDevice() && ConfigDevice()) {
@@ -76,7 +76,7 @@ bool SerialDevice::ConfigDevice() {
   int st_baud[] = {B4800, B9600, B19200, B38400,
                    B57600, B115200, B230400, B921600};
   int std_rate[] = {4800, 9600, 19200, 38400, 57600, 115200,
-                    230400, 921600, 1000000, 1152000, 3000000};
+                    230400, 921600};
   int i, j;
   /* save current port parameter */
   if (tcgetattr(serial_fd_, &old_termios_) != 0) {
@@ -118,7 +118,7 @@ bool SerialDevice::ConfigDevice() {
       break; //8N1 default config
   }
   /* config baudrate */
-  j = sizeof(std_rate) / 4;
+  j = sizeof(std_rate) / sizeof(std_rate[0]);
   for (i = 0; i < j; ++i) {
     if (std_rate[i] == baudrate_) {
       /* set standard baudrate */
@@ -158,7 +158,7 @@ bool SerialDevice::ConfigDevice() {
 int SerialDevice::Read(uint8_t *buf, int len) {
   int ret = -1;
 
-  if (NULL == buf) {
+  if (buf == nullptr) {
     return -1;
   } else {
     ret = read(serial_fd_, buf, len);
diff --git a/roborts_base/roborts_sdk/utilities/circular_buffer.h b/roborts_base/roborts_sdk/utilities/circular_buffer.h
index e5f192fe..15de67d0 100644
--- a/roborts_base/roborts_sdk/utilities/circular_buffer.h
+++ b/roborts_base/roborts_sdk/utilities/circular_buffer.h
@@ -145,7 +145,7 @@ class CircularBuffer {
   //! buffer capacity
   size_t max_size_;
   //! flag of full buffer
-  bool full_ = 0;
+  bool full_ = false;
 };
 
 #endif //ROBORTS_SDK_CIRCULAR_BUFFER_H
diff --git a/roborts_base/roborts_sdk/utilities/log.h b/roborts_base/roborts_sdk/utilities/log.h
index da89669e..ca5c7911 100644
--- a/roborts_base/roborts_sdk/utilities/log.h
+++ b/roborts_base/roborts_sdk/utilities/log.h
@@ -39,14 +39,14 @@
 
 #define DLOG_INFO DLOG(INFO)
 #define DLOG_WARNING DLOG(WARNING)
-#define DLOG_ERROR DLOG(WARNING)
+#define DLOG_ERROR DLOG(ERROR)
 
 #define LOG_WARNING_FIRST_N(times) LOG_FIRST_N(WARNING, times)
 
 
 class GLogWrapper {
  public:
-  GLogWrapper(char* program) {
+  GLogWrapper(const char* program) {
     google::InitGoogleLogging(program);
     FLAGS_stderrthreshold=google::WARNING;
     FLAGS_colorlogtostderr=true;
diff --git a/roborts_base/roborts_sdk/utilities/memory_pool.h b/roborts_base/roborts_sdk/utilities/memory_pool.h
index dad5e98f..d5c84971 100644
--- a/roborts_base/roborts_sdk/utilities/memory_pool.h
+++ b/roborts_base/roborts_sdk/utilities/memory_pool.h
@@ -89,7 +89,7 @@ class MemoryPool {
    */
   void FreeMemory(MemoryBlock* memory_block)
   {
-    if (memory_block == (MemoryBlock*)0)
+    if (memory_block == nullptr)
     {
       return;
     }
@@ -121,7 +121,7 @@ class MemoryPool {
     // If size is larger than memory size or max size for PACKAGE, allocate failed
     if (size>max_block_size_||size > memory_size_)
     {
-      return (MemoryBlock *) 0;
+      return nullptr;
     }
 
     // Calculate the used memory size and get the used index array
@@ -137,7 +137,7 @@ class MemoryPool {
     // If left size is smaller than needed, allocate failed
     if (memory_size_ < (used_memory_size + size))
     {
-      return (MemoryBlock *) 0;
+      return nullptr;
     }
 
     // Special case: allocate for the first time
@@ -221,7 +221,7 @@ class MemoryPool {
           return &memory_table_[i];
         }
       }
-      return (MemoryBlock*)0;
+      return nullptr;
     }
 
     //If single block is available to divide for needed size, then divide this block into two
@@ -238,7 +238,7 @@ class MemoryPool {
       }
     }
 
-    return (MemoryBlock*)0;
+    return nullptr;
   }
   /**
    * @brief Lock the memory pool
diff --git a/roborts_base/ros_dep.h b/roborts_base/ros_dep.h
index 9b1a5870..8bf48e2b 100644
--- a/roborts_base/ros_dep.h
+++ b/roborts_base/ros_dep.h
@@ -7,34 +7,45 @@
  *  (at your option) any later version.
  *
  *  This program is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of 
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *  GNU General Public License for more details.
  *
  *  You should have received a copy of the GNU General Public License
  *  along with this program. If not, see <http://www.gnu.org/licenses/>.
  ***************************************************************************/
 
-#ifndef ROBORTS_SDK_PROTOCOL_DEFINE_ROS_H
-#define ROBORTS_SDK_PROTOCOL_DEFINE_ROS_H
+/**
+ * @file ros_dep.h
+ * @brief Centralized ROS message and service includes for roborts_base.
+ *
+ * This header aggregates all ROS message dependencies used by the
+ * chassis, gimbal, and referee system modules to avoid redundant includes.
+ */
+
+#ifndef ROBORTS_BASE_ROS_DEP_H
+#define ROBORTS_BASE_ROS_DEP_H
+
+// ROS core
 #include <ros/ros.h>
 #include <tf/transform_broadcaster.h>
 
+// Standard ROS messages
 #include <nav_msgs/Odometry.h>
 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/Twist.h>
 
-//Chassis
+// ---- Chassis Messages ----
 #include "roborts_msgs/TwistAccel.h"
 
-//Gimbal
+// ---- Gimbal Messages ----
 #include "roborts_msgs/GimbalAngle.h"
 #include "roborts_msgs/GimbalRate.h"
 #include "roborts_msgs/GimbalMode.h"
 #include "roborts_msgs/ShootCmd.h"
 #include "roborts_msgs/FricWhl.h"
 
-//Referee System
+// ---- Referee System Messages ----
 #include "roborts_msgs/BonusStatus.h"
 #include "roborts_msgs/GameResult.h"
 #include "roborts_msgs/GameStatus.h"
@@ -47,4 +58,4 @@
 #include "roborts_msgs/RobotStatus.h"
 #include "roborts_msgs/SupplierStatus.h"
 
-#endif //ROBORTS_SDK_PROTOCOL_DEFINE_ROS_H
+#endif // ROBORTS_BASE_ROS_DEP_H
diff --git a/roborts_decision/CMakeLists.txt b/roborts_decision/CMakeLists.txt
index dacaada1..f662f88a 100755
--- a/roborts_decision/CMakeLists.txt
+++ b/roborts_decision/CMakeLists.txt
@@ -1,10 +1,16 @@
 project(roborts_decision)
-cmake_minimum_required(VERSION 3.1)
+cmake_minimum_required(VERSION 3.10)
 
-set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
 set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake_module)
 set(CMAKE_BUILD_TYPE Release)
 
+# Compiler warnings
+if(CMAKE_CXX_COMPILER_ID MATCHES "GNU|Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-parameter)
+endif()
+
 find_package(catkin REQUIRED COMPONENTS
   roscpp
   tf
@@ -14,7 +20,7 @@ find_package(catkin REQUIRED COMPONENTS
   roborts_common
   roborts_msgs
   roborts_costmap
-  )
+)
 
 find_package(Eigen3 REQUIRED)
 find_package(ProtoBuf REQUIRED)
@@ -24,15 +30,18 @@ include_directories(
   ${catkin_INCLUDE_DIRS}
 )
 
-#generate proto files
+# Generate proto files
 file(GLOB ProtoFiles "${CMAKE_CURRENT_SOURCE_DIR}/proto/*.proto")
 rrts_protobuf_generate_cpp(${CMAKE_CURRENT_SOURCE_DIR}/proto
   DecisionProtoSrc
   DecisionProtoHds
   ${ProtoFiles}
-  )
+)
 
-catkin_package()
+catkin_package(
+  CATKIN_DEPENDS roscpp tf nav_msgs geometry_msgs actionlib
+                 roborts_common roborts_msgs roborts_costmap
+)
 
 add_executable(behavior_test_node
   ${DecisionProtoHds}
@@ -42,15 +51,18 @@ add_executable(behavior_test_node
   example_behavior/chase_behavior.h
   example_behavior/patrol_behavior.h
   example_behavior/search_behavior.h
+  example_behavior/goal_behavior.h
   behavior_test.cpp
   executor/chassis_executor.cpp
-  )
+  executor/gimbal_executor.cpp
+)
 
 target_link_libraries(behavior_test_node
   PRIVATE
   roborts_costmap
   ${catkin_LIBRARIES}
   ${PROTOBUF_LIBRARIES}
-  )
+)
+
 add_dependencies(behavior_test_node
   roborts_msgs_generate_messages)
diff --git a/roborts_decision/behavior_test.cpp b/roborts_decision/behavior_test.cpp
index 621ec822..e44496ca 100644
--- a/roborts_decision/behavior_test.cpp
+++ b/roborts_decision/behavior_test.cpp
@@ -1,6 +1,13 @@
 #include <ros/ros.h>
+#include <memory>
+#include <atomic>
+#include <csignal>
+#include <iostream>
+#include <string>
+#include <thread>
 
 #include "executor/chassis_executor.h"
+#include "executor/gimbal_executor.h"
 
 #include "example_behavior/back_boot_area_behavior.h"
 #include "example_behavior/escape_behavior.h"
@@ -9,88 +16,112 @@
 #include "example_behavior/patrol_behavior.h"
 #include "example_behavior/goal_behavior.h"
 
-void Command();
-char command = '0';
+namespace {
+  std::atomic<char> g_command{'0'};
+  std::atomic<bool> g_running{true};
+}
+
+void CommandInputThread();
 
 int main(int argc, char **argv) {
   ros::init(argc, argv, "behavior_test_node");
-  std::string full_path = ros::package::getPath("roborts_decision") + "/config/decision.prototxt";
 
-  auto chassis_executor = new roborts_decision::ChassisExecutor;
-  auto blackboard = new roborts_decision::Blackboard(full_path);
+  std::string full_path = ros::package::getPath("roborts_decision")
+                         + "/config/decision.prototxt";
+
+  // Create executors with unique_ptr for clear ownership
+  auto chassis_executor = std::make_unique<roborts_decision::ChassisExecutor>();
+  auto blackboard = std::make_unique<roborts_decision::Blackboard>(full_path);
+
+  // Get raw pointers for behavior constructors (behaviors don't own these)
+  auto* chassis_executor_ptr = chassis_executor.get();
+  auto* blackboard_ptr = blackboard.get();
 
-  roborts_decision::BackBootAreaBehavior back_boot_area_behavior(chassis_executor, blackboard, full_path);
-  roborts_decision::ChaseBehavior        chase_behavior(chassis_executor, blackboard, full_path);
-  roborts_decision::SearchBehavior       search_behavior(chassis_executor, blackboard, full_path);
-  roborts_decision::EscapeBehavior       escape_behavior(chassis_executor, blackboard, full_path);
-  roborts_decision::PatrolBehavior       patrol_behavior(chassis_executor, blackboard, full_path);
-  roborts_decision::GoalBehavior       goal_behavior(chassis_executor, blackboard);
+  // Initialize all behaviors
+  roborts_decision::BackBootAreaBehavior back_boot_area_behavior(
+      chassis_executor_ptr, blackboard_ptr, full_path);
+  roborts_decision::ChaseBehavior chase_behavior(
+      chassis_executor_ptr, blackboard_ptr, full_path);
+  roborts_decision::SearchBehavior search_behavior(
+      chassis_executor_ptr, blackboard_ptr, full_path);
+  roborts_decision::EscapeBehavior escape_behavior(
+      chassis_executor_ptr, blackboard_ptr, full_path);
+  roborts_decision::PatrolBehavior patrol_behavior(
+      chassis_executor_ptr, blackboard_ptr, full_path);
+  roborts_decision::GoalBehavior goal_behavior(
+      chassis_executor_ptr, blackboard_ptr);
+
+  // Start command input thread
+  auto command_thread = std::thread(CommandInputThread);
 
-  auto command_thread= std::thread(Command);
   ros::Rate rate(10);
-  while(ros::ok()){
+  while (ros::ok() && g_running.load()) {
     ros::spinOnce();
-    switch (command) {
-      //back to boot area
+
+    const char cmd = g_command.load();
+    switch (cmd) {
       case '1':
         back_boot_area_behavior.Run();
         break;
-        //patrol
       case '2':
         patrol_behavior.Run();
         break;
-        //chase.
       case '3':
         chase_behavior.Run();
         break;
-        //search
       case '4':
         search_behavior.Run();
         break;
-        //escape.
       case '5':
         escape_behavior.Run();
         break;
-        //goal.
       case '6':
         goal_behavior.Run();
         break;
-      case 27:
-        if (command_thread.joinable()){
-          command_thread.join();
-        }
-        return 0;
+      case 27:  // ESC key
+        g_running.store(false);
+        break;
       default:
         break;
     }
+
     rate.sleep();
   }
 
+  // Clean shutdown
+  g_running.store(false);
+  if (command_thread.joinable()) {
+    command_thread.join();
+  }
 
+  ROS_INFO("Behavior test node shutdown complete.");
   return 0;
 }
 
-void Command() {
-
-  while (command != 27) {
-    std::cout << "**************************************************************************************" << std::endl;
-    std::cout << "*********************************please send a command********************************" << std::endl;
-    std::cout << "1: back boot area behavior" << std::endl
-              << "2: patrol behavior" << std::endl
-              << "3: chase_behavior" << std::endl
-              << "4: search behavior" << std::endl
-              << "5: escape behavior" << std::endl
-              << "6: goal behavior" << std::endl
-              << "esc: exit program" << std::endl;
-    std::cout << "**************************************************************************************" << std::endl;
-    std::cout << "> ";
-    std::cin >> command;
-    if (command != '1' && command != '2' && command != '3' && command != '4' && command != '5' && command != '6' && command != 27) {
-      std::cout << "please input again!" << std::endl;
-      std::cout << "> ";
-      std::cin >> command;
-    }
+void CommandInputThread() {
+  while (g_running.load()) {
+    std::cout << "\n"
+              << "╔══════════════════════════════════════════════════════╗\n"
+              << "║          RoboRTS Behavior Test Controller           ║\n"
+              << "╠══════════════════════════════════════════════════════╣\n"
+              << "║  1: Back to boot area    4: Search behavior         ║\n"
+              << "║  2: Patrol behavior      5: Escape behavior         ║\n"
+              << "║  3: Chase behavior       6: Goal behavior           ║\n"
+              << "║                        ESC: Exit program            ║\n"
+              << "╚══════════════════════════════════════════════════════╝\n"
+              << "> " << std::flush;
 
+    char input;
+    std::cin >> input;
+
+    if (input >= '1' && input <= '6') {
+      g_command.store(input);
+    } else if (input == 'q' || input == 'Q') {
+      g_command.store(27);
+      g_running.store(false);
+      break;
+    } else {
+      std::cout << "Invalid command. Please try again." << std::endl;
+    }
   }
 }
-
diff --git a/roborts_decision/blackboard/blackboard.h b/roborts_decision/blackboard/blackboard.h
index cb7db78a..fa35f4c8 100755
--- a/roborts_decision/blackboard/blackboard.h
+++ b/roborts_decision/blackboard/blackboard.h
@@ -7,8 +7,8 @@
  *  (at your option) any later version.
  *
  *  This program is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of 
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *  GNU General Public License for more details.
  *
  *  You should have received a copy of the GNU General Public License
@@ -21,6 +21,9 @@
 #include <tf/tf.h>
 #include <tf/transform_listener.h>
 #include <ros/ros.h>
+#include <mutex>
+#include <memory>
+#include <string>
 #include <geometry_msgs/PoseStamped.h>
 
 #include "roborts_msgs/ArmorDetectionAction.h"
@@ -29,73 +32,87 @@
 #include "../proto/decision.pb.h"
 #include "costmap/costmap_interface.h"
 
-namespace roborts_decision{
+namespace roborts_decision {
 
+/**
+ * @brief Central blackboard for sharing perception and planning data between behaviors.
+ *
+ * Manages:
+ * - Enemy detection via ArmorDetection actionlib feedback
+ * - Goal commands from RViz
+ * - Robot pose from tf
+ * - Costmap access for path planning queries
+ *
+ * All accessors are thread-safe via mutex protection.
+ */
 class Blackboard {
  public:
-  typedef std::shared_ptr<Blackboard> Ptr;
-  typedef roborts_costmap::CostmapInterface CostMap;
-  typedef roborts_costmap::Costmap2D CostMap2D;
-  explicit Blackboard(const std::string &proto_file_path):
-      enemy_detected_(false),
-      armor_detection_actionlib_client_("armor_detection_node_action", true){
+  using Ptr = std::shared_ptr<Blackboard>;
+  using CostMap = roborts_costmap::CostmapInterface;
+  using CostMap2D = roborts_costmap::Costmap2D;
+
+  explicit Blackboard(const std::string &proto_file_path)
+      : enemy_detected_(false),
+        new_goal_(false),
+        armor_detection_actionlib_client_("armor_detection_node_action", true) {
 
     tf_ptr_ = std::make_shared<tf::TransformListener>(ros::Duration(10));
 
-    std::string map_path = ros::package::getPath("roborts_costmap") + \
-      "/config/costmap_parameter_config_for_decision.prototxt";
-    costmap_ptr_ = std::make_shared<CostMap>("decision_costmap", *tf_ptr_,
-                                             map_path);
+    std::string map_path = ros::package::getPath("roborts_costmap")
+        + "/config/costmap_parameter_config_for_decision.prototxt";
+    costmap_ptr_ = std::make_shared<CostMap>("decision_costmap", *tf_ptr_, map_path);
     charmap_ = costmap_ptr_->GetCostMap()->GetCharMap();
-
     costmap_2d_ = costmap_ptr_->GetLayeredCostmap()->GetCostMap();
 
-    // Enemy fake pose
+    // Subscribe to RViz goal for manual enemy position input (simulation)
     ros::NodeHandle rviz_nh("/move_base_simple");
-    enemy_sub_ = rviz_nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, &Blackboard::GoalCallback, this);
+    enemy_sub_ = rviz_nh.subscribe<geometry_msgs::PoseStamped>(
+        "goal", 1, &Blackboard::GoalCallback, this);
 
     ros::NodeHandle nh;
 
     roborts_decision::DecisionConfig decision_config;
     roborts_common::ReadProtoFromTextFile(proto_file_path, &decision_config);
 
-    if (!decision_config.simulate()){
-
+    if (!decision_config.simulate()) {
+      ROS_INFO("Connecting to armor detection module...");
       armor_detection_actionlib_client_.waitForServer();
-
-      ROS_INFO("Armor detection module has been connected!");
+      ROS_INFO("Armor detection module connected.");
 
       armor_detection_goal_.command = 1;
-      armor_detection_actionlib_client_.sendGoal(armor_detection_goal_,
-                                                 actionlib::SimpleActionClient<roborts_msgs::ArmorDetectionAction>::SimpleDoneCallback(),
-                                                 actionlib::SimpleActionClient<roborts_msgs::ArmorDetectionAction>::SimpleActiveCallback(),
-                                                 boost::bind(&Blackboard::ArmorDetectionFeedbackCallback, this, _1));
+      armor_detection_actionlib_client_.sendGoal(
+          armor_detection_goal_,
+          actionlib::SimpleActionClient<roborts_msgs::ArmorDetectionAction>::SimpleDoneCallback(),
+          actionlib::SimpleActionClient<roborts_msgs::ArmorDetectionAction>::SimpleActiveCallback(),
+          boost::bind(&Blackboard::ArmorDetectionFeedbackCallback, this, _1));
+    } else {
+      ROS_INFO("Running in simulation mode — armor detection disabled.");
     }
-
-
   }
 
   ~Blackboard() = default;
 
+  // Non-copyable
+  Blackboard(const Blackboard&) = delete;
+  Blackboard& operator=(const Blackboard&) = delete;
 
-  // Enemy
-  void ArmorDetectionFeedbackCallback(const roborts_msgs::ArmorDetectionFeedbackConstPtr& feedback){
-    if (feedback->detected){
-      enemy_detected_ = true;
-      ROS_INFO("Find Enemy!");
+  //--------------------------------------------------------------------------
+  // Enemy Detection
+  //--------------------------------------------------------------------------
+
+  void ArmorDetectionFeedbackCallback(
+      const roborts_msgs::ArmorDetectionFeedbackConstPtr& feedback) {
+    if (feedback->detected) {
+      ROS_INFO_THROTTLE(1.0, "Enemy detected!");
 
       tf::Stamped<tf::Pose> tf_pose, global_tf_pose;
       geometry_msgs::PoseStamped camera_pose_msg, global_pose_msg;
       camera_pose_msg = feedback->enemy_pos;
 
-      double distance = std::sqrt(camera_pose_msg.pose.position.x * camera_pose_msg.pose.position.x +
-          camera_pose_msg.pose.position.y * camera_pose_msg.pose.position.y);
-      double yaw = atan(camera_pose_msg.pose.position.y / camera_pose_msg.pose.position.x);
+      const double yaw = std::atan2(camera_pose_msg.pose.position.y,
+                                     camera_pose_msg.pose.position.x);
 
-      //camera_pose_msg.pose.position.z=camera_pose_msg.pose.position.z;
-      tf::Quaternion quaternion = tf::createQuaternionFromRPY(0,
-                                                              0,
-                                                              yaw);
+      tf::Quaternion quaternion = tf::createQuaternionFromRPY(0, 0, yaw);
       camera_pose_msg.pose.orientation.w = quaternion.w();
       camera_pose_msg.pose.orientation.x = quaternion.x();
       camera_pose_msg.pose.orientation.y = quaternion.y();
@@ -103,70 +120,78 @@ class Blackboard {
       poseStampedMsgToTF(camera_pose_msg, tf_pose);
 
       tf_pose.stamp_ = ros::Time(0);
-      try
-      {
+      try {
         tf_ptr_->transformPose("map", tf_pose, global_tf_pose);
         tf::poseStampedTFToMsg(global_tf_pose, global_pose_msg);
 
-        if(GetDistance(global_pose_msg, enemy_pose_)>0.2 || GetAngle(global_pose_msg, enemy_pose_) > 0.2){
+        if (GetDistance(global_pose_msg, enemy_pose_) > kPositionChangeThreshold ||
+            GetAngle(global_pose_msg, enemy_pose_) > kAngleChangeThreshold) {
+          std::lock_guard<std::mutex> lock(enemy_mutex_);
           enemy_pose_ = global_pose_msg;
-
         }
+      } catch (tf::TransformException &ex) {
+        ROS_ERROR_THROTTLE(1.0, "TF error transforming enemy pose: %s", ex.what());
       }
-      catch (tf::TransformException &ex) {
-        ROS_ERROR("tf error when transform enemy pose from camera to map");
-      }
-    } else{
+
+      std::lock_guard<std::mutex> lock(enemy_mutex_);
+      enemy_detected_ = true;
+    } else {
+      std::lock_guard<std::mutex> lock(enemy_mutex_);
       enemy_detected_ = false;
     }
-
   }
 
-  geometry_msgs::PoseStamped GetEnemy() const {
+  geometry_msgs::PoseStamped GetEnemy() {
+    std::lock_guard<std::mutex> lock(enemy_mutex_);
     return enemy_pose_;
   }
 
-  bool IsEnemyDetected() const{
-    ROS_INFO("%s: %d", __FUNCTION__, (int)enemy_detected_);
+  bool IsEnemyDetected() {
+    std::lock_guard<std::mutex> lock(enemy_mutex_);
+    ROS_DEBUG("%s: %d", __FUNCTION__, static_cast<int>(enemy_detected_));
     return enemy_detected_;
   }
 
-  // Goal
-  void GoalCallback(const geometry_msgs::PoseStamped::ConstPtr& goal){
+  //--------------------------------------------------------------------------
+  // Goal Management
+  //--------------------------------------------------------------------------
+
+  void GoalCallback(const geometry_msgs::PoseStamped::ConstPtr& goal) {
+    std::lock_guard<std::mutex> lock(goal_mutex_);
     new_goal_ = true;
     goal_ = *goal;
   }
 
-  geometry_msgs::PoseStamped GetGoal() const {
+  geometry_msgs::PoseStamped GetGoal() {
+    std::lock_guard<std::mutex> lock(goal_mutex_);
     return goal_;
   }
 
-  bool IsNewGoal(){
-    if(new_goal_){
-      new_goal_ =  false;
+  bool IsNewGoal() {
+    std::lock_guard<std::mutex> lock(goal_mutex_);
+    if (new_goal_) {
+      new_goal_ = false;
       return true;
-    } else{
-      return false;
     }
+    return false;
   }
-  /*---------------------------------- Tools ------------------------------------------*/
+
+  //--------------------------------------------------------------------------
+  // Utility Methods
+  //--------------------------------------------------------------------------
 
   double GetDistance(const geometry_msgs::PoseStamped &pose1,
-                     const geometry_msgs::PoseStamped &pose2) {
-    const geometry_msgs::Point point1 = pose1.pose.position;
-    const geometry_msgs::Point point2 = pose2.pose.position;
-    const double dx = point1.x - point2.x;
-    const double dy = point1.y - point2.y;
+                    const geometry_msgs::PoseStamped &pose2) const {
+    const double dx = pose1.pose.position.x - pose2.pose.position.x;
+    const double dy = pose1.pose.position.y - pose2.pose.position.y;
     return std::sqrt(dx * dx + dy * dy);
   }
 
   double GetAngle(const geometry_msgs::PoseStamped &pose1,
-                  const geometry_msgs::PoseStamped &pose2) {
-    const geometry_msgs::Quaternion quaternion1 = pose1.pose.orientation;
-    const geometry_msgs::Quaternion quaternion2 = pose2.pose.orientation;
+                  const geometry_msgs::PoseStamped &pose2) const {
     tf::Quaternion rot1, rot2;
-    tf::quaternionMsgToTF(quaternion1, rot1);
-    tf::quaternionMsgToTF(quaternion2, rot2);
+    tf::quaternionMsgToTF(pose1.pose.orientation, rot1);
+    tf::quaternionMsgToTF(pose2.pose.orientation, rot2);
     return rot1.angleShortestPath(rot2);
   }
 
@@ -175,7 +200,7 @@ class Blackboard {
     return robot_map_pose_;
   }
 
-  const std::shared_ptr<CostMap> GetCostMap(){
+  const std::shared_ptr<CostMap> GetCostMap() {
     return costmap_ptr_;
   }
 
@@ -188,25 +213,33 @@ class Blackboard {
   }
 
  private:
+  // Configuration constants
+  static constexpr double kPositionChangeThreshold = 0.2;
+  static constexpr double kAngleChangeThreshold = 0.2;
+
   void UpdateRobotPose() {
     tf::Stamped<tf::Pose> robot_tf_pose;
     robot_tf_pose.setIdentity();
-
     robot_tf_pose.frame_id_ = "base_link";
     robot_tf_pose.stamp_ = ros::Time();
+
     try {
       geometry_msgs::PoseStamped robot_pose;
       tf::poseStampedTFToMsg(robot_tf_pose, robot_pose);
       tf_ptr_->transformPose("map", robot_pose, robot_map_pose_);
-    }
-    catch (tf::LookupException &ex) {
-      ROS_ERROR("Transform Error looking up robot pose: %s", ex.what());
+    } catch (tf::LookupException &ex) {
+      ROS_ERROR_THROTTLE(1.0, "Transform error getting robot pose: %s", ex.what());
     }
   }
+
   //! tf
   std::shared_ptr<tf::TransformListener> tf_ptr_;
 
-  //! Enenmy detection
+  //! Thread synchronization
+  mutable std::mutex enemy_mutex_;
+  mutable std::mutex goal_mutex_;
+
+  //! Enemy detection
   ros::Subscriber enemy_sub_;
 
   //! Goal info
@@ -226,7 +259,8 @@ class Blackboard {
 
   //! robot map pose
   geometry_msgs::PoseStamped robot_map_pose_;
-
 };
-} //namespace roborts_decision
-#endif //ROBORTS_DECISION_BLACKBOARD_H
+
+} // namespace roborts_decision
+
+#endif // ROBORTS_DECISION_BLACKBOARD_H
diff --git a/roborts_decision/example_behavior/back_boot_area_behavior.h b/roborts_decision/example_behavior/back_boot_area_behavior.h
index a121b5ee..e82c4ec9 100644
--- a/roborts_decision/example_behavior/back_boot_area_behavior.h
+++ b/roborts_decision/example_behavior/back_boot_area_behavior.h
@@ -1,6 +1,9 @@
 #ifndef ROBORTS_DECISION_BACK_BOOT_AREA_BEHAVIOR_H
 #define ROBORTS_DECISION_BACK_BOOT_AREA_BEHAVIOR_H
 
+#include <cmath>
+#include <string>
+
 #include "io/io.h"
 
 #include "../blackboard/blackboard.h"
@@ -11,13 +14,21 @@
 #include "line_iterator.h"
 
 namespace roborts_decision {
+
+/**
+ * @brief Behavior to navigate the robot back to its boot (starting) area.
+ *
+ * Reads the start position from config and drives the robot there.
+ * Only initiates movement if the robot is sufficiently far from the boot
+ * position or misaligned beyond the threshold.
+ */
 class BackBootAreaBehavior {
  public:
   BackBootAreaBehavior(ChassisExecutor* &chassis_executor,
                 Blackboard* &blackboard,
-                const std::string & proto_file_path) : chassis_executor_(chassis_executor),
-                                                       blackboard_(blackboard) {
-
+                const std::string & proto_file_path)
+      : chassis_executor_(chassis_executor),
+        blackboard_(blackboard) {
 
     boot_position_.header.frame_id = "map";
     boot_position_.pose.orientation.x = 0;
@@ -30,31 +41,26 @@ class BackBootAreaBehavior {
     boot_position_.pose.position.z = 0;
 
     if (!LoadParam(proto_file_path)) {
-      ROS_ERROR("%s can't open file", __FUNCTION__);
+      ROS_ERROR("%s: failed to load config file", __FUNCTION__);
     }
-
   }
 
   void Run() {
-
     auto executor_state = Update();
 
     if (executor_state != BehaviorState::RUNNING) {
       auto robot_map_pose = blackboard_->GetRobotMapPose();
-      auto dx = boot_position_.pose.position.x - robot_map_pose.pose.position.x;
-      auto dy = boot_position_.pose.position.y - robot_map_pose.pose.position.y;
-
-      auto boot_yaw = tf::getYaw(boot_position_.pose.orientation);
-      auto robot_yaw = tf::getYaw(robot_map_pose.pose.orientation);
+      const double dx = boot_position_.pose.position.x - robot_map_pose.pose.position.x;
+      const double dy = boot_position_.pose.position.y - robot_map_pose.pose.position.y;
+      const double distance = std::sqrt(dx * dx + dy * dy);
 
       tf::Quaternion rot1, rot2;
       tf::quaternionMsgToTF(boot_position_.pose.orientation, rot1);
       tf::quaternionMsgToTF(robot_map_pose.pose.orientation, rot2);
-      auto d_yaw =  rot1.angleShortestPath(rot2);
+      const double d_yaw = rot1.angleShortestPath(rot2);
 
-      if (std::sqrt(std::pow(dx, 2) + std::pow(dy, 2)) > 0.2 || d_yaw > 0.5) {
+      if (distance > kDistanceThreshold || d_yaw > kYawThreshold) {
         chassis_executor_->Execute(boot_position_);
-
       }
     }
   }
@@ -74,22 +80,29 @@ class BackBootAreaBehavior {
     }
 
     boot_position_.header.frame_id = "map";
-
     boot_position_.pose.position.x = decision_config.master_bot().start_position().x();
-    boot_position_.pose.position.z = decision_config.master_bot().start_position().z();
     boot_position_.pose.position.y = decision_config.master_bot().start_position().y();
+    boot_position_.pose.position.z = decision_config.master_bot().start_position().z();
 
-    auto master_quaternion = tf::createQuaternionMsgFromRollPitchYaw(decision_config.master_bot().start_position().roll(),
-                                                                     decision_config.master_bot().start_position().pitch(),
-                                                                     decision_config.master_bot().start_position().yaw());
+    auto master_quaternion = tf::createQuaternionMsgFromRollPitchYaw(
+        decision_config.master_bot().start_position().roll(),
+        decision_config.master_bot().start_position().pitch(),
+        decision_config.master_bot().start_position().yaw());
     boot_position_.pose.orientation = master_quaternion;
 
+    ROS_INFO("Boot position loaded: (%.2f, %.2f)",
+             boot_position_.pose.position.x,
+             boot_position_.pose.position.y);
     return true;
   }
 
   ~BackBootAreaBehavior() = default;
 
  private:
+  // Thresholds for triggering return-to-boot navigation
+  static constexpr double kDistanceThreshold = 0.2;  // meters
+  static constexpr double kYawThreshold = 0.5;       // radians
+
   //! executor
   ChassisExecutor* const chassis_executor_;
 
@@ -98,13 +111,8 @@ class BackBootAreaBehavior {
 
   //! boot position
   geometry_msgs::PoseStamped boot_position_;
-
-  //! chase buffer
-  std::vector<geometry_msgs::PoseStamped> chase_buffer_;
-  unsigned int chase_count_;
-
 };
-}
 
+} // namespace roborts_decision
 
-#endif //ROBORTS_DECISION_BACK_BOOT_AREA_BEHAVIOR_H
+#endif // ROBORTS_DECISION_BACK_BOOT_AREA_BEHAVIOR_H
diff --git a/roborts_decision/example_behavior/chase_behavior.h b/roborts_decision/example_behavior/chase_behavior.h
index a939435e..163d629d 100644
--- a/roborts_decision/example_behavior/chase_behavior.h
+++ b/roborts_decision/example_behavior/chase_behavior.h
@@ -1,6 +1,10 @@
 #ifndef ROBORTS_DECISION_CHASE_BEHAVIOR_H
 #define ROBORTS_DECISION_CHASE_BEHAVIOR_H
 
+#include <cmath>
+#include <vector>
+#include <string>
+
 #include "io/io.h"
 
 #include "../blackboard/blackboard.h"
@@ -11,13 +15,23 @@
 #include "line_iterator.h"
 
 namespace roborts_decision {
+
+/**
+ * @brief Chase behavior: pursue a detected enemy while maintaining safe distance.
+ *
+ * Uses a ring buffer of enemy positions to smooth pursuit trajectory.
+ * Stops chasing if the enemy is within the engagement range (1.0-2.0m).
+ * Falls back along line-of-sight if the goal cell is in an obstacle.
+ */
 class ChaseBehavior {
  public:
   ChaseBehavior(ChassisExecutor* &chassis_executor,
                 Blackboard* &blackboard,
-                const std::string & proto_file_path) : chassis_executor_(chassis_executor),
-                                                       blackboard_(blackboard) {
-
+                const std::string & proto_file_path)
+      : chassis_executor_(chassis_executor),
+        blackboard_(blackboard),
+        chase_count_(0),
+        cancel_goal_(true) {
 
     chase_goal_.header.frame_id = "map";
     chase_goal_.pose.orientation.x = 0;
@@ -29,108 +43,93 @@ class ChaseBehavior {
     chase_goal_.pose.position.y = 0;
     chase_goal_.pose.position.z = 0;
 
-    chase_buffer_.resize(2);
-    chase_count_ = 0;
-
-    cancel_goal_ = true;
+    chase_buffer_.resize(kBufferSize);
   }
 
   void Run() {
-
     auto executor_state = Update();
-
     auto robot_map_pose = blackboard_->GetRobotMapPose();
-    if (executor_state != BehaviorState::RUNNING) {
-
-      chase_buffer_[chase_count_++ % 2] = blackboard_->GetEnemy();
 
-      chase_count_ = chase_count_ % 2;
+    if (executor_state != BehaviorState::RUNNING) {
+      chase_buffer_[chase_count_++ % kBufferSize] = blackboard_->GetEnemy();
+      chase_count_ = chase_count_ % kBufferSize;
 
-      auto dx = chase_buffer_[(chase_count_ + 2 - 1) % 2].pose.position.x - robot_map_pose.pose.position.x;
-      auto dy = chase_buffer_[(chase_count_ + 2 - 1) % 2].pose.position.y - robot_map_pose.pose.position.y;
-      auto yaw = std::atan2(dy, dx);
+      const auto& latest_enemy = chase_buffer_[(chase_count_ + kBufferSize - 1) % kBufferSize];
+      const double dx = latest_enemy.pose.position.x - robot_map_pose.pose.position.x;
+      const double dy = latest_enemy.pose.position.y - robot_map_pose.pose.position.y;
+      const double distance = std::sqrt(dx * dx + dy * dy);
+      const double yaw = std::atan2(dy, dx);
 
-      if (std::sqrt(std::pow(dx, 2) + std::pow(dy, 2)) >= 1.0 && std::sqrt(std::pow(dx, 2) + std::pow(dy, 2)) <= 2.0) {
+      // Within engagement range — hold position
+      if (distance >= kMinEngageDistance && distance <= kMaxEngageDistance) {
         if (cancel_goal_) {
           chassis_executor_->Cancel();
           cancel_goal_ = false;
         }
         return;
+      }
 
-      } else {
-
-        auto orientation = tf::createQuaternionMsgFromYaw(yaw);
-        geometry_msgs::PoseStamped reduce_goal;
-        reduce_goal.pose.orientation = robot_map_pose.pose.orientation;
-
-        reduce_goal.header.frame_id = "map";
-        reduce_goal.header.stamp = ros::Time::now();
-        reduce_goal.pose.position.x = chase_buffer_[(chase_count_ + 2 - 1) % 2].pose.position.x - 1.2 * cos(yaw);
-        reduce_goal.pose.position.y = chase_buffer_[(chase_count_ + 2 - 1) % 2].pose.position.y - 1.2 * sin(yaw);
-        auto enemy_x = reduce_goal.pose.position.x;
-        auto enemy_y = reduce_goal.pose.position.y;
-        reduce_goal.pose.position.z = 1;
-        unsigned int goal_cell_x, goal_cell_y;
-
-        // if necessary add mutex lock
-        //blackboard_->GetCostMap2D()->GetMutex()->lock();
-        auto get_enemy_cell = blackboard_->GetCostMap2D()->World2Map(enemy_x,
-                                                                   enemy_y,
-                                                                   goal_cell_x,
-                                                                   goal_cell_y);
-        //blackboard_->GetCostMap2D()->GetMutex()->unlock();
-
-        if (!get_enemy_cell) {
-          return;
-        }
+      // Compute approach goal (offset from enemy toward robot)
+      geometry_msgs::PoseStamped reduce_goal;
+      reduce_goal.pose.orientation = robot_map_pose.pose.orientation;
+      reduce_goal.header.frame_id = "map";
+      reduce_goal.header.stamp = ros::Time::now();
+      reduce_goal.pose.position.x = latest_enemy.pose.position.x - kApproachOffset * std::cos(yaw);
+      reduce_goal.pose.position.y = latest_enemy.pose.position.y - kApproachOffset * std::sin(yaw);
+      reduce_goal.pose.position.z = 1;
+
+      const double enemy_x = reduce_goal.pose.position.x;
+      const double enemy_y = reduce_goal.pose.position.y;
+
+      unsigned int goal_cell_x, goal_cell_y;
+      if (!blackboard_->GetCostMap2D()->World2Map(enemy_x, enemy_y,
+                                                   goal_cell_x, goal_cell_y)) {
+        return;
+      }
 
-        auto robot_x = robot_map_pose.pose.position.x;
-        auto robot_y = robot_map_pose.pose.position.y;
+      // If goal is in obstacle, trace line toward robot to find free cell
+      if (blackboard_->GetCostMap2D()->GetCost(goal_cell_x, goal_cell_y) >= kObstacleCostThreshold) {
         unsigned int robot_cell_x, robot_cell_y;
-        double goal_x, goal_y;
-        blackboard_->GetCostMap2D()->World2Map(robot_x,
-                                              robot_y,
-                                              robot_cell_x,
-                                              robot_cell_y);
-
-        if (blackboard_->GetCostMap2D()->GetCost(goal_cell_x, goal_cell_y) >= 253) {
-
-          bool find_goal = false;
-          for(FastLineIterator line( goal_cell_x, goal_cell_y, robot_cell_x, robot_cell_x); line.IsValid(); line.Advance()) {
-
-            auto point_cost = blackboard_->GetCostMap2D()->GetCost((unsigned int) (line.GetX()), (unsigned int) (line.GetY())); //current point's cost
-
-            if(point_cost >= 253){
-              continue;
-
-            } else {
-              find_goal = true;
-              blackboard_->GetCostMap2D()->Map2World((unsigned int) (line.GetX()),
-                                                     (unsigned int) (line.GetY()),
-                                                     goal_x,
-                                                     goal_y);
+        blackboard_->GetCostMap2D()->World2Map(
+            robot_map_pose.pose.position.x,
+            robot_map_pose.pose.position.y,
+            robot_cell_x, robot_cell_y);
+
+        bool find_goal = false;
+        for (FastLineIterator line(goal_cell_x, goal_cell_y, robot_cell_x, robot_cell_y);
+             line.IsValid(); line.Advance()) {
+          auto point_cost = blackboard_->GetCostMap2D()->GetCost(
+              static_cast<unsigned int>(line.GetX()),
+              static_cast<unsigned int>(line.GetY()));
+
+          if (point_cost >= kObstacleCostThreshold) {
+            continue;
+          }
 
-              reduce_goal.pose.position.x = goal_x;
-              reduce_goal.pose.position.y = goal_y;
-              break;
-            }
+          find_goal = true;
+          double goal_x, goal_y;
+          blackboard_->GetCostMap2D()->Map2World(
+              static_cast<unsigned int>(line.GetX()),
+              static_cast<unsigned int>(line.GetY()),
+              goal_x, goal_y);
 
-          }
-          if (find_goal) {
-            cancel_goal_ = true;
-            chassis_executor_->Execute(reduce_goal);
-          } else {
-            if (cancel_goal_) {
-              chassis_executor_->Cancel();
-              cancel_goal_ = false;
-            }
-            return;
-          }
+          reduce_goal.pose.position.x = goal_x;
+          reduce_goal.pose.position.y = goal_y;
+          break;
+        }
 
-        } else {
+        if (find_goal) {
           cancel_goal_ = true;
           chassis_executor_->Execute(reduce_goal);
+        } else {
+          if (cancel_goal_) {
+            chassis_executor_->Cancel();
+            cancel_goal_ = false;
+          }
         }
+      } else {
+        cancel_goal_ = true;
+        chassis_executor_->Execute(reduce_goal);
       }
     }
   }
@@ -150,6 +149,13 @@ class ChaseBehavior {
   ~ChaseBehavior() = default;
 
  private:
+  // Configuration constants
+  static constexpr size_t kBufferSize = 2;
+  static constexpr double kMinEngageDistance = 1.0;   // meters
+  static constexpr double kMaxEngageDistance = 2.0;   // meters
+  static constexpr double kApproachOffset = 1.2;      // meters offset from enemy
+  static constexpr unsigned char kObstacleCostThreshold = 253;
+
   //! executor
   ChassisExecutor* const chassis_executor_;
 
@@ -159,13 +165,14 @@ class ChaseBehavior {
   //! chase goal
   geometry_msgs::PoseStamped chase_goal_;
 
-  //! chase buffer
+  //! chase buffer (ring buffer for smoothing)
   std::vector<geometry_msgs::PoseStamped> chase_buffer_;
   unsigned int chase_count_;
 
   //! cancel flag
   bool cancel_goal_;
 };
-}
 
-#endif //ROBORTS_DECISION_CHASE_BEHAVIOR_H
+} // namespace roborts_decision
+
+#endif // ROBORTS_DECISION_CHASE_BEHAVIOR_H
diff --git a/roborts_decision/example_behavior/escape_behavior.h b/roborts_decision/example_behavior/escape_behavior.h
index 43b344aa..a7b991a3 100644
--- a/roborts_decision/example_behavior/escape_behavior.h
+++ b/roborts_decision/example_behavior/escape_behavior.h
@@ -1,6 +1,10 @@
 #ifndef ROBORTS_DECISION_ESCAPEBEHAVIOR_H
 #define ROBORTS_DECISION_ESCAPEBEHAVIOR_H
 
+#include <cmath>
+#include <random>
+#include <string>
+
 #include "io/io.h"
 #include "roborts_msgs/TwistAccel.h"
 
@@ -11,119 +15,129 @@
 
 #include "line_iterator.h"
 
-namespace roborts_decision{
+namespace roborts_decision {
+
+/**
+ * @brief Escape behavior: flee from a detected enemy to a position behind obstacles.
+ *
+ * Selects a random goal on the opposite side of the field from the enemy,
+ * validates that there are sufficient obstacles blocking line-of-sight,
+ * and navigates to that position. Falls back to spinning in place if
+ * no enemy is detected or the enemy position can't be mapped.
+ */
 class EscapeBehavior {
  public:
   EscapeBehavior(ChassisExecutor* &chassis_executor,
                  Blackboard* &blackboard,
-                 const std::string & proto_file_path) : chassis_executor_(chassis_executor),
-                                                        blackboard_(blackboard) {
+                 const std::string & proto_file_path)
+      : chassis_executor_(chassis_executor),
+        blackboard_(blackboard) {
 
-    // init whirl velocity
-    whirl_vel_.accel.linear.x = 0;
+    // Initialize whirl velocity to zero
     whirl_vel_.accel.linear.x = 0;
     whirl_vel_.accel.linear.y = 0;
     whirl_vel_.accel.linear.z = 0;
-
     whirl_vel_.accel.angular.x = 0;
     whirl_vel_.accel.angular.y = 0;
     whirl_vel_.accel.angular.z = 0;
 
     if (!LoadParam(proto_file_path)) {
-      ROS_ERROR("%s can't open file", __FUNCTION__);
+      ROS_ERROR("%s: failed to load config file", __FUNCTION__);
     }
-
   }
 
   void Run() {
-
     auto executor_state = Update();
 
     if (executor_state != BehaviorState::RUNNING) {
-
       if (blackboard_->IsEnemyDetected()) {
-
-        geometry_msgs::PoseStamped enemy;
-        enemy = blackboard_->GetEnemy();
-        float goal_yaw, goal_x, goal_y;
-        unsigned int goal_cell_x, goal_cell_y;
-        unsigned int enemy_cell_x, enemy_cell_y;
-
-        std::random_device rd;
-        std::mt19937 gen(rd());
-
+        geometry_msgs::PoseStamped enemy = blackboard_->GetEnemy();
         auto robot_map_pose = blackboard_->GetRobotMapPose();
+
+        // Determine escape region based on enemy position
         float x_min, x_max;
         if (enemy.pose.position.x < left_x_limit_) {
           x_min = right_random_min_x_;
           x_max = right_random_max_x_;
-
         } else if (enemy.pose.position.x > right_x_limit_) {
           x_min = left_random_min_x_;
           x_max = left_random_max_x_;
-
         } else {
           if ((robot_x_limit_ - robot_map_pose.pose.position.x) >= 0) {
             x_min = left_random_min_x_;
             x_max = left_random_max_x_;
-
           } else {
             x_min = right_random_min_x_;
             x_max = right_random_max_x_;
           }
         }
 
-        std::uniform_real_distribution<float> x_uni_dis(x_min, x_max);
-        std::uniform_real_distribution<float> y_uni_dis(0, 5);
-        //std::uniform_real_distribution<float> yaw_uni_dis(-M_PI, M_PI);
-
-        auto get_enemy_cell = blackboard_->GetCostMap2D()->World2Map(enemy.pose.position.x,
-                                               enemy.pose.position.y,
-                                               enemy_cell_x,
-                                               enemy_cell_y);
+        // Random goal generation in the escape region
+        std::random_device rd;
+        std::mt19937 gen(rd());
+        std::uniform_real_distribution<float> x_dist(x_min, x_max);
+        std::uniform_real_distribution<float> y_dist(0.0f, kFieldYMax);
 
-        if (!get_enemy_cell) {
+        unsigned int enemy_cell_x, enemy_cell_y;
+        if (!blackboard_->GetCostMap2D()->World2Map(
+                enemy.pose.position.x, enemy.pose.position.y,
+                enemy_cell_x, enemy_cell_y)) {
           chassis_executor_->Execute(whirl_vel_);
           return;
         }
 
+        // Find a goal with sufficient obstacle cover
+        float goal_x, goal_y;
+        unsigned int goal_cell_x, goal_cell_y;
+        int attempts = 0;
 
-        while (true) {
-          goal_x = x_uni_dis(gen);
-          goal_y = y_uni_dis(gen);
-          auto get_goal_cell = blackboard_->GetCostMap2D()->World2Map(goal_x,
-                                                                      goal_y,
-                                                                      goal_cell_x,
-                                                                      goal_cell_y);
+        while (attempts < kMaxSearchAttempts) {
+          ++attempts;
+          goal_x = x_dist(gen);
+          goal_y = y_dist(gen);
 
-          if (!get_goal_cell) {
+          if (!blackboard_->GetCostMap2D()->World2Map(
+                  goal_x, goal_y, goal_cell_x, goal_cell_y)) {
             continue;
           }
 
           auto index = blackboard_->GetCostMap2D()->GetIndex(goal_cell_x, goal_cell_y);
-//          costmap_2d_->GetCost(goal_cell_x, goal_cell_y);
-          if (blackboard_->GetCharMap()[index] >= 253) {
+          if (blackboard_->GetCharMap()[index] >= kObstacleCostThreshold) {
             continue;
           }
 
+          // Count obstacles along line of sight to enemy
           unsigned int obstacle_count = 0;
-          for(FastLineIterator line( goal_cell_x, goal_cell_y, enemy_cell_x, enemy_cell_y); line.IsValid(); line.Advance()) {
-            auto point_cost = blackboard_->GetCostMap2D()->GetCost((unsigned int)(line.GetX()), (unsigned int)(line.GetY())); //current point's cost
+          for (FastLineIterator line(goal_cell_x, goal_cell_y, enemy_cell_x, enemy_cell_y);
+               line.IsValid(); line.Advance()) {
+            auto point_cost = blackboard_->GetCostMap2D()->GetCost(
+                static_cast<unsigned int>(line.GetX()),
+                static_cast<unsigned int>(line.GetY()));
 
-            if(point_cost > 253){
+            if (point_cost > kObstacleCostThreshold) {
               obstacle_count++;
             }
-
           }
 
-          if (obstacle_count > 5) { //TODO:  this should write in the proto file
+          if (obstacle_count > kMinObstacleCount) {
             break;
           }
         }
-        Eigen::Vector2d pose_to_enemy(enemy.pose.position.x - robot_map_pose.pose.position.x,
-                                      enemy.pose.position.y - robot_map_pose.pose.position.y);
-        goal_yaw = static_cast<float > (std::atan2(pose_to_enemy.coeffRef(1), pose_to_enemy.coeffRef(0)));
-        auto quaternion = tf::createQuaternionMsgFromRollPitchYaw(0,0,goal_yaw);
+
+        if (attempts >= kMaxSearchAttempts) {
+          ROS_WARN("EscapeBehavior: failed to find covered escape goal after %d attempts",
+                   kMaxSearchAttempts);
+          chassis_executor_->Execute(whirl_vel_);
+          return;
+        }
+
+        // Face toward the enemy while escaping
+        Eigen::Vector2d pose_to_enemy(
+            enemy.pose.position.x - robot_map_pose.pose.position.x,
+            enemy.pose.position.y - robot_map_pose.pose.position.y);
+        float goal_yaw = static_cast<float>(
+            std::atan2(pose_to_enemy.coeffRef(1), pose_to_enemy.coeffRef(0)));
+        auto quaternion = tf::createQuaternionMsgFromRollPitchYaw(0, 0, goal_yaw);
 
         geometry_msgs::PoseStamped escape_goal;
         escape_goal.header.frame_id = "map";
@@ -131,14 +145,12 @@ class EscapeBehavior {
         escape_goal.pose.position.x = goal_x;
         escape_goal.pose.position.y = goal_y;
         escape_goal.pose.orientation = quaternion;
-        //return exploration_goal;
         chassis_executor_->Execute(escape_goal);
       } else {
+        // No enemy detected — spin in place
         chassis_executor_->Execute(whirl_vel_);
-        return;
       }
     }
-
   }
 
   void Cancel() {
@@ -170,27 +182,31 @@ class EscapeBehavior {
     return true;
   }
 
-  ~EscapeBehavior() {
-
-  }
+  ~EscapeBehavior() = default;
 
  private:
+  // Configuration constants
+  static constexpr unsigned char kObstacleCostThreshold = 253;
+  static constexpr unsigned int kMinObstacleCount = 5;
+  static constexpr float kFieldYMax = 5.0f;
+  static constexpr int kMaxSearchAttempts = 1000;  // prevent infinite loop
+
   //! executor
   ChassisExecutor* const chassis_executor_;
 
-  float left_x_limit_, right_x_limit_;
-  float robot_x_limit_;
-  float left_random_min_x_, left_random_max_x_;
-  float right_random_min_x_, right_random_max_x_;
+  //! field limits from config
+  float left_x_limit_ = 0, right_x_limit_ = 0;
+  float robot_x_limit_ = 0;
+  float left_random_min_x_ = 0, left_random_max_x_ = 0;
+  float right_random_min_x_ = 0, right_random_max_x_ = 0;
 
   //! perception information
   Blackboard* const blackboard_;
 
   //! whirl velocity
-//  geometry_msgs::Twist whirl_vel_;
   roborts_msgs::TwistAccel whirl_vel_;
-  
 };
-}
 
-#endif //ROBORTS_DECISION_ESCAPEBEHAVIOR_H
+} // namespace roborts_decision
+
+#endif // ROBORTS_DECISION_ESCAPEBEHAVIOR_H
diff --git a/roborts_decision/example_behavior/goal_behavior.h b/roborts_decision/example_behavior/goal_behavior.h
index 3dc9a5ca..fb4aff19 100644
--- a/roborts_decision/example_behavior/goal_behavior.h
+++ b/roborts_decision/example_behavior/goal_behavior.h
@@ -1,25 +1,33 @@
 #ifndef ROBORTS_DECISION_GOAL_BEHAVIOR_H
 #define ROBORTS_DECISION_GOAL_BEHAVIOR_H
 
-
 #include "io/io.h"
 
 #include "../blackboard/blackboard.h"
 #include "../executor/chassis_executor.h"
 #include "../behavior_tree/behavior_state.h"
 
-
 namespace roborts_decision {
+
+/**
+ * @brief Goal behavior: navigate to a goal set via RViz or other external source.
+ *
+ * Monitors the blackboard for new goals and sends them to the chassis executor.
+ * This is the simplest behavior — it just forwards external goals.
+ */
 class GoalBehavior {
  public:
   GoalBehavior(ChassisExecutor* &chassis_executor,
-               Blackboard* &blackboard) :
-      chassis_executor_(chassis_executor),
-      blackboard_(blackboard) { }
+               Blackboard* &blackboard)
+      : chassis_executor_(chassis_executor),
+        blackboard_(blackboard) {}
 
   void Run() {
-    if(blackboard_->IsNewGoal()){
-      chassis_executor_->Execute(blackboard_->GetGoal());
+    if (blackboard_->IsNewGoal()) {
+      auto goal = blackboard_->GetGoal();
+      ROS_INFO("GoalBehavior: navigating to (%.2f, %.2f)",
+               goal.pose.position.x, goal.pose.position.y);
+      chassis_executor_->Execute(goal);
     }
   }
 
@@ -39,11 +47,8 @@ class GoalBehavior {
 
   //! perception information
   Blackboard* const blackboard_;
-
-  //! planning goal
-  geometry_msgs::PoseStamped planning_goal_;
-
 };
-}
 
-#endif //ROBORTS_DECISION_GOAL_BEHAVIOR_H
+} // namespace roborts_decision
+
+#endif // ROBORTS_DECISION_GOAL_BEHAVIOR_H
diff --git a/roborts_decision/example_behavior/patrol_behavior.h b/roborts_decision/example_behavior/patrol_behavior.h
index 3efb9719..20b7daa6 100644
--- a/roborts_decision/example_behavior/patrol_behavior.h
+++ b/roborts_decision/example_behavior/patrol_behavior.h
@@ -1,6 +1,10 @@
 #ifndef ROBORTS_DECISION_PATROL_BEHAVIOR_H
 #define ROBORTS_DECISION_PATROL_BEHAVIOR_H
 
+#include <cmath>
+#include <vector>
+#include <string>
+
 #include "io/io.h"
 
 #include "../blackboard/blackboard.h"
@@ -11,39 +15,42 @@
 #include "line_iterator.h"
 
 namespace roborts_decision {
+
+/**
+ * @brief Patrol behavior: cycle through a set of waypoints loaded from config.
+ *
+ * Navigates to patrol goals in round-robin order. When a goal is reached,
+ * the next goal in the sequence is sent to the chassis executor.
+ */
 class PatrolBehavior {
  public:
   PatrolBehavior(ChassisExecutor* &chassis_executor,
                  Blackboard* &blackboard,
-                 const std::string & proto_file_path) : chassis_executor_(chassis_executor),
-                                                        blackboard_(blackboard) {
-
-    patrol_count_ = 0;
-    point_size_ = 0;
+                 const std::string & proto_file_path)
+      : chassis_executor_(chassis_executor),
+        blackboard_(blackboard),
+        patrol_count_(0),
+        point_size_(0) {
 
     if (!LoadParam(proto_file_path)) {
-      ROS_ERROR("%s can't open file", __FUNCTION__);
+      ROS_ERROR("%s: failed to load config file", __FUNCTION__);
     }
-
   }
 
   void Run() {
-
     auto executor_state = Update();
-
-    std::cout << "state: " << (int)(executor_state) << std::endl;
+    ROS_DEBUG("Patrol state: %d", static_cast<int>(executor_state));
 
     if (executor_state != BehaviorState::RUNNING) {
-
       if (patrol_goals_.empty()) {
-        ROS_ERROR("patrol goal is empty");
+        ROS_ERROR("Patrol goals are empty — check config file");
         return;
       }
 
-      std::cout << "send goal" << std::endl;
+      ROS_DEBUG("Patrol: navigating to waypoint %d/%d",
+                patrol_count_ + 1, point_size_);
       chassis_executor_->Execute(patrol_goals_[patrol_count_]);
-      patrol_count_ = ++patrol_count_ % point_size_;
-
+      patrol_count_ = (patrol_count_ + 1) % point_size_;
     }
   }
 
@@ -61,23 +68,26 @@ class PatrolBehavior {
       return false;
     }
 
-    point_size_ = (unsigned int)(decision_config.point().size());
+    point_size_ = static_cast<unsigned int>(decision_config.point().size());
     patrol_goals_.resize(point_size_);
-    for (int i = 0; i != point_size_; i++) {
+
+    for (unsigned int i = 0; i < point_size_; i++) {
       patrol_goals_[i].header.frame_id = "map";
       patrol_goals_[i].pose.position.x = decision_config.point(i).x();
       patrol_goals_[i].pose.position.y = decision_config.point(i).y();
       patrol_goals_[i].pose.position.z = decision_config.point(i).z();
 
-      tf::Quaternion quaternion = tf::createQuaternionFromRPY(decision_config.point(i).roll(),
-                                                              decision_config.point(i).pitch(),
-                                                              decision_config.point(i).yaw());
+      tf::Quaternion quaternion = tf::createQuaternionFromRPY(
+          decision_config.point(i).roll(),
+          decision_config.point(i).pitch(),
+          decision_config.point(i).yaw());
       patrol_goals_[i].pose.orientation.x = quaternion.x();
       patrol_goals_[i].pose.orientation.y = quaternion.y();
       patrol_goals_[i].pose.orientation.z = quaternion.z();
       patrol_goals_[i].pose.orientation.w = quaternion.w();
     }
 
+    ROS_INFO("Loaded %d patrol waypoints", point_size_);
     return true;
   }
 
@@ -94,8 +104,8 @@ class PatrolBehavior {
   std::vector<geometry_msgs::PoseStamped> patrol_goals_;
   unsigned int patrol_count_;
   unsigned int point_size_;
-
 };
-}
 
-#endif //ROBORTS_DECISION_PATROL_BEHAVIOR_H
+} // namespace roborts_decision
+
+#endif // ROBORTS_DECISION_PATROL_BEHAVIOR_H
diff --git a/roborts_decision/example_behavior/search_behavior.h b/roborts_decision/example_behavior/search_behavior.h
index 413e8515..408d4e27 100644
--- a/roborts_decision/example_behavior/search_behavior.h
+++ b/roborts_decision/example_behavior/search_behavior.h
@@ -1,6 +1,11 @@
 #ifndef ROBORTS_DECISION_SEARCH_BEHAVIOR_H
 #define ROBORTS_DECISION_SEARCH_BEHAVIOR_H
 
+#include <cmath>
+#include <vector>
+#include <string>
+#include <algorithm>
+
 #include "io/io.h"
 
 #include "../blackboard/blackboard.h"
@@ -11,13 +16,23 @@
 #include "line_iterator.h"
 
 namespace roborts_decision {
+
+/**
+ * @brief Search behavior: systematically search the area where an enemy was last seen.
+ *
+ * Divides the field into 4 quadrants and selects the appropriate search region
+ * based on the enemy's last known position. Searches through waypoints in
+ * the selected region, starting from the closest one.
+ */
 class SearchBehavior {
  public:
   SearchBehavior(ChassisExecutor* &chassis_executor,
                 Blackboard* &blackboard,
-                const std::string & proto_file_path) : chassis_executor_(chassis_executor),
-                                                       blackboard_(blackboard) {
-
+                const std::string & proto_file_path)
+      : chassis_executor_(chassis_executor),
+        blackboard_(blackboard),
+        search_index_(0),
+        search_count_(0) {
 
     last_position_.header.frame_id = "map";
     last_position_.pose.orientation.x = 0;
@@ -29,50 +44,40 @@ class SearchBehavior {
     last_position_.pose.position.y = 0;
     last_position_.pose.position.z = 0;
 
-    search_index_ = 0;
-    search_count_ = 0;
-
     if (!LoadParam(proto_file_path)) {
-      ROS_ERROR("%s can't open file", __FUNCTION__);
+      ROS_ERROR("%s: failed to load config file", __FUNCTION__);
     }
-
   }
 
   void Run() {
-
     auto executor_state = Update();
 
-    double yaw;
-    double x_diff;
-    double y_diff;
-
     if (executor_state != BehaviorState::RUNNING) {
       auto robot_map_pose = blackboard_->GetRobotMapPose();
-      if (search_count_ == 5) {
-        x_diff = last_position_.pose.position.x - robot_map_pose.pose.position.x;
-        y_diff = last_position_.pose.position.y - robot_map_pose.pose.position.y;
 
-        auto last_x = last_position_.pose.position.x;
-        auto last_y = last_position_.pose.position.y;
+      if (search_count_ == kInitialSearchCount) {
+        // First search step: determine which quadrant and find closest waypoint
+        const double x_diff = last_position_.pose.position.x - robot_map_pose.pose.position.x;
+        const double y_diff = last_position_.pose.position.y - robot_map_pose.pose.position.y;
+        const double last_x = last_position_.pose.position.x;
+        const double last_y = last_position_.pose.position.y;
 
-        if (last_x < 4.2 && last_y < 2.75) {
+        // Select the search region based on quadrant
+        if (last_x < kFieldMidX && last_y < kFieldMidY) {
           search_region_ = search_region_1_;
-
-        } else if (last_x > 4.2 && last_y < 2.75) {
+        } else if (last_x > kFieldMidX && last_y < kFieldMidY) {
           search_region_ = search_region_2_;
-
-        } else if (last_x < 4.2 && last_y > 2.75) {
+        } else if (last_x < kFieldMidX && last_y > kFieldMidY) {
           search_region_ = search_region_3_;
-
         } else {
           search_region_ = search_region_4_;
-
         }
 
-        double search_min_dist = 99999;
+        // Find the closest waypoint in the selected region
+        double search_min_dist = std::numeric_limits<double>::max();
         for (unsigned int i = 0; i < search_region_.size(); ++i) {
           auto dist_sq = std::pow(search_region_[i].pose.position.x - last_x, 2)
-              + std::pow(search_region_[i].pose.position.y - last_y, 2);
+                       + std::pow(search_region_[i].pose.position.y - last_y, 2);
 
           if (dist_sq < search_min_dist) {
             search_min_dist = dist_sq;
@@ -80,8 +85,8 @@ class SearchBehavior {
           }
         }
 
-        yaw = std::atan2(y_diff, x_diff);
-
+        // Navigate to last known enemy position first
+        const double yaw = std::atan2(y_diff, x_diff);
         auto orientation = tf::createQuaternionMsgFromYaw(yaw);
 
         geometry_msgs::PoseStamped goal;
@@ -93,11 +98,15 @@ class SearchBehavior {
         search_count_--;
 
       } else if (search_count_ > 0) {
-        auto search_goal = search_region_[(search_index_++ )];
+        // Subsequent steps: cycle through waypoints in the search region
+        if (search_region_.empty()) {
+          ROS_WARN("Search region is empty, cannot search");
+          return;
+        }
+        auto search_goal = search_region_[search_index_++];
         chassis_executor_->Execute(search_goal);
-        search_index_ = (unsigned int) (search_index_% search_region_.size());
+        search_index_ = search_index_ % static_cast<unsigned int>(search_region_.size());
         search_count_--;
-
       }
     }
   }
@@ -116,84 +125,63 @@ class SearchBehavior {
       return false;
     }
 
-    // may have more efficient way to search a region(enemy where disappear)
-    search_region_.resize((unsigned int)(decision_config.search_region_1().size()));
-    for (int i = 0; i != decision_config.search_region_1().size(); i++) {
-      geometry_msgs::PoseStamped search_point;
-      search_point.header.frame_id = "map";
-      search_point.pose.position.x = decision_config.search_region_1(i).x();
-      search_point.pose.position.y = decision_config.search_region_1(i).y();
-      search_point.pose.position.z = decision_config.search_region_1(i).z();
+    // Load all 4 search regions from config
+    LoadSearchRegion(decision_config.search_region_1(), search_region_1_);
+    LoadSearchRegion(decision_config.search_region_2(), search_region_2_);
+    LoadSearchRegion(decision_config.search_region_3(), search_region_3_);
+    LoadSearchRegion(decision_config.search_region_4(), search_region_4_);
 
-      auto quaternion = tf::createQuaternionMsgFromRollPitchYaw(decision_config.search_region_1(i).roll(),
-                                                                decision_config.search_region_1(i).pitch(),
-                                                                decision_config.search_region_1(i).yaw());
-      search_point.pose.orientation = quaternion;
-      search_region_1_.push_back(search_point);
-    }
-
-    for (int i = 0; i != decision_config.search_region_2().size(); i++) {
-      geometry_msgs::PoseStamped search_point;
-      search_point.header.frame_id = "map";
-      search_point.pose.position.x = decision_config.search_region_2(i).x();
-      search_point.pose.position.y = decision_config.search_region_2(i).y();
-      search_point.pose.position.z = decision_config.search_region_2(i).z();
-
-      auto quaternion = tf::createQuaternionMsgFromRollPitchYaw(decision_config.search_region_2(i).roll(),
-                                                                decision_config.search_region_2(i).pitch(),
-                                                                decision_config.search_region_2(i).yaw());
-      search_point.pose.orientation = quaternion;
-      search_region_2_.push_back(search_point);
-    }
-
-    for (int i = 0; i != decision_config.search_region_3().size(); i++) {
-      geometry_msgs::PoseStamped search_point;
-      search_point.header.frame_id = "map";
-      search_point.pose.position.x = decision_config.search_region_3(i).x();
-      search_point.pose.position.y = decision_config.search_region_3(i).y();
-      search_point.pose.position.z = decision_config.search_region_3(i).z();
-
-      auto quaternion = tf::createQuaternionMsgFromRollPitchYaw(decision_config.search_region_3(i).roll(),
-                                                                decision_config.search_region_3(i).pitch(),
-                                                                decision_config.search_region_3(i).yaw());
-      search_point.pose.orientation = quaternion;
-      search_region_3_.push_back(search_point);
-    }
-
-    for (int i = 0; i != decision_config.search_region_4().size(); i++) {
-      geometry_msgs::PoseStamped search_point;
-      search_point.header.frame_id = "map";
-      search_point.pose.position.x = decision_config.search_region_4(i).x();
-      search_point.pose.position.y = decision_config.search_region_4(i).y();
-      search_point.pose.position.z = decision_config.search_region_4(i).z();
-
-      auto quaternion = tf::createQuaternionMsgFromRollPitchYaw(decision_config.search_region_4(i).roll(),
-                                                                decision_config.search_region_4(i).pitch(),
-                                                                decision_config.search_region_4(i).yaw());
-      search_point.pose.orientation = quaternion;
-      search_region_4_.push_back(search_point);
-    }
+    ROS_INFO("Loaded search regions: R1=%zu, R2=%zu, R3=%zu, R4=%zu",
+             search_region_1_.size(), search_region_2_.size(),
+             search_region_3_.size(), search_region_4_.size());
     return true;
   }
 
   void SetLastPosition(geometry_msgs::PoseStamped last_position) {
     last_position_ = last_position;
-    search_count_ = 5;
+    search_count_ = kInitialSearchCount;
   }
 
   ~SearchBehavior() = default;
 
  private:
+  // Configuration constants
+  static constexpr int kInitialSearchCount = 5;
+  static constexpr double kFieldMidX = 4.2;
+  static constexpr double kFieldMidY = 2.75;
+
+  /**
+   * @brief Helper to load a search region from protobuf config
+   */
+  template<typename RegionConfig>
+  void LoadSearchRegion(const RegionConfig& config,
+                        std::vector<geometry_msgs::PoseStamped>& region) {
+    region.clear();
+    region.reserve(config.size());
+    for (int i = 0; i < config.size(); i++) {
+      geometry_msgs::PoseStamped search_point;
+      search_point.header.frame_id = "map";
+      search_point.pose.position.x = config.Get(i).x();
+      search_point.pose.position.y = config.Get(i).y();
+      search_point.pose.position.z = config.Get(i).z();
+
+      auto quaternion = tf::createQuaternionMsgFromRollPitchYaw(
+          config.Get(i).roll(), config.Get(i).pitch(), config.Get(i).yaw());
+      search_point.pose.orientation = quaternion;
+      region.push_back(search_point);
+    }
+  }
+
   //! executor
   ChassisExecutor* const chassis_executor_;
 
   //! perception information
   Blackboard* const blackboard_;
 
-  //! chase goal
+  //! last known enemy position
   geometry_msgs::PoseStamped last_position_;
 
-  //! search buffer
+  //! search buffers
   std::vector<geometry_msgs::PoseStamped> search_region_1_;
   std::vector<geometry_msgs::PoseStamped> search_region_2_;
   std::vector<geometry_msgs::PoseStamped> search_region_3_;
@@ -201,8 +189,8 @@ class SearchBehavior {
   std::vector<geometry_msgs::PoseStamped> search_region_;
   unsigned int search_count_;
   unsigned int search_index_;
-
 };
-}
 
-#endif //ROBORTS_DECISION_SEARCH_BEHAVIOR_H
+} // namespace roborts_decision
+
+#endif // ROBORTS_DECISION_SEARCH_BEHAVIOR_H
diff --git a/roborts_decision/executor/chassis_executor.cpp b/roborts_decision/executor/chassis_executor.cpp
index 443bc876..2d2266a0 100644
--- a/roborts_decision/executor/chassis_executor.cpp
+++ b/roborts_decision/executor/chassis_executor.cpp
@@ -1,74 +1,84 @@
 #include <actionlib/client/simple_action_client.h>
+#include <chrono>
+#include <thread>
 #include "chassis_executor.h"
 
-namespace roborts_decision{
+namespace roborts_decision {
 
-ChassisExecutor::ChassisExecutor():execution_mode_(ExcutionMode::IDLE_MODE), execution_state_(BehaviorState::IDLE),
-                                   global_planner_client_("global_planner_node_action", true),
-                                   local_planner_client_("local_planner_node_action", true)
-{
+namespace {
+  constexpr int kCancelSleepMs = 50;  // sleep after canceling accel mode
+  constexpr int kCmdVelAccQueueSize = 100;
+  constexpr int kCmdVelQueueSize = 1;
+} // anonymous namespace
+
+ChassisExecutor::ChassisExecutor()
+    : execution_mode_(ExcutionMode::IDLE_MODE),
+      execution_state_(BehaviorState::IDLE),
+      global_planner_client_("global_planner_node_action", true),
+      local_planner_client_("local_planner_node_action", true) {
   ros::NodeHandle nh;
-  cmd_vel_acc_pub_ = nh.advertise<roborts_msgs::TwistAccel>("cmd_vel_acc", 100);
-  cmd_vel_pub_     = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
+  cmd_vel_acc_pub_ = nh.advertise<roborts_msgs::TwistAccel>("cmd_vel_acc", kCmdVelAccQueueSize);
+  cmd_vel_pub_     = nh.advertise<geometry_msgs::Twist>("cmd_vel", kCmdVelQueueSize);
+
+  ROS_INFO("Waiting for global planner action server...");
   global_planner_client_.waitForServer();
-  ROS_INFO("Global planer server start!");
+  ROS_INFO("Global planner server connected.");
+
+  ROS_INFO("Waiting for local planner action server...");
   local_planner_client_.waitForServer();
-  ROS_INFO("Local planer server start!");
+  ROS_INFO("Local planner server connected.");
 }
 
-void ChassisExecutor::Execute(const geometry_msgs::PoseStamped &goal){
+void ChassisExecutor::Execute(const geometry_msgs::PoseStamped &goal) {
   execution_mode_ = ExcutionMode::GOAL_MODE;
   global_planner_goal_.goal = goal;
-  global_planner_client_.sendGoal(global_planner_goal_,
-                                  GlobalActionClient::SimpleDoneCallback(),
-                                  GlobalActionClient::SimpleActiveCallback(),
-                                  boost::bind(&ChassisExecutor::GlobalPlannerFeedbackCallback, this, _1));
+  global_planner_client_.sendGoal(
+      global_planner_goal_,
+      GlobalActionClient::SimpleDoneCallback(),
+      GlobalActionClient::SimpleActiveCallback(),
+      boost::bind(&ChassisExecutor::GlobalPlannerFeedbackCallback, this, _1));
 }
 
-void ChassisExecutor::Execute(const geometry_msgs::Twist &twist){
-  if( execution_mode_ == ExcutionMode::GOAL_MODE){
+void ChassisExecutor::Execute(const geometry_msgs::Twist &twist) {
+  if (execution_mode_ == ExcutionMode::GOAL_MODE) {
     Cancel();
   }
   execution_mode_ = ExcutionMode::SPEED_MODE;
   cmd_vel_pub_.publish(twist);
 }
 
-void ChassisExecutor::Execute(const roborts_msgs::TwistAccel &twist_accel){
-  if( execution_mode_ == ExcutionMode::GOAL_MODE){
+void ChassisExecutor::Execute(const roborts_msgs::TwistAccel &twist_accel) {
+  if (execution_mode_ == ExcutionMode::GOAL_MODE) {
     Cancel();
   }
   execution_mode_ = ExcutionMode::SPEED_WITH_ACCEL_MODE;
-
   cmd_vel_acc_pub_.publish(twist_accel);
 }
 
-BehaviorState ChassisExecutor::Update(){
+BehaviorState ChassisExecutor::Update() {
   actionlib::SimpleClientGoalState state = actionlib::SimpleClientGoalState::LOST;
-  switch (execution_mode_){
+
+  switch (execution_mode_) {
     case ExcutionMode::IDLE_MODE:
       execution_state_ = BehaviorState::IDLE;
       break;
 
     case ExcutionMode::GOAL_MODE:
       state = global_planner_client_.getState();
-      if (state == actionlib::SimpleClientGoalState::ACTIVE){
-        ROS_INFO("%s : ACTIVE", __FUNCTION__);
+      if (state == actionlib::SimpleClientGoalState::ACTIVE) {
+        ROS_DEBUG("%s: ACTIVE", __FUNCTION__);
         execution_state_ = BehaviorState::RUNNING;
-
       } else if (state == actionlib::SimpleClientGoalState::PENDING) {
-        ROS_INFO("%s : PENDING", __FUNCTION__);
+        ROS_DEBUG("%s: PENDING", __FUNCTION__);
         execution_state_ = BehaviorState::RUNNING;
-
       } else if (state == actionlib::SimpleClientGoalState::SUCCEEDED) {
-        ROS_INFO("%s : SUCCEEDED", __FUNCTION__);
+        ROS_INFO("%s: SUCCEEDED", __FUNCTION__);
         execution_state_ = BehaviorState::SUCCESS;
-
       } else if (state == actionlib::SimpleClientGoalState::ABORTED) {
-        ROS_INFO("%s : ABORTED", __FUNCTION__);
+        ROS_WARN("%s: ABORTED", __FUNCTION__);
         execution_state_ = BehaviorState::FAILURE;
-
       } else {
-        ROS_ERROR("Error: %s", state.toString().c_str());
+        ROS_ERROR("Unexpected planner state: %s", state.toString().c_str());
         execution_state_ = BehaviorState::FAILURE;
       }
       break;
@@ -82,16 +92,16 @@ BehaviorState ChassisExecutor::Update(){
       break;
 
     default:
-      ROS_ERROR("Wrong Execution Mode");
+      ROS_ERROR("Unknown Execution Mode");
   }
-  return execution_state_;
 
-};
+  return execution_state_;
+}
 
-void ChassisExecutor::Cancel(){
-  switch (execution_mode_){
+void ChassisExecutor::Cancel() {
+  switch (execution_mode_) {
     case ExcutionMode::IDLE_MODE:
-      ROS_WARN("Nothing to be canceled.");
+      ROS_DEBUG("Nothing to cancel — already idle.");
       break;
 
     case ExcutionMode::GOAL_MODE:
@@ -108,19 +118,21 @@ void ChassisExecutor::Cancel(){
     case ExcutionMode::SPEED_WITH_ACCEL_MODE:
       cmd_vel_acc_pub_.publish(zero_twist_accel_);
       execution_mode_ = ExcutionMode::IDLE_MODE;
-      usleep(50000);
+      // Brief sleep to ensure the zero-velocity command is processed
+      std::this_thread::sleep_for(std::chrono::milliseconds(kCancelSleepMs));
       break;
+
     default:
-      ROS_ERROR("Wrong Execution Mode");
+      ROS_ERROR("Unknown Execution Mode");
   }
-
 }
 
-void ChassisExecutor::GlobalPlannerFeedbackCallback(const roborts_msgs::GlobalPlannerFeedbackConstPtr& global_planner_feedback){
+void ChassisExecutor::GlobalPlannerFeedbackCallback(
+    const roborts_msgs::GlobalPlannerFeedbackConstPtr& global_planner_feedback) {
   if (!global_planner_feedback->path.poses.empty()) {
     local_planner_goal_.route = global_planner_feedback->path;
     local_planner_client_.sendGoal(local_planner_goal_);
   }
 }
 
-}
\ No newline at end of file
+} // namespace roborts_decision
\ No newline at end of file
diff --git a/roborts_decision/executor/chassis_executor.h b/roborts_decision/executor/chassis_executor.h
index bd320604..0feb456e 100644
--- a/roborts_decision/executor/chassis_executor.h
+++ b/roborts_decision/executor/chassis_executor.h
@@ -1,5 +1,6 @@
 #ifndef ROBORTS_DECISION_CHASSIS_EXECUTOR_H
 #define ROBORTS_DECISION_CHASSIS_EXECUTOR_H
+
 #include <ros/ros.h>
 #include <actionlib/client/simple_action_client.h>
 
@@ -10,87 +11,102 @@
 
 #include "../behavior_tree/behavior_state.h"
 
-namespace roborts_decision{
-/***
- * @brief Chassis Executor to execute different abstracted task for chassis module
+namespace roborts_decision {
+
+/**
+ * @brief Chassis Executor to execute different abstracted tasks for the chassis module.
+ *
+ * Supports three execution modes:
+ * - GOAL_MODE: Navigation to a target pose via global+local planner
+ * - SPEED_MODE: Direct velocity control
+ * - SPEED_WITH_ACCEL_MODE: Velocity with acceleration control
  */
-class ChassisExecutor{
+class ChassisExecutor {
+  using GlobalActionClient = actionlib::SimpleActionClient<roborts_msgs::GlobalPlannerAction>;
+  using LocalActionClient = actionlib::SimpleActionClient<roborts_msgs::LocalPlannerAction>;
 
-  typedef actionlib::SimpleActionClient<roborts_msgs::GlobalPlannerAction> GlobalActionClient;
-  typedef actionlib::SimpleActionClient<roborts_msgs::LocalPlannerAction> LocalActionClient;
  public:
   /**
    * @brief Chassis execution mode for different tasks
    */
-  enum class ExcutionMode{
+  enum class ExcutionMode {
     IDLE_MODE,            ///< Default idle mode with no task
     GOAL_MODE,            ///< Goal-targeted task mode using global and local planner
     SPEED_MODE,           ///< Velocity task mode
     SPEED_WITH_ACCEL_MODE ///< Velocity with acceleration task mode
   };
+
   /**
    * @brief Constructor of ChassisExecutor
    */
   ChassisExecutor();
   ~ChassisExecutor() = default;
+
+  // Non-copyable
+  ChassisExecutor(const ChassisExecutor&) = delete;
+  ChassisExecutor& operator=(const ChassisExecutor&) = delete;
+
   /**
-   * @brief Execute the goal-targeted task using global and local planner with actionlib
-   * @param goal Given taget goal
+   * @brief Execute a goal-targeted task using global and local planner
+   * @param goal Given target goal
    */
   void Execute(const geometry_msgs::PoseStamped &goal);
+
   /**
-   * @brief Execute the velocity task with publisher
+   * @brief Execute a velocity task
    * @param twist Given velocity
    */
   void Execute(const geometry_msgs::Twist &twist);
+
   /**
-   * @brief Execute the velocity with acceleration task with publisher
+   * @brief Execute a velocity with acceleration task
    * @param twist_accel Given velocity with acceleration
    */
   void Execute(const roborts_msgs::TwistAccel &twist_accel);
+
   /**
    * @brief Update the current chassis executor state
-   * @return Current chassis executor state(same with behavior state)
+   * @return Current chassis executor state (same with behavior state)
    */
   BehaviorState Update();
+
   /**
-   * @brief Cancel the current task and deal with the mode transition
+   * @brief Cancel the current task and return to idle
    */
   void Cancel();
 
  private:
-  /***
-   * @brief Global planner actionlib feedback callback function to send the global planner path to local planner
-   * @param global_planner_feedback  Global planner actionlib feedback, which mainly consists of global planner path output
+  /**
+   * @brief Global planner feedback callback — forwards path to local planner
    */
-  void GlobalPlannerFeedbackCallback(const roborts_msgs::GlobalPlannerFeedbackConstPtr& global_planner_feedback);
+  void GlobalPlannerFeedbackCallback(
+      const roborts_msgs::GlobalPlannerFeedbackConstPtr& global_planner_feedback);
+
   //! execution mode of the executor
   ExcutionMode execution_mode_;
-  //! execution state of the executor (same with behavior state)
+  //! execution state of the executor
   BehaviorState execution_state_;
 
   //! global planner actionlib client
-  actionlib::SimpleActionClient<roborts_msgs::GlobalPlannerAction> global_planner_client_;
+  GlobalActionClient global_planner_client_;
   //! local planner actionlib client
-  actionlib::SimpleActionClient<roborts_msgs::LocalPlannerAction> local_planner_client_;
+  LocalActionClient local_planner_client_;
   //! global planner actionlib goal
   roborts_msgs::GlobalPlannerGoal global_planner_goal_;
   //! local planner actionlib goal
   roborts_msgs::LocalPlannerGoal local_planner_goal_;
 
-  //! velocity control publisher in ROS
+  //! velocity control publisher
   ros::Publisher cmd_vel_pub_;
-  //! zero twist in form of ROS geometry_msgs::Twist
+  //! zero twist for stopping
   geometry_msgs::Twist zero_twist_;
 
-  //! velocity with accel publisher in ROS
+  //! velocity with accel publisher
   ros::Publisher cmd_vel_acc_pub_;
-  //! zero twist with acceleration in form of ROS roborts_msgs::TwistAccel
+  //! zero twist with acceleration for stopping
   roborts_msgs::TwistAccel zero_twist_accel_;
-
-
 };
-}
 
+} // namespace roborts_decision
 
-#endif //ROBORTS_DECISION_CHASSIS_EXECUTOR_H
\ No newline at end of file
+#endif // ROBORTS_DECISION_CHASSIS_EXECUTOR_H
\ No newline at end of file
diff --git a/roborts_decision/executor/gimbal_executor.h b/roborts_decision/executor/gimbal_executor.h
index d72dde36..db9a3ddd 100644
--- a/roborts_decision/executor/gimbal_executor.h
+++ b/roborts_decision/executor/gimbal_executor.h
@@ -1,67 +1,81 @@
 #ifndef ROBORTS_DECISION_GIMBAL_EXECUTOR_H
 #define ROBORTS_DECISION_GIMBAL_EXECUTOR_H
+
 #include "ros/ros.h"
 
 #include "roborts_msgs/GimbalAngle.h"
 #include "roborts_msgs/GimbalRate.h"
 
 #include "../behavior_tree/behavior_state.h"
-namespace roborts_decision{
-/***
- * @brief Gimbal Executor to execute different abstracted task for gimbal module
+
+namespace roborts_decision {
+
+/**
+ * @brief Gimbal Executor to execute different abstracted tasks for the gimbal module.
+ *
+ * Supports two execution modes:
+ * - ANGLE_MODE: Direct angle control
+ * - RATE_MODE: Angular rate control
  */
-class GimbalExecutor{
+class GimbalExecutor {
  public:
   /**
    * @brief Gimbal execution mode for different tasks
    */
-  enum class ExcutionMode{
+  enum class ExcutionMode {
     IDLE_MODE,   ///< Default idle mode with no task
     ANGLE_MODE,  ///< Angle task mode
     RATE_MODE    ///< Rate task mode
   };
+
   /**
    * @brief Constructor of GimbalExecutor
    */
   GimbalExecutor();
   ~GimbalExecutor() = default;
-  /***
-   * @brief Execute the gimbal angle task with publisher
+
+  // Non-copyable
+  GimbalExecutor(const GimbalExecutor&) = delete;
+  GimbalExecutor& operator=(const GimbalExecutor&) = delete;
+
+  /**
+   * @brief Execute gimbal angle control
    * @param gimbal_angle Given gimbal angle
    */
   void Execute(const roborts_msgs::GimbalAngle &gimbal_angle);
-  /***
-   * @brief Execute the gimbal rate task with publisher
+
+  /**
+   * @brief Execute gimbal rate control
    * @param gimbal_rate Given gimbal rate
    */
   void Execute(const roborts_msgs::GimbalRate &gimbal_rate);
+
   /**
    * @brief Update the current gimbal executor state
-   * @return Current gimbal executor state(same with behavior state)
+   * @return Current gimbal executor state
    */
   BehaviorState Update();
+
   /**
-   * @brief Cancel the current task and deal with the mode transition
+   * @brief Cancel the current task and return to idle
    */
   void Cancel();
 
  private:
-  //! execution mode of the executor
+  //! execution mode of the executor (note: maintaining original naming for API compat)
   ExcutionMode excution_mode_;
-  //! execution state of the executor (same with behavior state)
+  //! execution state of the executor
   BehaviorState execution_state_;
 
-  //! gimbal rate control publisher in ROS
+  //! gimbal rate control publisher
   ros::Publisher cmd_gimbal_rate_pub_;
-  //! zero gimbal rate in form of ROS roborts_msgs::GimbalRate
+  //! zero gimbal rate for stopping
   roborts_msgs::GimbalRate zero_gimbal_rate_;
 
-  //! gimbal angle control publisher in ROS
+  //! gimbal angle control publisher
   ros::Publisher cmd_gimbal_angle_pub_;
-
-
 };
-}
 
+} // namespace roborts_decision
 
-#endif //ROBORTS_DECISION_GIMBAL_EXECUTOR_H
\ No newline at end of file
+#endif // ROBORTS_DECISION_GIMBAL_EXECUTOR_H
\ No newline at end of file
diff --git a/roborts_decision/package.xml b/roborts_decision/package.xml
index 46ac45a8..2d43f9b2 100755
--- a/roborts_decision/package.xml
+++ b/roborts_decision/package.xml
@@ -1,31 +1,26 @@
-<package>
+<?xml version="1.0"?>
+<package format="2">
     <name>roborts_decision</name>
-    <version>1.0.0</version>
+    <version>2.0.0</version>
     <description>
-        The roborts decision package provides decision-making for RoboMaster AI Robot
+        The roborts_decision package provides behavior-based decision-making
+        for the RoboMaster AI Robot. Includes example behaviors for patrol,
+        chase, escape, search, and goal navigation.
     </description>
     <author>RoboMaster</author>
     <maintainer email="robomaster@dji.com">RoboMaster</maintainer>
-    <license>GPL 3.0 </license>
+    <license>GPL 3.0</license>
     <url>https://github.com/RoboMaster/RoboRTS</url>
 
     <buildtool_depend>catkin</buildtool_depend>
 
-    <build_depend>actionlib</build_depend>
-    <build_depend>roscpp</build_depend>
-    <build_depend>roborts_msgs</build_depend>
-    <build_depend>rospy</build_depend>
-    <build_depend>roborts_common</build_depend>
-    <build_depend>roborts_costmap</build_depend>
-
-    <run_depend>actionlib</run_depend>
-    <run_depend>roscpp</run_depend>
-    <run_depend>roborts_msgs</run_depend>
-    <run_depend>rospy</run_depend>
-    <run_depend>roborts_common</run_depend>
-    <run_depend>roborts_costmap</run_depend>
-
-    <export>
-    </export>
+    <depend>actionlib</depend>
+    <depend>roscpp</depend>
+    <depend>roborts_msgs</depend>
+    <depend>roborts_common</depend>
+    <depend>roborts_costmap</depend>
+    <depend>tf</depend>
+    <depend>nav_msgs</depend>
+    <depend>geometry_msgs</depend>
 
 </package>