tanjianbin/lzwc-terminal-unitreeGo2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Refactor configuration management by introducing CustomConfig class. Updated config.hpp to streamline settings and removed legacy configuration presets. Adjusted controller and custom_robot classes to utilize the new configuration structure. Enhanced error handling and logging during initialization processes.

Browse Source
This commit is contained in:
Sucan126
2025-09-08 18:16:50 +08:00
parent f72ce9ce58
commit 926d88972d
8 changed files with 188 additions and 556 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@@ -29,6 +29,7 @@ set(SOURCES
src/custom_robot.cpp
src/config.cpp
src/logger.cpp
# src/mqtt.cpp # Disabled MQTT for now
)
# Create executable with simple name

216
include/config.hpp
View File

@@ -3,205 +3,101 @@
#include <string>
#include <string_view>
namespace customConfig {
namespace custom {
// Configuration template selector
enum class ConfigPreset {
Default,
HighPerformance,
Development,
Safety
};
// Motion gait enum
/**
* @brief Robot motion gait types
*/
enum class Gait : int {
IDLE = 0,
TROT = 1,
TROT_RUNNING = 2
IDLE = 0, // Standing still
TROT = 1, // Normal walking
TROT_RUNNING = 2 // Fast running
};
// Network configuration
// Network settings
constexpr std::string_view NETWORK_INTERFACE = "eth0";
// MQTT configuration
// MQTT settings
constexpr std::string_view MQTT_BROKER = "localhost";
constexpr int MQTT_PORT = 1883;
constexpr std::string_view 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 configuration
// Topic settings
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 configuration
// Robot control settings
constexpr double CONTROL_FREQUENCY = 200.0; // Hz
constexpr double STATE_PUBLISH_FREQUENCY = 50.0; // Hz
// Safety configuration
// Safety settings
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 configuration
// Motion settings
constexpr double STAND_HEIGHT = 0.0; // relative height
constexpr Gait DEFAULT_GAIT = Gait::IDLE;
// Video configuration
// Video settings
constexpr int VIDEO_WIDTH = 1280;
constexpr int VIDEO_HEIGHT = 720;
constexpr int VIDEO_FPS = 30;
constexpr std::string_view VIDEO_FORMAT = "h264";
constexpr bool VIDEO_ENABLED = true;
// Audio configuration
// Audio settings
constexpr int AUDIO_SAMPLE_RATE = 16000;
constexpr int AUDIO_CHANNELS = 1;
constexpr std::string_view AUDIO_FORMAT = "pcm";
constexpr bool AUDIO_ENABLED = true;
// Configuration presets using template specialization
template<ConfigPreset P>
struct ConfigParams;
class CustomConfig {
public:
// 网络和MQTT连接
std::string network_interface;
std::string mqtt_broker;
int mqtt_port;
std::string mqtt_username;
std::string mqtt_password;
std::string mqtt_client_id;
// MQTT话题
std::string topic_prefix;
std::string topic_cmd;
std::string topic_state;
std::string topic_video;
std::string topic_audio;
// 机器人控制和安全
double control_frequency;
double state_publish_frequency;
double max_linear_velocity;
double max_angular_velocity;
double emergency_stop_timeout;
double stand_height;
int default_gait;
// 多媒体
bool enable_video;
bool enable_audio;
int video_width;
int video_height;
int video_fps;
std::string video_format;
int audio_sample_rate;
int audio_channels;
std::string audio_format;
void loadDefaults();
// 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;
};
// 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
};
// 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;
};
// 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
static constexpr std::string_view network_interface = NETWORK_INTERFACE;
// MQTT settings
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
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 (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 (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
static constexpr double stand_height = STAND_HEIGHT;
static constexpr Gait default_gait = DEFAULT_GAIT;
// Video settings
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
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);
}
};
// Type alias for current configuration
using RobotConfig = CompileTimeConfig<ACTIVE_PRESET>;
// 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;
}
// 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
} // namespace custom

9
include/controller.hpp
View File

@@ -17,19 +17,19 @@ namespace custom {
class Controller {
public:
explicit Controller(const std::string& networkInterface);
Controller();
~Controller();
bool initialize();
bool start();
bool stop();
void shutdown() { stop(); }
bool isRunning() const { return running_; }
// Sport
bool StandUp();
bool StandDown();
bool Sit();
bool Lie();
bool Damp();
bool RecoveryStand();
bool StopMove();
@@ -47,12 +47,11 @@ public:
private:
std::unique_ptr<unitree::robot::go2::SportClient> sportClient_;
std::unique_ptr<unitree::robot::go2::ObstaclesAvoidClient> obstacleClient_;
std::unique_ptr<unitree::robot::go2::SportClient> sc_;
std::unique_ptr<unitree::robot::go2::ObstaclesAvoidClient> oac_;
std::atomic<bool> running_;
std::atomic<bool> initialized_;
std::string networkInterface_;
};
} // namespace custom

7
include/custom_robot.hpp
View File

@@ -6,6 +6,8 @@
#include <memory>
#include <atomic>
#include <string>
#include <vector>
#include <unitree/robot/go2/robot_state/robot_state_client.hpp>
namespace custom {
@@ -18,6 +20,8 @@ public:
bool initialize();
bool start();
Controller* getController() const { return controller_.get(); }
// Robot state methods
bool GetServiceList(std::vector<unitree::robot::go2::ServiceState>& serviceList);
bool SwitchService(const std::string& serviceName, bool enable);
@@ -25,8 +29,9 @@ public:
private:
std::unique_ptr<Controller> controller_;
std::unique_ptr<unitree::robot::go2::RobotStateClient> stateClient_;
std::unique_ptr<unitree::robot::go2::RobotStateClient> rsc_;
CustomConfig config_;
std::atomic<bool> running_;
std::atomic<bool> initialized_;
};

351
src/config.cpp
View File

@@ -1,319 +1,42 @@
#include "config.hpp"
#include "logger.hpp"
#include <fstream>
#include <iostream>
namespace customConfig {
namespace custom {
Config::Config() {
// Load default configuration
loadDefaults();
void CustomConfig::loadDefaults() {
// 网络和MQTT连接
network_interface = std::string(NETWORK_INTERFACE);
mqtt_broker = std::string(MQTT_BROKER);
mqtt_port = MQTT_PORT;
mqtt_username = std::string(MQTT_USERNAME);
mqtt_password = std::string(MQTT_PASSWORD);
mqtt_client_id = std::string(MQTT_CLIENT_ID);
// MQTT话题
topic_prefix = std::string(TOPIC_PREFIX);
topic_cmd = std::string(TOPIC_CMD);
topic_state = std::string(TOPIC_STATE);
topic_video = std::string(TOPIC_VIDEO);
topic_audio = std::string(TOPIC_AUDIO);
// 机器人控制和安全
control_frequency = CONTROL_FREQUENCY;
state_publish_frequency = STATE_PUBLISH_FREQUENCY;
max_linear_velocity = MAX_LINEAR_VELOCITY;
max_angular_velocity = MAX_ANGULAR_VELOCITY;
emergency_stop_timeout = EMERGENCY_STOP_TIMEOUT;
stand_height = STAND_HEIGHT;
default_gait = static_cast<int>(DEFAULT_GAIT);
// 多媒体
enable_video = VIDEO_ENABLED;
enable_audio = AUDIO_ENABLED;
video_width = VIDEO_WIDTH;
video_height = VIDEO_HEIGHT;
video_fps = VIDEO_FPS;
video_format = std::string(VIDEO_FORMAT);
audio_sample_rate = AUDIO_SAMPLE_RATE;
audio_channels = AUDIO_CHANNELS;
audio_format = std::string(AUDIO_FORMAT);
}
Config& Config::getInstance() {
static Config instance;
return instance;
}
void Config::loadDefaults() {
config_.network_interface = NETWORK_INTERFACE;
config_.mqtt_broker = MQTT_BROKER;
config_.mqtt_port = MQTT_PORT;
config_.mqtt_username = MQTT_USERNAME;
config_.mqtt_password = MQTT_PASSWORD;
config_.mqtt_client_id = MQTT_CLIENT_ID;
config_.topic_prefix = TOPIC_PREFIX;
config_.cmd_topic = TOPIC_CMD;
config_.state_topic = TOPIC_STATE;
config_.video_topic = TOPIC_VIDEO;
config_.audio_topic = TOPIC_AUDIO;
config_.control_frequency = CONTROL_FREQUENCY;
config_.state_publish_frequency = STATE_PUBLISH_FREQUENCY;
config_.enable_video = VIDEO_ENABLED;
config_.enable_audio = AUDIO_ENABLED;
config_.max_linear_velocity = MAX_LINEAR_VELOCITY;
config_.max_angular_velocity = MAX_ANGULAR_VELOCITY;
config_.emergency_stop_timeout = EMERGENCY_STOP_TIMEOUT;
config_.stand_height = STAND_HEIGHT;
config_.default_gait = static_cast<int>(DEFAULT_GAIT);
config_.video_width = VIDEO_WIDTH;
config_.video_height = VIDEO_HEIGHT;
config_.video_fps = VIDEO_FPS;
config_.video_format = VIDEO_FORMAT;
config_.audio_sample_rate = AUDIO_SAMPLE_RATE;
config_.audio_channels = AUDIO_CHANNELS;
config_.audio_format = AUDIO_FORMAT;
}
void Config::loadHighPerformancePreset() {
loadDefaults();
using hp = ConfigParams<ConfigPreset::HighPerformance>;
config_.control_frequency = hp::control_frequency;
config_.state_publish_frequency = hp::state_publish_frequency;
config_.max_linear_velocity = hp::max_linear_velocity;
config_.max_angular_velocity = hp::max_angular_velocity;
config_.enable_video = hp::video_enabled;
config_.enable_audio = hp::audio_enabled;
}
void Config::loadDevelopmentPreset() {
loadDefaults();
using dev = ConfigParams<ConfigPreset::Development>;
config_.control_frequency = dev::control_frequency;
config_.state_publish_frequency = dev::state_publish_frequency;
config_.max_linear_velocity = dev::max_linear_velocity;
config_.max_angular_velocity = dev::max_angular_velocity;
config_.enable_video = dev::video_enabled;
config_.enable_audio = dev::audio_enabled;
}
void Config::loadSafetyPreset() {
loadDefaults();
using safe = ConfigParams<ConfigPreset::Safety>;
config_.control_frequency = safe::control_frequency;
config_.state_publish_frequency = safe::state_publish_frequency;
config_.max_linear_velocity = safe::max_linear_velocity;
config_.max_angular_velocity = safe::max_angular_velocity;
config_.emergency_stop_timeout = safe::emergency_stop_timeout;
config_.enable_video = safe::video_enabled;
config_.enable_audio = safe::audio_enabled;
}
bool Config::loadConfig(const std::string& configFile) {
// For now just load defaults, JSON loading can be added later if needed
loadDefaults();
/* TODO: Add JSON loading support if needed
try {
std::ifstream file(configFile);
if (!file.is_open()) {
LOG_WARN("Config file not found: " + configFile + ", using defaults");
loadDefaults();
return false;
}
nlohmann::json j;
file >> j;
// Load network settings
if (j.contains("network")) {
auto& network = j["network"];
if (network.contains("interface")) {
config_.network_interface = network["interface"];
}
}
// Load MQTT settings
if (j.contains("mqtt")) {
auto& mqtt = j["mqtt"];
if (mqtt.contains("broker")) config_.mqtt_broker = mqtt["broker"];
if (mqtt.contains("port")) config_.mqtt_port = mqtt["port"];
if (mqtt.contains("username")) config_.mqtt_username = mqtt["username"];
if (mqtt.contains("password")) config_.mqtt_password = mqtt["password"];
if (mqtt.contains("client_id")) config_.mqtt_client_id = mqtt["client_id"];
}
// Load topics
if (j.contains("topics")) {
auto& topics = j["topics"];
if (topics.contains("prefix")) config_.topic_prefix = topics["prefix"];
if (topics.contains("cmd")) config_.cmd_topic = topics["cmd"];
if (topics.contains("state")) config_.state_topic = topics["state"];
if (topics.contains("video")) config_.video_topic = topics["video"];
if (topics.contains("audio")) config_.audio_topic = topics["audio"];
}
// Load robot settings
if (j.contains("robot")) {
auto& robot = j["robot"];
if (robot.contains("control_frequency")) config_.control_frequency = robot["control_frequency"];
if (robot.contains("state_publish_frequency")) config_.state_publish_frequency = robot["state_publish_frequency"];
if (robot.contains("enable_video")) config_.enable_video = robot["enable_video"];
if (robot.contains("enable_audio")) config_.enable_audio = robot["enable_audio"];
}
// Load safety settings
if (j.contains("safety")) {
auto& safety = j["safety"];
if (safety.contains("max_linear_velocity")) config_.max_linear_velocity = safety["max_linear_velocity"];
if (safety.contains("max_angular_velocity")) config_.max_angular_velocity = safety["max_angular_velocity"];
if (safety.contains("emergency_stop_timeout")) config_.emergency_stop_timeout = safety["emergency_stop_timeout"];
}
// Load motion settings
if (j.contains("motion")) {
auto& motion = j["motion"];
if (motion.contains("stand_height")) config_.stand_height = motion["stand_height"];
if (motion.contains("default_gait")) config_.default_gait = motion["default_gait"];
}
// Load video settings
if (j.contains("video")) {
auto& video = j["video"];
if (video.contains("width")) config_.video_width = video["width"];
if (video.contains("height")) config_.video_height = video["height"];
if (video.contains("fps")) config_.video_fps = video["fps"];
if (video.contains("format")) config_.video_format = video["format"];
}
// Load audio settings
if (j.contains("audio")) {
auto& audio = j["audio"];
if (audio.contains("sample_rate")) config_.audio_sample_rate = audio["sample_rate"];
if (audio.contains("channels")) config_.audio_channels = audio["channels"];
if (audio.contains("format")) config_.audio_format = audio["format"];
}
LOG_INFO("Configuration loaded successfully from: " + configFile);
return true;
} catch (const std::exception& e) {
LOG_ERROR("Failed to load config: " + std::string(e.what()));
loadDefaults();
return false;
}
*/
return true;
}
bool Config::saveConfig(const std::string& configFile) {
try {
nlohmann::json j;
// Network settings
j["network"]["interface"] = config_.network_interface;
// MQTT settings
j["mqtt"]["broker"] = config_.mqtt_broker;
j["mqtt"]["port"] = config_.mqtt_port;
j["mqtt"]["username"] = config_.mqtt_username;
j["mqtt"]["password"] = config_.mqtt_password;
j["mqtt"]["client_id"] = config_.mqtt_client_id;
// Topics
j["topics"]["prefix"] = config_.topic_prefix;
j["topics"]["cmd"] = config_.cmd_topic;
j["topics"]["state"] = config_.state_topic;
j["topics"]["video"] = config_.video_topic;
j["topics"]["audio"] = config_.audio_topic;
// Robot settings
j["robot"]["control_frequency"] = config_.control_frequency;
j["robot"]["state_publish_frequency"] = config_.state_publish_frequency;
j["robot"]["enable_video"] = config_.enable_video;
j["robot"]["enable_audio"] = config_.enable_audio;
// Safety settings
j["safety"]["max_linear_velocity"] = config_.max_linear_velocity;
j["safety"]["max_angular_velocity"] = config_.max_angular_velocity;
j["safety"]["emergency_stop_timeout"] = config_.emergency_stop_timeout;
// Motion settings
j["motion"]["stand_height"] = config_.stand_height;
j["motion"]["default_gait"] = config_.default_gait;
// Video settings
j["video"]["width"] = config_.video_width;
j["video"]["height"] = config_.video_height;
j["video"]["fps"] = config_.video_fps;
j["video"]["format"] = config_.video_format;
// Audio settings
j["audio"]["sample_rate"] = config_.audio_sample_rate;
j["audio"]["channels"] = config_.audio_channels;
j["audio"]["format"] = config_.audio_format;
std::ofstream file(configFile);
if (!file.is_open()) {
LOG_ERROR("Failed to open config file for writing: " + configFile);
return false;
}
file << j.dump(4) << std::endl;
LOG_INFO("Configuration saved to: " + configFile);
return true;
} catch (const std::exception& e) {
LOG_ERROR("Failed to save config: " + std::string(e.what()));
return false;
}
}
std::string Config::getFullTopic(const std::string& topic) const {
return config_.topic_prefix + "/" + topic;
}
bool Config::validateConfig() {
bool valid = true;
// Validate frequencies
if (config_.control_frequency <= 0 || config_.control_frequency > 1000) {
LOG_WARN("Invalid control frequency, setting to default");
config_.control_frequency = CONTROL_FREQUENCY;
valid = false;
}
if (config_.state_publish_frequency <= 0 || config_.state_publish_frequency > 500) {
LOG_WARN("Invalid state publish frequency, setting to default");
config_.state_publish_frequency = STATE_PUBLISH_FREQUENCY;
valid = false;
}
// Validate velocities
if (config_.max_linear_velocity <= 0 || config_.max_linear_velocity > 5.0) {
LOG_WARN("Invalid max linear velocity, setting to default");
config_.max_linear_velocity = MAX_LINEAR_VELOCITY;
valid = false;
}
if (config_.max_angular_velocity <= 0 || config_.max_angular_velocity > 10.0) {
LOG_WARN("Invalid max angular velocity, setting to default");
config_.max_angular_velocity = MAX_ANGULAR_VELOCITY;
valid = false;
}
// Validate timeout
if (config_.emergency_stop_timeout <= 0 || config_.emergency_stop_timeout > 60.0) {
LOG_WARN("Invalid emergency stop timeout, setting to default");
config_.emergency_stop_timeout = EMERGENCY_STOP_TIMEOUT;
valid = false;
}
// Validate video settings
if (config_.video_width <= 0 || config_.video_height <= 0) {
LOG_WARN("Invalid video dimensions, setting to default");
config_.video_width = VIDEO_WIDTH;
config_.video_height = VIDEO_HEIGHT;
valid = false;
}
if (config_.video_fps <= 0 || config_.video_fps > 120) {
LOG_WARN("Invalid video FPS, setting to default");
config_.video_fps = VIDEO_FPS;
valid = false;
}
// Validate audio settings
if (config_.audio_sample_rate <= 0) {
LOG_WARN("Invalid audio sample rate, setting to default");
config_.audio_sample_rate = AUDIO_SAMPLE_RATE;
valid = false;
}
if (config_.audio_channels <= 0) {
LOG_WARN("Invalid audio channels, setting to default");
config_.audio_channels = AUDIO_CHANNELS;
valid = false;
}
return valid;
}
} // namespace customConfig
} // namespace custom

70
src/controller.cpp
View File

@@ -1,13 +1,13 @@
#include "controller.hpp"
#include "config.hpp"
#include "logger.hpp"
#include <unitree/robot/channel/channel_factory.hpp>
#include <unitree/common/thread/thread.hpp>
namespace custom {
Controller::Controller(const std::string& networkInterface)
: networkInterface_(networkInterface), running_(false), initialized_(false) {
Controller::Controller()
: running_(false), initialized_(false) {
LOG_INFO("Controller created");
}
Controller::~Controller() {
@@ -16,24 +16,20 @@ Controller::~Controller() {
}
LOG_INFO("Shutting down robot controller");
sportClient_.reset();
obstacleClient_.reset();
sc_.reset();
oac_.reset();
initialized_ = false;
}
bool Controller::initialize() {
try {
LOG_INFO("Initializing robot controller with interface: " + networkInterface_);
sc_ = std::make_unique<unitree::robot::go2::SportClient>();
sc_->SetTimeout(10.0f);
sc_->Init();
unitree::robot::ChannelFactory::Instance()->Init(0, networkInterface_);
sportClient_ = std::make_unique<unitree::robot::go2::SportClient>();
sportClient_->SetTimeout(10.0f);
sportClient_->Init();
obstacleClient_ = std::make_unique<unitree::robot::go2::ObstaclesAvoidClient>();
obstacleClient_->SetTimeout(3.0f);
obstacleClient_->Init();
oac_ = std::make_unique<unitree::robot::go2::ObstaclesAvoidClient>();
oac_->SetTimeout(3.0f);
oac_->Init();
initialized_ = true;
@@ -70,8 +66,6 @@ bool Controller::stop() {
return true;
}
LOG_INFO("Stopping robot controller");
running_ = false;
LOG_INFO("Robot controller stopped");
@@ -84,7 +78,7 @@ bool Controller::stop() {
bool Controller::StandUp() {
LOG_INFO("Standing up");
try {
return sportClient_ && sportClient_->StandUp() == 0;
return sc_ && sc_->StandUp() == 0;
} catch (const std::exception& e) {
LOG_ERROR("Stand up failed: " + std::string(e.what()));
return false;
@@ -94,7 +88,7 @@ bool Controller::StandUp() {
bool Controller::StandDown() {
LOG_INFO("Standing down");
try {
return sportClient_ && sportClient_->StandDown() == 0;
return sc_ && sc_->StandDown() == 0;
} catch (const std::exception& e) {
LOG_ERROR("Stand down failed: " + std::string(e.what()));
return false;
@@ -104,27 +98,17 @@ bool Controller::StandDown() {
bool Controller::Sit() {
LOG_INFO("Sitting");
try {
return sportClient_ && sportClient_->Sit() == 0;
return sc_ && sc_->Sit() == 0;
} catch (const std::exception& e) {
LOG_ERROR("Sit failed: " + std::string(e.what()));
return false;
}
}
bool Controller::Lie() {
LOG_INFO("Lying down");
try {
return sportClient_ && sportClient_->StandDown() == 0;
} catch (const std::exception& e) {
LOG_ERROR("Lie down failed: " + std::string(e.what()));
return false;
}
}
bool Controller::Damp() {
LOG_INFO("Damping");
try {
return sportClient_ && sportClient_->Damp() == 0;
return sc_ && sc_->Damp() == 0;
} catch (const std::exception& e) {
LOG_ERROR("Damp failed: " + std::string(e.what()));
return false;
@@ -135,8 +119,8 @@ bool Controller::RecoveryStand() {
LOG_INFO("Executing recovery stand");
try {
if (sportClient_) {
int32_t result = sportClient_->RecoveryStand();
if (sc_) {
int32_t result = sc_->RecoveryStand();
return result == 0;
}
return false;
@@ -148,7 +132,7 @@ bool Controller::RecoveryStand() {
bool Controller::StopMove() {
try {
return sportClient_ && sportClient_->StopMove() == 0;
return sc_ && sc_->StopMove() == 0;
} catch (const std::exception& e) {
LOG_ERROR("Stop move failed: " + std::string(e.what()));
return false;
@@ -157,7 +141,7 @@ bool Controller::StopMove() {
bool Controller::BalanceStand() {
try {
return sportClient_ && sportClient_->BalanceStand() == 0;
return sc_ && sc_->BalanceStand() == 0;
} catch (const std::exception& e) {
LOG_ERROR("Balance stand failed: " + std::string(e.what()));
return false;
@@ -166,7 +150,7 @@ bool Controller::BalanceStand() {
bool Controller::Dance1() {
try {
return sportClient_ && sportClient_->Dance1() == 0;
return sc_ && sc_->Dance1() == 0;
} catch (const std::exception& e) {
LOG_ERROR("Dance1 failed: " + std::string(e.what()));
return false;
@@ -175,7 +159,7 @@ bool Controller::Dance1() {
bool Controller::Dance2() {
try {
return sportClient_ && sportClient_->Dance2() == 0;
return sc_ && sc_->Dance2() == 0;
} catch (const std::exception& e) {
LOG_ERROR("Dance2 failed: " + std::string(e.what()));
return false;
@@ -184,7 +168,7 @@ bool Controller::Dance2() {
bool Controller::Hello() {
try {
return sportClient_ && sportClient_->Hello() == 0;
return sc_ && sc_->Hello() == 0;
} catch (const std::exception& e) {
LOG_ERROR("Hello failed: " + std::string(e.what()));
return false;
@@ -196,7 +180,7 @@ bool Controller::Hello() {
bool Controller::SwitchSet(bool enable) {
try {
return obstacleClient_ && obstacleClient_->SwitchSet(enable) == 0;
return oac_ && oac_->SwitchSet(enable) == 0;
} catch (const std::exception& e) {
LOG_ERROR("Switch set failed: " + std::string(e.what()));
return false;
@@ -205,7 +189,7 @@ bool Controller::SwitchSet(bool enable) {
bool Controller::SwitchGet(bool& enable) {
try {
return obstacleClient_ && obstacleClient_->SwitchGet(enable) == 0;
return oac_ && oac_->SwitchGet(enable) == 0;
} catch (const std::exception& e) {
LOG_ERROR("Switch get failed: " + std::string(e.what()));
return false;
@@ -214,7 +198,7 @@ bool Controller::SwitchGet(bool& enable) {
bool Controller::UseRemoteCommandFromApi(bool isRemoteCommandsFromApi) {
try {
return obstacleClient_ && obstacleClient_->UseRemoteCommandFromApi(isRemoteCommandsFromApi) == 0;
return oac_ && oac_->UseRemoteCommandFromApi(isRemoteCommandsFromApi) == 0;
} catch (const std::exception& e) {
LOG_ERROR("Use remote command from api failed: " + std::string(e.what()));
return false;
@@ -223,7 +207,7 @@ bool Controller::UseRemoteCommandFromApi(bool isRemoteCommandsFromApi) {
bool Controller::MoveToAbsolutePosition(float x, float y, float yaw) {
try {
return obstacleClient_ && obstacleClient_->MoveToAbsolutePosition(x, y, yaw) == 0;
return oac_ && oac_->MoveToAbsolutePosition(x, y, yaw) == 0;
} catch (const std::exception& e) {
LOG_ERROR("Move to absolute position failed: " + std::string(e.what()));
return false;
@@ -232,7 +216,7 @@ bool Controller::MoveToAbsolutePosition(float x, float y, float yaw) {
bool Controller::MoveToIncrementPosition(float x, float y, float yaw) {
try {
return obstacleClient_ && obstacleClient_->MoveToIncrementPosition(x, y, yaw) == 0;
return oac_ && oac_->MoveToIncrementPosition(x, y, yaw) == 0;
} catch (const std::exception& e) {
LOG_ERROR("Move to increment position failed: " + std::string(e.what()));
return false;

74
src/custom_robot.cpp
View File

@@ -2,13 +2,22 @@
#include <thread>
#include <chrono>
#include <functional>
#include <unitree/robot/channel/channel_factory.hpp>
namespace custom {
CustomRobot::CustomRobot()
: initialized_(false), running_(false) {
// 创建并加载配置
config_.loadDefaults();
try {
unitree::robot::ChannelFactory::Instance()->Init(0, config_.network_interface);
LOG_INFO("ChannelFactory initialized with interface: " + config_.network_interface);
} catch (const std::exception& e) {
LOG_ERROR("Failed to initialize ChannelFactory: " + std::string(e.what()));
}
}
CustomRobot::~CustomRobot() {
LOG_INFO("Shutting down CustomRobot");
@@ -17,32 +26,30 @@ CustomRobot::~CustomRobot() {
controller_.reset();
}
if (stateClient_) {
stateClient_.reset();
if (rsc_) {
rsc_.reset();
}
try {
unitree::robot::ChannelFactory::Instance()->Release();
LOG_INFO("ChannelFactory released");
} catch (const std::exception& e) {
LOG_ERROR("Failed to release ChannelFactory: " + std::string(e.what()));
}
initialized_ = false;
}
bool CustomRobot::initialize() {
LOG_INFO("Initializing CustomRobot");
auto& config = Config::getInstance().getConfig();
controller_ = std::make_unique<Controller>(config.network_interface);
if (!controller_->initialize()) {
LOG_ERROR("Failed to initialize robot controller");
return false;
}
// Initialize robot state client
try {
stateClient_ = std::make_unique<unitree::robot::go2::RobotStateClient>();
stateClient_->SetTimeout(10.0f);
stateClient_->Init();
LOG_INFO("RobotStateClient initialized successfully");
controller_ = std::make_unique<Controller>();
controller_->initialize();
rsc_ = std::make_unique<unitree::robot::go2::RobotStateClient>();
rsc_->SetTimeout(3.0);
rsc_->Init();
} catch (const std::exception& e) {
LOG_ERROR("Failed to initialize RobotStateClient: " + std::string(e.what()));
LOG_ERROR("Failed to initialize : " + std::string(e.what()));
return false;
}
@@ -63,6 +70,23 @@ bool CustomRobot::start() {
return true;
}
std::vector<unitree::robot::go2::ServiceState> serviceList;
if (!GetServiceList(serviceList)) {
LOG_ERROR("Failed to get service list");
return false;
}
for (const auto& service : serviceList) {
LOG_INFO("Service - name: " + service.name + ", protect: " + std::to_string(service.protect) + ", status: " + std::to_string(service.status));
}
if (!SwitchService("sport_mode", false)) {
LOG_ERROR("Failed to stop sport_mode service");
return false;
}
std::this_thread::sleep_for(std::chrono::seconds(1));
if (!controller_->start()) {
LOG_ERROR("Failed to start robot controller");
return false;
@@ -73,13 +97,13 @@ bool CustomRobot::start() {
}
bool CustomRobot::GetServiceList(std::vector<unitree::robot::go2::ServiceState>& serviceList) {
if (!initialized_ || !stateClient_) {
if (!initialized_ || !rsc_) {
LOG_ERROR("Robot not initialized or stateClient is null");
return false;
}
try {
int32_t ret = stateClient_->ServiceList(serviceList);
int32_t ret = rsc_->ServiceList(serviceList);
if (ret != 0) {
LOG_ERROR("Failed to get service list, error code: " + std::to_string(ret));
return false;
@@ -93,14 +117,14 @@ bool CustomRobot::GetServiceList(std::vector<unitree::robot::go2::ServiceState>&
}
bool CustomRobot::SwitchService(const std::string& serviceName, bool enable) {
if (!initialized_ || !stateClient_) {
if (!initialized_ || !rsc_) {
LOG_ERROR("Robot not initialized or stateClient is null");
return false;
}
try {
int32_t status;
int32_t ret = stateClient_->ServiceSwitch(serviceName, enable ? 1 : 0, status);
int32_t ret = rsc_->ServiceSwitch(serviceName, enable ? 1 : 0, status);
if (ret != 0) {
LOG_ERROR("Failed to switch service " + serviceName + ", error code: " + std::to_string(ret));
return false;
@@ -114,13 +138,13 @@ bool CustomRobot::SwitchService(const std::string& serviceName, bool enable) {
}
bool CustomRobot::SetReportFreq(int32_t interval, int32_t duration) {
if (!initialized_ || !stateClient_) {
if (!initialized_ || !rsc_) {
LOG_ERROR("Robot not initialized or stateClient is null");
return false;
}
try {
int32_t ret = stateClient_->SetReportFreq(interval, duration);
int32_t ret = rsc_->SetReportFreq(interval, duration);
if (ret != 0) {
LOG_ERROR("Failed to set report frequency, error code: " + std::to_string(ret));
return false;
@@ -133,4 +157,4 @@ bool CustomRobot::SetReportFreq(int32_t interval, int32_t duration) {
}
}
}
} // namespace custom

2
src/main.cpp
View File

@@ -23,7 +23,7 @@ int main(int argc, char** argv) {
signal(SIGTERM, signalHandler);
try {
g_robot = std::make_unique<CustomRobot>("");
g_robot = std::make_unique<CustomRobot>();
if (!g_robot->initialize()) {
LOG_ERROR("Failed to initialize robot");