From 27e2fee50a6d18f619fed76e2f580086a605bd2d Mon Sep 17 00:00:00 2001 From: UESTCsecurity Date: Thu, 14 Mar 2024 10:29:15 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E8=8B=A5=E5=B9=B2=E4=BB=A3?= =?UTF-8?q?=E7=A0=81=E6=B3=A8=E9=87=8A=E4=B9=B1=E7=A0=81=E9=97=AE=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../ros_ws/src/upbot_following/CMakeLists.txt | 53 +++ .../ros_ws/src/upbot_following/include/Mat.h | 83 ++++ .../src/upbot_following/include/align.h | 44 +++ .../src/upbot_following/include/lighthouse.h | 29 ++ .../src/upbot_following/include/mapping.h | 40 ++ .../src/upbot_following/include/senddata.h | 50 +++ .../ros_ws/src/upbot_following/include/type.h | 50 +++ .../ros_ws/src/upbot_following/include/uwb.h | 60 +++ .../ros_ws/src/upbot_following/package.xml | 75 ++++ .../ros_ws/src/upbot_following/src/Mat.cpp | 368 ++++++++++++++++++ .../ros_ws/src/upbot_following/src/align.cpp | 94 +++++ .../src/upbot_following/src/lighthouse.cpp | 79 ++++ .../ros_ws/src/upbot_following/src/main.cpp | 61 +++ .../src/upbot_following/src/mapping.cpp | 149 +++++++ .../src/upbot_following/src/senddata.cpp | 147 +++++++ .../ros_ws/src/upbot_following/src/uwb.cpp | 121 ++++++ .../ros_ws/src/upbot_location/include/Mat.h | 55 ++- .../ros_ws/src/upbot_location/src/Mat.cpp | 30 +- .../src/upbot_location/src/senddata.cpp | 15 +- 19 files changed, 1557 insertions(+), 46 deletions(-) create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/CMakeLists.txt create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/Mat.h create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/align.h create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/lighthouse.h create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/mapping.h create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/senddata.h create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/type.h create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/uwb.h create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/package.xml create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/Mat.cpp create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/align.cpp create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/lighthouse.cpp create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/main.cpp create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/mapping.cpp create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/senddata.cpp create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/uwb.cpp diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/CMakeLists.txt b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/CMakeLists.txt new file mode 100644 index 0000000..16fcef8 --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/CMakeLists.txt @@ -0,0 +1,53 @@ +cmake_minimum_required(VERSION 3.0.2) +project(upbot_following) + +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 upbot_following +# # 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/system.cpp + 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}) + + diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/Mat.h b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/Mat.h new file mode 100644 index 0000000..9f0a9b3 --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/Mat.h @@ -0,0 +1,83 @@ +/******************** (C) COPYRIGHT 2022 Geek************************************ +* File Name : Mat.h +* Current Version : V1.0 +* Author : logzhan +* Date of Issued : 2022.09.14 +* Comments : �����ľ������ +********************************************************************************/ +/* Header File Including -----------------------------------------------------*/ +#ifndef _H_MAT_ +#define _H_MAT_ + +#define MAT_MAX 15 //�������ܴ����������� + + +#include +#define _USE_MATH_DEFINES +#include + +class Mat +{ +public: + Mat(); + Mat(int setm,int setn,int kind);//kind=1��λ��kind=0�����,��������ʼ�����ݡ� + void Init(int setm,int setn,int kind);//kind=1��λ��kind=0�����,��������ʼ�����ݡ� + + void Zero(void); + //��Щ�ؼ�����Ӧ����Ϊprivate�ġ�����Ϊ�˷��㣬��Ҳ������public + int m;//���� + int n;//���� + double mat[MAT_MAX][MAT_MAX];//������������ + + //����ľ��� + Mat SubMat(int a,int b,int lm,int ln);//��ȡ����һ���� + void FillSubMat(int a,int b,Mat s);//����Ӿ��� + + //����ר�� + double absvec();//����������ij��ȡ����Ǹ���Ԫ�صľ���ֵ�� + double Sqrt();//�������ȵ�ƽ�� + friend Mat operator ^(Mat a,Mat b);//��� + + //���� + 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);//ת�� + friend Mat operator /(Mat a,Mat b);//a*inv(b) + friend Mat operator %(Mat a,Mat b);//inv(a)*b + + //MAT inv();//����� + +private: + // Ϊ���ø�˹��Ԫ��������һЩ���� + // �������� + void RowExchange(int a, int b); + // ijһ�г���ϵ�� + void RowMul(int a,double k); + // ��ijһ�мӼ���һ�еı��� + void RowAdd(int a,int b, double k); + // �������� + void ColExchange(int a, int b); + // ijһ�г���ϵ�� + void ColMul(int a,double k); + // ��ijһ�мӼ���һ�еı��� + void ColAdd(int a,int b,double k); + + +}; + + + + + + +#endif + + + + + + diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/align.h b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/align.h new file mode 100644 index 0000000..834c358 --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/align.h @@ -0,0 +1,44 @@ +#include +#include +#include +#include +#include +#include +#include +#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> data_queue; + std::shared_ptr uwb_; + std::shared_ptr lighthouse_; + }; +}; +#endif diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/lighthouse.h b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/lighthouse.h new file mode 100644 index 0000000..333f9e5 --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/lighthouse.h @@ -0,0 +1,29 @@ +#include +#include +#include +#include +#include +#include +#include "type.h" +#include +#include +#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 diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/mapping.h b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/mapping.h new file mode 100644 index 0000000..f590e6d --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/mapping.h @@ -0,0 +1,40 @@ +#include +#include +#include +#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_; + std::shared_ptr align_; + + private: + int realWidth, realHeight; + std::queue> posDataQueue; + std::vector posData = std::vector(3, {-1, -1}); + //cv::Point2d imuPoint = {-1,-1}; + // std::queue posDataQueue; + + + bool readPos = false; + cv::Mat img; + }; + +} + +#endif diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/senddata.h b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/senddata.h new file mode 100644 index 0000000..7d645d8 --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/senddata.h @@ -0,0 +1,50 @@ +#include +#include +#include +#include +#include +#include +#include "uwb.h" +#include +#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_ptruwb); + void Run(std::shared_ptruwb); + void odomCB(const nav_msgs::Odometry& odom); + void stereoCB(const pibot_msgs::dis_info_array::ConstPtr& stereo); + + std::shared_ptr 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 \ No newline at end of file diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/type.h b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/type.h new file mode 100644 index 0000000..53b77e2 --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/type.h @@ -0,0 +1,50 @@ +#include +#include +#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 \ No newline at end of file diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/uwb.h b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/uwb.h new file mode 100644 index 0000000..3bed627 --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/uwb.h @@ -0,0 +1,60 @@ +#include +#include +#include +#include +#include +#include +#include "type.h" +#include +#include +#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 v_buffer_imu_odom_pose_data_; + + + Uwb_data uwb_data_; + // ros_merge_test::RawImu sub_imu_; + // std::queue imu_odom_queue_; + // std::queue 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 diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/package.xml b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/package.xml new file mode 100644 index 0000000..5adc08c --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/package.xml @@ -0,0 +1,75 @@ + + + upbot_following + 0.0.0 + The upbot_following package + + + + + luoruidi + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + std_msgs + pibot_msgs + + geometry_msgs + message_generation + + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + message_runtime + geometry_msgs + + + + + + + + diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/Mat.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/Mat.cpp new file mode 100644 index 0000000..19cea34 --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/Mat.cpp @@ -0,0 +1,368 @@ +/******************** (C) COPYRIGHT 2022 Geek************************************ +* File Name : Mat.cpp +* Current Version : V1.0 +* Author : logzhan +* Date of Issued : 2022.09.14 +* Comments : ��������� +********************************************************************************/ +/* 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(bm = setm; + this->n = setn; + if((kind==0)||(kind==1)) + { + memset(mat,0,MAT_MAX*MAT_MAX*sizeof(double)); + } + + if(kind==1) + { + int x; + //Cԭ�е�max min�ᵼ�����������Ա������и��������Ķ�����Ҫֱ�ӷŵ�max���档 + 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;xs) + { + p=xb; + s=sxb; + } + } + //ͬʱ�任������� + if(x!=p) + { + a.ColExchange(x,p); + b.ColExchange(x,p); + } + + //��һ�й�һ + double k=1/b.mat[x][x];//��һ�䲻ҪǶ�׵����������У��������Ϊ���²�ͬ�����¼������ + a.ColMul(x,k); + b.ColMul(x,k); + + //�������й��� + for(xb=0;xbs) + { + p=xb; + s=sxb; + } + } + //ͬʱ�任������� + if(x!=p) + { + a.RowExchange(x,p); + b.RowExchange(x,p); + } + + //��һ�й�һ + double k=1/a.mat[x][x];//��һ�䲻ҪǶ�׵����������У��������Ϊ���²�ͬ�����¼������ + a.RowMul(x,k); + b.RowMul(x,k); + + //�������й��� + for(xb=0;xbuwb_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; + } + +}; + + + diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/lighthouse.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/lighthouse.cpp new file mode 100644 index 0000000..562b494 --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/lighthouse.cpp @@ -0,0 +1,79 @@ +#include "lighthouse.h" +#include +#include + +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; + + } +}; \ No newline at end of file diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/main.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/main.cpp new file mode 100644 index 0000000..d277e6e --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/main.cpp @@ -0,0 +1,61 @@ +#include "../include/system.h" +#include "../include/uwb.h" +#include "../include/lighthouse.h" +#include "Mat.h" + + +#include +#include +#include +#include "senddata.h" + + + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "locate_info_pub_node"); // Initialize the ROS node + + std::shared_ptr system = std::make_shared(); + std::shared_ptr mp = std::make_shared(); + std::shared_ptr uwb = std::make_shared(); + std::shared_ptr sender = std::make_shared(); + std::shared_ptr align = std::make_shared(); + std::shared_ptr lighthouse = std::make_shared(); + + + + + 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(); +} diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/mapping.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/mapping.cpp new file mode 100644 index 0000000..08de7e1 --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/mapping.cpp @@ -0,0 +1,149 @@ +#include "mapping.h" +#include +#include // 包含 usleep() 函数所在的头文件 +#include +#include + +namespace uwb_slam +{ + bool Mapping::checkQueue() + { + //std::unique_lock lock(mMutexMap); + return !posDataQueue.empty(); + } + + void Mapping::feedPos(const cv::Point2d & imuPosData, const cv::Point2d & uwbPosData, const cv::Point2d & syncPosData) + { + //std::unique_lock lock(mMutexMap); + posDataQueue.push({imuPosData, uwbPosData, syncPosData}); + + } + + + void Mapping::process() + { + { + + //std::unique_lock lock(mMutexMap); + //std::cout << "SIZE: " <(imuPosPointY, imuPosPointX) = 0; + img.at(imuPosPointY, imuPosPointX) = cv::Vec3b(0,0,255); //imu trace is red + img.at(uwbPosPointY, uwbPosPointX) = cv::Vec3b(0,255,0); //uwb trace is green + img.at(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(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(j,i)= cv::Vec3b(0,0,0); + int i = 0, j = 0; + img.at(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"<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: " <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 + diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/senddata.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/senddata.cpp new file mode 100644 index 0000000..b3e1f8a --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/senddata.cpp @@ -0,0 +1,147 @@ +#include "senddata.h" + +#define angleLimit 20 +#define disLimit 200 + +namespace uwb_slam +{ + void Senddata::Run(std::shared_ptruwb){ + + ros::Rate loop_rate(10); + // position_pub_=nh_.advertise("uwb_odom",50); + cmd_vel_pub_ = nh_.advertise("cmd_vel",50); + obstacles_sub_ = nh_.subscribe("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_ptruwb) + // { + + // 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 \ No newline at end of file diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/uwb.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/uwb.cpp new file mode 100644 index 0000000..15013f0 --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/uwb.cpp @@ -0,0 +1,121 @@ +#include "uwb.h" +#include +#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<(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: "<d[0]<<" aoa: "<aoa[0]< lock(mMutexUwb); +// return !v_buffer_imu_odom_pose_data_.empty(); +// } + +// void Uwb::Run() { +// while(1) +// { +// if(checknewdata()) +// { +// { +// std::unique_lock 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 lock(mMutexUwb); +// v_buffer_imu_odom_pose_data_.push(imu_odom_pose_data); +// } + diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/Mat.h b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/Mat.h index f8cc745..0586c6c 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/Mat.h +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/Mat.h @@ -3,13 +3,13 @@ * Current Version : V1.0 * Author : logzhan * Date of Issued : 2022.09.14 -* Comments : �����ľ������ +* Comments : 导航的矩阵计算 ********************************************************************************/ /* Header File Including -----------------------------------------------------*/ #ifndef _H_MAT_ #define _H_MAT_ -#define MAT_MAX 15 //�������ܴ����������� +#define MAT_MAX 15 //决定了能处理的最大矩阵 #include @@ -20,60 +20,55 @@ class Mat { public: Mat(); - Mat(int setm,int setn,int kind);//kind=1��λ��kind=0�����,��������ʼ�����ݡ� - void Init(int setm,int setn,int kind);//kind=1��λ��kind=0�����,��������ʼ�����ݡ� + Mat(int setm,int setn,int kind);//kind=1单位阵，kind=0零矩阵,其它不初始化内容。 + void Init(int setm,int setn,int kind);//kind=1单位阵，kind=0零矩阵,其它不初始化内容。 void Zero(void); - //��Щ�ؼ�����Ӧ����Ϊprivate�ġ�����Ϊ�˷��㣬��Ҳ������public - int m;//���� - int n;//���� - double mat[MAT_MAX][MAT_MAX];//������������ + //这些关键数本应该作为private的。但是为了方便，我也做成了public + int m;//行数 + int n;//列数 + double mat[MAT_MAX][MAT_MAX];//矩阵数据内容 - //����ľ��� - Mat SubMat(int a,int b,int lm,int ln);//��ȡ����һ���� - void FillSubMat(int a,int b,Mat s);//����Ӿ��� + //特殊的矩阵 + Mat SubMat(int a,int b,int lm,int ln);//获取矩阵一部分 + void FillSubMat(int a,int b,Mat s);//填充子矩阵 - //����ר�� - double absvec();//����������ij��ȡ����Ǹ���Ԫ�صľ���ֵ�� - double Sqrt();//�������ȵ�ƽ�� - friend Mat operator ^(Mat a,Mat b);//��� + //向量专用 + double absvec();//这个是向量的长度。不是个别元素的绝对值。 + double Sqrt();//向量长度的平方 + friend Mat operator ^(Mat a,Mat b);//叉乘 - //���� + //运算 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);//ת�� + friend Mat operator ~(Mat a);//转置 friend Mat operator /(Mat a,Mat b);//a*inv(b) friend Mat operator %(Mat a,Mat b);//inv(a)*b - //MAT inv();//����� + //MAT inv();//逆矩阵 private: - // Ϊ���ø�˹��Ԫ��������һЩ���� - // �������� + // 为了用高斯消元法，做的一些函数 + // 交换两行 void RowExchange(int a, int b); - // ijһ�г���ϵ�� + // 某一行乘以系数 void RowMul(int a,double k); - // ��ijһ�мӼ���һ�еı��� + // 对某一行加减另一行的倍数 void RowAdd(int a,int b, double k); - // �������� + // 交换两列 void ColExchange(int a, int b); - // ijһ�г���ϵ�� + // 某一列乘以系数 void ColMul(int a,double k); - // ��ijһ�мӼ���һ�еı��� + // 对某一列加减另一列的倍数 void ColAdd(int a,int b,double k); }; - - - - - #endif diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/Mat.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/Mat.cpp index 4192cb5..8bbf4da 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/Mat.cpp +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/Mat.cpp @@ -3,7 +3,7 @@ * Current Version : V1.0 * Author : logzhan * Date of Issued : 2022.09.14 -* Comments : ��������� +* Comments : 矩阵运算库 ********************************************************************************/ /* Header File Including -----------------------------------------------------*/ #include "Mat.h" @@ -28,7 +28,7 @@ int mini(int a,int b) } -//��Ҫ�ڳ�Ա�����ڵ��ù��캯�� +//不要在成员函数内调用构造函数 Mat::Mat() { Init(1,1,0); @@ -51,7 +51,7 @@ void Mat::Init(int setm,int setn,int kind) if(kind==1) { int x; - //Cԭ�е�max min�ᵼ�����������Ա������и��������Ķ�����Ҫֱ�ӷŵ�max���档 + //C原有的max min会导致两次运行自变量。有附带操作的东西不要直接放到max里面。 int xend = mini(this->m, this->n); for(x=0;x < xend;x++){ mat[x][x] = 1; @@ -184,12 +184,12 @@ Mat operator ~(Mat a) Mat operator /(Mat a,Mat b) { - //��˹��Ԫ�� + //高斯消元法 int x,xb; for(x=0;xuwb){ - + // 初始化了一个名为loop_rate的ros::Rate对象,频率设置为10赫兹 ros::Rate loop_rate(10); + // 初始化一个ROS发布者，用于发布nav_msgs::Odometry类型的消息 + // 主题被设置为"uwb_odom"，队列大小为50 position_pub_=nh_.advertise("uwb_odom",50); + // 初始化了一个ROS订阅者，用于订阅"odom"主题。它指定了当在该主题上接收到 + // 消息时，将调用Senddata类的odomCB回调函数。队列大小被设置为10 odom_sub_=nh_.subscribe("odom",10,&Senddata::odomCB,this); while(ros::ok()){ + // 按照10Hz频率发布uwb信息 publishOdometry(uwb); ros::spinOnce(); + // 用于控制循环速率 loop_rate.sleep(); } } void Senddata::odomCB(const nav_msgs::Odometry& odom){ + // 这个地方接收的是轮速里程计的信息 + // 包含位置和姿态 sub_odom_ = odom; return; } + /**--------------------------------------------------------------------- + * Function : Senddata::publishOdometry + * Description : 发布UWB里程计数据，这里读取的数据到底是什么，依旧存在疑问 + * Date : 2023/12/13 zhanli@review + *---------------------------------------------------------------------**/ void Senddata::publishOdometry(std::shared_ptruwb) {