融合UWB定位，小车验证版

Code/MowingRobot/pibot_ros/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/test/room_exploration_client.launch

@@ -13,7 +13,7 @@
 	<!-- 大车参数 -->
 	<!-- <arg name="robot_radius" default="0.3"/>
 	<arg name="coverage_radius" default="0.3"/> -->
-	<arg name="use_test_maps" default="true"/>
+	<arg name="use_test_maps" default="false"/>
 
 	<!--  -->
 	<node ns="room_exploration" pkg="ipa_room_exploration" type="room_exploration_client" name="room_exploration_client" output="screen">
@@ -27,6 +27,6 @@
 	<!-- <node pkg="ipa_room_exploration" type="talkerUltraSound" name="talkerUltraSound" required="true"/> -->
 
 	<!-- <include file="$(find ipa_room_exploration)/ros/launch/clean_work.launch"/> -->
-	<include file="$(find pibot_navigation)/launch/nav.launch"/>
+	<!-- <include file="$(find pibot_navigation)/launch/nav.launch"/> -->
 
 </launch>

Code/MowingRobot/pibot_ros/ros_ws/src/pibot_bringup/launch/bringup_with_imu.launch

@@ -37,8 +37,8 @@
     <node pkg="tf" type="static_transform_publisher" name="base_imu_to_base_link"
         args="0 0.0 0 0 0.0  0.0 /base_link /imu_link 40" />  
 
-    <!-- <node pkg="tf" type="static_transform_publisher" name="fix_map" args="1 -1 0 0 0 0 /map /odom 50" />  -->
+    <node pkg="tf" type="static_transform_publisher" name="fix_map" args="0 0 0 0 0 0 /map /odom 50" /> 
-    <node pkg="tf" type="static_transform_publisher" name="fix_map" args="-2 -2 0 0 0 0 /map /odom 50" /> 
+    <!-- <node pkg="tf" type="static_transform_publisher" name="fix_map" args="-2 -2 0 0 0 0 /map /odom 50" />  -->
 
     <!-- <node pkg="tf" type="static_transform_publisher" name="fix_odom" args="0 0 0 0 0 0 /odom /base_link 50" /> -->
     <!-- <node pkg="tf" type="static_transform_publisher" name="map_to_odom" args="1.0 1.0 0.0 0 0 0.0 /map /odom 1000"/>     -->
@@ -66,8 +66,8 @@
     <group ns="/" if="$(arg use_odom)" >
         <!-- robot pose ekf -->
         <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf" output="screen">
-            <!-- <param name="output_frame" value="odom" /> -->
+            <param name="output_frame" value="odom" />
-              <param name="output_frame" value="odom" />
+            <param name="output_frame" type="bool" value="false" />
             <param name="base_footprint_frame" value="base_link"/>
             <param name="freq" value="2000.0"/>
             <param name="sensor_timeout" value="1.0"/>  
@@ -82,7 +82,8 @@
         <!-- odom type adapter -->
         <node pkg="pibot_bringup" type="odom_ekf.py" name="odom_ekf" output="screen">
             <remap from="input" to="robot_pose_ekf/odom_combined"/>
-            <remap from="output" to="odom"/>
+            <remap from="output" to="odom_imu_ekf"/>
         </node>
     </group>
+
 </launch>

Code/MowingRobot/pibot_ros/ros_ws/src/pibot_bringup/scripts/odom_ekf.py

@@ -67,8 +67,8 @@ class OdomEKF():
     def pub_ekf_odom(self, msg):
         odom = Odometry()
         odom.header = msg.header
-        odom.header.frame_id = '/odom'
+        # odom.header.frame_id = '/odom'
-        odom.child_frame_id = 'base_link'
+        # odom.child_frame_id = 'base_link'
         odom.pose = msg.pose
         odom.twist = self.twist
         self.ekf_pub.publish(odom)

Code/MowingRobot/pibot_ros/ros_ws/src/pibot_navigation/launch/nav.launch

@@ -12,7 +12,7 @@
   <node name="map_server" pkg="map_server" type="map_server" args="$(find pibot_navigation)/maps/$(arg map_name)"/>
 
   <include file="$(find pibot_navigation)/launch/include/move_base.launch.xml" />
-
+  <include file="$(find upbot_location)/launch/upbot_location.launch" />
   <!-- <include file="$(find pibot_navigation)/launch/include/amcl.launch.xml" /> -->
   
 </launch>

Code/MowingRobot/pibot_ros/ros_ws/src/pibot_navigation/params/dwa_local_planner_params_apollo.yaml

@@ -33,6 +33,7 @@ DWAPlannerROS:
 # Goal Tolerance Parameters
   yaw_goal_tolerance: 0.2 
   xy_goal_tolerance: 0.15  
+  latch_xy_goal_tolerance: true
   # latch_xy_goal_tolerance: false
 
 # Forward Simulation Parameters

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/CMakeLists.txt

@@ -8,6 +8,7 @@ find_package(catkin REQUIRED COMPONENTS
   message_generation
   pibot_msgs
   geometry_msgs
+  tf2_ros
 )
 
 # 寻找OpenCV库
@@ -18,7 +19,7 @@ find_package(Boost REQUIRED)
 # catkin_package(
 # #  INCLUDE_DIRS include
 # #  LIBRARIES upbot_location
-# #  CATKIN_DEPENDS roscpp rospy std_msgs
+# #  CATKIN_DEPENDS roscpp rospy std_msgs tf2_ros
 # #  DEPENDS system_lib
 # CATKIN_DEPENDS message_runtime std_msgs geometry_msgs
 # )

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/align.h

@@ -1,3 +1,5 @@
+#ifndef ALIGN_H
+#define AlIGN_H
 #include <cmath>
 #include <ros/ros.h>
 #include <opencv2/opencv.hpp>
@@ -11,39 +13,37 @@
 #include "lighthouse.h"
 #include "Mat.h"
 
-#ifndef ALIGN_H
+
-#define AlIGN_H
 namespace uwb_slam{
     class Align
     {
     public:
         Align(){
-            imuPos.Init(2, 1, 0);
+            imu_odomPos.Init(2, 1, 0);
             uwbPos.Init(2, 1, 0);
             syncPos.Init(2, 1, 0);
-            imuStartPos.Init(2, 1, 0);
+            imu_odom_StartPos.Init(2, 1, 0);
             Rotate.Init(2,2,0);
             uwbStartPos.Init(2,1,0);
         };
         void Run();
-        void wheel_odomCB(const nav_msgs::Odometry& wheel_odom);
         void imuCB(const pibot_msgs::RawImu& imu);
-        void odomCB(const nav_msgs::Odometry& odom);
+        void odom_imuCB(const nav_msgs::Odometry& odom);
 
     public:
         ros::NodeHandle nh_;
-        ros::Subscriber wheel_odom_sub_;
         ros::Subscriber imu_sub_;
-        ros::Subscriber odom_sub_;
+        ros::Subscriber odom_imu_sub_;
         Imu_odom_pose_data imu_odom_;
         Uwb_data uwb_data_;
         ros::Time imuDataRxTime, uwbDataRxTime, odomDataRxTime;
-        Mat imuStartPos;
+        Mat imu_odom_StartPos;
-        Mat imuPos;
+        Mat imu_odomPos;
         Mat uwbPos;
         Mat syncPos;
         Mat Rotate;
         Mat uwbStartPos;
+        double qx, qy, qz, qw;
         ros::Time odom_tmp_ ;
         bool write_data_ = false;
         cv::Mat img1;

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/mapping.h

@@ -2,7 +2,8 @@
 #include <opencv2/opencv.hpp>
 #include <mutex>
 #include "uwb.h"
-#include "align.h"
+// #include "align.h"
+#include "senddata.h"
 
 #ifndef MAPPING_H
 #define MAPPING_H

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/senddata.h

@@ -1,34 +1,38 @@
+
+#ifndef SENDDATA_H
+#define SENDDATA_H
 #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 <tf2_ros/transform_broadcaster.h>
+#include <geometry_msgs/TransformStamped.h>
 #include <mutex>
 #include "uwb.h"
-#ifndef SENDDATA_H
+#include "align.h"
-#define SENDDATA_H
+
 
 
 namespace uwb_slam{
-
     class Senddata
     {
         public:
         Senddata(){};
-        void publishOdometry( std::shared_ptr<uwb_slam::Uwb>uwb);
+        void publishOdometry();
-        void Run(std::shared_ptr<uwb_slam::Uwb>uwb);
+        void publishtf();
-        void odomCB(const nav_msgs::Odometry& odom);
+        void Run();
-
 
+        public:
+        std::shared_ptr<uwb_slam::Align> align_;
         std::mutex mMutexSend;
+
         private:
-        ros::Publisher position_pub_;
-        ros::Subscriber odom_sub_;
         ros::NodeHandle nh_;
-        nav_msgs::Odometry odom_;//odom的消息类型
+        ros::Publisher position_pub_;
-        nav_msgs::Odometry sub_odom_;//odom的消息类型
+        tf2_ros::TransformBroadcaster broadcaster;
-
+        nav_msgs::Odometry odom_imu_uwb_;//消息类型 发布融合uwb imu 里程计信息
-
+        geometry_msgs::TransformStamped transformStamped;//坐标变换消息
         
     };
 

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/launch/upbot_location.launch

@@ -0,0 +1,5 @@
+<launch>
+
+    <node pkg="upbot_location" type="upbot_location_node" name="upbot_location" output="screen" />
+
+</launch>

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/align.cpp

@@ -6,9 +6,8 @@ namespace uwb_slam{
         imuDataRxTime = ros::Time::now();
         uwbDataRxTime = 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);
+        odom_imu_sub_= nh_.subscribe("odom_imu_ekf",10,&Align::odom_imuCB,this);
 
         std::ofstream outfile("data.txt",std::ofstream::out);
         if(outfile.is_open()) {
@@ -18,7 +17,7 @@ namespace uwb_slam{
                     << "wX,wY,wZ,"\
                     << "qX,qY,qZ,qW,"\
                     << "vXY,angleV,"\
-                    << "imuPosX,imuPosY,"\
+                    << "imu_odomPosX,imu_odomPosY,"\
                     << "uwbPosX,uwbPosY,"\
                     << "syncPosX,syncPosY,"\
                     << "lightHousePosX,lightHousePosY,lightHousePosZ,"
@@ -28,7 +27,7 @@ namespace uwb_slam{
         }
         
         Mat prevPos(2, 1, 0);
-        Mat detPos(2, 1, 0);
+        Mat deltaPos(2, 1, 0);
         Mat predPos(2, 1, 0);
         Mat R(2, 2, 1);
         Mat Q(2, 2 ,1);
@@ -44,8 +43,8 @@ namespace uwb_slam{
         Mat I = F;
         bool imuStartFlag = 1;
         bool printFlag = 0;
-        double qx, qy, qz, qw;
+
-        float roll, imuStartRoll;
+        float yaw, imuStartyaw;
         while(1){
             // int key3 = cv::waitKey(1);
             // if(key3 == 's'){
@@ -54,42 +53,41 @@ namespace uwb_slam{
             //     printFlag = 0;
             // }
             if(imuDataRxTime!=imu_odom_.imu_data_.imu_t_){
-                std::cout << "imu received" << std::endl;
+                // std::cout << "imu received" << std::endl;
-                imuPos.mat[0][0] = imu_odom_.pose_[0] * 100;
+                imu_odomPos.mat[0][0] = imu_odom_.pose_[0] * 100;
-                imuPos.mat[1][0] = imu_odom_.pose_[1] * 100;
+                imu_odomPos.mat[1][0] = imu_odom_.pose_[1] * 100;
                 qx = imu_odom_.quat_[0];
                 qy = imu_odom_.quat_[1];
                 qz = imu_odom_.quat_[2];
                 qw = imu_odom_.quat_[3];
-                roll = -atan2(2*(qw*qz+qx*qy),1-2*(qy*qy+qz*qz));
+                yaw = -atan2(2*(qw*qz+qx*qy),1-2*(qy*qy+qz*qz));
 
-                std::cout<<"roll:"<<roll<<std::endl;
+                std::cout<<"yaw:"<<yaw<<std::endl;
-                if (imuStartFlag == 1 && imuPos.mat[0][0] != 0 && imuPos.mat[1][0] != 0) {
+                if (imuStartFlag == 1 && imu_odomPos.mat[0][0] != 0 && imu_odomPos.mat[1][0] != 0) {
-                    imuStartRoll = roll-PI/2;//跟imu摆放的位置有关，减PI / 2是为了让角度变成跟车前进方向的夹角
+                    imuStartyaw = yaw-PI/2;//跟imu摆放的位置有关，减PI / 2是为了让角度变成跟车前进方向的夹角
-
+                    imu_odom_StartPos = imu_odomPos;
-                    imuStartPos = imuPos;
                     imuStartFlag = 0;
                 }
-                //std::cout << imuStartPos.mat[0][0] << " " << imuStartPos.mat[1][0] << std::endl;
+                //std::cout << imu_odom_StartPos.mat[0][0] << " " << imu_odom_StartPos.mat[1][0] << std::endl;
-                roll = roll - imuStartRoll;
+                yaw = yaw - imuStartyaw;
-                imuPos = imuPos - imuStartPos;
+                imu_odomPos = imu_odomPos - imu_odom_StartPos;
-                detPos = imuPos - prevPos;
+                deltaPos = imu_odomPos - prevPos;
-                prevPos = imuPos;
+                prevPos = imu_odomPos;
                 if (uwbDataRxTime != uwb_->uwb_data_.uwb_t_) {
-                    std::cout << "uwb received" << std::endl;
+                    // std::cout << "uwb received" << std::endl;
-                    //这一步是为了把上述提到的UWB设备与imu设备不在同一坐标轴上设计的坐标轴对齐操作
+					//这一步是为了把上述提到的UWB设备与imu设备不在同一坐标轴上设计的坐标轴对齐操作
-                    Rotate.mat[0][0] = cos(roll);
+                    // Rotate.mat[0][0] = cos(yaw);
-                    Rotate.mat[0][1] = -sin(roll);
+                    // Rotate.mat[0][1] = -sin(yaw);
-                    Rotate.mat[1][0] = sin(roll);
+                    // Rotate.mat[1][0] = sin(yaw);
-                    Rotate.mat[1][1] = cos(roll);
+                    // Rotate.mat[1][1] = cos(yaw);
 
                     uwbPos.mat[0][0] = uwb_->uwb_data_.x_;
                     uwbPos.mat[1][0] = uwb_->uwb_data_.y_;
 
-                    uwbPos = uwbPos - Rotate * uwbStartPos;
+                    // uwbPos = uwbPos - Rotate * uwbStartPos;
-                    //后期如UWB摆放在imu正上方即删除
+					//后期如UWB摆放在imu正上方即删除
                     //卡尔曼更新过程
-                    predPos = F * syncPos + detPos;
+                    predPos = F * syncPos + deltaPos;
                     Z = H * uwbPos;
                     P = F * P * (~F) + Q;
                     Kg = P * (~H) / (H * P * (~H) + R);
@@ -97,27 +95,27 @@ namespace uwb_slam{
                     P = (I - Kg * H) * P;
                     uwbDataRxTime = uwb_->uwb_data_.uwb_t_;
                 } else {
-                    syncPos = syncPos + detPos;//如果UWB没有更新信息，则使用imu对齐位置进行更新
+                    syncPos = syncPos + deltaPos;//如果UWB没有更新信息，则使用imu对齐位置进行更新
                 }
                 imuDataRxTime = imu_odom_.imu_data_.imu_t_;
                 odomDataRxTime = odom_tmp_;
 
                 std::cout << "syncPos:" << syncPos.mat[0][0] << " " << syncPos.mat[1][0];
                 std::cout << " uwbPos:" << uwbPos.mat[0][0] << " " << uwbPos.mat[1][0];
-                std::cout << " imuPos:" << imuPos.mat[0][0] << " " << imuPos.mat[1][0];
+                std::cout << " imu_odomPos:" << imu_odomPos.mat[0][0] << " " << imu_odomPos.mat[1][0]<< std::endl;
-                std::cout << " lightHousePos:" << lighthouse_->data.x_ << " " << lighthouse_->data.y_ << std::endl;
+                // std::cout << " lightHousePos:" << lighthouse_->data.x_ << " " << lighthouse_->data.y_ << std::endl;
 
                 
-                outfile << imuDataRxTime << "," << odomDataRxTime << "," << uwbDataRxTime <<","\
+            //     outfile << imuDataRxTime << "," << odomDataRxTime << "," << uwbDataRxTime <<","\
-                        << imu_odom_.imu_data_.a_[0] << "," << imu_odom_.imu_data_.a_[1] << "," << imu_odom_.imu_data_.a_[2] << ","\
+            //             << imu_odom_.imu_data_.a_[0] << "," << imu_odom_.imu_data_.a_[1] << "," << imu_odom_.imu_data_.a_[2] << ","\
-                        << imu_odom_.imu_data_.w_[0] << "," << imu_odom_.imu_data_.w_[1] << "," << imu_odom_.imu_data_.w_[2] << ","\
+            //             << imu_odom_.imu_data_.w_[0] << "," << imu_odom_.imu_data_.w_[1] << "," << imu_odom_.imu_data_.w_[2] << ","\
-                        << qx << "," << qy << "," << qz << "," << qw << ","\
+            //             << qx << "," << qy << "," << qz << "," << qw << ","\
-                        << imu_odom_.vxy_ << "," <<imu_odom_.angle_v_ << ","\
+            //             << imu_odom_.vxy_ << "," <<imu_odom_.angle_v_ << ","\
-                        << imuPos.mat[0][0] << "," << imuPos.mat[1][0] << ","\
+            //             << imu_odomPos.mat[0][0] << "," << imu_odomPos.mat[1][0] << ","\
-                        << uwbPos.mat[0][0] << "," << uwbPos.mat[1][0] << ","\
+            //             << uwbPos.mat[0][0] << "," << uwbPos.mat[1][0] << ","\
-                        << syncPos.mat[0][0] << "," << syncPos.mat[1][0] << ","\
+            //             << syncPos.mat[0][0] << "," << syncPos.mat[1][0] << ","\
-                        << lighthouse_->data.x_ << "," << lighthouse_->data.y_ << "," << lighthouse_->data.z_ << ","\
+            //             << lighthouse_->data.x_ << "," << lighthouse_->data.y_ << "," << lighthouse_->data.z_ << ","\
-                        << uwb_->d[0] << "," << uwb_->d[1] << "," << uwb_->d[2] << "\n";
+            //             << uwb_->d[0] << "," << uwb_->d[1] << "," << uwb_->d[2] << "\n";
             }
             
         }
@@ -126,23 +124,7 @@ namespace uwb_slam{
         // std::cout<< "Data written to file." << std::endl;   
     }
     
-    
+//imu原始数据，不知道有没有纠正零偏的   
-    
-    void Align::wheel_odomCB(const nav_msgs::Odometry& wheel_odom)
-    {
-        imu_odom_.vxy_= wheel_odom.twist.twist.linear.x;
-        imu_odom_.angle_v_ = wheel_odom.twist.twist.angular.z;
-        // imu_odom_.pose_[0] = wheel_odom.pose.pose.position.x;
-        // imu_odom_.pose_[1] = wheel_odom.pose.pose.position.y;
-        // imu_odom_.pose_[2] = wheel_odom.pose.pose.position.z;
-        // imu_odom_.quat_[0] = wheel_odom.pose.pose.orientation.x;
-        // imu_odom_.quat_[1] = wheel_odom.pose.pose.orientation.y;
-        // imu_odom_.quat_[2] = wheel_odom.pose.pose.orientation.z;
-        // imu_odom_.quat_[3] = wheel_odom.pose.pose.orientation.w;
-
-    
-        return;
-    }
     void Align::imuCB(const pibot_msgs::RawImu& imu)
     {
         imu_odom_.imu_data_.imu_t_ = imu.header.stamp;
@@ -157,7 +139,8 @@ namespace uwb_slam{
         return;
     }
 
-    void Align::odomCB(const nav_msgs::Odometry& odom)
+//imu里程计融合位姿
+    void Align::odom_imuCB(const nav_msgs::Odometry& odom)
     {
         odom_tmp_ = odom.header.stamp;
         imu_odom_.pose_[0] = odom.pose.pose.position.x;
@@ -167,6 +150,9 @@ namespace uwb_slam{
         imu_odom_.quat_[1] = odom.pose.pose.orientation.y;
         imu_odom_.quat_[2] = odom.pose.pose.orientation.z;
         imu_odom_.quat_[3] = odom.pose.pose.orientation.w;
+
+        imu_odom_.vxy_= odom.twist.twist.linear.x;
+        imu_odom_.angle_v_ = odom.twist.twist.angular.z;
     }
 
 

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/main.cpp

@@ -4,7 +4,6 @@
 #include <iostream>
 #include <ros/ros.h>
 #include <thread>
-#include "senddata.h"
 
 
 
@@ -24,11 +23,7 @@ int main(int argc, char** argv)
     mp->align_ = align;
     align->uwb_ = uwb;
     align->lighthouse_ = lighthouse;
-
+    sender->align_ = align;
-    // //  control data fllow in system
-    // std::thread rose_trd ([&system]() {
-    //     system->Run();
-    // });
 
     // uwb serried read
     std::thread uwb_trd([&uwb]() {
@@ -40,8 +35,8 @@ int main(int argc, char** argv)
         mp->Run();
     });
 
-    std::thread sender_trd ([&sender, uwb]() {
+    std::thread sender_trd ([&sender]() {
-        sender->Run(uwb);
+        sender->Run();
     });
 
     std::thread align_trd ([&align]() {

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/mapping.cpp

@@ -109,7 +109,7 @@ namespace uwb_slam
                 if (key2 == 'q') {
                     //TODO: save
 
-                    std::string pngimage="/home/firefly/Project_Ros11/Project_Ros1/src/upbot_location/Map/output_image.png";//保存的图片文件路径
+                    std::string pngimage="/home/firefly/pibot_ros/ros_ws/src/upbot_location/map/output_image.png";//保存的图片文件路径
                     cv::imwrite(pngimage,img);
                     readPos = false;
                     
@@ -120,7 +120,7 @@ namespace uwb_slam
                     break;
                 }
         
-                this->feedPos(cv::Point2d(align_->imuPos.mat[0][0], align_->imuPos.mat[1][0]), cv::Point2d(align_->uwbPos.mat[0][0], align_->uwbPos.mat[1][0]), cv::Point2d(align_->syncPos.mat[0][0], align_->syncPos.mat[1][0]));
+                this->feedPos(cv::Point2d(align_->imu_odomPos.mat[0][0], align_->imu_odomPos.mat[1][0]), cv::Point2d(align_->uwbPos.mat[0][0], align_->uwbPos.mat[1][0]), cv::Point2d(align_->syncPos.mat[0][0], align_->syncPos.mat[1][0]));
                 //this->feedPos(cv::Point2d(uwb_->x, uwb_->y));
 
                 //uwb xieru

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/senddata.cpp

@@ -1,20 +1,27 @@
+	/**---------------------------------------------------------------------
+	* Function    : Senddata::publishOdometry
+	* Description : 发布UWB IMU 里程计融合的定位信息 一个发布给导航 一个发布给tftree 坐标变换使用
+	* Date        : 2024/3/19 linyuehang
+	*---------------------------------------------------------------------**/
+
 #include "senddata.h"
 
 
 namespace uwb_slam
 {
-    void Senddata::Run(std::shared_ptr<uwb_slam::Uwb>uwb){
+    void Senddata::Run(){
     // 初始化了一个名为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);
+        position_pub_=nh_.advertise<nav_msgs::Odometry>("odom",10);
     // 初始化了一个ROS订阅者，用于订阅"odom"主题。它指定了当在该主题上接收到
     // 消息时，将调用Senddata类的odomCB回调函数。队列大小被设置为10
-        odom_sub_=nh_.subscribe("odom",10,&Senddata::odomCB,this);
+
         while(ros::ok()){
         // 按照10Hz频率发布uwb信息
-            publishOdometry(uwb);
+            publishOdometry();
+			publishtf();
             ros::spinOnce();
         // 用于控制循环速率
             loop_rate.sleep();
@@ -22,54 +29,62 @@ namespace uwb_slam
 
 
     }
-    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)
+    void Senddata::publishOdometry()
     {
 
         std::mutex mMutexSend;
         ros::Time current_time = ros::Time::now();
 
         // 设置 Odometry 消息的头部信息
-        odom_.header.stamp = current_time;
+        odom_imu_uwb_.header.stamp = current_time;
-        odom_.header.frame_id = "odom";  // 设置坐标系为 "map"
+        odom_imu_uwb_.header.frame_id = "odom";  // 设置坐标系为 "map"
-        odom_.child_frame_id = "base_link";  // 设置坐标系为 "base_link"
+        odom_imu_uwb_.child_frame_id = "base_link";  // 设置坐标系为 "base_link"
 
+        // odom_imu_uwb_.header.frame_id = "odom";  
+        // odom_imu_uwb_.child_frame_id = "map";  
         // 填充 Odometry 消息的位置信息
-        odom_.pose.pose.position.x = uwb->uwb_data_.x_;
+        odom_imu_uwb_.pose.pose.position.x = align_->syncPos.mat[0][0]/100;
-        odom_.pose.pose.position.y = uwb->uwb_data_.y_;
+        odom_imu_uwb_.pose.pose.position.y = align_->syncPos.mat[1][0]/100;
-        odom_.pose.pose.position.z = 0.0;
+        odom_imu_uwb_.pose.pose.position.z = 0.0;
         
 
         // 填充 Odometry 消息的姿态信息（使用四元数来表示姿态）
-        // tf2::Quaternion quat;
+        odom_imu_uwb_.pose.pose.orientation.x = align_->qx;
-        // quat.setRPY(0, 0, uwb->theta);  // 设置了 yaw 角度，其他 roll 和 pitch 设置为 0
+        odom_imu_uwb_.pose.pose.orientation.y = align_->qy;
-        // odom.pose.pose.orientation.x = quat.x();
+        odom_imu_uwb_.pose.pose.orientation.z = align_->qz;
-        // odom.pose.pose.orientation.y = quat.y();
+        odom_imu_uwb_.pose.pose.orientation.w = align_->qw;
-        // 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_);
+        position_pub_.publish(odom_imu_uwb_);
 
 
     }
 
+    void Senddata::publishtf()
+    {
+        transformStamped.header.stamp = ros::Time::now();
+
+        transformStamped.header.frame_id = "odom"; // 父坐标系为 "odom"
+        transformStamped.child_frame_id = "base_link"; // 子坐标系为 "base_link"
+        transformStamped.transform.translation.x = align_->syncPos.mat[0][0]/100; // 设置坐标变换的平移部分
+        transformStamped.transform.translation.y = align_->syncPos.mat[1][0]/100;
+        transformStamped.transform.translation.z = 0.0;
+        transformStamped.transform.rotation.x = align_->qx; // 设置坐标变换的旋转部分（使用四元数表示）
+        transformStamped.transform.rotation.y = align_->qy;
+        transformStamped.transform.rotation.z = align_->qz;
+        transformStamped.transform.rotation.w = align_->qw;
+
+        broadcaster.sendTransform(transformStamped);
+    }
+