forked from ray/RobotKernal-UESTC
更新文档
parent
9074776d2b
commit
69cea1b043
|
|
@ -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>
|
||||
```
|
||||
|
||||
|
|
@ -56,7 +56,7 @@ sudo apt-get install coinor-*
|
|||
|
||||
#### 3.1 导航的启动
|
||||
|
||||
`PIBot_ROS`的文件解压或重新命名可以按照个人习惯即可，这里以解压后命名为`pibot_ros`为例，介绍命令启动导航
|
||||
`PIBot_ROS`的文件解压或重新命名可以按照个人习惯即可，这里以解压后命名为`pibot_ros`为例，介绍命令启动导航。
|
||||
|
||||
```shell
|
||||
# 进入到pibot_ros的工作空间
|
||||
|
|
@ -64,6 +64,7 @@ cd ~/pibot_ros/ros_ws
|
|||
# 配置环境变量
|
||||
source ./devel/setup.bash
|
||||
# 启动导航文件
|
||||
# 格式 roslaunch package_name launch_file_name
|
||||
roslaunch pibot_navigation nav.launch
|
||||
```
|
||||
|
||||
|
|
|
|||
|
|
@ -22,28 +22,24 @@ find_package(Boost REQUIRED)
|
|||
# CATKIN_DEPENDS message_runtime std_msgs geometry_msgs
|
||||
# )
|
||||
|
||||
|
||||
include_directories(
|
||||
# include
|
||||
${OpenCV_INCLUDE_DIRS}
|
||||
${catkin_INCLUDE_DIRS}
|
||||
${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/read_sensor_data.cpp
|
||||
include/senddata.h src/senddata.cpp)
|
||||
|
||||
|
||||
add_message_files(
|
||||
DIRECTORY msg
|
||||
FILES
|
||||
RawImu.msg
|
||||
|
||||
RawImu.msg
|
||||
)
|
||||
generate_messages(DEPENDENCIES std_msgs geometry_msgs)
|
||||
|
||||
|
|
|
|||
|
|
@ -6,7 +6,6 @@
|
|||
#ifndef MAPPING_H
|
||||
#define MAPPING_H
|
||||
|
||||
|
||||
namespace uwb_slam{
|
||||
class Mapping
|
||||
{
|
||||
|
|
@ -26,9 +25,7 @@ namespace uwb_slam{
|
|||
bool read_uwb_ = false;
|
||||
cv::Mat img;
|
||||
cv::Point2d cur_point = {-1,-1};
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -7,48 +7,30 @@
|
|||
#include "type.h"
|
||||
#include <queue>
|
||||
#include <chrono>
|
||||
#ifndef UWB_H
|
||||
#define UWB_H
|
||||
#ifndef __UWB_H__
|
||||
#define __UWB_H__
|
||||
|
||||
|
||||
namespace uwb_slam{
|
||||
|
||||
class Uwb
|
||||
{
|
||||
public:
|
||||
Uwb();
|
||||
void Run();
|
||||
bool checknewdata();
|
||||
void feed_imu_odom_pose_data();
|
||||
void UartUSBRead();
|
||||
class Uwb
|
||||
{
|
||||
public:
|
||||
Uwb();
|
||||
void Run();
|
||||
bool checknewdata();
|
||||
void feed_imu_odom_pose_data();
|
||||
void UartUSBRead();
|
||||
|
||||
|
||||
|
||||
public:
|
||||
int pre_seq = -1;
|
||||
int cur_seq = -1;
|
||||
uint8_t tmpdata[13];
|
||||
float x, y, theta, distance;
|
||||
|
||||
// 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_;
|
||||
};
|
||||
|
||||
public:
|
||||
int pre_seq = -1;
|
||||
int cur_seq = -1;
|
||||
uint8_t tmpdata[13];
|
||||
float x, y, theta, distance;
|
||||
|
||||
Uwb_data uwb_data_;
|
||||
std::mutex mMutexUwb;
|
||||
};
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -14,86 +14,86 @@ namespace uwb_slam{
|
|||
ros::Time tmp1 = ros::Time::now();
|
||||
ros::Time tmp2 = ros::Time::now();
|
||||
|
||||
|
||||
// 这个地方分别订阅了原始IMU、轮式里程计、里程计(推测是UWB)
|
||||
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);
|
||||
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())
|
||||
{
|
||||
img1 = cv::Mat(200, 200, CV_8UC1, cv::Scalar(255,255,255));
|
||||
cv::imshow("Image1",img1);
|
||||
int key2 = cv::waitKey(0);
|
||||
// if(key2 =='w'){
|
||||
// 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;
|
||||
}
|
||||
img1 = cv::Mat(200, 200, CV_8UC1, cv::Scalar(255,255,255));
|
||||
cv::imshow("Image1", img1);
|
||||
int key2 = cv::waitKey(0);
|
||||
// if(key2 =='w'){
|
||||
// bool write_data_ = true;
|
||||
// }
|
||||
|
||||
}
|
||||
// int count=0;
|
||||
// while(write_data_){
|
||||
while(1){
|
||||
|
||||
outfile.close();
|
||||
std::cout<< "Data written to file." << std::endl;
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
outfile.close();
|
||||
std::cout<< "Data written to file." << std::endl;
|
||||
}
|
||||
else{
|
||||
std::cout<<"file can not open"<<std::endl;
|
||||
|
|
|
|||
|
|
@ -22,31 +22,14 @@ int main(int argc, char** argv)
|
|||
// Initialize the ROS 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::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>();
|
||||
|
||||
// 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;
|
||||
system->Mapping_->uwb_ = uwb;
|
||||
system->Uwb_ = uwb;
|
||||
system->Sender_ = sender;
|
||||
system->Align_ = align;
|
||||
|
||||
mp->uwb_ = system->Uwb_;
|
||||
align->uwb_ = system->Uwb_;
|
||||
mp->uwb_ = uwb;
|
||||
align->uwb_ = uwb;
|
||||
|
||||
|
||||
// control data fllow in system
|
||||
std::thread rose_trd([&system]() {
|
||||
system->Run();
|
||||
});
|
||||
// uwb serried read
|
||||
std::thread uwb_trd([&uwb]() {
|
||||
uwb->Run();
|
||||
|
|
@ -67,5 +50,4 @@ int main(int argc, char** argv)
|
|||
|
||||
// Start the ROS node's main loop
|
||||
ros::spin();
|
||||
//System->run()
|
||||
}
|
||||
|
|
|
|||
|
|
@ -40,10 +40,7 @@ namespace uwb_slam
|
|||
|
||||
void Mapping::Run()
|
||||
{
|
||||
|
||||
//int key = cv::waitKey(0);//等待用户按下按键
|
||||
//std::cout << key << std::endl;
|
||||
int realWidth = AREA_SIZE / PIXEL_SCALE;
|
||||
int realWidth = AREA_SIZE / PIXEL_SCALE;
|
||||
int realHeight = AREA_SIZE / PIXEL_SCALE;
|
||||
|
||||
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)
|
||||
img.at<unsigned char>(j,i)= 0;
|
||||
|
||||
|
||||
|
||||
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)
|
||||
{
|
||||
// 这个地方会持续阻塞
|
||||
|
|
@ -94,23 +79,11 @@ namespace uwb_slam
|
|||
|
||||
this->feed_uwb_data(cv::Point2d(uwb_->x ,uwb_->y));
|
||||
|
||||
|
||||
//uwb xieru
|
||||
//std::cout << "cur_SEQ: " <<uwb_->cur_seq << std::endl;
|
||||
|
||||
if(check_uwb_point())
|
||||
{
|
||||
//std::cout << " start process" << std::endl;
|
||||
if(check_uwb_point()){
|
||||
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
|
||||
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -1,10 +0,0 @@
|
|||
#include "../include/system.h"
|
||||
|
||||
namespace uwb_slam{
|
||||
|
||||
void System::Run()
|
||||
{
|
||||
while(1){
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -2,7 +2,9 @@
|
|||
* File Name : uwb.cpp
|
||||
* Current Version : V1.0
|
||||
* Date of Issued : 2023.12.13 zhanli@review
|
||||
* Comments : UWB数据驱动, 负责从串口读取USB并发布出去
|
||||
* Comments : UWB数据驱动, 负责从串口读取USB并发布出去，这个代码可能存在以下
|
||||
改进：1) 创建固定的串口读取对象serial_port 2) 修改代码为异步
|
||||
读取。3) 确认串口数据的长度
|
||||
********************************************************************************/
|
||||
#include "uwb.h"
|
||||
#include <cmath>
|
||||
|
|
@ -18,16 +20,22 @@ namespace uwb_slam{
|
|||
void Uwb::Run() {
|
||||
while(1){
|
||||
// 这个地方不控制速率?
|
||||
// UartUSBRead 这个地方本身就是同步读取串口，是阻塞的函数
|
||||
this->UartUSBRead();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : UartUSBRead
|
||||
* Description : 通过串口读取数据，目前这段代码存在部分问题：1) 每次都重复创建
|
||||
* 串口读取对象，可能会影响性能。
|
||||
* Date : 2023/12/13 zhanli@review
|
||||
*---------------------------------------------------------------------**/
|
||||
void Uwb::UartUSBRead()
|
||||
{
|
||||
try {
|
||||
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::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)); // 设置停止位
|
||||
|
||||
uint8_t tmpdata[12];
|
||||
|
||||
size_t bytesRead = boost::asio::read(serial, boost::asio::buffer(tmpdata, 12)); // 读取串口数据
|
||||
// std::cerr << "after read" << std::endl;
|
||||
// 读取串口数据
|
||||
size_t bytesRead = boost::asio::read(serial, boost::asio::buffer(tmpdata, 12));
|
||||
|
||||
// for (int i=0;i< bytesRead;i++)
|
||||
// {
|
||||
// std::cout << "Received data: " << std::hex<<static_cast<int>(tmpdata[i]) ;
|
||||
// }
|
||||
// UWB获取的数据是半径R和角度Theta
|
||||
memcpy(&this->distance, &tmpdata[3], sizeof(distance));
|
||||
memcpy(&this->theta, &tmpdata[7], sizeof(theta));
|
||||
/*this->x = cosf(theta/180*PI)*distance+1000;
|
||||
this->y = sinf(theta/180*PI)*distance+1000;
|
||||
this->theta = theta;*/
|
||||
this->uwb_data_.x_ = cosf(theta/180*PI)*distance+1000;
|
||||
this->uwb_data_.y_ = sinf(theta/180*PI)*distance+1000;
|
||||
|
||||
// 这个地方是为了把UWB的坐标移动到图像的中心位置 2023/12/13@李瑞瑞
|
||||
this->uwb_data_.x_ = cosf(theta / 180*PI)*distance + 1000;
|
||||
this->uwb_data_.y_ = sinf(theta / 180*PI)*distance + 1000;
|
||||
// 获取此时的系统时间戳
|
||||
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;
|
||||
|
||||
} catch (const std::exception& ex) {
|
||||
|
|
@ -69,10 +63,6 @@ namespace uwb_slam{
|
|||
<< ex.what() << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
void fusion()
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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(); // 使节点等待足够的时间以达到设定的频率
|
||||
}
|
||||
|
||||
```
|
||||
|
||||
|
|
@ -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`文件，描述了要启动的节点以及对应要传递的参数。
|
||||
Loading…
Reference in New Issue