From 097c7dfb23331de97f954c3981fd3befb0c42ea3 Mon Sep 17 00:00:00 2001 From: ray <2954701669@qq.com> Date: Tue, 12 Mar 2024 10:08:03 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BB=A3=E7=A0=81=E6=95=B4=E7=90=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../ros_ws/src/upbot_location/CMakeLists.txt | 9 +---- .../upbot_location/include/read_sensor_data.h | 34 ----------------- .../src/upbot_location/include/system.h | 32 ---------------- .../ros_ws/src/upbot_location/src/main.cpp | 25 ++++-------- .../ros_ws/src/upbot_location/src/mapping.cpp | 31 ++------------- .../upbot_location/src/read_sensor_data.cpp | 30 --------------- .../src/upbot_location/src/senddata.cpp | 19 ++++++++-- .../ros_ws/src/upbot_location/src/system.cpp | 15 -------- .../ros_ws/src/upbot_location/src/uwb.cpp | 38 +------------------ 9 files changed, 31 insertions(+), 202 deletions(-) delete mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/read_sensor_data.h delete mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/system.h delete mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/read_sensor_data.cpp delete mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/system.cpp 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 ce986b8..08a31d5 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 @@ -32,22 +32,15 @@ include_directories( ) 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 - -# ) + generate_messages(DEPENDENCIES std_msgs geometry_msgs) catkin_package(CATKIN_DEPENDS std_msgs geometry_msgs) diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/read_sensor_data.h b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/read_sensor_data.h deleted file mode 100644 index 61a7df3..0000000 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/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/upbot_location/include/system.h b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/system.h deleted file mode 100644 index 848f92b..0000000 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/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/upbot_location/src/main.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/main.cpp index 14f98fc..f854eee 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/main.cpp +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/main.cpp @@ -15,24 +15,15 @@ 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(); - + 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(); - 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_; + mp->uwb_ = uwb; + mp->align_ = align; + align->uwb_ = uwb; + align->lighthouse_ = lighthouse; // // control data fllow in system // std::thread rose_trd ([&system]() { diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/mapping.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/mapping.cpp index a6240b7..81cdc97 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/mapping.cpp +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/mapping.cpp @@ -76,25 +76,7 @@ namespace uwb_slam cv::imshow("Image",img); - // bool printFlag = 0; - // std::ofstream outfile("data.txt",std::ofstream::out); - // if(outfile.is_open()) { - // std::cout << "start saving data" << key << std::endl; - // printFlag = 1; - // } else { - // std::cout<<"file can not open"<feed_uwb_data(cv::Point2d(uwb_->x,uwb_->y)); - readPos = true; - std::cout << "non" << key << std::endl; - cv::destroyAllWindows(); - } - */ + while(1){ int key = cv::waitKey(0); if (key == 'q' ) { @@ -109,14 +91,9 @@ namespace uwb_slam if (key2 == 'q') { //TODO: save - std::string pngimage="/home/firefly/Project_Ros11/Project_Ros1/src/upbot_location/Map/output_image.png";//保存的图片文件路径 - cv::imwrite(pngimage,img); - readPos = false; - - // outfile.close(); - // printFlag = 0; - // std::cout<< "Data written to file." << std::endl; - + std::string pngimage = "/home/firefly/Project_Ros/src/ros_merge_test/Map/output_image.png";//保存的图片文件路径 + cv::imwrite(pngimage, img); + read_uwb_ = false; break; } diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/read_sensor_data.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/read_sensor_data.cpp deleted file mode 100644 index e74d0cd..0000000 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/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/upbot_location/src/senddata.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/senddata.cpp index c6681ae..20ad4f8 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/senddata.cpp +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/senddata.cpp @@ -4,25 +4,38 @@ namespace uwb_slam { void Senddata::Run(std::shared_ptruwb){ - + // 初始化了一个名为loop_rate的ros::Rate对象,频率设置为10赫兹 ros::Rate loop_rate(10); + // 初始化一个ROS发布者，用于发布nav_msgs::Odometry类型的消息 + // 主题被设置为"uwb_odom"，队列大小为50 position_pub_=nh_.advertise("uwb_odom",50); + // 初始化了一个ROS订阅者，用于订阅"odom"主题。它指定了当在该主题上接收到 + // 消息时，将调用Senddata类的odomCB回调函数。队列大小被设置为10 odom_sub_=nh_.subscribe("odom",10,&Senddata::odomCB,this); while(ros::ok()){ + // 按照10Hz频率发布uwb信息 publishOdometry(uwb); ros::spinOnce(); + // 用于控制循环速率 loop_rate.sleep(); } } void Senddata::odomCB(const nav_msgs::Odometry& odom){ + // 这个地方接收的是轮速里程计的信息 + // 包含位置和姿态 sub_odom_ = odom; return; } - void Senddata::publishOdometry(std::shared_ptruwb) - { +/**--------------------------------------------------------------------- +* Function : Senddata::publishOdometry +* Description : 发布UWB里程计数据，这里读取的数据到底是什么，依旧存在疑问 +* Date : 2023/12/13 zhanli@review +*---------------------------------------------------------------------**/ +void Senddata::publishOdometry(std::shared_ptr uwb) +{ std::mutex mMutexSend; ros::Time current_time = ros::Time::now(); diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/system.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/system.cpp deleted file mode 100644 index 44f5878..0000000 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/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/upbot_location/src/uwb.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/uwb.cpp index 2a17887..94a5cf1 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/uwb.cpp +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/uwb.cpp @@ -2,8 +2,7 @@ #include #include "Mat.h" - -#define CARHEIGHT 20 +#define PI acos(-1) namespace uwb_slam{ Mat A; @@ -126,10 +125,7 @@ namespace uwb_slam{ } - void fusion() - { - - } + @@ -138,34 +134,4 @@ namespace uwb_slam{ }; -// bool Uwb::checknewdata() -// { -// std::unique_lock lock(mMutexUwb); -// return !v_buffer_imu_odom_pose_data_.empty(); -// } - -// void Uwb::Run() { -// while(1) -// { -// if(checknewdata()) -// { -// { -// std::unique_lock lock(mMutexUwb); -// Imu_odom_pose_data imu_odom_pose_data = v_buffer_imu_odom_pose_data_.front(); -// v_buffer_imu_odom_pose_data_.pop(); -// } - - - -// } -// } - - - -// } - -// void Uwb::feed_imu_odom_pose_data(const Imu_odom_pose_data& imu_odom_pose_data){ -// std::unique_lock lock(mMutexUwb); -// v_buffer_imu_odom_pose_data_.push(imu_odom_pose_data); -// } -- 2.37.2.windows.2