[fix]增加导航模式下停障提示。

saturn_controller/src/saturn_controller.cpp

@@ -2,8 +2,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
@@ -38,7 +36,7 @@ rclcpp::node_interfaces::NodeBaseInterface::SharedPtr SaturnController::get_node
 void SaturnController::init() {
     //初始化 grpc
     if (rcClientInterfaceInit(config_handler_->motion_computer_ip_address_) == false) {
-        RCLCPP_ERROR(node_->get_logger(), "rcClientInterfaceInit failed with %s",
+        SPDLOG_INFO("rcClientInterfaceInit failed with %s",
                      config_handler_->motion_computer_ip_address_.c_str());
     }
 
@@ -130,9 +128,6 @@ void SaturnController::init() {
 
 void SaturnController::cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr vel_msg) {
     common::ROBOT_TWIST rc_direct;
-    //SPDLOG_INFO("Received velocity command vx {} vy {} w {}",
-    //            vel_msg->linear.x * guardian_velocity_decay_ratio_, vel_msg->linear.y,
-    //            vel_msg->angular.z);
     rc_direct.linear.x = vel_msg->linear.x * guardian_velocity_decay_ratio_;  // todo
     rc_direct.linear.y = vel_msg->linear.y;
     rc_direct.linear.z = vel_msg->linear.z;
@@ -236,11 +231,27 @@ 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: %s",\
+        request->velocity_decay_ratio.c_str());
     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;
+        rcClientInterfaceSetGuardian(robot_control::common::NAV_OR_JOY_MODE::nav_control,
+                                    velocity_decay_ratio, trigger_source);
+
     } else if (request->velocity_decay_ratio < 1.0 && request->velocity_decay_ratio > 0) {
         this->guardian_velocity_decay_ratio_ = request->velocity_decay_ratio;
     } else {
+
+        //遥控器上增加显示
+        std::string trigger_source = "/front_obs_points";
+        float velocity_decay_ratio = 1;
+        rcClientInterfaceSetGuardian(robot_control::common::NAV_OR_JOY_MODE::nav_control,
+                                    velocity_decay_ratio, trigger_source);
+        
         this->guardian_velocity_decay_ratio_ = 1.0;
     }
     response->result = true;
@@ -490,6 +501,11 @@ void SaturnController::publishLeggedOdom() {
     odometry_data.pose.pose.position.y = common_status_data_.odometry.point.y;
     odometry_data.pose.pose.position.z = common_status_data_.odometry.point.z;
 
+    odometry_data.pose.pose.orientation.w = imu_data_.orientation.w;
+    odometry_data.pose.pose.orientation.x = imu_data_.orientation.x;
+    odometry_data.pose.pose.orientation.y = imu_data_.orientation.y;
+    odometry_data.pose.pose.orientation.z = imu_data_.orientation.z;
+
     odometry_data.twist.twist.linear.x = common_status_data_.odometry.twist.linear.x;
     odometry_data.twist.twist.linear.y = common_status_data_.odometry.twist.linear.y;
     odometry_data.twist.twist.linear.z = common_status_data_.odometry.twist.linear.z;