From f6634135ddb9a7ae6b9dd2c5d82fcb9e08cf2ace Mon Sep 17 00:00:00 2001 From: ray <2954701669@qq.com> Date: Thu, 14 Mar 2024 03:23:26 +0000 Subject: [PATCH] upbot_following --- .../ros_ws/src/FollowingCar/CMakeLists.txt | 53 -- .../ros_ws/src/FollowingCar/include/Mat.h | 83 -- .../ros_ws/src/FollowingCar/include/align.h | 44 -- .../src/FollowingCar/include/lighthouse.h | 29 - .../ros_ws/src/FollowingCar/include/mapping.h | 40 - .../src/FollowingCar/include/senddata.h | 50 -- .../ros_ws/src/FollowingCar/include/type.h | 50 -- .../ros_ws/src/FollowingCar/include/uwb.h | 60 -- .../ros_ws/src/FollowingCar/package.xml | 75 -- .../ros_ws/src/FollowingCar/src/Mat.cpp | 368 --------- .../ros_ws/src/FollowingCar/src/align.cpp | 94 --- .../src/FollowingCar/src/lighthouse.cpp | 79 -- .../ros_ws/src/FollowingCar/src/main.cpp | 60 -- .../ros_ws/src/FollowingCar/src/mapping.cpp | 149 ---- .../ros_ws/src/FollowingCar/src/senddata.cpp | 147 ---- .../ros_ws/src/FollowingCar/src/uwb.cpp | 121 --- .../ros_ws/src/upbot_following/CMakeLists.txt | 106 +-- .../ros_ws/src/upbot_following/include/Mat.h | 166 ++-- .../src/upbot_following/include/align.h | 88 +-- .../src/upbot_following/include/lighthouse.h | 58 +- .../src/upbot_following/include/mapping.h | 80 +- .../src/upbot_following/include/senddata.h | 98 +-- .../ros_ws/src/upbot_following/include/type.h | 98 +-- .../ros_ws/src/upbot_following/include/uwb.h | 120 +-- .../ros_ws/src/upbot_following/package.xml | 150 ++-- .../ros_ws/src/upbot_following/src/Mat.cpp | 736 +++++++++--------- .../ros_ws/src/upbot_following/src/align.cpp | 188 ++--- .../src/upbot_following/src/lighthouse.cpp | 156 ++-- .../ros_ws/src/upbot_following/src/main.cpp | 121 ++- .../src/upbot_following/src/mapping.cpp | 298 +++---- .../src/upbot_following/src/senddata.cpp | 292 +++---- .../ros_ws/src/upbot_following/src/uwb.cpp | 242 +++--- 32 files changed, 1498 insertions(+), 3001 deletions(-) delete mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/CMakeLists.txt delete mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/Mat.h delete mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/align.h delete mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/lighthouse.h delete mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/mapping.h delete mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/senddata.h delete mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/type.h delete mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/uwb.h delete mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/package.xml delete mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/Mat.cpp delete mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/align.cpp delete mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/lighthouse.cpp delete mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/main.cpp delete mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/mapping.cpp delete mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/senddata.cpp delete mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/uwb.cpp diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/CMakeLists.txt b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/CMakeLists.txt deleted file mode 100644 index 63fddec..0000000 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/CMakeLists.txt +++ /dev/null @@ -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}) - - 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 deleted file mode 100644 index f8cc745..0000000 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/Mat.h +++ /dev/null @@ -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 : �����ľ������ -********************************************************************************/ -/* 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 deleted file mode 100644 index 649b450..0000000 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/align.h +++ /dev/null @@ -1,44 +0,0 @@ -#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 deleted file mode 100644 index f585dd9..0000000 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/lighthouse.h +++ /dev/null @@ -1,29 +0,0 @@ -#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 deleted file mode 100644 index ac39d27..0000000 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/mapping.h +++ /dev/null @@ -1,40 +0,0 @@ -#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 deleted file mode 100644 index 6e21f61..0000000 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/senddata.h +++ /dev/null @@ -1,50 +0,0 @@ -#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 deleted file mode 100644 index 04b84d9..0000000 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/type.h +++ /dev/null @@ -1,50 +0,0 @@ -#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 deleted file mode 100644 index 7d1a939..0000000 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/uwb.h +++ /dev/null @@ -1,60 +0,0 @@ -#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 deleted file mode 100644 index 7415ed8..0000000 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/package.xml +++ /dev/null @@ -1,75 +0,0 @@ - - - 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 deleted file mode 100644 index 4192cb5..0000000 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/Mat.cpp +++ /dev/null @@ -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 : ��������� -********************************************************************************/ -/* 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 deleted file mode 100644 index 9aab08a..0000000 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/lighthouse.cpp +++ /dev/null @@ -1,79 +0,0 @@ -#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 deleted file mode 100644 index 953540d..0000000 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/main.cpp +++ /dev/null @@ -1,60 +0,0 @@ -#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 deleted file mode 100644 index 35aeb2a..0000000 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/mapping.cpp +++ /dev/null @@ -1,149 +0,0 @@ -#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 deleted file mode 100644 index 13280d8..0000000 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/senddata.cpp +++ /dev/null @@ -1,147 +0,0 @@ -#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 deleted file mode 100644 index 2db72c5..0000000 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/uwb.cpp +++ /dev/null @@ -1,121 +0,0 @@ -#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_following/CMakeLists.txt b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/CMakeLists.txt index 16fcef8..728b454 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/CMakeLists.txt +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/CMakeLists.txt @@ -1,53 +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}) - - +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 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/upbot_following/include/Mat.h b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/Mat.h index 9f0a9b3..f8cc745 100644 --- 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 @@ -1,83 +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 - - - - - - +/******************** (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 index 834c358..649b450 100644 --- 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 @@ -1,44 +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 +#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 index 333f9e5..f585dd9 100644 --- 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 @@ -1,29 +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 +#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 index f590e6d..ac39d27 100644 --- 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 @@ -1,40 +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 +#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 index 7d645d8..6e21f61 100644 --- 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 @@ -1,50 +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; - - - }; - -} - +#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 index 53b77e2..04b84d9 100644 --- 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 @@ -1,50 +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){}; -}; - -} +#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 index 3bed627..7d1a939 100644 --- 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 @@ -1,60 +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 +#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 index 5adc08c..8bdc0fa 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/package.xml +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/package.xml @@ -1,75 +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 - - - - - - - - + + + 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 index 19cea34..4192cb5 100644 --- 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 @@ -1,368 +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;xbm = 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; - } - -}; - - - +#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"<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; + } + +}; + + + 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 index 562b494..9aab08a 100644 --- 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 @@ -1,79 +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; - - } +#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 index d277e6e..953540d 100644 --- 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 @@ -1,61 +1,60 @@ -#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(); -} +#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/upbot_following/src/mapping.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/mapping.cpp index 08de7e1..35aeb2a 100644 --- 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 @@ -1,149 +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 - +#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 index b3e1f8a..13280d8 100644 --- 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 @@ -1,147 +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_); - - - // } - - - - - +#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 index 15013f0..2db72c5 100644 --- 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 @@ -1,121 +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); -// } - +#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); +// } + -- 2.37.2.windows.2