diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/CMakeLists.txt b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/CMakeLists.txt new file mode 100644 index 0000000..63fddec --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/CMakeLists.txt @@ -0,0 +1,53 @@ +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}) + + diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/Mat.h b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/Mat.h new file mode 100644 index 0000000..f8cc745 --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/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/FollowingCar/include/align.h b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/align.h new file mode 100644 index 0000000..649b450 --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/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/FollowingCar/include/lighthouse.h b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/lighthouse.h new file mode 100644 index 0000000..f585dd9 --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/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/FollowingCar/include/mapping.h b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/mapping.h new file mode 100644 index 0000000..ac39d27 --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/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/FollowingCar/include/senddata.h b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/senddata.h new file mode 100644 index 0000000..6e21f61 --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/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/FollowingCar/include/type.h b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/type.h new file mode 100644 index 0000000..04b84d9 --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/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/FollowingCar/include/uwb.h b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/uwb.h new file mode 100644 index 0000000..7d1a939 --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/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/FollowingCar/package.xml b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/package.xml new file mode 100644 index 0000000..7415ed8 --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/package.xml @@ -0,0 +1,75 @@ + + + FollowingCar + 0.0.0 + The FollowingCar 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/FollowingCar/src/Mat.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/Mat.cpp new file mode 100644 index 0000000..4192cb5 --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/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/FollowingCar/src/lighthouse.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/lighthouse.cpp new file mode 100644 index 0000000..9aab08a --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/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/FollowingCar/src/main.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/main.cpp new file mode 100644 index 0000000..953540d --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/main.cpp @@ -0,0 +1,60 @@ +#include "Mat.h" + +// #include "align.h" +#include "mapping.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 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/FollowingCar/src/mapping.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/mapping.cpp new file mode 100644 index 0000000..35aeb2a --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/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/FollowingCar/src/senddata.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/senddata.cpp new file mode 100644 index 0000000..13280d8 --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/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/FollowingCar/src/uwb.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/uwb.cpp new file mode 100644 index 0000000..2db72c5 --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/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/CMakeLists.txt b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/CMakeLists.txt index 08a31d5..26392cc 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/CMakeLists.txt +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/CMakeLists.txt @@ -40,7 +40,12 @@ add_library(${PROJECT_NAME} SHARED include/senddata.h src/senddata.cpp) - +# add_message_files( +# DIRECTORY msg +# FILES +# RawImu.msg + +# ) generate_messages(DEPENDENCIES std_msgs geometry_msgs) catkin_package(CATKIN_DEPENDS std_msgs geometry_msgs) diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/read_sensor_data.h b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/read_sensor_data.h new file mode 100644 index 0000000..61a7df3 --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/read_sensor_data.h @@ -0,0 +1,34 @@ +#include +#include "nav_msgs/Odometry.h" +#include "geometry_msgs/Twist.h" +#include "sensor_msgs/Imu.h" +#include "geometry_msgs/PoseStamped.h" +#include "geometry_msgs/PoseWithCovarianceStamped.h" +#include +#include "type.h" +#include "uwb.h" + + +#ifndef READ_SENSOR_DATA_H +#define READ_SENSOR_DATA_H + +namespace uwb_slam{ + typedef boost::shared_ptr OdomConstPtr; + typedef boost::shared_ptr ImuConstPtr; + class Read_sensor_data + { + public: + Read_sensor_data(); + + void Run(int argc, char* argv[]); + //void set_uwb(Uwb * uwb); + void imu_call_back(const ImuConstPtr& imu); + void odom_call_back(const OdomConstPtr& odom); + + private: + ros::Subscriber imu_sub_; + ros::Subscriber odom_sub_; + + }; +} +#endif \ No newline at end of file diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/main.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/main.cpp index f854eee..a8e5068 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/main.cpp +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/main.cpp @@ -1,9 +1,6 @@ -#include "../include/system.h" -#include "../include/uwb.h" #include "../include/lighthouse.h" #include "Mat.h" - - +#include "mapping.h" #include #include #include @@ -14,13 +11,16 @@ int main(int argc, char** argv) { ros::init(argc, argv, "locate_info_pub_node"); // Initialize the ROS node + 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(); - 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(); - mp->uwb_ = uwb; + + + mp->uwb_ = uwb; mp->align_ = align; align->uwb_ = uwb; align->lighthouse_ = lighthouse; diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/mapping.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/mapping.cpp index 81cdc97..a6240b7 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/mapping.cpp +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/mapping.cpp @@ -76,7 +76,25 @@ namespace uwb_slam cv::imshow("Image",img); - + // bool printFlag = 0; + // std::ofstream outfile("data.txt",std::ofstream::out); + // if(outfile.is_open()) { + // std::cout << "start saving data" << key << std::endl; + // printFlag = 1; + // } else { + // std::cout<<"file can not open"<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' ) { @@ -91,9 +109,14 @@ namespace uwb_slam if (key2 == 'q') { //TODO: save - std::string pngimage = "/home/firefly/Project_Ros/src/ros_merge_test/Map/output_image.png";//保存的图片文件路径 - cv::imwrite(pngimage, img); - read_uwb_ = false; + std::string pngimage="/home/firefly/Project_Ros11/Project_Ros1/src/upbot_location/Map/output_image.png";//保存的图片文件路径 + cv::imwrite(pngimage,img); + readPos = false; + + // outfile.close(); + // printFlag = 0; + // std::cout<< "Data written to file." << std::endl; + break; } diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/read_sensor_data.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/read_sensor_data.cpp new file mode 100644 index 0000000..e74d0cd --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/read_sensor_data.cpp @@ -0,0 +1,30 @@ +#include + +namespace uwb_slam { + + + //void Read_sensor_data::set_uwb(){} + + + void Read_sensor_data::imu_call_back(const ImuConstPtr& imu){ + Imu_data d1= Imu_data(imu->linear_acceleration.x,imu->linear_acceleration.y,imu->linear_acceleration.z, + imu->angular_velocity.x,imu->angular_velocity.y,imu->angular_velocity.z); + + } + void Read_sensor_data::odom_call_back(const OdomConstPtr& odom){ + Odom_data d1 = Odom_data(odom->pose.pose.position.x, odom->pose.pose.position.y, odom->pose.pose.position.z, + odom->pose.pose.orientation.w,odom->pose.pose.orientation.x, odom->pose.pose.orientation.y, odom->pose.pose.orientation.z, + odom->header.stamp,odom->twist.twist.linear.x,odom->twist.twist.linear.y,odom->twist.twist.angular.z); + + } + void Read_sensor_data::Run(int argc, char* argv[]){ + + ros::init(argc, argv, "imu_odom"); + // 创建一个节点句柄 + ros::NodeHandle nh; + //imu_sub_ = nh.subscribe("imu/data_raw", 1000, this->imu_call_back); + //odom_sub_ =nh.subscribe("odom", 1000,odom_call_back); + ros::spin(); + } + +} diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/senddata.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/senddata.cpp index 20ad4f8..c6681ae 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/senddata.cpp +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/senddata.cpp @@ -4,38 +4,25 @@ namespace uwb_slam { void Senddata::Run(std::shared_ptruwb){ - // 初始化了一个名为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_ptr uwb) -{ + void Senddata::publishOdometry(std::shared_ptruwb) + { std::mutex mMutexSend; ros::Time current_time = ros::Time::now(); diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/system.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/system.cpp new file mode 100644 index 0000000..44f5878 --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/system.cpp @@ -0,0 +1,15 @@ +#include "../include/system.h" +//#include "" + + +namespace uwb_slam{ + + void System::Run() + { + while(1){ + + } + + } + +} \ No newline at end of file diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/uwb.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/uwb.cpp index 94a5cf1..2a17887 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/uwb.cpp +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/uwb.cpp @@ -2,7 +2,8 @@ #include #include "Mat.h" -#define PI acos(-1) + +#define CARHEIGHT 20 namespace uwb_slam{ Mat A; @@ -125,7 +126,10 @@ namespace uwb_slam{ } - + void fusion() + { + + } @@ -134,4 +138,34 @@ namespace uwb_slam{ }; +// bool Uwb::checknewdata() +// { +// std::unique_lock 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); +// }