Remove MQTT dependency and refactor configuration structure. Updated CMakeLists.txt to eliminate MQTT library checks and adjusted config.hpp to use string_view for configuration parameters. Simplified controller and custom_robot classes by removing MQTT-related code and enhancing robot state management. Introduced service management methods in CustomRobot for better state handling.
This commit is contained in:
@@ -1,192 +1,207 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <nlohmann/json.hpp>
|
||||
#include <string_view>
|
||||
|
||||
namespace custom {
|
||||
namespace customConfig {
|
||||
|
||||
// Compile-time configuration flag
|
||||
constexpr bool USE_COMPILE_TIME_CONFIG = true;
|
||||
// Configuration template selector
|
||||
enum class ConfigPreset {
|
||||
Default,
|
||||
HighPerformance,
|
||||
Development,
|
||||
Safety
|
||||
};
|
||||
|
||||
// Network settings
|
||||
constexpr const char* NETWORK_INTERFACE = "eth0";
|
||||
// Motion gait enum
|
||||
enum class Gait : int {
|
||||
IDLE = 0,
|
||||
TROT = 1,
|
||||
TROT_RUNNING = 2
|
||||
};
|
||||
|
||||
// MQTT settings
|
||||
constexpr const char* MQTT_BROKER = "localhost";
|
||||
// Network configuration
|
||||
constexpr std::string_view NETWORK_INTERFACE = "eth0";
|
||||
|
||||
// MQTT configuration
|
||||
constexpr std::string_view MQTT_BROKER = "localhost";
|
||||
constexpr int MQTT_PORT = 1883;
|
||||
constexpr const char* MQTT_USERNAME = "";
|
||||
constexpr const char* MQTT_PASSWORD = "";
|
||||
constexpr const char* MQTT_CLIENT_ID = "unitree_go2_client";
|
||||
constexpr std::string_view MQTT_USERNAME = "";
|
||||
constexpr std::string_view MQTT_PASSWORD = "";
|
||||
constexpr std::string_view MQTT_CLIENT_ID = "unitree_go2_client";
|
||||
|
||||
// Topic settings
|
||||
constexpr const char* TOPIC_PREFIX = "unitree/go2";
|
||||
constexpr const char* TOPIC_CMD = "cmd";
|
||||
constexpr const char* TOPIC_STATE = "state";
|
||||
constexpr const char* TOPIC_VIDEO = "video";
|
||||
constexpr const char* TOPIC_AUDIO = "audio";
|
||||
// Topic configuration
|
||||
constexpr std::string_view TOPIC_PREFIX = "unitree/go2";
|
||||
constexpr std::string_view TOPIC_CMD = "cmd";
|
||||
constexpr std::string_view TOPIC_STATE = "state";
|
||||
constexpr std::string_view TOPIC_VIDEO = "video";
|
||||
constexpr std::string_view TOPIC_AUDIO = "audio";
|
||||
|
||||
// Robot control settings
|
||||
// Robot control configuration
|
||||
constexpr double CONTROL_FREQUENCY = 200.0; // Hz
|
||||
constexpr double STATE_PUBLISH_FREQUENCY = 50.0; // Hz
|
||||
constexpr bool ENABLE_VIDEO = true;
|
||||
constexpr bool ENABLE_AUDIO = true;
|
||||
|
||||
// Safety settings
|
||||
// Safety configuration
|
||||
constexpr double MAX_LINEAR_VELOCITY = 1.5; // m/s
|
||||
constexpr double MAX_ANGULAR_VELOCITY = 2.0; // rad/s
|
||||
constexpr double EMERGENCY_STOP_TIMEOUT = 5.0; // seconds
|
||||
|
||||
// Motion settings
|
||||
// Motion configuration
|
||||
constexpr double STAND_HEIGHT = 0.0; // relative height
|
||||
constexpr int GAIT = 0; // 0:idle, 1:trot, 2:trot_running
|
||||
constexpr Gait DEFAULT_GAIT = Gait::IDLE;
|
||||
|
||||
// Video settings
|
||||
// Video configuration
|
||||
constexpr int VIDEO_WIDTH = 1280;
|
||||
constexpr int VIDEO_HEIGHT = 720;
|
||||
constexpr int VIDEO_FPS = 30;
|
||||
constexpr const char* VIDEO_FORMAT = "h264";
|
||||
constexpr std::string_view VIDEO_FORMAT = "h264";
|
||||
constexpr bool VIDEO_ENABLED = true;
|
||||
|
||||
// Audio settings
|
||||
// Audio configuration
|
||||
constexpr int AUDIO_SAMPLE_RATE = 16000;
|
||||
constexpr int AUDIO_CHANNELS = 1;
|
||||
constexpr const char* AUDIO_FORMAT = "pcm";
|
||||
constexpr std::string_view AUDIO_FORMAT = "pcm";
|
||||
constexpr bool AUDIO_ENABLED = true;
|
||||
|
||||
// Configuration presets
|
||||
struct HighPerformancePreset {
|
||||
static constexpr double CONTROL_FREQUENCY = 500.0;
|
||||
static constexpr double STATE_PUBLISH_FREQUENCY = 100.0;
|
||||
static constexpr double MAX_LINEAR_VELOCITY = 2.5;
|
||||
static constexpr double MAX_ANGULAR_VELOCITY = 3.0;
|
||||
static constexpr bool ENABLE_VIDEO = false; // Disable for performance
|
||||
static constexpr bool ENABLE_AUDIO = false; // Disable for performance
|
||||
// Configuration presets using template specialization
|
||||
template<ConfigPreset P>
|
||||
struct ConfigParams;
|
||||
|
||||
// Default preset
|
||||
template<>
|
||||
struct ConfigParams<ConfigPreset::Default> {
|
||||
static constexpr double control_frequency = CONTROL_FREQUENCY;
|
||||
static constexpr double state_publish_frequency = STATE_PUBLISH_FREQUENCY;
|
||||
static constexpr double max_linear_velocity = MAX_LINEAR_VELOCITY;
|
||||
static constexpr double max_angular_velocity = MAX_ANGULAR_VELOCITY;
|
||||
static constexpr double emergency_stop_timeout = EMERGENCY_STOP_TIMEOUT;
|
||||
static constexpr bool video_enabled = VIDEO_ENABLED;
|
||||
static constexpr bool audio_enabled = AUDIO_ENABLED;
|
||||
};
|
||||
|
||||
struct DevelopmentPreset {
|
||||
static constexpr double CONTROL_FREQUENCY = 100.0;
|
||||
static constexpr double STATE_PUBLISH_FREQUENCY = 20.0;
|
||||
static constexpr double MAX_LINEAR_VELOCITY = 0.8;
|
||||
static constexpr double MAX_ANGULAR_VELOCITY = 1.0;
|
||||
static constexpr bool ENABLE_VIDEO = true;
|
||||
static constexpr bool ENABLE_AUDIO = true;
|
||||
// High performance preset
|
||||
template<>
|
||||
struct ConfigParams<ConfigPreset::HighPerformance> {
|
||||
static constexpr double control_frequency = 500.0;
|
||||
static constexpr double state_publish_frequency = 100.0;
|
||||
static constexpr double max_linear_velocity = 2.5;
|
||||
static constexpr double max_angular_velocity = 3.0;
|
||||
static constexpr double emergency_stop_timeout = EMERGENCY_STOP_TIMEOUT;
|
||||
static constexpr bool video_enabled = false; // Disabled for performance
|
||||
static constexpr bool audio_enabled = false; // Disabled for performance
|
||||
};
|
||||
|
||||
struct SafetyPreset {
|
||||
static constexpr double CONTROL_FREQUENCY = 50.0;
|
||||
static constexpr double STATE_PUBLISH_FREQUENCY = 10.0;
|
||||
static constexpr double MAX_LINEAR_VELOCITY = 0.5;
|
||||
static constexpr double MAX_ANGULAR_VELOCITY = 0.5;
|
||||
static constexpr double EMERGENCY_STOP_TIMEOUT = 2.0;
|
||||
static constexpr bool ENABLE_VIDEO = true;
|
||||
static constexpr bool ENABLE_AUDIO = true;
|
||||
// Development preset
|
||||
template<>
|
||||
struct ConfigParams<ConfigPreset::Development> {
|
||||
static constexpr double control_frequency = 100.0;
|
||||
static constexpr double state_publish_frequency = 20.0;
|
||||
static constexpr double max_linear_velocity = 0.8;
|
||||
static constexpr double max_angular_velocity = 1.0;
|
||||
static constexpr double emergency_stop_timeout = EMERGENCY_STOP_TIMEOUT;
|
||||
static constexpr bool video_enabled = true;
|
||||
static constexpr bool audio_enabled = true;
|
||||
};
|
||||
|
||||
// Runtime configuration structure - uses compile-time defaults
|
||||
struct RobotConfig {
|
||||
// Safety preset
|
||||
template<>
|
||||
struct ConfigParams<ConfigPreset::Safety> {
|
||||
static constexpr double control_frequency = 50.0;
|
||||
static constexpr double state_publish_frequency = 10.0;
|
||||
static constexpr double max_linear_velocity = 0.5;
|
||||
static constexpr double max_angular_velocity = 0.5;
|
||||
static constexpr double emergency_stop_timeout = 2.0;
|
||||
static constexpr bool video_enabled = true;
|
||||
static constexpr bool audio_enabled = true;
|
||||
};
|
||||
|
||||
// Compile-time configuration selection (set this to choose preset)
|
||||
constexpr ConfigPreset ACTIVE_PRESET = ConfigPreset::Default;
|
||||
|
||||
// Compile-time configuration struct
|
||||
template<ConfigPreset P = ACTIVE_PRESET>
|
||||
struct CompileTimeConfig {
|
||||
// Use selected preset parameters
|
||||
using params = ConfigParams<P>;
|
||||
|
||||
// Network settings
|
||||
std::string network_interface = NETWORK_INTERFACE;
|
||||
static constexpr std::string_view network_interface = NETWORK_INTERFACE;
|
||||
|
||||
// MQTT settings
|
||||
std::string mqtt_broker = MQTT_BROKER;
|
||||
int mqtt_port = MQTT_PORT;
|
||||
std::string mqtt_username = MQTT_USERNAME;
|
||||
std::string mqtt_password = MQTT_PASSWORD;
|
||||
std::string mqtt_client_id = MQTT_CLIENT_ID;
|
||||
static constexpr std::string_view mqtt_broker = MQTT_BROKER;
|
||||
static constexpr int mqtt_port = MQTT_PORT;
|
||||
static constexpr std::string_view mqtt_username = MQTT_USERNAME;
|
||||
static constexpr std::string_view mqtt_password = MQTT_PASSWORD;
|
||||
static constexpr std::string_view mqtt_client_id = MQTT_CLIENT_ID;
|
||||
|
||||
// Topics
|
||||
std::string topic_prefix = TOPIC_PREFIX;
|
||||
std::string cmd_topic = TOPIC_CMD;
|
||||
std::string state_topic = TOPIC_STATE;
|
||||
std::string video_topic = TOPIC_VIDEO;
|
||||
std::string audio_topic = TOPIC_AUDIO;
|
||||
static constexpr std::string_view topic_prefix = TOPIC_PREFIX;
|
||||
static constexpr std::string_view cmd_topic = TOPIC_CMD;
|
||||
static constexpr std::string_view state_topic = TOPIC_STATE;
|
||||
static constexpr std::string_view video_topic = TOPIC_VIDEO;
|
||||
static constexpr std::string_view audio_topic = TOPIC_AUDIO;
|
||||
|
||||
// Robot settings
|
||||
double control_frequency = CONTROL_FREQUENCY;
|
||||
double state_publish_frequency = STATE_PUBLISH_FREQUENCY;
|
||||
bool enable_video = ENABLE_VIDEO;
|
||||
bool enable_audio = ENABLE_AUDIO;
|
||||
// Robot settings (from preset)
|
||||
static constexpr double control_frequency = params::control_frequency;
|
||||
static constexpr double state_publish_frequency = params::state_publish_frequency;
|
||||
static constexpr bool enable_video = params::video_enabled;
|
||||
static constexpr bool enable_audio = params::audio_enabled;
|
||||
|
||||
// Safety settings
|
||||
double max_linear_velocity = MAX_LINEAR_VELOCITY;
|
||||
double max_angular_velocity = MAX_ANGULAR_VELOCITY;
|
||||
double emergency_stop_timeout = EMERGENCY_STOP_TIMEOUT;
|
||||
// Safety settings (from preset)
|
||||
static constexpr double max_linear_velocity = params::max_linear_velocity;
|
||||
static constexpr double max_angular_velocity = params::max_angular_velocity;
|
||||
static constexpr double emergency_stop_timeout = params::emergency_stop_timeout;
|
||||
|
||||
// Motion settings
|
||||
double stand_height = STAND_HEIGHT;
|
||||
double default_gait = GAIT;
|
||||
static constexpr double stand_height = STAND_HEIGHT;
|
||||
static constexpr Gait default_gait = DEFAULT_GAIT;
|
||||
|
||||
// Video settings
|
||||
int video_width = VIDEO_WIDTH;
|
||||
int video_height = VIDEO_HEIGHT;
|
||||
int video_fps = VIDEO_FPS;
|
||||
std::string video_format = VIDEO_FORMAT;
|
||||
static constexpr int video_width = VIDEO_WIDTH;
|
||||
static constexpr int video_height = VIDEO_HEIGHT;
|
||||
static constexpr int video_fps = VIDEO_FPS;
|
||||
static constexpr std::string_view video_format = VIDEO_FORMAT;
|
||||
|
||||
// Audio settings
|
||||
int audio_sample_rate = AUDIO_SAMPLE_RATE;
|
||||
int audio_channels = AUDIO_CHANNELS;
|
||||
std::string audio_format = AUDIO_FORMAT;
|
||||
static constexpr int audio_sample_rate = AUDIO_SAMPLE_RATE;
|
||||
static constexpr int audio_channels = AUDIO_CHANNELS;
|
||||
static constexpr std::string_view audio_format = AUDIO_FORMAT;
|
||||
|
||||
// Utility functions
|
||||
static std::string getFullTopic(std::string_view topic) {
|
||||
return std::string(topic_prefix) + "/" + std::string(topic);
|
||||
}
|
||||
};
|
||||
|
||||
class Config {
|
||||
public:
|
||||
static Config& getInstance();
|
||||
|
||||
/**
|
||||
* Load configuration from JSON file
|
||||
*/
|
||||
bool loadConfig(const std::string& configPath);
|
||||
|
||||
/**
|
||||
* Save current configuration to JSON file
|
||||
*/
|
||||
bool saveConfig(const std::string& configPath);
|
||||
|
||||
/**
|
||||
* Load configuration from compile-time defaults
|
||||
*/
|
||||
void loadDefaults();
|
||||
|
||||
/**
|
||||
* Load high-performance preset
|
||||
*/
|
||||
void loadHighPerformancePreset();
|
||||
|
||||
/**
|
||||
* Load development preset
|
||||
*/
|
||||
void loadDevelopmentPreset();
|
||||
|
||||
/**
|
||||
* Load safety preset
|
||||
*/
|
||||
void loadSafetyPreset();
|
||||
|
||||
/**
|
||||
* Validate configuration values
|
||||
*/
|
||||
bool validateConfig();
|
||||
|
||||
/**
|
||||
* Get current configuration
|
||||
*/
|
||||
const RobotConfig& getConfig() const { return config_; }
|
||||
|
||||
/**
|
||||
* Get mutable reference to configuration
|
||||
*/
|
||||
RobotConfig& getConfig() { return config_; }
|
||||
|
||||
/**
|
||||
* Convenience getters
|
||||
*/
|
||||
std::string getFullTopic(const std::string& topic) const;
|
||||
// Type alias for current configuration
|
||||
using RobotConfig = CompileTimeConfig<ACTIVE_PRESET>;
|
||||
|
||||
private:
|
||||
Config();
|
||||
~Config() = default;
|
||||
Config(const Config&) = delete;
|
||||
Config& operator=(const Config&) = delete;
|
||||
|
||||
RobotConfig config_;
|
||||
};
|
||||
// Configuration validation utilities
|
||||
template<ConfigPreset P>
|
||||
constexpr bool isConfigValid() {
|
||||
using config = ConfigParams<P>;
|
||||
return config::control_frequency > 0.0 &&
|
||||
config::state_publish_frequency > 0.0 &&
|
||||
config::max_linear_velocity > 0.0 &&
|
||||
config::max_angular_velocity > 0.0 &&
|
||||
config::emergency_stop_timeout > 0.0;
|
||||
}
|
||||
|
||||
} // namespace custom
|
||||
// Compile-time validation
|
||||
static_assert(isConfigValid<ACTIVE_PRESET>(), "Invalid configuration parameters");
|
||||
|
||||
// Configuration preset name utilities
|
||||
template<ConfigPreset P>
|
||||
constexpr const char* getPresetName() {
|
||||
if constexpr (P == ConfigPreset::Default) return "Default";
|
||||
else if constexpr (P == ConfigPreset::HighPerformance) return "HighPerformance";
|
||||
else if constexpr (P == ConfigPreset::Development) return "Development";
|
||||
else if constexpr (P == ConfigPreset::Safety) return "Safety";
|
||||
else return "Unknown";
|
||||
}
|
||||
|
||||
constexpr const char* getActivePresetName() {
|
||||
return getPresetName<ACTIVE_PRESET>();
|
||||
}
|
||||
|
||||
} // namespace customConfig
|
||||
|
||||
@@ -1,11 +1,7 @@
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
#include <thread>
|
||||
#include <atomic>
|
||||
#include <mutex>
|
||||
#include <condition_variable>
|
||||
#include <chrono>
|
||||
#include <nlohmann/json.hpp>
|
||||
|
||||
#include <unitree/robot/go2/sport/sport_client.hpp>
|
||||
@@ -14,220 +10,49 @@
|
||||
#include <unitree/robot/go2/vui/vui_client.hpp>
|
||||
#include <unitree/robot/channel/channel_subscriber.hpp>
|
||||
#include <unitree/idl/go2/SportModeState_.hpp>
|
||||
#include <unitree/idl/go2/LowState_.hpp>
|
||||
|
||||
namespace custom {
|
||||
|
||||
enum class RobotMode {
|
||||
IDLE,
|
||||
SPORT,
|
||||
LOW_LEVEL,
|
||||
RECOVERY
|
||||
};
|
||||
|
||||
enum class MotionState {
|
||||
STOPPED,
|
||||
STANDING,
|
||||
WALKING,
|
||||
RUNNING,
|
||||
SITTING,
|
||||
LYING,
|
||||
DANCING,
|
||||
SPECIAL_MOTION
|
||||
};
|
||||
|
||||
struct RobotState {
|
||||
RobotMode mode = RobotMode::IDLE;
|
||||
MotionState motion_state = MotionState::STOPPED;
|
||||
|
||||
// Position and orientation
|
||||
double position[3] = {0.0, 0.0, 0.0}; // x, y, z
|
||||
double orientation[3] = {0.0, 0.0, 0.0}; // roll, pitch, yaw
|
||||
double velocity[3] = {0.0, 0.0, 0.0}; // vx, vy, vyaw
|
||||
|
||||
// IMU data
|
||||
double imu_acc[3] = {0.0, 0.0, 0.0};
|
||||
double imu_gyro[3] = {0.0, 0.0, 0.0};
|
||||
double imu_quat[4] = {1.0, 0.0, 0.0, 0.0};
|
||||
|
||||
// Battery and system
|
||||
double battery_voltage = 0.0;
|
||||
double battery_current = 0.0;
|
||||
int battery_percentage = 0;
|
||||
double temperature = 0.0;
|
||||
|
||||
// Motor states (20 motors)
|
||||
double motor_positions[20] = {0};
|
||||
double motor_velocities[20] = {0};
|
||||
double motor_torques[20] = {0};
|
||||
double motor_temperatures[20] = {0};
|
||||
|
||||
// Status flags
|
||||
bool is_connected = false;
|
||||
bool emergency_stop = false;
|
||||
bool low_battery = false;
|
||||
bool overheated = false;
|
||||
|
||||
std::chrono::steady_clock::time_point timestamp;
|
||||
};
|
||||
|
||||
struct MotionCommand {
|
||||
enum Type {
|
||||
VELOCITY,
|
||||
POSITION,
|
||||
SPECIAL_ACTION,
|
||||
GAIT_CHANGE,
|
||||
BODY_POSE
|
||||
} type;
|
||||
|
||||
// Velocity command
|
||||
double linear_velocity[3] = {0.0, 0.0, 0.0}; // vx, vy, vz
|
||||
double angular_velocity[3] = {0.0, 0.0, 0.0}; // wx, wy, wz
|
||||
|
||||
// Position command
|
||||
double target_position[3] = {0.0, 0.0, 0.0};
|
||||
double target_orientation[3] = {0.0, 0.0, 0.0};
|
||||
|
||||
// Body pose
|
||||
double body_height = 0.0;
|
||||
double body_roll = 0.0;
|
||||
double body_pitch = 0.0;
|
||||
double body_yaw = 0.0;
|
||||
|
||||
// Special actions
|
||||
std::string action_name;
|
||||
nlohmann::json action_params;
|
||||
|
||||
// Gait parameters
|
||||
int gait_type = 0;
|
||||
double step_height = 0.0;
|
||||
|
||||
std::chrono::steady_clock::time_point timestamp;
|
||||
double duration = 0.0; // command duration in seconds
|
||||
};
|
||||
|
||||
class Controller {
|
||||
public:
|
||||
using StateCallback = std::function<void(const RobotState&)>;
|
||||
using ErrorCallback = std::function<void(const std::string&)>;
|
||||
|
||||
explicit Controller(const std::string& networkInterface);
|
||||
~Controller();
|
||||
|
||||
bool initialize();
|
||||
void shutdown();
|
||||
|
||||
// State management
|
||||
bool start();
|
||||
bool stop();
|
||||
bool isRunning() const { return running_; }
|
||||
|
||||
RobotState getCurrentState() const;
|
||||
void setStateCallback(StateCallback callback);
|
||||
void setErrorCallback(ErrorCallback callback);
|
||||
// Sport
|
||||
bool StandUp();
|
||||
bool StandDown();
|
||||
bool Sit();
|
||||
bool Lie();
|
||||
bool Damp();
|
||||
bool RecoveryStand();
|
||||
bool StopMove();
|
||||
bool BalanceStand();
|
||||
bool Dance1();
|
||||
bool Dance2();
|
||||
bool Hello();
|
||||
|
||||
// Obstacle
|
||||
bool SwitchSet(bool enable);
|
||||
bool SwitchGet(bool& enable);
|
||||
bool UseRemoteCommandFromApi(bool isRemoteCommandsFromApi);
|
||||
bool MoveToAbsolutePosition(float x, float y, float yaw);
|
||||
bool MoveToIncrementPosition(float x, float y, float yaw);
|
||||
|
||||
// Motion control
|
||||
bool executeCommand(const MotionCommand& command);
|
||||
bool emergencyStop();
|
||||
bool recoveryStand();
|
||||
|
||||
// Basic motions
|
||||
bool standUp();
|
||||
bool standDown();
|
||||
bool sit();
|
||||
bool lie();
|
||||
bool damp();
|
||||
|
||||
// Movement
|
||||
bool move(double vx, double vy, double vyaw);
|
||||
bool moveToPosition(double x, double y, double yaw);
|
||||
bool stop();
|
||||
|
||||
// Body control
|
||||
bool setBodyPose(double roll, double pitch, double yaw, double height);
|
||||
bool balanceStand();
|
||||
|
||||
// Gait control
|
||||
bool switchGait(int gaitType);
|
||||
bool setSpeedLevel(int level);
|
||||
|
||||
// Special actions
|
||||
bool performAction(const std::string& actionName, const nlohmann::json& params = {});
|
||||
|
||||
// Dance and tricks
|
||||
bool dance1();
|
||||
bool dance2();
|
||||
bool frontFlip();
|
||||
bool backFlip();
|
||||
bool hello();
|
||||
bool stretch();
|
||||
|
||||
// Obstacle avoidance
|
||||
bool enableObstacleAvoidance(bool enable);
|
||||
bool isObstacleAvoidanceEnabled();
|
||||
|
||||
// Video control
|
||||
bool startVideo();
|
||||
bool stopVideo();
|
||||
bool isVideoActive();
|
||||
|
||||
// Audio control
|
||||
bool playAudio(const std::string& text);
|
||||
bool stopAudio();
|
||||
|
||||
private:
|
||||
void controlLoop();
|
||||
void stateUpdateLoop();
|
||||
void processCommands();
|
||||
|
||||
void updateSportModeState(const void* message);
|
||||
void updateLowLevelState(const void* message);
|
||||
|
||||
bool validateCommand(const MotionCommand& command);
|
||||
void executeMotionCommand(const MotionCommand& command);
|
||||
|
||||
void handleError(const std::string& error);
|
||||
void checkSafetyLimits();
|
||||
|
||||
// SDK clients
|
||||
std::unique_ptr<unitree::robot::go2::SportClient> sportClient_;
|
||||
std::unique_ptr<unitree::robot::go2::ObstaclesAvoidClient> obstacleClient_;
|
||||
std::unique_ptr<unitree::robot::go2::VideoClient> videoClient_;
|
||||
std::unique_ptr<unitree::robot::go2::VuiClient> vuiClient_;
|
||||
|
||||
// State subscribers
|
||||
std::unique_ptr<unitree::robot::ChannelSubscriber<unitree_go::msg::dds_::SportModeState_>> sportStateSubscriber_;
|
||||
std::unique_ptr<unitree::robot::ChannelSubscriber<unitree_go::msg::dds_::LowState_>> lowStateSubscriber_;
|
||||
|
||||
// Current state
|
||||
mutable std::mutex stateMutex_;
|
||||
RobotState currentState_;
|
||||
|
||||
// Command queue
|
||||
std::queue<MotionCommand> commandQueue_;
|
||||
std::mutex commandMutex_;
|
||||
std::condition_variable commandCondition_;
|
||||
|
||||
// Control threads
|
||||
std::thread controlThread_;
|
||||
std::thread stateThread_;
|
||||
std::atomic<bool> running_;
|
||||
std::atomic<bool> initialized_;
|
||||
|
||||
// Callbacks
|
||||
StateCallback stateCallback_;
|
||||
ErrorCallback errorCallback_;
|
||||
|
||||
// Safety
|
||||
std::chrono::steady_clock::time_point lastCommandTime_;
|
||||
std::atomic<bool> emergencyStopActive_;
|
||||
|
||||
// Network
|
||||
std::string networkInterface_;
|
||||
|
||||
// Timing
|
||||
std::chrono::milliseconds controlPeriod_;
|
||||
std::chrono::milliseconds statePeriod_;
|
||||
};
|
||||
|
||||
} // namespace custom
|
||||
|
||||
@@ -1,123 +1,34 @@
|
||||
#pragma once
|
||||
|
||||
#include "controller.hpp"
|
||||
#include "mqtt.hpp"
|
||||
#include "config.hpp"
|
||||
#include "logger.hpp"
|
||||
|
||||
#include <memory>
|
||||
#include <thread>
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <nlohmann/json.hpp>
|
||||
#include <unitree/robot/go2/robot_state/robot_state_client.hpp>
|
||||
|
||||
namespace custom {
|
||||
|
||||
class CustomRobot {
|
||||
public:
|
||||
explicit CustomRobot(const std::string& configFile = "");
|
||||
explicit CustomRobot();
|
||||
~CustomRobot();
|
||||
|
||||
bool initialize();
|
||||
void shutdown();
|
||||
|
||||
bool start();
|
||||
bool stop();
|
||||
bool isRunning() const { return running_; }
|
||||
|
||||
// Main run loop
|
||||
void run();
|
||||
void runAsync();
|
||||
void waitForShutdown();
|
||||
// Robot state methods
|
||||
bool GetServiceList(std::vector<unitree::robot::go2::ServiceState>& serviceList);
|
||||
bool SwitchService(const std::string& serviceName, bool enable);
|
||||
bool SetReportFreq(int32_t interval, int32_t duration);
|
||||
|
||||
private:
|
||||
// MQTT message handlers
|
||||
void handleMqttMessage(const std::string& topic, const std::string& payload);
|
||||
void handleCommandMessage(const nlohmann::json& command);
|
||||
void handleConfigMessage(const nlohmann::json& config);
|
||||
void handleControlMessage(const nlohmann::json& control);
|
||||
std::unique_ptr<Controller> controller_;
|
||||
std::unique_ptr<unitree::robot::go2::RobotStateClient> stateClient_;
|
||||
|
||||
// Robot state handlers
|
||||
void handleRobotState(const RobotState& state);
|
||||
void handleRobotError(const std::string& error);
|
||||
|
||||
// Publishing functions
|
||||
void publishState();
|
||||
void publishHeartbeat();
|
||||
void publishError(const std::string& error);
|
||||
void publishResponse(const std::string& requestId, bool success, const std::string& message = "");
|
||||
|
||||
// Command processing
|
||||
bool processMotionCommand(const nlohmann::json& cmd);
|
||||
bool processSpecialAction(const nlohmann::json& cmd);
|
||||
bool processSystemCommand(const nlohmann::json& cmd);
|
||||
bool processConfigCommand(const nlohmann::json& cmd);
|
||||
|
||||
// Utility functions
|
||||
MotionCommand jsonToMotionCommand(const nlohmann::json& json);
|
||||
nlohmann::json robotStateToJson(const RobotState& state);
|
||||
nlohmann::json createStatusMessage();
|
||||
|
||||
void startPeriodicTasks();
|
||||
void stopPeriodicTasks();
|
||||
|
||||
// Connection management
|
||||
void handleMqttConnection(bool connected);
|
||||
void reconnectMqtt();
|
||||
|
||||
// Safety monitoring
|
||||
void safetyMonitorLoop();
|
||||
void checkEmergencyConditions();
|
||||
|
||||
// Components
|
||||
std::unique_ptr<Controller> robotController_;
|
||||
std::unique_ptr<MqttClient> mqttClient_;
|
||||
|
||||
// Configuration
|
||||
std::string configFile_;
|
||||
|
||||
// State
|
||||
std::atomic<bool> running_;
|
||||
std::atomic<bool> initialized_;
|
||||
std::atomic<bool> mqttConnected_;
|
||||
|
||||
// Threads
|
||||
std::thread mainThread_;
|
||||
std::thread safetyThread_;
|
||||
std::vector<std::thread> periodicThreads_;
|
||||
|
||||
// Timing
|
||||
std::chrono::steady_clock::time_point lastHeartbeat_;
|
||||
std::chrono::steady_clock::time_point lastStatePublish_;
|
||||
std::chrono::steady_clock::time_point lastCommandReceived_;
|
||||
|
||||
// Statistics
|
||||
struct Statistics {
|
||||
uint64_t commandsReceived = 0;
|
||||
uint64_t commandsExecuted = 0;
|
||||
uint64_t commandsFailed = 0;
|
||||
uint64_t statesPublished = 0;
|
||||
uint64_t errorsOccurred = 0;
|
||||
std::chrono::steady_clock::time_point startTime;
|
||||
} stats_;
|
||||
|
||||
// Safety parameters
|
||||
double maxIdleTime_ = 30.0; // seconds
|
||||
double heartbeatInterval_ = 5.0; // seconds
|
||||
double statePublishInterval_ = 0.02; // 50Hz
|
||||
|
||||
// Current robot state cache
|
||||
mutable std::mutex stateCacheMutex_;
|
||||
RobotState lastKnownState_;
|
||||
|
||||
// Request tracking
|
||||
std::mutex requestMutex_;
|
||||
std::map<std::string, std::chrono::steady_clock::time_point> pendingRequests_;
|
||||
|
||||
// Error handling
|
||||
std::mutex errorMutex_;
|
||||
std::queue<std::string> errorQueue_;
|
||||
void processErrorQueue();
|
||||
};
|
||||
|
||||
} // namespace custom
|
||||
|
||||
Reference in New Issue
Block a user