更新部分注释和文档

Code/RK3588/README.md

@@ -1,7 +1,9 @@
 # 目录说明
 
+更新：2023-12-13 詹力 Review
+
 1. `PIBot_ROS` : ROS小车的ROS上层支持包负责`IMU`和`ODM`等数据的读取
-2. `Robot_ROS_Driver` : 定位融合代码 
+2. `Robot_ROS_APP` : 定位融合代码 
 
 其他资料：
 

Code/RK3588/Robot_ROS_APP/src/ros_merge_test/include/align.h

@@ -26,6 +26,7 @@ namespace uwb_slam{
         ros::Subscriber wheel_odom_sub_;
         ros::Subscriber imu_sub_;
         ros::Subscriber odom_sub_;
+
         Imu_odom_pose_data imu_odom_;
         Uwb_data uwb_data_;
         ros::Time tmp ;
@@ -35,7 +36,6 @@ namespace uwb_slam{
         cv::Mat img1;
         std::queue<std::pair<Imu_odom_pose_data,Uwb_data>> data_queue;
         std::shared_ptr<uwb_slam::Uwb> uwb_;
-
     };
 };
 #endif

Code/RK3588/Robot_ROS_APP/src/ros_merge_test/include/read_sensor_data.h

@@ -15,10 +15,10 @@
 namespace uwb_slam{
     typedef boost::shared_ptr<nav_msgs::Odometry const> OdomConstPtr;
     typedef boost::shared_ptr<sensor_msgs::Imu const> ImuConstPtr;
-    class Read_sensor_data
+    class ReadSensorData
     {
         public:
-        Read_sensor_data();
+        ReadSensorData();
 
         void Run(int argc, char* argv[]);
         //void set_uwb(Uwb * uwb);

Code/RK3588/Robot_ROS_APP/src/ros_merge_test/include/senddata.h

@@ -0,0 +1,29 @@
+#include <ros/ros.h>
+#include <nav_msgs/Odometry.h>
+#include <geometry_msgs/Point.h>
+#include <geometry_msgs/Quaternion.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <mutex>
+#include "uwb.h"
+#ifndef SENDDATA_H
+#define SENDDATA_H
+
+namespace uwb_slam{
+class Senddata{
+    public:
+    Senddata(){};
+    void publishOdometry( std::shared_ptr<uwb_slam::Uwb>uwb);
+    void Run(std::shared_ptr<uwb_slam::Uwb>uwb);
+    void odomCB(const nav_msgs::Odometry& odom);
+
+    std::mutex mMutexSend;
+    private:
+    ros::Publisher position_pub_;
+    ros::Subscriber odom_sub_;
+    ros::NodeHandle nh_;
+    nav_msgs::Odometry odom_;//odom的消息类型
+    nav_msgs::Odometry sub_odom_;//odom的消息类型
+};
+}
+
+#endif

Code/RK3588/Robot_ROS_APP/src/ros_merge_test/include/type.h

@@ -19,12 +19,13 @@ struct Imu_data
 struct Imu_odom_pose_data
 {
     Imu_data imu_data_;                                   
-    double pose_[3];
+    double pose_[3];                                       /* 位置信息 */
-    double quat_[4];
+    double quat_[4];                                       /* 姿态信息(四元数) */
     double vxy_;
     double angle_v_;
     Imu_odom_pose_data(){};
-    Imu_odom_pose_data( Imu_data imu_data,double x,double y,double z, double qw, double qx, double qy, double qz,double vxy, double angle_v):imu_data_(imu_data),pose_{x,y,z},quat_{qw,qx,qy,qz},vxy_(vxy),angle_v_(angle_v){};
+    Imu_odom_pose_data( Imu_data imu_data,double x,double y,double z, double qw, double qx, 
+    double qy, double qz,double vxy, double angle_v):imu_data_(imu_data),pose_{x,y,z},quat_{qw,qx,qy,qz},vxy_(vxy),angle_v_(angle_v){};
 };
 
 /*struct Odom_data

Code/RK3588/Robot_ROS_APP/src/ros_merge_test/include/uwb.h

@@ -20,7 +20,7 @@ namespace uwb_slam{
         void Run();
         bool checknewdata();
         void feed_imu_odom_pose_data();
-        void Serread();
+        void UartUSBRead();
        
 
  

Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/align.cpp

@@ -1,11 +1,20 @@
+/******************** (C) COPYRIGHT 2023 UPBot **********************************
+* File Name          : align.cpp
+* Current Version    : V1.0
+* Date of Issued     : 2023.12.13 zhanli@review
+* Comments           : 传感器数据对齐
+********************************************************************************/
 #include "align.h"
 
 namespace uwb_slam{
+
     void Align::Run()
     {
         tmp = ros::Time::now();
         ros::Time tmp1 = ros::Time::now();
         ros::Time tmp2 = ros::Time::now();
+
+
         wheel_odom_sub_= nh_.subscribe("wheel_odom",10,&Align::wheel_odomCB,this);
         imu_sub_= nh_.subscribe("raw_imu",10,&Align::imuCB,this);
         odom_sub_= nh_.subscribe("odom",10,&Align::odomCB,this);
@@ -120,12 +129,20 @@ namespace uwb_slam{
         return;
     }
 
+    /**---------------------------------------------------------------------
+    * Function    : odomCB
+    * Description : 里程计的回调函数, 定期会被ROS调用传参，这个函数不能做过于耗时
+    *               的操作
+    * Input       : nav_msgs::Odometry& odom : 里程计输入结构体
+    * Date        : 2023/12/13 zhanli@review
+    *---------------------------------------------------------------------**/
     void Align::odomCB(const nav_msgs::Odometry& odom)
     {
         odom_tmp_ = odom.header.stamp;
         imu_odom_.pose_[0] = odom.pose.pose.position.x;
         imu_odom_.pose_[1] = odom.pose.pose.position.y;
         imu_odom_.pose_[2] = odom.pose.pose.position.z;
+        
         imu_odom_.quat_[0] = odom.pose.pose.orientation.x;
         imu_odom_.quat_[1] = odom.pose.pose.orientation.y;
         imu_odom_.quat_[2] = odom.pose.pose.orientation.z;

Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/main.cpp

@@ -0,0 +1,71 @@
+/******************** (C) COPYRIGHT 2023 UPBot **********************************
+* File Name          : main.cpp
+* Current Version    : V1.0
+* Date of Issued     : 2023.12.13 zhanli@review
+* Comments           : UPbot割草机器人项目传感器融合定位入口函数
+********************************************************************************/
+#include "../include/system.h"
+#include "../include/uwb.h"
+#include <iostream>
+#include <ros/ros.h>
+#include <thread>
+#include "senddata.h"
+
+
+/**---------------------------------------------------------------------
+* Function    : main
+* Description : 多传感器融合定位的入口函数
+* Date        : 2023/12/13 zhanli@review
+*---------------------------------------------------------------------**/
+int main(int argc, char** argv)
+{
+    // Initialize the ROS node
+    ros::init(argc, argv, "locate_info_pub_node"); 
+
+    std::shared_ptr<uwb_slam::System>   system    = std::make_shared<uwb_slam::System>();
+    std::shared_ptr<uwb_slam::Mapping>  mp        = std::make_shared<uwb_slam::Mapping>();
+    std::shared_ptr<uwb_slam::Uwb>      uwb       = std::make_shared<uwb_slam::Uwb>();
+    std::shared_ptr<uwb_slam::Senddata> sender    = std::make_shared<uwb_slam::Senddata>();
+    std::shared_ptr<uwb_slam::Align>    align     = std::make_shared<uwb_slam::Align>();
+
+    // uwb_slam::System* system = new uwb_slam::System();
+    // uwb_slam::Mapping* mp = new uwb_slam::Mapping();
+    // uwb_slam::Uwb*  uwb = new uwb_slam::Uwb();
+    // uwb_slam::Senddata* sender = new uwb_slam::Senddata();
+    
+    system->Mapping_       = mp;
+    system->Mapping_->uwb_ = uwb;
+    system->Uwb_           = uwb;
+    system->Sender_        = sender;
+    system->Align_         = align;
+    
+    mp->uwb_    = system->Uwb_;
+    align->uwb_ = system->Uwb_;
+
+
+    //  control data fllow in system
+    std::thread rose_trd([&system]() {
+        system->Run();
+    });
+    // uwb serried read
+    std::thread uwb_trd([&uwb]() {
+        uwb->Run();
+    });
+
+    // 建图部分暂时没有使用到
+    /*std::thread map_trd([&mp]() {
+        mp->Run();
+    });*/
+
+    std::thread sender_trd([&sender, uwb]() {
+        sender->Run(uwb);
+    });
+
+    std::thread align_trd([&align]() {
+        align->Run();
+    });
+      
+    // Start the ROS node's main loop  
+    ros::spin(); 
+    //System->run()
+}

Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/mapping.cpp

@@ -1,6 +1,6 @@
 #include "mapping.h"
 #include <mutex>
-#include <unistd.h>// 包含 usleep() 函数所在的头文件
+#include <unistd.h>
 #include <opencv2/core.hpp>
 #include <opencv2/highgui.hpp>
 
@@ -16,10 +16,8 @@ namespace uwb_slam
     {
         //std::unique_lock<std::mutex> lock(mMutexMap);
         mv_uwb_point_.push(data);
-
     }
 
-
     void Mapping::process()
     {
         {
@@ -72,19 +70,22 @@ namespace uwb_slam
             cv::destroyAllWindows();
          }
         */
-        while(1){
+        while(1)
+        {
+            // 这个地方会持续阻塞
             int key = cv::waitKey(0);
             if (key == 'q') {
                 read_uwb_ = true;
                 std::cout << "non" << key << std::endl;
                 //cv::destroyAllWindows();
             }
+
             while(read_uwb_ ) // 按下空格键
             {
                 int key2 = cv::waitKey(1);
                 if (key2 == 'q' ) {
                     //TODO: save
-                //   std::cout << << std::endl;
+
                     std::string pngimage = "/home/firefly/Project_Ros/src/ros_merge_test/Map/output_image.png";//保存的图片文件路径
                     cv::imwrite(pngimage, img);
                     read_uwb_ = false;
@@ -102,15 +103,10 @@ namespace uwb_slam
                     //std::cout << " start process" << std::endl;
                     process();
                     //std::cout << " end process" << std::endl;
-                     
                 }
             }
             // std::cout << "out" << key << std::endl;
-
         }
-
-
-
         //std::string pngimage="../Map/pngimage.png";//保存的图片文件路径
         //cv::imwrite(pngimage,img);
       

Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/read_sensor_data.cpp

@@ -6,24 +6,27 @@ namespace uwb_slam {
     //void Read_sensor_data::set_uwb(){}
 
 
-    void Read_sensor_data::imu_call_back(const ImuConstPtr& imu){
+    void ReadSensorData::imu_call_back(const ImuConstPtr& imu){
         Imu_data d1= Imu_data(imu->linear_acceleration.x,imu->linear_acceleration.y,imu->linear_acceleration.z,
                               imu->angular_velocity.x,imu->angular_velocity.y,imu->angular_velocity.z);
 
     }
-    void Read_sensor_data::odom_call_back(const OdomConstPtr& odom){
+    void ReadSensorData::odom_call_back(const OdomConstPtr& odom){
         Odom_data d1 = Odom_data(odom->pose.pose.position.x, odom->pose.pose.position.y, odom->pose.pose.position.z,
                                  odom->pose.pose.orientation.w,odom->pose.pose.orientation.x, odom->pose.pose.orientation.y, odom->pose.pose.orientation.z,
                                  odom->header.stamp,odom->twist.twist.linear.x,odom->twist.twist.linear.y,odom->twist.twist.angular.z);
 
     }
-    void Read_sensor_data::Run(int argc, char* argv[]){
+    void ReadSensorData::Run(int argc, char* argv[]){
 
         ros::init(argc, argv, "imu_odom");
         // 创建一个节点句柄
         ros::NodeHandle nh;
+
         //imu_sub_ =  nh.subscribe<std_msgs::String>("imu/data_raw", 1000, this->imu_call_back);
         //odom_sub_ = nh.subscribe("odom", 1000, odom_call_back);
+
+         // 运行ROS事件循环
         ros::spin();
     }
 

Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/senddata.cpp

@@ -0,0 +1,83 @@
+/******************** (C) COPYRIGHT 2023 UPBot **********************************
+* File Name          : senddata.cpp
+* Current Version    : V1.0
+* Date of Issued     : 2023.12.13 zhanli@review
+* Comments           : UWB数据的发送
+********************************************************************************/
+#include "senddata.h"
+namespace uwb_slam{
+
+/**---------------------------------------------------------------------
+* Function    : Senddata::Run
+* Description : UWB位置发布以及odm数据的订阅
+* Date        : 2023/12/13 zhanli@review
+*---------------------------------------------------------------------**/
+void Senddata::Run(std::shared_ptr<uwb_slam::Uwb>uwb){
+    // 初始化了一个名为loop_rate的ros::Rate对象,频率设置为10赫兹
+    ros::Rate loop_rate(10);
+    // 初始化一个ROS发布者，用于发布nav_msgs::Odometry类型的消息
+    // 主题被设置为"uwb_odom"，队列大小为50
+    position_pub_ = nh_.advertise<nav_msgs::Odometry>("uwb_odom", 50);
+    // 初始化了一个ROS订阅者，用于订阅"odom"主题。它指定了当在该主题上接收到
+    // 消息时，将调用Senddata类的odomCB回调函数。队列大小被设置为10
+    odom_sub_ = nh_.subscribe("odom", 10, &Senddata::odomCB,this);
+
+    while(ros::ok()){
+        // 按照10Hz频率发布uwb信息
+        publishOdometry(uwb);
+        ros::spinOnce();
+        // 用于控制循环速率
+        loop_rate.sleep();
+    }
+}
+
+void Senddata::odomCB(const nav_msgs::Odometry& odom){
+    // 这个地方接收的是轮速里程计的信息
+    // 包含位置和姿态
+    sub_odom_ = odom;
+    return;
+}
+
+/**---------------------------------------------------------------------
+* Function    : Senddata::publishOdometry
+* Description : 发布UWB里程计数据，这里读取的数据到底是什么，依旧存在疑问
+* Date        : 2023/12/13 zhanli@review
+*---------------------------------------------------------------------**/
+void Senddata::publishOdometry(std::shared_ptr<uwb_slam::Uwb> uwb)
+{
+
+    std::mutex mMutexSend;
+    
+    ros::Time current_time = ros::Time::now();
+
+    // 设置 Odometry 消息的头部信息
+    odom_.header.stamp = current_time;   // 这个地方获取的时间是否会存在问题?
+    odom_.header.frame_id = "odom";      // 设置坐标系为 "map"
+    odom_.child_frame_id = "base_link";  // 设置坐标系为 "base_link"
+    
+    // 填充 Odometry 消息的位置信息
+    odom_.pose.pose.position.x = uwb->x;
+    odom_.pose.pose.position.y = uwb->y;
+    odom_.pose.pose.position.z = 0.0;
+    
+
+    // 填充 Odometry 消息的姿态信息（使用四元数来表示姿态）
+    // tf2::Quaternion quat;
+    // quat.setRPY(0, 0, uwb->theta);  
+    
+    // 设置了 yaw 角度，其他 roll 和 pitch 设置为 0
+    // odom.pose.pose.orientation.x = quat.x();
+    // odom.pose.pose.orientation.y = quat.y();
+    // odom.pose.pose.orientation.z = quat.z();
+    // odom.pose.pose.orientation.w = quat.w();
+
+    // 从里程计拿到姿态信息
+    odom_.pose.pose.orientation.x = sub_odom_.pose.pose.orientation.x;
+    odom_.pose.pose.orientation.y = sub_odom_.pose.pose.orientation.y;
+    odom_.pose.pose.orientation.z = sub_odom_.pose.pose.orientation.z;
+    odom_.pose.pose.orientation.w = sub_odom_.pose.pose.orientation.w;
+
+    // 发布 Odometry 消息
+    position_pub_.publish(odom_);
+}
+} // namespace uwb_slam

Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/system.cpp

@@ -5,9 +5,6 @@ namespace uwb_slam{
     void System::Run()
     {
         while(1){
-
         }
-
     }
-    
 }

Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/uwb.cpp

@@ -1,3 +1,9 @@
+/******************** (C) COPYRIGHT 2023 UPBot **********************************
+* File Name          : uwb.cpp
+* Current Version    : V1.0
+* Date of Issued     : 2023.12.13 zhanli@review
+* Comments           : UWB数据驱动, 负责从串口读取USB并发布出去
+********************************************************************************/
 #include "uwb.h"
 #include <cmath>
 
@@ -7,26 +13,17 @@ namespace uwb_slam{
 
     //
     Uwb::Uwb(){
-            //int pre_seq = -1;
     }
 
     void Uwb::Run() {
-        
         while(1){
-        this->Serread();
+            // 这个地方不控制速率?
-           // std::cout<<"s"<<std::endl;
+            this->UartUSBRead();
-           // std::cout<<this->x<<std::endl;
+        }
-        /*if(t_tmp!=imu_odom_.imu_data_.imu_t_){
-            imu_odom_queue_.push(imu_odom_);
-            t_tmp=imu_odom_.imu_data_.imu_t_;
-        }*/
     }
 
 
-    }
+    void Uwb::UartUSBRead()
-
-
-    void Uwb::Serread()
     {
         try {
             boost::asio::io_service io;
@@ -38,7 +35,7 @@ namespace uwb_slam{
             serial.set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one));  // 设置停止位
 
             uint8_t tmpdata[12];
-            // std::cerr << "befor read" << std::endl;
+            
             size_t bytesRead = boost::asio::read(serial, boost::asio::buffer(tmpdata, 12)); // 读取串口数据
             // std::cerr << "after read" << std::endl;
 
@@ -66,51 +63,16 @@ namespace uwb_slam{
             // pre_seq = static_cast<int>(tmpdata[3]);
 
             std::cout << "theta: " << theta << " distance: " << distance << std::endl;
-            //std::cout std::endl;
-        } catch (const std::exception& ex) {
-            std::cerr << "Exception: " << ex.what() << std::endl;
-        }
-    }
             
+        } catch (const std::exception& ex) {
+            std::cerr << "[ERR]: uwb.cpp::Uart USB read data exception: " 
+                << ex.what() << std::endl;
+        }
+    }
     
     void fusion()
     {
-
     }
-
-    
-
-
 };
 
-// bool Uwb::checknewdata()
-// {
-//     std::unique_lock<std::mutex> lock(mMutexUwb);
-//     return !v_buffer_imu_odom_pose_data_.empty();
-// }
-
-// void Uwb::Run() {
-//     while(1)
-//     {
-//         if(checknewdata())
-//         {
-//             {
-//                 std::unique_lock<std::mutex> lock(mMutexUwb);
-//                 Imu_odom_pose_data imu_odom_pose_data = v_buffer_imu_odom_pose_data_.front();
-//                 v_buffer_imu_odom_pose_data_.pop();
-//             }
-
-
-
-//         }
-//     }
-
-
-
-// }
-
-// void Uwb::feed_imu_odom_pose_data(const Imu_odom_pose_data& imu_odom_pose_data){
-//     std::unique_lock<std::mutex> lock(mMutexUwb);
-//     v_buffer_imu_odom_pose_data_.push(imu_odom_pose_data);
-// }
 

Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/include/senddata.h

@@ -1,37 +0,0 @@
-#include <ros/ros.h>
-#include <nav_msgs/Odometry.h>
-#include <geometry_msgs/Point.h>
-#include <geometry_msgs/Quaternion.h>
-#include <tf2/LinearMath/Quaternion.h>
-#include <mutex>
-#include "uwb.h"
-#ifndef SENDDATA_H
-#define SENDDATA_H
-
-
-namespace uwb_slam{
-
-    class Senddata
-    {
-        public:
-        Senddata(){};
-        void publishOdometry( std::shared_ptr<uwb_slam::Uwb>uwb);
-        void Run(std::shared_ptr<uwb_slam::Uwb>uwb);
-        void odomCB(const nav_msgs::Odometry& odom);
-
-
-        std::mutex mMutexSend;
-        private:
-        ros::Publisher position_pub_;
-        ros::Subscriber odom_sub_;
-        ros::NodeHandle nh_;
-        nav_msgs::Odometry odom_;//odom的消息类型
-        nav_msgs::Odometry sub_odom_;//odom的消息类型
-
-
-      
-    };
-
-}
-
-#endif

Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/src/main.cpp

@@ -1,62 +0,0 @@
-#include "../include/system.h"
-#include "../include/uwb.h"
-
-
-#include <iostream>
-#include <ros/ros.h>
-#include <thread>
-#include "senddata.h"
-
-
-
-int main(int argc, char** argv)
-{
-    ros::init(argc, argv, "locate_info_pub_node"); // Initialize the ROS node
-
-    std::shared_ptr<uwb_slam::System>system = std::make_shared<uwb_slam::System>();
-    std::shared_ptr<uwb_slam::Mapping>mp = std::make_shared<uwb_slam::Mapping>();
-    std::shared_ptr<uwb_slam::Uwb>uwb = std::make_shared<uwb_slam::Uwb>();
-    std::shared_ptr<uwb_slam::Senddata>sender = std::make_shared<uwb_slam::Senddata>();
-    std::shared_ptr<uwb_slam::Align>align = std::make_shared<uwb_slam::Align>();
-
-    // uwb_slam::System* system = new uwb_slam::System();
-    // uwb_slam::Mapping* mp = new uwb_slam::Mapping();
-    // uwb_slam::Uwb*  uwb = new uwb_slam::Uwb();
-    // uwb_slam::Senddata* sender = new uwb_slam::Senddata();
-    
-    
-    system->Mapping_ = mp;
-    system->Mapping_->uwb_ = uwb;
-    system->Uwb_  = uwb;
-    system->Sender_ = sender;
-    system->Align_ = align;
-    
-
-    mp->uwb_ = system->Uwb_;
-    align->uwb_=system->Uwb_;
-
-
-    //  control data fllow in system
-    std::thread rose_trd ([&system]() {
-        system->Run();
-    });
-    // uwb serried read
-    std::thread uwb_trd([&uwb]() {
-        uwb->Run();
-    });
-    // build map
-    /*std::thread map_trd ([&mp]() {
-        mp->Run();
-    });*/
-
-    std::thread sender_trd ([&sender, uwb]() {
-        sender->Run(uwb);
-    });
-
-    std::thread align_trd ([&align]() {
-        align->Run();
-    });
-
-    ros::spin(); // Start the ROS node's main loop
-    //System->run()
-}

Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/src/senddata.cpp

@@ -1,63 +0,0 @@
-#include "senddata.h"
-
-namespace uwb_slam
-{
-    void Senddata::Run(std::shared_ptr<uwb_slam::Uwb>uwb){
-
-        ros::Rate loop_rate(10);
-        position_pub_=nh_.advertise<nav_msgs::Odometry>("uwb_odom",50);
-        odom_sub_=nh_.subscribe("odom",10,&Senddata::odomCB,this);
-        while(ros::ok()){
-            publishOdometry(uwb);
-            ros::spinOnce();
-            loop_rate.sleep();
-        }
-
-
-    }
-    void Senddata::odomCB(const nav_msgs::Odometry& odom){
-        sub_odom_ = odom;
-        return;
-    }
-
-    void Senddata::publishOdometry(std::shared_ptr<uwb_slam::Uwb>uwb)
-    {
-
-        std::mutex mMutexSend;
-        ros::Time current_time = ros::Time::now();
-
-        // 设置 Odometry 消息的头部信息
-        odom_.header.stamp = current_time;
-        odom_.header.frame_id = "odom";  // 设置坐标系为 "map"
-        odom_.child_frame_id = "base_link";  // 设置坐标系为 "base_link"
-        
-        // 填充 Odometry 消息的位置信息
-        odom_.pose.pose.position.x = uwb->x;
-        odom_.pose.pose.position.y = uwb->y;
-        odom_.pose.pose.position.z = 0.0;
-        
-
-        // 填充 Odometry 消息的姿态信息（使用四元数来表示姿态）
-        // tf2::Quaternion quat;
-        // quat.setRPY(0, 0, uwb->theta);  // 设置了 yaw 角度，其他 roll 和 pitch 设置为 0
-        // odom.pose.pose.orientation.x = quat.x();
-        // odom.pose.pose.orientation.y = quat.y();
-        // odom.pose.pose.orientation.z = quat.z();
-        // odom.pose.pose.orientation.w = quat.w();
-
-        odom_.pose.pose.orientation.x = sub_odom_.pose.pose.orientation.x;
-        odom_.pose.pose.orientation.y = sub_odom_.pose.pose.orientation.y;
-        odom_.pose.pose.orientation.z = sub_odom_.pose.pose.orientation.z;
-        odom_.pose.pose.orientation.w = sub_odom_.pose.pose.orientation.w;
-
-        // 发布 Odometry 消息
-        position_pub_.publish(odom_);
-
-
-    }
-
-
-    
-
-
-} // namespace uwb_slam