Browse Source

[feat]更新v2.2-dev grpc接口。

zourd1 8 months ago
parent
commit
60c7dbf8d4

+ 2 - 2
saturn_controller/sdk/include/com_define.h

@@ -17,8 +17,8 @@
 #define LEG_NUMBER 6
 
 //视觉落脚点
-#define HEIGHT_MAP_HEIGHT 60 //改成80
-#define HEIGHT_MAP_WIDTH 60 //改成80
+#define HEIGHT_MAP_HEIGHT 30 //改成80
+#define HEIGHT_MAP_WIDTH 30 //改成80
 
 #define GRID_SIZE 0.04
 

+ 7 - 4
saturn_controller/sdk/include/common.h

@@ -389,7 +389,6 @@ namespace robot_control
 
         //视觉落脚点功能,从Jetson主机下发数据到运动控制主机的数据类型
         typedef struct JETSON_VISUAL_FOOTHOLD_COMMAND_{
-            float height_map[HEIGHT_MAP_HEIGHT][HEIGHT_MAP_WIDTH];
             uint64_t timestamp;
             float map_center_point[2];
             float body_pos_act[3];
@@ -401,9 +400,9 @@ namespace robot_control
             float body_gyr_act[3];
             float body_orn_est[4];
             float body_gyr_est[3];
-            float neigh_height_map[HEIGHT_MAP_HEIGHT][HEIGHT_MAP_WIDTH];
-            float neigh_confidence_map[HEIGHT_MAP_HEIGHT][HEIGHT_MAP_WIDTH];
-            float neigh_roughness_map[HEIGHT_MAP_HEIGHT][HEIGHT_MAP_WIDTH];
+            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_{
@@ -535,6 +534,9 @@ namespace robot_control
             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_{
@@ -633,6 +635,7 @@ namespace robot_control
             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{

+ 12 - 2
saturn_controller/sdk/include/error_code.h

@@ -9,7 +9,8 @@
 #define RC_ERROR_DRIVER_BEYOND_LIMIT_CAN_NOT_ENABLE_CODE        0x00000005
 #define RC_ERROR_DRIVER_NEED_POS_CALIBRATE_CODE         0x00000006
 #define RC_ERROR_BATTARY_LEVEL_IS_TOO_LOW_CODE          0x00000007
-#define RC_ERROR_CODE_CUR_MAX_INDEX                     0x00000008
+#define RC_ERROR_DRIVER_ENCODER_HAS_BEEN_RESET_CODE          0x00000008
+#define RC_ERROR_CODE_CUR_MAX_INDEX                     0x00000009
 
 #define RC_ERROR_MAX_INDEX_CODE                         (CODE_WHEEL_NUM * 32)
 
@@ -21,6 +22,8 @@
 #define RC_ERROR_DRIVER_BEYOND_LIMIT_CAN_NOT_ENABLE_MSG "The joint is beyond limit, so can't enable. please rotate the joint."
 #define RC_ERROR_DRIVER_NEED_POS_CALIBRATE_MSG  "The driver's position need calibrate after power on."
 #define RC_ERROR_BATTARY_LEVEL_IS_TOO_LOW_MSG        "The Robot's battary is too low, must estop!"
+#define RC_ERROR_DRIVER_ENCODER_HAS_BEEN_RESET_MSG        "The Dirver's Encoder has been reset. You need recalibrate the robot joint position!"
+
 
 #define RC_JOINT_ERROR_CLEAR_7303_FAILED_CODE          0x00000001
 #define RC_JOINT_ERROR_STATUS_WORD_ERROR_CODE          0x00000002
@@ -37,7 +40,10 @@
 #define RC_JOINT_ERROR_ENCODER_BATTERY_CODE            0x0000000D
 #define RC_JOINT_ERROR_MOTOR_FAULT_OCCURED_CODE            0x0000000E
 #define RC_JOINT_ERROR_DRIVER_FAULT_OCCURED_CODE            0x0000000F
-#define RC_JOINT_ERROR_CODE_CUR_MAX_INDEX               0x00000010
+#define RC_JOINT_ERROR_DRIVER_CPU_TEMPERATURE_TOO_HEIGH_CODE        0x00000010
+#define RC_JOINT_ERROR_DRIVER_TEMPERATURE_TOO_HEIGH_CODE            0x00000011
+#define RC_JOINT_ERROR_MOTOR_TEMPERATURE_TOO_HEIGH_CODE             0x00000012
+#define RC_JOINT_ERROR_CODE_CUR_MAX_INDEX               0x00000013
 #define RC_JOINT_ERROR_MAX_INDEX_CODE                         (JOINT_CODE_WHEEL_NUM * 32)
 
 #define RC_JOINT_ERROR_CLEAR_7303_FAILED_MSG  "Clear driver's 7303 error, calibrate encode vlaue failed."
@@ -55,6 +61,10 @@
 #define RC_JOINT_ERROR_ENCODER_BATTERY_MSG       "Driver encoder battery 7303 error BisWnBit."
 #define RC_JOINT_ERROR_MOTOR_FAULT_OCCURED_MSG      "The motor has error. Please stop Robot."
 #define RC_JOINT_ERROR_DRIVER_FAULT_OCCURED_MSG      "The driver has error. Please stop Robot."
+#define RC_JOINT_ERROR_DRIVER_CPU_TEMPERATURE_TOO_HEIGH_MSG      "The driver's cpu temperature is too heigh. Please stop Robot."
+#define RC_JOINT_ERROR_DRIVER_TEMPERATURE_TOO_HEIGH_MSG      "The driver's temperature is too heigh. Please stop Robot."
+#define RC_JOINT_ERROR_MOTOR_TEMPERATURE_TOO_HEIGH_MSG      "The motor's temperature is too heigh. Please stop Robot."
+
 
 
 //warnning

+ 3 - 0
saturn_controller/sdk/include/mc_client_interface.h

@@ -117,6 +117,9 @@ bool rcClientInterfaceSetStairsHeigh(
 bool rcClientInterfaceSetUWBFunctionSwitch(
 			common::NAV_OR_JOY_MODE from,
 			bool enable);
+bool rcClientInterfaceSetWeightLoadAdaptive(
+			common::NAV_OR_JOY_MODE from,
+			bool enable);
 ////////////////////////////
 bool rcClientInterfaceGetLatestError(std::string* error_code);
 bool rcClientInterfaceGetHistoryError(int* error_count, std::vector<std::string>& error_codes);

BIN
saturn_controller/sdk/lib/arm/libmc_client.so


BIN
saturn_controller/sdk/lib/x86_64/libmc_client.so