Double
/
RobotKernal-UESTC
forked from ray/RobotKernal-UESTC
1
0
Fork 0
Code Pull Requests Activity

更新文档

LRD_Develop
詹力 2023-12-13 18:00:21 +08:00
parent 9074776d2b
commit 69cea1b043
16 changed files with 230 additions and 307 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
Code/RK3588/PIBot_ROS.md Normal file
View File

@ -0,0 +1,59 @@
# `PIBot_ROS`说明
> 更新：`zhanli` 2023-12-13
### 一、`PIBot_ROS`简介
​ `PIBot_ROS`本质上一个封装功能非常完善的功能包，其内部添加了许多已经整合好了功能，更加便于使用。
- `ros_package` : 内置了许多算法的功能包的压缩包
- `pypibot` : `pibot`相关的python支持功能
- `pibot_upstart` : 未知待补充
- `ros_ws` : `ROS`的工作空间
##### 1.1 `ros_ws`的目录结构
​ 下图中`catkin_workspcae`对应即是`ros_ws`文件夹。其下面一般具有`src`、`build`、`devel`等功能。
<img src="http://logzhan.ticp.io:30000/logzhan/PictureHost/raw/branch/main/RobotKernal-UESTC/ROS_File_Structure.png" style="zoom: 50%;" />
​ 上述文件的结构和说明如下面的文档所示：
```shell
WorkSpace --- 自定义的工作空间
|--- build:编译空间，用于存放CMake和catkin的缓存信息、配置信息和其他中间文件。
|--- devel:开发空间，用于存放编译后生成的目标文件，包括头文件、动态&静态链接库、可执行文件等。
|--- src: 源码
|-- package：功能包(ROS基本单元)包含多个节点、库与配置文件，包名所有字母小写，只能由字母、数字与下划线组成
|-- CMakeLists.txt 配置编译规则，比如源文件、依赖项、目标文件
|-- package.xml 包信息，比如:包名、版本、作者、依赖项...(以前版本是 manifest.xml)
|-- scripts 存储python文件
|-- src 存储C++源文件
|-- include 头文件
|-- msg 消息通信格式文件
|-- srv 服务通信格式文件
|-- action 动作格式文件
|-- launch 可一次性运行多个节点
|-- config 配置信息
|-- CMakeLists.txt: 编译的基本配置
```
##### 1.2 `package.xml`
​ 如上图所示，ROS的基本功能都是通过包(`package`)组织的。该文件定义有关软件包的属性，例如软件包名称，版本号，作者，维护者以及对其他catkin软件包的依赖性。请注意，该概念类似于旧版` rosbuild` 构建系统中使用的`manifest.xml`文件。
​ `package.xml`的部分内容如下图所示, 推测ROS系统通过`package.xml`的内容识别对应包的功能命令。
```xml
<?xml version="1.0"?>
<!-- 格式: 以前是 1，推荐使用格式 2 -->
<package format="2">
<!-- 包名 -->
<name>demo01_hello_vscode</name>
<!-- 版本 -->
<version>0.0.0</version>
<!-- 描述信息 -->
...
</package>
```

3
Code/RK3588/PIBot_ROS/README.md
View File

@ -56,7 +56,7 @@ sudo apt-get install coinor-*
#### 3.1 导航的启动 #### 3.1 导航的启动
​ `PIBot_ROS`的文件解压或重新命名可以按照个人习惯即可，这里以解压后命名为`pibot_ros`为例，介绍命令启动导航 ​ `PIBot_ROS`的文件解压或重新命名可以按照个人习惯即可，这里以解压后命名为`pibot_ros`为例，介绍命令启动导航。
```shell ```shell
# 进入到pibot_ros的工作空间 # 进入到pibot_ros的工作空间
@ -64,6 +64,7 @@ cd ~/pibot_ros/ros_ws
# 配置环境变量 # 配置环境变量
source ./devel/setup.bash source ./devel/setup.bash
# 启动导航文件 # 启动导航文件
# 格式 roslaunch package_name launch_file_name
roslaunch pibot_navigation nav.launch roslaunch pibot_navigation nav.launch
``` ```

10
Code/RK3588/Robot_ROS_APP/src/ros_merge_test/CMakeLists.txt
View File

@ -22,28 +22,24 @@ find_package(Boost REQUIRED)
# CATKIN_DEPENDS message_runtime std_msgs geometry_msgs # CATKIN_DEPENDS message_runtime std_msgs geometry_msgs
# ) # )
include_directories( include_directories(
# include # include
${OpenCV_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}
include include
) )
add_library(${PROJECT_NAME} SHARED add_library(${PROJECT_NAME} SHARED
src/system.cpp
src/uwb.cpp src/uwb.cpp
src/mapping.cpp src/mapping.cpp
src/align.cpp src/align.cpp
# src/read_sensor_data.cpp
include/senddata.h src/senddata.cpp) include/senddata.h src/senddata.cpp)
add_message_files( add_message_files(
DIRECTORY msg DIRECTORY msg
FILES FILES
RawImu.msg RawImu.msg
) )
generate_messages(DEPENDENCIES std_msgs geometry_msgs) generate_messages(DEPENDENCIES std_msgs geometry_msgs)

0
Code/RK3588/Robot_ROS_APP/src/ros_merge_test/include/avoid.h
View File

3
Code/RK3588/Robot_ROS_APP/src/ros_merge_test/include/mapping.h
View File

@ -6,7 +6,6 @@
#ifndef MAPPING_H #ifndef MAPPING_H
#define MAPPING_H #define MAPPING_H
namespace uwb_slam{ namespace uwb_slam{
class Mapping class Mapping
{ {
@ -26,9 +25,7 @@ namespace uwb_slam{
bool read_uwb_ = false; bool read_uwb_ = false;
cv::Mat img; cv::Mat img;
cv::Point2d cur_point = {-1,-1}; cv::Point2d cur_point = {-1,-1};
}; };
} }
#endif #endif

34
Code/RK3588/Robot_ROS_APP/src/ros_merge_test/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 ReadSensorData
{
public:
ReadSensorData();
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

31
Code/RK3588/Robot_ROS_APP/src/ros_merge_test/include/system.h
View File

@ -1,31 +0,0 @@
#ifndef SYSTEM_H
#define SYSTEM_H
#include <thread>
#include <string>
#include "mapping.h"
#include "uwb.h"
#include "senddata.h"
#include "align.h"
#include <iostream>
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_;
// Uwb* Uwb_ ;
// Senddata* Sender_;
// Mapping* Mapping_;
};
}
#endif

56
Code/RK3588/Robot_ROS_APP/src/ros_merge_test/include/uwb.h
View File

@ -7,48 +7,30 @@
#include "type.h" #include "type.h"
#include <queue> #include <queue>
#include <chrono> #include <chrono>
#ifndef UWB_H #ifndef __UWB_H__
#define UWB_H #define __UWB_H__
namespace uwb_slam{ namespace uwb_slam{
class Uwb class Uwb
{ {
public: public:
Uwb(); Uwb();
void Run(); void Run();
bool checknewdata(); bool checknewdata();
void feed_imu_odom_pose_data(); void feed_imu_odom_pose_data();
void UartUSBRead(); void UartUSBRead();
public:
int pre_seq = -1;
public: int cur_seq = -1;
int pre_seq = -1; uint8_t tmpdata[13];
int cur_seq = -1; float x, y, theta, distance;
uint8_t tmpdata[13];
float x, y, theta, distance; Uwb_data uwb_data_;
std::mutex mMutexUwb;
// std::queue<Imu_odom_pose_data> v_buffer_imu_odom_pose_data_; };
Uwb_data uwb_data_;
// ros_merge_test::RawImu sub_imu_;
// std::queue<Imu_odom_pose_data > imu_odom_queue_;
// std::queue<Uwb_data> 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 #endif

146
Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/align.cpp
View File

@ -14,86 +14,86 @@ namespace uwb_slam{
ros::Time tmp1 = ros::Time::now(); ros::Time tmp1 = ros::Time::now();
ros::Time tmp2 = ros::Time::now(); ros::Time tmp2 = ros::Time::now();
// 这个地方分别订阅了原始IMU、轮式里程计、里程计(推测是UWB)
wheel_odom_sub_= nh_.subscribe("wheel_odom",10,&Align::wheel_odomCB,this); wheel_odom_sub_= nh_.subscribe("wheel_odom",10,&Align::wheel_odomCB,this);
imu_sub_= nh_.subscribe("raw_imu",10,&Align::imuCB,this); imu_sub_ = nh_.subscribe("raw_imu",10,&Align::imuCB,this);
odom_sub_= nh_.subscribe("odom",10,&Align::odomCB,this); odom_sub_ = nh_.subscribe("odom",10,&Align::odomCB,this);
std::ofstream outfile("data.txt",std::ofstream::out); std::ofstream outfile("data.txt",std::ofstream::out);
if(outfile.is_open()) if(outfile.is_open())
{ {
img1 = cv::Mat(200, 200, CV_8UC1, cv::Scalar(255,255,255)); img1 = cv::Mat(200, 200, CV_8UC1, cv::Scalar(255,255,255));
cv::imshow("Image1",img1); cv::imshow("Image1", img1);
int key2 = cv::waitKey(0); int key2 = cv::waitKey(0);
// if(key2 =='w'){ // if(key2 =='w'){
// bool write_data_ = true; // bool write_data_ = true;
// } // }
// int count=0;
// while(write_data_){
while(1){
int key3 = cv::waitKey(1);
if(key3 == 'w'){
break;
}
if(tmp!=imu_odom_.imu_data_.imu_t_){
// outfile <<"imu_odom_: "<< "imu_timestamp "<<"imu_linear_acc_x_y_z "<<"imu_angular_x_y_z "<<
// "odom_vxy "<<"odom_angle_v_ "<<"\n";
// if(tmp1!=uwb_->uwb_data_.uwb_t_&& tmp2!=odom_tmp_){
outfile << std::left << std::setw(12)<<"imu_odom_: "<< std::setw(10)<< imu_odom_.imu_data_.imu_t_.sec << '.' <<std::setw(11)<< imu_odom_.imu_data_.imu_t_.nsec << std::setw(11)
<<imu_odom_.imu_data_.a_[0] << std::setw(11)<<imu_odom_.imu_data_.a_[1] << std::setw(11)<<imu_odom_.imu_data_.a_[2] << std::setw(12)
<<imu_odom_.imu_data_.w_[0] << std::setw(12)<<imu_odom_.imu_data_.w_[1] << std::setw(12)<<imu_odom_.imu_data_.w_[2] << std::setw(11)
<<imu_odom_.vxy_ << std::setw(11)<<imu_odom_.angle_v_ << std::setw(7)
<<" pose: " << std::setw(1)<<(odom_tmp_-tmp2).sec<<'.'<<std::setw(12)<<(odom_tmp_-tmp2).nsec<<std::setw(11)
<<imu_odom_.pose_[0] << std::setw(12)<<imu_odom_.pose_[1] << std::setw(2)<<imu_odom_.pose_[2] << std::setw(12)
<<imu_odom_.quat_[0] << std::setw(12)<<imu_odom_.quat_[1] << std::setw(12)<<imu_odom_.quat_[2] << std::setw(12)<<imu_odom_.quat_[3] << std::setw(6)
<<" uwb: " << std::setw(1)<<(uwb_->uwb_data_.uwb_t_-tmp1).sec<<'.'<<std::setw(13)<<(uwb_->uwb_data_.uwb_t_-tmp1).nsec << std::setw(9)
<<uwb_->uwb_data_.x_ << std::setw(9)<<uwb_->uwb_data_.y_<<"\n";
tmp1 = uwb_->uwb_data_.uwb_t_;
tmp2 = odom_tmp_;
// }
// else if(tmp1!=uwb_->uwb_data_.uwb_t_){
// outfile <<"imu_odom_: "<< imu_odom_.imu_data_.imu_t_ <<"*"
// <<imu_odom_.imu_data_.a_[0]<<"*"<<imu_odom_.imu_data_.a_[1]<<"*"<<imu_odom_.imu_data_.a_[2]<<"*"
// <<imu_odom_.imu_data_.w_[0]<<"*"<<imu_odom_.imu_data_.w_[1]<<"*"<<imu_odom_.imu_data_.w_[2]<<"*"
// <<imu_odom_.vxy_<<"*"<<imu_odom_.angle_v_<<"*"
// <<"pose: "<<"****************************"
// <<"uwb: "<<uwb_->uwb_data_.uwb_t_<<"*"<<uwb_->uwb_data_.x_<<"*"<<uwb_->uwb_data_.y_<<"\n";
// tmp1 = uwb_->uwb_data_.uwb_t_;
// }
// else if(tmp2!=odom_tmp_){
// outfile <<"imu_odom_: "<< imu_odom_.imu_data_.imu_t_ <<"*"
// <<imu_odom_.imu_data_.a_[0]<<"*"<<imu_odom_.imu_data_.a_[1]<<"*"<<imu_odom_.imu_data_.a_[2]<<"*"
// <<imu_odom_.imu_data_.w_[0]<<"*"<<imu_odom_.imu_data_.w_[1]<<"*"<<imu_odom_.imu_data_.w_[2]<<"*"
// <<imu_odom_.vxy_<<"*"<<imu_odom_.angle_v_<<"*"
// <<"pose: "
// <<imu_odom_.pose_[0]<<"*"<<imu_odom_.pose_[1]<<"*"<<imu_odom_.pose_[2]<<"*"
// <<imu_odom_.quat_[0]<<"*"<<imu_odom_.quat_[1]<<"*"<<imu_odom_.quat_[2]<<"*"<<imu_odom_.quat_[3]<<"*"
// <<"uwb: "<<"****************************"<<"\n";
// tmp2 = odom_tmp_;
// }
// else {
// outfile <<"imu_odom_: "<< imu_odom_.imu_data_.imu_t_ <<"*"
// <<imu_odom_.imu_data_.a_[0]<<"*"<<imu_odom_.imu_data_.a_[1]<<"*"<<imu_odom_.imu_data_.a_[2]<<"*"
// <<imu_odom_.imu_data_.w_[0]<<"*"<<imu_odom_.imu_data_.w_[1]<<"*"<<imu_odom_.imu_data_.w_[2]<<"*"
// <<imu_odom_.vxy_<<"*"<<imu_odom_.angle_v_<<"*"
// <<"pose: "<<"****************************"
// <<"uwb: "<<"****************************"<<"\n";
// }
tmp = imu_odom_.imu_data_.imu_t_;
// tmp1 = uwb_->uwb_data_.uwb_t_;
// if(count>300)
// break;
}
} // int count=0;
// while(write_data_){
while(1){
outfile.close(); int key3 = cv::waitKey(1);
std::cout<< "Data written to file." << std::endl; if(key3 == 'w'){
break;
}
if(tmp!=imu_odom_.imu_data_.imu_t_)
{
// outfile <<"imu_odom_: "<< "imu_timestamp "<<"imu_linear_acc_x_y_z "<<"imu_angular_x_y_z "<<
// "odom_vxy "<<"odom_angle_v_ "<<"\n";
// if(tmp1!=uwb_->uwb_data_.uwb_t_&& tmp2!=odom_tmp_){
outfile << std::left << std::setw(12)<<"imu_odom_: "<< std::setw(10)<< imu_odom_.imu_data_.imu_t_.sec << '.' <<std::setw(11)<< imu_odom_.imu_data_.imu_t_.nsec << std::setw(11)
<<imu_odom_.imu_data_.a_[0] << std::setw(11)<<imu_odom_.imu_data_.a_[1] << std::setw(11)<<imu_odom_.imu_data_.a_[2] << std::setw(12)
<<imu_odom_.imu_data_.w_[0] << std::setw(12)<<imu_odom_.imu_data_.w_[1] << std::setw(12)<<imu_odom_.imu_data_.w_[2] << std::setw(11)
<<imu_odom_.vxy_ << std::setw(11)<<imu_odom_.angle_v_ << std::setw(7)
<<" pose: " << std::setw(1)<<(odom_tmp_-tmp2).sec<<'.'<<std::setw(12)<<(odom_tmp_-tmp2).nsec<<std::setw(11)
<<imu_odom_.pose_[0] << std::setw(12)<<imu_odom_.pose_[1] << std::setw(2)<<imu_odom_.pose_[2] << std::setw(12)
<<imu_odom_.quat_[0] << std::setw(12)<<imu_odom_.quat_[1] << std::setw(12)<<imu_odom_.quat_[2] << std::setw(12)<<imu_odom_.quat_[3] << std::setw(6)
<<" uwb: " << std::setw(1)<<(uwb_->uwb_data_.uwb_t_-tmp1).sec<<'.'<<std::setw(13)<<(uwb_->uwb_data_.uwb_t_-tmp1).nsec << std::setw(9)
<<uwb_->uwb_data_.x_ << std::setw(9)<<uwb_->uwb_data_.y_<<"\n";
tmp1 = uwb_->uwb_data_.uwb_t_;
tmp2 = odom_tmp_;
// }
// else if(tmp1!=uwb_->uwb_data_.uwb_t_){
// outfile <<"imu_odom_: "<< imu_odom_.imu_data_.imu_t_ <<"*"
// <<imu_odom_.imu_data_.a_[0]<<"*"<<imu_odom_.imu_data_.a_[1]<<"*"<<imu_odom_.imu_data_.a_[2]<<"*"
// <<imu_odom_.imu_data_.w_[0]<<"*"<<imu_odom_.imu_data_.w_[1]<<"*"<<imu_odom_.imu_data_.w_[2]<<"*"
// <<imu_odom_.vxy_<<"*"<<imu_odom_.angle_v_<<"*"
// <<"pose: "<<"****************************"
// <<"uwb: "<<uwb_->uwb_data_.uwb_t_<<"*"<<uwb_->uwb_data_.x_<<"*"<<uwb_->uwb_data_.y_<<"\n";
// tmp1 = uwb_->uwb_data_.uwb_t_;
// }
// else if(tmp2!=odom_tmp_){
// outfile <<"imu_odom_: "<< imu_odom_.imu_data_.imu_t_ <<"*"
// <<imu_odom_.imu_data_.a_[0]<<"*"<<imu_odom_.imu_data_.a_[1]<<"*"<<imu_odom_.imu_data_.a_[2]<<"*"
// <<imu_odom_.imu_data_.w_[0]<<"*"<<imu_odom_.imu_data_.w_[1]<<"*"<<imu_odom_.imu_data_.w_[2]<<"*"
// <<imu_odom_.vxy_<<"*"<<imu_odom_.angle_v_<<"*"
// <<"pose: "
// <<imu_odom_.pose_[0]<<"*"<<imu_odom_.pose_[1]<<"*"<<imu_odom_.pose_[2]<<"*"
// <<imu_odom_.quat_[0]<<"*"<<imu_odom_.quat_[1]<<"*"<<imu_odom_.quat_[2]<<"*"<<imu_odom_.quat_[3]<<"*"
// <<"uwb: "<<"****************************"<<"\n";
// tmp2 = odom_tmp_;
// }
// else {
// outfile <<"imu_odom_: "<< imu_odom_.imu_data_.imu_t_ <<"*"
// <<imu_odom_.imu_data_.a_[0]<<"*"<<imu_odom_.imu_data_.a_[1]<<"*"<<imu_odom_.imu_data_.a_[2]<<"*"
// <<imu_odom_.imu_data_.w_[0]<<"*"<<imu_odom_.imu_data_.w_[1]<<"*"<<imu_odom_.imu_data_.w_[2]<<"*"
// <<imu_odom_.vxy_<<"*"<<imu_odom_.angle_v_<<"*"
// <<"pose: "<<"****************************"
// <<"uwb: "<<"****************************"<<"\n";
// }
tmp = imu_odom_.imu_data_.imu_t_;
// tmp1 = uwb_->uwb_data_.uwb_t_;
// if(count>300)
// break;
}
}
outfile.close();
std::cout<< "Data written to file." << std::endl;
} }
else{ else{
std::cout<<"file can not open"<<std::endl; std::cout<<"file can not open"<<std::endl;

22
Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/main.cpp
View File

@ -22,31 +22,14 @@ int main(int argc, char** argv)
// Initialize the ROS node // Initialize the ROS node
ros::init(argc, argv, "locate_info_pub_node"); ros::init(argc, argv, "locate_info_pub_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::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::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::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::Align> align = std::make_shared<uwb_slam::Align>();
// uwb_slam::System* system = new uwb_slam::System();
// uwb_slam::Mapping* mp = new uwb_slam::Mapping();
// uwb_slam::Uwb* uwb = new uwb_slam::Uwb();
// uwb_slam::Senddata* sender = new uwb_slam::Senddata();
system->Mapping_ = mp; mp->uwb_ = uwb;
system->Mapping_->uwb_ = uwb; align->uwb_ = uwb;
system->Uwb_ = uwb;
system->Sender_ = sender;
system->Align_ = align;
mp->uwb_ = system->Uwb_;
align->uwb_ = system->Uwb_;
// control data fllow in system
std::thread rose_trd([&system]() {
system->Run();
});
// uwb serried read // uwb serried read
std::thread uwb_trd([&uwb]() { std::thread uwb_trd([&uwb]() {
uwb->Run(); uwb->Run();
@ -67,5 +50,4 @@ int main(int argc, char** argv)
// Start the ROS node's main loop // Start the ROS node's main loop
ros::spin(); ros::spin();
//System->run()
} }

31
Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/mapping.cpp
View File

@ -40,10 +40,7 @@ namespace uwb_slam
void Mapping::Run() void Mapping::Run()
{ {
int realWidth = AREA_SIZE / PIXEL_SCALE;
//int key = cv::waitKey(0);//等待用户按下按键
//std::cout << key << std::endl;
int realWidth = AREA_SIZE / PIXEL_SCALE;
int realHeight = AREA_SIZE / PIXEL_SCALE; int realHeight = AREA_SIZE / PIXEL_SCALE;
img = cv::Mat(realHeight, realWidth, CV_8UC1, cv::Scalar(255,255,255)); img = cv::Mat(realHeight, realWidth, CV_8UC1, cv::Scalar(255,255,255));
@ -56,20 +53,8 @@ namespace uwb_slam
for (int i=199+8;i<210;i+=1) for (int i=199+8;i<210;i+=1)
img.at<unsigned char>(j,i)= 0; img.at<unsigned char>(j,i)= 0;
cv::imshow("Image",img); cv::imshow("Image",img);
/*
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));
read_uwb_ = true;
std::cout << "non" << key << std::endl;
cv::destroyAllWindows();
}
*/
while(1) while(1)
{ {
// 这个地方会持续阻塞 // 这个地方会持续阻塞
@ -94,23 +79,11 @@ namespace uwb_slam
this->feed_uwb_data(cv::Point2d(uwb_->x ,uwb_->y)); this->feed_uwb_data(cv::Point2d(uwb_->x ,uwb_->y));
if(check_uwb_point()){
//uwb xieru
//std::cout << "cur_SEQ: " <<uwb_->cur_seq << std::endl;
if(check_uwb_point())
{
//std::cout << " start process" << std::endl;
process(); 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 } // namespace uwb_slam

33
Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/read_sensor_data.cpp
View File

@ -1,33 +0,0 @@
#include <read_sensor_data.h>
namespace uwb_slam {
//void Read_sensor_data::set_uwb(){}
void ReadSensorData::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 ReadSensorData::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 ReadSensorData::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事件循环
ros::spin();
}
}

10
Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/system.cpp
View File

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

48
Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/uwb.cpp
View File

@ -2,7 +2,9 @@
* File Name : uwb.cpp * File Name : uwb.cpp
* Current Version : V1.0 * Current Version : V1.0
* Date of Issued : 2023.12.13 zhanli@review * Date of Issued : 2023.12.13 zhanli@review
* Comments : UWB数据驱动, 负责从串口读取USB并发布出去 * Comments : UWB数据驱动, 负责从串口读取USB并发布出去，这个代码可能存在以下
改进：1) 创建固定的串口读取对象serial_port 2) 修改代码为异步
读取。3) 确认串口数据的长度
********************************************************************************/ ********************************************************************************/
#include "uwb.h" #include "uwb.h"
#include <cmath> #include <cmath>
@ -18,16 +20,22 @@ namespace uwb_slam{
void Uwb::Run() { void Uwb::Run() {
while(1){ while(1){
// 这个地方不控制速率? // 这个地方不控制速率?
// UartUSBRead 这个地方本身就是同步读取串口，是阻塞的函数
this->UartUSBRead(); this->UartUSBRead();
} }
} }
/**---------------------------------------------------------------------
* Function : UartUSBRead
* Description : 通过串口读取数据，目前这段代码存在部分问题：1) 每次都重复创建
* 串口读取对象，可能会影响性能。
* Date : 2023/12/13 zhanli@review
*---------------------------------------------------------------------**/
void Uwb::UartUSBRead() void Uwb::UartUSBRead()
{ {
try { try {
boost::asio::io_service io; boost::asio::io_service io;
boost::asio::serial_port serial(io, "/dev/ttyUSB0"); // 替换成你的串口设备路径 boost::asio::serial_port serial(io, "/dev/ttyUSB0"); // 替换成你的串口设备路径
serial.set_option(boost::asio::serial_port_base::baud_rate(115200)); // 设置波特率 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::character_size(8)); // 设置数据位
@ -35,33 +43,19 @@ namespace uwb_slam{
serial.set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one)); // 设置停止位 serial.set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one)); // 设置停止位
uint8_t tmpdata[12]; uint8_t tmpdata[12];
// 读取串口数据
size_t bytesRead = boost::asio::read(serial, boost::asio::buffer(tmpdata, 12)); // 读取串口数据 size_t bytesRead = boost::asio::read(serial, boost::asio::buffer(tmpdata, 12));
// std::cerr << "after read" << std::endl;
// for (int i=0;i< bytesRead;i++) // UWB获取的数据是半径R和角度Theta
// {
// std::cout << "Received data: " << std::hex<<static_cast<int>(tmpdata[i]) ;
// }
memcpy(&this->distance, &tmpdata[3], sizeof(distance)); memcpy(&this->distance, &tmpdata[3], sizeof(distance));
memcpy(&this->theta, &tmpdata[7], sizeof(theta)); memcpy(&this->theta, &tmpdata[7], sizeof(theta));
/*this->x = cosf(theta/180*PI)*distance+1000;
this->y = sinf(theta/180*PI)*distance+1000; // 这个地方是为了把UWB的坐标移动到图像的中心位置 2023/12/13@李瑞瑞
this->theta = theta;*/ this->uwb_data_.x_ = cosf(theta / 180*PI)*distance + 1000;
this->uwb_data_.x_ = cosf(theta/180*PI)*distance+1000; this->uwb_data_.y_ = sinf(theta / 180*PI)*distance + 1000;
this->uwb_data_.y_ = sinf(theta/180*PI)*distance+1000; // 获取此时的系统时间戳
this->uwb_data_.uwb_t_ = ros::Time::now(); this->uwb_data_.uwb_t_ = ros::Time::now();
// uwb_data_queue_.push(uwb_data_);
//std::cout << "uwb_data_: " << uwb_data_.uwb_t_<< std::endl;
// cur_seq = static_cast<int>(tmpdata[3]);
//std::cout << "****** cur _ sequence: " << cur_seq << "x: " << x << " y: " << y <<std::endl;
// if( cur_seq - pre_seq != 1){
// std::cout << "****** cur _ sequence: " << cur_seq << "pre _ sequence: " << pre_seq << " ******\n";
// }
// pre_seq = static_cast<int>(tmpdata[3]);
std::cout << "theta: " << theta << " distance: " << distance << std::endl; std::cout << "theta: " << theta << " distance: " << distance << std::endl;
} catch (const std::exception& ex) { } catch (const std::exception& ex) {
@ -69,10 +63,6 @@ namespace uwb_slam{
<< ex.what() << std::endl; << ex.what() << std::endl;
} }
} }
void fusion()
{
}
}; };

18
Docs/ROS基础之基本函数.md Normal file
View File

@ -0,0 +1,18 @@
# ROS基础笔记
##### 1.1 `ros::Rate`的用法
​ `ros::Rate` 是ROS中用于控制循环频率的工具。在你提到的代码中，`ros::Rate loop_rate(10);` 创建了一个 `ros::Rate` 对象，其目的是控制循环的频率。
```c++
ros::Rate loop_rate(10); // 设置事件循环的频率为10Hz
while (ros::ok()) {
// 处理其他任务，这个地方不能处理过于耗时的操作
ros::spinOnce(); // 处理一次事件循环
loop_rate.sleep(); // 使节点等待足够的时间以达到设定的频率
}
```

33
Docs/ROS基础之基本命令.md Normal file
View File

@ -0,0 +1,33 @@
# ROS基础之基础命令
> https://blog.csdn.net/sinat_16643223/article/details/113935412
## 一、常用命令
##### 1.1 `catkin_make`编译命令
```shell
# 进入工作空间
cd ros_ws
# 编译
catkin_make
```
​ 我们在使用ROS的时候经常会使用`catkin_make`命令，但我们时常会有疑惑`cakin_make`和`make`的区别。我们可以简单理解为`catkin_make`是`make`的扩展，它更加适合ROS相关的编译。
​ `Catkin` 是 ROS 中使用的构建系统，而 `CMake `是 `Catkin` 的底层构建系统。具体而言，`Catkin` 使用 `CMake` 作为其构建系统的基础，它对 `CMake` 进行了扩展，以适应 ROS 的特殊需求和机器人软件包的组织。
​ 以下是它们之间的关系：
1. **`CMake`：** `CMake` 是一个跨平台的开源构建系统，它使用一种基于文本的 DSL（领域特定语言）来描述软件项目的构建过程。`CMake` 生成适用于目标平台的构建系统文件，例如 `Makefile` 或` Visual Studio` 项目文件。
2. **`Catkin`：** Catkin 是 ROS 特有的构建系统，构建在 `CMake` 之上。`Catkin` 扩展了 `CMake`，以便更好地支持 `ROS` 软件包的组织和构建。`Catkin` 引入了一些 ROS 特有的概念，例如工作空间（workspace）、软件包（package）、消息和服务生成等。
​ 在一个 `ROS` 工作空间中，通常会包含一个或多个` Catkin` 软件包，每个软件包都包含一个 `CMakeLists.txt` 文件，其中定义了软件包的构建规则、依赖关系等。总的来说，**Catkin 通过扩展 `CMake`，提供了一个更高级别的构建系统，使得在 ROS 中更容易管理和构建机器人软件项目**。 CMake 提供了通用的构建系统功能，而 `Catkin` 在其基础上添加了 `ROS` 特有的工具和概念。
##### 1.2 `roslaunch`命令
```shell
roslaunch pibot_navigation nav.launch
```
​ 在命令`roslaunch pibot_navigation nav.launch`中，`roslaunch`本质上是一个可执行文件。`pibot_navigation `是工作空间`workspace`中软件包。`ros_launch`是通过软件包下面的`package.xml`识别出功能包。`nav.launch`是`pibot_navigation`下面`launch`文件夹下的一个`nav.launch`文件，其本质是个一个`xml`文件，描述了要启动的节点以及对应要传递的参数。