jhy
/
RobotKernal-UESTC
forked from logzhan/RobotKernal-UESTC
1
0
Fork 0
Code Pull Requests Activity

Merge pull request '代码整理' (#11) from ray/RobotKernal-UESTC:main into main 

合并瑞瑞-代码整理
main
詹力 2024-03-12 10:49:44 +08:00
parent 8dcaf69f58 097c7dfb23
commit c3989a16b9
9 changed files with 31 additions and 202 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/CMakeLists.txt
View File

@ -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)

34
Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/read_sensor_data.h
View File

@ -1,34 +0,0 @@
#include <ros/ros.h>
#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 <boost/thread/mutex.hpp>
#include "type.h"
#include "uwb.h"
#ifndef READ_SENSOR_DATA_H
#define READ_SENSOR_DATA_H
namespace uwb_slam{
typedef boost::shared_ptr<nav_msgs::Odometry const> OdomConstPtr;
typedef boost::shared_ptr<sensor_msgs::Imu const> 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

32
Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/system.h
View File

@ -1,32 +0,0 @@
#include <thread>
#include <string>
#include "mapping.h"
#include "uwb.h"
#include "senddata.h"
// #include "align.h"
#include <iostream>
#ifndef SYSTEM_H
#define SYSTEM_H
namespace uwb_slam{
class System{
public:
System() {
}
void Run();
public:
std::shared_ptr<uwb_slam::Mapping>Mapping_;
std::shared_ptr<uwb_slam::Uwb>Uwb_;
std::shared_ptr<uwb_slam::Senddata>Sender_;
std::shared_ptr<uwb_slam::Align>Align_;
std::shared_ptr<uwb_slam::Lighthouse>Lighthouse_;
// Uwb* Uwb_ ;
// Senddata* Sender_;
// Mapping* Mapping_;
};
}
#endif

17
Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/main.cpp
View File

@ -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<uwb_slam::System> system = std::make_shared<uwb_slam::System>();
std::shared_ptr<uwb_slam::Mapping> mp = std::make_shared<uwb_slam::Mapping>();
std::shared_ptr<uwb_slam::Uwb> uwb = std::make_shared<uwb_slam::Uwb>();
std::shared_ptr<uwb_slam::Senddata> sender = std::make_shared<uwb_slam::Senddata>();
std::shared_ptr<uwb_slam::Align> align = std::make_shared<uwb_slam::Align>();
std::shared_ptr<uwb_slam::Lighthouse> lighthouse = std::make_shared<uwb_slam::Lighthouse>();
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]() {

29
Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/mapping.cpp
View File

@ -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"<<std::endl;
// }
/*
std::cout << "waiting" <<std::endl;
int key = cv::waitKey(0);
if (key == 'q' || key == 27) {
this->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;
}

30
Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/read_sensor_data.cpp
View File

@ -1,30 +0,0 @@
#include <read_sensor_data.h>
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<std_msgs::String>("imu/data_raw", 1000, this->imu_call_back);
//odom_sub_ =nh.subscribe("odom", 1000,odom_call_back);
ros::spin();
}
}

19
Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/senddata.cpp
View File

@ -4,25 +4,38 @@
namespace uwb_slam
{
void Senddata::Run(std::shared_ptr<uwb_slam::Uwb>uwb){
// 初始化了一个名为loop_rate的ros::Rate对象,频率设置为10赫兹
ros::Rate loop_rate(10);
// 初始化一个ROS发布者，用于发布nav_msgs::Odometry类型的消息
// 主题被设置为"uwb_odom"，队列大小为50
position_pub_=nh_.advertise<nav_msgs::Odometry>("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_ptr<uwb_slam::Uwb>uwb)
{
/**---------------------------------------------------------------------
* Function : Senddata::publishOdometry
* Description : 发布UWB里程计数据，这里读取的数据到底是什么，依旧存在疑问
* Date : 2023/12/13 zhanli@review
*---------------------------------------------------------------------**/
void Senddata::publishOdometry(std::shared_ptr<uwb_slam::Uwb> uwb)
{
std::mutex mMutexSend;
ros::Time current_time = ros::Time::now();

15
Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/system.cpp
View File

@ -1,15 +0,0 @@
#include "../include/system.h"
//#include ""
namespace uwb_slam{
void System::Run()
{
while(1){
}
}
}

36
Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/uwb.cpp
View File

@ -2,8 +2,7 @@
#include <cmath>
#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<std::mutex> lock(mMutexUwb);
// return !v_buffer_imu_odom_pose_data_.empty();
// }
// void Uwb::Run() {
// while(1)
// {
// if(checknewdata())
// {
// {
// std::unique_lock<std::mutex> 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<std::mutex> lock(mMutexUwb);
// v_buffer_imu_odom_pose_data_.push(imu_odom_pose_data);
// }