[fix] 更新grpc接口。

saturn_controller/sdk/include/com_define.h

@@ -30,6 +30,10 @@
 
 #define DRIVER_WATCH_DOG_TIMEOUT 3000
 
+#define MAX_ERROR_QUEUE_SIZE 200
+#define CODE_WHEEL_NUM 5
+#define JOINT_CODE_WHEEL_NUM 10
+
 typedef enum _DRIVE_MODE{
     POSITION_MODE = 0,
     VELOCITY_MODE,
@@ -60,14 +64,65 @@ typedef enum _EN_DRIVER_ECAT_SLAVES_TYPE{
 	LENOVO_M_DRIVER_TYPE = 3,
 	MOTOREVO_DRIVER_TYPE = 4,
 	SYNAPTICON_DRIVER_TYPE = 5,
+	LENOVO_S_ECAT_DRIVER_TYPE = 6,
 }EN_DRIVER_ECAT_SLAVES_TYPE;
 
-typedef enum _EN_TRQ_SENSOR_ECAT_SLAVES_TYPE{
+typedef enum _EN_TRQ_SENSOR_ECAT_SLAVES_TYPE {
 	NONE_TRQ_SENSOR_TYPE = 0,
 	SJTU_TRQ_SENSOR_TYPE = 1,
 	YULI_TRQ_SENSOR_TYPE = 2,
 }EN_TRQ_SENSOR_ECAT_SLAVES_TYPE;
 
+typedef enum _EN_BMS_TYPE {
+	NONE_BMS = 0,
+	MERBER,
+	MICRO_BOARD_BMS
+}EN_BMS_TYPE;
+
+typedef enum _EN_EMOJI_LIGHT_TYPE{
+	NONE_LED_TYPE = 0,
+	ONLY_LED_TYPE = 1,
+	EMOJI_LED_TYPE = 2,
+}EN_EMOJI_LIGHT_TYPE;
+
+typedef enum _EN_MICRO_EMOJI_CONTROL_CMD {
+	LED_NULL = 0,
+    EMOJI_CLOSE,  //关闭所有LED
+    EMOJI_OFF,  //关机
+    EMOJI_ON,  //开机
+    EMOJI_RUN,  //正常运行
+    EMOJI_SMILE,  //笑脸
+    EMOJI_CRY,  //哭泣
+    EMOJI_FOLLOW,  //跟随
+    LOW_BATTERY,  //低电量
+    LED_CHARGING,  //充电中(>50%)
+    LED_LOWCHARGING,  //充电中(<49%)
+    LED_CHARGE_DONE  //充电完成
+}EN_MICRO_EMOJI_CONTROL_CMD;
+
+typedef enum _EN_MICRO_BMS_CMD {
+	BMS_VOLTAGE,  // 2Byte 获取电压（单位10mv）
+	BMS_CURRENT,  // 2Byte 获取电流（单位10mA）充电为正，放电为负
+	BMS_SOC,  // 2Byte 剩余容量（单位10mAh）
+	BMS_FCC,  // 2Byte 实际充满容量（单位10mAh）
+	BMS_RSOC,  // 1Byte 剩余容量百分比
+	BMS_TEMP,  // 1Byte 电池温度（单位度）
+	BMS_STATE,  // 2Byte 保护状态
+	BMS_INFO, //45Byte BMS完整信息
+	BMS_CMD0,  // 解除软件关闭MOS管动作
+	BMS_CMD1,  // 软件关闭充电MOS, 解除软件关闭放电MOS
+	BMS_CMD2,  // 软件关闭放电MOS,解除软件关闭充电MOS
+	BMS_CMD3,  // 软件同时关闭充放电MOS
+	BMS_CHARGING, //充电指令，成功，0xff01dd, 失败 0xff02dd 充电完成0xff03dd  总共6Byte
+	BMS_DISCHARGING //关闭充电 成功0xff01dd 失败0xff02dd 总共6Byte
+}EN_MICRO_BMS_CMD;
+
+typedef enum _EN_LAMP_EFFECT_THEME{
+	NONE_LAMP_THEME = 0,
+	SNOW_BLACK_THEME = 1,
+	COLD_GREY_THEME = 2,
+}EN_LAMP_EFFECT_THEME;
+
 typedef enum _ROBORT_BODY_MODEL{
 	GS_SYNAPTICON = 0,
 	IS_FORCE_POS_MIXING = 1,

saturn_controller/sdk/include/common.h

@@ -274,6 +274,13 @@ namespace robot_control
             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,
@@ -281,7 +288,10 @@ namespace robot_control
             WALKING = 2,
             STAIRS = 3,
             CHARGE = 4,
-            PERCEIVE_STAIRS = 5
+            PERCEIVE_STAIRS = 5,
+            SNOW = 6,
+            SLIPPY = 7,
+            STONE = 8
         }SCENE_TYPE;
 
         //机器人场景切换状态枚举
@@ -414,6 +424,13 @@ namespace robot_control
             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;
@@ -451,6 +468,23 @@ namespace robot_control
             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; //机器人处于站立趴下状态(目前不用，芳勇认为测试中可能会用)
@@ -470,20 +504,37 @@ namespace robot_control
             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; //军营用于显示前左右三个方位的障碍 (芳哥，这是新加接口)
+            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;
+            bool 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;
         }ROBOT_COMMON_STATUS;
 
         typedef enum JOINT_LIMIT_STATE_{
@@ -495,7 +546,9 @@ namespace robot_control
         typedef struct ROBOT_JOINT_STATUS_{
             std::string name;
             float current;//电机数据（电流值）
-            float joint_temperature;//关节温度
+            float joint_temperature;//驱动器温度
+            float motor_temperature;//电机温度
+            float driver_cpu_temperature;//驱动器cpu温度
             float torque_sensor_data;//扭矩传感器数据
             double position;
             double velocity;
@@ -504,6 +557,7 @@ namespace robot_control
             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;
@@ -562,6 +616,9 @@ namespace robot_control
             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_{
@@ -575,6 +632,7 @@ namespace robot_control
             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;
         }JOINTS_CONFIG_INFO;
 
         typedef struct _ROBOT_SPECIAL_CONFIG_INFO{
@@ -582,6 +640,9 @@ namespace robot_control
             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_BODY_TYPE{
@@ -612,6 +673,23 @@ namespace robot_control
             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
 }

saturn_controller/sdk/include/error_code.h

@@ -0,0 +1,83 @@
+#ifndef ERROR_CODE__H
+#define ERROR_CODE__H
+
+//error
+#define RC_ERROR_INDEX_BEYOND_MIN_CODE                  0x00000001
+#define RC_ERROR_INDEX_BEYOND_MAX_CODE                  0x00000002
+#define RC_ERROR_DRIVER_ENABLE_FAILED_CODE              0x00000003
+#define RC_ERROR_IMU_DATA_IS_NOT_CORRECT_CODE           0x00000004
+#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_MAX_INDEX_CODE                         (CODE_WHEEL_NUM * 32)
+
+
+#define RC_ERROR_INDEX_BEYOND_MIN_MSG  "The Error index value beyond the min limit."
+#define RC_ERROR_INDEX_BEYOND_MAX_MSG  "The Error index value beyond the max limit."
+#define RC_ERROR_DRIVER_ENABLE_FAILED_MSG   "The driver enable failed."
+#define RC_ERROR_IMU_DATA_IS_NOT_CORRECT_MSG    "The IMU data is not correct. Please check IMU."
+#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_JOINT_ERROR_CLEAR_7303_FAILED_CODE          0x00000001
+#define RC_JOINT_ERROR_STATUS_WORD_ERROR_CODE          0x00000002
+#define RC_JOINT_ERROR_OVER_VOLTAGE_CODE               0x00000003 
+#define RC_JOINT_ERROR_STO_DISABLE_CODE                0x00000004
+#define RC_JOINT_ERROR_POS_BEYOND_LIMIT_CODE           0x00000005
+#define RC_JOINT_ERROR_VEL_BEYOND_LIMIT_CODE           0x00000006
+#define RC_JOINT_ERROR_FORCE_BEYOND_LIMIT_CODE         0x00000007
+#define RC_JOINT_ERROR_POS_BEYOND_A_LOOP_CODE          0x00000008
+#define RC_JOINT_ERROR_POSITION_GAP_TOO_BIG_CODE       0x00000009
+#define RC_JOINT_ERROR_TRQ_SENSOR_IS_NOT_CORRECT_CODE  0x0000000A
+#define RC_JOINT_ERROR_ENCODER_BISERBIT_CODE           0x0000000B
+#define RC_JOINT_ERROR_ENCODER_BISWNBIT_CODE           0x0000000C
+#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_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."
+#define RC_JOINT_ERROR_STATUS_WORD_ERROR_MSG    "The Driver has error, the status word is error.Maybe is 7303, but not sure. Please check log."
+#define RC_JOINT_ERROR_OVER_VOLTAGE_MSG    "The driver's valtage is too high."
+#define RC_JOINT_ERROR_STO_DISABLE_MSG     "Maybe the driver's STO button is disable. Please check and release it."
+#define RC_JOINT_ERROR_POS_BEYOND_LIMIT_MSG    "The driver's position is beyond the limit."
+#define RC_JOINT_ERROR_VEL_BEYOND_LIMIT_MSG    "The driver's velocity is beyond the limit."
+#define RC_JOINT_ERROR_FORCE_BEYOND_LIMIT_MSG    "The driver's force is beyond the limit."
+#define RC_JOINT_ERROR_POS_BEYOND_A_LOOP_MSG    "Driver position beyond a loop."
+#define RC_JOINT_ERROR_POSITION_GAP_TOO_BIG_MSG    "The driver's position occurred a huge springboard."
+#define RC_JOINT_ERROR_TRQ_SENSOR_IS_NOT_CORRECT_MSG    "The driver's trq sensor is not correct."
+#define RC_JOINT_ERROR_ENCODER_BISERBIT_MSG       "7303 error BisErBit."
+#define RC_JOINT_ERROR_ENCODER_BISWNBIT_MSG       "7303 error BisWnBit."
+#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."
+
+
+//warnning
+#define RC_WARN_MASTER_DATA_PACKET_LOSS_CODE            0x00000001
+#define RC_WARN_BATTARY_LEVEL_IS_TOO_LOW_CODE           0x00000002
+#define RC_WARN_CODE_CUR_MAX_INDEX                      0x00000003
+#define RC_WARN_MAX_INDEX_CODE                          (CODE_WHEEL_NUM * 32)
+
+
+#define RC_WARN_MASTER_DATA_PACKET_LOSS_MSG         "The EtherCat Master's data packet lossed."
+#define RC_WARN_BATTARY_LEVEL_IS_TOO_LOW_MSG        "The Robot's battary is too low, please charge!"
+
+#define RC_JOINT_WARN_CODE_CUR_MAX_INDEX                      0x00000001
+#define RC_JOINT_WARN_MAX_INDEX_CODE                          (JOINT_CODE_WHEEL_NUM * 32)
+
+//note
+#define RC_NOTE_DRIVER_TEMPERATURE_TOO_HIGH_CODE        0x00000001
+#define RC_NOTE_CODE_CUR_MAX_INDEX                      0x00000002
+#define RC_NOTE_MAX_INDEX_CODE                          (CODE_WHEEL_NUM * 32)
+
+#define RC_NOTE_DRIVER_TEMPERATURE_TOO_HIGH_MSG         "The driver's temperature is too high."
+
+#define RC_JOINT_NOTE_CODE_CUR_MAX_INDEX                      0x00000001
+#define RC_JOINT_NOTE_MAX_INDEX_CODE                          (JOINT_CODE_WHEEL_NUM * 32)
+
+#endif

saturn_controller/sdk/include/mc_client_interface.h

@@ -76,7 +76,7 @@ bool rcClientInterfaceAutoRechargeCmd(
 bool rcClientInterfaceGetChargeState(common::JETSON_CHARGE_STATE *charge_state);
 bool rcClientInterfaceLightStripControl(
             robot_control::common::NAV_OR_JOY_MODE from,
-            std::string port, int cmd, int deviceAddr, int data);
+            std::string port, EN_MICRO_EMOJI_CONTROL_CMD cmd, int deviceAddr, int data);
 bool rcClientInterfaceSetGuardian(
 			common::NAV_OR_JOY_MODE from,
 			float velocity_decay_ratio,
@@ -96,6 +96,33 @@ bool rcClientInterfaceGetDepthImage(
 bool rcClientInterfaceGetRgbImage(
 			common::RGB_IMAGE *rgb_image);
 bool rcClientInterfaceGetMotionControllerTimestamp(unsigned long *timestamp);
+bool rcClientInterfaceSetNavigationPositioningState(
+			common::NAV_OR_JOY_MODE from,
+			bool enable);
+bool rcClientInterfaceSetNavigationInitialPos(
+			common::NAV_OR_JOY_MODE from,
+			bool enable);
+bool rcClientInterfaceSetTerrainSegDetectionResults(
+			common::NAV_OR_JOY_MODE from,
+			common::TERRAIN_SEG_DETECTION_RESULTS terrain);
+bool rcClientInterfaceSetTargetBodyOrientation(
+			common::NAV_OR_JOY_MODE from,
+			float roll, float pitch, float yaw);
+bool rcClientInterfaceSetEQAState(
+			common::NAV_OR_JOY_MODE from,
+			common::EQA_STATE eqa_state);
+bool rcClientInterfaceSetStairsHeigh(
+			common::NAV_OR_JOY_MODE from,
+			common::STAIRS_HEIGH_STATE stairs_heigh_value);
+bool rcClientInterfaceSetUWBFunctionSwitch(
+			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);
+bool rcClientInterfaceGetNavigationPositioningState(bool* state);
+bool rcClientInterfaceGetNavigationInitialPos(bool* init);
+bool rcClientInterfaceGetEQAState(robot_control::common::EQA_STATE *eqa_state);
 bool rcClientInterfaceDemoControl(
 			common::NAV_OR_JOY_MODE from,
 			int mode,
@@ -117,6 +144,7 @@ bool rcClientInterfaceReCalibrateAllDriverAndSave(bool is_rough);
 bool rcClientInterfaceSetJointTestParameter (common::JointTestData joint_test_data);
 bool rcClientInterfaceSineMoveJointTestSwitch (bool be_enable);
 bool rcClientInterfaceQuitJointTest ();
+bool rcClientInterfaceClearErrorHistory(common::NAV_OR_JOY_MODE from);
 
 // }
 #endif // MC_CLIENT_INTERFACE_H

saturn_controller/src/saturn_controller.cpp

@@ -4,29 +4,6 @@ namespace saturn_ros2
     SaturnController::SaturnController(const rclcpp::NodeOptions &options)
         : node_(std::make_shared<rclcpp::Node>("saturn_controller", options))
     {
-
-        /*
-        // spdlog config
-        spdlog::init_thread_pool(8192, 1);
-        // 10M * 3 log files
-        auto rotate_file_sink =
-            std::make_shared<spdlog::sinks::rotating_file_sink_mt>(
-                "/root/logs/main_entry.spdlogger.log", 1024 * 1024 * 10, 3, false);
-        // log file without color
-        rotate_file_sink->set_pattern("[%D %T:%e %s:%#] [%L] %v");
-
-        auto std_color_sink = std::make_shared<spdlog::sinks::stdout_color_sink_mt>(
-            spdlog::color_mode::always);
-        std_color_sink->set_pattern("[%D %T:%e %s:%#] %^[%L] %v %$");
-        std::vector<spdlog::sink_ptr> sinks{rotate_file_sink, std_color_sink};
-        auto async_logger = std::make_shared<spdlog::async_logger>(
-            "async_logger", sinks.begin(), sinks.end(), spdlog::thread_pool(),
-            spdlog::async_overflow_policy::overrun_oldest);
-
-        spdlog::set_default_logger(async_logger);
-        spdlog::flush_every(std::chrono::seconds(5));
-        */
-
         SPDLOG_INFO("====== SaturnController created ======");
         config_handler_ = std::make_shared<ConfigHandler>(node_);
         init();
@@ -66,13 +43,6 @@ namespace saturn_ros2
             std::chrono::nanoseconds((int)(1e9 / config_handler_->imu_timer_frequency_)),
             std::bind(&SaturnController::imuTimerCallback, this), imu_timer_cbg_);
 
-        // leg_odom_timer_cbg_ =
-        //     node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
-
-        // legged_odom_timer_ = node_->create_wall_timer(
-        //     std::chrono::nanoseconds((int)(1e9 / config_handler_->legged_odom_timer_frequency_)),
-        //     std::bind(&SaturnController::leggedOodmTimerCallback, this), leg_odom_timer_cbg_);
-
         // subscription
         cmd_vel_callback_cg_ =
             node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
@@ -279,7 +249,6 @@ namespace saturn_ros2
         if (request->velocity_decay_ratio <= 0)
         {
             this->guardian_velocity_decay_ratio_ = 0.0;
-
             // 遥控器上增加显示
             std::string trigger_source = "/front_obs_points";
             float velocity_decay_ratio = -2;
@@ -292,7 +261,6 @@ namespace saturn_ros2
         }
         else
         {
-
             // 遥控器上增加显示
             std::string trigger_source = "/front_obs_points";
             float velocity_decay_ratio = 1;