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

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.

Browse Source
This commit is contained in:
Sucan126
2025-09-07 20:20:34 +08:00
parent ecb4c602a1
commit f72ce9ce58
8 changed files with 373 additions and 1519 deletions
Download Patch File Download Diff File Expand all files Collapse all files

89
src/config.cpp
View File

@@ -3,7 +3,7 @@
#include <fstream>
#include <iostream>
namespace custom {
namespace customConfig {
Config::Config() {
// Load default configuration
@@ -16,8 +16,6 @@ Config& Config::getInstance() {
}
void Config::loadDefaults() {
using namespace config;
config_.network_interface = NETWORK_INTERFACE;
config_.mqtt_broker = MQTT_BROKER;
config_.mqtt_port = MQTT_PORT;
@@ -33,15 +31,15 @@ void Config::loadDefaults() {
config_.control_frequency = CONTROL_FREQUENCY;
config_.state_publish_frequency = STATE_PUBLISH_FREQUENCY;
config_.enable_video = ENABLE_VIDEO;
config_.enable_audio = ENABLE_AUDIO;
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 = GAIT;
config_.default_gait = static_cast<int>(DEFAULT_GAIT);
config_.video_width = VIDEO_WIDTH;
config_.video_height = VIDEO_HEIGHT;
@@ -55,46 +53,48 @@ void Config::loadDefaults() {
void Config::loadHighPerformancePreset() {
loadDefaults();
config_.control_frequency = HighPerformancePreset::CONTROL_FREQUENCY;
config_.state_publish_frequency = HighPerformancePreset::STATE_PUBLISH_FREQUENCY;
config_.max_linear_velocity = HighPerformancePreset::MAX_LINEAR_VELOCITY;
config_.max_angular_velocity = HighPerformancePreset::MAX_ANGULAR_VELOCITY;
config_.enable_video = HighPerformancePreset::ENABLE_VIDEO;
config_.enable_audio = HighPerformancePreset::ENABLE_AUDIO;
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();
config_.control_frequency = DevelopmentPreset::CONTROL_FREQUENCY;
config_.state_publish_frequency = DevelopmentPreset::STATE_PUBLISH_FREQUENCY;
config_.max_linear_velocity = DevelopmentPreset::MAX_LINEAR_VELOCITY;
config_.max_angular_velocity = DevelopmentPreset::MAX_ANGULAR_VELOCITY;
config_.enable_video = DevelopmentPreset::ENABLE_VIDEO;
config_.enable_audio = DevelopmentPreset::ENABLE_AUDIO;
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();
config_.control_frequency = SafetyPreset::CONTROL_FREQUENCY;
config_.state_publish_frequency = SafetyPreset::STATE_PUBLISH_FREQUENCY;
config_.max_linear_velocity = SafetyPreset::MAX_LINEAR_VELOCITY;
config_.max_angular_velocity = SafetyPreset::MAX_ANGULAR_VELOCITY;
config_.emergency_stop_timeout = SafetyPreset::EMERGENCY_STOP_TIMEOUT;
config_.enable_video = SafetyPreset::ENABLE_VIDEO;
config_.enable_audio = SafetyPreset::ENABLE_AUDIO;
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) {
// If compile-time config is enabled, skip JSON loading
if (config::USE_COMPILE_TIME_CONFIG) {
loadDefaults();
return true;
}
// 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");
setDefaults();
loadDefaults();
return false;
}
@@ -175,9 +175,12 @@ bool Config::loadConfig(const std::string& configFile) {
} catch (const std::exception& e) {
LOG_ERROR("Failed to load config: " + std::string(e.what()));
setDefaults();
loadDefaults();
return false;
}
*/
return true;
}
bool Config::saveConfig(const std::string& configFile) {
@@ -253,64 +256,64 @@ bool Config::validateConfig() {
// Validate frequencies
if (config_.control_frequency <= 0 || config_.control_frequency > 1000) {
LOG_WARN("Invalid control frequency, setting to default");
config_.control_frequency = config::CONTROL_FREQUENCY;
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 = config::STATE_PUBLISH_FREQUENCY;
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 = config::MAX_LINEAR_VELOCITY;
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 = config::MAX_ANGULAR_VELOCITY;
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 = config::EMERGENCY_STOP_TIMEOUT;
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 = config::VIDEO_WIDTH;
config_.video_height = config::VIDEO_HEIGHT;
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 = config::VIDEO_FPS;
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 = config::AUDIO_SAMPLE_RATE;
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 = config::AUDIO_CHANNELS;
config_.audio_channels = AUDIO_CHANNELS;
valid = false;
}
return valid;
}
} // namespace custom
} // namespace customConfig

467
src/controller.cpp
View File

@@ -7,63 +7,34 @@
namespace custom {
Controller::Controller(const std::string& networkInterface)
: networkInterface_(networkInterface), running_(false), initialized_(false),
emergencyStopActive_(false) {
auto& config = Config::getInstance().getConfig();
controlPeriod_ = std::chrono::milliseconds(static_cast<int>(1000.0 / config.control_frequency));
statePeriod_ = std::chrono::milliseconds(static_cast<int>(1000.0 / config.state_publish_frequency));
: networkInterface_(networkInterface), running_(false), initialized_(false) {
}
Controller::~Controller() {
shutdown();
if (running_) {
stop();
}
LOG_INFO("Shutting down robot controller");
sportClient_.reset();
obstacleClient_.reset();
initialized_ = false;
}
bool Controller::initialize() {
try {
LOG_INFO("Initializing robot controller with interface: " + networkInterface_);
// Initialize channel factory
unitree::robot::ChannelFactory::Instance()->Init(0, networkInterface_);
// Initialize sport client
sportClient_ = std::make_unique<unitree::robot::go2::SportClient>();
sportClient_->SetTimeout(10.0f);
sportClient_->Init();
// Initialize obstacle avoidance client
obstacleClient_ = std::make_unique<unitree::robot::go2::ObstaclesAvoidClient>();
obstacleClient_->SetTimeout(3.0f);
obstacleClient_->Init();
// Initialize video client
videoClient_ = std::make_unique<unitree::robot::go2::VideoClient>();
videoClient_->Init();
// Initialize VUI client
vuiClient_ = std::make_unique<unitree::robot::go2::VuiClient>();
vuiClient_->Init();
// Initialize state subscribers
sportStateSubscriber_.reset(
new unitree::robot::ChannelSubscriber<unitree_go::msg::dds_::SportModeState_>("rt/sportmodestate")
);
sportStateSubscriber_->InitChannel(
std::bind(&Controller::updateSportModeState, this, std::placeholders::_1), 1
);
lowStateSubscriber_.reset(
new unitree::robot::ChannelSubscriber<unitree_go::msg::dds_::LowState_>("rt/lowstate")
);
lowStateSubscriber_->InitChannel(
std::bind(&Controller::updateLowLevelState, this, std::placeholders::_1), 1
);
// Initialize robot state
{
std::lock_guard<std::mutex> lock(stateMutex_);
currentState_ = RobotState{};
currentState_.timestamp = std::chrono::steady_clock::now();
}
initialized_ = true;
LOG_INFO("Robot controller initialized successfully");
@@ -75,26 +46,6 @@ bool Controller::initialize() {
}
}
void Controller::shutdown() {
if (running_) {
stop();
}
LOG_INFO("Shutting down robot controller");
// Reset all clients
sportClient_.reset();
obstacleClient_.reset();
videoClient_.reset();
vuiClient_.reset();
// Reset subscribers
sportStateSubscriber_.reset();
lowStateSubscriber_.reset();
initialized_ = false;
}
bool Controller::start() {
if (!initialized_) {
LOG_ERROR("Cannot start: robot controller not initialized");
@@ -109,12 +60,6 @@ bool Controller::start() {
LOG_INFO("Starting robot controller");
running_ = true;
emergencyStopActive_ = false;
lastCommandTime_ = std::chrono::steady_clock::now();
// Start control threads
controlThread_ = std::thread(&Controller::controlLoop, this);
stateThread_ = std::thread(&Controller::stateUpdateLoop, this);
LOG_INFO("Robot controller started successfully");
return true;
@@ -127,100 +72,16 @@ bool Controller::stop() {
LOG_INFO("Stopping robot controller");
// Stop all motion first
emergencyStop();
running_ = false;
commandCondition_.notify_all();
// Wait for threads to finish
if (controlThread_.joinable()) {
controlThread_.join();
}
if (stateThread_.joinable()) {
stateThread_.join();
}
LOG_INFO("Robot controller stopped");
return true;
}
RobotState Controller::getCurrentState() const {
std::lock_guard<std::mutex> lock(stateMutex_);
return currentState_;
}
void Controller::setStateCallback(StateCallback callback) {
stateCallback_ = callback;
}
void Controller::setErrorCallback(ErrorCallback callback) {
errorCallback_ = callback;
}
bool Controller::executeCommand(const MotionCommand& command) {
if (!running_) {
LOG_ERROR("Cannot execute command: robot controller not running");
return false;
}
if (emergencyStopActive_) {
LOG_WARN("Cannot execute command: emergency stop active");
return false;
}
if (!validateCommand(command)) {
LOG_ERROR("Invalid motion command");
return false;
}
{
std::lock_guard<std::mutex> lock(commandMutex_);
commandQueue_.push(command);
lastCommandTime_ = std::chrono::steady_clock::now();
}
commandCondition_.notify_one();
return true;
}
bool Controller::emergencyStop() {
LOG_WARN("Emergency stop activated");
emergencyStopActive_ = true;
try {
if (sportClient_) {
sportClient_->StopMove();
sportClient_->Damp();
}
return true;
} catch (const std::exception& e) {
LOG_ERROR("Emergency stop failed: " + std::string(e.what()));
return false;
}
}
bool Controller::recoveryStand() {
LOG_INFO("Executing recovery stand");
try {
if (sportClient_) {
int32_t result = sportClient_->RecoveryStand();
if (result == 0) {
emergencyStopActive_ = false;
return true;
}
}
return false;
} catch (const std::exception& e) {
LOG_ERROR("Recovery stand failed: " + std::string(e.what()));
return false;
}
}
// Basic motions
bool Controller::standUp() {
bool Controller::StandUp() {
LOG_INFO("Standing up");
try {
return sportClient_ && sportClient_->StandUp() == 0;
@@ -230,7 +91,7 @@ bool Controller::standUp() {
}
}
bool Controller::standDown() {
bool Controller::StandDown() {
LOG_INFO("Standing down");
try {
return sportClient_ && sportClient_->StandDown() == 0;
@@ -240,7 +101,7 @@ bool Controller::standDown() {
}
}
bool Controller::sit() {
bool Controller::Sit() {
LOG_INFO("Sitting");
try {
return sportClient_ && sportClient_->Sit() == 0;
@@ -250,7 +111,7 @@ bool Controller::sit() {
}
}
bool Controller::lie() {
bool Controller::Lie() {
LOG_INFO("Lying down");
try {
return sportClient_ && sportClient_->StandDown() == 0;
@@ -260,7 +121,7 @@ bool Controller::lie() {
}
}
bool Controller::damp() {
bool Controller::Damp() {
LOG_INFO("Damping");
try {
return sportClient_ && sportClient_->Damp() == 0;
@@ -270,57 +131,31 @@ bool Controller::damp() {
}
}
// Movement
bool Controller::move(double vx, double vy, double vyaw) {
try {
auto& config = Config::getInstance().getConfig();
// Apply safety limits
vx = std::max(-config.max_linear_velocity, std::min(config.max_linear_velocity, vx));
vy = std::max(-config.max_linear_velocity, std::min(config.max_linear_velocity, vy));
vyaw = std::max(-config.max_angular_velocity, std::min(config.max_angular_velocity, vyaw));
return sportClient_ && sportClient_->Move(vx, vy, vyaw) == 0;
} catch (const std::exception& e) {
LOG_ERROR("Move failed: " + std::string(e.what()));
return false;
}
}
bool Controller::moveToPosition(double x, double y, double yaw) {
try {
return obstacleClient_ && obstacleClient_->MoveToAbsolutePosition(x, y, yaw) == 0;
} catch (const std::exception& e) {
LOG_ERROR("Move to position failed: " + std::string(e.what()));
return false;
}
}
bool Controller::stop() {
try {
return sportClient_ && sportClient_->StopMove() == 0;
} catch (const std::exception& e) {
LOG_ERROR("Stop failed: " + std::string(e.what()));
return false;
}
}
// Body control
bool Controller::setBodyPose(double roll, double pitch, double yaw, double height) {
bool Controller::RecoveryStand() {
LOG_INFO("Executing recovery stand");
try {
if (sportClient_) {
sportClient_->Euler(roll, pitch, yaw);
sportClient_->BodyHeight(height);
return true;
int32_t result = sportClient_->RecoveryStand();
return result == 0;
}
return false;
} catch (const std::exception& e) {
LOG_ERROR("Set body pose failed: " + std::string(e.what()));
LOG_ERROR("Recovery stand failed: " + std::string(e.what()));
return false;
}
}
bool Controller::balanceStand() {
bool Controller::StopMove() {
try {
return sportClient_ && sportClient_->StopMove() == 0;
} catch (const std::exception& e) {
LOG_ERROR("Stop move failed: " + std::string(e.what()));
return false;
}
}
bool Controller::BalanceStand() {
try {
return sportClient_ && sportClient_->BalanceStand() == 0;
} catch (const std::exception& e) {
@@ -329,8 +164,7 @@ bool Controller::balanceStand() {
}
}
// Special actions
bool Controller::dance1() {
bool Controller::Dance1() {
try {
return sportClient_ && sportClient_->Dance1() == 0;
} catch (const std::exception& e) {
@@ -339,7 +173,7 @@ bool Controller::dance1() {
}
}
bool Controller::dance2() {
bool Controller::Dance2() {
try {
return sportClient_ && sportClient_->Dance2() == 0;
} catch (const std::exception& e) {
@@ -348,7 +182,7 @@ bool Controller::dance2() {
}
}
bool Controller::hello() {
bool Controller::Hello() {
try {
return sportClient_ && sportClient_->Hello() == 0;
} catch (const std::exception& e) {
@@ -357,219 +191,52 @@ bool Controller::hello() {
}
}
// Control loops
void Controller::controlLoop() {
LOG_INFO("Control loop started");
while (running_) {
auto startTime = std::chrono::steady_clock::now();
try {
processCommands();
checkSafetyLimits();
} catch (const std::exception& e) {
LOG_ERROR("Control loop error: " + std::string(e.what()));
handleError(e.what());
}
auto endTime = std::chrono::steady_clock::now();
auto elapsed = endTime - startTime;
if (elapsed < controlPeriod_) {
std::this_thread::sleep_for(controlPeriod_ - elapsed);
}
}
LOG_INFO("Control loop stopped");
}
void Controller::stateUpdateLoop() {
LOG_INFO("State update loop started");
while (running_) {
auto startTime = std::chrono::steady_clock::now();
try {
// State is updated via callbacks, just trigger callback here
if (stateCallback_) {
RobotState state = getCurrentState();
stateCallback_(state);
}
} catch (const std::exception& e) {
LOG_ERROR("State update error: " + std::string(e.what()));
}
auto endTime = std::chrono::steady_clock::now();
auto elapsed = endTime - startTime;
if (elapsed < statePeriod_) {
std::this_thread::sleep_for(statePeriod_ - elapsed);
}
}
LOG_INFO("State update loop stopped");
}
void Controller::processCommands() {
std::unique_lock<std::mutex> lock(commandMutex_);
while (!commandQueue_.empty()) {
auto command = commandQueue_.front();
commandQueue_.pop();
lock.unlock();
executeMotionCommand(command);
lock.lock();
bool Controller::SwitchSet(bool enable) {
try {
return obstacleClient_ && obstacleClient_->SwitchSet(enable) == 0;
} catch (const std::exception& e) {
LOG_ERROR("Switch set failed: " + std::string(e.what()));
return false;
}
}
void Controller::executeMotionCommand(const MotionCommand& command) {
switch (command.type) {
case MotionCommand::VELOCITY:
move(command.linear_velocity[0], command.linear_velocity[1], command.angular_velocity[2]);
break;
case MotionCommand::POSITION:
moveToPosition(command.target_position[0], command.target_position[1], command.target_orientation[2]);
break;
case MotionCommand::BODY_POSE:
setBodyPose(command.body_roll, command.body_pitch, command.body_yaw, command.body_height);
break;
case MotionCommand::SPECIAL_ACTION:
performAction(command.action_name, command.action_params);
break;
default:
LOG_WARN("Unknown motion command type");
break;
bool Controller::SwitchGet(bool& enable) {
try {
return obstacleClient_ && obstacleClient_->SwitchGet(enable) == 0;
} catch (const std::exception& e) {
LOG_ERROR("Switch get failed: " + std::string(e.what()));
return false;
}
}
bool Controller::validateCommand(const MotionCommand& command) {
auto& config = Config::getInstance().getConfig();
// Check velocity limits
if (command.type == MotionCommand::VELOCITY) {
double vx = std::abs(command.linear_velocity[0]);
double vy = std::abs(command.linear_velocity[1]);
double vyaw = std::abs(command.angular_velocity[2]);
if (vx > config.max_linear_velocity || vy > config.max_linear_velocity ||
vyaw > config.max_angular_velocity) {
return false;
}
}
return true;
}
void Controller::updateSportModeState(const void* message) {
auto* state = static_cast<const unitree_go::msg::dds_::SportModeState_*>(message);
std::lock_guard<std::mutex> lock(stateMutex_);
// Update position
currentState_.position[0] = state->position()[0];
currentState_.position[1] = state->position()[1];
currentState_.position[2] = state->position()[2];
// Update orientation
currentState_.orientation[0] = state->imu_state().rpy()[0];
currentState_.orientation[1] = state->imu_state().rpy()[1];
currentState_.orientation[2] = state->imu_state().rpy()[2];
// Update velocity
currentState_.velocity[0] = state->velocity()[0];
currentState_.velocity[1] = state->velocity()[1];
currentState_.velocity[2] = state->velocity()[2];
// Update IMU
for (int i = 0; i < 3; i++) {
currentState_.imu_acc[i] = state->imu_state().accelerometer()[i];
currentState_.imu_gyro[i] = state->imu_state().gyroscope()[i];
}
for (int i = 0; i < 4; i++) {
currentState_.imu_quat[i] = state->imu_state().quaternion()[i];
}
currentState_.timestamp = std::chrono::steady_clock::now();
currentState_.is_connected = true;
}
void Controller::updateLowLevelState(const void* message) {
auto* state = static_cast<const unitree_go::msg::dds_::LowState_*>(message);
std::lock_guard<std::mutex> lock(stateMutex_);
// Update motor states
for (int i = 0; i < 20 && i < state->motor_state().size(); i++) {
currentState_.motor_positions[i] = state->motor_state()[i].q();
currentState_.motor_velocities[i] = state->motor_state()[i].dq();
currentState_.motor_torques[i] = state->motor_state()[i].tau_est();
currentState_.motor_temperatures[i] = state->motor_state()[i].temperature();
}
// Update battery info
if (!state->bms_state().empty()) {
currentState_.battery_voltage = state->bms_state()[0].voltage();
currentState_.battery_current = state->bms_state()[0].current();
currentState_.battery_percentage = state->bms_state()[0].soc();
}
currentState_.timestamp = std::chrono::steady_clock::now();
}
void Controller::checkSafetyLimits() {
auto& config = Config::getInstance().getConfig();
auto now = std::chrono::steady_clock::now();
// Check command timeout
auto timeSinceLastCommand = std::chrono::duration_cast<std::chrono::seconds>(
now - lastCommandTime_).count();
if (timeSinceLastCommand > config.emergency_stop_timeout && !emergencyStopActive_) {
LOG_WARN("Command timeout exceeded, activating emergency stop");
emergencyStop();
}
// Check battery level
{
std::lock_guard<std::mutex> lock(stateMutex_);
if (currentState_.battery_percentage < 10) {
currentState_.low_battery = true;
if (!emergencyStopActive_) {
LOG_WARN("Low battery detected, activating emergency stop");
emergencyStop();
}
}
bool Controller::UseRemoteCommandFromApi(bool isRemoteCommandsFromApi) {
try {
return obstacleClient_ && obstacleClient_->UseRemoteCommandFromApi(isRemoteCommandsFromApi) == 0;
} catch (const std::exception& e) {
LOG_ERROR("Use remote command from api failed: " + std::string(e.what()));
return false;
}
}
void Controller::handleError(const std::string& error) {
if (errorCallback_) {
errorCallback_(error);
bool Controller::MoveToAbsolutePosition(float x, float y, float yaw) {
try {
return obstacleClient_ && obstacleClient_->MoveToAbsolutePosition(x, y, yaw) == 0;
} catch (const std::exception& e) {
LOG_ERROR("Move to absolute position failed: " + std::string(e.what()));
return false;
}
}
bool Controller::performAction(const std::string& actionName, const nlohmann::json& params) {
LOG_INFO("Performing action: " + actionName);
if (actionName == "dance1") return dance1();
if (actionName == "dance2") return dance2();
if (actionName == "hello") return hello();
if (actionName == "sit") return sit();
if (actionName == "stand_up") return standUp();
if (actionName == "stand_down") return standDown();
if (actionName == "damp") return damp();
if (actionName == "balance_stand") return balanceStand();
if (actionName == "recovery_stand") return recoveryStand();
LOG_WARN("Unknown action: " + actionName);
return false;
bool Controller::MoveToIncrementPosition(float x, float y, float yaw) {
try {
return obstacleClient_ && obstacleClient_->MoveToIncrementPosition(x, y, yaw) == 0;
} catch (const std::exception& e) {
LOG_ERROR("Move to increment position failed: " + std::string(e.what()));
return false;
}
}
} // namespace custom
} // namespace custom

692
src/custom_robot.cpp
View File

@@ -1,71 +1,48 @@
#include "custom_robot.hpp"
#include <thread>
#include <chrono>
#include <sstream>
#include <functional>
namespace custom {
CustomRobot::CustomRobot(const std::string& configFile)
: configFile_(configFile.empty() ? "config/robot_config.json" : configFile),
running_(false), initialized_(false), mqttConnected_(false) {
stats_.startTime = std::chrono::steady_clock::now();
}
CustomRobot::CustomRobot()
: initialized_(false), running_(false) {
}
CustomRobot::~CustomRobot() {
shutdown();
LOG_INFO("Shutting down CustomRobot");
if (controller_) {
controller_->shutdown();
controller_.reset();
}
if (stateClient_) {
stateClient_.reset();
}
initialized_ = false;
}
bool CustomRobot::initialize() {
LOG_INFO("Initializing CustomRobot");
// Load configuration
if (!Config::getInstance().loadConfig(configFile_)) {
LOG_WARN("Failed to load config file, using defaults");
}
auto& config = Config::getInstance().getConfig();
// Initialize robot controller
robotController_ = std::make_unique<Controller>(config.network_interface);
if (!robotController_->initialize()) {
controller_ = std::make_unique<Controller>(config.network_interface);
if (!controller_->initialize()) {
LOG_ERROR("Failed to initialize robot controller");
return false;
}
// Set robot controller callbacks
robotController_->setStateCallback(
std::bind(&CustomRobot::handleRobotState, this, std::placeholders::_1)
);
robotController_->setErrorCallback(
std::bind(&CustomRobot::handleRobotError, this, std::placeholders::_1)
);
// Initialize MQTT client
mqttClient_ = std::make_unique<MqttClient>(
config.mqtt_broker, config.mqtt_port, config.mqtt_client_id
);
// Set MQTT callbacks
mqttClient_->setMessageCallback(
std::bind(&CustomRobot::handleMqttMessage, this, std::placeholders::_1, std::placeholders::_2)
);
mqttClient_->setConnectionCallback(
std::bind(&CustomRobot::handleMqttConnection, this, std::placeholders::_1)
);
// Connect to MQTT broker
if (!mqttClient_->connect(config.mqtt_username, config.mqtt_password)) {
LOG_ERROR("Failed to connect to MQTT broker");
return false;
}
// Start MQTT message processor
mqttClient_->startMessageProcessor();
// Subscribe to command topics
std::string cmdTopic = Config::getInstance().getFullTopic(config.cmd_topic);
if (!mqttClient_->subscribe(cmdTopic + "/+")) { // Subscribe to all command subtopics
LOG_ERROR("Failed to subscribe to command topics");
// Initialize robot state client
try {
stateClient_ = std::make_unique<unitree::robot::go2::RobotStateClient>();
stateClient_->SetTimeout(10.0f);
stateClient_->Init();
LOG_INFO("RobotStateClient initialized successfully");
} catch (const std::exception& e) {
LOG_ERROR("Failed to initialize RobotStateClient: " + std::string(e.what()));
return false;
}
@@ -74,24 +51,6 @@ bool CustomRobot::initialize() {
return true;
}
void CustomRobot::shutdown() {
LOG_INFO("Shutting down CustomRobot");
stop();
if (mqttClient_) {
mqttClient_->stopMessageProcessor();
mqttClient_->disconnect();
mqttClient_.reset();
}
if (robotController_) {
robotController_->shutdown();
robotController_.reset();
}
initialized_ = false;
}
bool CustomRobot::start() {
if (!initialized_) {
@@ -104,579 +63,74 @@ bool CustomRobot::start() {
return true;
}
LOG_INFO("Starting CustomRobot");
// Start robot controller
if (!robotController_->start()) {
if (!controller_->start()) {
LOG_ERROR("Failed to start robot controller");
return false;
}
running_ = true;
// Start periodic tasks
startPeriodicTasks();
// Publish initial status
publishHeartbeat();
LOG_INFO("CustomRobot started successfully");
return true;
}
bool CustomRobot::stop() {
if (!running_) {
bool CustomRobot::GetServiceList(std::vector<unitree::robot::go2::ServiceState>& serviceList) {
if (!initialized_ || !stateClient_) {
LOG_ERROR("Robot not initialized or stateClient is null");
return false;
}
try {
int32_t ret = stateClient_->ServiceList(serviceList);
if (ret != 0) {
LOG_ERROR("Failed to get service list, error code: " + std::to_string(ret));
return false;
}
LOG_INFO("Successfully retrieved service list with " + std::to_string(serviceList.size()) + " services");
return true;
}
LOG_INFO("Stopping CustomRobot");
running_ = false;
// Stop periodic tasks
stopPeriodicTasks();
// Stop robot controller
if (robotController_) {
robotController_->stop();
}
// Publish final status
if (mqttClient_ && mqttClient_->isConnected()) {
nlohmann::json status;
status["status"] = "stopped";
status["timestamp"] = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::system_clock::now().time_since_epoch()).count();
auto& config = Config::getInstance().getConfig();
std::string statusTopic = Config::getInstance().getFullTopic(config.state_topic + "/status");
mqttClient_->publishJson(statusTopic, status);
}
LOG_INFO("CustomRobot stopped");
return true;
}
void CustomRobot::run() {
if (!start()) {
LOG_ERROR("Failed to start CustomRobot");
return;
}
LOG_INFO("CustomRobot main loop started");
while (running_) {
try {
// Process any pending errors
processErrorQueue();
// Sleep for a short period
std::this_thread::sleep_for(std::chrono::milliseconds(100));
} catch (const std::exception& e) {
LOG_ERROR("Main loop error: " + std::string(e.what()));
}
}
LOG_INFO("CustomRobot main loop ended");
}
void CustomRobot::runAsync() {
mainThread_ = std::thread(&CustomRobot::run, this);
}
void CustomRobot::waitForShutdown() {
if (mainThread_.joinable()) {
mainThread_.join();
}
}
void CustomRobot::handleMqttMessage(const std::string& topic, const std::string& payload) {
try {
LOG_DEBUG("Received MQTT message on topic: " + topic);
auto& config = Config::getInstance().getConfig();
std::string cmdPrefix = Config::getInstance().getFullTopic(config.cmd_topic);
if (topic.find(cmdPrefix) == 0) {
// Parse JSON payload
nlohmann::json message = nlohmann::json::parse(payload);
// Extract command type from topic
std::string cmdType = topic.substr(cmdPrefix.length() + 1);
if (cmdType == "motion") {
handleCommandMessage(message);
} else if (cmdType == "config") {
handleConfigMessage(message);
} else if (cmdType == "control") {
handleControlMessage(message);
} else {
LOG_WARN("Unknown command type: " + cmdType);
}
stats_.commandsReceived++;
}
} catch (const nlohmann::json::parse_error& e) {
LOG_ERROR("JSON parse error: " + std::string(e.what()));
publishError("Invalid JSON in message: " + std::string(e.what()));
} catch (const std::exception& e) {
LOG_ERROR("Message handling error: " + std::string(e.what()));
publishError("Message handling error: " + std::string(e.what()));
LOG_ERROR("Exception in getServiceList: " + std::string(e.what()));
return false;
}
}
void CustomRobot::handleCommandMessage(const nlohmann::json& command) {
std::string requestId = command.value("request_id", "");
try {
if (!command.contains("type")) {
publishResponse(requestId, false, "Missing command type");
return;
}
std::string cmdType = command["type"];
bool success = false;
if (cmdType == "motion") {
success = processMotionCommand(command);
} else if (cmdType == "special_action") {
success = processSpecialAction(command);
} else if (cmdType == "system") {
success = processSystemCommand(command);
} else {
publishResponse(requestId, false, "Unknown command type: " + cmdType);
return;
}
if (success) {
stats_.commandsExecuted++;
publishResponse(requestId, true);
} else {
stats_.commandsFailed++;
publishResponse(requestId, false, "Command execution failed");
}
lastCommandReceived_ = std::chrono::steady_clock::now();
} catch (const std::exception& e) {
LOG_ERROR("Command processing error: " + std::string(e.what()));
publishResponse(requestId, false, "Command processing error: " + std::string(e.what()));
stats_.commandsFailed++;
}
}
bool CustomRobot::processMotionCommand(const nlohmann::json& cmd) {
if (!robotController_) {
bool CustomRobot::SwitchService(const std::string& serviceName, bool enable) {
if (!initialized_ || !stateClient_) {
LOG_ERROR("Robot not initialized or stateClient is null");
return false;
}
MotionCommand motionCmd = jsonToMotionCommand(cmd);
return robotController_->executeCommand(motionCmd);
try {
int32_t status;
int32_t ret = stateClient_->ServiceSwitch(serviceName, enable ? 1 : 0, status);
if (ret != 0) {
LOG_ERROR("Failed to switch service " + serviceName + ", error code: " + std::to_string(ret));
return false;
}
LOG_INFO("Successfully switched service " + serviceName + " to " + (enable ? "enabled" : "disabled"));
return true;
} catch (const std::exception& e) {
LOG_ERROR("Exception in switchService: " + std::string(e.what()));
return false;
}
}
bool CustomRobot::processSpecialAction(const nlohmann::json& cmd) {
if (!robotController_ || !cmd.contains("action")) {
bool CustomRobot::SetReportFreq(int32_t interval, int32_t duration) {
if (!initialized_ || !stateClient_) {
LOG_ERROR("Robot not initialized or stateClient is null");
return false;
}
std::string action = cmd["action"];
nlohmann::json params = cmd.value("params", nlohmann::json::object());
return robotController_->performAction(action, params);
}
bool CustomRobot::processSystemCommand(const nlohmann::json& cmd) {
if (!cmd.contains("command")) {
try {
int32_t ret = stateClient_->SetReportFreq(interval, duration);
if (ret != 0) {
LOG_ERROR("Failed to set report frequency, error code: " + std::to_string(ret));
return false;
}
LOG_INFO("Successfully set report frequency: interval=" + std::to_string(interval) + ", duration=" + std::to_string(duration));
return true;
} catch (const std::exception& e) {
LOG_ERROR("Exception in setReportFreq: " + std::string(e.what()));
return false;
}
std::string sysCmd = cmd["command"];
if (sysCmd == "emergency_stop") {
return robotController_ && robotController_->emergencyStop();
} else if (sysCmd == "recovery_stand") {
return robotController_ && robotController_->recoveryStand();
} else if (sysCmd == "start") {
return robotController_ && robotController_->start();
} else if (sysCmd == "stop") {
return robotController_ && robotController_->stop();
}
return false;
}
void CustomRobot::handleConfigMessage(const nlohmann::json& config) {
// Handle configuration updates
LOG_INFO("Received configuration update");
try {
// Update configuration and save
auto& currentConfig = Config::getInstance().getConfig();
if (config.contains("control_frequency")) {
currentConfig.control_frequency = config["control_frequency"];
}
if (config.contains("state_publish_frequency")) {
currentConfig.state_publish_frequency = config["state_publish_frequency"];
}
// Save updated configuration
Config::getInstance().saveConfig(configFile_);
LOG_INFO("Configuration updated successfully");
} catch (const std::exception& e) {
LOG_ERROR("Configuration update error: " + std::string(e.what()));
}
}
void CustomRobot::handleControlMessage(const nlohmann::json& control) {
// Handle control messages (start/stop/pause etc.)
if (control.contains("command")) {
std::string cmd = control["command"];
if (cmd == "start" && !running_) {
start();
} else if (cmd == "stop" && running_) {
stop();
} else if (cmd == "restart") {
stop();
std::this_thread::sleep_for(std::chrono::seconds(1));
start();
}
}
}
void CustomRobot::handleRobotState(const RobotState& state) {
// Cache the latest state
{
std::lock_guard<std::mutex> lock(stateCacheMutex_);
lastKnownState_ = state;
}
// Publish state if enough time has passed
auto now = std::chrono::steady_clock::now();
auto timeSinceLastPublish = std::chrono::duration<double>(now - lastStatePublish_).count();
if (timeSinceLastPublish >= statePublishInterval_) {
publishState();
lastStatePublish_ = now;
}
}
void CustomRobot::handleRobotError(const std::string& error) {
{
std::lock_guard<std::mutex> lock(errorMutex_);
errorQueue_.push(error);
}
stats_.errorsOccurred++;
}
void CustomRobot::publishState() {
if (!mqttClient_ || !mqttClient_->isConnected()) {
return;
}
try {
RobotState state;
{
std::lock_guard<std::mutex> lock(stateCacheMutex_);
state = lastKnownState_;
}
nlohmann::json stateJson = robotStateToJson(state);
auto& config = Config::getInstance().getConfig();
std::string stateTopic = Config::getInstance().getFullTopic(config.state_topic + "/robot");
mqttClient_->publishJson(stateTopic, stateJson);
stats_.statesPublished++;
} catch (const std::exception& e) {
LOG_ERROR("State publishing error: " + std::string(e.what()));
}
}
void CustomRobot::publishHeartbeat() {
if (!mqttClient_ || !mqttClient_->isConnected()) {
return;
}
try {
nlohmann::json heartbeat = createStatusMessage();
auto& config = Config::getInstance().getConfig();
std::string heartbeatTopic = Config::getInstance().getFullTopic(config.state_topic + "/heartbeat");
mqttClient_->publishJson(heartbeatTopic, heartbeat);
lastHeartbeat_ = std::chrono::steady_clock::now();
} catch (const std::exception& e) {
LOG_ERROR("Heartbeat publishing error: " + std::string(e.what()));
}
}
void CustomRobot::publishError(const std::string& error) {
if (!mqttClient_ || !mqttClient_->isConnected()) {
return;
}
try {
nlohmann::json errorMsg;
errorMsg["error"] = error;
errorMsg["timestamp"] = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::system_clock::now().time_since_epoch()).count();
auto& config = Config::getInstance().getConfig();
std::string errorTopic = Config::getInstance().getFullTopic(config.state_topic + "/error");
mqttClient_->publishJson(errorTopic, errorMsg);
} catch (const std::exception& e) {
LOG_ERROR("Error publishing error: " + std::string(e.what()));
}
}
void CustomRobot::publishResponse(const std::string& requestId, bool success, const std::string& message) {
if (!mqttClient_ || !mqttClient_->isConnected() || requestId.empty()) {
return;
}
try {
nlohmann::json response;
response["request_id"] = requestId;
response["success"] = success;
response["message"] = message;
response["timestamp"] = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::system_clock::now().time_since_epoch()).count();
auto& config = Config::getInstance().getConfig();
std::string responseTopic = Config::getInstance().getFullTopic(config.state_topic + "/response");
mqttClient_->publishJson(responseTopic, response);
} catch (const std::exception& e) {
LOG_ERROR("Response publishing error: " + std::string(e.what()));
}
}
MotionCommand CustomRobot::jsonToMotionCommand(const nlohmann::json& json) {
MotionCommand cmd;
cmd.timestamp = std::chrono::steady_clock::now();
if (json.contains("motion_type")) {
std::string motionType = json["motion_type"];
if (motionType == "velocity") {
cmd.type = MotionCommand::VELOCITY;
} else if (motionType == "position") {
cmd.type = MotionCommand::POSITION;
} else if (motionType == "body_pose") {
cmd.type = MotionCommand::BODY_POSE;
} else if (motionType == "special_action") {
cmd.type = MotionCommand::SPECIAL_ACTION;
}
}
// Parse velocity command
if (json.contains("linear_velocity")) {
auto vel = json["linear_velocity"];
if (vel.is_array() && vel.size() >= 3) {
cmd.linear_velocity[0] = vel[0];
cmd.linear_velocity[1] = vel[1];
cmd.linear_velocity[2] = vel[2];
}
}
if (json.contains("angular_velocity")) {
auto vel = json["angular_velocity"];
if (vel.is_array() && vel.size() >= 3) {
cmd.angular_velocity[0] = vel[0];
cmd.angular_velocity[1] = vel[1];
cmd.angular_velocity[2] = vel[2];
}
}
// Parse position command
if (json.contains("target_position")) {
auto pos = json["target_position"];
if (pos.is_array() && pos.size() >= 3) {
cmd.target_position[0] = pos[0];
cmd.target_position[1] = pos[1];
cmd.target_position[2] = pos[2];
}
}
// Parse body pose
if (json.contains("body_height")) cmd.body_height = json["body_height"];
if (json.contains("body_roll")) cmd.body_roll = json["body_roll"];
if (json.contains("body_pitch")) cmd.body_pitch = json["body_pitch"];
if (json.contains("body_yaw")) cmd.body_yaw = json["body_yaw"];
// Parse special action
if (json.contains("action_name")) {
cmd.action_name = json["action_name"];
}
if (json.contains("action_params")) {
cmd.action_params = json["action_params"];
}
// Parse duration
if (json.contains("duration")) {
cmd.duration = json["duration"];
}
return cmd;
}
nlohmann::json CustomRobot::robotStateToJson(const RobotState& state) {
nlohmann::json json;
// Basic info
json["timestamp"] = std::chrono::duration_cast<std::chrono::milliseconds>(
state.timestamp.time_since_epoch()).count();
json["is_connected"] = state.is_connected;
json["emergency_stop"] = state.emergency_stop;
// Position and orientation
json["position"] = {state.position[0], state.position[1], state.position[2]};
json["orientation"] = {state.orientation[0], state.orientation[1], state.orientation[2]};
json["velocity"] = {state.velocity[0], state.velocity[1], state.velocity[2]};
// IMU data
json["imu"]["acceleration"] = {state.imu_acc[0], state.imu_acc[1], state.imu_acc[2]};
json["imu"]["gyroscope"] = {state.imu_gyro[0], state.imu_gyro[1], state.imu_gyro[2]};
json["imu"]["quaternion"] = {state.imu_quat[0], state.imu_quat[1], state.imu_quat[2], state.imu_quat[3]};
// Battery info
json["battery"]["voltage"] = state.battery_voltage;
json["battery"]["current"] = state.battery_current;
json["battery"]["percentage"] = state.battery_percentage;
json["battery"]["low_battery"] = state.low_battery;
// System info
json["temperature"] = state.temperature;
json["overheated"] = state.overheated;
// Motor states (first 12 leg motors)
json["motors"]["positions"] = std::vector<double>(state.motor_positions, state.motor_positions + 12);
json["motors"]["velocities"] = std::vector<double>(state.motor_velocities, state.motor_velocities + 12);
json["motors"]["torques"] = std::vector<double>(state.motor_torques, state.motor_torques + 12);
json["motors"]["temperatures"] = std::vector<double>(state.motor_temperatures, state.motor_temperatures + 12);
return json;
}
nlohmann::json CustomRobot::createStatusMessage() {
nlohmann::json status;
status["status"] = running_ ? "running" : "stopped";
status["initialized"] = initialized_;
status["mqtt_connected"] = mqttConnected_;
status["robot_connected"] = robotController_ && robotController_->isRunning();
// Statistics
status["stats"]["commands_received"] = stats_.commandsReceived;
status["stats"]["commands_executed"] = stats_.commandsExecuted;
status["stats"]["commands_failed"] = stats_.commandsFailed;
status["stats"]["states_published"] = stats_.statesPublished;
status["stats"]["errors_occurred"] = stats_.errorsOccurred;
// Uptime
auto now = std::chrono::steady_clock::now();
auto uptime = std::chrono::duration_cast<std::chrono::seconds>(now - stats_.startTime).count();
status["uptime_seconds"] = uptime;
status["timestamp"] = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::system_clock::now().time_since_epoch()).count();
return status;
}
void CustomRobot::startPeriodicTasks() {
// Heartbeat thread
periodicThreads_.emplace_back([this]() {
while (running_) {
auto now = std::chrono::steady_clock::now();
auto timeSinceLastHeartbeat = std::chrono::duration<double>(now - lastHeartbeat_).count();
if (timeSinceLastHeartbeat >= heartbeatInterval_) {
publishHeartbeat();
}
std::this_thread::sleep_for(std::chrono::seconds(1));
}
});
// Safety monitor thread
safetyThread_ = std::thread(&CustomRobot::safetyMonitorLoop, this);
}
void CustomRobot::stopPeriodicTasks() {
// Wait for all periodic threads to finish
for (auto& thread : periodicThreads_) {
if (thread.joinable()) {
thread.join();
}
}
periodicThreads_.clear();
if (safetyThread_.joinable()) {
safetyThread_.join();
}
}
void CustomRobot::handleMqttConnection(bool connected) {
mqttConnected_ = connected;
if (connected) {
LOG_INFO("MQTT connection established");
// Resubscribe to topics
auto& config = Config::getInstance().getConfig();
std::string cmdTopic = Config::getInstance().getFullTopic(config.cmd_topic);
mqttClient_->subscribe(cmdTopic + "/+");
// Publish connection status
publishHeartbeat();
} else {
LOG_WARN("MQTT connection lost");
}
}
void CustomRobot::safetyMonitorLoop() {
while (running_) {
try {
checkEmergencyConditions();
std::this_thread::sleep_for(std::chrono::milliseconds(500));
} catch (const std::exception& e) {
LOG_ERROR("Safety monitor error: " + std::string(e.what()));
}
}
}
void CustomRobot::checkEmergencyConditions() {
// Check command timeout
auto now = std::chrono::steady_clock::now();
auto timeSinceLastCommand = std::chrono::duration<double>(now - lastCommandReceived_).count();
if (timeSinceLastCommand > maxIdleTime_ && robotController_ && robotController_->isRunning()) {
LOG_WARN("Maximum idle time exceeded, stopping robot");
robotController_->emergencyStop();
}
}
void CustomRobot::processErrorQueue() {
std::lock_guard<std::mutex> lock(errorMutex_);
while (!errorQueue_.empty()) {
std::string error = errorQueue_.front();
errorQueue_.pop();
publishError(error);
}
}
} // namespace custom
}

12
src/main.cpp
View File

@@ -11,24 +11,18 @@ std::unique_ptr<CustomRobot> g_robot;
void signalHandler(int signal) {
LOG_INFO("Received signal " + std::to_string(signal) + ", shutting down...");
if (g_robot) {
g_robot->stop();
}
exit(0);
}
int main(int argc, char** argv) {
// Initialize logger with default settings
Logger::getInstance().setLevel(LogLevel::INFO);
LOG_INFO("Starting Unitree GO2 System v0.0.1");
// Setup signal handlers
signal(SIGINT, signalHandler);
signal(SIGTERM, signalHandler);
try {
// Create robot with compile-time defaults (no config file needed)
g_robot = std::make_unique<CustomRobot>("");
if (!g_robot->initialize()) {
@@ -37,11 +31,7 @@ int main(int argc, char** argv) {
}
LOG_INFO("Robot initialized successfully");
LOG_INFO("Press Ctrl+C to stop the robot");
// Run the robot
g_robot->run();
g_robot->start();
} catch (const std::exception& e) {
LOG_ERROR("Exception: " + std::string(e.what()));
return 1;