代码整理

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

@@ -32,22 +32,15 @@ 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

@@ -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

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/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

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

@@ -15,24 +15,15 @@ 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::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::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::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::Align> align = std::make_shared<uwb_slam::Align>();
-    std::shared_ptr<uwb_slam::Lighthouse> lighthouse = std::make_shared<uwb_slam::Lighthouse>();
     
-    
+	mp->uwb_ = uwb;
-    system->Mapping_ = mp;
+    mp->align_ = align;
-    system->Uwb_ = uwb;
+    align->uwb_ = uwb;
-    system->Sender_ = sender;
+    align->lighthouse_ = lighthouse;
-    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,25 +76,7 @@ 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' ) {
@@ -109,14 +91,9 @@ 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/Project_Ros/src/ros_merge_test/Map/output_image.png";//保存的图片文件路径
-                    cv::imwrite(pngimage,img);
+                    cv::imwrite(pngimage, img);
-                    readPos = false;
+                    read_uwb_ = 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

@@ -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();
-    }
-
-}

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

@@ -4,25 +4,38 @@
 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;
     }
 
-    void Senddata::publishOdometry(std::shared_ptr<uwb_slam::Uwb>uwb)
+/**---------------------------------------------------------------------
-    {
+* Function    : Senddata::publishOdometry
+* Description : 发布UWB里程计数据，这里读取的数据到底是什么，依旧存在疑问
+* Date        : 2023/12/13 zhanli@review
+*---------------------------------------------------------------------**/
+void Senddata::publishOdometry(std::shared_ptr<uwb_slam::Uwb> uwb)
+{
 
         std::mutex mMutexSend;
         ros::Time current_time = ros::Time::now();

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

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