feat(monitor): 添加监控模块用于订阅和打印运动状态和里程计数据

添加新的Monitor类，用于订阅并打印SportModeState和Odometry消息数据
移除custom_robot.cpp中冗余的日志信息
更新CMakeLists.txt以包含新的监控模块

CMakeLists.txt

@@ -38,6 +38,7 @@ set(SOURCES
     src/navigation.cpp
     src/low_controller.cpp
     src/recharge.cpp
+    src/monitor.cpp
 )
 
 add_executable(main ${SOURCES})

include/monitor.hpp

@@ -0,0 +1,28 @@
+#ifndef MONITOR_HPP
+#define MONITOR_HPP
+
+#include <unitree/idl/go2/SportModeState_.hpp>
+#include <unitree/idl/ros2/Odometry_.hpp>
+#include <unitree/robot/channel/channel_subscriber.hpp>
+#include <memory>
+
+namespace custom {
+
+class Monitor {
+public:
+    Monitor();
+    ~Monitor();
+    
+    void initialize();
+
+private:
+    void sportModeStateCallback(const void* message);
+    void odometryCallback(const void* message);
+    
+    std::unique_ptr<unitree::robot::ChannelSubscriber<unitree_go::msg::dds_::SportModeState_>> sport_mode_state_subscriber_;
+    std::unique_ptr<unitree::robot::ChannelSubscriber<nav_msgs::msg::dds_::Odometry_>> odometry_subscriber_;
+};
+
+} // namespace custom
+
+#endif // MONITOR_HPP

src/custom_robot.cpp

@@ -153,7 +153,6 @@ void CustomRobot::trajControl()
         LOG_ERROR("Controller not available for dynamic trajectory.");
         return;
     }
-    LOG_INFO("Dynamic trajectory control started");
     float vx = 0.3f;
     float delta = 0.06f;
     traj_count_ += traj_dt_;

src/monitor.cpp

@@ -0,0 +1,127 @@
+#include "monitor.hpp"
+#include <unitree/robot/channel/channel_factory.hpp>
+#include <iostream>
+
+namespace custom {
+
+Monitor::Monitor() = default;
+
+Monitor::~Monitor() = default;
+
+void Monitor::initialize() {
+    // Initialize ChannelFactory
+    unitree::robot::ChannelFactory::Instance()->Init();
+    
+    // Create subscriber for SportModeState
+    sport_mode_state_subscriber_ = std::make_unique<unitree::robot::ChannelSubscriber<unitree_go::msg::dds_::SportModeState_>>(
+        "rt/sportmodestate"
+    );
+    
+    // Initialize channel with callback
+    sport_mode_state_subscriber_->InitChannel([this](const void* message) {
+        this->sportModeStateCallback(message);
+    });
+    
+    // Create subscriber for Odometry
+    odometry_subscriber_ = std::make_unique<unitree::robot::ChannelSubscriber<nav_msgs::msg::dds_::Odometry_>>(
+        "rt/odometry"
+    );
+    
+    // Initialize odometry channel with callback
+    odometry_subscriber_->InitChannel([this](const void* message) {
+        this->odometryCallback(message);
+    });
+    
+    std::cout << "Monitor initialized with SportModeState and Odometry subscriptions" << std::endl;
+}
+
+void Monitor::sportModeStateCallback(const void* message) {
+    const unitree_go::msg::dds_::SportModeState_* state = 
+        static_cast<const unitree_go::msg::dds_::SportModeState_*>(message);
+    
+    // Print position data
+    std::cout << "Position: " 
+              << state->position()[0] << ", " 
+              << state->position()[1] << ", " 
+              << state->position()[2] << std::endl;
+    
+    // Print IMU data
+    const auto& imu = state->imu_state();
+    
+    std::cout << "IMU Quaternion: " 
+              << imu.quaternion()[0] << ", " 
+              << imu.quaternion()[1] << ", " 
+              << imu.quaternion()[2] << ", " 
+              << imu.quaternion()[3] << std::endl;
+    
+    std::cout << "IMU Gyroscope: " 
+              << imu.gyroscope()[0] << ", " 
+              << imu.gyroscope()[1] << ", " 
+              << imu.gyroscope()[2] << std::endl;
+    
+    std::cout << "IMU Accelerometer: " 
+              << imu.accelerometer()[0] << ", " 
+              << imu.accelerometer()[1] << ", " 
+              << imu.accelerometer()[2] << std::endl;
+    
+    std::cout << "IMU RPY: " 
+              << imu.rpy()[0] << ", " 
+              << imu.rpy()[1] << ", " 
+              << imu.rpy()[2] << std::endl;
+    
+    std::cout << "IMU Temperature: " << imu.temperature() << std::endl;
+    std::cout << "---" << std::endl;
+}
+
+void Monitor::odometryCallback(const void* message) {
+    const nav_msgs::msg::dds_::Odometry_* odometry = 
+        static_cast<const nav_msgs::msg::dds_::Odometry_*>(message);
+    
+    // Print header information
+    std::cout << "Odometry Header:" << std::endl;
+    std::cout << "  Frame ID: " << odometry->child_frame_id() << std::endl;
+    
+    // Print pose information
+    const auto& pose = odometry->pose().pose();
+    std::cout << "Pose Position: " 
+              << pose.position().x() << ", " 
+              << pose.position().y() << ", " 
+              << pose.position().z() << std::endl;
+    
+    std::cout << "Pose Orientation (Quaternion): " 
+              << pose.orientation().x() << ", " 
+              << pose.orientation().y() << ", " 
+              << pose.orientation().z() << ", " 
+              << pose.orientation().w() << std::endl;
+    
+    // Print pose covariance (first few elements)
+    const auto& pose_cov = odometry->pose().covariance();
+    std::cout << "Pose Covariance (first 6): ";
+    for (int i = 0; i < 6; ++i) {
+        std::cout << pose_cov[i] << " ";
+    }
+    std::cout << std::endl;
+    
+    // Print twist information
+    const auto& twist = odometry->twist().twist();
+    std::cout << "Twist Linear: " 
+              << twist.linear().x() << ", " 
+              << twist.linear().y() << ", " 
+              << twist.linear().z() << std::endl;
+    
+    std::cout << "Twist Angular: " 
+              << twist.angular().x() << ", " 
+              << twist.angular().y() << ", " 
+              << twist.angular().z() << std::endl;
+    
+    // Print twist covariance (first few elements)
+    const auto& twist_cov = odometry->twist().covariance();
+    std::cout << "Twist Covariance (first 6): ";
+    for (int i = 0; i < 6; ++i) {
+        std::cout << twist_cov[i] << " ";
+    }
+    std::cout << std::endl;
+    std::cout << "=== Odometry End ===" << std::endl;
+}
+
+} // namespace custom