feat(recharge): Implement automatic recharge functionality

- Added Recharge class for managing automatic recharge capabilities using ArUco markers.
- Integrated recharge functionality into CustomRobot, including command processing for starting and stopping recharge.
- Updated CMakeLists.txt to include the new recharge.cpp source file.
- Enhanced README.md to document the new auto recharge support feature.

src/custom_robot.cpp

@@ -8,6 +8,7 @@
 #include <iomanip>
 #include <nlohmann/json.hpp>
 #include <unitree/robot/channel/channel_factory.hpp>
+#include "recharge.hpp"
 
 namespace custom {
 
@@ -54,6 +55,11 @@ CustomRobot::~CustomRobot() {
         navigation_.reset();
     }
     
+    if (recharge_) {
+        recharge_->Close();
+        recharge_.reset();
+    }
+    
     try {
         unitree::robot::ChannelFactory::Instance()->Release();
         LOG_INFO("ChannelFactory released");
@@ -79,6 +85,9 @@ bool CustomRobot::initialize() {
         rsc_->SetTimeout(3.0f);
         rsc_->Init();
         
+        recharge_ = std::make_unique<Recharge>();
+        recharge_->Init();
+
         if (!initializeMqtt()) {
             LOG_ERROR("Failed to initialize MQTT client");
             return false;
@@ -145,7 +154,7 @@ bool CustomRobot::GetServiceList(std::vector<unitree::robot::go2::ServiceState>&
             return false;
         }
         
-        LOG_INFO("Successfully retrieved service list with " + std::to_string(serviceList.size()) + " services");
+        // LOG_INFO("Successfully retrieved service list with " + std::to_string(serviceList.size()) + " services");
         // for (const auto& service : serviceList) {
         //     std::string statusStr = (service.status == 1) ? "ACTIVE" : "INACTIVE";
         //     LOG_INFO("Service: " + service.name + " | Status: " + statusStr + " | Protect: " + 
@@ -280,6 +289,8 @@ void CustomRobot::processCmd(const nlohmann::json& message) {
             success = processMscCmd(cmd, message);
         } else if (type == "low") {
             success = processLowCmd(cmd, message);
+        } else if (type == "recharge") {
+            success = processRechargeCmd(cmd, message);
         } else {
             LOG_ERROR("Unknown command type: " + type);
             return;
@@ -333,6 +344,31 @@ bool CustomRobot::processLowCmd(const std::string& cmd, const nlohmann::json& me
     return false;
 }
 
+bool CustomRobot::processRechargeCmd(const std::string& cmd, const nlohmann::json& message) {
+    if (!recharge_) {
+        LOG_ERROR("Recharge module not initialized");
+        return false;
+    }
+
+    try {
+        if (cmd == "start") {
+            LOG_INFO("Starting recharge process...");
+            std::string result = recharge_->StartRecharge();
+            LOG_INFO("Recharge process finished with result: " + result);
+            return result == "aruco_success";
+        } else if (cmd == "stop") {
+            LOG_INFO("Stopping recharge process...");
+            return recharge_->StopRecharge();
+        } else {
+            LOG_ERROR("Unknown recharge command: " + cmd);
+            return false;
+        }
+    } catch (const std::exception& e) {
+        LOG_ERROR("Error executing recharge command " + cmd + ": " + std::string(e.what()));
+        return false;
+    }
+}
+
 bool CustomRobot::processOacCmd(const std::string& cmd, const nlohmann::json& message) {
     if (!controller_) {
         LOG_ERROR("Controller not initialized");
@@ -360,6 +396,14 @@ bool CustomRobot::processOacCmd(const std::string& cmd, const nlohmann::json& me
             }
             return result;
             
+        } else if (cmd == "Move") {
+            if (!message.contains("param")) {
+                LOG_ERROR("Move cmd missing 'param'");
+                return false;
+            }
+            auto param = message["param"];
+            return controller_->ObstacleMove(param["x"], param["y"], param["yaw"]);
+            
         } else if (cmd == "UseRemoteCommandFromApi") {
             if (!message.contains("param") || !message["param"].contains("enable")) {
                 LOG_ERROR("UseRemoteCommandFromApi cmd missing 'enable' parameter");