起因是参照原厂代码写了一个读取下位机的程式但是编译的时候报错.
只好自己參考網上已經相關資料重新寫過.主要先把框架搭出來, 就是cmakelist.txt跟建構函數 .. 因為ROS2一直在更新 , 在寫這段代碼主要參考舊的做法就是main 裡面 使用 rclcpp::init(argc, argv); 這種方式 . 後面會換成ROS2新的註冊式.
CMakeLists.txt
主要需要注意的地方:
1. find_package(serial REQUIRED) : 上一章編譯的serial驅動這時候用上了, 要加入這個包
2.這三件套就是編出執行檔且把相關 lib鏈接起來.
add_executable(serial_reader src/serial_reader.cpp)
target_link_libraries(serial_reader serial)
ament_target_dependencies(serial_reader rclcppstd_msgs serial)
其他地方應該就是千篇一律了.
代碼部分:分兩塊標頭檔以及程式本體
標頭檔：没啥好说的,就一堆參數宣告.
#include <memory>
#include <inttypes.h>
#include "rclcpp/rclcpp.hpp"
//#include "std_msgs/msg/string.hpp"
#include <iostream>
#include <string.h>
#include <string>
#include <iostream>
#include <math.h>
#include <stdlib.h>
#include <unistd.h>
#include <rcl/types.h>
#include <sys/stat.h>
#include <serial/serial.h>
#include <fcntl.h>
#include <stdbool.h>
using namespace std;
//Macro definition
//宏定义
#define SEND_DATA_CHECK 1 //Send data check flag bits //发送数据校验标志位
#define READ_DATA_CHECK 0 //Receive data to check flag bits //接收数据校验标志位
#define FRAME_HEADER 0X7B //Frame head //帧头
#define FRAME_TAIL 0X7D //Frame tail //帧尾
#define RECEIVE_DATA_SIZE 24 //The length of the data sent by the lower computer //下位机发送过来的数据的长度
#define SEND_DATA_SIZE 11 //The length of data sent by ROS to the lower machine //ROS向下位机发送的数据的长度
#define PI 3.1415926f //PI //圆周率
// 超声波测量距离相关变量
#define Distance_DATA_size 19
#define Distance_HEADER 0XFA //Frame_header //帧头
#define Distance_TAIL 0XFC //Frame_tail //帧尾
//自动回充相关
#define AutoCharge_HEADER 0X7C //Frame_header //自动回充数据帧头
#define AutoCharge_TAIL 0X7F //Frame_tail //自动回充数据帧尾
#define AutoCharge_DATA_SIZE 8 //下位机发送过来的自动回充数据的长度
//extern sensor_msgs::msg::Imu Mpu6050; //External variables, IMU topic data //外部变量，IMU话题数据
//Relative to the range set by the IMU gyroscope, the range is ±500°, corresponding data range is ±32768
//The gyroscope raw data is converted in radian (rad) units, 1/65.5/57.30=0.00026644
//与IMU陀螺仪设置的量程有关，量程±500°，对应数据范围±32768
//陀螺仪原始数据转换位弧度(rad)单位，1/65.5/57.30=0.00026644
#define GYROSCOPE_RATIO 0.00026644f
//Relates to the range set by the IMU accelerometer, range is ±2g, corresponding data range is ±32768
//Accelerometer original data conversion bit m/s^2 units, 32768/2g=32768/19.6=1671.84
//与IMU加速度计设置的量程有关，量程±2g，对应数据范围±32768
//加速度计原始数据转换位m/s^2单位，32768/2g=32768/19.6=1671.84
#define ACCEl_RATIO 1671.84f
//Covariance matrix for speedometer topic data for robt_pose_ekf feature pack
//协方差矩阵，用于里程计话题数据，用于robt_pose_ekf功能包
const double odom_pose_covariance[36] = {1e-3, 0, 0, 0, 0, 0,
0, 1e-3, 0, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e3 };
const double odom_pose_covariance2[36] = {1e-9, 0, 0, 0, 0, 0,
0, 1e-3, 1e-9, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e-9 };
const double odom_twist_covariance[36] = {1e-3, 0, 0, 0, 0, 0,
0, 1e-3, 0, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e3 };
const double odom_twist_covariance2[36] = {1e-9, 0, 0, 0, 0, 0,
0, 1e-3, 1e-9, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e-9} ;
//Data structure for speed and position
//速度、位置数据结构体
typedef struct __Vel_Pos_Data_
{
float X;
float Y;
float Z;
}Vel_Pos_Data;
//IMU data structure
//IMU数据结构体
typedef struct __MPU6050_DATA_
{
short accele_x_data;
short accele_y_data;
short accele_z_data;
short gyros_x_data;
short gyros_y_data;
short gyros_z_data;
}MPU6050_DATA;
//The structure of the ROS to send data to the down machine
//ROS向下位机发送数据的结构体
typedef struct _SEND_DATA_
{
uint8_t tx[SEND_DATA_SIZE];
float X_speed;
float Y_speed;
float Z_speed;
unsigned char Frame_Tail;
}SEND_DATA;
//The structure in which the lower computer sends data to the ROS
//下位机向ROS发送数据的结构体
typedef struct _RECEIVE_DATA_
{
uint8_t rx[RECEIVE_DATA_SIZE];
uint8_t Flag_Stop;
unsigned char Frame_Header;
float X_speed;
float Y_speed;
float Z_speed;
float Power_Voltage;
unsigned char Frame_Tail;
}RECEIVE_DATA;
//下位机发送的自动回充相关数据结构体
typedef struct _RECEIVE_AutoCharge_DATA_
{
uint8_t rx[AutoCharge_HEADER]; //8字节
unsigned char Frame_Header; //帧头
unsigned char Frame_Tail; //帧尾
}RECEIVE_AutoCharge_DATA;
//下位机发送的超声波相关数据结构体
//下位机向ROS发送的超声波数据结构体
typedef struct _DISTANCE_DATA_
{
uint8_t rx[Distance_DATA_size];
unsigned char Frame_Header;
unsigned char Frame_Tail;
}DISTANCE_DATA;
typedef struct _Distance_
{
float A;
float B;
float C;
float D;
float E;
float F;
float G;
float H;
}Supersonic_data;
