[fix]1.解决IS高度调整时没有做型号适配的问题。[feat]增加docking state支持。

saturn_controller/README.md

@@ -11,7 +11,11 @@ ros2 service call /GS_003/robot_command ysc_robot_msgs/srv/RobotCommand "comamnd
 ros2 service call /GS_003/robot_command ysc_robot_msgs/srv/RobotCommand "comamnd_name: 'StairsGait'"
 
 ## 切换到平地步态
-ros2 service call /GS_003/robot_command ysc_robot_msgs/srv/RobotCommand "comamnd_name: 'OrdinaryGait'"
+ros2 service call /GS_005/robot_command ysc_robot_msgs/srv/RobotCommand "comamnd_name: 'OrdinaryGait'"
+
+ros2 service call /GS_005/robot_command ysc_robot_msgs/srv/RobotCommand "comamnd_name: 'StayNormal'"
+
+ros2 service call /GS_005/robot_command ysc_robot_msgs/srv/RobotCommand "comamnd_name: 'StayDown'"
 
 ## 发布速度指令
 ros2 topic pub /GS_003/cmd_vel geometry_msgs/msg/Twist "linear:

saturn_controller/include/saturn_controller/saturn_controller.hpp

@@ -129,11 +129,11 @@ class SaturnController {
     long int robot_heartbeat_ = 0;
     float guardian_velocity_decay_ratio_ = 1.0;
 
-    bool is_getting_com_success = true;
-    bool is_getting_imu_success = true;
-    bool is_getting_vf_success = true;
+    bool is_getting_com_success_ = true;
+    bool is_getting_imu_success_ = true;
+    bool is_getting_vf_success_ = true;
 
-    int reconnect_count = 0;
+    int reconnect_count_ = 0;
 
     void init();
 

saturn_controller/src/saturn_controller.cpp

@@ -191,11 +191,13 @@ void SaturnController::robotCommandCallback(
         result = rcClientInterfaceSetScene(robot_control::common::NAV_OR_JOY_MODE::nav_control,
                                             common::SCENE_TYPE::WALKING);
     } else if("StayNormal" == request->comamnd_name) {
-        SPDLOG_INFO("Standing Height StayNormal is called, standing height of saturn is set to 0.4.");
-        result = rcClientInterfaceBodyHighAdjust(robot_control::common::NAV_OR_JOY_MODE::nav_control,40);
+        int height = (int)(common_status_data_.max_height * 0.8 * 100);
+        SPDLOG_INFO("Standing Height StayNormal is called, standing height of saturn is set to {} cm", height);
+        result = rcClientInterfaceBodyHighAdjust(robot_control::common::NAV_OR_JOY_MODE::nav_control,height);
     } else if("StayDown" == request->comamnd_name) {
-        SPDLOG_INFO("Standing Height StayDown is called, standing height of saturn is set to 0.3.");
-        result = rcClientInterfaceBodyHighAdjust(robot_control::common::NAV_OR_JOY_MODE::nav_control,30);
+        int height = (int)(common_status_data_.max_height * 0.6 * 100);
+        SPDLOG_INFO("Standing Height StayDown is called, standing height of saturn is set to {} cm", height);
+        result = rcClientInterfaceBodyHighAdjust(robot_control::common::NAV_OR_JOY_MODE::nav_control,height);
     } else if("SlopeGait" == request->comamnd_name) {
         SPDLOG_WARN("SlopeGait is not supported yet.");
         result = false;
@@ -231,7 +233,7 @@ void SaturnController::guardianCallback(
 void SaturnController::guardianServiceCallback(
     const daystar_navigation_msgs::srv::Guardian::Request::SharedPtr request,
     daystar_navigation_msgs::srv::Guardian::Response::SharedPtr response) {
-    SPDLOG_WARN("Guardian service is called, velocity_decay_ratio: %f", request->velocity_decay_ratio);
+    SPDLOG_WARN("Guardian service is called, velocity_decay_ratio: {}",request->velocity_decay_ratio);
     if (request->velocity_decay_ratio <= 0) {
         this->guardian_velocity_decay_ratio_ = 0.0;
 
@@ -262,8 +264,8 @@ void SaturnController::taskModeCallback(const std_msgs::msg::Bool::SharedPtr tas
 
 void SaturnController::mainTimerCallback() {
     bool result = rcClientInterfaceGetCommonStatus(&common_status_data_);
-    if(is_getting_com_success == true && result == false){
-        is_getting_com_success = false;
+    if(is_getting_com_success_ == true && result == false){
+        is_getting_com_success_ = false;
         SPDLOG_INFO("rcClientInterfaceGetCommonStatus starts to fail.");
 
         //重新init
@@ -272,12 +274,12 @@ void SaturnController::mainTimerCallback() {
                         config_handler_->motion_computer_ip_address_.c_str());
         }
 
-    } else if(is_getting_com_success == false && result == true){
-        is_getting_com_success = true;
+    } else if(is_getting_com_success_ == false && result == true){
+        is_getting_com_success_ = true;
         SPDLOG_INFO("rcClientInterfaceGetCommonStatus starts to succeed.");
-    }  else if(is_getting_com_success == false && result == false){
-        reconnect_count ++;
-        if(reconnect_count % 500 == 0) {
+    }  else if(is_getting_com_success_ == false && result == false){
+        reconnect_count_ ++;
+        if(reconnect_count_ % 500 == 0) {
             if (rcClientInterfaceInit(config_handler_->motion_computer_ip_address_) == false) {
                 SPDLOG_INFO("rcClientInterfaceInit failed with %s",
                             config_handler_->motion_computer_ip_address_.c_str());
@@ -299,8 +301,8 @@ void SaturnController::mainTimerCallback() {
 void SaturnController::imuTimerCallback() {
     auto imu_message = sensor_msgs::msg::Imu();
     bool result = rcClientInterfaceGetImuData(&imu_data_);
-    if(is_getting_imu_success == true && result == false){
-        is_getting_imu_success = false;
+    if(is_getting_imu_success_ == true && result == false){
+        is_getting_imu_success_ = false;
         SPDLOG_INFO("rcClientInterfaceGetImuData starts to fail.");
 
         //重新init
@@ -309,12 +311,12 @@ void SaturnController::imuTimerCallback() {
                         config_handler_->motion_computer_ip_address_.c_str());
         }
 
-    } else if(is_getting_imu_success == false && result == true){
-        is_getting_imu_success = true;
+    } else if(is_getting_imu_success_ == false && result == true){
+        is_getting_imu_success_ = true;
         SPDLOG_INFO("rcClientInterfaceGetImuData starts to succeed.");
-    }  else if(is_getting_imu_success == false && result == false){
-        reconnect_count ++;
-        if(reconnect_count % 500 == 0) {
+    }  else if(is_getting_imu_success_ == false && result == false){
+        reconnect_count_ ++;
+        if(reconnect_count_ % 500 == 0) {
             if (rcClientInterfaceInit(config_handler_->motion_computer_ip_address_) == false) {
                 SPDLOG_INFO("rcClientInterfaceInit failed with %s",
                             config_handler_->motion_computer_ip_address_.c_str());
@@ -348,45 +350,7 @@ void SaturnController::imuTimerCallback() {
 }
 
 void SaturnController::leggedOodmTimerCallback() {
-
     //先把足式里程计放在主callback里发布，如果对不同频率有需求，再重新使用这里
-
-    // int64_t leg_odom_start_time = std::chrono::time_point_cast<std::chrono::nanoseconds> \
-    //         (std::chrono::high_resolution_clock::now()).time_since_epoch().count();
-    // bool result = rcClientInterfaceGetVisualFootholdState(&foothold_state_data_);
-    // int64_t leg_odom_end_time = std::chrono::time_point_cast<std::chrono::nanoseconds> \
-    //         (std::chrono::high_resolution_clock::now()).time_since_epoch().count();
-
-    // if(is_getting_vf_success == true && result == false){
-    //     is_getting_vf_success = false;
-    //     SPDLOG_INFO("rcClientInterfaceGetVisualFootholdState starts to fail. Tried to reconnect.");
-
-    // } else if(is_getting_vf_success == false && result == true){
-    //     is_getting_vf_success = true;
-    //     SPDLOG_INFO("rcClientInterfaceGetVisualFootholdState starts to succeed.");
-    // } else if(is_getting_vf_success == false && result == false){
-    //     reconnect_count ++;
-    //     if(reconnect_count % 500 == 0) {
-    //         if (rcClientInterfaceInit(config_handler_->motion_computer_ip_address_) == false) {
-    //             SPDLOG_INFO("rcClientInterfaceInit failed with %s",
-    //                         config_handler_->motion_computer_ip_address_.c_str());
-    //         }
-    //     }
-    // }
-
-    // auto odometry_data = nav_msgs::msg::Odometry();
-    // // odometry_data.header.stamp = rclcpp::Clock().now();
-    // int sec = foothold_state_data_.timestamp / 1000000000;
-    // int nanosec = foothold_state_data_.timestamp % 1000000000;
-    // odometry_data.header.stamp.sec = sec;
-    // odometry_data.header.stamp.nanosec = nanosec;
-
-    // odometry_data.header.frame_id = "odom";
-    // odometry_data.child_frame_id = "base_link";
-    // odometry_data.pose.pose.position.x = foothold_state_data_.robot_odometry.point.x;
-    // odometry_data.pose.pose.position.y = foothold_state_data_.robot_odometry.point.y;
-    // odometry_data.pose.pose.position.z = foothold_state_data_.robot_odometry.point.z;
-    // legged_odom_publisher_->publish(odometry_data);
 }
 
 void SaturnController::publishRobotState() {  // todo 这里逻辑还不完善
@@ -401,21 +365,26 @@ void SaturnController::publishRobotState() {  // todo 这里逻辑还不完善
     } else {
         robot_state.data = 7;
     }
-    // SPDLOG_INFO("publishRobotState {} robot_state {} ",
-    //             common_status_data_.belie_or_stand, robot_state);
     robot_state_publisher_->publish(robot_state);
 }
 
-void SaturnController::publishDockingState() {  //临时逻辑 todo
+void SaturnController::publishDockingState() {
     auto pub_dock_state = std_msgs::msg::UInt32();
     if (common_status_data_.charge_status.charge_switch_state ==
+        robot_control::common::CHARGE_SWITCH_STATE::NULL_CHARGE_SWITCH) {
+        pub_dock_state.data = 0;
+    } else if (common_status_data_.charge_status.charge_switch_state ==
+        robot_control::common::CHARGE_SWITCH_STATE::ENTERING_TILE) {
+        pub_dock_state.data = 1;
+    } else if (common_status_data_.charge_status.charge_switch_state ==
         robot_control::common::CHARGE_SWITCH_STATE::ENTER_SUCCEEDED) {
         pub_dock_state.data = 4;
-    } else if ((common_status_data_.charge_status.charge_switch_state ==
-                robot_control::common::CHARGE_SWITCH_STATE::ENTER_FAILED) ||
-               ((common_status_data_.charge_status.charge_switch_state ==
-                 robot_control::common::CHARGE_SWITCH_STATE::EXIT_SUCCEEDED))) {
-        pub_dock_state.data = 0;
+    } else if (common_status_data_.charge_status.charge_switch_state ==
+        robot_control::common::CHARGE_SWITCH_STATE::EXITING_TILE) {
+        pub_dock_state.data = 6;
+    } else if (common_status_data_.charge_status.charge_switch_state ==
+        robot_control::common::CHARGE_SWITCH_STATE::EXIT_SUCCEEDED) {
+        pub_dock_state.data = 7;
     }
     docking_state_publisher_->publish(pub_dock_state);
 }