Skip to content

创建客户应用

本文将指导用户如何使用 Daystar Bot SDK 创建一个属于自己的高层应用例程程序。

利用 Daystar Bot SDK可以使得用户创建各式各样的高层应用,本小节会手把手教学用户使用 SDK 创建一个 “让Daystar Bot机器人站立并往前行走一段距离” 的高层应用。

查阅 SDK 接口

lenovo-daystar-sdk/lenovo_daystar_*.h 是 Daystar Bot SDK 提供的接口头文件,通过调用其中高层接口控制 Daystar Bot 机器人执行指定动作。

该头文件位于sdk下载包 install/include/lenovo-daystar-sdk/lenovo_daystar_sdk.hinstall/include/lenovo-daystar-sdk/lenovo_daystar_sport.h,见于下文

cpp
#ifndef LENOVO_DAYSTAR_SDK_H
#define LENOVO_DAYSTAR_SDK_H

#include <memory>
#include <string>
#include <functional>

// 包含子系统的声明
#include "lenovo_daystar_sport.h"
#include "lenovo_daystar_rps.h"
#include "lenovo_daystar_ptz.h"

// #include "lenovo_daystar_router.h"

// 注意:不再包含内部实现细节头文件
// 这样我们确保用户不需要gRPC环境就能使用SDK

namespace Lenovo
{
    namespace Daystar
    {

        // 前向声明
        class RobotClientImpl;

        /**
         * @brief Daystar SDK主类,作为所有功能的统一入口点
         *
         * 该类封装了底层RobotClient,并提供对Sport、RPS和Router子系统的访问
         */
        class SDK
        {
        public:
            /**
             * @brief 构造函数,连接到指定地址的机器人服务
             *
             * @param server_address 服务器地址,格式为"地址:端口"
             */
            explicit SDK(const std::string &motion_server_address = "192.168.144.103:50051", const std::string &perception_server_address = "192.168.100.105");

            /**
             * @brief 析构函数
             */
            ~SDK();

            /**
             * @brief 获取Sport子系统接口
             *
             * @return Sport子系统引用
             */
            class Sport &getSport();

            /**
             * @brief 获取RPS子系统接口
             *
             * @return RPS子系统引用
             */
            class RPS &getRPS();

            /**
             * @brief 获取PTZ子系统接口
             *
             * @return PTZ子系统引用
             */
            class PTZ &getPTZ();

            /**
             * @brief 获取Router子系统接口
             *
             * @return Router子系统引用
             * @note 当前版本暂时不提供Router功能
             */
            // class Router &getRouter();

            /**
             * @brief 检查与服务器的连接状态
             *
             * @return 如果连接正常则返回true,否则返回false
             */
            bool isConnected() const;

            /**
             * @brief 获取服务器名称
             *
             * @return 服务器名称,如果未连接则返回空字符串
             */
            std::string getServerName() const;

            /**
             * @brief 获取服务器版本
             *
             * @return 服务器版本,如果未连接则返回空字符串
             */
            std::string getServerVersion() const;

            /**
             * @brief 通用调用接口,用于调用任意后端方法
             *
             * @param method_name 要调用的方法名
             * @param params 方法参数,键值对形式
             * @param request_id 请求ID,用于跟踪请求,如果为空则自动生成
             * @param metadata 附加元数据
             * @return 调用结果
             */
            GenericCallResponse genericCall(
                const std::string &method_name,
                const std::map<std::string, std::any> &params,
                const std::string &request_id = "",
                const std::map<std::string, std::string> &metadata = {});

        private:
            std::unique_ptr<RobotClientImpl> impl_; // 内部实现
            std::unique_ptr<PTZ> ptz_;
        };

    } // namespace Daystar
} // namespace Lenovo

#endif // LENOVO_DAYSTAR_SDK_H
cpp
#ifndef LENOVO_DAYSTAR_SPORT_H
#define LENOVO_DAYSTAR_SPORT_H

#include <memory>
#include <string>
#include <functional>
#include "lenovo-daystar-sdk/lenovo_daystar_types.h"

namespace Lenovo
{
    namespace Daystar
    {

        // 前向声明
        class RobotClientImpl;
        class SportImpl;

        /**
         * @brief Sport子系统类,封装McsClient功能
         *
         * 该类提供机器人运动控制相关功能,并隐藏gRPC实现细节
         */
        class Sport
        {
        public:
            /**
             * @brief 构造函数
             *
             * @param impl 内部实现指针
             */
            explicit Sport(RobotClientImpl *impl);

            /**
             * @brief 析构函数
             */
            ~Sport();

            /**
             * @brief 设置MCS状态回调(合并回调接口)
             *
             * 该回调函数会在MCS系统状态发生变化时被调用,包含机器人的完整状态信息。
             * 状态信息包括:
             * - 心跳计数(heartBeat):用于检测通信状态
             * - 场景类型(scene):当前运动场景模式
             * - 场景切换状态(switchState):场景切换进度
             * - 驱动器状态(driverState):驱动器使能状态
             * - 控制模式(joyMode):当前控制模式(导航/手柄)
             * - 停障开关状态(guardianSwitchActive):安全停障功能状态
             * - 停障速度衰减比率(guardianVelocityDecayRatio):各方向的速度衰减系数
             * - 运动参数(motion):当前速度和高度信息
             * - 最大速度(maxSpeed):机器人当前配置的最大移动速度(m/s),用于计算实际运动速度
             * - 错误信息(errorMessage):系统错误描述
             *
             * @param callback MCS状态回调函数,接收包含所有MCS系统状态的完整状态结构体
             * @note 回调函数在后台线程中执行,请确保线程安全
             * @see MCSStatusInfo 状态结构体的详细定义
             */
            void setStatusCallback(MCSStatusCallback callback);

            /**
             * @brief 设置MCS运动参数回调(合并回调接口)
             *
             * 该回调函数会在机器人运动参数发生变化时被调用,包含实时的运动状态信息。
             * 运动参数包括:
             * - 当前速度(speed):机器人当前的移动速度,范围0.0-1.0
             * - 当前高度(height):机器人当前的身体高度,范围0.0-1.0
             *
             * 这些参数反映了机器人的实时运动状态,可用于监控和调试。
             *
             * @param callback MCS运动参数回调函数,接收包含速度和高度的运动参数结构体
             * @note 回调函数在后台线程中执行,请确保线程安全
             * @see MCSMotionInfo 运动参数结构体的详细定义
             */
            void setMotionCallback(MCSMotionCallback callback);

            // /**
            //  * @brief 获取当前系统心跳值
            //  *
            //  * @return 当前的心跳计数值
            //  */
            // int64_t getCurrentHeartBeat() const;

            /**
             * @brief 清除驱动错误。
             *
             * 该函数用于清除指定关节的驱动错误。
             *
             * @param mask 指定关节选择掩码。
             * @return bool 返回 `true` 表示清除成功,返回 `false` 表示清除失败。
             */
            bool clearDriverError(uint32_t joint_mask);

            /**
             * @brief 重新校准所有驱动器并保存。
             * @todo 移除
             *
             * 该函数用于重新校准所有驱动器,并保存校准结果。
             *
             * @param enable 是否启用校准。
             * @return bool 返回 `true` 表示校准成功,返回 `false` 表示校准失败。
             */
            bool reCalibrateAllDriverAndSave(bool enable);

            /**
             * @brief 对机器人的关节上力矩传感器进行清零的接口。
             * @todo 移除
             *
             * 该函数用于清零机器人的关节力矩传感器。
             *
             * @param from 指定操作模式(导航或操纵杆)。
             * @return bool 返回 `true` 表示操作成功,返回 `false` 表示操作失败。
             */
            bool ZeroTorqueSensor();

            /**
             * @brief 启用或禁用驱动器
             *
             * @param enable 是否启用
             * @return 操作是否成功
             */
            bool enableDriver(bool enable);

            /**
             * @brief 控制机器人趴下或站起
             *
             * @param lieDown true表示趴下,false表示站起
             * @return 操作是否成功
             */
            bool lieDownOrStandUp(bool lieDown);

            /**
             * @brief 方向运动控制
             *
             * 控制机器人在三个自由度上的运动:前后移动、左右移动和旋转。
             *
             * **参数范围和方向说明:**
             * - 所有参数的有效范围:-1.0 ≤ (x, y, yaw) ≤ 1.0
             * - 参数符号表示运动方向:
             *   * x > 0:向前移动,x < 0:向后移动
             *   * y > 0:向左移动,y < 0:向右移动
             *   * yaw > 0:逆时针旋转,yaw < 0:顺时针旋转
             * - 参数绝对值表示速度大小:|x|, |y|, |yaw| 越大,速度越快
             *
             * **实际速度计算公式:**
             * 最终的运动速度由三个因素决定:
             * 1. **最大速度 (maxSpeed)**:机器人硬件配置的最大速度(m/s),通过setStatusCallback获取
             * 2. **速度系数 (percent)**:通过adjustSpeed函数设置的速度百分比(0-100)
             * 3. **当前输入值**:本函数传入的x, y, yaw参数
             *
             * 实际速度计算:
             * - 实际X方向速度 = x × maxSpeed × percent / 100 (m/s)
             * - 实际Y方向速度 = y × maxSpeed × percent / 100 (m/s)
             * - 实际旋转速度 = yaw × maxSpeed × percent / 100 (rad/s)
             *
             * **⚠️ 重要安全机制:持续调用要求**
             * - **必须持续调用**:机器人运动需要持续的move()命令来维持,这是一个安全保护机制
             * - **超时自动停止**:如果后端检测到超过200ms没有收到新的move()命令,所有方向的速度将立即被置为零
             * - **实时控制**:这意味着move()函数需要以高于5Hz的频率持续调用(建议10-20Hz)
             * - **安全设计**:此机制确保在通信中断或程序异常时,机器人能够自动停止运动
             *
             * **推荐调用模式:**
             * ```cpp
             * // 错误的使用方式 - 只调用一次
             * sport.move(0.5f, 0.0f, 0.0f);  // 机器人会在200ms后自动停止
             *
             * // 正确的使用方式 - 持续调用
             * while (need_to_move) {
             *     sport.move(0.5f, 0.0f, 0.0f);  // 持续发送运动命令
             *     std::this_thread::sleep_for(std::chrono::milliseconds(50));  // 20Hz频率
             * }
             *
             * // 明确停止
             * sport.move(0.0f, 0.0f, 0.0f);  // 发送停止命令
             * ```
             *
             * **使用示例:**
             * ```cpp
             * // 设置速度系数为50%
             * sport.adjustSpeed(50);
             *
             * // 向前移动3秒钟
             * auto start_time = std::chrono::steady_clock::now();
             * while (std::chrono::steady_clock::now() - start_time < std::chrono::seconds(3)) {
             *     sport.move(0.5f, 0.0f, 0.0f);  // 以50%最大速度向前
             *     std::this_thread::sleep_for(std::chrono::milliseconds(50));
             * }
             *
             * // 停止移动
             * sport.move(0.0f, 0.0f, 0.0f);
             * ```
             *
             * @param x 前后方向速度分量,范围[-1.0, 1.0],正值向前,负值向后
             * @param y 左右方向速度分量,范围[-1.0, 1.0],正值向左,负值向右
             * @param yaw 旋转速度分量,范围[-1.0, 1.0],正值逆时针,负值顺时针
             * @return 操作是否成功
             * @warning 请确保参数在有效范围内,超出范围可能导致不可预期的行为
             * @warning 必须持续调用此函数(<200ms间隔)以维持运动,否则机器人将自动停止
             * @note 建议以10-20Hz的频率调用此函数以确保平滑的运动控制
             * @see adjustSpeed() 设置速度系数
             * @see setStatusCallback() 获取最大速度信息
             */
            bool move(float x, float y, float yaw);

            /**
             * @brief 设置步态
             *
             * @param gaitType 步态类型,对应GaitRequest中的类型
             * @return 操作是否成功
             */
            // bool setGait(int gaitType);

            /**
             * @brief 调整机身高度
             *
             * @param percent 高度百分比,范围0-100
             * @return 操作是否成功
             */
            bool adjustBodyHeight(int percent);

            /**
             * @brief 调整速度系数
             *
             * 设置机器人运动的速度系数,该系数会影响所有运动命令的实际执行速度。
             *
             * **功能说明:**
             * - 速度系数是一个全局设置,影响move()函数中所有方向的运动速度
             * - 设置后会立即生效,影响后续所有的运动命令
             * - 该设置不会影响机器人的最大速度配置,只是在此基础上进行缩放
             *
             * **速度计算关系:**
             * ```
             * 实际运动速度 = move()参数 × maxSpeed × (percent/100)
             * ```
             * 其中:
             * - move()参数:通过move(x,y,yaw)传入的速度分量[-1.0, 1.0]
             * - maxSpeed:机器人配置的最大速度(m/s),通过setStatusCallback获取
             * - percent:本函数设置的速度百分比[0, 100]
             *
             * **使用示例:**
             * ```cpp
             * // 设置速度为最大速度的30%
             * sport.adjustSpeed(30);
             * sport.move(1.0f, 0.0f, 0.0f);  // 实际速度 = 1.0 × maxSpeed × 0.3
             *
             * // 设置速度为最大速度的100%
             * sport.adjustSpeed(100);
             * sport.move(0.5f, 0.0f, 0.0f);  // 实际速度 = 0.5 × maxSpeed × 1.0
             * ```
             *
             * @param percent 速度百分比,有效范围0-100
             *                - 0:停止所有运动
             *                - 50:使用50%的最大速度
             *                - 100:使用100%的最大速度
             * @return 操作是否成功
             * @note 建议在机器人运动前先设置合适的速度系数,特别是在测试阶段使用较低的速度
             * @warning 请确保百分比在0-100范围内,超出范围可能导致不可预期的行为
             * @see move() 运动控制函数
             * @see setStatusCallback() 获取最大速度信息
             */
            bool adjustSpeed(int percent);

            /**
             * @brief 设置场景
             *
             * @param sceneType 场景类型,使用SceneType枚举
             * @return 操作是否成功
             */
            bool setScene(SceneType sceneType);

            /**
             * @brief 自动恢复
             * @todo 移除
             *
             * @return 操作是否成功
             */
            bool selfRecovery();

            /**
             * @brief 驱动急停
             *
             * @return 操作是否成功
             */
            bool emergencyStop();

            /**
             * @brief 恢复急停
             *
             * @return 操作是否成功
             */
            bool resumeFromEmergencyStop();

            /**
             * @brief 设置导航或手柄控制
             *
             * @param controllerMode 控制模式,使用Lenovo::Daystar::ControllerMode枚举
             * @return 操作是否成功
             */
            bool setControlMode(Lenovo::Daystar::ControllerMode controllerMode);

            /**
             * @brief 进入或退出充电
             * @todo 移除
             *
             * @param enter true表示进入充电模式,false表示退出充电模式
             * @return 操作是否成功
             */
            bool enterOrExitCharge(bool enter);

            /**
             * @brief 设置停障开关
             *
             * @param enable true表示开启停障,false表示关闭停障
             * @return 操作是否成功
             */
            bool setGuardianSwitch(bool enable);

            /**
             * @brief 演示控制
             *
             * @param demoType 演示类型
             * @param enable 是否启用, 默认为false启用
             * @return 操作是否成功
             */
            bool demoControl(int demoType, bool enable = false);

            /**
             * @brief 清除当前错误
             *
             * @return
             */
            void ClearLatestHistory();

            /**
             * @brief 获取MC版本
             *
             * @param mcIndex MC索引
             * @return MC版本字符串,如果获取失败则返回空字符串
             */
            std::string getMcVersion(int mcIndex = 0);

            /**
             * @brief 获取机器人名称
             *
             * @param mcIndex MC索引
             * @return 机器人名称字符串,如果获取失败则返回空字符串
             */
            std::string getRobotName(int mcIndex = 0);

        private:
            std::unique_ptr<SportImpl> impl_; // 内部实现
        };

    } // namespace Daystar
} // namespace Lenovo

#endif // LENOVO_DAYSTAR_SPORT_H

根据前文描述的应用情况,经查阅,用到的函数有

  1. explicit SDK(const std::string &server_address = "192.168.144.103:50051"); 默认的SDK构造
  2. Sport &SDK::getSport();获取运动控制子系统
  3. bool Sport::enableDriver(bool enable); 上使能
  4. bool Sport::lieDownOrStandUp(bool enable); 设置机器人姿态
  5. bool Sport::setScene(Lenovo::Daystar::SceneType sceneType); 设置机器人场景
  6. bool Sport::adjustSpeed(int percent);****设置机器人移动速度系数
  7. bool Sport::move(float x, float y, float yaw);控制机器人移动
  8. void Sport::setMotionCallback(MCSMotionCallback callback)void Sport::setStatusCallback(MCSStatusCallback callback)设置状态回调

创建应用文件

接下来集成sdk创建你的应用工程。

以下叙述假设工作路径为 /home/daystar/sdk_test_project

依次输入下列命令。把sdk so库文件和 .h 头文件拷贝到应用工程对应目录下(根据使用的开发平台相应选择sdk arm或者x86 的so库文件),创建app_test.cpp文件,编写调用sdk接口和应用逻辑。

bash
cd /home/daystar/sdk_test_project
mkdir include src lib
cp /sdk/install/include/* include/
cp /sdk/install/lib/* lib/
cd src
touch app_test.cpp

接下来让我们编辑 app_test.cpp

  1. 包含头文件:
cpp
#include "lenovo-daystar-sdk/lenovo_daystar_sdk.h"
  1. 调用SDK函数实现应用逻辑
cpp
Lenovo::Daystar::SDK sdk; // 构造SDK
auto &sport = sdk.getSport(); // 获取运动客户端
sport.enableDriver(true); // 上使能
sport.lieDownOrStandUp(true); // 站立
sport.adjustSpeed(50); // 设定最大速度
for (int i = 0; i < 20; ++i) { // 以0.3的速度前进2秒
  sport.move(0.3, 0.0, 0.0);
  std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
sport.lieDownOrStandUp(false); // 趴下
sport.enableDriver(false); // 下使能

完整代码为

cpp
#include "lenovo-daystar-sdk/lenovo_daystar_sdk.h"
#include "lenovo-daystar-sdk/lenovo_daystar_types.h"
#include <iostream>
#include <chrono>
#include <thread>
#include <string>
#include <sstream>
#include <cmath>

void printSportMenu()
{
    std::cout << "\n========== Sport Control Menu ==========" << std::endl;
    std::cout << "1.  检查连接状态" << std::endl;
    std::cout << "2.  获取服务器信息" << std::endl;
    std::cout << "3.  启用驱动器" << std::endl;
    std::cout << "4.  禁用驱动器" << std::endl;
    std::cout << "5.  机器人趴下" << std::endl;
    std::cout << "6.  机器人站立" << std::endl;
    std::cout << "7.  设置场景模式" << std::endl;
    std::cout << "8.  调整速度" << std::endl;
    std::cout << "9.  机器人移动" << std::endl;
    std::cout << "10. 旋转机器人" << std::endl;
    std::cout << "11. 停止运动" << std::endl;
    std::cout << "12. 设置身体姿态" << std::endl;
    std::cout << "13. 设置步态参数" << std::endl;
    std::cout << "14. 获取运动状态" << std::endl;
    std::cout << "15. 获取系统状态" << std::endl;
    std::cout << "16. 设置状态回调" << std::endl;
    std::cout << "17. 设置运动回调" << std::endl;
    std::cout << "18. 获取心跳值" << std::endl;
    std::cout << "19. 通用调用测试" << std::endl;
    std::cout << "0.  退出程序" << std::endl;
    std::cout << "=========================================" << std::endl;
    std::cout << "请选择操作 (0-19): ";
}

float getFloatInput(const std::string &prompt)
{
    float value;
    std::cout << prompt;
    std::cin >> value;
    return value;
}

int getIntInput(const std::string &prompt)
{
    int value;
    std::cout << prompt;
    std::cin >> value;
    return value;
}

bool getBoolInput(const std::string &prompt)
{
    int value;
    std::cout << prompt << " (1=是, 0=否): ";
    std::cin >> value;
    return value != 0;
}

std::string getStringInput(const std::string &prompt)
{
    std::string value;
    std::cout << prompt;
    std::cin.ignore();
    std::getline(std::cin, value);
    return value;
}

void printMCSMotionInfo(const Lenovo::Daystar::MCSMotionInfo &info)
{
    std::cout << "运动状态信息:" << std::endl;
    std::cout << "  速度: " << info.speed << " (0.0-1.0)" << std::endl;
    std::cout << "  高度: " << info.height << " (0.0-1.0)" << std::endl;
}

void printMCSStatusInfo(const Lenovo::Daystar::MCSStatusInfo &info)
{
    std::cout << "系统状态信息:" << std::endl;
    std::cout << "  错误码: " << info.errorCode.error_code << std::endl;
    std::cout << "  场景: " << static_cast<int>(info.scene) << std::endl;
    std::cout << "  场景切换状态: " << static_cast<int>(info.switchState) << std::endl;
    std::cout << "  驱动器启用状态: " << static_cast<int>(info.driverState) << std::endl;
    std::cout << "  控制模式: " << static_cast<int>(info.controllerMode) << std::endl;
    std::cout << "  保护模式开关: " << (info.guardianSwitchActive ? "开启" : "关闭") << std::endl;
    std::cout << "  障碍物检测比例:" << std::endl;
    std::cout << "    前方: " << info.guardianVelocityDecayRatio.front_ratio << std::endl;
    std::cout << "    左侧: " << info.guardianVelocityDecayRatio.left_ratio << std::endl;
    std::cout << "    右侧: " << info.guardianVelocityDecayRatio.right_ratio << std::endl;
    std::cout << "    后方: " << info.guardianVelocityDecayRatio.rear_ratio << std::endl;
}

int main()
{
    std::cout << "Sport 交互式控制程序启动..." << std::endl;

    try
    {
        // 创建SDK实例
        Lenovo::Daystar::SDK sdk("192.168.100.103:50051");

        // 获取Sport子系统
        Lenovo::Daystar::Sport &sport = sdk.getSport();

        // 全局变量用于存储回调数据
        Lenovo::Daystar::MCSMotionInfo latest_motion;
        Lenovo::Daystar::MCSStatusInfo latest_status;

        int choice;
        bool running = true;

        while (running)
        {
            printSportMenu();
            std::cin >> choice;

            switch (choice)
            {
            case 0:
                running = false;
                std::cout << "正在退出程序..." << std::endl;
                break;

            case 1:
            {
                if (sdk.isConnected())
                {
                    std::cout << "✓ 机器人已连接" << std::endl;
                }
                else
                {
                    std::cout << "✗ 机器人未连接" << std::endl;
                }
                break;
            }

            case 2:
            {
                std::cout << "服务器名称: " << sdk.getServerName() << std::endl;
                std::cout << "服务器版本: " << sdk.getServerVersion() << std::endl;
                break;
            }

            case 3:
            {
                bool enabled = getBoolInput("启用驱动器?");
                if (sport.enableDriver(enabled))
                {
                    std::cout << "✓ 驱动器" << (enabled ? "启用" : "禁用") << "成功" << std::endl;
                }
                else
                {
                    std::cout << "✗ 驱动器操作失败" << std::endl;
                }
                break;
            }

            case 4:
            {
                if (sport.enableDriver(false))
                {
                    std::cout << "✓ 驱动器已禁用" << std::endl;
                }
                else
                {
                    std::cout << "✗ 驱动器禁用失败" << std::endl;
                }
                break;
            }

            case 5:
            {
                if (sport.lieDownOrStandUp(false))
                { // false = 趴下
                    std::cout << "✓ 机器人开始站立" << std::endl;
                }
                else
                {
                    std::cout << "✗ 机器人站立失败" << std::endl;
                }
                break;
            }

            case 6:
            {
                if (sport.lieDownOrStandUp(true))
                { // true = 站立
                    std::cout << "✓ 机器人开始趴下" << std::endl;
                }
                else
                {
                    std::cout << "✗ 机器人趴下失败" << std::endl;
                }
                break;
            }

            case 7:
            {
                std::cout << "可用场景:" << std::endl;
                std::cout << "  0 - IDEL (待机)" << std::endl;
                std::cout << "  1 - WALKING (行走)" << std::endl;
                std::cout << "  2 - TROTTING (小跑)" << std::endl;
                std::cout << "  3 - CLIMBING_STAIRS (爬楼梯)" << std::endl;
                std::cout << "  4 - SPECIAL (特殊模式)" << std::endl;

                int scene = getIntInput("选择场景模式 (0-4): ");
                if (sport.setScene(static_cast<Lenovo::Daystar::SceneType>(scene)))
                {
                    std::cout << "✓ 场景设置成功" << std::endl;
                }
                else
                {
                    std::cout << "✗ 场景设置失败" << std::endl;
                }
                break;
            }

            case 8:
            {
                int speed = getIntInput("输入速度百分比 (0-100): ");
                if (sport.adjustSpeed(speed))
                {
                    std::cout << "✓ 速度调整成功: " << speed << "%" << std::endl;
                }
                else
                {
                    std::cout << "✗ 速度调整失败" << std::endl;
                }
                break;
            }

            case 9:
            {
                float x = getFloatInput("输入X方向速度 (m/s): ");
                float y = getFloatInput("输入Y方向速度 (m/s): ");
                float z = getFloatInput("输入旋转角速度 (rad/s): ");
                if (sport.move(x, y, z))
                {
                    std::cout << "✓ 移动命令已发送: (" << x << ", " << y << ", " << z << ")" << std::endl;
                }
                else
                {
                    std::cout << "✗ 移动命令发送失败" << std::endl;
                }
                break;
            }

            case 10:
            {
                float angular_speed = getFloatInput("输入旋转速度 (rad/s): ");
                if (sport.move(0.0f, 0.0f, angular_speed))
                {
                    std::cout << "✓ 旋转命令已发送: " << angular_speed << " rad/s" << std::endl;
                }
                else
                {
                    std::cout << "✗ 旋转命令发送失败" << std::endl;
                }
                break;
            }

            case 11:
            {
                if (sport.move(0.0f, 0.0f, 0.0f))
                {
                    std::cout << "✓ 机器人已停止运动" << std::endl;
                }
                else
                {
                    std::cout << "✗ 停止命令发送失败" << std::endl;
                }
                break;
            }

            case 12:
            {
                std::cout << "⚠️  身体姿态控制功能暂不可用" << std::endl;
                std::cout << "请使用其他可用功能" << std::endl;
                break;
            }

            case 13:
            {
                std::cout << "⚠️  步态参数设置功能暂不可用" << std::endl;
                std::cout << "请使用其他可用功能" << std::endl;
                break;
            }

            case 14:
            {
                std::cout << "当前运动状态:" << std::endl;
                printMCSMotionInfo(latest_motion);
                break;
            }

            case 15:
            {
                std::cout << "当前系统状态:" << std::endl;
                printMCSStatusInfo(latest_status);
                break;
            }

            case 16:
            {
                sport.setStatusCallback([&latest_status](const Lenovo::Daystar::MCSStatusInfo &status)
                                        {
                        latest_status = status;
                        std::cout << "[回调] 系统状态更新" << std::endl;
                        printMCSStatusInfo(status); });
                std::cout << "✓ 状态回调已设置" << std::endl;
                break;
            }

            case 17:
            {
                sport.setMotionCallback([&latest_motion](const Lenovo::Daystar::MCSMotionInfo &motion)
                                        {
                        latest_motion = motion;
                        std::cout << "[回调] 运动状态更新" << std::endl;
                        printMCSMotionInfo(motion); });
                std::cout << "✓ 运动回调已设置" << std::endl;
                break;
            }

            case 18:
            {
                std::cout << "当前心跳值: " << latest_status.heartBeat << std::endl;
                break;
            }

            case 19:
            {
                std::cout << "测试通用调用 - 获取机器人状态" << std::endl;
                std::map<std::string, std::any> params;
                auto response = sdk.genericCall("GetRobotStatus", params);
                std::cout << "通用调用结果:" << std::endl;
                std::cout << "  成功: " << (response.success ? "是" : "否") << std::endl;
                std::cout << "  请求ID: " << response.request_id << std::endl;
                std::cout << "  错误信息: " << response.error_message << std::endl;
                break;
            }

            default:
                std::cout << "✗ 无效选择,请重新输入!" << std::endl;
                break;
            }

            // 等待一小段时间,让回调消息有机会显示
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
    }
    catch (const std::exception &e)
    {
        std::cerr << "错误: " << e.what() << std::endl;
        return 1;
    }

    std::cout << "Sport控制程序已退出!" << std::endl;
    return 0;
}

编译应用文件

当完成应用逻辑的编写后,接下来开始编译应用文件。

sdk_test_project/CMakeLists.txt 文件添加编译命令, 将刚才写的例程加入以生成对应可执行文件,然后保存文件即可。

cpp
cmake_minimum_required(VERSION 3.10)

project(sdk_test_project)

# Set C++ standard
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)

set(ROBOT_CLIENT_LIB ${CMAKE_CURRENT_LIST_DIR}/lib/liblenovo_daystar_sdk.so)

# Add executable
add_executable(app_test src/app_test.cpp)

# Link libraries
target_link_libraries(app_test
    ${ROBOT_CLIENT_LIB}
)

# Include directories
target_include_directories(app_test PRIVATE
    ${CMAKE_CURRENT_SOURCE_DIR}
    ${CMAKE_CURRENT_LIST_DIR}/include
)

最后编译例程运行

bash
cd /home/daystar/sdk_test_project
mkdir build
cd build
cmake ..
make

运行应用

连接机器人网络(例如IS_Plus_001),确保开发设备跟机器人在同一个局域网内,打开一个终端,并依次执行以下命令以运行应用。按照终端提示文本按Enter键进行下一步。

bash
cd /home/daystar/sdk_test_project/build
sudo ./app_test

注意

该应用会将控制命令经由运控服务转发至 Daystar Bot 机器人,故运行该应用前请确保机器人处于正常运行状态,具体请查阅《App绑定》篇。


创建客户应用(Python)

本文还提供给用户在Python环境下使用 Daystar Bot SDK 创建一个属于自己的高层应用程序。

当前支持python3.10及以上版本环境

编程接口

以下是SDK Python Client的核心编程接口,可以在自己的Python程序中使用:

python
from daystar_sdk.client import RobotClient

# 使用上下文管理器自动处理连接
with RobotClient(host='192.168.144.103', port=50051) as client:
    # 系统信息查询
    success, version = client.get_mc_version()
    success, name = client.get_robot_name()
    
    # RPS相关操作
    success, battery_level, message = client.rps_get_battery_level()
    print(f"电池电量: {battery_level}%")
    
    success, message = client.set_rps_light_style(2)  # 呼吸灯
    time.sleep(2) 
    
    # MCS运动控制
    success = client.driver_enable(True)         # 启用驱动器
    time.sleep(1)
    success = client.lie_down_or_stand_up(True)  # 站立
    time.sleep(3)
    success = client.set_guardian_switch(True)   # 启用避障功能
    time.sleep(1)
    success = client.body_height_adjust(41)      # 调整高度为41%, IS最高41
    time.sleep(2)
    success = client.speed_adjust(34)            # 调整速度为34%, IS默认34
    time.sleep(2)
    success = client.set_scene(2)                # 设置步行场景
    time.sleep(2)
    success = client.direction_movement(linear_x=0.3)  # 前进
    time.sleep(3)  # 等待移动完成
    success = client.direction_movement(linear_x=0.0)  # 停止前进
    time.sleep(1)
    success = client.direction_movement(angular_z=0.4)  # 旋转
    time.sleep(3)  # 等待旋转完成
    success = client.direction_movement(angular_z=0.0)  # 停止旋转
    time.sleep(2) 
    
    # 功能开关
    success = client.demo_control(2, True)               # 启用演示模式2:打招呼
    time.sleep(15) 
    
    # 状态查询
    success, common_status = client.get_common_status()
        if success:
            logging.info(f"网络状态: {common_status.network}%")
            logging.info(f"机器人姿态: {'站立' if common_status.belie_or_stand == 3 else '躺下'}")
            logging.info(f"急停状态: {'已急停' if common_status.emergency else '未急停'}")
            logging.info(f"避障状态: {'启用' if common_status.guardian_switch else '禁用'}")
            logging.info(f"当前速度: {common_status.cur_speed}")
            logging.info(f"当前高度: {common_status.cur_height}")
            logging.info(f"驱动器状态: {'启用' if common_status.driver_enable_state == 1 else '禁用'}")
            logging.info(f"当前场景: {common_status.cur_scene}")
    time.sleep(1)

    success = client.lie_down_or_stand_up(False)  # 趴下
    time.sleep(5)

    # 禁用驱动器
    logging.info("禁用驱动器...危险行为, 请确保机器人已躺下且周围无障碍物, 输入DISABLE DRIVER确认")
    success, common_status = client.get_common_status()
    if common_status.belie_or_stand == 3:
        logging.error("机器人未躺下, 取消禁用驱动器")
    else:
        user_input = input("请输入确认信息: ")
        if user_input == "DISABLE DRIVER":
            success = client.driver_enable(False)
            if success:
                logging.info("驱动器已禁用")
            else:
                logging.error("禁用驱动器失败")
        else:
            logging.error("未输入确认信息, 取消禁用驱动器, 可通过按下急停按钮强制趴下")
    time.sleep(1)

注意:完整示例代码请参考 examples 目录下 motion_demo.pyperception_demo.py