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

@@ -32,15 +32,22 @@ include_directories(
 )
 
 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
      
+# )
 generate_messages(DEPENDENCIES std_msgs geometry_msgs)
 
 catkin_package(CATKIN_DEPENDS std_msgs geometry_msgs)

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

@@ -0,0 +1,34 @@
+#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

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

@@ -0,0 +1,32 @@
+#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

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

@@ -15,15 +15,24 @@ 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>();
 
-	mp->uwb_ = uwb;
+    
-    mp->align_ = align;
+    system->Mapping_ = mp;
-    align->uwb_ = uwb;
+    system->Uwb_ = uwb;
-    align->lighthouse_ = lighthouse;
+    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_;
 
     // //  control data fllow in system
     // std::thread rose_trd ([&system]() {

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

@@ -76,7 +76,25 @@ namespace uwb_slam
 
         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' ) {
@@ -91,9 +109,14 @@ namespace uwb_slam
                 if (key2 == 'q') {
                     //TODO: save
 
-                    std::string pngimage = "/home/firefly/Project_Ros/src/ros_merge_test/Map/output_image.png";//保存的图片文件路径
+                    std::string pngimage="/home/firefly/Project_Ros11/Project_Ros1/src/upbot_location/Map/output_image.png";//保存的图片文件路径
                     cv::imwrite(pngimage,img);
-                    read_uwb_ = false;
+                    readPos = false;
+                    
+                    // outfile.close();
+                    // printFlag = 0;
+                    // std::cout<< "Data written to file." << std::endl;
+                    
                     break;
                 }
         

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

@@ -0,0 +1,30 @@
+#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();
+    }
+
+}

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

@@ -4,36 +4,23 @@
 namespace uwb_slam
 {
     void Senddata::Run(std::shared_ptr<uwb_slam::Uwb>uwb){
-    // 初始化了一个名为loop_rate的ros::Rate对象,频率设置为10赫兹
+
         ros::Rate loop_rate(10);
-    // 初始化一个ROS发布者，用于发布nav_msgs::Odometry类型的消息
-    // 主题被设置为"uwb_odom"，队列大小为50
         position_pub_=nh_.advertise<nav_msgs::Odometry>("uwb_odom",50);
-    // 初始化了一个ROS订阅者，用于订阅"odom"主题。它指定了当在该主题上接收到
-    // 消息时，将调用Senddata类的odomCB回调函数。队列大小被设置为10
         odom_sub_=nh_.subscribe("odom",10,&Senddata::odomCB,this);
         while(ros::ok()){
-        // 按照10Hz频率发布uwb信息
             publishOdometry(uwb);
             ros::spinOnce();
-        // 用于控制循环速率
             loop_rate.sleep();
         }
 
 
     }
     void Senddata::odomCB(const nav_msgs::Odometry& odom){
-    // 这个地方接收的是轮速里程计的信息
-    // 包含位置和姿态
         sub_odom_ = odom;
         return;
     }
 
-/**---------------------------------------------------------------------
-* Function    : Senddata::publishOdometry
-* Description : 发布UWB里程计数据，这里读取的数据到底是什么，依旧存在疑问
-* Date        : 2023/12/13 zhanli@review
-*---------------------------------------------------------------------**/
     void Senddata::publishOdometry(std::shared_ptr<uwb_slam::Uwb>uwb)
     {
 

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

@@ -0,0 +1,15 @@
+#include "../include/system.h"
+//#include ""
+
+
+namespace uwb_slam{
+
+    void System::Run()
+    {
+        while(1){
+
+        }
+
+    }
+    
+}

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

@@ -2,7 +2,8 @@
 #include <cmath>
 #include "Mat.h"
 
-#define PI acos(-1)
+
+#define CARHEIGHT 20
 
 namespace uwb_slam{
     Mat A;
@@ -125,7 +126,10 @@ namespace uwb_slam{
     }
     
  
+    void fusion()
+    {
 
+    }
 
 
 
@@ -134,4 +138,34 @@ namespace uwb_slam{
 
 };
 
+// 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);
+// }