From 414117743d58211779a064084bd20f54c70f679e Mon Sep 17 00:00:00 2001 From: ray <2954701669@qq.com> Date: Mon, 11 Mar 2024 07:25:52 +0000 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9msg=E6=96=87=E4=BB=B6?= =?UTF-8?q?=E5=9C=A8pibot=5Fmsgs=E4=B8=8B=EF=BC=8C=E7=BB=9F=E4=B8=80?= =?UTF-8?q?=E7=BC=96=E8=AF=91?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../ros_ws/src/FollowingCar/CMakeLists.txt | 60 --- .../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 -- .../FollowingCar/include/read_sensor_data.h | 34 -- .../src/FollowingCar/include/senddata.h | 50 --- .../ros_ws/src/FollowingCar/include/system.h | 32 -- .../ros_ws/src/FollowingCar/include/type.h | 50 --- .../ros_ws/src/FollowingCar/include/uwb.h | 60 --- .../ros_ws/src/FollowingCar/msg/RawImu.msg | 7 - .../src/FollowingCar/msg/dis_info_array.msg | 1 - .../ros_ws/src/FollowingCar/package.xml | 73 ---- .../ros_ws/src/FollowingCar/src/Mat.cpp | 368 ------------------ .../ros_ws/src/FollowingCar/src/align.cpp | 101 ----- .../src/FollowingCar/src/lighthouse.cpp | 79 ---- .../ros_ws/src/FollowingCar/src/main.cpp | 65 ---- .../ros_ws/src/FollowingCar/src/mapping.cpp | 149 ------- .../src/FollowingCar/src/read_sensor_data.cpp | 30 -- .../ros_ws/src/FollowingCar/src/senddata.cpp | 147 ------- .../ros_ws/src/FollowingCar/src/system.cpp | 15 - .../ros_ws/src/FollowingCar/src/uwb.cpp | 121 ------ .../ros_ws/src/pibot_msgs/CMakeLists.txt | 2 + .../msg/dis_info.msg | 0 .../src/pibot_msgs/msg/dis_info_array.msg | 1 + .../ros_ws/src/upbot_location/CMakeLists.txt | 13 +- .../ros_ws/src/upbot_location/include/align.h | 4 +- .../ros_ws/src/upbot_location/msg/RawImu.msg | 7 - .../ros_ws/src/upbot_location/package.xml | 6 +- .../ros_ws/src/upbot_location/src/align.cpp | 2 +- .../ros_ws/src/upbot_vision/CMakeLists.txt | 21 +- .../ros_ws/src/upbot_vision/msg/dis_info.msg | 4 - .../src/upbot_vision/msg/dis_info_array.msg | 1 - .../ros_ws/src/upbot_vision/package.xml | 5 +- .../ros_ws/src/upbot_vision/src/main.cc | 10 +- 35 files changed, 34 insertions(+), 1680 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/read_sensor_data.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/system.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/msg/RawImu.msg delete mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/msg/dis_info_array.msg 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/read_sensor_data.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/system.cpp delete mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/uwb.cpp rename Code/MowingRobot/pibot_ros/ros_ws/src/{FollowingCar => pibot_msgs}/msg/dis_info.msg (100%) create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/pibot_msgs/msg/dis_info_array.msg delete mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/msg/RawImu.msg delete mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/msg/dis_info.msg delete mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/msg/dis_info_array.msg 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 7c776e9..0000000 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/CMakeLists.txt +++ /dev/null @@ -1,60 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(FollowingCar) - -find_package(catkin REQUIRED COMPONENTS - roscpp - rospy - std_msgs - message_generation - geometry_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/system.cpp - src/uwb.cpp - src/mapping.cpp - src/align.cpp - src/Mat.cpp - src/lighthouse.cpp - # src/read_sensor_data.cpp - include/senddata.h src/senddata.cpp) - - -add_message_files( - DIRECTORY msg - FILES - RawImu.msg - dis_info_array.msg - dis_info.msg - -) -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 2ef0d39..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 "FollowingCar/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 FollowingCar::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 333f9e5..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/read_sensor_data.h b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/read_sensor_data.h deleted file mode 100644 index 61a7df3..0000000 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/read_sensor_data.h +++ /dev/null @@ -1,34 +0,0 @@ -#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/FollowingCar/include/senddata.h b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/senddata.h deleted file mode 100644 index 3994793..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 "FollowingCar/dis_info.h" -#include "FollowingCar/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 FollowingCar::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/system.h b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/system.h deleted file mode 100644 index 848f92b..0000000 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/system.h +++ /dev/null @@ -1,32 +0,0 @@ -#include -#include -#include "mapping.h" -#include "uwb.h" -#include "senddata.h" -// #include "align.h" -#include - -#ifndef SYSTEM_H -#define SYSTEM_H - -namespace uwb_slam{ - class System{ - - public: - System() { - } - void Run(); - public: - - std::shared_ptrMapping_; - std::shared_ptrUwb_; - std::shared_ptrSender_; - std::shared_ptrAlign_; - std::shared_ptrLighthouse_; - - // Uwb* Uwb_ ; - // Senddata* Sender_; - // Mapping* Mapping_; - }; -} -#endif 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 53b77e2..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/msg/RawImu.msg b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/msg/RawImu.msg deleted file mode 100644 index 3c5ad41..0000000 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/msg/RawImu.msg +++ /dev/null @@ -1,7 +0,0 @@ -Header header -bool accelerometer -bool gyroscope -bool magnetometer -geometry_msgs/Vector3 raw_linear_acceleration -geometry_msgs/Vector3 raw_angular_velocity -geometry_msgs/Vector3 raw_magnetic_field diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/msg/dis_info_array.msg b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/msg/dis_info_array.msg deleted file mode 100644 index 39ec3cf..0000000 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/msg/dis_info_array.msg +++ /dev/null @@ -1 +0,0 @@ -FollowingCar/dis_info[] dis \ No newline at end of file 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 eed5c12..0000000 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/package.xml +++ /dev/null @@ -1,73 +0,0 @@ - - - FollowingCar - 0.0.0 - The FollowingCar package - - - - - luoruidi - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - rospy - std_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; - // imu_odom_.pose_[0] = wheel_odom.pose.pose.position.x; - // imu_odom_.pose_[1] = wheel_odom.pose.pose.position.y; - // imu_odom_.pose_[2] = wheel_odom.pose.pose.position.z; - // imu_odom_.quat_[0] = wheel_odom.pose.pose.orientation.x; - // imu_odom_.quat_[1] = wheel_odom.pose.pose.orientation.y; - // imu_odom_.quat_[2] = wheel_odom.pose.pose.orientation.z; - // imu_odom_.quat_[3] = wheel_odom.pose.pose.orientation.w; - return; - } - void Align::imuCB(const FollowingCar::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 562b494..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 bed1c0f..0000000 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/main.cpp +++ /dev/null @@ -1,65 +0,0 @@ -#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(); - - - system->Mapping_ = mp; - system->Uwb_ = uwb; - system->Sender_ = sender; - system->Align_ = align; - system->Lighthouse_ = lighthouse; - - mp->uwb_ = system->Uwb_; - mp->align_ = system->Align_; - align->uwb_ = system->Uwb_; - align->lighthouse_ = system->Lighthouse_; - sender->uwb_ = system->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 d9555b3..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/FollowingCar/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/read_sensor_data.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/read_sensor_data.cpp deleted file mode 100644 index e74d0cd..0000000 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/read_sensor_data.cpp +++ /dev/null @@ -1,30 +0,0 @@ -#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/FollowingCar/src/senddata.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/senddata.cpp deleted file mode 100644 index 2f659aa..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 FollowingCar::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/system.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/system.cpp deleted file mode 100644 index 44f5878..0000000 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/system.cpp +++ /dev/null @@ -1,15 +0,0 @@ -#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/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/pibot_msgs/CMakeLists.txt b/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_msgs/CMakeLists.txt index 839266a..5698069 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_msgs/CMakeLists.txt +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_msgs/CMakeLists.txt @@ -13,6 +13,8 @@ add_message_files( RobotState.msg NaviStatus.msg RelocateResult.msg + dis_info.msg + dis_info_array.msg ) add_service_files( diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/msg/dis_info.msg b/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_msgs/msg/dis_info.msg similarity index 100% rename from Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/msg/dis_info.msg rename to Code/MowingRobot/pibot_ros/ros_ws/src/pibot_msgs/msg/dis_info.msg diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_msgs/msg/dis_info_array.msg b/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_msgs/msg/dis_info_array.msg new file mode 100644 index 0000000..cccfba6 --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_msgs/msg/dis_info_array.msg @@ -0,0 +1 @@ +pibot_msgs/dis_info[] dis \ No newline at end of file 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 fee63c4..ce986b8 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 @@ -6,6 +6,7 @@ find_package(catkin REQUIRED COMPONENTS rospy std_msgs message_generation + pibot_msgs geometry_msgs ) @@ -41,15 +42,15 @@ add_library(${PROJECT_NAME} SHARED include/senddata.h src/senddata.cpp) -add_message_files( - DIRECTORY msg - FILES - RawImu.msg +# add_message_files( +# DIRECTORY msg +# FILES +# RawImu.msg -) +# ) generate_messages(DEPENDENCIES std_msgs geometry_msgs) -catkin_package(CATKIN_DEPENDS message_runtime std_msgs geometry_msgs) +catkin_package(CATKIN_DEPENDS std_msgs geometry_msgs) add_executable(${PROJECT_NAME}_node src/main.cpp) diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/align.h b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/align.h index 28eda90..b11132c 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/align.h +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/align.h @@ -5,7 +5,7 @@ #include #include #include -#include "upbot_location/RawImu.h" +#include "pibot_msgs/RawImu.h" #include "type.h" #include "uwb.h" #include "lighthouse.h" @@ -27,7 +27,7 @@ namespace uwb_slam{ }; void Run(); void wheel_odomCB(const nav_msgs::Odometry& wheel_odom); - void imuCB(const upbot_location::RawImu& imu); + void imuCB(const pibot_msgs::RawImu& imu); void odomCB(const nav_msgs::Odometry& odom); public: diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/msg/RawImu.msg b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/msg/RawImu.msg deleted file mode 100644 index 3c5ad41..0000000 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/msg/RawImu.msg +++ /dev/null @@ -1,7 +0,0 @@ -Header header -bool accelerometer -bool gyroscope -bool magnetometer -geometry_msgs/Vector3 raw_linear_acceleration -geometry_msgs/Vector3 raw_angular_velocity -geometry_msgs/Vector3 raw_magnetic_field diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/package.xml b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/package.xml index 346644a..5727245 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/package.xml +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/package.xml @@ -53,7 +53,9 @@ rospy std_msgs geometry_msgs - message_generation + pibot_msgs + + roscpp rospy @@ -61,7 +63,7 @@ roscpp rospy std_msgs - message_runtime + geometry_msgs diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/align.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/align.cpp index 72f46a5..941d989 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/align.cpp +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/align.cpp @@ -141,7 +141,7 @@ namespace uwb_slam{ return; } - void Align::imuCB(const upbot_location::RawImu& imu) + 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; diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/CMakeLists.txt b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/CMakeLists.txt index 13b4f1c..f39ac6e 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/CMakeLists.txt +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/CMakeLists.txt @@ -4,6 +4,7 @@ project(upbot_vision) find_package(catkin REQUIRED COMPONENTS roscpp std_msgs + pibot_msgs message_generation ) @@ -11,20 +12,16 @@ find_package(catkin REQUIRED COMPONENTS find_package(OpenCV REQUIRED) find_package(Threads REQUIRED) -add_message_files( - FILES - dis_info.msg - dis_info_array.msg -) -generate_messages( - DEPENDENCIES - std_msgs -) + +# generate_messages( +# DEPENDENCIES +# std_msgs +# ) catkin_package( - CATKIN_DEPENDS roscpp std_msgs message_runtime + CATKIN_DEPENDS roscpp std_msgs ) @@ -50,14 +47,14 @@ add_library( src/ranging.cc ) -add_dependencies(head ${${PROJECT_NAME}_EXPORTED_TARGETS} upbot_vision_generate_messages_cpp ${catkin_EXPORTED_TARGETS}) +add_dependencies(head ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(head ${catkin_LIBRARIES} ) add_executable(main src/main.cc) # add_executable(sub_dis src/sub_dis.cc) -add_dependencies(main ${${PROJECT_NAME}_EXPORTED_TARGETS} upbot_vision_generate_messages_cpp ${catkin_EXPORTED_TARGETS}) +add_dependencies(main ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) # add_dependencies(sub_dis ${${PROJECT_NAME}_EXPORTED_TARGETS} upbot_vision_generate_messages_cpp ${catkin_EXPORTED_TARGETS}) # target_link_libraries(sub_dis diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/msg/dis_info.msg b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/msg/dis_info.msg deleted file mode 100644 index fc9a9af..0000000 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/msg/dis_info.msg +++ /dev/null @@ -1,4 +0,0 @@ -float32 distance -float32 width -float32 height -float32 angle diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/msg/dis_info_array.msg b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/msg/dis_info_array.msg deleted file mode 100644 index fae3856..0000000 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/msg/dis_info_array.msg +++ /dev/null @@ -1 +0,0 @@ -upbot_vision/dis_info[] dis \ No newline at end of file diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/package.xml b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/package.xml index 036e992..f24d591 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/package.xml +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/package.xml @@ -52,14 +52,15 @@ ros_py roscpp std_msgs + message_generation + pibot_msgs ros_py roscpp std_msgs ros_py roscpp std_msgs - message_generation - message_runtime + diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/src/main.cc b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/src/main.cc index 95b37b8..7438b1a 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/src/main.cc +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/src/main.cc @@ -10,8 +10,8 @@ #include "opencv2/imgcodecs/imgcodecs.hpp" #include #include -#include "upbot_vision/dis_info.h" -#include "upbot_vision/dis_info_array.h" +#include "pibot_msgs/dis_info.h" +#include "pibot_msgs/dis_info_array.h" #define ANGLE_TO_RADIAN(angle) ((angle)*3141.59 / 180000) @@ -24,13 +24,13 @@ void *ranging(void *args) // ranging线程 ros::Publisher dis_pub_; ros::NodeHandle nh; // <>内为自定义ros消息类型 存储障碍物信息包括与障碍物的距离、障碍物宽度、高度和与障碍物之间的夹角 - dis_pub_ = nh.advertise("ceju_info",10); + dis_pub_ = nh.advertise("ceju_info",10); while (true) { /*进入测距并发布障碍物信息*/; std::vector result = r.get_range(); cv ::Mat info = result[2]; - upbot_vision::dis_info_array dis_array; + pibot_msgs::dis_info_array dis_array; if(!info.empty()) { for(int i=0;i(i,0)>0&&info.at(i,0)<200) { - upbot_vision::dis_info data; + pibot_msgs::dis_info data; data.distance = info.at(i,0); data.width = info.at(i,1); data.height = info.at(i,2);