jhy
/
RobotKernal-UESTC
forked from logzhan/RobotKernal-UESTC
1
0
Fork 0
Code Pull Requests Activity

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

合并Carfollowing到upbot_following
main
詹力 2024-03-14 11:27:42 +08:00
parent 0377f50557 f6634135dd
commit 1861adb239
32 changed files with 1498 additions and 3001 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/CMakeLists.txt
View File

@ -1,53 +0,0 @@
cmake_minimum_required(VERSION 3.0.2)
project(FollowingCar)
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
message_generation
geometry_msgs
pibot_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/uwb.cpp
src/mapping.cpp
src/align.cpp
src/Mat.cpp
src/lighthouse.cpp
include/senddata.h src/senddata.cpp)
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})

83
Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/Mat.h
View File

@ -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>ij<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);
// ijһ<C4B3>г<EFBFBD><D0B3><EFBFBD>ϵ<EFBFBD><CFB5>
void RowMul(int a,double k);
// <20><>ijһ<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);
// ijһ<C4B3>г<EFBFBD><D0B3><EFBFBD>ϵ<EFBFBD><CFB5>
void ColMul(int a,double k);
// <20><>ijһ<C4B3>мӼ<D0BC><D3BC><EFBFBD>һ<EFBFBD>еı<D0B5><C4B1><EFBFBD>
void ColAdd(int a,int b,double k);
};
#endif

44
Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/align.h
View File

@ -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 "pibot_msgs/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 pibot_msgs::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

29
Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/lighthouse.h
View File

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

40
Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/mapping.h
View File

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

50
Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/senddata.h
View File

@ -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 "pibot_msgs/dis_info.h"
#include "pibot_msgs/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 pibot_msgs::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

50
Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/type.h
View File

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

60
Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/uwb.h
View File

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

75
Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/package.xml
View File

@ -1,75 +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>pibot_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>

368
Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/Mat.cpp
View File

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

94
Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/align.cpp
View File

@ -1,94 +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;
return;
}
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;
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;
}
};

79
Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/lighthouse.cpp
View File

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

60
Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/main.cpp
View File

@ -1,60 +0,0 @@
#include "Mat.h"
// #include "align.h"
#include "mapping.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::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;
align->uwb_ =uwb;
align->lighthouse_ = lighthouse;
sender->uwb_ = 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();
}

149
Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/mapping.cpp
View File

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

147
Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/senddata.cpp
View File

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

121
Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/uwb.cpp
View File

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

106
Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/CMakeLists.txt
View File

@ -1,53 +1,53 @@
cmake_minimum_required(VERSION 3.0.2) cmake_minimum_required(VERSION 3.0.2)
project(upbot_following) project(upbot_following)
find_package(catkin REQUIRED COMPONENTS find_package(catkin REQUIRED COMPONENTS
roscpp roscpp
rospy rospy
std_msgs std_msgs
message_generation message_generation
geometry_msgs geometry_msgs
pibot_msgs pibot_msgs
) )
# 寻找OpenCV库 # 寻找OpenCV库
find_package(OpenCV REQUIRED) find_package(OpenCV REQUIRED)
# 查找 Boost 库 # 查找 Boost 库
find_package(Boost REQUIRED) find_package(Boost REQUIRED)
# catkin_package( # catkin_package(
# # INCLUDE_DIRS include # # INCLUDE_DIRS include
# # LIBRARIES upbot_following # # LIBRARIES FollowingCar
# # CATKIN_DEPENDS roscpp rospy std_msgs # # CATKIN_DEPENDS roscpp rospy std_msgs
# # DEPENDS system_lib # # DEPENDS system_lib
# CATKIN_DEPENDS message_runtime std_msgs geometry_msgs # CATKIN_DEPENDS message_runtime std_msgs geometry_msgs
# ) # )
include_directories( include_directories(
# include # include
${OpenCV_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}
include include
) )
add_library(${PROJECT_NAME} SHARED add_library(${PROJECT_NAME} SHARED
src/system.cpp
src/uwb.cpp src/uwb.cpp
src/mapping.cpp src/mapping.cpp
src/align.cpp src/align.cpp
src/Mat.cpp src/Mat.cpp
src/lighthouse.cpp src/lighthouse.cpp
include/senddata.h src/senddata.cpp) include/senddata.h src/senddata.cpp)
generate_messages(DEPENDENCIES std_msgs geometry_msgs) generate_messages(DEPENDENCIES std_msgs geometry_msgs)
catkin_package(CATKIN_DEPENDS message_runtime std_msgs geometry_msgs) catkin_package(CATKIN_DEPENDS message_runtime std_msgs geometry_msgs)
add_executable(${PROJECT_NAME}_node src/main.cpp) add_executable(${PROJECT_NAME}_node src/main.cpp)
target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES} ${OpenCV_LIBS} ${Boost_LIBRARIES} pthread ${PROJECT_NAME}) target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES} ${OpenCV_LIBS} ${Boost_LIBRARIES} pthread ${PROJECT_NAME})

166
Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/Mat.h
View File

@ -1,83 +1,83 @@
/******************** (C) COPYRIGHT 2022 Geek************************************ /******************** (C) COPYRIGHT 2022 Geek************************************
* File Name : Mat.h * File Name : Mat.h
* Current Version : V1.0 * Current Version : V1.0
* Author : logzhan * Author : logzhan
* Date of Issued : 2022.09.14 * Date of Issued : 2022.09.14
* Comments : <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> * Comments : <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
********************************************************************************/ ********************************************************************************/
/* Header File Including -----------------------------------------------------*/ /* Header File Including -----------------------------------------------------*/
#ifndef _H_MAT_ #ifndef _H_MAT_
#define _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> #define MAT_MAX 15 //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܴ<EFBFBD><DCB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
#include <string.h> #include <string.h>
#define _USE_MATH_DEFINES #define _USE_MATH_DEFINES
#include <math.h> #include <math.h>
class Mat class Mat
{ {
public: public:
Mat(); 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> 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 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); 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 //<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 m;//<2F><><EFBFBD><EFBFBD>
int n;//<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> double mat[MAT_MAX][MAT_MAX];//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD>ľ<EFBFBD><C4BE><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> 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> void FillSubMat(int a,int b,Mat s);//<2F><><EFBFBD><EFBFBD>Ӿ<EFBFBD><D3BE><EFBFBD>
//<2F><><EFBFBD><EFBFBD>ר<EFBFBD><D7A8> //<2F><><EFBFBD><EFBFBD>ר<EFBFBD><D7A8>
double absvec();//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ij<EFBFBD><C4B3>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>Ǹ<EFBFBD><C7B8><EFBFBD>Ԫ<EFBFBD>صľ<D8B5><C4BE><EFBFBD>ֵ<EFBFBD><D6B5> double absvec();//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ij<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> double Sqrt();//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȵ<EFBFBD>ƽ<EFBFBD><C6BD>
friend Mat operator ^(Mat a,Mat b);//<2F><><EFBFBD> friend Mat operator ^(Mat a,Mat b);//<2F><><EFBFBD>
//<2F><><EFBFBD><EFBFBD> //<2F><><EFBFBD><EFBFBD>
friend Mat operator *(double k,Mat a); 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,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,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);//ת<><D7AA>
friend Mat operator /(Mat a,Mat b);//a*inv(b) friend Mat operator /(Mat a,Mat b);//a*inv(b)
friend Mat operator %(Mat a,Mat b);//inv(a)*b friend Mat operator %(Mat a,Mat b);//inv(a)*b
//MAT inv();//<2F><><EFBFBD><EFBFBD><EFBFBD> //MAT inv();//<2F><><EFBFBD><EFBFBD><EFBFBD>
private: private:
// Ϊ<><CEAA><EFBFBD>ø<EFBFBD>˹<EFBFBD><CBB9>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һЩ<D2BB><D0A9><EFBFBD><EFBFBD> // Ϊ<><CEAA><EFBFBD>ø<EFBFBD>˹<EFBFBD><CBB9>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һЩ<D2BB><D0A9><EFBFBD><EFBFBD>
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void RowExchange(int a, int b); void RowExchange(int a, int b);
// ijһ<C4B3>г<EFBFBD><D0B3><EFBFBD>ϵ<EFBFBD><CFB5> // ijһ<C4B3>г<EFBFBD><D0B3><EFBFBD>ϵ<EFBFBD><CFB5>
void RowMul(int a,double k); void RowMul(int a,double k);
// <20><>ijһ<C4B3>мӼ<D0BC><D3BC><EFBFBD>һ<EFBFBD>еı<D0B5><C4B1><EFBFBD> // <20><>ijһ<C4B3>мӼ<D0BC><D3BC><EFBFBD>һ<EFBFBD>еı<D0B5><C4B1><EFBFBD>
void RowAdd(int a,int b, double k); void RowAdd(int a,int b, double k);
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void ColExchange(int a, int b); void ColExchange(int a, int b);
// ijһ<C4B3>г<EFBFBD><D0B3><EFBFBD>ϵ<EFBFBD><CFB5> // ijһ<C4B3>г<EFBFBD><D0B3><EFBFBD>ϵ<EFBFBD><CFB5>
void ColMul(int a,double k); void ColMul(int a,double k);
// <20><>ijһ<C4B3>мӼ<D0BC><D3BC><EFBFBD>һ<EFBFBD>еı<D0B5><C4B1><EFBFBD> // <20><>ijһ<C4B3>мӼ<D0BC><D3BC><EFBFBD>һ<EFBFBD>еı<D0B5><C4B1><EFBFBD>
void ColAdd(int a,int b,double k); void ColAdd(int a,int b,double k);
}; };
#endif #endif

88
Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/align.h
View File

@ -1,44 +1,44 @@
#include <cmath> #include <cmath>
#include <ros/ros.h> #include <ros/ros.h>
#include <opencv2/opencv.hpp> #include <opencv2/opencv.hpp>
#include <nav_msgs/Odometry.h> #include <nav_msgs/Odometry.h>
#include <utility> #include <utility>
#include <queue> #include <queue>
#include <fstream> #include <fstream>
#include "pibot_msgs/RawImu.h" #include "pibot_msgs/RawImu.h"
#include "type.h" #include "type.h"
#include "uwb.h" #include "uwb.h"
#include "lighthouse.h" #include "lighthouse.h"
#include "Mat.h" #include "Mat.h"
#ifndef ALIGN_H #ifndef ALIGN_H
#define AlIGN_H #define AlIGN_H
namespace uwb_slam{ namespace uwb_slam{
class Align class Align
{ {
public: public:
Align(){ Align(){
}; };
void Run(); void Run();
void wheel_odomCB(const nav_msgs::Odometry& wheel_odom); void wheel_odomCB(const nav_msgs::Odometry& wheel_odom);
void imuCB(const pibot_msgs::RawImu& imu); void imuCB(const pibot_msgs::RawImu& imu);
void odomCB(const nav_msgs::Odometry& odom); void odomCB(const nav_msgs::Odometry& odom);
public: public:
ros::NodeHandle nh_; ros::NodeHandle nh_;
ros::Subscriber wheel_odom_sub_; ros::Subscriber wheel_odom_sub_;
ros::Subscriber imu_sub_; ros::Subscriber imu_sub_;
ros::Subscriber odom_sub_; ros::Subscriber odom_sub_;
Imu_odom_pose_data imu_odom_; Imu_odom_pose_data imu_odom_;
Uwb_data uwb_data_; Uwb_data uwb_data_;
ros::Time imuDataRxTime, uwbDataRxTime, odomDataRxTime; ros::Time imuDataRxTime, uwbDataRxTime, odomDataRxTime;
ros::Time odom_tmp_ ; ros::Time odom_tmp_ ;
bool write_data_ = false; bool write_data_ = false;
cv::Mat img1; cv::Mat img1;
std::queue<std::pair<Imu_odom_pose_data,Uwb_data>> data_queue; std::queue<std::pair<Imu_odom_pose_data,Uwb_data>> data_queue;
std::shared_ptr<uwb_slam::Uwb> uwb_; std::shared_ptr<uwb_slam::Uwb> uwb_;
std::shared_ptr<uwb_slam::Lighthouse> lighthouse_; std::shared_ptr<uwb_slam::Lighthouse> lighthouse_;
}; };
}; };
#endif #endif

58
Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/lighthouse.h
View File

@ -1,29 +1,29 @@
#include <ros/ros.h> #include <ros/ros.h>
#include <mutex> #include <mutex>
#include <boost/asio.hpp> #include <boost/asio.hpp>
#include <iostream> #include <iostream>
#include <string> #include <string>
#include <cstdint> #include <cstdint>
#include "type.h" #include "type.h"
#include <queue> #include <queue>
#include <chrono> #include <chrono>
#ifndef __LIGHTHOUSE_H__ #ifndef __LIGHTHOUSE_H__
#define __LIGHTHOUSE_H__ #define __LIGHTHOUSE_H__
namespace uwb_slam{ namespace uwb_slam{
class Lighthouse{ class Lighthouse{
public: public:
Lighthouse(); Lighthouse();
~Lighthouse(); ~Lighthouse();
void Run(); void Run();
void UDPRead(); void UDPRead();
// Listen PORT // Listen PORT
int PORT = 12345; int PORT = 12345;
int UdpSocket = -1; int UdpSocket = -1;
LightHouseData data; LightHouseData data;
std::mutex mMutexLighthouse; std::mutex mMutexLighthouse;
}; };
}; };
#endif #endif

80
Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/mapping.h
View File

@ -1,40 +1,40 @@
#include <queue> #include <queue>
#include <opencv2/opencv.hpp> #include <opencv2/opencv.hpp>
#include <mutex> #include <mutex>
#include "uwb.h" #include "uwb.h"
#include "align.h" #include "align.h"
#ifndef MAPPING_H #ifndef MAPPING_H
#define MAPPING_H #define MAPPING_H
namespace uwb_slam{ namespace uwb_slam{
class Mapping class Mapping
{ {
public: public:
const double PIXEL_SCALE = 1.0; //xiangsudian cm const double PIXEL_SCALE = 1.0; //xiangsudian cm
const int AREA_SIZE = 1000; //map size cm const int AREA_SIZE = 1000; //map size cm
Mapping() {}; Mapping() {};
void Run(); void Run();
bool checkQueue(); bool checkQueue();
void feedPos(const cv::Point2d & imuPosData, const cv::Point2d & uwbPosData, const cv::Point2d & syncPosData); void feedPos(const cv::Point2d & imuPosData, const cv::Point2d & uwbPosData, const cv::Point2d & syncPosData);
void process(); void process();
std::mutex mMutexMap; std::mutex mMutexMap;
std::shared_ptr<uwb_slam::Uwb> uwb_; std::shared_ptr<uwb_slam::Uwb> uwb_;
std::shared_ptr<uwb_slam::Align> align_; std::shared_ptr<uwb_slam::Align> align_;
private: private:
int realWidth, realHeight; int realWidth, realHeight;
std::queue<std::vector<cv::Point2d>> posDataQueue; std::queue<std::vector<cv::Point2d>> posDataQueue;
std::vector<cv::Point2d> posData = std::vector<cv::Point2d>(3, {-1, -1}); std::vector<cv::Point2d> posData = std::vector<cv::Point2d>(3, {-1, -1});
//cv::Point2d imuPoint = {-1,-1}; //cv::Point2d imuPoint = {-1,-1};
// std::queue<cv::Point2d> posDataQueue; // std::queue<cv::Point2d> posDataQueue;
bool readPos = false; bool readPos = false;
cv::Mat img; cv::Mat img;
}; };
} }
#endif #endif

98
Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/senddata.h
View File

@ -1,50 +1,50 @@
#include <ros/ros.h> #include <ros/ros.h>
#include <nav_msgs/Odometry.h> #include <nav_msgs/Odometry.h>
#include <geometry_msgs/Point.h> #include <geometry_msgs/Point.h>
#include <geometry_msgs/Quaternion.h> #include <geometry_msgs/Quaternion.h>
#include <tf2/LinearMath/Quaternion.h> #include <tf2/LinearMath/Quaternion.h>
#include <mutex> #include <mutex>
#include "uwb.h" #include "uwb.h"
#include <geometry_msgs/Twist.h> #include <geometry_msgs/Twist.h>
#include "pibot_msgs/dis_info.h" #include "pibot_msgs/dis_info.h"
#include "pibot_msgs/dis_info_array.h" #include "pibot_msgs/dis_info_array.h"
#ifndef SENDDATA_H #ifndef SENDDATA_H
#define SENDDATA_H #define SENDDATA_H
namespace uwb_slam{ namespace uwb_slam{
class Senddata class Senddata
{ {
public: public:
Senddata(){}; Senddata(){};
void publishOdometry( std::shared_ptr<uwb_slam::Uwb>uwb); void publishOdometry( std::shared_ptr<uwb_slam::Uwb>uwb);
void Run(std::shared_ptr<uwb_slam::Uwb>uwb); void Run(std::shared_ptr<uwb_slam::Uwb>uwb);
void odomCB(const nav_msgs::Odometry& odom); void odomCB(const nav_msgs::Odometry& odom);
void stereoCB(const pibot_msgs::dis_info_array::ConstPtr& stereo); void stereoCB(const pibot_msgs::dis_info_array::ConstPtr& stereo);
std::shared_ptr<uwb_slam::Uwb> uwb_; std::shared_ptr<uwb_slam::Uwb> uwb_;
std::mutex mMutexSend; std::mutex mMutexSend;
private: private:
ros::Publisher position_pub_; ros::Publisher position_pub_;
ros::Publisher cmd_vel_pub_; ros::Publisher cmd_vel_pub_;
ros::Subscriber odom_sub_; ros::Subscriber odom_sub_;
ros::Subscriber obstacles_sub_; ros::Subscriber obstacles_sub_;
ros::NodeHandle nh_; ros::NodeHandle nh_;
nav_msgs::Odometry odom_;//odom的消息类型 nav_msgs::Odometry odom_;//odom的消息类型
nav_msgs::Odometry sub_odom_;//odom的消息类型 nav_msgs::Odometry sub_odom_;//odom的消息类型
geometry_msgs::Twist cmd_vel_; geometry_msgs::Twist cmd_vel_;
bool flag_ = 0; bool flag_ = 0;
}; };
} }
#endif #endif

98
Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/type.h
View File

@ -1,50 +1,50 @@
#include <ros/ros.h> #include <ros/ros.h>
#include <ros/time.h> #include <ros/time.h>
#ifndef TYPE_H #ifndef TYPE_H
#define TYPE_H #define TYPE_H
namespace uwb_slam{ namespace uwb_slam{
struct Imu_data struct Imu_data
{ {
ros::Time imu_t_; ros::Time imu_t_;
double a_[3]; double a_[3];
double g_[3]; double g_[3];
double m_[3]; double m_[3];
Imu_data(){}; Imu_data(){};
Imu_data(double ax,double ay,double az, double gx, double gy, double gz, double mx, double my, double mz) 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}{}; :a_{ax,ay,az},g_{gx,gy,gz},m_{mx,my,mz}{};
}; };
struct Imu_odom_pose_data struct Imu_odom_pose_data
{ {
Imu_data imu_data_; Imu_data imu_data_;
double pose_[3]; double pose_[3];
double quat_[4]; double quat_[4];
double vxy_; double vxy_;
double angle_v_; double angle_v_;
Imu_odom_pose_data(){}; 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){}; 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 struct Uwb_data
{ {
float x_,y_; float x_,y_;
ros::Time uwb_t_; ros::Time uwb_t_;
Uwb_data(){}; Uwb_data(){};
Uwb_data(float x,float y,float t):x_(x),y_(y),uwb_t_(t){}; Uwb_data(float x,float y,float t):x_(x),y_(y),uwb_t_(t){};
}; };
struct LightHouseData struct LightHouseData
{ {
float x_,y_,z_; float x_,y_,z_;
float qw_,qx_,qy_,qz_; float qw_,qx_,qy_,qz_;
ros::Time lighthouse_t_; ros::Time lighthouse_t_;
LightHouseData(){}; LightHouseData(){};
LightHouseData(float x,float y,float z, float qw, float qx, float qy, float qz, float t): 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){}; x_(x),y_(y),z_(z),qw_(qw), qx_(qx), qy_(qy), qz_(qz), lighthouse_t_(t){};
}; };
} }
#endif #endif

120
Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/uwb.h
View File

@ -1,60 +1,60 @@
#include <ros/ros.h> #include <ros/ros.h>
#include <mutex> #include <mutex>
#include <boost/asio.hpp> #include <boost/asio.hpp>
#include <iostream> #include <iostream>
#include <string> #include <string>
#include <cstdint> #include <cstdint>
#include "type.h" #include "type.h"
#include <queue> #include <queue>
#include <chrono> #include <chrono>
#ifndef UWB_H #ifndef UWB_H
#define UWB_H #define UWB_H
#define PI acos(-1) #define PI acos(-1)
namespace uwb_slam{ namespace uwb_slam{
class Uwb class Uwb
{ {
public: public:
Uwb(); Uwb();
void Run(); void Run();
bool checknewdata(); bool checknewdata();
void feed_imu_odom_pose_data(); void feed_imu_odom_pose_data();
void Serread(); void Serread();
public: public:
int pre_seq = -1; int pre_seq = -1;
int cur_seq = -1; int cur_seq = -1;
int AnchorNum = 3; int AnchorNum = 3;
int AnchorPos[3][3]={ int AnchorPos[3][3]={
-240, 400, 113,\ -240, 400, 113,\
240, 400, 113,\ 240, 400, 113,\
-400, -240, 113 -400, -240, 113
};//基站坐标，序号+三维坐标 };//基站坐标，序号+三维坐标
int d[3]; int d[3];
int aoa[3]; int aoa[3];
// std::queue<Imu_odom_pose_data> v_buffer_imu_odom_pose_data_; // std::queue<Imu_odom_pose_data> v_buffer_imu_odom_pose_data_;
Uwb_data uwb_data_; Uwb_data uwb_data_;
// ros_merge_test::RawImu sub_imu_; // ros_merge_test::RawImu sub_imu_;
// std::queue<Imu_odom_pose_data > imu_odom_queue_; // std::queue<Imu_odom_pose_data > imu_odom_queue_;
// std::queue<Uwb_data> uwb_data_queue_; // std::queue<Uwb_data> uwb_data_queue_;
std::mutex mMutexUwb; std::mutex mMutexUwb;
//boost::asio::io_service io; //boost::asio::io_service io;
//boost::asio::serial_port s_port; //boost::asio::serial_port s_port;
// Imu_odom_pose_data imu_odom_pose_data_; // Imu_odom_pose_data imu_odom_pose_data_;
}; };
}; };
#endif #endif

150
Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/package.xml
View File

@ -1,75 +1,75 @@
<?xml version="1.0"?> <?xml version="1.0"?>
<package format="2"> <package format="2">
<name>upbot_following</name> <name>upbot_following</name>
<version>0.0.0</version> <version>0.0.0</version>
<description>The upbot_following package</description> <description>The upbot_following package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag --> <!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: --> <!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> --> <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="luoruidi@todo.todo">luoruidi</maintainer> <maintainer email="luoruidi@todo.todo">luoruidi</maintainer>
<!-- One license tag required, multiple allowed, one license per tag --> <!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: --> <!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 --> <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license> <license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag --> <!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository --> <!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: --> <!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/upbot_following</url> --> <!-- <url type="website">http://wiki.ros.org/upbot_following</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag --> <!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be --> <!-- Authors do not have to be maintainers, but could be -->
<!-- Example: --> <!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> --> <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies --> <!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies --> <!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: --> <!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies --> <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> --> <!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: --> <!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> --> <!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> --> <!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: --> <!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> --> <!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: --> <!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> --> <!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: --> <!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> --> <!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: --> <!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> --> <!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: --> <!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> --> <!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: --> <!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> --> <!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend> <buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend> <build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend> <build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend> <build_depend>std_msgs</build_depend>
<build_depend>pibot_msgs</build_depend> <build_depend>pibot_msgs</build_depend>
<build_depend>geometry_msgs</build_depend> <build_depend>geometry_msgs</build_depend>
<build_depend>message_generation</build_depend> <build_depend>message_generation</build_depend>
<build_export_depend>roscpp</build_export_depend> <build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend> <build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend> <build_export_depend>std_msgs</build_export_depend>
<exec_depend>roscpp</exec_depend> <exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend> <exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</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> <exec_depend>geometry_msgs</exec_depend>
<!-- The export tag contains other, unspecified, tags --> <!-- The export tag contains other, unspecified, tags -->
<export> <export>
<!-- Other tools can request additional information be placed here --> <!-- Other tools can request additional information be placed here -->
</export> </export>
</package> </package>

736
Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/Mat.cpp
View File

@ -1,368 +1,368 @@
/******************** (C) COPYRIGHT 2022 Geek************************************ /******************** (C) COPYRIGHT 2022 Geek************************************
* File Name : Mat.cpp * File Name : Mat.cpp
* Current Version : V1.0 * Current Version : V1.0
* Author : logzhan * Author : logzhan
* Date of Issued : 2022.09.14 * Date of Issued : 2022.09.14
* Comments : <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> * Comments : <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
********************************************************************************/ ********************************************************************************/
/* Header File Including -----------------------------------------------------*/ /* Header File Including -----------------------------------------------------*/
#include "Mat.h" #include "Mat.h"
double mind(double a,double b) double mind(double a,double b)
{ {
double c = a; double c = a;
if(b < c){ if(b < c){
c = b; c = b;
} }
return c; return c;
} }
int mini(int a,int b) int mini(int a,int b)
{ {
int c=a; int c=a;
if(b<c) if(b<c)
{ {
c=b; c=b;
} }
return c; return c;
} }
//<2F><>Ҫ<EFBFBD>ڳ<EFBFBD>Ա<EFBFBD><D4B1><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD><DAB5>ù<EFBFBD><C3B9>캯<EFBFBD><ECBAAF> //<2F><>Ҫ<EFBFBD>ڳ<EFBFBD>Ա<EFBFBD><D4B1><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD><DAB5>ù<EFBFBD><C3B9>캯<EFBFBD><ECBAAF>
Mat::Mat() Mat::Mat()
{ {
Init(1,1,0); Init(1,1,0);
} }
Mat::Mat(int setm,int setn,int kind) Mat::Mat(int setm,int setn,int kind)
{ {
Init(setm,setn,kind); Init(setm,setn,kind);
} }
void Mat::Init(int setm,int setn,int kind) void Mat::Init(int setm,int setn,int kind)
{ {
this->m = setm; this->m = setm;
this->n = setn; this->n = setn;
if((kind==0)||(kind==1)) if((kind==0)||(kind==1))
{ {
memset(mat,0,MAT_MAX*MAT_MAX*sizeof(double)); memset(mat,0,MAT_MAX*MAT_MAX*sizeof(double));
} }
if(kind==1) if(kind==1)
{ {
int x; 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>档 //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); int xend = mini(this->m, this->n);
for(x=0;x < xend;x++){ for(x=0;x < xend;x++){
mat[x][x] = 1; mat[x][x] = 1;
} }
} }
} }
void Mat::Zero() { void Mat::Zero() {
} }
Mat Mat::SubMat(int a,int b,int lm,int ln) Mat Mat::SubMat(int a,int b,int lm,int ln)
{ {
int aend=a+lm-1; int aend=a+lm-1;
int bend=b+ln-1; int bend=b+ln-1;
Mat s(lm,ln,-1); Mat s(lm,ln,-1);
int x,y; int x,y;
for(x=0;x<lm;x++) for(x=0;x<lm;x++)
{ {
for(y=0;y<ln;y++) for(y=0;y<ln;y++)
{ {
s.mat[x][y]=mat[a+x][b+y]; s.mat[x][y]=mat[a+x][b+y];
} }
} }
return s; return s;
} }
void Mat::FillSubMat(int a,int b,Mat s) void Mat::FillSubMat(int a,int b,Mat s)
{ {
int x,y; int x,y;
for(x=0;x<s.m;x++) for(x=0;x<s.m;x++)
{ {
for(y=0;y<s.n;y++) for(y=0;y<s.n;y++)
{ {
mat[a+x][b+y]=s.mat[x][y]; mat[a+x][b+y]=s.mat[x][y];
} }
} }
} }
Mat operator *(double k, Mat a) Mat operator *(double k, Mat a)
{ {
Mat b(a.m,a.n,-1); Mat b(a.m,a.n,-1);
int x,y; int x,y;
for(x=0;x<a.m;x++) for(x=0;x<a.m;x++)
{ {
for(y=0;y<a.n;y++) for(y=0;y<a.n;y++)
{ {
b.mat[x][y]=k*a.mat[x][y]; b.mat[x][y]=k*a.mat[x][y];
} }
} }
return b; return b;
} }
Mat operator *(Mat a,double k) Mat operator *(Mat a,double k)
{ {
return k*a; return k*a;
} }
Mat operator /(Mat a,double k) Mat operator /(Mat a,double k)
{ {
return (1/k)*a; return (1/k)*a;
} }
Mat operator *(Mat a,Mat b) Mat operator *(Mat a,Mat b)
{ {
Mat c(a.m,b.n,-1); Mat c(a.m,b.n,-1);
int x,y,z; int x,y,z;
double s; double s;
for(x=0;x<a.m;x++) for(x=0;x<a.m;x++)
{ {
for(y=0;y<b.n;y++) for(y=0;y<b.n;y++)
{ {
s=0; s=0;
for(z=0;z<a.n;z++) for(z=0;z<a.n;z++)
{ {
s=s+a.mat[x][z]*b.mat[z][y]; s=s+a.mat[x][z]*b.mat[z][y];
} }
c.mat[x][y]=s; c.mat[x][y]=s;
} }
} }
return c; return c;
} }
Mat operator +(Mat a,Mat b) Mat operator +(Mat a,Mat b)
{ {
Mat c=a; Mat c=a;
int x,y; int x,y;
for(x=0;x<c.m;x++) for(x=0;x<c.m;x++)
{ {
for(y=0;y<c.n;y++) for(y=0;y<c.n;y++)
{ {
c.mat[x][y]+=b.mat[x][y]; c.mat[x][y]+=b.mat[x][y];
} }
} }
return c; return c;
} }
Mat operator -(Mat a,Mat b) Mat operator -(Mat a,Mat b)
{ {
Mat c=a; Mat c=a;
int x,y; int x,y;
for(x=0;x<c.m;x++) for(x=0;x<c.m;x++)
{ {
for(y=0;y<c.n;y++) for(y=0;y<c.n;y++)
{ {
c.mat[x][y]-=b.mat[x][y]; c.mat[x][y]-=b.mat[x][y];
} }
} }
return c; return c;
} }
Mat operator ~(Mat a) Mat operator ~(Mat a)
{ {
Mat b(a.n,a.m,-1); Mat b(a.n,a.m,-1);
int x,y; int x,y;
for(x=0;x<a.m;x++) for(x=0;x<a.m;x++)
{ {
for(y=0;y<a.n;y++) for(y=0;y<a.n;y++)
{ {
b.mat[y][x]=a.mat[x][y]; b.mat[y][x]=a.mat[x][y];
} }
} }
return b; return b;
} }
Mat operator /(Mat a,Mat b) Mat operator /(Mat a,Mat b)
{ {
//<2F><>˹<EFBFBD><CBB9>Ԫ<EFBFBD><D4AA> //<2F><>˹<EFBFBD><CBB9>Ԫ<EFBFBD><D4AA>
int x,xb; int x,xb;
for(x=0;x<b.n;x++) 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> //<2F><><EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD>ѵ<EFBFBD><D1B5>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD>ʼԪ<CABC><D4AA><EFBFBD><EFBFBD><EFBFBD>
double s=0; double s=0;
int p=x; int p=x;
double sxb; double sxb;
for(xb=x;xb<b.n;xb++) for(xb=x;xb<b.n;xb++)
{ {
sxb=fabs(b.mat[x][xb]); sxb=fabs(b.mat[x][xb]);
if(sxb>s) if(sxb>s)
{ {
p=xb; p=xb;
s=sxb; s=sxb;
} }
} }
//ͬʱ<CDAC>任<EFBFBD><E4BBBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> //ͬʱ<CDAC>任<EFBFBD><E4BBBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if(x!=p) if(x!=p)
{ {
a.ColExchange(x,p); a.ColExchange(x,p);
b.ColExchange(x,p); b.ColExchange(x,p);
} }
//<2F><>һ<EFBFBD>й<EFBFBD>һ //<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> 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); a.ColMul(x,k);
b.ColMul(x,k); b.ColMul(x,k);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>й<EFBFBD><D0B9><EFBFBD> //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>й<EFBFBD><D0B9><EFBFBD>
for(xb=0;xb<b.n;xb++) for(xb=0;xb<b.n;xb++)
{ {
if(xb!=x) if(xb!=x)
{ {
k=(-b.mat[x][xb]); k=(-b.mat[x][xb]);
a.ColAdd(xb,x,k); a.ColAdd(xb,x,k);
b.ColAdd(xb,x,k); b.ColAdd(xb,x,k);
} }
} }
} }
return a; return a;
} }
Mat operator %(Mat a,Mat b) Mat operator %(Mat a,Mat b)
{ {
//<2F><>˹<EFBFBD><CBB9>Ԫ<EFBFBD><D4AA> //<2F><>˹<EFBFBD><CBB9>Ԫ<EFBFBD><D4AA>
int x,xb; int x,xb;
for(x=0;x<a.m;x++) 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> //<2F><><EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD>ѵ<EFBFBD><D1B5>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD>ʼԪ<CABC><D4AA><EFBFBD><EFBFBD><EFBFBD>
double s=0; double s=0;
int p=x; int p=x;
double sxb; double sxb;
for(xb=x;xb<a.m;xb++) for(xb=x;xb<a.m;xb++)
{ {
sxb=fabs(a.mat[xb][x]); sxb=fabs(a.mat[xb][x]);
if(sxb>s) if(sxb>s)
{ {
p=xb; p=xb;
s=sxb; s=sxb;
} }
} }
//ͬʱ<CDAC>任<EFBFBD><E4BBBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> //ͬʱ<CDAC>任<EFBFBD><E4BBBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if(x!=p) if(x!=p)
{ {
a.RowExchange(x,p); a.RowExchange(x,p);
b.RowExchange(x,p); b.RowExchange(x,p);
} }
//<2F><>һ<EFBFBD>й<EFBFBD>һ //<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> 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); a.RowMul(x,k);
b.RowMul(x,k); b.RowMul(x,k);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>й<EFBFBD><D0B9><EFBFBD> //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>й<EFBFBD><D0B9><EFBFBD>
for(xb=0;xb<a.m;xb++) for(xb=0;xb<a.m;xb++)
{ {
if(xb!=x) if(xb!=x)
{ {
k=(-a.mat[xb][x]); k=(-a.mat[xb][x]);
a.RowAdd(xb,x,k); a.RowAdd(xb,x,k);
b.RowAdd(xb,x,k); b.RowAdd(xb,x,k);
} }
} }
} }
return b; return b;
} }
void Mat::RowExchange(int a, int b) void Mat::RowExchange(int a, int b)
{ {
double s[MAT_MAX]; double s[MAT_MAX];
int ncpy=n*sizeof(double); int ncpy=n*sizeof(double);
memcpy(s,mat[a],ncpy); memcpy(s,mat[a],ncpy);
memcpy(mat[a],mat[b],ncpy); memcpy(mat[a],mat[b],ncpy);
memcpy(mat[b],s,ncpy); memcpy(mat[b],s,ncpy);
} }
void Mat::RowMul(int a,double k) void Mat::RowMul(int a,double k)
{ {
int y; int y;
for(y=0;y<n;y++) for(y=0;y<n;y++)
{ {
mat[a][y]= mat[a][y]*k; mat[a][y]= mat[a][y]*k;
} }
} }
void Mat::RowAdd(int a,int b,double k) void Mat::RowAdd(int a,int b,double k)
{ {
int y; int y;
for(y=0;y<n;y++) for(y=0;y<n;y++)
{ {
mat[a][y]= mat[a][y]+ mat[b][y]*k; mat[a][y]= mat[a][y]+ mat[b][y]*k;
} }
} }
void Mat::ColExchange(int a, int b) void Mat::ColExchange(int a, int b)
{ {
double s; double s;
int x; int x;
for(x=0;x<m;x++) for(x=0;x<m;x++)
{ {
s=mat[x][a]; s=mat[x][a];
mat[x][a]=mat[x][b]; mat[x][a]=mat[x][b];
mat[x][b]=s; mat[x][b]=s;
} }
} }
void Mat::ColMul(int a,double k) void Mat::ColMul(int a,double k)
{ {
int x; int x;
for(x=0;x<m;x++) for(x=0;x<m;x++)
{ {
mat[x][a]=mat[x][a]*k; mat[x][a]=mat[x][a]*k;
} }
} }
void Mat::ColAdd(int a,int b,double k) void Mat::ColAdd(int a,int b,double k)
{ {
int x; int x;
for(x=0;x<m;x++) for(x=0;x<m;x++)
{ {
mat[x][a]=mat[x][a]+mat[x][b]*k; mat[x][a]=mat[x][a]+mat[x][b]*k;
} }
} }
double Mat::Sqrt() double Mat::Sqrt()
{ {
int x; int x;
double numx; double numx;
double s=0; double s=0;
for(x=0;x<m;x++) for(x=0;x<m;x++)
{ {
numx=mat[x][0]; numx=mat[x][0];
s+=(numx*numx); s+=(numx*numx);
} }
return s; return s;
} }
double Mat::absvec() double Mat::absvec()
{ {
return sqrt(Sqrt()); return sqrt(Sqrt());
} }
Mat operator ^(Mat a, Mat b) Mat operator ^(Mat a, Mat b)
{ {
double ax=a.mat[0][0]; double ax=a.mat[0][0];
double ay=a.mat[1][0]; double ay=a.mat[1][0];
double az=a.mat[2][0]; double az=a.mat[2][0];
double bx=b.mat[0][0]; double bx=b.mat[0][0];
double by=b.mat[1][0]; double by=b.mat[1][0];
double bz=b.mat[2][0]; double bz=b.mat[2][0];
Mat c(3,1,-1); Mat c(3,1,-1);
c.mat[0][0]=ay*bz-az*by; c.mat[0][0]=ay*bz-az*by;
c.mat[1][0]=(-(ax*bz-az*bx)); c.mat[1][0]=(-(ax*bz-az*bx));
c.mat[2][0]=ax*by-ay*bx; c.mat[2][0]=ax*by-ay*bx;
return c; return c;
} }

188
Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/align.cpp
View File

@ -1,94 +1,94 @@
#include "align.h" #include "align.h"
namespace uwb_slam{ namespace uwb_slam{
void Align::Run() void Align::Run()
{ {
imuDataRxTime = ros::Time::now(); imuDataRxTime = ros::Time::now();
uwbDataRxTime = ros::Time::now(); uwbDataRxTime = ros::Time::now();
wheel_odom_sub_= nh_.subscribe("wheel_odom",10,&Align::wheel_odomCB,this); wheel_odom_sub_= nh_.subscribe("wheel_odom",10,&Align::wheel_odomCB,this);
imu_sub_= nh_.subscribe("raw_imu",10,&Align::imuCB,this); imu_sub_= nh_.subscribe("raw_imu",10,&Align::imuCB,this);
odom_sub_= nh_.subscribe("odom",10,&Align::odomCB,this); odom_sub_= nh_.subscribe("odom",10,&Align::odomCB,this);
std::ofstream outfile("data.txt",std::ofstream::out); std::ofstream outfile("data.txt",std::ofstream::out);
if(outfile.is_open()) { if(outfile.is_open()) {
std::cout << "start saving data" << std::endl; std::cout << "start saving data" << std::endl;
outfile << "imuDataRxTime,odomDataRxTime,uwbDataRxTime,"\ outfile << "imuDataRxTime,odomDataRxTime,uwbDataRxTime,"\
<< "aX,aY,aZ,"\ << "aX,aY,aZ,"\
<< "gX,gY,gZ"\ << "gX,gY,gZ"\
<< "mX,mY,mZ,"\ << "mX,mY,mZ,"\
<< "qW,qX,qY,qZ,"\ << "qW,qX,qY,qZ,"\
<< "vXY,angleV,"\ << "vXY,angleV,"\
<< "imuPosX,imuPosY,"\ << "imuPosX,imuPosY,"\
<< "lightHousePosX,lightHousePosY,lightHousePosZ,"\ << "lightHousePosX,lightHousePosY,lightHousePosZ,"\
<< "lightHouseQW,lightHouseQX,lightHouseQY,lightHouseQZ"\ << "lightHouseQW,lightHouseQX,lightHouseQY,lightHouseQZ"\
<< "d1,d2,d3\n"; << "d1,d2,d3\n";
} else { } else {
std::cout<<"file can not open"<<std::endl; std::cout<<"file can not open"<<std::endl;
} }
while(1){ while(1){
if(imuDataRxTime!=imu_odom_.imu_data_.imu_t_){ if(imuDataRxTime!=imu_odom_.imu_data_.imu_t_){
//std::cout << "imu received" << std::endl; //std::cout << "imu received" << std::endl;
imuDataRxTime = imu_odom_.imu_data_.imu_t_; imuDataRxTime = imu_odom_.imu_data_.imu_t_;
odomDataRxTime = odom_tmp_; odomDataRxTime = odom_tmp_;
uwbDataRxTime = uwb_->uwb_data_.uwb_t_; uwbDataRxTime = uwb_->uwb_data_.uwb_t_;
outfile << imuDataRxTime << "," << odomDataRxTime << "," << uwbDataRxTime <<","\ 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_.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_.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_.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_.quat_[0] << "," << imu_odom_.quat_[0] << "," << imu_odom_.quat_[0] << "," << imu_odom_.quat_[0] << ","\
<< imu_odom_.vxy_ << "," << imu_odom_.angle_v_ << ","\ << imu_odom_.vxy_ << "," << imu_odom_.angle_v_ << ","\
<< imu_odom_.pose_[0] << "," << imu_odom_.pose_[1] << ","\ << imu_odom_.pose_[0] << "," << imu_odom_.pose_[1] << ","\
<< lighthouse_->data.x_ << "," << lighthouse_->data.y_ << "," << lighthouse_->data.z_ << ","\ << lighthouse_->data.x_ << "," << lighthouse_->data.y_ << "," << lighthouse_->data.z_ << ","\
<< lighthouse_->data.qw_ << "," << lighthouse_->data.qx_ << "," << lighthouse_->data.qy_ << "," << lighthouse_->data.qz_ << ","\ << lighthouse_->data.qw_ << "," << lighthouse_->data.qx_ << "," << lighthouse_->data.qy_ << "," << lighthouse_->data.qz_ << ","\
<< uwb_->d[0] << "," << uwb_->d[1] << "," << uwb_->d[2] << "\n"; << uwb_->d[0] << "," << uwb_->d[1] << "," << uwb_->d[2] << "\n";
} }
} }
// outfile.close(); // outfile.close();
// std::cout<< "Data written to file." << std::endl; // std::cout<< "Data written to file." << std::endl;
} }
void Align::wheel_odomCB(const nav_msgs::Odometry& wheel_odom) void Align::wheel_odomCB(const nav_msgs::Odometry& wheel_odom)
{ {
imu_odom_.vxy_= wheel_odom.twist.twist.linear.x; imu_odom_.vxy_= wheel_odom.twist.twist.linear.x;
imu_odom_.angle_v_ = wheel_odom.twist.twist.angular.z; imu_odom_.angle_v_ = wheel_odom.twist.twist.angular.z;
return; return;
} }
void Align::imuCB(const pibot_msgs::RawImu& imu) void Align::imuCB(const pibot_msgs::RawImu& imu)
{ {
imu_odom_.imu_data_.imu_t_ = imu.header.stamp; 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_[0] = imu.raw_linear_acceleration.x;
imu_odom_.imu_data_.a_[1] = imu.raw_linear_acceleration.y; 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_.a_[2] = imu.raw_linear_acceleration.z;
imu_odom_.imu_data_.g_[0] = imu.raw_angular_velocity.x; 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_[1] = imu.raw_angular_velocity.y;
imu_odom_.imu_data_.g_[2] = imu.raw_angular_velocity.z; 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_[0] = imu.raw_magnetic_field.x;
imu_odom_.imu_data_.m_[1] = imu.raw_magnetic_field.y; imu_odom_.imu_data_.m_[1] = imu.raw_magnetic_field.y;
imu_odom_.imu_data_.m_[2] = imu.raw_magnetic_field.z; imu_odom_.imu_data_.m_[2] = imu.raw_magnetic_field.z;
return; return;
} }
void Align::odomCB(const nav_msgs::Odometry& odom) void Align::odomCB(const nav_msgs::Odometry& odom)
{ {
odom_tmp_ = odom.header.stamp; odom_tmp_ = odom.header.stamp;
imu_odom_.pose_[0] = odom.pose.pose.position.x; imu_odom_.pose_[0] = odom.pose.pose.position.x;
imu_odom_.pose_[1] = odom.pose.pose.position.y; imu_odom_.pose_[1] = odom.pose.pose.position.y;
imu_odom_.pose_[2] = odom.pose.pose.position.z; imu_odom_.pose_[2] = odom.pose.pose.position.z;
imu_odom_.quat_[0] = odom.pose.pose.orientation.w; imu_odom_.quat_[0] = odom.pose.pose.orientation.w;
imu_odom_.quat_[1] = odom.pose.pose.orientation.x; imu_odom_.quat_[1] = odom.pose.pose.orientation.x;
imu_odom_.quat_[2] = odom.pose.pose.orientation.y; imu_odom_.quat_[2] = odom.pose.pose.orientation.y;
imu_odom_.quat_[3] = odom.pose.pose.orientation.z; imu_odom_.quat_[3] = odom.pose.pose.orientation.z;
} }
}; };

156
Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/lighthouse.cpp
View File

@ -1,79 +1,79 @@
#include "lighthouse.h" #include "lighthouse.h"
#include <cmath> #include <cmath>
#include <string> #include <string>
namespace uwb_slam{ namespace uwb_slam{
Lighthouse::Lighthouse(){ Lighthouse::Lighthouse(){
std::cout << "Start Run. " << std::endl; std::cout << "Start Run. " << std::endl;
// 创建UDP套接字 // 创建UDP套接字
UdpSocket = socket(AF_INET, SOCK_DGRAM, 0); UdpSocket = socket(AF_INET, SOCK_DGRAM, 0);
if (UdpSocket == -1) { if (UdpSocket == -1) {
std::cerr << "Error creating UDP socket." << std::endl; std::cerr << "Error creating UDP socket." << std::endl;
return; return;
} }
std::cout << "Creating UDP socket sucess. " << std::endl; std::cout << "Creating UDP socket sucess. " << std::endl;
// 设置套接字地址结构 // 设置套接字地址结构
sockaddr_in udpAddress; sockaddr_in udpAddress;
std::memset(&udpAddress, 0, sizeof(udpAddress)); std::memset(&udpAddress, 0, sizeof(udpAddress));
udpAddress.sin_family = AF_INET; udpAddress.sin_family = AF_INET;
udpAddress.sin_addr.s_addr = htonl(INADDR_ANY); udpAddress.sin_addr.s_addr = htonl(INADDR_ANY);
udpAddress.sin_port = htons(PORT); udpAddress.sin_port = htons(PORT);
// 将套接字绑定到地址 // 将套接字绑定到地址
if (bind(UdpSocket, (struct sockaddr*)&udpAddress, sizeof(udpAddress)) == -1) { if (bind(UdpSocket, (struct sockaddr*)&udpAddress, sizeof(udpAddress)) == -1) {
std::cerr << "Error binding UDP socket." << std::endl; std::cerr << "Error binding UDP socket." << std::endl;
close(UdpSocket); close(UdpSocket);
} }
} }
Lighthouse::~Lighthouse(){ Lighthouse::~Lighthouse(){
close(UdpSocket); close(UdpSocket);
} }
void Lighthouse::Run() { void Lighthouse::Run() {
while(1){ while(1){
// 这是一个阻塞线程 // 这是一个阻塞线程
this->UDPRead(); this->UDPRead();
} }
} }
void Lighthouse::UDPRead(){ void Lighthouse::UDPRead(){
char buffer[1024]; char buffer[1024];
ssize_t bytesRead = recvfrom(UdpSocket, buffer, sizeof(buffer), 0, nullptr, nullptr); ssize_t bytesRead = recvfrom(UdpSocket, buffer, sizeof(buffer), 0, nullptr, nullptr);
if (bytesRead == -1) { if (bytesRead == -1) {
std::cerr << "Error receiving data." << std::endl; std::cerr << "Error receiving data." << std::endl;
return; return;
} }
std::string data(buffer); std::string data(buffer);
float x,y,z,qw,qx,qy,qz; float x,y,z,qw,qx,qy,qz;
qw = 1.0; qw = 1.0;
sscanf(data.c_str(),"%f %f %f %f %f %f",&x,&y,&z,&qx,&qy,&qz); sscanf(data.c_str(),"%f %f %f %f %f %f",&x,&y,&z,&qx,&qy,&qz);
mMutexLighthouse.lock(); mMutexLighthouse.lock();
this->data.x_ = x; this->data.x_ = x;
this->data.y_ = y; this->data.y_ = y;
this->data.z_ = z; this->data.z_ = z;
this->data.qw_ = qw; this->data.qw_ = qw;
this->data.qx_ = qx; this->data.qx_ = qx;
this->data.qy_ = qy; this->data.qy_ = qy;
this->data.qz_ = qz; this->data.qz_ = qz;
mMutexLighthouse.unlock(); mMutexLighthouse.unlock();
// 打印接收到的消息 // 打印接收到的消息
buffer[bytesRead] = '\0'; buffer[bytesRead] = '\0';
//std::cout << "Received: " << buffer << std::endl; //std::cout << "Received: " << buffer << std::endl;
} }
}; };

121
Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/main.cpp
View File

@ -1,61 +1,60 @@
#include "../include/system.h" #include "Mat.h"
#include "../include/uwb.h"
#include "../include/lighthouse.h" // #include "align.h"
#include "Mat.h" #include "mapping.h"
#include <iostream>
#include <iostream> #include <ros/ros.h>
#include <ros/ros.h> #include <thread>
#include <thread> #include "senddata.h"
#include "senddata.h"
int main(int argc, char** argv)
int main(int argc, char** argv) {
{ ros::init(argc, argv, "locate_info_pub_node"); // Initialize the ROS node
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>();
std::shared_ptr<uwb_slam::Lighthouse> lighthouse = std::make_shared<uwb_slam::Lighthouse>();
mp->uwb_ = uwb;
mp->uwb_ = uwb; mp->align_ = align;
mp->align_ = align; align->uwb_ =uwb;
align->uwb_ =uwb; align->lighthouse_ = lighthouse;
align->lighthouse_ = lighthouse; sender->uwb_ = uwb;
sender->uwb_ = uwb; // // control data fllow in system
// // control data fllow in system // std::thread rose_trd ([&system]() {
// std::thread rose_trd ([&system]() { // system->Run();
// system->Run(); // });
// });
// uwb serried read
// uwb serried read std::thread uwb_trd([&uwb]() {
std::thread uwb_trd([&uwb]() { uwb->Run();
uwb->Run(); });
});
// build map
// build map // std::thread map_trd ([&mp]() {
// std::thread map_trd ([&mp]() { // mp->Run();
// mp->Run(); // });
// });
std::thread sender_trd ([&sender, uwb]() {
std::thread sender_trd ([&sender, uwb]() { sender->Run(uwb);
sender->Run(uwb); });
});
// std::thread align_trd ([&align]() {
// std::thread align_trd ([&align]() { // align->Run();
// align->Run(); // });
// });
// std::thread lighthouse_trd ([&lighthouse]() {
// std::thread lighthouse_trd ([&lighthouse]() { // lighthouse->Run();
// lighthouse->Run(); // });
// });
ros::spin();
ros::spin(); }
}

298
Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/mapping.cpp
View File

@ -1,149 +1,149 @@
#include "mapping.h" #include "mapping.h"
#include <mutex> #include <mutex>
#include <unistd.h>// 包含 usleep() 函数所在的头文件 #include <unistd.h>// 包含 usleep() 函数所在的头文件
#include <opencv2/core.hpp> #include <opencv2/core.hpp>
#include <opencv2/highgui.hpp> #include <opencv2/highgui.hpp>
namespace uwb_slam namespace uwb_slam
{ {
bool Mapping::checkQueue() bool Mapping::checkQueue()
{ {
//std::unique_lock<std::mutex> lock(mMutexMap); //std::unique_lock<std::mutex> lock(mMutexMap);
return !posDataQueue.empty(); return !posDataQueue.empty();
} }
void Mapping::feedPos(const cv::Point2d & imuPosData, const cv::Point2d & uwbPosData, const cv::Point2d & syncPosData) void Mapping::feedPos(const cv::Point2d & imuPosData, const cv::Point2d & uwbPosData, const cv::Point2d & syncPosData)
{ {
//std::unique_lock<std::mutex> lock(mMutexMap); //std::unique_lock<std::mutex> lock(mMutexMap);
posDataQueue.push({imuPosData, uwbPosData, syncPosData}); posDataQueue.push({imuPosData, uwbPosData, syncPosData});
} }
void Mapping::process() void Mapping::process()
{ {
{ {
//std::unique_lock<std::mutex> lock(mMutexMap); //std::unique_lock<std::mutex> lock(mMutexMap);
//std::cout << "SIZE: " <<posDataQueue.size() << std::endl; //std::cout << "SIZE: " <<posDataQueue.size() << std::endl;
posData = posDataQueue.front(); posData = posDataQueue.front();
//std::cout << "x: " <<cur_point.x << " y:" << cur_point.y << std::endl; //std::cout << "x: " <<cur_point.x << " y:" << cur_point.y << std::endl;
posDataQueue.pop(); posDataQueue.pop();
} }
/*生成图*/ /*生成图*/
int imuPosPointX = posData[0].x / PIXEL_SCALE + ( fmod(posData[0].x ,PIXEL_SCALE) != 0) + realWidth / 2; 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 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 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 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 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; 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 << "syncPos:" <<posData[2].x << " " << posData[2].y;
// std::cout << " uwbPos:" <<posData[1].x << " " << posData[1].x; // std::cout << " uwbPos:" <<posData[1].x << " " << posData[1].x;
// std::cout << " imuPos:" <<posData[0].x << " " << posData[0].y << std::endl; // std::cout << " imuPos:" <<posData[0].x << " " << posData[0].y << std::endl;
// std::cout << "syncPos:" <<syncPosPointX << " " << syncPosPointY; // std::cout << "syncPos:" <<syncPosPointX << " " << syncPosPointY;
// std::cout << " uwbPos:" <<uwbPosPointX << " " << uwbPosPointY; // std::cout << " uwbPos:" <<uwbPosPointX << " " << uwbPosPointY;
// std::cout << " imuPos:" <<imuPosPointX << " " << imuPosPointY << std::endl; // std::cout << " imuPos:" <<imuPosPointX << " " << imuPosPointY << std::endl;
// img.at<unsigned char>(imuPosPointY, imuPosPointX) = 0; // 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>(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>(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 img.at<cv::Vec3b>(syncPosPointY, syncPosPointX) = cv::Vec3b(255,0,0); //sync trace is blue
} }
void Mapping::Run() void Mapping::Run()
{ {
//int key = cv::waitKey(0);//等待用户按下按键 //int key = cv::waitKey(0);//等待用户按下按键
//std::cout << key << std::endl; //std::cout << key << std::endl;
realWidth = AREA_SIZE / PIXEL_SCALE; realWidth = AREA_SIZE / PIXEL_SCALE;
realHeight = AREA_SIZE / PIXEL_SCALE; realHeight = AREA_SIZE / PIXEL_SCALE;
img = cv::Mat(realHeight, realWidth, CV_8UC3, cv::Scalar(255,255,255)); img = cv::Mat(realHeight, realWidth, CV_8UC3, cv::Scalar(255,255,255));
//cankao //cankao
for (int j=0;j<AREA_SIZE / PIXEL_SCALE;j+=10) for (int j=0;j<AREA_SIZE / PIXEL_SCALE;j+=10)
for (int i=0;i<AREA_SIZE / PIXEL_SCALE;i+=10) for (int i=0;i<AREA_SIZE / PIXEL_SCALE;i+=10)
img.at<cv::Vec3b>(j,i)= cv::Vec3b(0,0,0); 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 j=realHeight/2-1; j<=realHeight/2+1; j+=1)
for (int i=realWidth/2-1; i<=realWidth/2+1; i+=1) for (int i=realWidth/2-1; i<=realWidth/2+1; i+=1)
img.at<cv::Vec3b>(j,i)= cv::Vec3b(0,0,0); img.at<cv::Vec3b>(j,i)= cv::Vec3b(0,0,0);
int i = 0, j = 0; int i = 0, j = 0;
img.at<cv::Vec3b>(j,i)= cv::Vec3b(0,0,0); img.at<cv::Vec3b>(j,i)= cv::Vec3b(0,0,0);
cv::imshow("Image",img); cv::imshow("Image",img);
// bool printFlag = 0; // bool printFlag = 0;
// std::ofstream outfile("data.txt",std::ofstream::out); // std::ofstream outfile("data.txt",std::ofstream::out);
// if(outfile.is_open()) { // if(outfile.is_open()) {
// std::cout << "start saving data" << key << std::endl; // std::cout << "start saving data" << key << std::endl;
// printFlag = 1; // printFlag = 1;
// } else { // } else {
// std::cout<<"file can not open"<<std::endl; // std::cout<<"file can not open"<<std::endl;
// } // }
/* /*
std::cout << "waiting" <<std::endl; std::cout << "waiting" <<std::endl;
int key = cv::waitKey(0); int key = cv::waitKey(0);
if (key == 'q' || key == 27) { if (key == 'q' || key == 27) {
this->feed_uwb_data(cv::Point2d(uwb_->x,uwb_->y)); this->feed_uwb_data(cv::Point2d(uwb_->x,uwb_->y));
readPos = true; readPos = true;
std::cout << "non" << key << std::endl; std::cout << "non" << key << std::endl;
cv::destroyAllWindows(); cv::destroyAllWindows();
} }
*/ */
while(1){ while(1){
int key = cv::waitKey(0); int key = cv::waitKey(0);
if (key == 'q' ) { if (key == 'q' ) {
readPos = true; readPos = true;
std::cout << "start mapping" << key << std::endl; std::cout << "start mapping" << key << std::endl;
//cv::destroyAllWindows(); //cv::destroyAllWindows();
} }
while( readPos )//按下空格键 while( readPos )//按下空格键
{ {
int key2 = cv::waitKey(1); int key2 = cv::waitKey(1);
if (key2 == 'q') { if (key2 == 'q') {
//TODO: save //TODO: save
std::string pngimage="/home/firefly/Project_Ros11/Project_Ros1/src/pibot_msgs/Map/output_image.png";//保存的图片文件路径 std::string pngimage="/home/firefly/Project_Ros11/Project_Ros1/src/pibot_msgs/Map/output_image.png";//保存的图片文件路径
cv::imwrite(pngimage,img); cv::imwrite(pngimage,img);
readPos = false; readPos = false;
// outfile.close(); // outfile.close();
// printFlag = 0; // printFlag = 0;
// std::cout<< "Data written to file." << std::endl; // std::cout<< "Data written to file." << std::endl;
break; 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(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)); //this->feedPos(cv::Point2d(uwb_->x, uwb_->y));
//uwb xieru //uwb xieru
//std::cout << "cur_SEQ: " <<uwb_->cur_seq << std::endl; //std::cout << "cur_SEQ: " <<uwb_->cur_seq << std::endl;
if(checkQueue()) if(checkQueue())
{ {
//std::cout << " start process" << std::endl; //std::cout << " start process" << std::endl;
process(); process();
//std::cout << " end process" << std::endl; //std::cout << " end process" << std::endl;
} }
} }
// std::cout << "out" << key << std::endl; // std::cout << "out" << key << std::endl;
} }
//std::string pngimage="../Map/pngimage.png";//保存的图片文件路径 //std::string pngimage="../Map/pngimage.png";//保存的图片文件路径
//cv::imwrite(pngimage,img); //cv::imwrite(pngimage,img);
/*ros 发送图片给导航 */ /*ros 发送图片给导航 */
} }
} // namespace uwb_slam } // namespace uwb_slam

292
Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/senddata.cpp
View File

@ -1,147 +1,147 @@
#include "senddata.h" #include "senddata.h"
#define angleLimit 20 #define angleLimit 20
#define disLimit 200 #define disLimit 200
namespace uwb_slam namespace uwb_slam
{ {
void Senddata::Run(std::shared_ptr<uwb_slam::Uwb>uwb){ void Senddata::Run(std::shared_ptr<uwb_slam::Uwb>uwb){
ros::Rate loop_rate(10); ros::Rate loop_rate(10);
// position_pub_=nh_.advertise<nav_msgs::Odometry>("uwb_odom",50); // position_pub_=nh_.advertise<nav_msgs::Odometry>("uwb_odom",50);
cmd_vel_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel",50); cmd_vel_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel",50);
obstacles_sub_ = nh_.subscribe<pibot_msgs::dis_info_array>("ceju_info",1,boost::bind(&Senddata::stereoCB,this,_1)); obstacles_sub_ = nh_.subscribe<pibot_msgs::dis_info_array>("ceju_info",1,boost::bind(&Senddata::stereoCB,this,_1));
// odom_sub_=nh_.subscribe("odom",10,&Senddata::odomCB,this); // odom_sub_=nh_.subscribe("odom",10,&Senddata::odomCB,this);
while(ros::ok()){ while(ros::ok()){
// publishOdometry(uwb); // publishOdometry(uwb);
if (flag_) if (flag_)
{ {
cmd_vel_.linear.x = 0; cmd_vel_.linear.x = 0;
cmd_vel_.angular.z = -1; cmd_vel_.angular.z = -1;
for (int i = 0; i < 17; ++i) { for (int i = 0; i < 17; ++i) {
cmd_vel_pub_.publish(cmd_vel_); cmd_vel_pub_.publish(cmd_vel_);
loop_rate.sleep(); loop_rate.sleep();
} }
cmd_vel_.linear.x = 0.2; cmd_vel_.linear.x = 0.2;
cmd_vel_.angular.z = 0; cmd_vel_.angular.z = 0;
for (int i = 0; i < 20; ++i) { for (int i = 0; i < 20; ++i) {
cmd_vel_pub_.publish(cmd_vel_); cmd_vel_pub_.publish(cmd_vel_);
loop_rate.sleep(); loop_rate.sleep();
} }
cmd_vel_.linear.x = 0; cmd_vel_.linear.x = 0;
cmd_vel_.angular.z = 1; cmd_vel_.angular.z = 1;
for (int i = 0; i < 16; ++i) { for (int i = 0; i < 16; ++i) {
cmd_vel_pub_.publish(cmd_vel_); cmd_vel_pub_.publish(cmd_vel_);
loop_rate.sleep(); loop_rate.sleep();
} }
cmd_vel_.linear.x = 0; cmd_vel_.linear.x = 0;
cmd_vel_.angular.z = 0; cmd_vel_.angular.z = 0;
flag_ = 0; flag_ = 0;
} }
else if(abs(uwb->aoa[0]) > 180 || abs(uwb->d[0]) > 2000) else if(abs(uwb->aoa[0]) > 180 || abs(uwb->d[0]) > 2000)
{ {
cmd_vel_.angular.z = 0; cmd_vel_.angular.z = 0;
cmd_vel_.linear.x = 0; cmd_vel_.linear.x = 0;
} }
else if(uwb->aoa[0] > angleLimit && uwb->aoa[0] <= 180) else if(uwb->aoa[0] > angleLimit && uwb->aoa[0] <= 180)
{ {
float angularSpeed = (float)uwb->aoa[0] / 100 + 1; float angularSpeed = (float)uwb->aoa[0] / 100 + 1;
cmd_vel_.angular.z = angularSpeed; cmd_vel_.angular.z = angularSpeed;
cmd_vel_.linear.x = 0; cmd_vel_.linear.x = 0;
} }
else if(uwb->aoa[0] < -angleLimit) else if(uwb->aoa[0] < -angleLimit)
{ {
float angularSpeed = (float)uwb->aoa[0] / 100 - 1; float angularSpeed = (float)uwb->aoa[0] / 100 - 1;
cmd_vel_.angular.z = angularSpeed; cmd_vel_.angular.z = angularSpeed;
cmd_vel_.linear.x = 0; cmd_vel_.linear.x = 0;
} }
else if(uwb->d[0] > disLimit) else if(uwb->d[0] > disLimit)
{ {
float lineSpeed = (float)uwb->d[0] / 1000 + 0.1; float lineSpeed = (float)uwb->d[0] / 1000 + 0.1;
cmd_vel_.angular.z = 0; cmd_vel_.angular.z = 0;
cmd_vel_.linear.x = lineSpeed; cmd_vel_.linear.x = lineSpeed;
} }
else if(uwb->d[0] < disLimit - 30) else if(uwb->d[0] < disLimit - 30)
{ {
cmd_vel_.angular.z = 0; cmd_vel_.angular.z = 0;
cmd_vel_.linear.x = -0.2; cmd_vel_.linear.x = -0.2;
} }
else else
{ {
cmd_vel_.angular.z = 0; cmd_vel_.angular.z = 0;
cmd_vel_.linear.x = 0; cmd_vel_.linear.x = 0;
} }
cmd_vel_pub_.publish(cmd_vel_); cmd_vel_pub_.publish(cmd_vel_);
ros::spinOnce(); ros::spinOnce();
loop_rate.sleep(); loop_rate.sleep();
} }
} }
// void Senddata::odomCB(const nav_msgs::Odometry& odom){ // void Senddata::odomCB(const nav_msgs::Odometry& odom){
// sub_odom_ = odom; // sub_odom_ = odom;
// return; // return;
// } // }
void Senddata::stereoCB(const pibot_msgs::dis_info_array::ConstPtr& stereo) void Senddata::stereoCB(const pibot_msgs::dis_info_array::ConstPtr& stereo)
{ {
for (const auto& obstacle_info :stereo->dis) for (const auto& obstacle_info :stereo->dis)
{ {
float distance = obstacle_info.distance; float distance = obstacle_info.distance;
// float width = obstacle_info.width; // float width = obstacle_info.width;
// float height = obstacle_info.height; // float height = obstacle_info.height;
// float angle = obstacle_info.angle; // float angle = obstacle_info.angle;
// if(distance>5&&distance<45&&cmd_vel_.linear.x!=0) // if(distance>5&&distance<45&&cmd_vel_.linear.x!=0)
if(distance>5&&distance<45) if(distance>5&&distance<45)
{ {
flag_ = 1; flag_ = 1;
} }
} }
return; return;
} }
// void Senddata::publishOdometry(std::shared_ptr<uwb_slam::Uwb>uwb) // void Senddata::publishOdometry(std::shared_ptr<uwb_slam::Uwb>uwb)
// { // {
// std::mutex mMutexSend; // std::mutex mMutexSend;
// ros::Time current_time = ros::Time::now(); // ros::Time current_time = ros::Time::now();
// // 设置 Odometry 消息的头部信息 // // 设置 Odometry 消息的头部信息
// odom_.header.stamp = current_time; // odom_.header.stamp = current_time;
// odom_.header.frame_id = "odom"; // 设置坐标系为 "map" // odom_.header.frame_id = "odom"; // 设置坐标系为 "map"
// odom_.child_frame_id = "base_link"; // 设置坐标系为 "base_link" // odom_.child_frame_id = "base_link"; // 设置坐标系为 "base_link"
// // 填充 Odometry 消息的位置信息 // // 填充 Odometry 消息的位置信息
// odom_.pose.pose.position.x = uwb->uwb_data_.x_; // odom_.pose.pose.position.x = uwb->uwb_data_.x_;
// odom_.pose.pose.position.y = uwb->uwb_data_.y_; // odom_.pose.pose.position.y = uwb->uwb_data_.y_;
// odom_.pose.pose.position.z = 0.0; // odom_.pose.pose.position.z = 0.0;
// // 填充 Odometry 消息的姿态信息（使用四元数来表示姿态） // // 填充 Odometry 消息的姿态信息（使用四元数来表示姿态）
// // tf2::Quaternion quat; // // tf2::Quaternion quat;
// // quat.setRPY(0, 0, uwb->theta); // 设置了 yaw 角度，其他 roll 和 pitch 设置为 0 // // quat.setRPY(0, 0, uwb->theta); // 设置了 yaw 角度，其他 roll 和 pitch 设置为 0
// // odom.pose.pose.orientation.x = quat.x(); // // odom.pose.pose.orientation.x = quat.x();
// // odom.pose.pose.orientation.y = quat.y(); // // odom.pose.pose.orientation.y = quat.y();
// // odom.pose.pose.orientation.z = quat.z(); // // odom.pose.pose.orientation.z = quat.z();
// // odom.pose.pose.orientation.w = quat.w(); // // odom.pose.pose.orientation.w = quat.w();
// odom_.pose.pose.orientation.x = sub_odom_.pose.pose.orientation.x; // 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.y = sub_odom_.pose.pose.orientation.y;
// odom_.pose.pose.orientation.z = sub_odom_.pose.pose.orientation.z; // odom_.pose.pose.orientation.z = sub_odom_.pose.pose.orientation.z;
// odom_.pose.pose.orientation.w = sub_odom_.pose.pose.orientation.w; // odom_.pose.pose.orientation.w = sub_odom_.pose.pose.orientation.w;
// // 发布 Odometry 消息 // // 发布 Odometry 消息
// position_pub_.publish(odom_); // position_pub_.publish(odom_);
// } // }
} // namespace uwb_slam } // namespace uwb_slam

242
Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/uwb.cpp
View File

@ -1,121 +1,121 @@
#include "uwb.h" #include "uwb.h"
#include <cmath> #include <cmath>
#include "Mat.h" #include "Mat.h"
#define CARHEIGHT 20 #define CARHEIGHT 20
#define DISINDEX 3 #define DISINDEX 3
#define AOAINDEX 15 #define AOAINDEX 15
#define DATALENGTH 27 #define DATALENGTH 27
namespace uwb_slam{ namespace uwb_slam{
Uwb::Uwb(){ Uwb::Uwb(){
} }
void Uwb::Run() { void Uwb::Run() {
while(1){ while(1){
this->Serread(); this->Serread();
} }
} }
void Uwb::Serread(){ void Uwb::Serread(){
try { try {
boost::asio::io_service io; boost::asio::io_service io;
boost::asio::serial_port serial(io, "/dev/ttyUSB1"); // 替换成你的串口设备路径 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::baud_rate(115200)); // 设置波特率
serial.set_option(boost::asio::serial_port_base::character_size(8)); // 设置数据位 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::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)); // 设置停止位 serial.set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one)); // 设置停止位
uint8_t tmpdata[DATALENGTH]; uint8_t tmpdata[DATALENGTH];
// std::cerr << "befor read" << std::endl; // std::cerr << "befor read" << std::endl;
size_t bytesRead = boost::asio::read(serial, boost::asio::buffer(tmpdata, sizeof(tmpdata))); // 读取串口数据 size_t bytesRead = boost::asio::read(serial, boost::asio::buffer(tmpdata, sizeof(tmpdata))); // 读取串口数据
// std::cerr << "after read" << std::endl; // std::cerr << "after read" << std::endl;
this->uwb_data_.uwb_t_ = ros::Time::now(); this->uwb_data_.uwb_t_ = ros::Time::now();
for (int i=0;i< bytesRead;i++) for (int i=0;i< bytesRead;i++)
{ {
std::cout << std::hex<<static_cast<int>(tmpdata[i]) << " "; std::cout << std::hex<<static_cast<int>(tmpdata[i]) << " ";
} }
std::cout << std::endl; std::cout << std::endl;
memcpy(&this->d[0], &tmpdata[DISINDEX], sizeof(d[0])); memcpy(&this->d[0], &tmpdata[DISINDEX], sizeof(d[0]));
memcpy(&this->d[1], &tmpdata[DISINDEX + sizeof(d[0])], 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->d[2], &tmpdata[DISINDEX + sizeof(d[0]) * 2], sizeof(d[0]));
memcpy(&this->aoa[0], &tmpdata[AOAINDEX], sizeof(aoa[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[1], &tmpdata[AOAINDEX + sizeof(aoa[0])], sizeof(aoa[0]));
memcpy(&this->aoa[2], &tmpdata[AOAINDEX + sizeof(aoa[0]) * 2], 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; //std::cout << "d:" << d[0] << " " << d[1] << " " << d[2] << std::endl;
// if(abs(d[0]) > 2000 || abs(d[1]) > 2000 || abs(d[2]) > 2000) { // if(abs(d[0]) > 2000 || abs(d[1]) > 2000 || abs(d[2]) > 2000) {
// return; // return;
// } // }
// for(int i=0; i<3; i++) // 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)); // 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; 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[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[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; // d[2] = ((((-2.0616e-07 * d[2]) + 3.3886e-04) * d[2]) + 0.8231) * d[2] + 8.1566;
} catch (const std::exception& ex) { } catch (const std::exception& ex) {
std::cerr << "Exception: " << ex.what() << std::endl; std::cerr << "Exception: " << ex.what() << std::endl;
} }
} }
void fusion() void fusion()
{ {
} }
}; };
// bool Uwb::checknewdata() // bool Uwb::checknewdata()
// { // {
// std::unique_lock<std::mutex> lock(mMutexUwb); // std::unique_lock<std::mutex> lock(mMutexUwb);
// return !v_buffer_imu_odom_pose_data_.empty(); // return !v_buffer_imu_odom_pose_data_.empty();
// } // }
// void Uwb::Run() { // void Uwb::Run() {
// while(1) // while(1)
// { // {
// if(checknewdata()) // if(checknewdata())
// { // {
// { // {
// std::unique_lock<std::mutex> lock(mMutexUwb); // std::unique_lock<std::mutex> lock(mMutexUwb);
// Imu_odom_pose_data imu_odom_pose_data = v_buffer_imu_odom_pose_data_.front(); // Imu_odom_pose_data imu_odom_pose_data = v_buffer_imu_odom_pose_data_.front();
// v_buffer_imu_odom_pose_data_.pop(); // v_buffer_imu_odom_pose_data_.pop();
// } // }
// } // }
// } // }
// } // }
// void Uwb::feed_imu_odom_pose_data(const Imu_odom_pose_data& imu_odom_pose_data){ // void Uwb::feed_imu_odom_pose_data(const Imu_odom_pose_data& imu_odom_pose_data){
// std::unique_lock<std::mutex> lock(mMutexUwb); // std::unique_lock<std::mutex> lock(mMutexUwb);
// v_buffer_imu_odom_pose_data_.push(imu_odom_pose_data); // v_buffer_imu_odom_pose_data_.push(imu_odom_pose_data);
// } // }