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.
This commit is contained in:
@@ -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_;
|
||||
};
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user