Skip to content

Device Status Service Interface

functionC++ROS
get regular status information
Get Basic Status
Get Control Mode
Get the current scene (gait)
Get pile-on status
Get joint status information

get regular status information

C++ interface

obtain the robot regular state information, most of the ontology information can be in the structure.

rcClientInterfaceGetCommonStatusintroduction

cpp
/**
* @brief Get general status information about the controlled robot (the main interface for robot status feedback).
*
* This function is used to get the general status information of the robot.
*
* @param com_state Pointer to a structure that holds information about the state of the robot.
* @return bool Returns `true` for success, `false` for failure.
*/
bool rcClientInterfaceGetCommonStatus(common::ROBOT_COMMON_STATUS *com_state);

Structure Definition

cpp
typedef struct ROBOT_COMMON_STATUS_{
    int network; //Robot's current network state
    BODY_POSTURE_STATE belie_or_stand; //Robot is in the stand-down state.
    bool emergency; //Is the robot in an emergency state?
    bool avoidance; //Automatic avoidance state
    long int heartbeat; //heartbeat
    float cur_speed; //Current speed
    float cur_height; //Current height
    MC_QUATERNIONS position; //Position in the robot's world coordinate system, xyz and yaw angles.
    float max_height; //Maximum height of the robot.
    float max_speed; //Maximum speed of the robot.
    SCENE_SWITCH_STATE cur_scene_switch_state; //Successful switching of the scene.
    SCENE_TYPE cur_scene; //Current scene; //What is the current scene?
    DRIVER_ENABLE_STATE driver_enable_state.
    BODY_ODOMETRY odometry; //Robot odometry information; //Robot odometry information.
    float tip_position_wrt_hip[LEG_NUMBER*3]; //Position of the foot end relative to the hip coordinate system
    float vf_planned_tip_position_wrt_base[LEG_NUMBER * 3]; //Position of the foot end relative to the hip coordinate system.
    float touch_detection[LEG_NUMBER];//touch detection, range from 0 to 1.0, equal to 1.0 for absolute touch, later will be changed to probability, but at present there are still only two possibilities 1 or 0
    float target_body_orientation[3]; //raw pitch yaw
    //Error code
    int error_code.
    bool has_error; unsigned int error_wheel[3]; //raw pitch yaw
    unsigned int error_wheel[CODE_WHEEL_NUM]; //Error code.
    unsigned int warn_wheel[CODE_WHEEL_NUM]; unsigned int note_wheel[CODE_WHEEL_NUM]; }
    unsigned int note_wheel[CODE_WHEEL_NUM]; unsigned int
    unsigned int joint_error_wheel[JOINT_CODE_WHEEL_NUM];
    unsigned int joint_note_wheel[JOINT_CODE_WHEEL_NUM];
    ROBOT_CHARGE_STATUS charge_status.
    ROBOT_CHARGE_STATUS charge_status; bool salute_state.
    NAV_OR_JOY_MODE joy_mode;
    EQA_STATE eqa_state; bool guardian_switch; bool guardian_status; eqa_state
    bool guardian_switch; //Indicates the state of the current switch for the stopping function.
    bool visual_foothold_switch; //Indicates the current state of the visual foothold switch.
    GUARDIAN_VELOCITY_DECAY_RATIO guardian_velocity_decay_ratio; //Used for displaying the obstacles in the front, left and right directions.
    LIE_DOWN_ADJUST_ lie_down_adjust; //Used to determine whether or not an end-of-foot position adjustment is being made before getting down.
    float speed_bar_on_controller; //Currently selected line speed on the robot remote controller
    CALIBRATE_POSE_STATE calibrate_pos_state; //Calibrate_pos_state; //Calibrate_pos_state; //Calibrate_pos_state
int self_test_

use Case

cpp
common::ROBOT_COMMON_STATUS com_state;
bool success = rcClientInterfaceGetCommonStatus(&com_state);
if (success) {
    std::cout << "Common Status Retrieved Successfully." << std::endl;
    // Output or process com_state data
} else {
    std::cout << "Failed to Retrieve Common Status." << std::endl;
}

Precautions

  • com_state must point to a validcommon::ROBOT_COMMON_STATUS structure.

get Basic Status

C++ interface

reference 1.1 rcClientInterfaceGetCommonStatus

use Case

cpp
common::ROBOT_COMMON_STATUS com_state;
bool success = rcClientInterfaceGetCommonStatus(&com_state);
if (success) {
    std::cout << "Common Status Retrieved Successfully." << std::endl;
    auto statue = com_state.belie_or_stand
    // Output or process com_state data
} else {
    std::cout << "Failed to Retrieve Common Status." << std::endl;
}

Status Code Definition

cpp
typedef enum BODY_POSTURE_STATE_{
    NULL_POSTURE = 0,
    STARTMOVING = 1,
    MOVING = 2,
    LIEDOWN = 3,
    STANDUP = 4,
    CHARGING=5  
}BODY_POSTURE_STATE;

ROS2 interface

this interface is used to publish the basic status information of the robot.

Topic NameTopic typerole
/robot_statestd_msgs::msg::UInt32issued

message structure

std_msgs::msg::UInt32 the message type contains the following fields:

0:lie down

2:stand up

7:null

sample message

cpp
std_msgs::msg::UInt32 state_msg;
state_msg.data = 2; // standup

test Method

bash
ros2 topic echo /robot_state

get Control Mode

obtain robot control mode information: in navigation mode or manual mode.

C++ interface

refer to [Device Status Service Interface]-[Obtaining regular Status Information].

cpp
common::ROBOT_COMMON_STATUS com_state;
bool success = rcClientInterfaceGetCommonStatus(&com_state);
auto state = com_state.joy_mode

The status code is as follows:

cpp
typedef enum NAV_OR_JOY_MODE_{
    nall_control = 0,
    nav_control = 1,
    joy_control = 2
}NAV_OR_JOY_MODE;

ROS2 interface

this interface is used to publish the current control mode of the robot.

Topic NameTopic typerole
/control_modestd_msgs::msg::UInt8issued

message structure

std_msgs::msg::UInt8the message type contains the following fields:

  • data: Indicates the control mode of the robot, that is, the status code.
  • status Code Description:
    • 0: Handheld remote control
    • 1: Automatic (navigation)

sample message

cpp
std_msgs::msg::UInt8 control_mode_msg;
control_mode_msg.data = 1;

Test Method

bash
ros2 topic echo /control_mode

get the robot's current scene (gait)

C++ interface

Refer to [Device Status Service Interface]-[Obtaining regular Status Information]].

cpp
common::ROBOT_COMMON_STATUS com_state;
bool success = rcClientInterfaceGetCommonStatus(&com_state);
auto state = com_state.cur_scene

The status code is as follows:

cpp
typedef enum SCENE_TYPE_{
    NULL_SCENE = 0, // none
    LIE_DOWN = 1, // Lay Down
    WALKING = 2,// Walking
    STAIRS = 3,// stairs
    CHARGE = 4,// Charge
    PERCEIVE_STAIRS = 5,//
    SNOW = 6,// Snow
    SLIPPY = 7,// Landslide
    STONE = 8,// gravel road
}SCENE_TYPE;

ROS2 interface

This interface is used to publish the current gait of the robot.

Topic NameTopic typerole
/scenestd_msgs::msg::UInt8issued

message structure

std_msgs::msg::UInt8the message type contains the following fields:

  • data: Represents the current gait of the robot. The gait code is as follows:
    • 0: None
    • 1: Get down.
    • 2: Walking
    • 3: Stairs
    • 4: Charging

sample message

cpp
std_msgs::msg::UInt8 scene_msg;
scene_msg.data = 2;

Test Method

bash
ros2 topic echo /scene

get pile-on status

C++ interface

refer to [Device Status Service Interface]-[Obtaining regular Status Information]].

cpp
common::ROBOT_COMMON_STATUS com_state;
bool success = rcClientInterfaceGetCommonStatus(&com_state);
auto state = com_state.charge_status.charge_switch_state

The status code is as follows:

cpp
typedef enum CHARGE_SWITCH_STATE_{
    NULL_CHARGE_SWITCH = 0,
    EXIT_SUCCEEDED = 1,  
    EXITING_TILE = 2, 
    EXIT_FAILED=3, 
    ENTER_SUCCEEDED = 4,
    ENTERING_TILE = 5,
    ENTER_FAILED=6   
}CHARGE_SWITCH_STATE;

get joint status information

C++ interface

rcClientInterfaceGetJointsStatusintroduction

cpp
/**
* @brief Get information about the state of the body on the robot joints.
*
* This function is used to get information about the state of the robot's joints.
*
* @param state Pointer to a structure that holds information about the state of the robot's joints.
* @return bool Returns `true` for success, `false` for failure.
*/
bool rcClientInterfaceGetJointsStatus(common::ROBOT_JOINTS_STATUS *state);

Use Case

cpp
common::ROBOT_JOINTS_STATUS state;
bool success = rcClientInterfaceGetJointsStatus(&state);
if (success) {
    std::cout << "Joints Status Retrieved Successfully." << std::endl;

} else {
    std::cout << "Failed to Retrieve Joints Status." << std::endl;
}

Precautions

  • state must point to a validcommon::ROBOT_JOINTS_STATUS structure.