Add initial project structure with CMake configuration, source files, and README documentation for Unitree GO2 Custom Controller

2025-09-07 16:43:22 +08:00
parent b2b6ddf245
commit 6acff483dd
16 changed files with 3429 additions and 1 deletions
--- a/src/controller.cpp
+++ b/src/controller.cpp
@@ -0,0 +1,575 @@
+#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),
+      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));
+}
+
+Controller::~Controller() {
+    shutdown();
+}
+
+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_->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");
+        return true;
+        
+    } catch (const std::exception& e) {
+        LOG_ERROR("Failed to initialize robot controller: " + std::string(e.what()));
+        return false;
+    }
+}
+
+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");
+        return false;
+    }
+    
+    if (running_) {
+        LOG_WARN("Robot controller already running");
+        return true;
+    }
+    
+    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;
+}
+
+bool Controller::stop() {
+    if (!running_) {
+        return true;
+    }
+    
+    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() {
+    LOG_INFO("Standing up");
+    try {
+        return sportClient_ && sportClient_->StandUp() == 0;
+    } catch (const std::exception& e) {
+        LOG_ERROR("Stand up failed: " + std::string(e.what()));
+        return false;
+    }
+}
+
+bool Controller::standDown() {
+    LOG_INFO("Standing down");
+    try {
+        return sportClient_ && sportClient_->StandDown() == 0;
+    } catch (const std::exception& e) {
+        LOG_ERROR("Stand down failed: " + std::string(e.what()));
+        return false;
+    }
+}
+
+bool Controller::sit() {
+    LOG_INFO("Sitting");
+    try {
+        return sportClient_ && sportClient_->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;
+    } catch (const std::exception& e) {
+        LOG_ERROR("Damp failed: " + std::string(e.what()));
+        return false;
+    }
+}
+
+// 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) {
+    try {
+        if (sportClient_) {
+            sportClient_->Euler(roll, pitch, yaw);
+            sportClient_->BodyHeight(height);
+            return true;
+        }
+        return false;
+    } catch (const std::exception& e) {
+        LOG_ERROR("Set body pose failed: " + std::string(e.what()));
+        return false;
+    }
+}
+
+bool Controller::balanceStand() {
+    try {
+        return sportClient_ && sportClient_->BalanceStand() == 0;
+    } catch (const std::exception& e) {
+        LOG_ERROR("Balance stand failed: " + std::string(e.what()));
+        return false;
+    }
+}
+
+// Special actions
+bool Controller::dance1() {
+    try {
+        return sportClient_ && sportClient_->Dance1() == 0;
+    } catch (const std::exception& e) {
+        LOG_ERROR("Dance1 failed: " + std::string(e.what()));
+        return false;
+    }
+}
+
+bool Controller::dance2() {
+    try {
+        return sportClient_ && sportClient_->Dance2() == 0;
+    } catch (const std::exception& e) {
+        LOG_ERROR("Dance2 failed: " + std::string(e.what()));
+        return false;
+    }
+}
+
+bool Controller::hello() {
+    try {
+        return sportClient_ && sportClient_->Hello() == 0;
+    } catch (const std::exception& e) {
+        LOG_ERROR("Hello failed: " + std::string(e.what()));
+        return false;
+    }
+}
+
+// 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();
+    }
+}
+
+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::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();
+            }
+        }
+    }
+}
+
+void Controller::handleError(const std::string& error) {
+    if (errorCallback_) {
+        errorCallback_(error);
+    }
+}
+
+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;
+}
+
+} // namespace custom