refactor: 将spdlog改为rclcpp

saturn_controller/src/saturn_controller.cpp

@@ -5,7 +5,7 @@ namespace saturn_ros2 {
 SaturnController::SaturnController(const rclcpp::NodeOptions &options)
     : node_(std::make_shared<rclcpp::Node>("saturn_controller", options)),
       guardian_velocity_decay_ratio_(2.0) {
-  SPDLOG_INFO("====== SaturnController created ======");
+  RCLCPP_INFO(node_->get_logger(), "====== SaturnController created ======");
   config_handler_ = std::make_shared<ConfigHandler>(node_);
   init();
 }
@@ -19,16 +19,10 @@ void SaturnController::init() {
   // 初始化 grpc
   if (rcClientInterfaceInit(config_handler_->motion_computer_ip_address_) ==
       false) {
-    SPDLOG_INFO("rcClientInterfaceInit failed with %s",
+    RCLCPP_ERROR(node_->get_logger(), "rcClientInterfaceInit failed with %s",
                 config_handler_->motion_computer_ip_address_.c_str());
   }
 
-  SPDLOG_INFO("main_timer_frequency_ {} !!!",
-              config_handler_->main_timer_frequency_);
-  SPDLOG_INFO("imu_timer_frequency_ {} !!!",
-              config_handler_->imu_timer_frequency_);
-  SPDLOG_INFO("legged_odom_timer_frequency_ {} !!!",
-              config_handler_->legged_odom_timer_frequency_);
 
   // timer
   main_timer_cbg_ = node_->create_callback_group(
@@ -144,8 +138,9 @@ void SaturnController::cmdVelCallback(const Twist::ConstSharedPtr vel_msg) {
                                           ? 0.0f
                                           : nominal_velocity_decay_ratio;
   if (std::fabs(actual_velocity_decay_ratio) < 1e-4)
-    SPDLOG_INFO("Guardian triggered, ignoring cmd_vel.x... ");
-  rc_direct.linear.x = vel_msg->linear.x * actual_velocity_decay_ratio;  // todo
+    RCLCPP_INFO_THROTTLE(node_->get_logger(), *node_->get_clock(), 2000,
+                         "Guardian triggered, ignoring cmd_vel.x... ");
+  rc_direct.linear.x = vel_msg->linear.x * actual_velocity_decay_ratio; // todo
   rc_direct.linear.y = vel_msg->linear.y;
   rc_direct.linear.z = vel_msg->linear.z;
   rc_direct.angular.x = vel_msg->angular.x;
@@ -162,7 +157,7 @@ void SaturnController::robotCommandCallback(
   bool result = false;
   std::string command = request->comamnd_name;
 
-  SPDLOG_INFO("Received command: {}", command.c_str());
+  RCLCPP_INFO(node_->get_logger(), "Received command: %s", command.c_str());
 
   if ("GoToDock" == request->comamnd_name) {
     response->result = goToDock();
@@ -183,7 +178,7 @@ void SaturnController::robotCommandCallback(
         robot_control::common::CONTROL_SOURCE_MODE::nav_control, rc_direct,
         robot_heartbeat_);
   } else if ("StandUpDown" == request->comamnd_name) {
-    SPDLOG_INFO("stand or lie {}", common_status_data_.belie_or_stand);
+    RCLCPP_INFO(node_->get_logger(), "current stand or lie: %d", common_status_data_.belie_or_stand);
     if (common_status_data_.belie_or_stand ==
         robot_control::common::BODY_POSTURE_STATE::STANDUP) {
       result = rcClientInterfaceLieDownOrStandUp(
@@ -202,60 +197,55 @@ void SaturnController::robotCommandCallback(
     nav_joy = common::CONTROL_SOURCE_MODE::joy_control;
     result = rcClientInterfaceSetNavOrJoyControl(nav_joy);
   } else if ("StairsGait" == request->comamnd_name) {
-    SPDLOG_INFO("StairsGait is called");
     result = rcClientInterfaceSetScene(
         robot_control::common::CONTROL_SOURCE_MODE::nav_control,
         common::SCENE_TYPE::STAIRS);
   } else if ("OrdinaryGait" == request->comamnd_name) {
-    SPDLOG_INFO("OrdinaryGait is called");
     result = rcClientInterfaceSetScene(
         robot_control::common::CONTROL_SOURCE_MODE::nav_control,
         common::SCENE_TYPE::WALKING);
   } else if ("RLOrdinaryGait" == request->comamnd_name) {
-    SPDLOG_INFO("RLOrdinaryGait is called");
     result = rcClientInterfaceSetScene(
         robot_control::common::CONTROL_SOURCE_MODE::nav_control,
         common::SCENE_TYPE::RL);
   } else if ("RLMountainGait" == request->comamnd_name) {
-    SPDLOG_INFO("RLMountainGait is called");
     result = rcClientInterfaceSetScene(
         robot_control::common::CONTROL_SOURCE_MODE::nav_control,
         common::SCENE_TYPE::RL);
   } else if ("StayNormal" == request->comamnd_name) {
     int height = (int)(common_status_data_.max_height * 0.8 * 100);
-    SPDLOG_INFO(
+    RCLCPP_INFO(node_->get_logger(),
         "Standing Height StayNormal is called, standing height of saturn is "
-        "set to {} cm",
+        "set to %d cm",
         height);
     result = rcClientInterfaceBodyHighAdjust(
         robot_control::common::CONTROL_SOURCE_MODE::nav_control, height);
   } else if ("StayDown" == request->comamnd_name) {
     int height = (int)(common_status_data_.max_height * 0.6 * 100);
-    SPDLOG_INFO(
+    RCLCPP_INFO(node_->get_logger(),
         "Standing Height StayDown is called, standing height of saturn is set "
-        "to {} cm",
+        "to %d cm",
         height);
     result = rcClientInterfaceBodyHighAdjust(
         robot_control::common::CONTROL_SOURCE_MODE::nav_control, height);
   } else if ("SlopeGait" == request->comamnd_name) {
-    SPDLOG_INFO("SlopeGait is called");
     result = rcClientInterfaceSetScene(
         robot_control::common::CONTROL_SOURCE_MODE::nav_control,
         common::SCENE_TYPE::ROUGH);
   } else if ("VstairsGait" == request->comamnd_name) {
-    SPDLOG_WARN("VstairsGait is not supported yet.");
+    RCLCPP_WARN(node_->get_logger(), "VstairsGait is not supported yet.");
     result = false;
   } else if ("AccuStairsGait" == request->comamnd_name) {
-    SPDLOG_WARN("AccuStairsGait is not supported yet.");
+    RCLCPP_WARN(node_->get_logger(), "AccuStairsGait is not supported yet.");
     result = false;
   } else if ("SingleStairsGait" == request->comamnd_name) {
-    SPDLOG_WARN("SingleStairsGait is not supported yet.");
+    RCLCPP_WARN(node_->get_logger(), "SingleStairsGait is not supported yet.");
     result = false;
   } else if ("ResetDockState" == request->comamnd_name) {
-    SPDLOG_WARN("ResetDockState is not supported yet.");
+    RCLCPP_WARN(node_->get_logger(), "ResetDockState is not supported yet.");
     result = false;
   } else {
-    SPDLOG_WARN("Invalid robot command name!!!");
+    RCLCPP_WARN(node_->get_logger(), "Invalid robot command name!!!");
     result = false;
   }
   response->result = result;
@@ -278,8 +268,10 @@ bool SaturnController::leaveDock() {
 bool SaturnController::waitForDockFinishing(bool is_on_dock,
                                             size_t time_out_s) {
   if (!rcClientInterfaceEnterOrExitCharge(
-          robot_control::common::CONTROL_SOURCE_MODE::nav_control, is_on_dock)) {
-    SPDLOG_WARN("gRPC not connected, command loading failed.");
+          robot_control::common::CONTROL_SOURCE_MODE::nav_control,
+          is_on_dock)) {
+    RCLCPP_WARN(node_->get_logger(),
+                "gRPC not connected, command loading failed.");
     return false;
   }
   size_t retry = 0;
@@ -292,13 +284,14 @@ bool SaturnController::waitForDockFinishing(bool is_on_dock,
             : common_status_data_.charge_status.charge_switch_state ==
                   robot_control::common::CHARGE_SWITCH_STATE::EXIT_SUCCEEDED;
     if (query_state) {
-      SPDLOG_INFO("Robot finished {} task!", task_type);
+      RCLCPP_INFO(node_->get_logger(), "Robot finished %s task!", task_type);
       return true;
     }
-    SPDLOG_INFO("Waiting for finishing {} task for {}s", task_type, retry);
+    RCLCPP_INFO(node_->get_logger(), "Waiting for finishing %s task for %ld s", task_type, retry);
     std::this_thread::sleep_for(std::chrono::seconds(1));
   }
-  SPDLOG_ERROR("Robot did not finish {} task in: {} seconds.", task_type,
+  RCLCPP_ERROR(node_->get_logger(),
+               "Robot did not finish %s task in: %ld seconds.", task_type,
                time_out_s);
   return false;
 }
@@ -308,16 +301,17 @@ void SaturnController::guardianCallback(
   float velocity_decay_ratio = guardian_msg->velocity_decay_ratio;
   std::string trigger_source = guardian_msg->trigger_source;
   rcClientInterfaceSetGuardian(
-      robot_control::common::CONTROL_SOURCE_MODE::nav_control, velocity_decay_ratio,
-      trigger_source);
+      robot_control::common::CONTROL_SOURCE_MODE::nav_control,
+      velocity_decay_ratio, trigger_source);
 }
 
 void SaturnController::guardianServiceCallback(
     const GuardianSrv::Request::SharedPtr request,
     GuardianSrv::Response::SharedPtr response) {
   // hyan(20250219): triger source自动调整
-  SPDLOG_WARN("Guardian is triggered by {}, velocity_decay_ratio: {}",
-              request->trigger_source, request->velocity_decay_ratio);
+  RCLCPP_WARN(node_->get_logger(),
+              "Guardian is triggered by %s, velocity_decay_ratio: %lf",
+              (request->trigger_source).c_str(), request->velocity_decay_ratio);
   // 遥控器上增加显示
   if (request->velocity_decay_ratio <= 0) {
     rcClientInterfaceSetGuardian(
@@ -352,26 +346,29 @@ void SaturnController::mainTimerCallback() {
       rcClientInterfaceGetCommonStatus(&common_status_data_);
   if (is_getting_com_success_ == true && common_status_result_ == false) {
     is_getting_com_success_ = false;
-    SPDLOG_INFO("rcClientInterfaceGetCommonStatus starts to fail.");
+    RCLCPP_INFO(node_->get_logger(),
+                "rcClientInterfaceGetCommonStatus starts to fail.");
 
     // 重新init
     if (rcClientInterfaceInit(config_handler_->motion_computer_ip_address_) ==
         false) {
-      SPDLOG_ERROR("rcClientInterfaceInit failed with {}",
+      RCLCPP_ERROR(node_->get_logger(), "rcClientInterfaceInit failed with %s",
                    config_handler_->motion_computer_ip_address_.c_str());
     }
   } else if (is_getting_com_success_ == false &&
              common_status_result_ == true) {
     is_getting_com_success_ = true;
-    SPDLOG_INFO("rcClientInterfaceGetCommonStatus starts to succeed.");
+    RCLCPP_INFO(node_->get_logger(),
+                "rcClientInterfaceGetCommonStatus starts to succeed.");
   } else if (is_getting_com_success_ == false &&
              common_status_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());
+        RCLCPP_ERROR(node_->get_logger(),
+                     "rcClientInterfaceInit failed with %s",
+                     config_handler_->motion_computer_ip_address_.c_str());
       }
     }
   }
@@ -394,28 +391,29 @@ void SaturnController::imuTimerCallback() {
     imu_result = rcClientInterfaceGetImuData(&imu_data_);
   } catch (...) {
     imu_result = false;
-    SPDLOG_INFO("rcclient interface imu error!!!");
+    RCLCPP_ERROR(node_->get_logger(), "rcclient interface imu error!!!");
   }
 
   if (is_getting_imu_success_ == true && imu_result == false) {
     is_getting_imu_success_ = false;
-    SPDLOG_INFO("rcClientInterfaceGetImuData starts to fail.");
+    RCLCPP_INFO(node_->get_logger(),
+                "rcClientInterfaceGetImuData starts to fail.");
 
     // 重新init
     if (rcClientInterfaceInit(config_handler_->motion_computer_ip_address_) ==
         false) {
-      SPDLOG_INFO("rcClientInterfaceInit failed with %s",
+      RCLCPP_WARN(node_->get_logger(), "rcClientInterfaceInit failed with %s",
                   config_handler_->motion_computer_ip_address_.c_str());
     }
   } else if (is_getting_imu_success_ == false && imu_result == true) {
     is_getting_imu_success_ = true;
-    SPDLOG_INFO("rcClientInterfaceGetImuData starts to succeed.");
+    RCLCPP_INFO(node_->get_logger(), "rcClientInterfaceGetImuData starts to succeed.");
   } else if (is_getting_imu_success_ == false && imu_result == false) {
     reconnect_count_++;
     if (reconnect_count_ % 500 == 0) {
       if (rcClientInterfaceInit(config_handler_->motion_computer_ip_address_) ==
           false) {
-        SPDLOG_INFO("rcClientInterfaceInit failed with %s",
+        RCLCPP_ERROR(node_->get_logger(), "rcClientInterfaceInit failed with %s",
                     config_handler_->motion_computer_ip_address_.c_str());
       }
     }
@@ -454,40 +452,40 @@ void SaturnController::leggedOodmTimerCallback() {
   // 先把足式里程计放在主callback里发布，如果对不同频率有需求，再重新使用这里
 }
 
-void SaturnController::publishRobotState() {  // todo 这里逻辑还不完善
+void SaturnController::publishRobotState() { // todo 这里逻辑还不完善
   using SDK_SCENE_TYPE = robot_control::common::SCENE_TYPE;
   auto curr_state = ROBOT_STATE::LIE_DOWN;
 
   switch (common_status_data_.cur_scene) {
-    case SDK_SCENE_TYPE::NULL_SCENE: {
-      curr_state = ROBOT_STATE::LIE_DOWN;
-      break;
-    }
-    case SDK_SCENE_TYPE::LIE_DOWN: {
-      curr_state = ROBOT_STATE::LIE_DOWN;
-      break;
-    }
-    case SDK_SCENE_TYPE::WALKING:
-    case SDK_SCENE_TYPE::ROUGH:
-    case SDK_SCENE_TYPE::PERCEIVE_STAIRS:
-    case SDK_SCENE_TYPE::SNOW:
-    case SDK_SCENE_TYPE::SLIPPY:
-    case SDK_SCENE_TYPE::STAIRS: {
-      if (common_status_data_.cur_speed >= 0.05) {
-        curr_state = ROBOT_STATE::STEPPING;
-      } else {
-        curr_state = ROBOT_STATE::STAND_UP;
-      }
-      break;
-    }
-    case SDK_SCENE_TYPE::CHARGE: {
-      curr_state = ROBOT_STATE::LIE_DOWN;
-      break;
-    }
-    default: {
-      curr_state = ROBOT_STATE::LIE_DOWN;
-      break;
-    }
+  case SDK_SCENE_TYPE::NULL_SCENE: {
+    curr_state = ROBOT_STATE::LIE_DOWN;
+    break;
+  }
+  case SDK_SCENE_TYPE::LIE_DOWN: {
+    curr_state = ROBOT_STATE::LIE_DOWN;
+    break;
+  }
+  case SDK_SCENE_TYPE::WALKING:
+  case SDK_SCENE_TYPE::ROUGH:
+  case SDK_SCENE_TYPE::PERCEIVE_STAIRS:
+  case SDK_SCENE_TYPE::SNOW:
+  case SDK_SCENE_TYPE::SLIPPY:
+  case SDK_SCENE_TYPE::STAIRS: {
+    if (common_status_data_.cur_speed >= 0.05) {
+      curr_state = ROBOT_STATE::STEPPING;
+    } else {
+      curr_state = ROBOT_STATE::STAND_UP;
+    }
+    break;
+  }
+  case SDK_SCENE_TYPE::CHARGE: {
+    curr_state = ROBOT_STATE::LIE_DOWN;
+    break;
+  }
+  default: {
+    curr_state = ROBOT_STATE::LIE_DOWN;
+    break;
+  }
   }
 
   auto robot_state = std_msgs::msg::UInt32();
@@ -500,32 +498,32 @@ void SaturnController::publishDockingState() {
   auto curr_state = DOCK_STATE::IDLE;
 
   switch (common_status_data_.charge_status.charge_switch_state) {
-    case SDK_CHARGE_SWITCH_STATE::NULL_CHARGE_SWITCH: {
-      curr_state = DOCK_STATE::IDLE;
-      break;
-    }
-    case SDK_CHARGE_SWITCH_STATE::EXIT_SUCCEEDED: {
-      curr_state = DOCK_STATE::IDLE;
-      break;
-    }
-    case SDK_CHARGE_SWITCH_STATE::EXITING_TILE: {
-      curr_state = DOCK_STATE::LEAVING_DOCK;
-      break;
-    }
-    case SDK_CHARGE_SWITCH_STATE::ENTER_SUCCEEDED: {
-      curr_state = DOCK_STATE::LIE_ON_DOCKER;
-      break;
-    }
-    case SDK_CHARGE_SWITCH_STATE::ENTERING_TILE: {
-      curr_state = DOCK_STATE::GOING_TO_DOCK;
-      break;
-    }
-    case SDK_CHARGE_SWITCH_STATE::EXIT_FAILED:
-    case SDK_CHARGE_SWITCH_STATE::ENTER_FAILED:
-    default: {
-      curr_state = DOCK_STATE::IDLE;
-      break;
-    }
+  case SDK_CHARGE_SWITCH_STATE::NULL_CHARGE_SWITCH: {
+    curr_state = DOCK_STATE::IDLE;
+    break;
+  }
+  case SDK_CHARGE_SWITCH_STATE::EXIT_SUCCEEDED: {
+    curr_state = DOCK_STATE::IDLE;
+    break;
+  }
+  case SDK_CHARGE_SWITCH_STATE::EXITING_TILE: {
+    curr_state = DOCK_STATE::LEAVING_DOCK;
+    break;
+  }
+  case SDK_CHARGE_SWITCH_STATE::ENTER_SUCCEEDED: {
+    curr_state = DOCK_STATE::LIE_ON_DOCKER;
+    break;
+  }
+  case SDK_CHARGE_SWITCH_STATE::ENTERING_TILE: {
+    curr_state = DOCK_STATE::GOING_TO_DOCK;
+    break;
+  }
+  case SDK_CHARGE_SWITCH_STATE::EXIT_FAILED:
+  case SDK_CHARGE_SWITCH_STATE::ENTER_FAILED:
+  default: {
+    curr_state = DOCK_STATE::IDLE;
+    break;
+  }
   }
   auto pub_dock_state = std_msgs::msg::UInt32();
   pub_dock_state.data = static_cast<uint32_t>(curr_state);
@@ -537,18 +535,18 @@ void SaturnController::publishControlMode() {
   auto curr_mode = CONTROL_MODE::NULL_MODE;
 
   switch (common_status_data_.control_source) {
-    case SDK_CONTROL_MODE::nav_control: {
-      curr_mode = CONTROL_MODE::NAVI_CONTROL;
-      break;
-    }
-    case SDK_CONTROL_MODE::joy_control: {
-      curr_mode = CONTROL_MODE::JOY_CONTROL;
-      break;
-    }
-    default: {
-      curr_mode = CONTROL_MODE::NULL_MODE;
-      break;
-    }
+  case SDK_CONTROL_MODE::nav_control: {
+    curr_mode = CONTROL_MODE::NAVI_CONTROL;
+    break;
+  }
+  case SDK_CONTROL_MODE::joy_control: {
+    curr_mode = CONTROL_MODE::JOY_CONTROL;
+    break;
+  }
+  default: {
+    curr_mode = CONTROL_MODE::NULL_MODE;
+    break;
+  }
   }
   auto nav_or_manual_state = std_msgs::msg::UInt8();
   nav_or_manual_state.data = static_cast<uint8_t>(curr_mode);
@@ -559,51 +557,51 @@ void SaturnController::publishGait() {
   using SDK_SCENE_TYPE = robot_control::common::SCENE_TYPE;
   auto curr_scene = Robot_Gait::TROT;
   switch (common_status_data_.cur_scene) {
-    case SDK_SCENE_TYPE::NULL_SCENE: {
-      curr_scene = Robot_Gait::TROT;
-      break;
-    }
-    case SDK_SCENE_TYPE::LIE_DOWN: {
-      curr_scene = Robot_Gait::TROT;
-      break;
-    }
-    case SDK_SCENE_TYPE::WALKING: {
-      curr_scene = Robot_Gait::TROT;
-      break;
-    }
-    case SDK_SCENE_TYPE::STAIRS: {
-      curr_scene = Robot_Gait::NORMAL_STAIR;
-      break;
-    }
-    case SDK_SCENE_TYPE::CHARGE: {
-      curr_scene = Robot_Gait::TROT;
-      break;
-    }
-    case SDK_SCENE_TYPE::ROUGH: {
-      curr_scene = Robot_Gait::SLOPE;
-      break;
-    }
-    case SDK_SCENE_TYPE::PERCEIVE_STAIRS: {
-      curr_scene = Robot_Gait::SENSE_STAIR;
-      break;
-    }
-    case SDK_SCENE_TYPE::RL: {
-      curr_scene = Robot_Gait::RL_TROT;
-      break;
-    }
-    case SDK_SCENE_TYPE::SNOW:
-    case SDK_SCENE_TYPE::SLIPPY:
-    default: {
-      curr_scene = Robot_Gait::TROT;
-      break;
-    }
+  case SDK_SCENE_TYPE::NULL_SCENE: {
+    curr_scene = Robot_Gait::TROT;
+    break;
+  }
+  case SDK_SCENE_TYPE::LIE_DOWN: {
+    curr_scene = Robot_Gait::TROT;
+    break;
+  }
+  case SDK_SCENE_TYPE::WALKING: {
+    curr_scene = Robot_Gait::TROT;
+    break;
+  }
+  case SDK_SCENE_TYPE::STAIRS: {
+    curr_scene = Robot_Gait::NORMAL_STAIR;
+    break;
+  }
+  case SDK_SCENE_TYPE::CHARGE: {
+    curr_scene = Robot_Gait::TROT;
+    break;
+  }
+  case SDK_SCENE_TYPE::ROUGH: {
+    curr_scene = Robot_Gait::SLOPE;
+    break;
+  }
+  case SDK_SCENE_TYPE::PERCEIVE_STAIRS: {
+    curr_scene = Robot_Gait::SENSE_STAIR;
+    break;
+  }
+  case SDK_SCENE_TYPE::RL: {
+    curr_scene = Robot_Gait::RL_TROT;
+    break;
+  }
+  case SDK_SCENE_TYPE::SNOW:
+  case SDK_SCENE_TYPE::SLIPPY:
+  default: {
+    curr_scene = Robot_Gait::TROT;
+    break;
+  }
   }
   auto gait_scene_state = std_msgs::msg::UInt8();
   gait_scene_state.data = static_cast<uint8_t>(curr_scene);
   gait_publisher_->publish(gait_scene_state);
 }
 
-void SaturnController::publishBatteryState() {  // todo
+void SaturnController::publishBatteryState() { // todo
   float battery_power = 0.0;
   battery_power = common_status_data_.charge_status.battery_info.level / 100;
   if (battery_power < 0) {