upbot_following

合并瑞瑞验证后代码

Code/MowingRobot/README.md

@@ -1,17 +1,60 @@
 # 目录说明
 
-更新：2023-12-13 詹力 Review
+> 更新：2024-03-14 詹力 Review 更新文档
+>
+> 1. `pibot_ros` : 机器人硬件底盘控制 & 全覆盖、双目测距、目标检测、UWB融合算法。
+> 2. `test_map` : 跃杭测试代码
+>
 
-1. `PIBot_ROS` : ROS小车的ROS上层支持包负责`IMU`和`ODM`等数据的读取
-2. `Robot_ROS_APP` : 定位融合代码 
+### 一、PIbot_ROS说明
 
-其他资料：
+​		[PIbot](https://gitee.com/pibot/pibot_bringup/blob/master/doc/README.md#1-%E8%AF%B4%E6%98%8E)原先淘宝购买的机器人底盘配套的ROS驱动代码，能够实现ROS和底盘的控制以及传感器数据的读取。
 
-PIBot官方资料：https://gitee.com/pibot/pibot_bringup/blob/master/doc/README.md#1-%E8%AF%B4%E6%98%8E
+#### 1.1 Pibot的作用
 
-> 注意：`build`文件夹是编译生成，不需要纳入git管理
+#### 1.2 相关算法目录
 
-### 启动全覆盖 & 避障
+​		PIbot_ROS是一个ROS的工作空间，工作空间的介绍见`Docs/ROS工作目录介绍`。割草机器人项目的相关算法包在`pibot_ros\ros_ws\src\`下。
+
+​		ipa_coverage_planning : 全覆盖路径规划算法，主要是跃杭在开发
+
+​		upbot_following ：割草机器人UWB跟随算法包
+
+​		upbot_location :  割草机器人UWB融合定位
+
+​		upbot_vision : 割草机器人视觉包，负责目标检测和双目测距
+
+#### 1.3 PIbot编译
+
+​		`pibot_ros`的编译如下，相关支持功能包的安装以及PIbot使用的各种细节坑见`Docs\PIbot-ROS安装.md`。编译的时一定需要先编译pibot_msgs包(目前未彻底解决，无法准确控制catkin编译按照顺序和)。
+
+```shell
+# 进入home目录
+cd ~
+# 下载代码
+git clone http://logzhan.ticp.io:30000/logzhan/RobotKernal-UESTC.git
+# 把PIBot_ROS移动到Home目录下
+mv RobotKernal-UESTC/Code/MowingRobot/pibot_ros ~/pibot_ros
+
+# 要运行pibot还得安装相关支持包
+#导航相关
+sudo apt-get install ros-noetic-navigation
+#定位相关
+sudo apt-get install ros-noetic-robot-pose-ekf
+
+# 编译代码
+# 1.先编译pibot_msgs生成对应的头文件
+# 这是因为catkin_make编译顺序不可控，虽然参照网上的写法控制顺序，但是没有起到作用。这回导致自# 定义的msg还没有生成头文件就被引用导致编译不通过。
+catkin_make --pkg pibot_msgs
+# 2.编译全部Package
+catkin_make
+```
+
+
+
+### 二、实验测试
+
+#### 2.1 启动全覆盖 & 避障
 
 ```shell
 # 启动机器人和底盘通信连接

Code/MowingRobot/UPbot_Script/runBringUp.sh

@@ -1,4 +0,0 @@
-#!/bin/bash
-cd ~/pibot_ros/ros_ws/
-source ./devel/setup.bash
-roslaunch pibot_bringup bringup_with_imu.launch

Code/MowingRobot/UPbot_Script/runCarLocation.sh

@@ -1,4 +0,0 @@
-#!/bin/bash
-cd ~/project/projectCarLocation
-source ./devel/setup.bash
-rosrun ros_merge_test ros_merge_test_node

Code/MowingRobot/UPbot_Script/runPDR.sh

@@ -1,4 +0,0 @@
-#!/bin/bash
-cd ~/project/projectPDR
-source ./devel/setup.bash
-rosrun ros_merge_test ros_merge_test_node

Code/MowingRobot/UPbot_Script/runRoomExplorationClient.sh

@@ -1,4 +0,0 @@
-#!/bin/bash
-cd ~/pibot_ros/ros_ws/
-source ./devel/setup.bash
-roslaunch ipa_room_exploration room_exploration_client.launch

Code/MowingRobot/UPbot_Script/runRoomExplorationServer.sh

@@ -1,4 +0,0 @@
-#!/bin/bash
-cd ~/pibot_ros/ros_ws/
-source ./devel/setup.bash
-roslaunch ipa_room_exploration room_exploration_action_server.launch

Code/MowingRobot/UPbot_Script/runStereo.sh

@@ -1,4 +0,0 @@
-#!/bin/bash
-cd ~/obj_dec/
-source ./devel/setup.bash
-rosrun rknn_yolov5_demo main

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

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.0.2)
-project(FollowingCar)
+project(upbot_following)
 
 find_package(catkin REQUIRED COMPONENTS
   roscpp

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/package.xml

@@ -1,8 +1,8 @@
 <?xml version="1.0"?>
 <package format="2">
-  <name>FollowingCar</name>
+  <name>upbot_following</name>
   <version>0.0.0</version>
-  <description>The FollowingCar package</description>
+  <description>The upbot_following package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag -->
   <!-- Example:  -->
@@ -19,7 +19,7 @@
   <!-- 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> -->
+  <!-- <url type="website">http://wiki.ros.org/upbot_following</url> -->
 
 
   <!-- Author tags are optional, multiple are allowed, one per tag -->

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

@@ -3,13 +3,13 @@
 * Current Version    : V1.0
 * Author             : logzhan
 * Date of Issued     : 2022.09.14
-* Comments           : <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+* Comments           : 导航的矩阵计算
 ********************************************************************************/
 /* Header File Including -----------------------------------------------------*/
 #ifndef _H_MAT_
 #define _H_MAT_
 
-#define MAT_MAX 15 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+#define MAT_MAX 15 //决定了能处理的最大矩阵
 
 
 #include <string.h>
@@ -20,60 +20,55 @@ class Mat
 {
 public:
 	Mat();
-	Mat(int setm,int setn,int kind);//kind=1<EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>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<EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>kind=0<><30><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD>ݡ<EFBFBD>
+	Mat(int setm,int setn,int kind);//kind=1单位阵，kind=0零矩阵,其它不初始化内容。
+	void Init(int setm,int setn,int kind);//kind=1单位阵，kind=0零矩阵,其它不初始化内容。
 
 	void Zero(void);
-	//<EFBFBD><EFBFBD>Щ<EFBFBD>ؼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӧ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊprivate<EFBFBD>ġ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD>˷<EFBFBD><EFBFBD>㣬<EFBFBD><EFBFBD>Ҳ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>public
-	int m;//<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
-	int n;//<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
-	double mat[MAT_MAX][MAT_MAX];//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+	//这些关键数本应该作为private的。但是为了方便，我也做成了public
+	int m;//行数
+	int n;//列数
+	double mat[MAT_MAX][MAT_MAX];//矩阵数据内容
 
-	//<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD><EFBFBD><EFBFBD>
-	Mat SubMat(int a,int b,int lm,int ln);//<EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
-	void FillSubMat(int a,int b,Mat s);//<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӿ<EFBFBD><EFBFBD><EFBFBD>
+	//特殊的矩阵
+	Mat SubMat(int a,int b,int lm,int ln);//获取矩阵一部分
+	void FillSubMat(int a,int b,Mat s);//填充子矩阵
 
-	//<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ר<EFBFBD><EFBFBD>
-	double absvec();//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ij<EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǹ<EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD>صľ<EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD>
-	double Sqrt();//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȵ<EFBFBD>ƽ<EFBFBD><EFBFBD>
-	friend Mat operator ^(Mat a,Mat b);//<EFBFBD><EFBFBD><EFBFBD>
+	//向量专用
+	double absvec();//这个是向量的长度。不是个别元素的绝对值。
+	double Sqrt();//向量长度的平方
+	friend Mat operator ^(Mat a,Mat b);//叉乘
 
-	//<EFBFBD><EFBFBD><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);//ת<EFBFBD><EFBFBD>	
+	friend Mat operator ~(Mat a);//转置	
 	friend Mat operator /(Mat a,Mat b);//a*inv(b)
 	friend Mat operator %(Mat a,Mat b);//inv(a)*b
 
-	//MAT inv();//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+	//MAT inv();//逆矩阵
 
 private:
-	// Ϊ<EFBFBD><EFBFBD><EFBFBD>ø<EFBFBD>˹<EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һЩ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
-	// <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+	// 为了用高斯消元法，做的一些函数
+	// 交换两行
 	void RowExchange(int a, int b);
-	// ijһ<EFBFBD>г<EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><EFBFBD>
+	// 某一行乘以系数
 	void RowMul(int a,double k);
-	// <EFBFBD><EFBFBD>ijһ<EFBFBD>мӼ<EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>еı<EFBFBD><EFBFBD><EFBFBD>
+	// 对某一行加减另一行的倍数
 	void RowAdd(int a,int b, double k);
-	// <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+	// 交换两列
 	void ColExchange(int a, int b);
-	// ijһ<EFBFBD>г<EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><EFBFBD>
+	// 某一列乘以系数
 	void ColMul(int a,double k);
-	// <EFBFBD><EFBFBD>ijһ<EFBFBD>мӼ<EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>еı<EFBFBD><EFBFBD><EFBFBD>
+	// 对某一列加减另一列的倍数
 	void ColAdd(int a,int b,double k);
 	
 
 };
 
-
-
-
-
-
 #endif
 
 

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

@@ -3,7 +3,7 @@
 * Current Version    : V1.0
 * Author             : logzhan
 * Date of Issued     : 2022.09.14
-* Comments           : <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+* Comments           : 矩阵运算库
 ********************************************************************************/
 /* Header File Including -----------------------------------------------------*/
 #include "Mat.h"
@@ -28,7 +28,7 @@ int mini(int a,int b)
 	
 }
 
-//<EFBFBD><EFBFBD>Ҫ<EFBFBD>ڳ<EFBFBD>Ա<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD><EFBFBD>ù<EFBFBD><EFBFBD>캯<EFBFBD><EFBFBD>
+//不要在成员函数内调用构造函数
 Mat::Mat()
 {
 	Init(1,1,0);
@@ -51,7 +51,7 @@ void Mat::Init(int setm,int setn,int kind)
 	if(kind==1)
 	{
 		int x;
-		//Cԭ<EFBFBD>е<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>档
+		//C原有的max min会导致两次运行自变量。有附带操作的东西不要直接放到max里面。
 		int xend = mini(this->m, this->n);
 		for(x=0;x < xend;x++){
 			mat[x][x] = 1;
@@ -184,12 +184,12 @@ Mat operator ~(Mat a)
 
 Mat operator /(Mat a,Mat b)
 {
-	//<EFBFBD><EFBFBD>˹<EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD>
+	//高斯消元法
 
 	int x,xb;
 	for(x=0;x<b.n;x++)
 	{
-		//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD><EFBFBD><EFBFBD>ѵ<EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼԪ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+		//首先找到最佳的列。让起始元素最大
 		double s=0;
 		int p=x;
 		double sxb;
@@ -202,19 +202,19 @@ Mat operator /(Mat a,Mat b)
 				s=sxb;
 			}
 		}
-		//ͬʱ<EFBFBD>任<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+		//同时变换两侧矩阵
 		if(x!=p)
 		{
 			a.ColExchange(x,p);
 			b.ColExchange(x,p);
 		}
 
-		//<EFBFBD><EFBFBD>һ<EFBFBD>й<EFBFBD>һ
-		double k=1/b.mat[x][x];//<EFBFBD><EFBFBD>һ<EFBFBD>䲻ҪǶ<EFBFBD>׵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><EFBFBD><EFBFBD>²<EFBFBD>ͬ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+		//这一列归一
+		double k=1/b.mat[x][x];//这一句不要嵌套到下面两行中，否则会因为更新不同步导致计算错误。
 		a.ColMul(x,k);
 		b.ColMul(x,k);
 
-		//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>й<EFBFBD><EFBFBD><EFBFBD>
+		//把其它列归零
 		for(xb=0;xb<b.n;xb++)
 		{
 			if(xb!=x)
@@ -232,11 +232,11 @@ Mat operator /(Mat a,Mat b)
 
 Mat operator %(Mat a,Mat b)
 {
-	//<EFBFBD><EFBFBD>˹<EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD>
+	//高斯消元法
 	int x,xb;
 	for(x=0;x<a.m;x++)
 	{
-		//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD><EFBFBD><EFBFBD>ѵ<EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼԪ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+		//首先找到最佳的行。让起始元素最大
 		double s=0;
 		int p=x;
 		double sxb;
@@ -249,19 +249,19 @@ Mat operator %(Mat a,Mat b)
 				s=sxb;
 			}
 		}
-		//ͬʱ<EFBFBD>任<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+		//同时变换两侧矩阵
 		if(x!=p)
 		{
 			a.RowExchange(x,p);
 			b.RowExchange(x,p);
 		}
 
-		//<EFBFBD><EFBFBD>һ<EFBFBD>й<EFBFBD>һ
-		double k=1/a.mat[x][x];//<EFBFBD><EFBFBD>һ<EFBFBD>䲻ҪǶ<EFBFBD>׵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><EFBFBD><EFBFBD>²<EFBFBD>ͬ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+		//这一行归一
+		double k=1/a.mat[x][x];//这一句不要嵌套到下面两行中，否则会因为更新不同步导致计算错误。
 		a.RowMul(x,k);
 		b.RowMul(x,k);
 
-		//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>й<EFBFBD><EFBFBD><EFBFBD>
+		//把其它行归零
 		for(xb=0;xb<a.m;xb++)
 		{
 			if(xb!=x)

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

@@ -4,23 +4,36 @@
 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)
     {