Refactor configuration management by introducing CustomConfig class. Updated config.hpp to streamline settings and removed legacy configuration presets. Adjusted controller and custom_robot classes to utilize the new configuration structure. Enhanced error handling and logging during initialization processes.

src/custom_robot.cpp

@@ -2,13 +2,22 @@
 #include <thread>
 #include <chrono>
 #include <functional>
+#include <unitree/robot/channel/channel_factory.hpp>
 
 namespace custom {
 
 CustomRobot::CustomRobot()
     : initialized_(false), running_(false) {
-
+    // 创建并加载配置
+    config_.loadDefaults();
+    
+    try {
+        unitree::robot::ChannelFactory::Instance()->Init(0, config_.network_interface);
+        LOG_INFO("ChannelFactory initialized with interface: " + config_.network_interface);
+    } catch (const std::exception& e) {
+        LOG_ERROR("Failed to initialize ChannelFactory: " + std::string(e.what()));
     }
+}
 
 CustomRobot::~CustomRobot() {
     LOG_INFO("Shutting down CustomRobot");
@@ -17,32 +26,30 @@ CustomRobot::~CustomRobot() {
         controller_.reset();
     }
     
-    if (stateClient_) {
-        stateClient_.reset();
+    if (rsc_) {
+        rsc_.reset();
+    }
+    
+    try {
+        unitree::robot::ChannelFactory::Instance()->Release();
+        LOG_INFO("ChannelFactory released");
+    } catch (const std::exception& e) {
+        LOG_ERROR("Failed to release ChannelFactory: " + std::string(e.what()));
     }
     
     initialized_ = false;
 }
 
 bool CustomRobot::initialize() {
-    LOG_INFO("Initializing CustomRobot");
-    
-    auto& config = Config::getInstance().getConfig();
-    
-    controller_ = std::make_unique<Controller>(config.network_interface);
-    if (!controller_->initialize()) {
-        LOG_ERROR("Failed to initialize robot controller");
-        return false;
-    }
-    
-    // Initialize robot state client
     try {
-        stateClient_ = std::make_unique<unitree::robot::go2::RobotStateClient>();
-        stateClient_->SetTimeout(10.0f);
-        stateClient_->Init();
-        LOG_INFO("RobotStateClient initialized successfully");
+        controller_ = std::make_unique<Controller>();
+        controller_->initialize();
+
+        rsc_ = std::make_unique<unitree::robot::go2::RobotStateClient>();
+        rsc_->SetTimeout(3.0);
+        rsc_->Init();
     } catch (const std::exception& e) {
-        LOG_ERROR("Failed to initialize RobotStateClient: " + std::string(e.what()));
+        LOG_ERROR("Failed to initialize : " + std::string(e.what()));
         return false;
     }
     
@@ -63,6 +70,23 @@ bool CustomRobot::start() {
         return true;
     }
     
+    std::vector<unitree::robot::go2::ServiceState> serviceList;
+    if (!GetServiceList(serviceList)) {
+        LOG_ERROR("Failed to get service list");
+        return false;
+    }
+    
+    for (const auto& service : serviceList) {
+        LOG_INFO("Service - name: " + service.name + ", protect: " + std::to_string(service.protect) + ", status: " + std::to_string(service.status));
+    }
+    
+    if (!SwitchService("sport_mode", false)) {
+        LOG_ERROR("Failed to stop sport_mode service");
+        return false;
+    }
+    
+    std::this_thread::sleep_for(std::chrono::seconds(1));
+    
     if (!controller_->start()) {
         LOG_ERROR("Failed to start robot controller");
         return false;
@@ -73,13 +97,13 @@ bool CustomRobot::start() {
 }
 
 bool CustomRobot::GetServiceList(std::vector<unitree::robot::go2::ServiceState>& serviceList) {
-    if (!initialized_ || !stateClient_) {
+    if (!initialized_ || !rsc_) {
         LOG_ERROR("Robot not initialized or stateClient is null");
         return false;
     }
     
     try {
-        int32_t ret = stateClient_->ServiceList(serviceList);
+        int32_t ret = rsc_->ServiceList(serviceList);
         if (ret != 0) {
             LOG_ERROR("Failed to get service list, error code: " + std::to_string(ret));
             return false;
@@ -93,14 +117,14 @@ bool CustomRobot::GetServiceList(std::vector<unitree::robot::go2::ServiceState>&
 }
 
 bool CustomRobot::SwitchService(const std::string& serviceName, bool enable) {
-    if (!initialized_ || !stateClient_) {
+    if (!initialized_ || !rsc_) {
         LOG_ERROR("Robot not initialized or stateClient is null");
         return false;
     }
     
     try {
         int32_t status;
-        int32_t ret = stateClient_->ServiceSwitch(serviceName, enable ? 1 : 0, status);
+        int32_t ret = rsc_->ServiceSwitch(serviceName, enable ? 1 : 0, status);
         if (ret != 0) {
             LOG_ERROR("Failed to switch service " + serviceName + ", error code: " + std::to_string(ret));
             return false;
@@ -114,13 +138,13 @@ bool CustomRobot::SwitchService(const std::string& serviceName, bool enable) {
 }
 
 bool CustomRobot::SetReportFreq(int32_t interval, int32_t duration) {
-    if (!initialized_ || !stateClient_) {
+    if (!initialized_ || !rsc_) {
         LOG_ERROR("Robot not initialized or stateClient is null");
         return false;
     }
     
     try {
-        int32_t ret = stateClient_->SetReportFreq(interval, duration);
+        int32_t ret = rsc_->SetReportFreq(interval, duration);
         if (ret != 0) {
             LOG_ERROR("Failed to set report frequency, error code: " + std::to_string(ret));
             return false;
@@ -133,4 +157,4 @@ bool CustomRobot::SetReportFreq(int32_t interval, int32_t duration) {
     }
 }
 
-} 
+} // namespace custom