Merge pull request 'main' (#10) from ray/RobotKernal-UESTC:main into main

修改msg文件在pibot_msgs下，统一编译
2024-03-11 16:20:48 +08:00 · 2024-03-11 16:20:48 +08:00 · 97734979dc
parent 7bb2ee4d5f 5703edb223
commit 97734979dc
36 changed files with 46 additions and 1680 deletions
--- a/Code/MowingRobot/pibot_ros/ros_ws/README.md
+++ b/Code/MowingRobot/pibot_ros/ros_ws/README.md
@ -0,0 +1,12 @@
+该版本将所有的msg文件放在pibot_msgs文件下，编译时需先单独编译pibot_msgs功能包。再编译整个工作空间。
+
+单独编译pibot_msgs: `catkin_make --pkg pibot_msgs`
+
+编译整个工作空间：`catkin_make`
+
+运行目标检测： `rosrun upbot_vision main`
+
+运行定位：`rosrun upbot_location rosrun upbot_location upbot_location_node `
+
+运行跟随：`rosrun FollowingCar FollowingCar_node `
+
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/CMakeLists.txt
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/CMakeLists.txt
@ -1,60 +0,0 @@
-cmake_minimum_required(VERSION 3.0.2)
-project(FollowingCar)
-
-find_package(catkin REQUIRED COMPONENTS
-  roscpp
-  rospy
-  std_msgs
-  message_generation
-  geometry_msgs
-)
-
-# 寻找OpenCV库
-find_package(OpenCV REQUIRED)
-# 查找 Boost 库
-find_package(Boost REQUIRED)
-
-# catkin_package(
-# #  INCLUDE_DIRS include
-# #  LIBRARIES FollowingCar
-# #  CATKIN_DEPENDS roscpp rospy std_msgs
-# #  DEPENDS system_lib
-# CATKIN_DEPENDS message_runtime std_msgs geometry_msgs
-# )
-
-
-include_directories(
-# include
- ${OpenCV_INCLUDE_DIRS}
-  ${catkin_INCLUDE_DIRS}
-        include
-)
-
-add_library(${PROJECT_NAME} SHARED
-        src/system.cpp
-        src/uwb.cpp
-        src/mapping.cpp
-        src/align.cpp
-        src/Mat.cpp
-        src/lighthouse.cpp
-        # src/read_sensor_data.cpp
-        include/senddata.h src/senddata.cpp)
-
-
-add_message_files(
-    DIRECTORY msg
-    FILES
-        RawImu.msg
-        dis_info_array.msg
-        dis_info.msg
-     
-)
-generate_messages(DEPENDENCIES std_msgs geometry_msgs)
-
-catkin_package(CATKIN_DEPENDS message_runtime std_msgs geometry_msgs)
-add_executable(${PROJECT_NAME}_node src/main.cpp)
-
-
-target_link_libraries(${PROJECT_NAME}_node  ${catkin_LIBRARIES}  ${OpenCV_LIBS} ${Boost_LIBRARIES} pthread ${PROJECT_NAME})
-
-
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/Mat.h
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/Mat.h
@ -1,83 +0,0 @@
-/******************** (C) COPYRIGHT 2022 Geek************************************
-* File Name          : Mat.h
-* Current Version    : V1.0
-* Author             : logzhan
-* Date of Issued     : 2022.09.14
-* Comments           : <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
-********************************************************************************/
-/* Header File Including -----------------------------------------------------*/
-#ifndef _H_MAT_
-#define _H_MAT_
-
-#define MAT_MAX 15 //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܴ<EFBFBD><DCB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
-
-
-#include <string.h>
-#define _USE_MATH_DEFINES
-#include <math.h>
-
-class Mat
-{
-public:
-	Mat();
-	Mat(int setm,int setn,int kind);//kind=1<><31>λ<EFBFBD><CEBB>kind=0<><30><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD>ݡ<EFBFBD>
-	void Init(int setm,int setn,int kind);//kind=1<><31>λ<EFBFBD><CEBB>kind=0<><30><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD>ݡ<EFBFBD>
-
-	void Zero(void);
-	//<2F><>Щ<EFBFBD>ؼ<EFBFBD><D8BC><EFBFBD><EFBFBD><EFBFBD>Ӧ<EFBFBD><D3A6><EFBFBD><EFBFBD>Ϊprivate<74>ġ<EFBFBD><C4A1><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD>˷<EFBFBD><CBB7>㣬<EFBFBD><E3A3AC>Ҳ<EFBFBD><D2B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>public
-	int m;//<2F><><EFBFBD><EFBFBD>
-	int n;//<2F><><EFBFBD><EFBFBD>
-	double mat[MAT_MAX][MAT_MAX];//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
-
-	//<2F><><EFBFBD><EFBFBD>ľ<EFBFBD><C4BE><EFBFBD>
-	Mat SubMat(int a,int b,int lm,int ln);//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>
-	void FillSubMat(int a,int b,Mat s);//<2F><><EFBFBD><EFBFBD>Ӿ<EFBFBD><D3BE><EFBFBD>
-
-	//<2F><><EFBFBD><EFBFBD>ר<EFBFBD><D7A8>
-	double absvec();//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĳ<EFBFBD><C4B3>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>Ǹ<EFBFBD><C7B8><EFBFBD>Ԫ<EFBFBD>صľ<D8B5><C4BE><EFBFBD>ֵ<EFBFBD><D6B5>
-	double Sqrt();//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȵ<EFBFBD>ƽ<EFBFBD><C6BD>
-	friend Mat operator ^(Mat a,Mat b);//<2F><><EFBFBD>
-
-	//<2F><><EFBFBD><EFBFBD>
-	friend Mat operator *(double k,Mat a);
-	friend Mat operator *(Mat a,double k);
-	friend Mat operator /(Mat a,double k);
-	friend Mat operator *(Mat a,Mat b);
-	friend Mat operator +(Mat a,Mat b);
-	friend Mat operator -(Mat a,Mat b);
-	friend Mat operator ~(Mat a);//ת<><D7AA>	
-	friend Mat operator /(Mat a,Mat b);//a*inv(b)
-	friend Mat operator %(Mat a,Mat b);//inv(a)*b
-
-	//MAT inv();//<2F><><EFBFBD><EFBFBD><EFBFBD>
-
-private:
-	// Ϊ<><CEAA><EFBFBD>ø<EFBFBD>˹<EFBFBD><CBB9>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һЩ<D2BB><D0A9><EFBFBD><EFBFBD>
-	// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
-	void RowExchange(int a, int b);
-	// ĳһ<C4B3>г<EFBFBD><D0B3><EFBFBD>ϵ<EFBFBD><CFB5>
-	void RowMul(int a,double k);
-	// <20><>ĳһ<C4B3>мӼ<D0BC><D3BC><EFBFBD>һ<EFBFBD>еı<D0B5><C4B1><EFBFBD>
-	void RowAdd(int a,int b, double k);
-	// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
-	void ColExchange(int a, int b);
-	// ĳһ<C4B3>г<EFBFBD><D0B3><EFBFBD>ϵ<EFBFBD><CFB5>
-	void ColMul(int a,double k);
-	// <20><>ĳһ<C4B3>мӼ<D0BC><D3BC><EFBFBD>һ<EFBFBD>еı<D0B5><C4B1><EFBFBD>
-	void ColAdd(int a,int b,double k);
-	
-
-};
-
-
-
-
-
-
-#endif
-
-
-
-
-
-
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/align.h
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/align.h
@ -1,44 +0,0 @@
-#include <cmath>
-#include <ros/ros.h>
-#include <opencv2/opencv.hpp>
-#include <nav_msgs/Odometry.h>
-#include <utility>
-#include <queue>
-#include <fstream>
-#include "FollowingCar/RawImu.h"
-#include "type.h"
-#include "uwb.h"
-#include "lighthouse.h"
-#include "Mat.h"
-
-#ifndef ALIGN_H
-#define AlIGN_H
-namespace uwb_slam{
-    class Align
-    {
-    public:
-        Align(){
-
-        };
-        void Run();
-        void wheel_odomCB(const nav_msgs::Odometry& wheel_odom);
-        void imuCB(const FollowingCar::RawImu& imu);
-        void odomCB(const nav_msgs::Odometry& odom);
-
-    public:
-        ros::NodeHandle nh_;
-        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 imuDataRxTime, uwbDataRxTime, odomDataRxTime;
-        ros::Time odom_tmp_ ;
-        bool write_data_ = false;
-        cv::Mat img1;
-        std::queue<std::pair<Imu_odom_pose_data,Uwb_data>> data_queue;
-        std::shared_ptr<uwb_slam::Uwb> uwb_;
-        std::shared_ptr<uwb_slam::Lighthouse> lighthouse_;
-    };
-};
-#endif
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/lighthouse.h
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/lighthouse.h
@ -1,29 +0,0 @@
-#include <ros/ros.h>
-#include <mutex>
-#include <boost/asio.hpp>
-#include <iostream>
-#include <string>
-#include <cstdint>
-#include "type.h"
-#include <queue>
-#include <chrono>
-#ifndef __LIGHTHOUSE_H__
-#define __LIGHTHOUSE_H__
-
-namespace uwb_slam{
-class Lighthouse{
-public:
-    Lighthouse();
-    ~Lighthouse();
-    void Run();
-    void UDPRead();
-    // Listen PORT
-    int PORT = 12345;
-    int UdpSocket = -1;
-
-    LightHouseData data;
-
-    std::mutex mMutexLighthouse;
-};
-};
-#endif
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/mapping.h
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/mapping.h
@ -1,40 +0,0 @@
-#include <queue>
-#include <opencv2/opencv.hpp>
-#include <mutex>
-#include "uwb.h"
-#include "align.h"
-
-#ifndef MAPPING_H
-#define MAPPING_H
-
-
-namespace uwb_slam{
-    class Mapping
-    {
-        public:
-        const double PIXEL_SCALE = 1.0;     //xiangsudian cm
-        const int    AREA_SIZE = 1000;      //map size cm
-        Mapping() {};
-        void Run();
-        bool checkQueue();
-        void feedPos(const cv::Point2d & imuPosData, const cv::Point2d & uwbPosData, const cv::Point2d & syncPosData);
-        void process();
-        std::mutex mMutexMap;
-        std::shared_ptr<uwb_slam::Uwb> uwb_;
-        std::shared_ptr<uwb_slam::Align> align_;
-
-        private:
-        int realWidth, realHeight;
-        std::queue<std::vector<cv::Point2d>> posDataQueue;
-        std::vector<cv::Point2d> posData = std::vector<cv::Point2d>(3, {-1, -1});
-        //cv::Point2d imuPoint = {-1,-1};
-        // std::queue<cv::Point2d> posDataQueue;
-        
-
-        bool readPos = false;
-        cv::Mat img;
-    };
-
-}
-
-#endif
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/read_sensor_data.h
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/read_sensor_data.h
@ -1,34 +0,0 @@
-#include <ros/ros.h>
-#include "nav_msgs/Odometry.h"
-#include "geometry_msgs/Twist.h"
-#include "sensor_msgs/Imu.h"
-#include "geometry_msgs/PoseStamped.h"
-#include "geometry_msgs/PoseWithCovarianceStamped.h"
-#include <boost/thread/mutex.hpp>
-#include "type.h"
-#include "uwb.h"
-
-
-#ifndef READ_SENSOR_DATA_H
-#define READ_SENSOR_DATA_H
-
-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
-    {
-        public:
-        Read_sensor_data();
-
-        void Run(int argc, char* argv[]);
-        //void set_uwb(Uwb * uwb);
-        void imu_call_back(const ImuConstPtr& imu);
-        void odom_call_back(const OdomConstPtr& odom);
-
-        private:
-        ros::Subscriber imu_sub_;
-        ros::Subscriber odom_sub_;
-        
-    };
-}
-#endif
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/senddata.h
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/senddata.h
@ -1,50 +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"
-#include <geometry_msgs/Twist.h>
-#include "FollowingCar/dis_info.h"
-#include "FollowingCar/dis_info_array.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);
-        void stereoCB(const FollowingCar::dis_info_array::ConstPtr& stereo);
-
-        std::shared_ptr<uwb_slam::Uwb> uwb_;
-
-
-        std::mutex mMutexSend;
-
-        private:
-        ros::Publisher position_pub_;
-	    ros::Publisher cmd_vel_pub_;
-        ros::Subscriber odom_sub_;
-    	ros::Subscriber obstacles_sub_;
-        ros::NodeHandle nh_;
-        
-        nav_msgs::Odometry odom_;//odom的消息类型
-        nav_msgs::Odometry sub_odom_;//odom的消息类型
-	    geometry_msgs::Twist cmd_vel_;
-
-        bool flag_ = 0; 
-
-      
-    };
-
-}
-
-#endif
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/system.h
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/system.h
@ -1,32 +0,0 @@
-#include <thread>
-#include <string>
-#include "mapping.h"
-#include "uwb.h"
-#include "senddata.h"
-// #include "align.h"
-#include <iostream>
-
-#ifndef SYSTEM_H
-#define SYSTEM_H
-
-namespace uwb_slam{
-    class System{
-
-    public:
-        System() {
-        }
-        void Run();
-    public:
-       
-        std::shared_ptr<uwb_slam::Mapping>Mapping_;
-        std::shared_ptr<uwb_slam::Uwb>Uwb_;
-        std::shared_ptr<uwb_slam::Senddata>Sender_;
-        std::shared_ptr<uwb_slam::Align>Align_;
-        std::shared_ptr<uwb_slam::Lighthouse>Lighthouse_;
-
-        // Uwb* Uwb_ ;
-        // Senddata* Sender_;
-        // Mapping* Mapping_;
-    };
-}
-#endif
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/type.h
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/type.h
@ -1,50 +0,0 @@
-#include <ros/ros.h>
-#include <ros/time.h>
-#ifndef TYPE_H
-#define TYPE_H
-
-namespace uwb_slam{
-struct Imu_data
-{
-    ros::Time imu_t_;
-    double a_[3];
-    double g_[3];
-    double m_[3];
-    Imu_data(){};
-    Imu_data(double ax,double ay,double az, double gx, double gy, double gz, double mx, double my, double mz)
-    :a_{ax,ay,az},g_{gx,gy,gz},m_{mx,my,mz}{};
-};
-
-
-
-struct Imu_odom_pose_data
-{
-    Imu_data imu_data_;
-    double pose_[3];
-    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){};
-};
-
-struct  Uwb_data
-{
-    float x_,y_;
-    ros::Time uwb_t_;
-    Uwb_data(){};
-    Uwb_data(float x,float y,float t):x_(x),y_(y),uwb_t_(t){};
-};
-
-struct LightHouseData
-{
-    float x_,y_,z_;
-    float qw_,qx_,qy_,qz_;
-    ros::Time lighthouse_t_;
-    LightHouseData(){};
-    LightHouseData(float x,float y,float z, float qw, float qx, float qy, float qz, float t):
-        x_(x),y_(y),z_(z),qw_(qw), qx_(qx), qy_(qy), qz_(qz), lighthouse_t_(t){};
-};
-
-}
-#endif
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/uwb.h
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/uwb.h
@ -1,60 +0,0 @@
-#include <ros/ros.h>
-#include <mutex>
-#include <boost/asio.hpp>
-#include <iostream>
-#include <string>
-#include <cstdint>
-#include "type.h"
-#include <queue>
-#include <chrono>
-#ifndef UWB_H
-#define UWB_H
-
-#define PI acos(-1)
-namespace uwb_slam{
-
-    class Uwb
-    {
-    public:
-        Uwb();
-        void Run();
-        bool checknewdata();
-        void feed_imu_odom_pose_data();
-        void Serread();
-       
-
- 
-    public:
-        int pre_seq = -1;
-        int cur_seq = -1;
-        int AnchorNum = 3;
-        int AnchorPos[3][3]={
-            -240, 400, 113,\
-            240, 400, 113,\
-            -400, -240, 113
-        };//基站坐标，序号+三维坐标
-        int d[3];
-        int aoa[3];
-     
-    // std::queue<Imu_odom_pose_data> v_buffer_imu_odom_pose_data_;
-   
-       
-        Uwb_data uwb_data_;
-        // ros_merge_test::RawImu sub_imu_;
-        // std::queue<Imu_odom_pose_data > imu_odom_queue_;
-        // std::queue<Uwb_data> uwb_data_queue_;
-        std::mutex mMutexUwb;
-    //boost::asio::io_service io;
-    //boost::asio::serial_port s_port;
-
-    // Imu_odom_pose_data imu_odom_pose_data_;
-    };
-
-};
-
-
-
-
-
-
-#endif
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/msg/RawImu.msg
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/msg/RawImu.msg
@ -1,7 +0,0 @@
-Header header
-bool accelerometer
-bool gyroscope
-bool magnetometer
-geometry_msgs/Vector3 raw_linear_acceleration
-geometry_msgs/Vector3 raw_angular_velocity
-geometry_msgs/Vector3 raw_magnetic_field
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/msg/dis_info_array.msg
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/msg/dis_info_array.msg
@ -1 +0,0 @@
-FollowingCar/dis_info[] dis
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/package.xml
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/package.xml
@ -1,73 +0,0 @@
-<?xml version="1.0"?>
-<package format="2">
-  <name>FollowingCar</name>
-  <version>0.0.0</version>
-  <description>The FollowingCar package</description>
-
-  <!-- One maintainer tag required, multiple allowed, one person per tag -->
-  <!-- Example:  -->
-  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
-  <maintainer email="luoruidi@todo.todo">luoruidi</maintainer>
-
-
-  <!-- One license tag required, multiple allowed, one license per tag -->
-  <!-- Commonly used license strings: -->
-  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
-  <license>TODO</license>
-
-
-  <!-- Url tags are optional, but multiple are allowed, one per tag -->
-  <!-- Optional attribute type can be: website, bugtracker, or repository -->
-  <!-- Example: -->
-  <!-- <url type="website">http://wiki.ros.org/FollowingCar</url> -->
-
-
-  <!-- Author tags are optional, multiple are allowed, one per tag -->
-  <!-- Authors do not have to be maintainers, but could be -->
-  <!-- Example: -->
-  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
-
-
-  <!-- The *depend tags are used to specify dependencies -->
-  <!-- Dependencies can be catkin packages or system dependencies -->
-  <!-- Examples: -->
-  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
-  <!--   <depend>roscpp</depend> -->
-  <!--   Note that this is equivalent to the following: -->
-  <!--   <build_depend>roscpp</build_depend> -->
-  <!--   <exec_depend>roscpp</exec_depend> -->
-  <!-- Use build_depend for packages you need at compile time: -->
-  <!--   <build_depend>message_generation</build_depend> -->
-  <!-- Use build_export_depend for packages you need in order to build against this package: -->
-  <!--   <build_export_depend>message_generation</build_export_depend> -->
-  <!-- Use buildtool_depend for build tool packages: -->
-  <!--   <buildtool_depend>catkin</buildtool_depend> -->
-  <!-- Use exec_depend for packages you need at runtime: -->
-  <!--   <exec_depend>message_runtime</exec_depend> -->
-  <!-- Use test_depend for packages you need only for testing: -->
-  <!--   <test_depend>gtest</test_depend> -->
-  <!-- Use doc_depend for packages you need only for building documentation: -->
-  <!--   <doc_depend>doxygen</doc_depend> -->
-  <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>rospy</build_depend>
-  <build_depend>std_msgs</build_depend>
-  <build_depend>geometry_msgs</build_depend>
-  <build_depend>message_generation</build_depend>
-
-  <build_export_depend>roscpp</build_export_depend>
-  <build_export_depend>rospy</build_export_depend>
-  <build_export_depend>std_msgs</build_export_depend>
-  <exec_depend>roscpp</exec_depend>
-  <exec_depend>rospy</exec_depend>
-  <exec_depend>std_msgs</exec_depend>
-  <exec_depend>message_runtime</exec_depend>
-  <exec_depend>geometry_msgs</exec_depend>
-
-
-  <!-- The export tag contains other, unspecified, tags -->
-  <export>
-    <!-- Other tools can request additional information be placed here -->
-
-  </export>
-</package>
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/Mat.cpp
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/Mat.cpp
@ -1,368 +0,0 @@
-/******************** (C) COPYRIGHT 2022 Geek************************************
-* File Name          : Mat.cpp
-* Current Version    : V1.0
-* Author             : logzhan
-* Date of Issued     : 2022.09.14
-* Comments           : <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
-********************************************************************************/
-/* Header File Including -----------------------------------------------------*/
-#include "Mat.h"
-
-double mind(double a,double b)
-{
-	double c = a;
-	if(b < c){
-		c = b;
-	}
-	return c;
-}
-
-int mini(int a,int b)
-{
-	int c=a;
-	if(b<c)
-	{
-		c=b;
-	}
-	return c;
-	
-}
-
-//<2F><>Ҫ<EFBFBD>ڳ<EFBFBD>Ա<EFBFBD><D4B1><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD><DAB5>ù<EFBFBD><C3B9>캯<EFBFBD><ECBAAF>
-Mat::Mat()
-{
-	Init(1,1,0);
-}
-
-Mat::Mat(int setm,int setn,int kind)
-{
-	Init(setm,setn,kind);
-}
-
-void Mat::Init(int setm,int setn,int kind)
-{
-	this->m = setm;
-	this->n = setn;
-	if((kind==0)||(kind==1))
-	{
-		memset(mat,0,MAT_MAX*MAT_MAX*sizeof(double));
-	}
-
-	if(kind==1)
-	{
-		int x;
-		//Cԭ<43>е<EFBFBD>max min<69>ᵼ<EFBFBD><E1B5BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ա<EFBFBD><D4B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>и<EFBFBD><D0B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ķ<EFBFBD><C4B6><EFBFBD><EFBFBD><EFBFBD>Ҫֱ<D2AA>ӷŵ<D3B7>max<61><78><EFBFBD>档
-		int xend = mini(this->m, this->n);
-		for(x=0;x < xend;x++){
-			mat[x][x] = 1;
-		}
-	}
-}
-void Mat::Zero() {
-
-}
-
-Mat Mat::SubMat(int a,int b,int lm,int ln)
-{
-
-	int aend=a+lm-1;
-	int bend=b+ln-1;
-
-
-	Mat s(lm,ln,-1);
-	int x,y;
-	for(x=0;x<lm;x++)
-	{
-		for(y=0;y<ln;y++)
-		{
-			s.mat[x][y]=mat[a+x][b+y];
-		}
-	}
-	return s;
-}
-
-
-void Mat::FillSubMat(int a,int b,Mat s)
-{
-	int x,y;
-	for(x=0;x<s.m;x++)
-	{
-		for(y=0;y<s.n;y++)
-		{
-			mat[a+x][b+y]=s.mat[x][y];
-		}
-	}
-}
-
-
-Mat operator *(double k, Mat a)
-{
-	Mat b(a.m,a.n,-1);
-	int x,y;
-	for(x=0;x<a.m;x++)
-	{
-		for(y=0;y<a.n;y++)
-		{
-			b.mat[x][y]=k*a.mat[x][y];
-		}
-	}
-	return b;
-}
-Mat operator *(Mat a,double k)
-{
-	return k*a;
-}
-Mat operator /(Mat a,double k)
-{
-	return (1/k)*a;
-}
-Mat operator *(Mat a,Mat b)
-{
-	Mat c(a.m,b.n,-1);
-	int x,y,z;
-	double s;
-	for(x=0;x<a.m;x++)
-	{
-		for(y=0;y<b.n;y++)
-		{
-			s=0;
-			for(z=0;z<a.n;z++)
-			{
-				s=s+a.mat[x][z]*b.mat[z][y];
-			}
-			c.mat[x][y]=s;
-		}
-	}
-	return c;
-
-}
-
-Mat operator +(Mat a,Mat b)
-{
-
-	Mat c=a;
-	int x,y;
-	for(x=0;x<c.m;x++)
-	{
-		for(y=0;y<c.n;y++)
-		{
-			c.mat[x][y]+=b.mat[x][y];
-		}
-	}
-	return c;
-}
-Mat operator -(Mat a,Mat b)
-{
-
-	Mat c=a;
-	int x,y;
-	for(x=0;x<c.m;x++)
-	{
-		for(y=0;y<c.n;y++)
-		{
-			c.mat[x][y]-=b.mat[x][y];
-		}
-	}
-	return c;
-}
-Mat operator ~(Mat a)
-{
-	Mat b(a.n,a.m,-1);
-	int x,y;
-	for(x=0;x<a.m;x++)
-	{
-		for(y=0;y<a.n;y++)
-		{
-			b.mat[y][x]=a.mat[x][y];
-
-		}
-	}
-	return b;
-}
-
-
-
-Mat operator /(Mat a,Mat b)
-{
-	//<2F><>˹<EFBFBD><CBB9>Ԫ<EFBFBD><D4AA>
-
-	int x,xb;
-	for(x=0;x<b.n;x++)
-	{
-		//<2F><><EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD>ѵ<EFBFBD><D1B5>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD>ʼԪ<CABC><D4AA><EFBFBD><EFBFBD><EFBFBD>
-		double s=0;
-		int p=x;
-		double sxb;
-		for(xb=x;xb<b.n;xb++)
-		{
-			sxb=fabs(b.mat[x][xb]);
-			if(sxb>s)
-			{
-				p=xb;
-				s=sxb;
-			}
-		}
-		//ͬʱ<CDAC>任<EFBFBD><E4BBBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
-		if(x!=p)
-		{
-			a.ColExchange(x,p);
-			b.ColExchange(x,p);
-		}
-
-		//<2F><>һ<EFBFBD>й<EFBFBD>һ
-		double k=1/b.mat[x][x];//<2F><>һ<EFBFBD>䲻ҪǶ<D2AA>׵<EFBFBD><D7B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD>²<EFBFBD>ͬ<EFBFBD><CDAC><EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD><C2BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
-		a.ColMul(x,k);
-		b.ColMul(x,k);
-
-		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>й<EFBFBD><D0B9><EFBFBD>
-		for(xb=0;xb<b.n;xb++)
-		{
-			if(xb!=x)
-			{
-				k=(-b.mat[x][xb]);
-				a.ColAdd(xb,x,k);
-				b.ColAdd(xb,x,k);
-			}
-		}
-	}
-	
-	return a;
-}
-
-
-Mat operator %(Mat a,Mat b)
-{
-	//<2F><>˹<EFBFBD><CBB9>Ԫ<EFBFBD><D4AA>
-	int x,xb;
-	for(x=0;x<a.m;x++)
-	{
-		//<2F><><EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD>ѵ<EFBFBD><D1B5>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD>ʼԪ<CABC><D4AA><EFBFBD><EFBFBD><EFBFBD>
-		double s=0;
-		int p=x;
-		double sxb;
-		for(xb=x;xb<a.m;xb++)
-		{
-			sxb=fabs(a.mat[xb][x]);
-			if(sxb>s)
-			{
-				p=xb;
-				s=sxb;
-			}
-		}
-		//ͬʱ<CDAC>任<EFBFBD><E4BBBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
-		if(x!=p)
-		{
-			a.RowExchange(x,p);
-			b.RowExchange(x,p);
-		}
-
-		//<2F><>һ<EFBFBD>й<EFBFBD>һ
-		double k=1/a.mat[x][x];//<2F><>һ<EFBFBD>䲻ҪǶ<D2AA>׵<EFBFBD><D7B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD>²<EFBFBD>ͬ<EFBFBD><CDAC><EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD><C2BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
-		a.RowMul(x,k);
-		b.RowMul(x,k);
-
-		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>й<EFBFBD><D0B9><EFBFBD>
-		for(xb=0;xb<a.m;xb++)
-		{
-			if(xb!=x)
-			{
-				k=(-a.mat[xb][x]);
-				a.RowAdd(xb,x,k);
-				b.RowAdd(xb,x,k);
-			}
-		}
-	}
-	
-	return b;
-}
-
-
-void Mat::RowExchange(int a, int b)
-{
-	double s[MAT_MAX];
-	int ncpy=n*sizeof(double);
-	memcpy(s,mat[a],ncpy);
-	memcpy(mat[a],mat[b],ncpy);
-	memcpy(mat[b],s,ncpy);
-}
-void Mat::RowMul(int a,double k)
-{
-	int y;
-	for(y=0;y<n;y++)
-	{
-		mat[a][y]= mat[a][y]*k;
-	}
-}
-void Mat::RowAdd(int a,int b,double k)
-{
-	int y;
-	for(y=0;y<n;y++)
-	{
-		mat[a][y]= mat[a][y]+ mat[b][y]*k;
-	}
-}
-
-void Mat::ColExchange(int a, int b)
-{
-	double s;
-	int x;
-	for(x=0;x<m;x++)
-	{
-		s=mat[x][a];
-		mat[x][a]=mat[x][b];
-		mat[x][b]=s;
-	}
-}
-void Mat::ColMul(int a,double k)
-{
-	int x;
-	for(x=0;x<m;x++)
-	{
-		mat[x][a]=mat[x][a]*k;
-	}
-}
-void Mat::ColAdd(int a,int b,double k)
-{
-	int x;
-	for(x=0;x<m;x++)
-	{
-		mat[x][a]=mat[x][a]+mat[x][b]*k;
-	}
-}
-
-
-
-double Mat::Sqrt()
-{
-	int x;
-	double numx;
-	double s=0;
-	for(x=0;x<m;x++)
-	{
-		numx=mat[x][0];
-		s+=(numx*numx);
-	}
-	return s;
-}
-double Mat::absvec()
-{
-	return sqrt(Sqrt());
-}
-
-Mat operator ^(Mat a, Mat b)
-{
-	double ax=a.mat[0][0];
-	double ay=a.mat[1][0];
-	double az=a.mat[2][0];
-	double bx=b.mat[0][0];
-	double by=b.mat[1][0];
-	double bz=b.mat[2][0];
-
-	Mat c(3,1,-1);
-	c.mat[0][0]=ay*bz-az*by;
-	c.mat[1][0]=(-(ax*bz-az*bx));
-	c.mat[2][0]=ax*by-ay*bx;
-
-	return c;
-}
-
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/align.cpp
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/align.cpp
@ -1,101 +0,0 @@
-#include "align.h"
-
-namespace uwb_slam{
-    void Align::Run()
-    {
-        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);
-
-        std::ofstream outfile("data.txt",std::ofstream::out);
-        if(outfile.is_open()) {
-            std::cout << "start saving data" << std::endl;
-            outfile << "imuDataRxTime,odomDataRxTime,uwbDataRxTime,"\
-                    << "aX,aY,aZ,"\
-                    << "gX,gY,gZ"\
-                    << "mX,mY,mZ,"\
-                    << "qW,qX,qY,qZ,"\
-                    << "vXY,angleV,"\
-                    << "imuPosX,imuPosY,"\
-                    << "lightHousePosX,lightHousePosY,lightHousePosZ,"\
-                    << "lightHouseQW,lightHouseQX,lightHouseQY,lightHouseQZ"\
-                    << "d1,d2,d3\n";
-        } else {
-            std::cout<<"file can not open"<<std::endl;
-        }
-
-        while(1){
-            if(imuDataRxTime!=imu_odom_.imu_data_.imu_t_){
-                //std::cout << "imu received" << std::endl;
-                imuDataRxTime = imu_odom_.imu_data_.imu_t_;
-                odomDataRxTime = odom_tmp_;
-                uwbDataRxTime = uwb_->uwb_data_.uwb_t_;
-    
-                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_.g_[0] << "," << imu_odom_.imu_data_.g_[1] << "," << imu_odom_.imu_data_.g_[2] << ","\
-                        << imu_odom_.imu_data_.m_[0] << "," << imu_odom_.imu_data_.m_[1] << "," << imu_odom_.imu_data_.m_[2] << ","\
-                        << imu_odom_.quat_[0] << "," << imu_odom_.quat_[0] << "," << imu_odom_.quat_[0] << "," << imu_odom_.quat_[0] << ","\
-                        << imu_odom_.vxy_ << "," << imu_odom_.angle_v_ << ","\
-                        << imu_odom_.pose_[0] << "," << imu_odom_.pose_[1] << ","\
-                        << lighthouse_->data.x_ << "," << lighthouse_->data.y_ << "," << lighthouse_->data.z_ << ","\
-                        << lighthouse_->data.qw_ << "," << lighthouse_->data.qx_ << "," << lighthouse_->data.qy_ << "," << lighthouse_->data.qz_ << ","\
-                        << uwb_->d[0] << "," << uwb_->d[1] << "," << uwb_->d[2] << "\n";
-            }
-        }
-        // outfile.close();
-        // 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;
-    }
-    void Align::imuCB(const FollowingCar::RawImu& imu)
-    {
-        imu_odom_.imu_data_.imu_t_ = imu.header.stamp;
-        imu_odom_.imu_data_.a_[0] = imu.raw_linear_acceleration.x;
-        imu_odom_.imu_data_.a_[1] = imu.raw_linear_acceleration.y;
-        imu_odom_.imu_data_.a_[2] = imu.raw_linear_acceleration.z;
-
-        imu_odom_.imu_data_.g_[0] = imu.raw_angular_velocity.x;
-        imu_odom_.imu_data_.g_[1] = imu.raw_angular_velocity.y;
-        imu_odom_.imu_data_.g_[2] = imu.raw_angular_velocity.z;
-
-        imu_odom_.imu_data_.m_[0] = imu.raw_magnetic_field.x;
-        imu_odom_.imu_data_.m_[1] = imu.raw_magnetic_field.y;
-        imu_odom_.imu_data_.m_[2] = imu.raw_magnetic_field.z;
-
-        return;
-    }
-
-    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.w;
-        imu_odom_.quat_[1] = odom.pose.pose.orientation.x;
-        imu_odom_.quat_[2] = odom.pose.pose.orientation.y;
-        imu_odom_.quat_[3] = odom.pose.pose.orientation.z;
-    }
-
-};
-
-
-
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/lighthouse.cpp
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/lighthouse.cpp
@ -1,79 +0,0 @@
-#include "lighthouse.h"
-#include <cmath>
-#include <string>
-
-namespace uwb_slam{
-    Lighthouse::Lighthouse(){
-        std::cout << "Start Run. "  << std::endl;
-
-        // 创建UDP套接字
-        UdpSocket = socket(AF_INET, SOCK_DGRAM, 0);
-        if (UdpSocket == -1) {
-            std::cerr << "Error creating UDP socket." << std::endl;
-            return;
-        }
-
-        std::cout << "Creating UDP socket sucess. "  << std::endl;
-
-        // 设置套接字地址结构
-        sockaddr_in udpAddress;
-        std::memset(&udpAddress, 0, sizeof(udpAddress));
-        udpAddress.sin_family = AF_INET;
-        udpAddress.sin_addr.s_addr = htonl(INADDR_ANY);
-        udpAddress.sin_port = htons(PORT);
-
-        // 将套接字绑定到地址
-        if (bind(UdpSocket, (struct sockaddr*)&udpAddress, sizeof(udpAddress)) == -1) {
-            std::cerr << "Error binding UDP socket." << std::endl;
-            close(UdpSocket);
-        }
-    }
-
-    Lighthouse::~Lighthouse(){
-        close(UdpSocket);
-    }
-
-    void Lighthouse::Run() {
-        while(1){
-            // 这是一个阻塞线程
-            this->UDPRead();
-        }
-    }
-
-    void Lighthouse::UDPRead(){
-        char buffer[1024];
-        ssize_t bytesRead = recvfrom(UdpSocket, buffer, sizeof(buffer), 0, nullptr, nullptr);
-        if (bytesRead == -1) {
-            std::cerr << "Error receiving data." << std::endl;
-            return;
-        }
-
-        std::string data(buffer);
-
-        float x,y,z,qw,qx,qy,qz;
-
-        qw = 1.0;
-
-        sscanf(data.c_str(),"%f %f %f %f %f %f",&x,&y,&z,&qx,&qy,&qz);
-
-        mMutexLighthouse.lock();
-
-        this->data.x_ = x;
-        this->data.y_ = y;
-        this->data.z_ = z;
-
-        this->data.qw_ = qw;
-        this->data.qx_ = qx;
-        this->data.qy_ = qy;
-        this->data.qz_ = qz;
-
-        
-
-        mMutexLighthouse.unlock();
-
-        // 打印接收到的消息
-        buffer[bytesRead] = '\0';
-        //std::cout << "Received: " << buffer << std::endl;
-
-    }
-};
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/main.cpp
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/main.cpp
@ -1,65 +0,0 @@
-#include "../include/system.h"
-#include "../include/uwb.h"
-#include "../include/lighthouse.h"
-#include "Mat.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>();
-    std::shared_ptr<uwb_slam::Lighthouse> lighthouse = std::make_shared<uwb_slam::Lighthouse>();
-
-    
-    system->Mapping_ = mp;
-    system->Uwb_ = uwb;
-    system->Sender_ = sender;
-    system->Align_ = align;
-    system->Lighthouse_ = lighthouse;
-
-    mp->uwb_ = system->Uwb_;
-    mp->align_ = system->Align_;
-    align->uwb_ = system->Uwb_;
-    align->lighthouse_ = system->Lighthouse_;
-    sender->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();
-    // });
-
-    // std::thread lighthouse_trd ([&lighthouse]() {
-    //     lighthouse->Run();
-    // });
-
-    ros::spin(); 
-}
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/mapping.cpp
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/mapping.cpp
@ -1,149 +0,0 @@
-#include "mapping.h"
-#include <mutex>
-#include <unistd.h>// 包含 usleep() 函数所在的头文件
-#include <opencv2/core.hpp>
-#include <opencv2/highgui.hpp>
-
-namespace uwb_slam
-{
-    bool Mapping::checkQueue()
-    {
-        //std::unique_lock<std::mutex> lock(mMutexMap);
-        return !posDataQueue.empty();
-    }
-
-    void Mapping::feedPos(const cv::Point2d & imuPosData, const cv::Point2d & uwbPosData, const cv::Point2d & syncPosData)
-    {
-        //std::unique_lock<std::mutex> lock(mMutexMap);
-        posDataQueue.push({imuPosData, uwbPosData, syncPosData});
-
-    }
-    
-
-    void Mapping::process()
-    {
-        {   
-            
-            //std::unique_lock<std::mutex> lock(mMutexMap);
-            //std::cout << "SIZE:  " <<posDataQueue.size() << std::endl;
-            posData = posDataQueue.front();
-            //std::cout << "x: " <<cur_point.x << " y:" << cur_point.y << std::endl;
-            posDataQueue.pop();
-        }
-        /*生成图*/
-
-        int imuPosPointX = posData[0].x / PIXEL_SCALE + ( fmod(posData[0].x ,PIXEL_SCALE) != 0) + realWidth / 2;
-        int imuPosPointY = posData[0].y / PIXEL_SCALE + ( fmod(posData[0].y ,PIXEL_SCALE) != 0) + realHeight / 2;
-        int uwbPosPointX = posData[1].x / PIXEL_SCALE + ( fmod(posData[1].x ,PIXEL_SCALE) != 0) + realWidth / 2;
-        int uwbPosPointY = posData[1].y / PIXEL_SCALE + ( fmod(posData[1].y ,PIXEL_SCALE) != 0) + realHeight / 2;
-        int syncPosPointX = posData[2].x / PIXEL_SCALE + ( fmod(posData[2].x ,PIXEL_SCALE) != 0) + realWidth / 2;
-        int syncPosPointY = posData[2].y / PIXEL_SCALE + ( fmod(posData[2].y ,PIXEL_SCALE) != 0) + realHeight / 2;
-        // std::cout << "syncPos:" <<posData[2].x << " " << posData[2].y;
-        // std::cout << " uwbPos:" <<posData[1].x << " " << posData[1].x;
-        // std::cout << " imuPos:" <<posData[0].x << " " << posData[0].y << std::endl;
-
-        // std::cout << "syncPos:" <<syncPosPointX << " " << syncPosPointY;
-        // std::cout << " uwbPos:" <<uwbPosPointX << " " << uwbPosPointY;
-        // std::cout << " imuPos:" <<imuPosPointX << " " << imuPosPointY << std::endl;
-        // img.at<unsigned char>(imuPosPointY, imuPosPointX) = 0;   
-        img.at<cv::Vec3b>(imuPosPointY, imuPosPointX) = cv::Vec3b(0,0,255);     //imu trace is red
-        img.at<cv::Vec3b>(uwbPosPointY, uwbPosPointX) = cv::Vec3b(0,255,0);     //uwb trace is green
-        img.at<cv::Vec3b>(syncPosPointY, syncPosPointX) = cv::Vec3b(255,0,0);   //sync trace is blue
-    }
-
-
-    void Mapping::Run()
-    {
-        
-        //int key = cv::waitKey(0);//等待用户按下按键
-        //std::cout << key << std::endl;
-        realWidth = AREA_SIZE / PIXEL_SCALE;
-        realHeight = AREA_SIZE / PIXEL_SCALE;
-
-        img = cv::Mat(realHeight, realWidth, CV_8UC3, cv::Scalar(255,255,255));
-        //cankao
-        for (int j=0;j<AREA_SIZE / PIXEL_SCALE;j+=10)
-            for (int i=0;i<AREA_SIZE / PIXEL_SCALE;i+=10)
-                img.at<cv::Vec3b>(j,i)= cv::Vec3b(0,0,0);
-        
-        for (int j=realHeight/2-1; j<=realHeight/2+1; j+=1)
-            for (int i=realWidth/2-1; i<=realWidth/2+1; i+=1)
-                img.at<cv::Vec3b>(j,i)= cv::Vec3b(0,0,0);
-        int i = 0, j = 0;
-        img.at<cv::Vec3b>(j,i)= cv::Vec3b(0,0,0);
-
-
-
-        cv::imshow("Image",img);
-
-        // bool printFlag = 0;
-        // std::ofstream outfile("data.txt",std::ofstream::out);
-        // if(outfile.is_open()) {
-        //     std::cout << "start saving data" << key << std::endl;
-        //     printFlag = 1;
-        // } else {
-        //     std::cout<<"file can not open"<<std::endl;
-        // }
-
-        /*
-        std::cout << "waiting" <<std::endl;
-        int key = cv::waitKey(0);
-        if (key == 'q' || key == 27) {
-            this->feed_uwb_data(cv::Point2d(uwb_->x,uwb_->y));
-            readPos = true;
-            std::cout << "non" << key << std::endl;
-            cv::destroyAllWindows();
-         }
-        */
-        while(1){
-            int key = cv::waitKey(0);
-            if (key == 'q' ) {
-                readPos = true;
-                std::cout << "start mapping" << key << std::endl;
-        
-                //cv::destroyAllWindows();
-            }
-            while( readPos )//按下空格键
-            {
-                int key2 = cv::waitKey(1);
-                if (key2 == 'q') {
-                    //TODO: save
-
-                    std::string pngimage="/home/firefly/Project_Ros11/Project_Ros1/src/FollowingCar/Map/output_image.png";//保存的图片文件路径
-                    cv::imwrite(pngimage,img);
-                    readPos = false;
-                    
-                    // outfile.close();
-                    // printFlag = 0;
-                    // std::cout<< "Data written to file." << std::endl;
-                    
-                    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(uwb_->x, uwb_->y));
-
-                //uwb xieru
-                //std::cout << "cur_SEQ: " <<uwb_->cur_seq  << std::endl;
-
-                if(checkQueue())
-                {
-                    //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);
-      
-        /*ros 发送图片给导航 */
-    }
-} // namespace uwb_slam
-
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/read_sensor_data.cpp
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/read_sensor_data.cpp
@ -1,30 +0,0 @@
-#include <read_sensor_data.h>
-
-namespace uwb_slam {
-
-
-    //void Read_sensor_data::set_uwb(){}
-
-
-    void Read_sensor_data::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){
-        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[]){
-
-        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::spin();
-    }
-
-}
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/senddata.cpp
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/senddata.cpp
@ -1,147 +0,0 @@
-#include "senddata.h"
-
-#define angleLimit 20
-#define disLimit 200
-
-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);
-        cmd_vel_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel",50);
-    	obstacles_sub_ = nh_.subscribe<FollowingCar::dis_info_array>("ceju_info",1,boost::bind(&Senddata::stereoCB,this,_1));
-        // odom_sub_=nh_.subscribe("odom",10,&Senddata::odomCB,this);
-        while(ros::ok()){
-            // publishOdometry(uwb);
-            if (flag_)
-            {
-                cmd_vel_.linear.x = 0; 
-	            cmd_vel_.angular.z = -1;
-                for (int i = 0; i < 17; ++i) {  
-                    cmd_vel_pub_.publish(cmd_vel_);
-                    loop_rate.sleep();
-                }
-
-                cmd_vel_.linear.x = 0.2; 
-	            cmd_vel_.angular.z = 0;
-                for (int i = 0; i < 20; ++i) {  
-                    cmd_vel_pub_.publish(cmd_vel_);
-                    loop_rate.sleep();
-                }
-
-                cmd_vel_.linear.x = 0; 
-	            cmd_vel_.angular.z = 1;
-                for (int i = 0; i < 16; ++i) {  
-                    cmd_vel_pub_.publish(cmd_vel_);
-                    loop_rate.sleep();
-                }
-
-                cmd_vel_.linear.x = 0; 
-	            cmd_vel_.angular.z = 0;
-                flag_ = 0;
-            }
-            else if(abs(uwb->aoa[0]) > 180 || abs(uwb->d[0]) > 2000)
-            {
-                cmd_vel_.angular.z = 0;
-                cmd_vel_.linear.x = 0;
-            }
-            else if(uwb->aoa[0] > angleLimit && uwb->aoa[0] <= 180)
-            {
-                float angularSpeed = (float)uwb->aoa[0] / 100 + 1;
-                cmd_vel_.angular.z = angularSpeed;
-                cmd_vel_.linear.x = 0;
-            }
-            else if(uwb->aoa[0] < -angleLimit)
-            {
-                float angularSpeed = (float)uwb->aoa[0] / 100 - 1;
-                cmd_vel_.angular.z = angularSpeed;
-                cmd_vel_.linear.x = 0;
-            }
-            else if(uwb->d[0] > disLimit)
-            {
-                float lineSpeed = (float)uwb->d[0] / 1000 + 0.1;
-                cmd_vel_.angular.z = 0;
-                cmd_vel_.linear.x = lineSpeed;
-            }
-            else if(uwb->d[0] < disLimit - 30)
-            {
-                cmd_vel_.angular.z = 0;
-                cmd_vel_.linear.x = -0.2;
-            }
-            else 
-            {
-                cmd_vel_.angular.z = 0;
-                cmd_vel_.linear.x = 0; 
-            }
-            cmd_vel_pub_.publish(cmd_vel_);
-            ros::spinOnce();
-            loop_rate.sleep();
-        }
-
-
-    }
-    // void Senddata::odomCB(const nav_msgs::Odometry& odom){
-    //     sub_odom_ = odom;
-    //     return;
-    // }
-    void Senddata::stereoCB(const FollowingCar::dis_info_array::ConstPtr& stereo)
-    {
-        for (const auto& obstacle_info :stereo->dis)
-        {
-            float distance = obstacle_info.distance;
-            // float width = obstacle_info.width;
-            // float height = obstacle_info.height;
-            // float angle = obstacle_info.angle;
-
-            // if(distance>5&&distance<45&&cmd_vel_.linear.x!=0)
-            if(distance>5&&distance<45)
-            {
-                flag_ = 1;
-            }
-        }
-	
-	    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->uwb_data_.x_;
-    //     odom_.pose.pose.position.y = uwb->uwb_data_.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
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/system.cpp
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/system.cpp
@ -1,15 +0,0 @@
-#include "../include/system.h"
-//#include ""
-
-
-namespace uwb_slam{
-
-    void System::Run()
-    {
-        while(1){
-
-        }
-
-    }
-    
-}
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/uwb.cpp
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/uwb.cpp
@ -1,121 +0,0 @@
-#include "uwb.h"
-#include <cmath>
-#include "Mat.h"
-
-
-#define CARHEIGHT 20
-#define DISINDEX 3
-#define AOAINDEX 15
-#define DATALENGTH 27
-
-namespace uwb_slam{
-    Uwb::Uwb(){
-    
-    }
-
-    void Uwb::Run() {
-        
-        while(1){
-        this->Serread();
-
-        }
-
-
-    }
-
-
-    void Uwb::Serread(){
-        try {
-        boost::asio::io_service io;
-        boost::asio::serial_port serial(io, "/dev/ttyUSB1"); // 替换成你的串口设备路径
-
-        serial.set_option(boost::asio::serial_port_base::baud_rate(115200)); // 设置波特率
-        serial.set_option(boost::asio::serial_port_base::character_size(8)); // 设置数据位
-        serial.set_option(boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none)); // 设置校验位
-        serial.set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one)); // 设置停止位
-
-        uint8_t tmpdata[DATALENGTH];
-        // std::cerr << "befor read" << std::endl;
-        size_t bytesRead = boost::asio::read(serial, boost::asio::buffer(tmpdata, sizeof(tmpdata))); // 读取串口数据
-        // std::cerr << "after read" << std::endl;
-        this->uwb_data_.uwb_t_ = ros::Time::now();
-        
-        for (int i=0;i< bytesRead;i++)
-        {
-            std::cout << std::hex<<static_cast<int>(tmpdata[i]) << " ";
-        }
-        std::cout << std::endl;
-
-        memcpy(&this->d[0], &tmpdata[DISINDEX], sizeof(d[0]));
-        memcpy(&this->d[1], &tmpdata[DISINDEX + sizeof(d[0])], sizeof(d[0]));
-        memcpy(&this->d[2], &tmpdata[DISINDEX + sizeof(d[0]) * 2], sizeof(d[0]));
-        memcpy(&this->aoa[0], &tmpdata[AOAINDEX], sizeof(aoa[0]));
-        memcpy(&this->aoa[1], &tmpdata[AOAINDEX + sizeof(aoa[0])], sizeof(aoa[0]));
-        memcpy(&this->aoa[2], &tmpdata[AOAINDEX + sizeof(aoa[0]) * 2], sizeof(aoa[0]));
-
-        //std::cout << "d:" << d[0] << " " << d[1] << " " << d[2] << std::endl;
-
-        // if(abs(d[0]) > 2000 || abs(d[1]) > 2000 || abs(d[2]) > 2000) {
-        //     return;
-        // }
-        // for(int i=0; i<3; i++)
-        // {
-        //     this->d[i] = sqrt(this->d[i] * this->d[i] - (AnchorPos[i][2] - CARHEIGHT) * (AnchorPos[i][2] - CARHEIGHT));
-        // }
-        
-        std::cout<<"d: "<<this->d[0]<<" aoa: "<<this->aoa[0]<<std::endl;
-        d[0] = ((((-1.4229e-07 * d[0]) + 1.8784e-04) * d[0]) + 0.9112) * d[0] + 2.4464;
-        // d[1] = ((((-2.3004e-07 * d[1]) + 3.2942e-04) * d[1]) + 0.8385) * d[1] + 6.2770;
-        // d[2] = ((((-2.0616e-07 * d[2]) + 3.3886e-04) * d[2]) + 0.8231) * d[2] + 8.1566;   
-    } catch (const std::exception& ex) {
-        std::cerr << "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);
-// }
-
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_msgs/CMakeLists.txt
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_msgs/CMakeLists.txt
@ -13,6 +13,8 @@ add_message_files(
        RobotState.msg
        NaviStatus.msg 
        RelocateResult.msg
+        dis_info.msg
+        dis_info_array.msg
 )

 add_service_files(
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/msg/dis_info.msg
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/msg/dis_info.msg
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_msgs/msg/dis_info_array.msg
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_msgs/msg/dis_info_array.msg
@ -0,0 +1 @@
+pibot_msgs/dis_info[] dis
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/CMakeLists.txt
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/CMakeLists.txt
@ -6,6 +6,7 @@ find_package(catkin REQUIRED COMPONENTS
  rospy
  std_msgs
  message_generation
+  pibot_msgs
  geometry_msgs
 )

@ -41,15 +42,15 @@ add_library(${PROJECT_NAME} SHARED
        include/senddata.h src/senddata.cpp)


-add_message_files(
-    DIRECTORY msg
-    FILES
-        RawImu.msg
+# add_message_files(
+#     DIRECTORY msg
+#     FILES
+#         RawImu.msg
     
-)
+# )
 generate_messages(DEPENDENCIES std_msgs geometry_msgs)

-catkin_package(CATKIN_DEPENDS message_runtime std_msgs geometry_msgs)
+catkin_package(CATKIN_DEPENDS std_msgs geometry_msgs)
 add_executable(${PROJECT_NAME}_node src/main.cpp)


--- 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
@ -5,7 +5,7 @@
 #include <utility>
 #include <queue>
 #include <fstream>
-#include "upbot_location/RawImu.h"
+#include "pibot_msgs/RawImu.h"
 #include "type.h"
 #include "uwb.h"
 #include "lighthouse.h"
@ -27,7 +27,7 @@ namespace uwb_slam{
        };
        void Run();
        void wheel_odomCB(const nav_msgs::Odometry& wheel_odom);
-        void imuCB(const upbot_location::RawImu& imu);
+        void imuCB(const pibot_msgs::RawImu& imu);
        void odomCB(const nav_msgs::Odometry& odom);

    public:
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/msg/RawImu.msg
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/msg/RawImu.msg
@ -1,7 +0,0 @@
-Header header
-bool accelerometer
-bool gyroscope
-bool magnetometer
-geometry_msgs/Vector3 raw_linear_acceleration
-geometry_msgs/Vector3 raw_angular_velocity
-geometry_msgs/Vector3 raw_magnetic_field
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/package.xml
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/package.xml
@ -53,7 +53,9 @@
  <build_depend>rospy</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>geometry_msgs</build_depend>
-  <build_depend>message_generation</build_depend>
+  <build_depend>pibot_msgs</build_depend>
+
+  <!-- <build_depend>message_generation</build_depend> -->

  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
@ -61,7 +63,7 @@
  <exec_depend>roscpp</exec_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>std_msgs</exec_depend>
-  <exec_depend>message_runtime</exec_depend>
+  <!-- <exec_depend>message_runtime</exec_depend> -->
  <exec_depend>geometry_msgs</exec_depend>


--- 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
@ -141,7 +141,7 @@ namespace uwb_slam{
    
        return;
    }
-    void Align::imuCB(const upbot_location::RawImu& imu)
+    void Align::imuCB(const pibot_msgs::RawImu& imu)
    {
        imu_odom_.imu_data_.imu_t_ = imu.header.stamp;
        imu_odom_.imu_data_.a_[0] = imu.raw_linear_acceleration.x;
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/CMakeLists.txt
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/CMakeLists.txt
@ -4,6 +4,7 @@ project(upbot_vision)
 find_package(catkin REQUIRED COMPONENTS
  roscpp
  std_msgs
+  pibot_msgs
  message_generation
 )

@ -11,20 +12,16 @@ find_package(catkin REQUIRED COMPONENTS
 find_package(OpenCV  REQUIRED)
 find_package(Threads REQUIRED)

-add_message_files(
-  FILES
-  dis_info.msg
-  dis_info_array.msg
-)
-generate_messages(
-  DEPENDENCIES
-  std_msgs
-)
+
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs
+# )


 catkin_package(

- CATKIN_DEPENDS roscpp std_msgs message_runtime
+ CATKIN_DEPENDS roscpp std_msgs 

 )

@ -50,14 +47,14 @@ add_library(
  src/ranging.cc
 )

-add_dependencies(head ${${PROJECT_NAME}_EXPORTED_TARGETS} upbot_vision_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(head ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 target_link_libraries(head
  ${catkin_LIBRARIES}
 
 )
 add_executable(main src/main.cc)
 # add_executable(sub_dis src/sub_dis.cc)
-add_dependencies(main ${${PROJECT_NAME}_EXPORTED_TARGETS} upbot_vision_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(main ${${PROJECT_NAME}_EXPORTED_TARGETS}  ${catkin_EXPORTED_TARGETS})
 # add_dependencies(sub_dis ${${PROJECT_NAME}_EXPORTED_TARGETS} upbot_vision_generate_messages_cpp ${catkin_EXPORTED_TARGETS})

 # target_link_libraries(sub_dis 
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/msg/dis_info.msg
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/msg/dis_info.msg
@ -1,4 +0,0 @@
-float32 distance
-float32 width
-float32 height
-float32 angle
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/msg/dis_info_array.msg
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/msg/dis_info_array.msg
@ -1 +0,0 @@
-upbot_vision/dis_info[] dis
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/package.xml
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/package.xml
@ -52,14 +52,15 @@
  <build_depend>ros_py</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>std_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>pibot_msgs</build_depend>
  <build_export_depend>ros_py</build_export_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>
  <exec_depend>ros_py</exec_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>std_msgs</exec_depend>
-  <build_depend>message_generation</build_depend>
-  <exec_depend>message_runtime</exec_depend>
+ 



--- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/src/main.cc
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/src/main.cc
@ -10,8 +10,8 @@
 #include "opencv2/imgcodecs/imgcodecs.hpp"
 #include <ctime>
 #include <queue>
-#include "upbot_vision/dis_info.h"
-#include "upbot_vision/dis_info_array.h"
+#include "pibot_msgs/dis_info.h"
+#include "pibot_msgs/dis_info_array.h"

 #define ANGLE_TO_RADIAN(angle) ((angle)*3141.59 / 180000)

@ -24,13 +24,13 @@ void *ranging(void *args)		// ranging线程
 	ros::Publisher dis_pub_;
 	ros::NodeHandle nh; 
 	// <>内为自定义ros消息类型 存储障碍物信息包括与障碍物的距离、障碍物宽度、高度和与障碍物之间的夹角
-    dis_pub_ = nh.advertise<upbot_vision::dis_info_array>("ceju_info",10);
+    dis_pub_ = nh.advertise<pibot_msgs::dis_info_array>("ceju_info",10);
 	while (true)
 	{
 		/*进入测距并发布障碍物信息*/;
 		std::vector<cv::Mat> result = r.get_range();
 		cv ::Mat info = result[2];
-		upbot_vision::dis_info_array dis_array;
+		pibot_msgs::dis_info_array dis_array;
 		if(!info.empty())
 		{
 			for(int i=0;i<info.rows;i++)
@ -38,7 +38,7 @@ void *ranging(void *args)		// ranging线程
 				// std::cout<<"1"<<std::endl;
 				if(info.at<float>(i,0)>0&&info.at<float>(i,0)<200)
 				{
-					upbot_vision::dis_info data;
+					pibot_msgs::dis_info data;
 					data.distance = info.at<float>(i,0);
 					data.width = info.at<float>(i,1);
 					data.height = info.at<float>(i,2);