更新文档

Code/RK3588/PIBot_ROS.md

@@ -0,0 +1,59 @@
+# `PIBot_ROS`说明
+
+> 更新：`zhanli` 2023-12-13
+
+### 一、`PIBot_ROS`简介		
+
+​		`PIBot_ROS`本质上一个封装功能非常完善的功能包，其内部添加了许多已经整合好了功能，更加便于使用。
+
+- `ros_package` : 内置了许多算法的功能包的压缩包
+- `pypibot` : `pibot`相关的python支持功能
+- `pibot_upstart` : 未知待补充
+- `ros_ws` : `ROS`的工作空间 
+
+##### 1.1 `ros_ws`的目录结构
+
+​		下图中`catkin_workspcae`对应即是`ros_ws`文件夹。其下面一般具有`src`、`build`、`devel`等功能。
+
+<img src="http://logzhan.ticp.io:30000/logzhan/PictureHost/raw/branch/main/RobotKernal-UESTC/ROS_File_Structure.png" style="zoom: 50%;" />
+
+​		上述文件的结构和说明如下面的文档所示：
+
+```shell
+WorkSpace --- 自定义的工作空间
+    |--- build:编译空间，用于存放CMake和catkin的缓存信息、配置信息和其他中间文件。
+    |--- devel:开发空间，用于存放编译后生成的目标文件，包括头文件、动态&静态链接库、可执行文件等。
+    |--- src: 源码
+        |-- package：功能包(ROS基本单元)包含多个节点、库与配置文件，包名所有字母小写，只能由字母、数字与下划线组成
+            |-- CMakeLists.txt 配置编译规则，比如源文件、依赖项、目标文件
+            |-- package.xml 包信息，比如:包名、版本、作者、依赖项...(以前版本是 manifest.xml)
+            |-- scripts 存储python文件
+            |-- src 存储C++源文件
+            |-- include 头文件
+            |-- msg 消息通信格式文件
+            |-- srv 服务通信格式文件
+            |-- action 动作格式文件
+            |-- launch 可一次性运行多个节点 
+            |-- config 配置信息
+        |-- CMakeLists.txt: 编译的基本配置
+```
+
+##### 1.2 `package.xml`		
+
+​		如上图所示，ROS的基本功能都是通过包(`package`)组织的。该文件定义有关软件包的属性，例如软件包名称，版本号，作者，维护者以及对其他catkin软件包的依赖性。请注意，该概念类似于旧版` rosbuild` 构建系统中使用的`manifest.xml`文件。
+
+​		`package.xml`的部分内容如下图所示,  推测ROS系统通过`package.xml`的内容识别对应包的功能命令。
+
+```xml
+<?xml version="1.0"?>
+<!-- 格式: 以前是 1，推荐使用格式 2 -->
+<package format="2">
+  <!-- 包名 -->
+  <name>demo01_hello_vscode</name>
+  <!-- 版本 -->
+  <version>0.0.0</version>
+  <!-- 描述信息 -->
+  ...
+</package>
+```
+

Code/RK3588/PIBot_ROS/README.md

@@ -56,7 +56,7 @@ sudo apt-get install coinor-*
 
 #### 3.1 导航的启动
 
-​		`PIBot_ROS`的文件解压或重新命名可以按照个人习惯即可，这里以解压后命名为`pibot_ros`为例，介绍命令启动导航
+​		`PIBot_ROS`的文件解压或重新命名可以按照个人习惯即可，这里以解压后命名为`pibot_ros`为例，介绍命令启动导航。
 
 ```shell
 # 进入到pibot_ros的工作空间
@@ -64,6 +64,7 @@ cd ~/pibot_ros/ros_ws
 # 配置环境变量
 source ./devel/setup.bash
 # 启动导航文件
+# 格式 roslaunch package_name launch_file_name
 roslaunch pibot_navigation nav.launch
 ```
 

Code/RK3588/Robot_ROS_APP/src/ros_merge_test/CMakeLists.txt

@@ -22,7 +22,6 @@ find_package(Boost REQUIRED)
 # CATKIN_DEPENDS message_runtime std_msgs geometry_msgs
 # )
 
-
 include_directories(
 # include
 ${OpenCV_INCLUDE_DIRS}
@@ -31,11 +30,9 @@ include_directories(
 )
 
 add_library(${PROJECT_NAME} SHARED
-        src/system.cpp
         src/uwb.cpp
         src/mapping.cpp
         src/align.cpp
-        # src/read_sensor_data.cpp
         include/senddata.h src/senddata.cpp)
 
 
@@ -43,7 +40,6 @@ add_message_files(
     DIRECTORY msg
     FILES
     RawImu.msg
-     
 )
 generate_messages(DEPENDENCIES std_msgs geometry_msgs)
 

Code/RK3588/Robot_ROS_APP/src/ros_merge_test/include/mapping.h

@@ -6,7 +6,6 @@
 #ifndef MAPPING_H
 #define MAPPING_H
 
-
 namespace uwb_slam{
     class Mapping
     {
@@ -26,9 +25,7 @@ namespace uwb_slam{
         bool read_uwb_ = false;
         cv::Mat img;
         cv::Point2d cur_point = {-1,-1};
-
     };
-
 }
 
 #endif

Code/RK3588/Robot_ROS_APP/src/ros_merge_test/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 ReadSensorData
-    {
-        public:
-        ReadSensorData();
-
-        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/RK3588/Robot_ROS_APP/src/ros_merge_test/include/system.h

@@ -1,31 +0,0 @@
-#ifndef SYSTEM_H
-#define SYSTEM_H
-
-#include <thread>
-#include <string>
-#include "mapping.h"
-#include "uwb.h"
-#include "senddata.h"
-#include "align.h"
-#include <iostream>
-
-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_;
-
-        // Uwb* Uwb_ ;
-        // Senddata* Sender_;
-        // Mapping* Mapping_;
-    };
-}
-#endif

Code/RK3588/Robot_ROS_APP/src/ros_merge_test/include/uwb.h

@@ -7,8 +7,8 @@
 #include "type.h"
 #include <queue>
 #include <chrono>
-#ifndef UWB_H
+#ifndef __UWB_H__
-#define UWB_H
+#define __UWB_H__
 
 
 namespace uwb_slam{
@@ -22,33 +22,15 @@ namespace uwb_slam{
     void feed_imu_odom_pose_data();
     void UartUSBRead();
        
-
- 
 public:
     int pre_seq = -1;
     int cur_seq = -1;
     uint8_t tmpdata[13];
     float x, y, theta, distance;
     
-    // 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

Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/align.cpp

@@ -14,7 +14,7 @@ namespace uwb_slam{
         ros::Time tmp1 = ros::Time::now();
         ros::Time tmp2 = ros::Time::now();
 
-
+        // 这个地方分别订阅了原始IMU、轮式里程计、里程计(推测是UWB)
         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);
@@ -37,7 +37,8 @@ namespace uwb_slam{
                 if(key3 == 'w'){
                     break;
                 }
-            if(tmp!=imu_odom_.imu_data_.imu_t_){
+                if(tmp!=imu_odom_.imu_data_.imu_t_)
+                {
                     // outfile <<"imu_odom_: "<< "imu_timestamp  "<<"imu_linear_acc_x_y_z  "<<"imu_angular_x_y_z  "<<
                     // "odom_vxy  "<<"odom_angle_v_  "<<"\n";
                     // if(tmp1!=uwb_->uwb_data_.uwb_t_&& tmp2!=odom_tmp_){
@@ -89,7 +90,6 @@ namespace uwb_slam{
                     // if(count>300)
                     //     break;
                 }
-            
             }
 
             outfile.close();

Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/main.cpp

@@ -22,31 +22,14 @@ int main(int argc, char** argv)
     // Initialize the ROS node
     ros::init(argc, argv, "locate_info_pub_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>();
     
-    // uwb_slam::System* system = new uwb_slam::System();
+    mp->uwb_    = uwb;
-    // uwb_slam::Mapping* mp = new uwb_slam::Mapping();
+    align->uwb_ = uwb;
-    // uwb_slam::Uwb*  uwb = new uwb_slam::Uwb();
-    // uwb_slam::Senddata* sender = new uwb_slam::Senddata();
 
-    system->Mapping_       = mp;
-    system->Mapping_->uwb_ = uwb;
-    system->Uwb_           = uwb;
-    system->Sender_        = sender;
-    system->Align_         = align;
-    
-    mp->uwb_    = system->Uwb_;
-    align->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();
@@ -67,5 +50,4 @@ int main(int argc, char** argv)
       
     // Start the ROS node's main loop  
     ros::spin(); 
-    //System->run()
 }

Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/mapping.cpp

@@ -40,9 +40,6 @@ namespace uwb_slam
 
     void Mapping::Run()
     {
-        
-        //int key = cv::waitKey(0);//等待用户按下按键
-        //std::cout << key << std::endl;
         int realWidth  = AREA_SIZE / PIXEL_SCALE;
         int realHeight = AREA_SIZE / PIXEL_SCALE;
 
@@ -56,20 +53,8 @@ namespace uwb_slam
             for (int i=199+8;i<210;i+=1)
                 img.at<unsigned char>(j,i)= 0;
 
-
-
         cv::imshow("Image",img);
 
-        /*
-        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));
-            read_uwb_ = true;
-            std::cout << "non" << key << std::endl;
-            cv::destroyAllWindows();
-         }
-        */
         while(1)
         {
             // 这个地方会持续阻塞
@@ -94,23 +79,11 @@ namespace uwb_slam
         
                 this->feed_uwb_data(cv::Point2d(uwb_->x ,uwb_->y));
           
-
+                if(check_uwb_point()){
-                //uwb xieru
-                //std::cout << "cur_SEQ: " <<uwb_->cur_seq  << std::endl;
-
-                if(check_uwb_point())
-                {
-                    //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
 

Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/read_sensor_data.cpp

@@ -1,33 +0,0 @@
-#include <read_sensor_data.h>
-
-namespace uwb_slam {
-
-
-    //void Read_sensor_data::set_uwb(){}
-
-
-    void ReadSensorData::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 ReadSensorData::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 ReadSensorData::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事件循环
-        ros::spin();
-    }
-
-}

Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/system.cpp

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

Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/uwb.cpp

@@ -2,7 +2,9 @@
 * File Name          : uwb.cpp
 * Current Version    : V1.0
 * Date of Issued     : 2023.12.13 zhanli@review
-* Comments           : UWB数据驱动, 负责从串口读取USB并发布出去
+* Comments           : UWB数据驱动, 负责从串口读取USB并发布出去，这个代码可能存在以下
+                       改进：1) 创建固定的串口读取对象serial_port 2) 修改代码为异步
+                       读取。3) 确认串口数据的长度
 ********************************************************************************/
 #include "uwb.h"
 #include <cmath>
@@ -18,11 +20,17 @@ namespace uwb_slam{
     void Uwb::Run() {
         while(1){
             // 这个地方不控制速率?
+            // UartUSBRead 这个地方本身就是同步读取串口，是阻塞的函数
             this->UartUSBRead();
         }
     }
 
-
+    /**---------------------------------------------------------------------
+    * Function    : UartUSBRead
+    * Description : 通过串口读取数据，目前这段代码存在部分问题：1) 每次都重复创建
+    *               串口读取对象，可能会影响性能。
+    * Date        : 2023/12/13 zhanli@review
+    *---------------------------------------------------------------------**/
     void Uwb::UartUSBRead()
     {
         try {
@@ -35,33 +43,19 @@ namespace uwb_slam{
             serial.set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one));  // 设置停止位
 
             uint8_t tmpdata[12];
+            // 读取串口数据
+            size_t bytesRead = boost::asio::read(serial, boost::asio::buffer(tmpdata, 12)); 
 
-            size_t bytesRead = boost::asio::read(serial, boost::asio::buffer(tmpdata, 12)); // 读取串口数据
+            // UWB获取的数据是半径R和角度Theta
-            // std::cerr << "after read" << std::endl;
-
-            // for (int i=0;i< bytesRead;i++)
-            // {
-            //     std::cout << "Received data: " << std::hex<<static_cast<int>(tmpdata[i]) ;
-            // }
             memcpy(&this->distance, &tmpdata[3], sizeof(distance));
             memcpy(&this->theta, &tmpdata[7], sizeof(theta));
-            /*this->x = cosf(theta/180*PI)*distance+1000;
+
-            this->y = sinf(theta/180*PI)*distance+1000;
+            // 这个地方是为了把UWB的坐标移动到图像的中心位置 2023/12/13@李瑞瑞
-            this->theta = theta;*/
             this->uwb_data_.x_ = cosf(theta / 180*PI)*distance + 1000;
             this->uwb_data_.y_ = sinf(theta / 180*PI)*distance + 1000;
+            // 获取此时的系统时间戳
             this->uwb_data_.uwb_t_ = ros::Time::now(); 
             
-            // uwb_data_queue_.push(uwb_data_);
-        
-            //std::cout << "uwb_data_: " << uwb_data_.uwb_t_<< std::endl;
-            // cur_seq = static_cast<int>(tmpdata[3]);
-            //std::cout << "****** cur _ sequence: " <<  cur_seq << "x: " << x << " y: " << y <<std::endl;
-            // if( cur_seq  - pre_seq != 1){
-            //     std::cout << "****** cur _ sequence: " << cur_seq << "pre _ sequence: " << pre_seq << " ******\n";
-            // }
-            // pre_seq = static_cast<int>(tmpdata[3]);
-
             std::cout << "theta: " << theta << " distance: " << distance << std::endl;
             
         } catch (const std::exception& ex) {
@@ -69,10 +63,6 @@ namespace uwb_slam{
                 << ex.what() << std::endl;
         }
     }
-    
-    void fusion()
-    {
-    }
 };
 
 

Docs/ROS基础之基本函数.md

@@ -0,0 +1,18 @@
+# ROS基础笔记
+
+##### 1.1 `ros::Rate`的用法
+
+​		`ros::Rate` 是ROS中用于控制循环频率的工具。在你提到的代码中，`ros::Rate loop_rate(10);` 创建了一个 `ros::Rate` 对象，其目的是控制循环的频率。
+
+```c++
+ros::Rate loop_rate(10);  // 设置事件循环的频率为10Hz
+
+while (ros::ok()) {
+    // 处理其他任务，这个地方不能处理过于耗时的操作
+
+    ros::spinOnce();    // 处理一次事件循环
+    loop_rate.sleep();  // 使节点等待足够的时间以达到设定的频率
+}
+
+```
+

Docs/ROS基础之基本命令.md

@@ -0,0 +1,33 @@
+# ROS基础之基础命令
+
+> https://blog.csdn.net/sinat_16643223/article/details/113935412
+
+## 一、常用命令
+
+##### 1.1 `catkin_make`编译命令
+
+```shell
+# 进入工作空间
+cd ros_ws
+# 编译
+catkin_make
+```
+
+​		我们在使用ROS的时候经常会使用`catkin_make`命令，但我们时常会有疑惑`cakin_make`和`make`的区别。我们可以简单理解为`catkin_make`是`make`的扩展，它更加适合ROS相关的编译。
+
+​		`Catkin` 是 ROS 中使用的构建系统，而 `CMake `是 `Catkin` 的底层构建系统。具体而言，`Catkin` 使用 `CMake` 作为其构建系统的基础，它对 `CMake` 进行了扩展，以适应 ROS 的特殊需求和机器人软件包的组织。
+
+​		以下是它们之间的关系：
+
+1. **`CMake`：** `CMake` 是一个跨平台的开源构建系统，它使用一种基于文本的 DSL（领域特定语言）来描述软件项目的构建过程。`CMake` 生成适用于目标平台的构建系统文件，例如 `Makefile` 或` Visual Studio` 项目文件。
+2. **`Catkin`：** Catkin 是 ROS 特有的构建系统，构建在 `CMake` 之上。`Catkin` 扩展了 `CMake`，以便更好地支持 `ROS` 软件包的组织和构建。`Catkin` 引入了一些 ROS 特有的概念，例如工作空间（workspace）、软件包（package）、消息和服务生成等。
+
+​		在一个 `ROS` 工作空间中，通常会包含一个或多个` Catkin` 软件包，每个软件包都包含一个 `CMakeLists.txt` 文件，其中定义了软件包的构建规则、依赖关系等。总的来说，**Catkin 通过扩展 `CMake`，提供了一个更高级别的构建系统，使得在 ROS 中更容易管理和构建机器人软件项目**。 CMake 提供了通用的构建系统功能，而 `Catkin` 在其基础上添加了 `ROS` 特有的工具和概念。
+
+##### 1.2 `roslaunch`命令
+
+```shell
+roslaunch pibot_navigation nav.launch
+```
+
+​		在命令`roslaunch pibot_navigation nav.launch`中，`roslaunch`本质上是一个可执行文件。`pibot_navigation `是工作空间`workspace`中软件包。`ros_launch`是通过软件包下面的`package.xml`识别出功能包。`nav.launch`是`pibot_navigation`下面`launch`文件夹下的一个`nav.launch`文件，其本质是个一个`xml`文件，描述了要启动的节点以及对应要传递的参数。