Skip to content

高层运动服务接口


介绍

高层控制接口:通过调用 ROS2 Topic/调用 ROS2 Service/调用 C++ 函数/python function,来给 Saturn Robot 发送速度控制、姿态控制等运动指令。

功能C++ROSPythonC#
初始化
上下使能
设置姿态
控制移动方向和速度
调整站立高度
设置步态
演示模式控制
调节运行速度
软件急停
恢复软件急停
设置控制模式
停障开关

初始化

C++接口

explicit SDK(const std::string &server_address = "192.168.144.103:50051")

功能说明
构造函数,连接到指定地址的机器人服务。

参数说明

参数名类型必填/默认值说明
server_addressstd::string"192.168.144.103:50051"服务器地址,格式为"地址:端口"。

返回值

类型说明
SDKSDK实例。

Python接口

RobotClient(host='192.168.144.103', port=50051)

功能说明
机器人初始化连接。

参数说明

参数名类型必填/默认值说明
hoststr'192.168.144.103'机器人主机地址。
portint50051机器人主机端口。

返回值

类型说明
RobotClientRobotClient实例。

C#接口

void InitialRobot(RobotType robotType, IConnectionStateListener connectionStateListener = null, IPerceptionConnectionStateListener perceptionConnectionStateListener = null)

功能说明
机器人初始化连接(无需设置ip和port)。

参数说明

参数名类型必填/默认值说明
robotTypeRobotType必填机器人类型。RobotType.IS: 六足机器人, RobotType.MC: 四轮足机器人。
connectionStateListenerIConnectionStateListenernull连接机器人状态回调。
perceptionConnectionStateListenerIPerceptionConnectionStateListenernull连接机器人状态回调。

返回值

类型说明
void无返回值。

void InitialRobot(RobotType robotType, string motionHostIP, string motionHostPort, string perceptionHostIP = null, string perceptionHostPort = null, IConnectionStateListener connectionStateListener = null, IPerceptionConnectionStateListener perceptionConnectionStateListener = null)

功能说明
机器人初始化连接(可自设置ip和port)。

参数说明

参数名类型必填/默认值说明
robotTypeRobotType必填机器人类型。
motionHostIPstring必填机器人SYSA主机地址。
motionHostPortstring必填机器人SYSA主机端口。
perceptionHostIPstringnull机器人SYSB主机地址。
perceptionHostPortstringnull机器人SYSB主机地址。
connectionStateListenerIConnectionStateListenernull连接机器人状态回调。
perceptionConnectionStateListenerIPerceptionConnectionStateListenernull连接机器人状态回调。

返回值

类型说明
void无返回值。

调用示例

csharp
// 创建SDK Manager
_robotSDKManager = RobotSDKManager.Instance;
// 使用默认地址
_robotSDKManager?.InitialRobot(RobotType.IS, this, this);

// 自行设置地址
_robotSDKManager?.InitialRobot(RobotType.IS, "192.168.144.103", null, "192.168.144.105", null, this, this);

上下使能

C++ 接口

bool enableDriver(bool enable)

功能说明
启用或禁用驱动器。

参数说明

参数名类型必填/默认值说明
enablebool必填是否启用。

返回值

类型说明
bool操作是否成功。

调用示例

cpp
Lenovo::Daystar::SDK sdk;

if (!sdk.isConnected()) {
    std::cerr << "Can not connect SDK server with default settings"
              << std::endl;
    return 1;
} else {
    std::cout << "Connected with default settings" << std::endl;
}
auto &sport = sdk.getSport();
bool enable = true;
bool success = sport.enableDriver(enable);
if (success) {
    std::cout << "Driver Enable Command Successful." << std::endl;
} else {
    std::cout << "Failed to Enable Driver." << std::endl;
}

Python 接口

driver_enable(enable=True, source=2)

功能说明
启用/禁用驱动器。

参数说明

参数名类型必填/默认值说明
enableboolTrue是否启用驱动器。
sourceint2控制模式 (1:导航控制模式, 2:手柄控制模式)。

返回值

类型说明
bool操作是否成功。

调用示例

python
from daystar_sdk.client import RobotClient
# 初始化客户端
with RobotClient(host='192.168.144.105', port=50051) as client:
    # 机器人驱动使能
    success = client.driver_enable(True)

C#接口

void EnableRobot()

功能说明
启用机器人驱动器。

参数说明

参数名类型必填/默认值说明
---无参数。

返回值

类型说明
void无返回值。

void DisableRobot()

功能说明
禁用机器人驱动器。

参数说明

参数名类型必填/默认值说明
---无参数。

返回值

类型说明
void无返回值。

注意事项

  • 需启动机器人驱动器之后才可以控制机器人行走及其他动作。
  • 驱动器的状态可以通过监听MotionState获取,详细使用参考《运动状态服务接口》章节。

设置机器人姿态

C++ 接口

bool lieDownOrStandUp(bool lieDown)

功能说明
控制机器人趴下或站起。

参数说明

参数名类型必填/默认值说明
lieDownbool必填true表示站立,false表示趴下。

返回值

类型说明
bool操作是否成功。

调用示例

cpp
Lenovo::Daystar::SDK sdk;

if (!sdk.isConnected()) {
    std::cerr << "Can not connect SDK server with default settings"
              << std::endl;
    return 1;
} else {
    std::cout << "Connected with default settings" << std::endl;
}
auto &sport = sdk.getSport();
bool enable = true;
bool success = sport.lieDownOrStandUp(enable);
if (success) {
    std::cout << "Lie Down Or Stand Up Command Successful." << std::endl;
} else {
    std::cout << "Failed to Lie Down Or Stand Up." << std::endl;
}

使用案例

cpp
Lenovo::Daystar::SDK sdk;

if (!sdk.isConnected()) {
    std::cerr << "Can not connect SDK server with default settings"
              << std::endl;
    return 1;
} else {
    std::cout << "Connected with default settings" << std::endl;
}
auto &sport = sdk.getSport();
bool enable = true;
bool success = sport.lieDownOrStandUp(enable);
if (success) {
    std::cout << "Lie Down Or Stand Up Command Successful." << std::endl;
} else {
    std::cout << "Failed to Lie Down Or Stand Up." << std::endl;
}

ROS2 接口

该接口用于订阅 ROS2 的 /cmd_vel topic,以获取机器人运动的速度指令。

Service 名称Service 类型角色
/nav/robot_commandysc_robot_msgs/srv/RobotCommand客户端

消息结构

ysc_robot_msgs/srv/RobotCommand 包括以下字段:

字段名称类型描述
RobotCommandstring包含一个字符串,表示具体命令。

测试方法

  • 调用接口,调一次切换起立/坐下状态:
bash
ros2 service call /nav/robot_command ysc_robot_msgs/srv/RobotCommand "comamnd_name: 'StandUpDown'"

Python 接口

lie_down_or_stand_up(state, source=2)

功能说明
控制机器人躺下或站起。

参数说明

参数名类型必填/默认值说明
statebool必填True为站起,False为躺下。
sourceint2控制模式 (1:导航控制模式, 2:手柄控制模式)。

返回值

类型说明
bool操作是否成功。

注意事项

  • 在使机器人站立前,必须先调用driver_enable(True)启用驱动器。

调用示例

python
from daystar_sdk.client import RobotClient

# 初始化客户端
with RobotClient(host='192.168.144.105', port=50051) as client:
    success = client.driver_enable(True)         # 启用驱动器
    success = client.lie_down_or_stand_up(True)  # 站起

C#接口

void SetPostureType(RobotPostureType type)

功能说明
设置机器人姿态。

参数说明

参数名类型必填/默认值说明
typeRobotPostureType必填姿态类型。RobotPostureType.Lie_Down: 趴下, RobotPostureType.Stand_Up: 站立。

返回值

类型说明
void无返回值。

注意事项

  • 姿态切换状态和当前姿态可通过监听MotionState获取,详细使用参考 《运动状态服务接口》 章节。

控制移动方向与速度

C++接口

bool move(float x, float y, float yaw)

功能说明
方向运动控制。控制机器人在三个自由度上的运动:前后移动、左右移动和旋转。

参数说明

参数名类型必填/默认值说明
xfloat必填前后方向速度分量,范围取决于步态和地形(详见下方说明),正值向前,负值向后。
yfloat必填左右方向速度分量,范围取决于步态和地形(详见下方说明),正值向左,负值向右。
yawfloat必填旋转速度分量,范围取决于步态和地形(详见下方说明),正值逆时针,负值顺时针。

返回值

类型说明
bool操作是否成功。

参数范围和方向说明:

  • 常规步态
    • 平地:x: [-1.0, 2.0], y: [-0.4, 0.4], yaw: [-0.8, 0.8]
    • 斜坡:x: [-0.8, 0.8], y: [-0.4, 0.4], yaw: [-0.6, 0.6]
  • RL步态(平地/斜坡等所有地形):
    • x: [-2.0, 2.0], y: [-0.4, 0.4], yaw: [-0.8, 0.8]
  • 单位:x, y 为 m/s;yaw 为 rad/s (近似对应)
  • 参数符号表示运动方向
    • 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
Lenovo::Daystar::SDK sdk;

if (!sdk.isConnected()) {
    std::cerr << "Can not connect SDK server with default settings"
              << std::endl;
    return 1;
} else {
    std::cout << "Connected with default settings" << std::endl;
}
auto &sport = sdk.getSport();
bool need_to_move = true;
while (need_to_move) {
    sport.move(0.5f, 0.0f, 0.0f);  // 持续发送运动命令
    std::this_thread::sleep_for(std::chrono::milliseconds(50));  // 20Hz频率
}

注意:完整案例代码请参考应用开发模块中的《创建客户应用》章节。

Python 接口

direction_movement(linear_x=0.0, linear_y=0.0, angular_z=0.0, source=2)

功能说明
控制机器人移动。

参数说明

参数名类型必填/默认值说明
linear_xfloat0.0前进/后退速度。范围取决于步态和地形(详见下方说明)。
linear_yfloat0.0左右移动速度。范围取决于步态和地形(详见下方说明)。
angular_zfloat0.0旋转速度。范围取决于步态和地形(详见下方说明)。
sourceint2控制模式 (1:导航控制模式, 2:手柄控制模式)。

返回值

类型说明
bool操作是否成功。

参数范围说明:

  • 常规步态
    • 平地:linear_x: [-1.0, 2.0] m/s, linear_y: [-0.4, 0.4] m/s, angular_z: [-0.8, 0.8] rad/s
    • 斜坡:linear_x: [-0.8, 0.8] m/s, linear_y: [-0.4, 0.4] m/s, angular_z: [-0.6, 0.6] rad/s
  • RL步态(平地/斜坡等所有地形):
    • linear_x: [-2.0, 2.0] m/s, linear_y: [-0.4, 0.4] m/s, angular_z: [-0.8, 0.8] rad/s

调用示例

python
from daystar_sdk.client import RobotClient
# 初始化客户端
with RobotClient(host='localhost', port=50051) as client:
    # 机器人移动
    success = client.direction_movement(linear_x=0.5)

ROS2 接口

该接口用于订阅 ROS2 的 /cmd_vel topic,以获取机器人运动的速度指令。

Topic 名称Topic 类型角色
/cmd_velgeometry_msgs::msg::Twist订阅方

消息结构

geometry_msgs::msg::Twist 包括以下字段:

字段名称类型描述
linearVector3包含 x, y, z,表示线速度。
angularVector3包含 x, y, z,表示角速度。

参数范围说明:

  • 常规步态
    • 平地:linear.x: [-1.0, 2.0] m/s, linear.y: [-0.4, 0.4] m/s, angular.z: [-0.8, 0.8] rad/s
    • 斜坡:linear.x: [-0.8, 0.8] m/s, linear.y: [-0.4, 0.4] m/s, angular.z: [-0.6, 0.6] rad/s
  • RL步态(平地/斜坡等所有地形):
    • linear.x: [-2.0, 2.0] m/s, linear.y: [-0.4, 0.4] m/s, angular.z: [-0.8, 0.8] rad/s

示例消息

cpp
geometry_msgs::msg::Twist twist_msg;
twist_msg.linear.x = 0.1;
twist_msg.linear.y = 0.0;
twist_msg.linear.z = 0.0;
twist_msg.angular.x = 0.0;
twist_msg.angular.y = 0.0;
twist_msg.angular.z = 0.0;

测试方法

  • 发布一次性消息 使用以下命令发布一次性消息到/cmd_vel topic:
bash
ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.1, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}"
  • 持续发布消息
    使用以下命令以每秒10次的频率持续发布消息到/cmd_vel topic:
bash
ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.2, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}"

注意事项

  • 确保ROS2节点已启动并正在订阅/cmd_vel topic。
  • 发布的消息格式必须严格符合geometry_msgs::msg::Twist消息类型的定义。
  • 持续发布消息时,注意控制发布频率,避免网络拥塞。

C#接口

void MoveRobot(float[] moveDirection)

功能说明
控制机器人移动。

参数说明

参数名类型必填/默认值说明
moveDirectionfloat[]必填移动和旋转参数。
moveDirection[0]: 前后方向速度分量,正值向前,负值向后。
moveDirection[1]: 左右方向速度分量,正值向左,负值向右。
moveDirection[2]: 旋转速度分量,正值逆时针,负值顺时针。
各分量范围取决于步态和地形(详见下方说明)。

返回值

类型说明
void无返回值。

参数范围说明:

  • 常规步态
    • 平地:moveDirection[0]: [-1.0, 2.0] m/s, moveDirection[1]: [-0.4, 0.4] m/s, moveDirection[2]: [-0.8, 0.8] rad/s
    • 斜坡:moveDirection[0]: [-0.8, 0.8] m/s, moveDirection[1]: [-0.4, 0.4] m/s, moveDirection[2]: [-0.6, 0.6] rad/s
  • RL步态(平地/斜坡等所有地形):
    • moveDirection[0]: [-2.0, 2.0] m/s, moveDirection[1]: [-0.4, 0.4] m/s, moveDirection[2]: [-0.8, 0.8] rad/s

注意事项

  • 必须持续调用:机器人运动需要持续的move命令来维持(<200ms间隔),否则机器人将自动停止。
  • 建议:以10-20Hz的频率调用此函数以确保平滑的运动控制
  • 停止运动:移动和旋转速度需赋值为0,MoveRobot(new float[] { 0, 0, 0 })。

调节站立高度

C++接口

bool adjustBodyHeight(int percent)

功能说明
调整机身高度。

参数说明

参数名类型必填/默认值说明
percentint必填高度百分比,范围20-50。

返回值

类型说明
bool操作是否成功。

调用示例

cpp
Lenovo::Daystar::SDK sdk;

if (!sdk.isConnected()) {
    std::cerr << "Can not connect SDK server with default settings"
              << std::endl;
    return 1;
} else {
    std::cout << "Connected with default settings" << std::endl;
}
auto &sport = sdk.getSport();
bool success = sport.adjustBodyHeight(30);
if (success) {
    std::cout << "Adjust Body Height Successful." << std::endl;
} else {
    std::cout << "Failed to Adjust Body Height." << std::endl;
}

注意事项

  • 确保传入的操作模式和高度调整比例height_adjustment是0到100的范围内的整数。

Python 接口

body_height_adjust(percent, source=2)

功能说明
调整机器人身体高度。

参数说明

参数名类型必填/默认值说明
percentint必填身体高度百分比(25-50)IS 最高为41,默认高度为34。
sourceint2控制模式 (1:导航控制模式, 2:手柄控制模式)。

返回值

类型说明
bool操作是否成功。

调用示例

python
from daystar_sdk.client import RobotClient
# 初始化客户端
with RobotClient(host='192.168.144.105', port=50051) as client:
    # 机器人调整机身高度
    success = client.body_height_adjust(50)

ROS2接口

该服务用于调整机器人的身体高度。

Service 名称Service 类型角色
/saturn/rc_height_scalesaturn_msgs::srv::HeightScale客户端

请求消息

saturn_msgs::srv::HeightScale::Request 消息类型包含以下字段:

字段名称类型描述
scaleint整数类型,表示高度比例

示例消息

cpp
saturn_msgs::srv::HeightScale::Request request;
request.scale = 8;

saturn_msgs::srv::HeightScale::Response response;

测试方法

bash
ros2 service call /saturn/rc_height_scale saturn_msgs/srv/HeightScale "{scale: 8}"

C#接口

void SetRobotHeight(int heightPercent)

功能说明
控制机器人高度。

参数说明

参数名类型必填/默认值说明
heightPercentint必填高度百分比,范围20-50。机器人高度范围0.2m-0.5m。

返回值

类型说明
void无返回值。

设置运动步态

C++接口

bool setScene(SceneType sceneType)

功能说明
设置机器人运动步态。

参数说明

参数名类型必填/默认值说明
sceneTypeSceneType必填步态类型,使用SceneType枚举。SceneType.WALKING: 行走步态, SceneType.STAIRS: 楼梯步态, SceneType.ROUGH: 斜坡步态, SceneType.RL_WALK: RL步态。

返回值

类型说明
bool操作是否成功。

调用示例

cpp
Lenovo::Daystar::SDK sdk;

if (!sdk.isConnected()) {
    std::cerr << "Can not connect SDK server with default settings"
              << std::endl;
    return 1;
} else {
    std::cout << "Connected with default settings" << std::endl;
}
auto &sport = sdk.getSport();
SceneType sceneType = SceneType.WALKING;
bool success = sport.setScene(sceneType);
if (success) {
    std::cout << "Adjust Body Height Successful." << std::endl;
} else {
    std::cout << "Failed to Adjust Body Height." << std::endl;
}

Python 接口

set_scene(scene_type, source=2)

功能说明
设置机器人场景。

参数说明

参数名类型必填/默认值说明
scene_typeint必填场景类型。2: 行走步态, 3: 楼梯步态, 8: 斜坡步态。
sourceint2控制模式 (1:导航控制模式, 2:手柄控制模式)。

返回值

类型说明
bool操作是否成功。

注意: 在调用此函数前,必须先调用driver_enable(True)启用驱动器,并通过lie_down_or_stand_up(True)使机器人站立。

调用示例

python
from daystar_sdk.client import RobotClient
# 初始化客户端
with RobotClient(host='192.168.144.105', port=50051) as client:
    # 机器人设置运动步态
    success = client.set_scene(1)

ROS2 接口

该服务用于调整机器人的运动步态。

Service 名称Service 类型角色
/saturn/robot_commandysc_robot_msgs::srv::RobotCommand客户端

请求消息

ysc_robot_msgs::srv::RobotCommand::Request 消息类型包含以下字段:

字段名称类型描述
command_namestring字符串,表示具体命令

示例消息

cpp
saturn_msgs::srv::HeightScale::Request request;
request.scale = "StairsGait"; //楼梯步态

saturn_msgs::srv::HeightScale::Response response;

测试方法

bash
ros2 service call /saturn/robot_command ysc_robot_msgs/srv/Robot_Command "{command_name: 'StairsGait'}"

C# 接口

void SetRobotScene(RobotSceneType robotSceneType)

功能说明
设置机器人运动步态。

参数说明

参数名类型必填/默认值说明
robotSceneTypeRobotSceneType必填步态类型,使用RobotSceneType枚举。RobotSceneType.Walking: 行走步态, RobotSceneType.Stairs: 楼梯步态, RobotSceneType.Slope: 斜坡步态, RobotSceneType.RlWalk: 强化学习步态。

返回值

类型说明
void无返回值。

注意事项

  • 步态切换状态和当前步态可通过监听MotionState获取,详细使用参考 《运动状态服务接口》 章节。
  • 设置步态前机器人必须是在站立姿态才可设置成功。

演示模式控制

C++接口

bool demoControl(int demoType, bool enable = true)

功能说明
演示控制。

参数说明

参数名类型必填/默认值说明
demoTypeint必填演示类型: 2(摇头),3(摆尾),4(打招呼)。
enablebooltrue是否启用。

返回值

类型说明
bool操作是否成功。

调用示例

cpp
Lenovo::Daystar::SDK sdk;

if (!sdk.isConnected()) {
    std::cerr << "Can not connect SDK server with default settings"
              << std::endl;
    return 1;
} else {
    std::cout << "Connected with default settings" << std::endl;
}
auto &sport = sdk.getSport();
int demoType = 2; // 示例值,表示演示模式
bool enable_demo = true; // 示例值,表示开启演示模式
bool success = sport.demoControl(demoType, enable_demo);
if (success) {
    std::cout << "Demo Mode Control Set Successfully." << std::endl;
} else {
    std::cout << "Failed to Set Demo Mode Control." << std::endl;
}

注意事项

  • 确保传入的演示模式和使能状态是有效的。

Python接口

demo_control(mode, enable, source=2)

功能说明
演示控制。

参数说明

参数名类型必填/默认值说明
modeint必填演示模式: 2(摇头),3(摆尾),4(打招呼)。
enablebool必填是否启用。
sourceint2控制模式 (1:导航控制模式, 2:手柄控制模式)。

返回值

类型说明
bool操作是否成功。

调用示例

python
from daystar_sdk.client import RobotClient
# 初始化客户端
with RobotClient(host='192.168.144.105', port=50051) as client:
    # 机器人调整速度范围
    success = client.demo_control(2, True)

C#接口

void SetPlaybackRoutine(BionicActionType playType)

功能说明
演示控制。

参数说明

参数名类型必填/默认值说明
playTypeBionicActionType必填演示动作类型。BionicActionType.SAY_HELLO: 打招呼, BionicActionType.SHAKE_HAND: 摇头, BionicActionType.FISHTAILING: 摆尾。

返回值

类型说明
void无返回值。

调节运行速度

C++接口

bool adjustSpeed(int percent)

功能说明
调整速度系数。设置机器人运动的速度系数,该系数会影响所有运动命令的实际执行速度。

参数说明

参数名类型必填/默认值说明
percentint必填速度百分比,有效范围0-100。系统默认值为60%。0:停止所有运动,50:使用50%的最大速度,100:使用100%的最大速度。

返回值

类型说明
bool操作是否成功。

调用示例

cpp
Lenovo::Daystar::SDK sdk;

if (!sdk.isConnected()) {
    std::cerr << "Can not connect SDK server with default settings"
              << std::endl;
    return 1;
} else {
    std::cout << "Connected with default settings" << std::endl;
}
auto &sport = sdk.getSport();
bool success = sport.adjustSpeed(70);
if (success) {
    std::cout << "Adjust Speed Successfully." << std::endl;
} else {
    std::cout << "Failed to Adjust Speed." << std::endl;
}

注意事项

  • 建议在机器人运动前先设置合适的速度系数,特别是在测试阶段使用较低的速度。
  • 请确保百分比在0-100范围内,超出范围可能导致不可预期的行为。

Python 接口

bool speed_adjust(percent, source=2)

功能说明
调整机器人速度。

参数说明

参数名类型必填/默认值说明
percentint必填速度百分比(0-100)。系统默认值为60%。
sourceint2控制模式 (1:导航控制模式, 2:手柄控制模式)。

返回值

类型说明
bool操作是否成功。

调用示例

python
from daystar_sdk.client import RobotClient
# 初始化客户端
with RobotClient(host='192.168.144.105', port=50051) as client:
    # 机器人调整速度范围
    success = client.speed_adjust(62)

C#接口

void SetRobotSpeed(int speedPercent)

功能说明
调整速度系数。设置机器人运动的速度系数,该系数会影响所有运动命令的实际执行速度。

参数说明

参数名类型必填/默认值说明
speedPercentint必填速度百分比,有效范围0-100。系统默认值为60%。0:停止所有运动,50:使用50%的最大速度,100:使用100%的最大速度。

返回值

类型说明
void无返回值。

注意事项

  • 建议在机器人运动前先设置合适的速度系数,特别是在测试阶段使用较低的速度。
  • 请确保百分比在0-100范围内,超出范围可能导致不可预期的行为
  • 获取机器人最大速度和当前速度,请详细使用参考**《运动状态服务接口》**章节。

注意事项

  • 建议在机器人运动前先设置合适的速度系数,特别是在测试阶段使用较低的速度。
  • 请确保百分比在0-100范围内,超出范围可能导致不可预期的行为
  • 获取机器人最大速度和当前速度,请详细使用参考**《运动状态服务接口》**章节。

软件急停

C++接口

bool emergencyStop()

功能说明
软件急停。

参数说明
无。

返回值

类型说明
bool操作是否成功。

调用示例

cpp
Lenovo::Daystar::SDK sdk;

if (!sdk.isConnected()) {
    std::cerr << "Can not connect SDK server with default settings"
              << std::endl;
    return 1;
} else {
    std::cout << "Connected with default settings" << std::endl;
}
auto &sport = sdk.getSport();
bool success = sport.emergencyStop();
if (success) {
    std::cout << "Emergency Stop Successfully." << std::endl;
} else {
    std::cout << "Failed to Emergency Stop." << std::endl;
}

Python 接口

driver_estop()

功能说明
驱动器急停。

参数说明
无。

返回值

类型说明
bool操作是否成功。

调用示例

python
from daystar_sdk.client import RobotClient
# 初始化客户端
with RobotClient(host='localhost', port=50051) as client:
    # 机器人急停
    success = client.driver_estop()

C# 接口

void SetDriverEStop()

功能说明
软件急停。

参数说明
无。

返回值

类型说明
void无返回值。

注意事项

  • 触发机器人急停后,需恢复软件急停才可以再次上使能站立控制机器人。
  • 获取机器人急停状态,请详细使用参考 《运动状态服务接口》 _章节。

恢复软件急停

C++接口

bool resumeFromEmergencyStop()

功能说明
恢复急停。

参数说明
无。

返回值

类型说明
bool操作是否成功。

调用示例

cpp
Lenovo::Daystar::SDK sdk;

if (!sdk.isConnected()) {
    std::cerr << "Can not connect SDK server with default settings"
              << std::endl;
    return 1;
} else {
    std::cout << "Connected with default settings" << std::endl;
}
auto &sport = sdk.getSport();
bool success = sport.resumeFromEmergencyStop();
if (success) {
    std::cout << "Resume From Emergency Stop Successfully." << std::endl;
} else {
    std::cout << "Failed to Resume From Emergency Stop." << std::endl;
}

Python 接口

resume_estop()

功能说明
恢复急停。

参数说明
无。

返回值

类型说明
bool操作是否成功。

调用示例

python
from daystar_sdk.client import RobotClient
# 初始化客户端
with RobotClient(host='192.168.144.105', port=50051) as client:
    # 机器人恢复软急停
    success = client.resume_estop()

C#接口

void SetResumeEStop()

功能说明
恢复软件急停。

参数说明
无。

返回值

类型说明
void无返回值。

设置控制模式

该服务设置控制机器人的模式,切换为手动或导航模式。

C++ 接口

bool setControlMode(Lenovo::Daystar::ControllerMode controllerMode)

功能说明
设置导航,手柄或者UWB控制。

参数说明

参数名类型必填/默认值说明
controllerModeLenovo::Daystar::ControllerMode必填控制模式,使用Lenovo::Daystar::ControllerMode枚举。NAV_CONTROL: 导航控制模式, JOY_CONTROL: 手柄控制模式, UWB_CONTROL: UWB控制模式。

返回值

类型说明
bool操作是否成功。

调用示例

cpp
Lenovo::Daystar::SDK sdk;

if (!sdk.isConnected()) {
    std::cerr << "Can not connect SDK server with default settings"
              << std::endl;
    return 1;
} else {
    std::cout << "Connected with default settings" << std::endl;
}
auto &sport = sdk.getSport();
Lenovo::Daystar::ControllerMode controllerMode = Lenovo::Daystar::ControllerMode.NAV_CONTROL;
bool success = sport.setControlMode(controllerMode);
if (success) {
    std::cout << "Set Control Mode Successful." << std::endl;
} else {
    std::cout << "Failed to Set Control Mode." << std::endl;
}

注意事项

  • 确保传入的控制模式是有效的。
  • 设置机器人位姿

Python 接口

set_nav_or_joy_control(nav_or_joy)

功能说明
设置导航或手柄控制。

参数说明

参数名类型必填/默认值说明
nav_or_joyint必填控制模式。1: 导航控制模式, 2: 手柄控制模式。

返回值

类型说明
bool操作是否成功。

调用示例

python
from daystar_sdk.client import RobotClient
# 初始化客户端
with RobotClient(host='192.168.144.105', port=50051) as client:
    # 机器人恢复软急停
    success = client.set_nav_or_joy_control(1)

ROS2 接口

Service 名称Service 类型角色
/saturn/robot_commandysc_robot_msgs::srv::RobotCommand客户端

请求消息

ysc_robot_msgs::srv::RobotCommand::Request 消息类型包含以下字段:

字段名称类型描述
command_namestring字符串,表示具体命令。
EnableNavigation: 导航模式
EnableTeleoption: 手柄控制模式

示例消息

cpp
saturn_msgs::srv::HeightScale::Request request;
request.scale = "EnableNavigation"; //导航

saturn_msgs::srv::HeightScale::Response response;

测试方法

bash
ros2 service call /saturn/rc_nav_or_manual saturn_msgs/srv/NavOrManual "{command_name: 'EnableNavigation'}"

C#接口

void SetControlMode(RobotControlType controlType)

功能说明
设置控制模式。

参数说明

参数名类型必填/默认值说明
controlTypeRobotControlType必填控制类型。
RobotControlType.Joy_Mode: 遥控器模式,
RobotControlType.Nav_Mode: 导航模式,
RobotControlType.Follow_Mode: 跟随模式。

返回值

类型说明
void无返回值。

注意事项

  • 手动控制时设置为遥控器模式。
  • 获取当前控制模式,请详细使用参考 《运动状态服务接口》 章节。

设置停障开关

C++ 接口

bool setGuardianSwitch(bool enable)

功能说明
设置停障开关。

参数说明

参数名类型必填/默认值说明
enablebool必填true表示开启停障,false表示关闭停障。

返回值

类型说明
bool操作是否成功。

调用示例

cpp
Lenovo::Daystar::SDK sdk;

if (!sdk.isConnected()) {
    std::cerr << "Can not connect SDK server with default settings"
              << std::endl;
    return 1;
} else {
    std::cout << "Connected with default settings" << std::endl;
}
auto &sport = sdk.getSport();
Lenovo::Daystar::ControllerMode controllerMode = Lenovo::Daystar::ControllerMode.NAV_CONTROL;
bool success = sport.setGuardianSwitch(true);;
if (success) {
    std::cout << "Set Control Mode Successful." << std::endl;
} else {
    std::cout << "Failed to Set Control Mode." << std::endl;
}

Python 接口

set_guardian_switch(guardian)

功能说明
设置停障。

参数说明

参数名类型必填/默认值说明
guardianbool必填True: 打开停障, False: 关闭停障。

返回值

类型说明
bool操作是否成功。

调用示例

python
from daystar_sdk.client import RobotClient
# 初始化客户端
with RobotClient(host='192.168.144.105', port=50051) as client:
    # 机器人恢复软急停
    success = client.set_guardian_switch(True)

C# 接口

void SetAvoidObstacle(bool enable)

功能说明
设置停障开关。

参数说明

参数名类型必填/默认值说明
enablebool必填true表示开启停障,false表示关闭停障。

返回值

类型说明
void无返回值。

注意事项

  • 获取停障状态,请详细使用参考 《运动状态服务接口》 章节。