saturn_ros2/saturn_controller/sdk/include/common.h

#ifndef COMMON_H
#define COMMON_H

#include <cstring>
#include <string>
#include "com_define.h"
// #include <Eigen/Dense>

typedef struct ROBOT_CONFIG_
{
    //utility
    double dt;
    double legIsRightLst[LEG_NUMBER];

    //kinematics and dynamics
    double l0;
    double l1;
    double l2;
    double lMin2;
    double hip[LEG_NUMBER][3];
    double legs_offset[LEG_NUMBER];

    //dynamics
    double m;
    double inertia[3][3];
    double leg_m;

    //control paramters
    double legStiffSw[3];
    double legStiffSt[3];
    double legDamp[3];
    double legStiffSphSw[3];
    double legStiffSphSt[3];
    double legDampSph[3];
    double jntStiffSw[3];
    double jntStiffSt[3];
    double jntDamp[3];
    double qddKp[3];
    double qddKd[3];

    //gait parameters
    double offset_lst[LEG_NUMBER];
    double stdP[LEG_NUMBER][3];
    double bodyH;

    // constraints
    double wdMax;

    //compensation
    double fricComp[3];
    
}ROBOT_CONFIG;

typedef union {
    float array[3];
    struct {
        float x;
        float y;
        float z;
    } axis;
} MeasurementVector;

typedef union {
    float array[3];
    struct {
        float roll;
        float pitch;
        float yaw;
    } axis;
} EulerAngleVector;

typedef union{
    double array[9];
} Covariance;

/**
 * @brief Quaternion.
 */
typedef union {
    float array[4];
    struct {
        float w;
        float x;
        float y;
        float z;
    } element;
} QuaternionVector;

typedef union {
    float array[3];
    struct {
        float x;
        float y;
        float z;
    } axis;
} GPSVector;

typedef struct TOP_CONFIG_INFO_{
    std::string robot_type;
    ROBOT_DRIVER_TYPES body_model;
}TOP_CONFIG_INFO;

namespace robot_control
{
    namespace common
    {
        const int GAIT_MSG_CAPACITY = 200;

        enum class CoreState {
            BOOT = 0,
            IDLE = 1,
            JOINTS_TEST = 2,
            STAND_STILL = 3,
            IN_PLANNING = 4
        };

        enum GaitMsgType //这里如果修改成enum class，需要芳勇配合修改参数接口
        {
            GMT_NO_MSG = 0,
            GMT_START = 1,
            GMT_ONLINE = 2,
            GMT_EM_STOP = 3,
            GMT_JOINT_CONTROL = 4,
            GMT_GAIT_MODE = 5
        };

        enum GaitID //这里如果修改成enum class，需要芳勇配合修改参数接口
        {
            GID_NO_GAIT    = 0,
            GID_IDLE_GAIT  = 1,
            GID_STANDSTILL = 2,
            GID_RECOVER    = 3,
            GID_HEX_NIMBLE     = 4,
            GID_JOINT_TEST = 5,
            GID_RL  = 6, //强化学习步态
            GID_JOINT_TRAJECTORY_CONTROL  = 7,
            GID_POSE_ADJUSTMENT=8,
            GID_FOOT_ADJUST=9,
            GID_LEG_TEST=10,
            GID_QUA_NIMBLE = 11,
            GID_MAX = 12
        };

        class GaitMsgHeader
        {
        public:
            short int gait_msg_type;
            short int gait_id;
            int data_length;
        };

        class GaitMsg
        {
        public:
            GaitMsgHeader header;
            char data[GAIT_MSG_CAPACITY];
            inline int getAllDataLength()
            {
                return header.data_length + sizeof(GaitMsgHeader);
            }
        };

        class Pose
        {
        public:
            float q[4];
            float gyro[3];
            float acc[3];
        };

        class ModelInput
        {
        public:
            unsigned long count;
            double actual_pos[MOTOR_NUM];
            double actual_vel[MOTOR_NUM];
            double actual_trq[MOTOR_NUM]; // trq read from the motor current
            double mea_trq[MOTOR_NUM];    // trq read from the torque sensor
            Pose body_pose;
            GaitMsg gait_msg;
            bool is_stand_up;
            int planner_stage;
        public:
            ModelInput()
            {
                is_stand_up = false;
            }
        };

        struct JointTestData
        {
            unsigned int axis_bitmask;
            float mag;
            float period;
            float kp_pos;
            float kd_pos;
            float kp_cur;
            float mu_clb;
            int test_type;
        };

        class ModelOutput
        {
        public:
            unsigned long count;
            double target_pos[MOTOR_NUM];
            double target_vel[MOTOR_NUM];
            double target_trq[MOTOR_NUM];
            double target_kp[MOTOR_NUM];
            double target_kd[MOTOR_NUM];
            CoreState core_state;
            GaitID current_gait_id;
        };

        typedef struct MC_QUATERNIONS_{
            float x;
            float y;
            float z;
            float w;
        }MC_QUATERNIONS;

        typedef struct MC_COORDINATE_{
            float x;
            float y;
            float z;
        }MC_COORDINATE;

        typedef struct MC_MOTION_IMU_DATA_{
            uint64_t timestamp;
            MC_QUATERNIONS orientation;
            double orientation_covariance[9];
            MC_COORDINATE angular_velocity;
            double angular_velocity_covariance[9];
            MC_COORDINATE linear_acceleration;
            double linear_acceleration_covariance[9];
        }MC_MOTION_IMU_DATA;

        typedef struct JOY_KEY_TYPE_{
            bool stand_up;
            int move_mode;
            int speed_gear;
            double dynamic_speed_x;
            double dynamic_speed_y;
            double static_speed_x;
            double static_speed_y;
        }JOY_KEY_TYPE;

        typedef enum ROBOT_DIRECTION_{
            FRONT = 0,
            REAR,
            LEFT,
            RIGHT
        }ROBOT_DIRECTION;

        typedef struct ROBOT_TWIST_{
            MC_COORDINATE linear;
            MC_COORDINATE angular;
        }ROBOT_TWIST;

        //0: Trot（默认）1: 奔跑  2: 跳跃 3: Saturn标志动作
        typedef enum ROBOT_GAIT_{
            TROT = 0,
            RUN,
            JUMP,
            SATURN,
            NIMBLE
        }ROBOT_GAIT;

        //int32 model = 1; //0: 自主导航， 1,遥控控制
        typedef enum ROBOT_CONTROL_MOVE_MODE_{
            NAVIGATION = 0,
            JOY
        }ROBOT_CONTROL_MOVE_MODE;

        typedef enum DRIVER_ENABLE_STATE_{
            UNKNOW = 0,
            ENABLED,
            ENABLING,
            ENABLE_FAILED,
            DISABLED
        }DRIVER_ENABLE_STATE;

        typedef enum EQA_STATE_{
            EQA_NULL = 0,
            LISTEN = 1,
            THINK = 2,
            EXECUTE = 3
        }EQA_STATE;

        //scene = 1;	//0: 平地 , 1: 楼梯 , 2: 沟壑 , 3: 陡坡 , 4: 限高
        typedef enum SCENE_TYPE_{
			NULL_SCENE = 0,
            LIE_DOWN = 1,
            WALKING = 2,
            STAIRS = 3,
            CHARGE = 4,
            PERCEIVE_STAIRS = 5,
            SNOW = 6,
            SLIPPY = 7,
            STONE = 8
        }SCENE_TYPE;

        //机器人场景切换状态枚举
        typedef enum SCENE_SWITCH_STATE_{
			NULL_SCENE_SWITCH = 0,
            SWITCHING = 1,
            SWITCH_SUCCEEDED = 2,
            SWITCH_FAILED = 3
        }SCENE_SWITCH_STATE;

        typedef enum BODY_POSTURE_STATE_{
            NULL_POSTURE = 0,
            STARTMOVING = 1,
            MOVING = 2,
			LIEDOWN = 3,
            STANDUP = 4,
            CHARGING=5  //在桩上  可能是充电，也可能已经充满了
        }BODY_POSTURE_STATE;

        //相机位置  
        typedef enum CHARGE_POSITION_STATE_{
            NULL_POSITION = 0,
            POSITION_OFF = 1,    //找不到位置 默认状态
            POSITION_FINDING = 2,//搜寻位置此时需要用户手动控制机器人进行移动
            POSITION_ONLINE = 3, //位置信息在线
            POSITION_FAILED = 4  //用户调整超时后进入该状态，考虑二维码破损造成的情况
        }CHARGE_POSITION_STATE;
        //上桩过程枚举
        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;

        typedef enum AUTO_CHARGE_STATE_{
            null_charge = 0,
            uncharge = 1,
            charging = 2,
            charge_finished = 3
        }AUTO_CHARGE_STATE;

        typedef struct ROBOT_BATTERY_INFO_{
            float level; //机器人电池当前电量
            AUTO_CHARGE_STATE state; //用来表示机器人电池 没充电 ，充电中，充电结束的状态 
        }ROBOT_BATTERY_INFO;

        typedef struct BODY_ODOMETRY_{
            ROBOT_TWIST twist;
            //geometry_msgs/Pose Point & Quaternion
            MC_COORDINATE point;
            MC_QUATERNIONS quaternion;
            // 6*6 covariance matrix
            // repeated pose = 2;
        }BODY_ODOMETRY;

        //充电相关状态 
        typedef struct ROBOT_CHARGE_STATUS_{ //运动控制主机和手机通讯的内容
            std::string charge_tile_name;           //充电桩唯一标识符 （考虑当前是否需要）
            CHARGE_POSITION_STATE position_state;   //检测到二维码位置时进行进行自动充电模式 反馈到手机
            CHARGE_SWITCH_STATE   charge_switch_state; //上桩过程状态   反馈到手机
            bool charge_succeeded;                  //电池与充电桩连接成功正在充电标志位  该标志位需要BMS与充电桩通信确认
            ROBOT_BATTERY_INFO battery_info;
            bool user_quit_charge_flag;
        }ROBOT_CHARGE_STATUS;

        typedef struct JETSON_CHARGE_STATE_{ //回冲功能上传Jetson的信息
            long heartbeat;                //心跳信息
            bool sensnor;                  //是否打开相机    Jetson 读该信息 
        }JETSON_CHARGE_STATE;

        typedef struct JETSON_CHARGE_COMMAND_{ //Jetson主机下发指令到运动主机的信息
            bool tag_state;				     //检测到二维码    Jetson 写该信息
            MC_QUATERNIONS position;         //充电桩位置信息  Jetson 写该位置
        }JETSON_CHARGE_COMMAND;
        
        //Jetson回冲相关的状态统一放在这里
        typedef struct JETSON_CHARGE_INFO_{ //统一管理接口
            JETSON_CHARGE_STATE charge_state;       //与充电相关的机器人状态
            JETSON_CHARGE_COMMAND charge_command;    //充电相关的位置信息
        }JETSON_CHARGE_INFO;

        //视觉落脚点功能，从运动控制主机上传到Jetson主机所用的数据类型,实机
        typedef struct JETSON_VISUAL_FOOTHOLD_STATE_{
            uint64_t timestamp;
            robot_control::common::BODY_ODOMETRY robot_odometry;
            float touch_detection[LEG_NUMBER];
            float tip_position_wrt_hip[LEG_NUMBER * 3];
            float vf_planned_tip_position_wrt_base[LEG_NUMBER * 3]; //规划出的足端相对机器人base的位置
            bool camera_switch; //是否打开相机
        }JETSON_VISUAL_FOOTHOLD_STATE;

        //视觉落脚点功能，从Jetson主机下发数据到运动控制主机的数据类型
        typedef struct JETSON_VISUAL_FOOTHOLD_COMMAND_{
            uint64_t timestamp;
            float map_center_point[2];
            float body_pos_act[3];
            float body_vel_act[3]; //现在机器人身体实际的速度并没有被用到，临时借用一下作为雷达最新的数据
            float body_pos_est[3];
            float body_vel_est[3];
            float body_orn_act[4];
            
            float body_gyr_act[3];
            float body_orn_est[4];
            float body_gyr_est[3];
            unsigned int neigh_height_map[HEIGHT_MAP_HEIGHT][HEIGHT_MAP_WIDTH];
            unsigned int neigh_roughness_map[HEIGHT_MAP_HEIGHT][HEIGHT_MAP_WIDTH];
            unsigned int neigh_smoothness_map[HEIGHT_MAP_HEIGHT][HEIGHT_MAP_WIDTH];
        }JETSON_VISUAL_FOOTHOLD_COMMAND;

        typedef struct DEPTH_IMAGE_{
            int image[DEPTH_IMAGE_HEIGHT][DEPTH_IMAGE_WIDTH];
        }DEPTH_IMAGE;

        typedef struct RGB_IMAGE_{
            int image[RGB_IMAGE_HEIGHT][RGB_IMAGE_WIDTH];
        }RGB_IMAGE;

        //视觉落脚点相关的状态统一放在这里
        typedef struct JETSON_VISUAL_FOOTHOLD_INFO_{ //统一管理接口
            uint64_t timestamp;
            JETSON_VISUAL_FOOTHOLD_COMMAND vis_foothold_command;  
            float touch_detection[LEG_NUMBER];
            float tip_position_wrt_hip[LEG_NUMBER * 3]; //当前机器人足端相对髋关节坐标系的位置
            float vf_planned_tip_position_wrt_base[LEG_NUMBER * 3]; //规划出的足端相对机器人base的位置
            bool camera_switch; //是否打开相机
        }JETSON_VISUAL_FOOTHOLD_INFO;

        typedef enum _STAIRS_HEIGH_STATE{
            STAIRS_HEIGH_NULL = 0,
            STAIRS_LESS_THAN_16CM = 1,
            STAIRS_LESS_THAN_20CM = 2,
            STAIRS_LESS_THAN_30CM = 3,
        }STAIRS_HEIGH_STATE;

        typedef struct ROBOT_DRIVER_VALUE_{
            double position;
            double velocity;
            double torque;
            double torque_sensor;
        }ROBOT_DRIVER_VALUE;

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

        typedef enum LIE_DOWN_ADJUST_{
            null = 0,
            start_to_adjust = 1,
            adjusting = 2,
            nimble_stop = 3,
            finish = 4
        }LIE_DOWN_ADJUST_;

        //停障功能从navigation端接收到的结果
        //在目前的使用中，当float<=-2，代表存在障碍物，否则代表不存在障碍物
        typedef struct GUARDIAN_VELOCITY_DECAY_RATIO_{
            float left_ratio;
            float front_ratio;
            float right_ratio;
            float rear_ratio;
        }GUARDIAN_VELOCITY_DECAY_RATIO;

        typedef enum CALIBRATE_POSE_STATE_{
            NULL_STATE = 0,
            START_CALIBRATE = 1,
            CALIBRATE_SUCCESS = 2,
            CALIBRATE_FAILED = 3
        }CALIBRATE_POSE_STATE;

        typedef enum TERRAIN_SEG_DETECTION_RESULTS_{
            NULL_TERRAIN = 0,
            WEI_ZHI = 1,
            CAO_DI = 2,
            CI_ZHUAN = 3,
            DI_BAN = 4,
            DI_TAN = 5,
            LI_QING_LU_MIAN = 6,
            LOU_TI = 7,
            LUAN_SHI_DI_MIAN = 8,
            LU_ZHUAN = 9,
            SHUI_NI_DI = 10,
            TAI_JIE = 11
        }TERRAIN_SEG_DETECTION_RESULTS;

        typedef struct ROBOT_COMMON_STATUS_{
            int network; //机器人当前网络状态
            BODY_POSTURE_STATE belie_or_stand; //机器人处于站立趴下状态(目前不用，芳勇认为测试中可能会用)
            bool emergency; //是否是处于紧急状态
            bool avoidance; //自动避障状态
            long int heartbeat; //心跳
            float cur_speed; //当前速度
            float cur_height; //当前高度
            MC_QUATERNIONS position; //机器人世界坐标系下的位置，xyz与yaw角
            float max_height; //机器人最大高度
            float max_speed; //机器人最大速度
            SCENE_SWITCH_STATE cur_scene_switch_state; //切换场景是否成功
            SCENE_TYPE cur_scene; //当前处于的场景,预设站立 预设应该是趴下比较合理
            DRIVER_ENABLE_STATE driver_enable_state;
            //机器人里程计信息
            BODY_ODOMETRY odometry;
            float tip_position_wrt_hip[LEG_NUMBER*3];//足端相对髋关节坐标系位置
            float vf_planned_tip_position_wrt_base[LEG_NUMBER * 3];
            float touch_detection[LEG_NUMBER];//触地检测，范围从0到1.0, 等于1.0为绝对触地，后期会变为概率，但目前还是只有1或者0两种可能
            float target_body_orientation[3]; //raw pitch yaw

            //错误代码
            int error_code;
            bool has_error;
            unsigned int error_wheel[CODE_WHEEL_NUM];
            unsigned int warn_wheel[CODE_WHEEL_NUM];
            unsigned int note_wheel[CODE_WHEEL_NUM];
            unsigned int joint_error_wheel[JOINT_CODE_WHEEL_NUM];
            unsigned int joint_warn_wheel[JOINT_CODE_WHEEL_NUM];
            unsigned int joint_note_wheel[JOINT_CODE_WHEEL_NUM];
            ROBOT_CHARGE_STATUS charge_status;
            bool salute_state;
            NAV_OR_JOY_MODE joy_mode;
            EQA_STATE eqa_state;

            bool guardian_switch; //表示当前停障功能的开关的状态
            bool visual_foothold_switch; //表示当前视觉落脚点功能开关的状态
            GUARDIAN_VELOCITY_DECAY_RATIO guardian_velocity_decay_ratio; //军营用于显示前左右三个方位的障碍

            LIE_DOWN_ADJUST_ lie_down_adjust; //用于判断趴下前是否正在做足端位置调整  
            float speed_bar_on_controller; //机器人遥控器上当前选中的线速度
            CALIBRATE_POSE_STATE calibrate_pos_state;
            int self_test_code;
            int encoder_calibrate_state;

            bool nav_pos_state;//当前导航定位位置是否正常
            bool nav_init_pos;//是否尝试初始化导航定位
            TERRAIN_SEG_DETECTION_RESULTS terrain_seg_result;
            bool uwb_state;
            STAIRS_HEIGH_STATE stairs_state;
            bool weight_load_switch;
            float cur_weight_load;
            bool exceeding_weight_load;
        }ROBOT_COMMON_STATUS;

        typedef enum JOINT_LIMIT_STATE_{
            NULL_LIMIT = 0,
            LIMITED = 1,
            NOLIMITED = 2
        }JOINT_LIMIT_STATE;
        //18个关节的关节状态
        typedef struct ROBOT_JOINT_STATUS_{
            std::string name;
            float current;//电机数据（电流值）
            float joint_temperature;//驱动器温度
            float motor_temperature;//电机温度
            float driver_cpu_temperature;//驱动器cpu温度
            float torque_sensor_data;//扭矩传感器数据
            double position;
            double velocity;
            double effort;
            double max_limit;
            double min_limit;
            JOINT_LIMIT_STATE limit_state;
            unsigned short driver_error_code;
            unsigned short motor_error_code;
            double target_position;
            double target_velocity;
            double target_effort;
        }ROBOT_JOINT_STATUS;

        typedef struct ROBOT_JOINTS_STATUS_{
            ROBOT_JOINT_STATUS joints[MOTOR_NUM];
            long int heartbeat;
        }ROBOT_JOINTS_STATUS;

        typedef struct ROBOT_PER_CYCLE_STATUS_{
            int network;//机器人当前网络状态
            bool lie_stand_state; //joystick连接状态
            bool emergency; //是否是处于紧急状态
            bool avoidance; //自动避障状态
            long int heartbeat; //心跳
            float speed;
            MC_QUATERNIONS position;
        }ROBOT_PER_CYCLE_STATUS;

        typedef struct ROBOT_TARGET_VALUE_{
            ROBOT_DRIVER_VALUE driver[MOTOR_NUM];
            long int heartbeat;
        }ROBOT_TARGET_VALUE;

        typedef enum enFormOfMovement_
        {
            JOINTS_CONTROL = 0,
            MOTION_CONTROL = 1
        }enFormOfMovement;

        typedef struct DRIVER_CONFIG_PARAMETER_{
            std::string motor_name;
            double position_scale;
            double velocity_scale;
            double torque_scale;
            bool motor_direction;
            bool direction_correction;
            int index_map;
            bool be_choosed;
            double init_pos_offset;
            double init_pos_offset_rough;
            double position_min_limit;
            double position_min_safety_limit;
            double position_max_limit;
            double position_max_safety_limit;
            double min_limit_kp;
            double min_limit_ki;
            double min_limit_kd;
            double max_limit_kp;
            double max_limit_ki;
            double max_limit_kd;
            int convert_to_deceleration_end;
            int one_turn_encorder_value;
            double driver_kp_scale;
            double driver_kd_scale;
            double trq_safety_limit;
            double target_torque;
            bool init_pos_check_switch;
            double init_position;
            double init_position_error;
        }DRIVER_CONFIG_PARAMETER;

        typedef struct SENSOR_CONFIG_PARAMETER_{
            double trq_sensor_scale;
            double trq_sensor_offset;
            bool trq_sensor_direction;
            int index_map;
        }SENSOR_CONFIG_PARAMETER;

        typedef struct _JOINTS_CONFIG_INFO{
            robot_control::common::DRIVER_CONFIG_PARAMETER driver_data[MOTOR_NUM];
            robot_control::common::SENSOR_CONFIG_PARAMETER sensor_data[THE_TRQ_SENSOR_NUM];
            bool trq_have_been_calibrated;
            bool joint_position_have_been_calibrated;
            bool encoder_has_been_reseted;
        }JOINTS_CONFIG_INFO;

        typedef struct _ROBOT_SPECIAL_CONFIG_INFO{
            std::string bms_address;
            std::string light_address;
            EN_DRIVER_ECAT_SLAVES_TYPE driver_type;
            EN_TRQ_SENSOR_ECAT_SLAVES_TYPE trq_sensor_type;
            EN_BMS_TYPE bms_type;
            EN_EMOJI_LIGHT_TYPE emoji_light_type;
            EN_LAMP_EFFECT_THEME lamp_effect_theme;
        }ROBOT_SPECIAL_CONFIG_INFO;

        typedef enum _ROBOT_WORKING_ENVIRINMENT{
            ROBOT_BODY_NON = 0,
            ROBOT_BODY_REAL,
            ROBOT_BODY_WEBOTS,
            ROBOT_BODY_GAZEBO
        }ROBOT_WORKING_ENVIRINMENT;

        typedef enum LOG_LEVEL_{
            LOG_LEVEL_NON = 0,
            LOG_LEVEL_DEBUG,
            LOG_LEVEL_NOTE,
            LOG_LEVEL_WARNING,
            LOG_LEVEL_ERROR,
            LOG_LEVEL_ALL,
            LOG_LEVEL_MAX
        }LOG_LEVEL;

        typedef enum LOG_MODULE_NAME_{
            MODULE_NON = 0,
            MODULE_CONTROL_CORE,
            MODULE_ENTRY,
            LOG_MODULE_MAX
        }LOG_MODULE_NAME;

        typedef struct ROBOT_POSE_ESTIMATE_{
            MC_COORDINATE point;
            MC_QUATERNIONS quaternion;
        }ROBOT_POSE_ESTIMATE;
        
        typedef struct BMS_STATE_INFO_{
            float sigle_overvoltage;
            float remaining_capacity;
            float battery_voltage_1th;
            float battery_voltage_2th;
            float battery_voltage_3th;
            float battery_voltage_4th;
            float battery_voltage_5th;
            float battery_voltage_6th;
            float total_voltage;
            float discharging_current;
            float remain_battery_capacity;
            float reconfigurable_system_on_chip;
            float battery_power;
            bool  is_charging;
        }BMS_STATE_INFO;
        //Log config
    } // namespace common
}

#endif