From d3487a2613b4aebd01aff4a3c86a06a1c430bbf9 Mon Sep 17 00:00:00 2001 From: Sucan126 <632190820@qq.com> Date: Tue, 30 Sep 2025 15:53:12 +0800 Subject: [PATCH] =?UTF-8?q?feat(monitor):=20=E6=B7=BB=E5=8A=A0=E7=9B=91?= =?UTF-8?q?=E6=8E=A7=E6=A8=A1=E5=9D=97=E7=94=A8=E4=BA=8E=E8=AE=A2=E9=98=85?= =?UTF-8?q?=E5=92=8C=E6=89=93=E5=8D=B0=E8=BF=90=E5=8A=A8=E7=8A=B6=E6=80=81?= =?UTF-8?q?=E5=92=8C=E9=87=8C=E7=A8=8B=E8=AE=A1=E6=95=B0=E6=8D=AE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 添加新的Monitor类，用于订阅并打印SportModeState和Odometry消息数据 移除custom_robot.cpp中冗余的日志信息 更新CMakeLists.txt以包含新的监控模块 --- CMakeLists.txt | 1 + include/monitor.hpp | 28 ++++++++++ src/custom_robot.cpp | 1 - src/monitor.cpp | 127 +++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 156 insertions(+), 1 deletion(-) create mode 100644 include/monitor.hpp create mode 100644 src/monitor.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index c80d730..7b06f2b 100644 --- a/CMakeLists.txt +++ b/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}) diff --git a/include/monitor.hpp b/include/monitor.hpp new file mode 100644 index 0000000..cb72286 --- /dev/null +++ b/include/monitor.hpp @@ -0,0 +1,28 @@ +#ifndef MONITOR_HPP +#define MONITOR_HPP + +#include +#include +#include +#include + +namespace custom { + +class Monitor { +public: + Monitor(); + ~Monitor(); + + void initialize(); + +private: + void sportModeStateCallback(const void* message); + void odometryCallback(const void* message); + + std::unique_ptr> sport_mode_state_subscriber_; + std::unique_ptr> odometry_subscriber_; +}; + +} // namespace custom + +#endif // MONITOR_HPP \ No newline at end of file diff --git a/src/custom_robot.cpp b/src/custom_robot.cpp index 542d220..6961330 100644 --- a/src/custom_robot.cpp +++ b/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_; diff --git a/src/monitor.cpp b/src/monitor.cpp new file mode 100644 index 0000000..c5d0393 --- /dev/null +++ b/src/monitor.cpp @@ -0,0 +1,127 @@ +#include "monitor.hpp" +#include +#include + +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>( + "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>( + "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(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(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 \ No newline at end of file