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.

CMakeLists.txt

@@ -38,6 +38,7 @@ set(SOURCES
     src/mqtt.cpp
     src/navigation.cpp
     src/low_controller.cpp
+    src/recharge.cpp
 )
 
 add_executable(main ${SOURCES})

README.md

@@ -12,6 +12,7 @@ A high-performance C++ implementation for controlling Unitree GO2 robot with rea
 - **⚙️ Configuration Management**: JSON-based configuration with runtime presets
 - **🧭 Navigation Support**: SLAM and navigation capabilities
 - **🎭 Special Actions**: Support for dances, tricks, and custom motions
+- **🔋 Auto Recharge Support**: ArUco marker-based automatic recharge capability
 - **🚀 High Performance**: Optimized for real-time control with configurable frequencies
 - **🔧 Development Tools**: Built-in scripts and utilities for easy deployment
 - **🔍 Service Status Monitoring**: Automatic printing of all robot service statuses

include/controller.hpp

@@ -61,11 +61,11 @@ public:
     bool WalkUpright(bool flag);
     bool CrossStep(bool flag);
     bool TrajectoryFollow(const std::vector<std::array<float, 6>>& path);
-    bool FootRaiseHeight(float height);
     
     // Obstacle 
     bool SwitchSet(bool enable);
     bool SwitchGet(bool& enable);
+    bool ObstacleMove(float x, float y, float yaw);
     bool UseRemoteCommandFromApi(bool isRemoteCommandsFromApi);
     bool MoveToAbsolutePosition(float x, float y, float yaw);
     bool MoveToIncrementPosition(float x, float y, float yaw);

include/custom_robot.hpp

@@ -5,6 +5,8 @@
 #include "logger.hpp"
 #include "mqtt.hpp"
 #include "navigation.hpp"
+#include "mqtt_client.hpp"
+#include "recharge.hpp"
 // #include "low_controller.hpp"
 
 #include <memory>
@@ -42,6 +44,7 @@ public:
     bool processNavCmd(const std::string& cmd, const nlohmann::json& message);
     bool processMscCmd(const std::string& cmd, const nlohmann::json& message);
     bool processLowCmd(const std::string& cmd, const nlohmann::json& message);
+    bool processRechargeCmd(const std::string& cmd, const nlohmann::json& message);
     void printServiceList(const std::vector<unitree::robot::go2::ServiceState>& serviceList, int filterStatus = -1);
 
 private:
@@ -51,6 +54,7 @@ private:
     std::unique_ptr<Navigation> navigation_;
     std::unique_ptr<unitree::robot::go2::RobotStateClient> rsc_;
     std::unique_ptr<MqttClient> mqttClient_;
+    std::unique_ptr<Recharge> recharge_;
     
     CustomConfig config_;  
     std::atomic<bool> running_;

include/navigation.hpp

@@ -1,5 +1,4 @@
-#ifndef __NAVIGATION_HPP__
+#pragma once
-#define __NAVIGATION_HPP__
 
 #include <unitree/robot/client/client.hpp>
 #include <nlohmann/json.hpp>
@@ -62,7 +61,6 @@ public:
 class Navigation : public unitree::robot::Client {
 public:
     Navigation();
-    ~Navigation();
 
     void Init() override;
 
@@ -92,4 +90,3 @@ private:
 
 } // namespace custom
 
-#endif // __NAVIGATION_HPP__

include/recharge.hpp

@@ -0,0 +1,45 @@
+#pragma once
+#include <unitree/robot/channel/channel_publisher.hpp>
+#include <unitree/robot/channel/channel_subscriber.hpp>
+#include <unitree/idl/ros2/std_msgs/msg/dds_/_String.hpp>
+#include <string>
+#include <vector>
+#include <functional>
+#include <mutex>
+#include <atomic>
+#include <thread>
+#include <condition_variable>
+
+#define RechargeTopic "rt/aruco_cmd"
+#define RechargeStateTopic "rt/aruco_state"
+
+namespace custom {
+    class Recharge {
+    public:
+        Recharge(double timeout = 60.0);
+        
+        bool Init();
+
+        std::string StartRecharge(int repeat = 1, double period_sec = 1.0);
+
+        bool StopRecharge(int repeat = 1, double period_sec = 1.0);
+
+        void RegisterStateCallback(std::function<void(const std::string&)> callback);
+
+        std::string GetLastState() const;
+
+    private:
+        void OnStateMessage(const void* message);
+
+        bool Publish(const std::string& data, int repeat = 1, double period_sec = 1.0);
+
+        double timeout_;
+        mutable std::mutex state_mutex_;
+        std::string last_state_;
+        std::vector<std::function<void(const std::string&)>> callbacks_;
+        std::condition_variable cv_;
+        
+        unitree::robot::ChannelPublisherPtr<std_msgs::msg::dds_::String_> cmd_pub_;
+        unitree::robot::ChannelSubscriberPtr<std_msgs::msg::dds_::String_> state_sub_;
+    };
+}

src/controller.cpp

@@ -287,13 +287,13 @@ bool Controller::Dance2() {
 
 bool Controller::LeftFlip() {
     return ExecuteSportCmd([](auto* sc) {
-        return sc->LeftFlip();
+        // return sc->LeftFlip();
     });
 }
 
 bool Controller::BackFlip() {
     return ExecuteSportCmd([](auto* sc) {
-        return sc->BackFlip();
+        // return sc->BackFlip();
     });
 }
 
@@ -321,6 +321,12 @@ bool Controller::FreeAvoid(bool flag) {
     });
 }
 
+bool Controller::ClassicWalk(bool flag) {
+    return ExecuteSportCmd([flag](auto* sc) {
+        return sc->ClassicWalk(flag);
+    });
+}
+
 bool Controller::WalkUpright(bool flag) {
     return ExecuteSportCmd([flag](auto* sc) {
         return sc->WalkUpright(flag);
@@ -334,25 +340,12 @@ bool Controller::CrossStep(bool flag) {
 }
 
 bool Controller::TrajectoryFollow(const std::vector<std::array<float, 6>>& path) {
-    // This function requires special handling as it needs to convert the path format
-    // For now, we'll provide a basic implementation
     return ExecuteSportCmd([](auto* sc) {
-        // This is a placeholder implementation
+        return 0;
-        // You'll need to implement the proper path conversion based on your needs
-        return 0; // Success
     });
 }
 
 
-bool Controller::FootRaiseHeight(float height) {
-    // This function may not be available in all SportClient implementations
-    // We'll provide a basic implementation that returns success
-    return ExecuteSportCmd([height](auto* sc) {
-        // This is a placeholder implementation
-        return 0; // Success
-    });
-}
-
 
 
 
@@ -368,6 +361,12 @@ bool Controller::SwitchGet(bool& enable) {
     });
 }
 
+bool Controller::ObstacleMove(float x, float y, float yaw) {
+    return ExecuteObstacleCmd([x, y, yaw](auto* oac) {
+        return oac->Move(x, y, yaw);
+    });
+}
+
 bool Controller::UseRemoteCommandFromApi(bool isRemoteCommandsFromApi) {
     return ExecuteObstacleCmd([isRemoteCommandsFromApi](auto* oac) {
         return oac->UseRemoteCommandFromApi(isRemoteCommandsFromApi);
@@ -386,6 +385,8 @@ bool Controller::MoveToIncrementPosition(float x, float y, float yaw) {
     });
 }
 
+
+
 bool Controller::CheckMode(std::string& form, std::string& name) {
     return ExecuteMotionSwitchCmd([&](auto* msc) {
         return msc->CheckMode(form, name);

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");

src/navigation.cpp

@@ -4,21 +4,18 @@
 namespace custom {
 
 Navigation::Navigation() : unitree::robot::Client(SLAM_SERVICE_NAME) {
-    subSlamInfo = unitree::robot::ChannelSubscriberPtr<std_msgs::msg::dds_::String_>(
+    subSlamInfo = std::make_shared<unitree::robot::ChannelSubscriber<std_msgs::msg::dds_::String_>>(SlamInfoTopic);
-        new unitree::robot::ChannelSubscriber<std_msgs::msg::dds_::String_>(SlamInfoTopic));
-    subSlamInfo->InitChannel(
-        std::bind(&Navigation::slamInfoHandler, this, std::placeholders::_1), 1);
     
-    subSlamKeyInfo = unitree::robot::ChannelSubscriberPtr<std_msgs::msg::dds_::String_>(
+    subSlamKeyInfo = std::make_shared<unitree::robot::ChannelSubscriber<std_msgs::msg::dds_::String_>>(SlamKeyInfoTopic);
-        new unitree::robot::ChannelSubscriber<std_msgs::msg::dds_::String_>(SlamKeyInfoTopic));
-    subSlamKeyInfo->InitChannel(
-        std::bind(&Navigation::slamKeyInfoHandler, this, std::placeholders::_1), 1);
 }
 
-Navigation::~Navigation() {}
-
 void Navigation::Init() {
     try {
+        subSlamInfo->InitChannel(
+            std::bind(&Navigation::slamInfoHandler, this, std::placeholders::_1), 1);
+        subSlamKeyInfo->InitChannel(
+            std::bind(&Navigation::slamKeyInfoHandler, this, std::placeholders::_1), 1);
+    
         SetApiVersion(SLAM_API_VERSION);
         UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_POSE_NAV_PL);
         UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_PAUSE_NAV);

src/recharge.cpp

@@ -0,0 +1,151 @@
+#include "recharge.hpp"
+#include "logger.hpp"
+#include <thread>
+#include <chrono>
+#include <algorithm>
+
+namespace custom {
+
+Recharge::Recharge(double timeout) 
+    : timeout_(timeout), 
+      last_state_("") {
+    cmd_pub_ = std::make_shared<unitree::robot::ChannelPublisher<std_msgs::msg::dds_::String_>>(RechargeTopic);
+    state_sub_ = std::make_shared<unitree::robot::ChannelSubscriber<std_msgs::msg::dds_::String_>>(RechargeStateTopic);
+
+}
+
+bool Recharge::Init() {
+    try {
+        cmd_pub_->InitChannel();
+        state_sub_->InitChannel(
+            std::bind(&Recharge::OnStateMessage, this, std::placeholders::_1), 
+            10
+        );
+        
+        LOG_INFO("Recharge initialized successfully");
+        return true;
+    } catch (const std::exception& e) {
+        LOG_ERROR("Failed to initialize Recharge: %s", e.what());
+        return false;
+    }
+}
+
+void Recharge::OnStateMessage(const void* message) {
+    if (!message) {
+        return;
+    }
+    
+    try {
+        const std_msgs::msg::dds_::String_* msg = 
+            static_cast<const std_msgs::msg::dds_::String_*>(message);
+        
+        if (!msg) {
+            return;
+        }
+        
+        std::string state_text = msg->data();
+        
+        {
+            std::lock_guard<std::mutex> lock(state_mutex_);
+            last_state_ = state_text;
+        }
+        
+        std::string s = state_text;
+        std::transform(s.begin(), s.end(), s.begin(), [](unsigned char c){ return std::tolower(c); });
+        
+        if (s == "aruco_success" || s == "aruco_failed") {
+            cv_.notify_all();
+        }
+        
+        for (const auto& callback : callbacks_) {
+            try {
+                callback(state_text);
+            } catch (const std::exception& e) {
+                LOG_ERROR("Error in state callback: %s", e.what());
+            }
+        }
+    } catch (const std::exception& e) {
+        LOG_ERROR("Error processing state message: %s", e.what());
+    }
+}
+
+std::string Recharge::StartRecharge(int repeat, double period_sec) {
+    {
+        std::lock_guard<std::mutex> lock(state_mutex_);
+        last_state_ = "";
+    }
+    
+    if (!Publish("aruco_start", repeat, period_sec)) {
+        LOG_ERROR("Failed to publish start recharge command");
+        return "publish_error";
+    }
+    
+    std::unique_lock<std::mutex> lock(state_mutex_);
+    if (cv_.wait_for(lock, std::chrono::duration<double>(timeout_), [this]{
+        std::string s = this->last_state_;
+        std::transform(s.begin(), s.end(), s.begin(),
+                   [](unsigned char c){ return std::tolower(c); });
+        return s == "aruco_success" || s == "aruco_failed";
+    })) {
+        std::string s = last_state_;
+        std::transform(s.begin(), s.end(), s.begin(),
+                   [](unsigned char c){ return std::tolower(c); });
+
+        if (s == "aruco_success") {
+            return "aruco_success";
+        } else {
+            return "aruco_failed";
+        }
+    } else {
+        std::string last_state = GetLastState();
+        return last_state.empty() ? "timeout" : last_state;
+    }
+}
+
+bool Recharge::Publish(const std::string& data, int repeat, double period_sec) {
+    if (!cmd_pub_) {
+        LOG_ERROR("Command publisher is not initialized");
+        return false;
+    }
+    
+    bool success = true;
+    
+    for (int i = 0; i < std::max(1, repeat); ++i) {
+        try {
+            std_msgs::msg::dds_::String_ msg;
+            msg.data(data);
+            
+            if (!cmd_pub_->Write(msg, 1000000)) {
+                LOG_WARN("Failed to publish message: %s", data.c_str());
+                success = false;
+            }
+            
+            if (i < repeat - 1) {
+                std::this_thread::sleep_for(
+                    std::chrono::milliseconds(static_cast<int>(period_sec * 1000))
+                );
+            }
+        } catch (const std::exception& e) {
+            LOG_ERROR("Error publishing message: %s", e.what());
+            success = false;
+        }
+    }
+    
+    return success;
+}
+
+bool Recharge::StopRecharge(int repeat, double period_sec) {
+    return Publish("aruco_stop", repeat, period_sec);
+}
+
+void Recharge::RegisterStateCallback(std::function<void(const std::string&)> callback) {
+    if (callback) {
+        callbacks_.push_back(callback);
+    }
+}
+
+std::string Recharge::GetLastState() const {
+    std::lock_guard<std::mutex> lock(state_mutex_);
+    return last_state_;
+}
+}