From f9631ad63d7461a6aeafa0364ed0c09883d7f4cb Mon Sep 17 00:00:00 2001
From: LXX <916401988@qq.com>
Date: Tue, 19 Mar 2024 20:48:24 +0800
Subject: [PATCH] =?UTF-8?q?=E8=9E=8D=E5=90=88UWB=E5=AE=9A=E4=BD=8D?=
 =?UTF-8?q?=EF=BC=8C=E5=B0=8F=E8=BD=A6=E9=AA=8C=E8=AF=81=E7=89=88?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../ros/test/room_exploration_client.launch   |   4 +-
 .../pibot_ros/ros_ws/src/map/160_160.png      | Bin 0 -> 994 bytes
 .../pibot_ros/ros_ws/src/map/250_250.png      | Bin 0 -> 1063 bytes
 .../pibot_ros/ros_ws/src/map/300_300.png      | Bin 0 -> 1100 bytes
 .../pibot_ros/ros_ws/src/map/400_400.png      | Bin 0 -> 1121 bytes
 .../pibot_ros/ros_ws/src/map/500_500.png      | Bin 0 -> 1181 bytes
 .../launch/bringup_with_imu.launch            |  11 +-
 .../src/pibot_bringup/scripts/odom_ekf.py     |   4 +-
 .../src/pibot_navigation/launch/nav.launch    |   2 +-
 .../dwa_local_planner_params_apollo.yaml      |   1 +
 .../ros_ws/src/upbot_location/CMakeLists.txt  |   3 +-
 .../ros_ws/src/upbot_location/include/align.h |  20 ++--
 .../src/upbot_location/include/mapping.h      |   3 +-
 .../src/upbot_location/include/senddata.h     |  32 +++---
 .../launch/upbot_location.launch              |   5 +
 .../ros_ws/src/upbot_location/map/1.png       | Bin 0 -> 36831 bytes
 .../ros_ws/src/upbot_location/src/align.cpp   | 106 ++++++++----------
 .../ros_ws/src/upbot_location/src/main.cpp    |  11 +-
 .../ros_ws/src/upbot_location/src/mapping.cpp |   4 +-
 .../src/upbot_location/src/senddata.cpp       |  73 +++++++-----
 20 files changed, 144 insertions(+), 135 deletions(-)
 create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/map/160_160.png
 create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/map/250_250.png
 create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/map/300_300.png
 create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/map/400_400.png
 create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/map/500_500.png
 create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/launch/upbot_location.launch
 create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/map/1.png

diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/test/room_exploration_client.launch b/Code/MowingRobot/pibot_ros/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/test/room_exploration_client.launch
index 3dd5d50..95a45f2 100644
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/test/room_exploration_client.launch
+++ b/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>
diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/map/160_160.png b/Code/MowingRobot/pibot_ros/ros_ws/src/map/160_160.png
new file mode 100644
index 0000000000000000000000000000000000000000..4c1dfbe0bdcef75e02cea03f35da7a09f3eff832
GIT binary patch
literal 994
zcmeAS@N?(olHy`uVBq!ia0y~yV4MKL96$kvrB^Zq7#NsWdb&7<R4~3h=*WA(LEy-S
z7W)^|4(?4~dSjC2D5Pfy6kRz!`;Llx?VgjqLxM@quo<;$lBRvbJ;jNqAOBnn%DkSg
KelF{r5}E)=_=~sz

literal 0
HcmV?d00001

diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/map/250_250.png b/Code/MowingRobot/pibot_ros/ros_ws/src/map/250_250.png
new file mode 100644
index 0000000000000000000000000000000000000000..5dbb7120974aa7f0cd8bebfe4487ceb38367a254
GIT binary patch
literal 1063
zcmeAS@N?(olHy`uVBq!ia0y~yV4MKL96$kvrB^Zq7#Nt}dAc};R4~3h=*WA(LEy-S
z7W)@shySK6y)j916w)pPicTNbwsg(=S)sc8gs%MhCn7taKAt*9X?x9{Nx@I1^@kUA
u-TCy<%Wq<|UEE2{k)+sT*(ju42>e;M-*}??gpLeQ3E=7K=d#Wzp$PyhjnS?E

literal 0
HcmV?d00001

diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/map/300_300.png b/Code/MowingRobot/pibot_ros/ros_ws/src/map/300_300.png
new file mode 100644
index 0000000000000000000000000000000000000000..911642175a31be56f9647c8732067d139c539889
GIT binary patch
literal 1100
zcmeAS@N?(olHy`uVBq!ia0y~yV4MKL96$kvrB^Zq7#LWDJzX3_Dj44$bmTqYAaG<u
zi~Wmfhxeu}y)j916jD0`ijEz(-syDiM}>O$2}$|)Pq-`}Kb{+-m|nAIvi6gn{_>*M
xIUhgzt(#zMckiU=sA3{QW2Vw5q;d#+Vh%R8o%pRIHx^V(c)I$ztaD0e0ssYt#M=M>

literal 0
HcmV?d00001

diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/map/400_400.png b/Code/MowingRobot/pibot_ros/ros_ws/src/map/400_400.png
new file mode 100644
index 0000000000000000000000000000000000000000..dd6b454c08d23efd42d77283573a8c4cb399696a
GIT binary patch
literal 1121
zcmeAS@N?(olHy`uVBq!ia0y~yV4MKL96$kvrB^Zq7#LVIJY5_^Dj44$G-PBj5NI*@
zpufxXBlBTZBh67rNeBpi-=Ug2N(`DXpd>Cw^%`l)A3Yp6aRM{n8Bp2b>FVdQ&MBb@
E0Op55CIA2c

literal 0
HcmV?d00001

diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/map/500_500.png b/Code/MowingRobot/pibot_ros/ros_ws/src/map/500_500.png
new file mode 100644
index 0000000000000000000000000000000000000000..7dff0905b4df6d59f2d06583010f810018082f4e
GIT binary patch
literal 1181
zcmeAS@N?(olHy`uVBq!ia0y~yV4MKL96$kvrB^Zq7#LVmJY5_^Dj44$bmV0)5IDS{
z#r_5FQP0XN(>y1RLSjQ;$#GlFNu$Jw3Ik%(#;7{apGN~zC$ebr@q@}8Pgg&ebxsLQ
E0LrrjM*si-

literal 0
HcmV?d00001

diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_bringup/launch/bringup_with_imu.launch b/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_bringup/launch/bringup_with_imu.launch
index 43eb274..3b4bb48 100644
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_bringup/launch/bringup_with_imu.launch
+++ b/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="-2 -2 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_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>
diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_bringup/scripts/odom_ekf.py b/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_bringup/scripts/odom_ekf.py
index 9db038d..87b9862 100755
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_bringup/scripts/odom_ekf.py
+++ b/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.child_frame_id = 'base_link'
+        # odom.header.frame_id = '/odom'
+        # odom.child_frame_id = 'base_link'
         odom.pose = msg.pose
         odom.twist = self.twist
         self.ekf_pub.publish(odom)
diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_navigation/launch/nav.launch b/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_navigation/launch/nav.launch
index e9faaac..1f4188c 100644
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_navigation/launch/nav.launch
+++ b/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>
diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_navigation/params/dwa_local_planner_params_apollo.yaml b/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_navigation/params/dwa_local_planner_params_apollo.yaml
index f10afd8..184977a 100644
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_navigation/params/dwa_local_planner_params_apollo.yaml
+++ b/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
diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/CMakeLists.txt b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/CMakeLists.txt
index 26392cc..35820e3 100644
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/CMakeLists.txt
+++ b/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
 # )
diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/align.h b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/align.h
index b11132c..601f30b 100644
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/align.h
+++ b/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 imuPos;
+        Mat imu_odom_StartPos;
+        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;
diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/mapping.h b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/mapping.h
index ac39d27..64b5e60 100644
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/mapping.h
+++ b/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
diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/senddata.h b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/senddata.h
index efef283..f582139 100644
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/senddata.h
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/senddata.h
@@ -1,35 +1,39 @@
+
+#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
-#define SENDDATA_H
+#include "align.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);
-
+        void publishOdometry();
+        void publishtf();
+        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的消息类型
-        nav_msgs::Odometry sub_odom_;//odom的消息类型
-
-
-      
+        ros::Publisher position_pub_;
+        tf2_ros::TransformBroadcaster broadcaster;
+        nav_msgs::Odometry odom_imu_uwb_;//消息类型 发布融合uwb imu 里程计信息
+        geometry_msgs::TransformStamped transformStamped;//坐标变换消息
+        
     };
 
 }
diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/launch/upbot_location.launch b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/launch/upbot_location.launch
new file mode 100644
index 0000000..f726f40
--- /dev/null
+++ b/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>
\ No newline at end of file
diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/map/1.png b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/map/1.png
new file mode 100644
index 0000000000000000000000000000000000000000..67d7af0c3873bc6d1f8ccb457eebbe6d549a0f87
GIT binary patch
literal 36831
zcmeHQ4OCNSnnpxHYz>hhsaF1?l1LqzZLM(35PkzhG2wzT)h(&%Y&k9MvMgsSIn5CN
zDB%(Z8ROV$N<fI{Y%N;rurqRC)tmK{*@f-aUE7^A(7l$EJv*LZFPSaLF$4R4UlNEG
z>=c5bdV6RL_vgDe@B6&Z`@HY>aZ|6{k^5x$>hPeTpeOV5vWcLe#s7`|4-1AZ*REz=
z3JQt~%FoUyu3mKKs(8^qrbmRduAMktfAIH3l8>L<TKswIwG~lsoxPA%yLRIp<=(pR
zM|M-#+s!r&{u#RFedp}D;Mt}b-*m^lSJSLpKC46j^PNZCguXC2{KEme1pid*^}ciM
z`P{QjGO_Aw_g=|BZ|$rOee?Qb0!(bx`GKU*sK0!aW7`*)n9}A4l0KvU_Ay}0DXaRC
zC4C?{HR|~{AD=t~R`nxG`ap7O)c<(l(N5)4z(jH-ey)joT1fiHf$}wvg+TIHHDa!b
zds;~P$brkO>||x<Z26v*oqjDczK!~3R|)xy%F52US3*97%F51J7Uw>YWM!vcA<4>4
ze?pR#owMy1`LN^^Bw5*MTPWSmw6DK>H|XWKu!H~lQ{AW8SN~5GGg}R~Aez>!5M_nv
z!ks$#HZq)DOWtQxR*3pD>$Ck&zh-^5|LNDP&-Op<{+#0sO#idwj|IQ-t_28`@M5Rn
z{kpn>m7uHyWhLk%y=ZA_rgN%m>C+;bb9;VjR91qr5_HyU0of^dzb?37r{Mj%;DVK%
ztn9pZdGk;1TwU~0#Lde~KK+A}^XrMbN`n8z(zd4Zl=9cCExVq0eL=mvY)!S9s`^uH
zZ|=hXn5qU}o%^QXs6)HOXCKFxA60@a!IGL&QEt1Mx?Y_7rt}4cnTH8tf1+}7tPRS?
z6Pyr{{zT>ESj&nR7^^)rgtGMMU!rH})4$N2q&z<)^~teV`t(auXX(?*(kDxw3t2i$
zQXWg6eo@Sml;?+<XmTu8eX_;7e|immSD#N0j{!{Wx5E}>fwV=WN~$S1uT10hp7CE_
z>ZyxKly@z_bYd3geETtF&n|7ECB<fny+NY>lkVNo%e1tGcZv_~l8Mcjc@OcKHEQZN
z#$-~e5Wz8_7inm%IA2mz(%R7%nP>vebY^@2gH(-&3ROH^Djtx+1DdxvKX7#V98z<F
z&+vvD=!zB|_~a0Fw(F449HmPf&SJ{d>tryv?g-|UyT$=BHb>NxV{lYadbrA}17yZ|
z{>{L?RB)TTH{giwi}q;>Ma!AxmxGP;^5HIWfXbV<JH3icD}rlpH3l0yqu@<1;Abe`
zfUSfrFO470x>(QEtr@;VuTNNdc%@RiL8cA0IG#ht4D_KPr12KVe}P|n!6%#gdc&N`
zH`G*@5IT>p8_1vbNCHMJFgWaRkpX3ouqH<&UMWc93dYx39GP&n3a$VU5sh%-Tqd#(
z$(*|xo1Uv%3s*fw9j2qz)V?BeC^48Dhy0r8IWU$~MGATnWLo&)43}HqhVdufA>h~j
z{Ce&X{6o0n6C&!<sZ?9pU!hK@`RHh?V;jJvua2%*C)36Pgg8n89Vrf`@(e-ns#j!x
z;J{aj-RnMs&;oF3`Ak7yMHHu~XO}C{xKbdd_fYyFB{>u?b7dOfLn`nmm1YkgBZoS8
zLkYplYFEc57&%sH9iY4H91~AB7Ligp9P9dH$i}6Y)ykxRywH1Yrk{1RxVZmR_tiv{
zS)aa6=2dMK`X<!r?LFhVMozrKS#hE5*$bPNz4`~;r>Xz`H!p|$+mifW{`ODjqM3ty
zrgk?&^3O%7^vkT#2?TS5&m7s!92`|j&PH{*NAa&1#yJ5;Ip=gRQ^0JnRA(6Y;4J`=
zrf;8hXxsMjHcAgB1_*^D0h%Q77D^wDe)2*R#>a$1QBtm8shHkJ=|f=;9LYhbQw$&o
zEAm85u{qsw2w-Ze5fDk~fy8z)d(_m3p4;0Z1F#$>w0cBnnDlJzyxw(iEI{+Fo(lj@
z%X5C{NYrzsD-VQi?rEsrVyWhcf#OOpAfAgP$mHy}m4ea;0ax$nEGpfYmThpH0`qkU
zt#x{?&9oSSTC)q_$~^?=g%N9V0g?2kJ_V_JpU<@L%%^$g4S@gV<A~Opj}clyFn36=
zjbsLtHAJ_W&y2xI>tqrk8Z=oC`(RvqEk63_YWQgT!?NK=`*S|HP^60L%VOeW-RKkN
z>YMn?7OucZUoxt>fNoP&&5sfMZS6*WeG{Ammx==ujGmz<Hd-9dSgH#U>I+%|1!M1Z
zV{Ympa&Lx52+|FX%=6Vb&iqyhPj${SUm?%#&OrnU{;ar3o&ZusfmhSc<ILaG4R&Ov
zTqMnlF;I?;;+l?XshCcV1c(?M&s!Wkl-Lv)7Ito={Hunr2tiMTU_4DtMf%`3R@~~4
z8G^X_lnR1W6p|+>eQZ5)=s}PB6C%Kkm4od74EV%Ac-Y;mF!<5QnFa@V6`c8=h=UN3
zYSS7$r>fTlb8SB~(nUskD|R-{+X|UMc{`q7BBpi6`AjlF4&aO{#3LqJf`GL9D@16h
zZSDk%D=C{`gN#a&R}P-RHv&=CpIR1qHlX>nI>&I63>|K*&Zyl_Z=RNG>CG0T0IG9D
z^CtvwPWUkuW`V2pfbkUA;-;!JoHuPtEGfMZ)2ESyDa5GR0U>#iFhODghle813}Vj=
z<ck2awlO1p+Xx%o=i`m8T$xFQ4+r9lz@Azprq`n@_E!k4xq1j&AeLOrov9*gWA+om
z{&tDys`2=$D<HkfnN^nR2tj|joDs=^<YLt*{5vc>m7d#jv<c%Zwgca3z$XNO3#CLW
zT`+wJpDIS!D={XoOBg)uKPh~V{B=7^j`ChDVNw;wknYUmcEBc!qY0c?bkD{_<Pu76
zUoK}Vrwokv6??ph+!rZGN5VJ@qvm5Hy@tD(wEw_!_(++Xj(NbsZ9LmcfCS!OOyWcL
z12J77uN)7zI8qS1j9r3V{Z#~*fp)ba$PD5<H;}IpA|S!(hq4WIh{E?p1U6I=Pt=lI
zyo7z<RP$;j;_u?K)yMAJ;jdr4aj^#s)l6tj1la8^?r1=;AXdxK2-EVAU;gCr%Su5{
zA!#p_Ga1N@lYXgyjo_?|1cZ&Pz9Ve&j_^Jjp$g?GQUBW_Yp4iZ+I`GJ$=wjJ-`oh?
zuo!%0tV1rmj--m$g|HcO!C+Q^b8v(p#z^PBZsB##;KSWGUXunB5{?tyek@L>jmTA&
zlo5-B7u$-cYKJ#Y$x)?=qa-;fpJ_Ord{*5+d~;)pF52P<!!d+2bQK{tRA*>}{WpZx
zD>(h8;XyNjso`3&Z4#sdQAnEXd)$|MJ-Zv+cHKz-gu=k-!oV>Z=}q#=G?@rAXf6gx
z1qO*zpPpGcz6yrp_GJN}dILTqbNpRbE^~0V$XY%<!M5z`Zs)0xA_wy8Zl7n01tU*9
z&qXeC+YyKn*>SsQumHT0YP;pJZXF$ENiny2JYVj1ML3@;B}jX|ocU`Mti4#UKvF}&
z`qVue6G7n08Imv!cwpjO;)V%`2#Cx?ku?`rv~KbBa{`5Hnb5iggwPXsXj~PPK_v=-
zAhbF=AtzMzzZl3$4@+bkuv@B30=+&{A7RTmw1Og8Xf2t^-QN1}o`C!F55|@Oqa(9O
zEKnE$3#xE%eTEWL9j*NtglAR%5biiA^0y27r5WH}Al<TyD32ZE3V6>ZpuCj~Rs|6Z
zBc<A&!7kpVM%r_HBb0PQ*yI{Fq1N*mU>k6f&EvW5PjI1^Zm7-|4MKc&5~wIZ7qJ!K
zk>qL$C4rrwgdjq3EOz1ueNAL_+93KdT?aTiJ(V^RKuN=`j{0fRU<J{dsX32Ct&%K3
zLMU@4&(E<5HbrYpxA6oCO(D5PIG81Z8XGB&QmjFoP0$qJ4qu_WjGd&ah#V_K6#OsP
z7g=95KrIe5?E1twLMn=4NKmr-lv4+66O|T{(xan9D^J%1)i_lBu?Rh8PuAu@hGy!N
zr5nfN<;=-ZWp^V=-|kin@WenH4`@}@rroKHm!0J@D^Q?HF{D0&lB<Y>Ce_4xIn;}C
zrUJmziEev5crxzc#>}nSovsmOPZEZsZ9U!>E6x-_7MF>}Z$TBU1BSAhP6RyCSE4+;
z<}wjOnteOy9Q64xTi-kDLp<~6NAPzDd>l#}pfqifpn#uPHT?H<;~A74e};ulBYzPz
zSRm+CXS5|QE~Qt&4MXYD??!~dQBSz?MB@eExDH$vzksYeg0c2|b{teTpsiiY0ZgEU
zhA<93f$|nqwdypF183df3Ot!12|MdaF})cTf}-)tz7lI>gkUfO#XR0=0?dLJUxZ*s
zlwh*EJ1Yk0za)#STjWfmvS(v+V_3H9rwCYh!BulT|DsTJoN&Df_pn$^!GX|h3iPDF
zMlP;JrZV!W;xOTi<<AnKRWUDBFLF~=&FTFiJaY#Rvh)sQYTRd50P!~P8FD3|JxD-@
zN&`*R^~klM$V;wIDSa}fA1e$8B2X2DTT-Mep`nAL{b_W7359A$cd;wm`HK`?B%Fvh
zOQ1&nol#BH5GLJza8a`ffvg|#`0pW@?O$Qsgh0DbN!mM6bLALjB|9|ApcLv?wY%D@
zrNgZ$Hpz4NGJuUbq4m({ORX48HNY5}R|8(~LgcET=`vHmcDHmFKLbrr@M#4!bf#^0
zc>86u?K$IE!q%o;CkY|PdWh&gHoEg7%EX`GG>MY!DF|$cv;+t^lvx!xs!?%z$w+H0
z;J&|ujwyuUK9pWHe~ZaNjq|QV$f+e<pFtj}>c<SNX&@wt!xqHIFQMa7L9->c15<Oj
z2Vm+!6N~Q%dL05I{S<PU(zOq|m`LT}&ZQK3rWtaktSAPG;$)x_JjbvPBXkfNX1W-Q
z5;M<G;sRJf${ZoI9rr6~s49TH@89ZHW8>1VzYIz}oAn!Y%f}Nr<$rl5@9LNKSAy4Z
zs;>Ce|5&G)GYpOxIdi2bd^iUgSl|gsGp{#!_!#{fnhVmn1NkXRKhnzUjkYLD5uu7P
zL%#v%D$_(UI}8x;W7xg1kU(qXjJB@<Je;-;4X8~~Q@7ys4&Fe!7V&K;mg^j7l%F$3
zTib-m;K{YP23vs;;rtN=ol_WRk(@E3TVt+7qIsmXIOtpJxl);?(A5Q}j$|4>$8%a+
z7eXjk6sCy=y=4=8{zx(XoR81wa&?Gr0{8nw3Z}@r!emlcH=54~fO&D__%3#J=z5u}
z7;dTrNZ4@%@3@4+`xOYg5;Zl@=CU~IjnKgWhha%=GZSf|nxlje%p8)?1Je*_lFNi8
zo-I^p=1mv~R|Nz)sBME|hngxwHIo|V<XpknjsE~5K;hiR?10N2p!8iZaS?GqUxBZ!
zz}JR5QjxuSekXD=9)W6v+Zlnv8<2St1-<m2ksz?0C>%FuipF7vBMJO}iqAlA6E3tC
z<10)}Iky4<K_gv&nxdWYfsA}rgrIc#jtR_aren>%zBF5#WpKR1KbKAInU{o(2gUJH
zS<fBWAagb0SjO?_YDUiJCaZz=$Kqv6MYpee*8-rs>HxQGHv~16aLx5x_%Qvcx)(4n
zm~+w0oN9YWLUS&jO4e+_=8uC?O-wi9srg(yx!SnH;Ali0YS%dw%4v~;!31#33QQW;
zxq=tOG|b|5L&puKS<g{=m<<PLJ&l1{Cy=>TtEo=F01xp7oDcrjeFSvU5Bsp20U)j<
z3c*&oUM6YgGw*@216Fzuhrm{SI=Z1-F!fRnIu07E8)N`Y1Q=~V%e?!MasTgrXM}00
zNcSHXK+KNB%S;e1Xhz*;lDhvA1~%g;MIHmAyPie<b$x++uoUL2z`~vG;_oD%fmtml
zge(}^RE6<@+|`Jav7{dRVF-TdMUFL<U_NMpxmReqHSjBImWtIFSP~4ZE|lME02JP7
z5gYhz0`L?IbC@CS*{_ALwBtc_bupboZJDP~b}#6V90xvh-p0f^mWZMUoTH7y8Vb!D
zyNUUCFcp!Ec6ox<4qiaZA0Vxs#9nbKki*^FZ<;V7j&(a<1C!F=06ag0DEEtiSH5%A
zwm!-CyA6ej2#OQ?8~0fkILKo;5JqlR1XB%6t~~t$Q7$r`Wb#kT%5cQs!4`r@c_<w7
z4c@`T4~F^fHhZ807CI@+6CVH%JrHLPv>9gHnLx|_4Woxf=N9Ob4hZ}@n%VXRX3d@6
z<!=wf8O}t&i9X~wX)|{oo52AA)o+2}PH)XPYnweVI|X<_PXJdFxmDE+ID4SYrZW08
zpk@Ek!S-MkG>y%$rkzP1uridDq4!R4eM4S)De~KPurk#9rwM-I-G?nh|7YT2=#7w|
z#TgNY-*Po<2>(#Q?Dy*<02jztV6xADVxRxCaKDI^ee9@TIcM5$rt#-99@(#8^6U5A
zuur~cpL~Ds)8xLx_h(Q0%`|iVGKQ&9*(cw#Prje^1Cd$T`IyVj8{;QF3JT)9m7kqa
zJZls^j1yT2%1Ti7$@dQ<(iXsXtxY|n-k(*`)Tr!}@7X8cvroSF>jUg(%}n1T3Hra6
z@99PNC@PhEIZ<DKUtadQS1Orw%eT7Ux%PYxQ}Sr_($F<{LGLD3z2$!A?7Dz<z_#An
z_ucQtH{HPpo8Y^#+^3mz<!in-AK%}OnzR9HPJcuIYfis%32V*;Z_c|xg4%ah^KVHN
QLGWLG&W`L4GhaUV{}8#$$N&HU

literal 0
HcmV?d00001

diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/align.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/align.cpp
index 8cb47b9..dbac7a0 100644
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/align.cpp
+++ b/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;
-                imuPos.mat[0][0] = imu_odom_.pose_[0] * 100;
-                imuPos.mat[1][0] = imu_odom_.pose_[1] * 100;
+                // std::cout << "imu received" << std::endl;
+                imu_odomPos.mat[0][0] = imu_odom_.pose_[0] * 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;
-                if (imuStartFlag == 1 && imuPos.mat[0][0] != 0 && imuPos.mat[1][0] != 0) {
-                    imuStartRoll = roll-PI/2;//跟imu摆放的位置有关，减PI / 2是为了让角度变成跟车前进方向的夹角
-
-                    imuStartPos = imuPos;
+                std::cout<<"yaw:"<<yaw<<std::endl;
+                if (imuStartFlag == 1 && imu_odomPos.mat[0][0] != 0 && imu_odomPos.mat[1][0] != 0) {
+                    imuStartyaw = yaw-PI/2;//跟imu摆放的位置有关，减PI / 2是为了让角度变成跟车前进方向的夹角
+                    imu_odom_StartPos = imu_odomPos;
                     imuStartFlag = 0;
                 }
-                //std::cout << imuStartPos.mat[0][0] << " " << imuStartPos.mat[1][0] << std::endl;
-                roll = roll - imuStartRoll;
-                imuPos = imuPos - imuStartPos;
-                detPos = imuPos - prevPos;
-                prevPos = imuPos;
+                //std::cout << imu_odom_StartPos.mat[0][0] << " " << imu_odom_StartPos.mat[1][0] << std::endl;
+                yaw = yaw - imuStartyaw;
+                imu_odomPos = imu_odomPos - imu_odom_StartPos;
+                deltaPos = imu_odomPos - prevPos;
+                prevPos = imu_odomPos;
                 if (uwbDataRxTime != uwb_->uwb_data_.uwb_t_) {
-                    std::cout << "uwb received" << std::endl;
-                    //这一步是为了把上述提到的UWB设备与imu设备不在同一坐标轴上设计的坐标轴对齐操作
-                    Rotate.mat[0][0] = cos(roll);
-                    Rotate.mat[0][1] = -sin(roll);
-                    Rotate.mat[1][0] = sin(roll);
-                    Rotate.mat[1][1] = cos(roll);
+                    // std::cout << "uwb received" << std::endl;
+					//这一步是为了把上述提到的UWB设备与imu设备不在同一坐标轴上设计的坐标轴对齐操作
+                    // Rotate.mat[0][0] = cos(yaw);
+                    // Rotate.mat[0][1] = -sin(yaw);
+                    // Rotate.mat[1][0] = sin(yaw);
+                    // 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;
-                    //后期如UWB摆放在imu正上方即删除
+                    // uwbPos = uwbPos - Rotate * uwbStartPos;
+					//后期如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 << " lightHousePos:" << lighthouse_->data.x_ << " " << lighthouse_->data.y_ << std::endl;
+                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;
 
                 
-                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_.w_[0] << "," << imu_odom_.imu_data_.w_[1] << "," << imu_odom_.imu_data_.w_[2] << ","\
-                        << qx << "," << qy << "," << qz << "," << qw << ","\
-                        << imu_odom_.vxy_ << "," <<imu_odom_.angle_v_ << ","\
-                        << imuPos.mat[0][0] << "," << imuPos.mat[1][0] << ","\
-                        << uwbPos.mat[0][0] << "," << uwbPos.mat[1][0] << ","\
-                        << syncPos.mat[0][0] << "," << syncPos.mat[1][0] << ","\
-                        << lighthouse_->data.x_ << "," << lighthouse_->data.y_ << "," << lighthouse_->data.z_ << ","\
-                        << uwb_->d[0] << "," << uwb_->d[1] << "," << uwb_->d[2] << "\n";
+            //     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_.w_[0] << "," << imu_odom_.imu_data_.w_[1] << "," << imu_odom_.imu_data_.w_[2] << ","\
+            //             << qx << "," << qy << "," << qz << "," << qw << ","\
+            //             << imu_odom_.vxy_ << "," <<imu_odom_.angle_v_ << ","\
+            //             << imu_odomPos.mat[0][0] << "," << imu_odomPos.mat[1][0] << ","\
+            //             << uwbPos.mat[0][0] << "," << uwbPos.mat[1][0] << ","\
+            //             << syncPos.mat[0][0] << "," << syncPos.mat[1][0] << ","\
+            //             << lighthouse_->data.x_ << "," << lighthouse_->data.y_ << "," << lighthouse_->data.z_ << ","\
+            //             << 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;   
     }
     
-    
-    
-    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;
-    }
+//imu原始数据，不知道有没有纠正零偏的   
     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;
     }
 
 
diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/main.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/main.cpp
index a8e5068..5d7d868 100644
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/main.cpp
+++ b/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;
-
-    // //  control data fllow in system
-    // std::thread rose_trd ([&system]() {
-    //     system->Run();
-    // });
+    sender->align_ = align;
 
     // 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]() {
-        sender->Run(uwb);
+    std::thread sender_trd ([&sender]() {
+        sender->Run();
     });
 
     std::thread align_trd ([&align]() {
diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/mapping.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/mapping.cpp
index a6240b7..d81a85d 100644
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/mapping.cpp
+++ b/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
diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/senddata.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/senddata.cpp
index 89c2ee8..e295b0a 100644
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/senddata.cpp
+++ b/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_.header.frame_id = "odom";  // 设置坐标系为 "map"
-        odom_.child_frame_id = "base_link";  // 设置坐标系为 "base_link"
-        
+        odom_imu_uwb_.header.stamp = current_time;
+        odom_imu_uwb_.header.frame_id = "odom";  // 设置坐标系为 "map"
+        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_.pose.pose.position.y = uwb->uwb_data_.y_;
-        odom_.pose.pose.position.z = 0.0;
+        odom_imu_uwb_.pose.pose.position.x = align_->syncPos.mat[0][0]/100;
+        odom_imu_uwb_.pose.pose.position.y = align_->syncPos.mat[1][0]/100;
+        odom_imu_uwb_.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_imu_uwb_.pose.pose.orientation.x = align_->qx;
+        odom_imu_uwb_.pose.pose.orientation.y = align_->qy;
+        odom_imu_uwb_.pose.pose.orientation.z = align_->qz;
+        odom_imu_uwb_.pose.pose.orientation.w = align_->qw;
 
-        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);
+    }
+