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.

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__
-#define __NAVIGATION_HPP__
+#pragma once
 
 #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_;
+    };
+}