一、USB口满足可同时读写，符号标识
O_RDWR ： 可读可写
O_NDELAY ：这个程序不关心DCD信号线所处的状态,端口的另一端是否激活或者停止。如果用户不指定了这个标志，则进程将会一直处在睡眠状态，直到DCD信号线被激活。
O_NOCTTY :表示打开的是一个终端设备，程序不会成为该端口的控制终端。如果不使用此标志，任务一个输入(eg:键盘中止信号等)都将影响进程。
O_NONBLOCK :阻塞与非阻塞方式标识符
阻塞方式-read- 实现：
在阻塞型驱动程序中，read 实现方式如下：
如果进程调用 read ，但设备 没有数据 或 数据不足，进程阻塞.
当新数据到达后，唤醒被阻塞进程.
阻塞方式-write- 实现：
在阻塞型驱动程序中，write 实现方式如下：
如果进程调用了 write ，但设备 没有足够的空间供其写入数据，进程阻塞.
当设备中的数据被读走后，缓冲区中空出部分空间，则唤醒进程.
非阻塞方式的读写操作：
阻塞方式是文件读写操作的 默认方式，但是应用程序员可通过使用O_NONBLOCK 标志来人为的设置读写操作为 非阻塞方式 .( 该标志定义在 < linux/fcntl.h > 中，在打开文件时指定 ) .
如果 设置了 O_NONBLOCK 标志，read 和 write 的行为是不同的 ，如果进程没有数据就绪时调用了 read ，或者在缓冲区没有空间时调用了 write ，系统只是 简单的返回 -EAGAIN，而 不会阻塞进程.
有两种方法：
１.打开时，通过打开参数控制.
open(COM_DEV_NAME, O_RDWR | O_NONBLOCK);//非阻塞
open(COM_DEV_NAME, O_RDWR); //阻塞
２.打开以后可以通过fcntl（）函数进行修改.
fcntl(socket_descriptor, F_SETFL, flags | O_NONBLOCK)； //设为非阻塞
fcntl(sock_descriptor, F_SETFL, 0)； //设为阻塞
二、ROS serial::serial class 非阻塞方式
serial: serial::Serial Class Reference
整体分为三个线程，读写和处理（另一个线程为pps时间同步线程，可忽略）：
#include "serial_port.h"
extern "C"{
#include "ppstest.h"
}
using namespace std;
int main(int argc, char** argv) {
ros::init(argc, argv, "serial_node");
ros::NodeHandle node;
ros::NodeHandle private_nh("~");
std::string serial_Name;
bool usb_log;
ros::param::get("~usb_name",serial_Name);
ros::param::get("~usb_log",usb_log);
SerialPub Ser(node, private_nh,serial_Name,usb_log);
std::thread t1(&Serial_Receive,&Ser);
std::thread t2(&PPS_Receive,&Ser);
std::thread t3(&Serial_Send,&Ser);
t1.detach();
t2.detach();
t3.detach();
Ser.Ser_pub();
}读写线程实现方式：
#include "serial_port.h"
extern "C"{
#include "ppstest.h"
}
void SerialPub::initialize() {
pps_bit=false;
handshake_bit=false;
revice_bit=false;
timestamp=0;
pre_timestamp=0;
time_dt=0;
speedv_cnt=0;
anglev_cnt=0;
Pre_right_enc=0;
Pre_left_enc=0;
odom_X=0;
odom_Y=0;
frame_buff[FRAME_LEN] = {0};
try {
//设置串口属性，并打开串口
Date_Ser.setPort(ser_port);
Date_Ser.setBaudrate(ser_baud);
//serial::Timeout to = serial::Timeout::simpleTimeout(3);//USB口改为1
serial::Timeout to = serial::Timeout::simpleTimeout(8);//串口改为50
Date_Ser.setTimeout(to);
Date_Ser.open();
Date_Ser.flush();
}
catch (serial::IOException& e) {
ROS_ERROR_STREAM("Serial port opening exception");
}
//检测串口是否已经打开，并给出提示信息
if(Date_Ser.isOpen()) {
ROS_INFO_STREAM("Serial port opened successfully");
}
else{
ROS_INFO_STREAM("Unable to open serial port");
}
}
SerialPub::SerialPub(ros::NodeHandle node, ros::NodeHandle private_nh,std::string serial_name,bool usb_log_) {
//private_nh.param("ser_port", ser_port, std::string("/dev/serial_px30"));
std::cout<<"**************************"<<serial_name<<std::endl;
test_cs=usb_log_;
my_time=new TIME[1];
//private_nh.param("ser_port", ser_port, std::string("/dev/ttyACM0"));
private_nh.param("ser_port", ser_port, serial_name);
private_nh.param("ser_baud", ser_baud, 115200);
ros::Rate loop_rate(200); //设置发送数据的频率为100Hz
initialize();
CldPotAi_Sub = node.subscribe("/cloud_point_ai",5, &SerialPub::PCA_Callback,this);
Location_Sub = node.subscribe("/Location_pose",5, &SerialPub::Loc_Callback,this);
pub_gps_fix = node.advertise<sensor_msgs::NavSatFix>("GPS_fix", 5);
pub_odom = node.advertise<nav_msgs::Odometry>("Ser_odom", 5);
pub_imu = node.advertise<sensor_msgs::Imu>("Ser_imu", 5);
}
void Serial_Send(SerialPub *Ser_)
{
std::vector<uint8_t> vec;
std::cout<<"ai serial send"<<std::endl;
uint8_t send_date_Head[] = {
0xA4,0x59,0x43,//begin
0x04,
0x01,0x01,
0x00,0x00,0x00,0x00,//length 6-9
0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,//time 10-17
0x00,0x00,0x00,0x00,//state 18-21
0x00,0x00,0x00,0x00,//error 22-25
0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,//date 26-37 location
//34 obs-length uint16_t
0x0D,0x0A};//38-39 end
uint32_t send_length=0x01;
uint32_t out_status=0;
uint64_t out_time=0;
uint32_t out_error=0;
uint8_t aaaaaa=0x00;
while(ros::ok()){
vec.clear();
std::this_thread::sleep_for(std::chrono::milliseconds(20));
if(Ser_->handshake_bit)
{
for(int i=0;i<40;i++){
vec.push_back(send_date_Head[i]);
}//head
out_time=std::chrono::duration_cast<std::chrono::microseconds>
(std::chrono::system_clock::now().time_since_epoch()).count();
//https://blog.csdn.net/ktigerhero3/article/details/108017281;
///save number
if(Ser_->AiCloud_bit&&Ser_->Location_bit)
{
out_status=0x01;
out_error=0x01;
send_length=40+Ser_->vec_ai.size();
for(int i_loc=0;i_loc<12;i_loc++)//gai i=0
{
vec[26+i_loc]=Ser_->vec_loc[i_loc];
}
vec.insert(vec.begin()+37,Ser_->vec_ai.begin(),Ser_->vec_ai.end());
Ser_->AiCloud_bit=false;
Ser_->Location_bit=false;
}
else if(Ser_->Location_bit){ //Loc short package vec_loc
out_status=0x02;
out_error=0x02;
send_length=40;
for(int i_loc=0;i_loc<12;i_loc++)
{
vec[26+i_loc]=Ser_->vec_loc[i_loc];
}
Ser_->Location_bit=false;
}
else if (Ser_->AiCloud_bit)
{
out_status=0x04;
out_error=0x04;
send_length=40;
vec.insert(vec.begin()+37,Ser_->vec_ai.begin(),Ser_->vec_ai.end());
Ser_->AiCloud_bit=false;
}
else
{ // AI + Loc big package vec_loc vec_ai
out_status=0x08;
out_error=0x08;
send_length=40;
}
vec[6]=(send_length>>24)&0xFF;
vec[7]=(send_length>>16)&0xFF;
vec[8]=(send_length>>8)&0xFF;
vec[9]=send_length;
vec[10]=out_time;
vec[11]=(out_time>>8)&0xFF;
vec[12]=(out_time>>16)&0xFF;
vec[13]=(out_time>>24)&0xFF;
vec[14]=(out_time>>32)&0xFF;
vec[15]=(out_time>>40)&0xFF;
vec[16]=(out_time>>48)&0xFF;
vec[17]=(out_time>>54)&0xFF;
vec[18]=out_status;
vec[19]=out_status<<8;
vec[20]=out_status<<16;
vec[21]=out_status<<24;
vec[22]=out_error;
vec[23]=out_error<<8;
vec[24]=out_error<<16;
vec[25]=out_error<<24;
if(vec[26]==0x00)
continue;
if(Ser_->Date_Ser.write(vec)){
Ser_->test_cs=false;
if(Ser_->test_cs)
{
//ROS_INFO_STREAM("serial send success");
std::cout<<"length****************"<<vec.size()<<std::endl;
for(int i=0;i<45; i++)
printf("%02x ", vec[i]);
// printf("%02x ", vec[vec.size()-2]);
// printf("%02x ", vec[vec.size()-1]);
//printf("%02x ", vec[26]);
// printf("\n");
}
}
else{
ROS_INFO_STREAM("serial send fail");
}
Ser_->Date_Ser.flushOutput();
}
}
}
void SerialPub::Loc_Callback(const std_msgs::UInt8MultiArray::ConstPtr &msg){
vec_loc.clear();
Location_bit=true;
for(int i = 0; i < msg->data.size();i++){
vec_loc.push_back(msg->data[i]);
} //for(int i = 0; i < vec.size();i++){printf("%02x ",vec[i]);}printf("\n");
}
void SerialPub::PCA_Callback(const std_msgs::UInt8MultiArray::ConstPtr &msg){
vec_ai.clear();
AiCloud_bit=true;
for(int i = 0; i < msg->data.size();i++){
vec_ai.push_back(msg->data[i]);
} //for(int i = 0; i < vec.size();i++){printf("%02x ",vec[i]);}printf("\n");
}
SerialPub::~SerialPub() {
delete my_time;
if(Date_Ser.isOpen())
Date_Ser.close();
}
void SerialPub::listen_ser()
{
if(test_cs){std::cout<<"run"<<std::endl;}
if (Date_Ser.waitReadable()) { //waitReadable会阻塞串口，直到有数据过来
memset(frame_buff,0,FRAME_LEN);
if(test_cs)
ROS_INFO_STREAM("serial receive");
n_size = Date_Ser.read(frame_buff, FRAME_LEN+80);
if(test_cs)
{
std::cout<<"read_num="<<n_size<<std::endl;
for(int i=0;i<n_size; i++)
printf("%02x ", frame_buff[i]);
printf("\n");
}
revice_bit=true;
}
}
void Serial_Receive(SerialPub *Ser_)
{
while(ros::ok()){
Ser_->listen_ser();
//std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
}
void PPS_Receive(SerialPub *Ser_)
{
std::cout<<"run1"<<std::endl;
pps_handle_t handle[4];
int avail_mode[4];
int ret;
std::string pps_str="/dev/pps0";
char dev_pps[20];
pps_str.copy(dev_pps, 8, 0);
*(dev_pps+9)='\0';
strcpy((dev_pps), pps_str.c_str());
std::cout<<"pps"<<dev_pps<<std::endl;
ret = find_source(dev_pps, &handle[0], &avail_mode[0]);//check pps source is opened, set /dev/pps0
if (ret < 0)
exit(EXIT_FAILURE);
printf("ok, found %d source(s), now start fetching data...\n",0);
while(ros::ok()){
ret = fetch_source(0, &handle[0], &avail_mode[0]);//support check a lot pps source block
if (ret < 0 && errno != ETIMEDOUT)
exit(EXIT_FAILURE);
if(ret==0)
{
Ser_->pps_bit=true;
}
}
time_pps_destroy(handle[0]); /**/
}
void SerialPub::Ser_pub(){
char *ret1;
char *ret2;
while (ros::ok())
{
if (revice_bit) {
revice_bit=false;
uint32_t yz_len=frame_buff[9]+(frame_buff[8]<<8)+(frame_buff[7]<<16)+(frame_buff[6]<<24);
if((yz_len==1)&&(handshake_bit==false)){
std::vector<uint8_t> vec_send;
for(int i=0;i<sizeof(HandShake);i++){
vec_send.push_back(HandShake[i]);
printf("%02x ", HandShake[i]);
}
printf("test handshake\n");
Date_Ser.write(vec_send);
}
else if (n_size == yz_len+12) {//验证长度
handshake_bit=true;
std::vector<uint8_t> vec_data_rcv(frame_buff, frame_buff + FRAME_LEN);
ret1 = strstr((char*)frame_buff,(char*)MSG_H);//验证头 遇到00会停止检测，只能验证头
if(ret1!=NULL)
{
//ret2 = strstr((char*)frame_buff, (char*)MSG_E);
if(frame_buff[yz_len+12-1]==0x0A&&frame_buff[yz_len+12-2]==0x0D)//验证尾
{
// qxwz_rtcm_sendGGAWithGGAString(sendbuf);
std::vector<uint8_t>::iterator match_TIME_H = std::search(vec_data_rcv.begin(), vec_data_rcv.end(),MSG_H_TIME, MSG_H_TIME + 3);
if(match_TIME_H!=vec_data_rcv.end()){
time_del(match_TIME_H);
}
else{
ROS_INFO_STREAM("find time false");
}
std::vector<uint8_t>::iterator match_GPS_H = std::search(vec_data_rcv.begin(), vec_data_rcv.end(),MSG_H_GPS, MSG_H_GPS + 3);
if(match_GPS_H!=vec_data_rcv.end()){
gps_del(match_GPS_H);
}
else{
ROS_INFO_STREAM("find rtk false");
}
std::vector<uint8_t>::iterator match_IMU_H = std::search(vec_data_rcv.begin(), vec_data_rcv.end(),MSG_H_IMU, MSG_H_IMU + 3);
if(match_IMU_H!=vec_data_rcv.end()){
imu_del(match_IMU_H);
}
else{
ROS_INFO_STREAM("find imu false");
}
std::vector<uint8_t>::iterator match_ODOM_H = std::search(vec_data_rcv.begin(), vec_data_rcv.end(),MSG_H_ODOM, MSG_H_ODOM + 3);
if((match_ODOM_H!=vec_data_rcv.end())){
odom_del(match_ODOM_H);
}
else{
ROS_INFO_STREAM("find odom false");
}
}
else{
ROS_INFO_STREAM("end false");
}
}
else{
ROS_INFO_STREAM("head false");
}
vec_data_rcv.clear();
}
else{
ROS_INFO_STREAM("length false");
}
}
else {
//std::cout<<"????"<<std::endl;
//Date_Ser.flush();
//ROS_INFO_STREAM("no data");
}
ros::spinOnce();
}
if(Date_Ser.isOpen())
Date_Ser.close();
}
bool SerialPub::time_del(std::vector<uint8_t>::iterator match_TIME_){
const TIME_frame_t * Time_frame_p=(const TIME_frame_t *) &*(match_TIME_+3);
if(test_cs)
printf("%d/%d/%d\n", Time_frame_p->ucYear,Time_frame_p->ucMonth,Time_frame_p->ucDay);
timestamp= Time_frame_p->MsOneDay;
time_dt=pre_timestamp-timestamp;
pre_timestamp=timestamp;
if(pps_bit){
my_time->year=Time_frame_p->ucYear;
my_time->month=Time_frame_p->ucMonth;
my_time->day=Time_frame_p->ucDay;
if(my_time->year<2000){
my_time->year=2021;
my_time->month=0x0c;
my_time->day=0x08;
}
my_time->hour=Time_frame_p->MsOneDay/1000/60/60;
my_time->minute=Time_frame_p->MsOneDay/1000/60;
my_time->second=(Time_frame_p->MsOneDay+500)/1000;
if(!systime.setSystemTime(my_time))
std::cout<<"settime false"<<std::endl;
pps_bit=false;
}
}
bool SerialPub::gps_del(std::vector<uint8_t>::iterator match_GPS_){
const GPS_frame_t * GPS_frame_p=(const GPS_frame_t *) &*(match_GPS_+3);
const GPS_frame_t1 * GPS_frame_p1=(const GPS_frame_t1 *) &*(match_GPS_+24);
float latstd = GPS_frame_p1->lat_std;
float lonstd = GPS_frame_p1->lon_std;
int levela = (int)GPS_frame_p->level;
//std::cout<<"level="<<levela<<std::endl;
//std::cout<<"latstd="<<latstd<<std::endl;
//std::cout<<"lon="<<lonstd<<std::endl;
float latvar = latstd*latstd;
float lonvar = lonstd*lonstd;
fix.header.stamp.fromNSec(timestamp);
fix.status.status=levela;
fix.altitude=GPS_frame_p->hgt;
fix.latitude=GPS_frame_p->lat;
fix.longitude=GPS_frame_p->lon;
fix.position_covariance={ lonvar, 0, 0,
0, latvar, 0,
0, 0, 0};
fix.header.frame_id = "GPS_fix";
pub_gps_fix.publish(fix);
return true;
}
bool SerialPub::imu_del(std::vector<uint8_t>::iterator match_IMU_){
const IMU_frame_t * IMU_frame_p=(const IMU_frame_t *) &*(match_IMU_+3);
imu_data.header.stamp.fromNSec(timestamp);
imu_data.header.frame_id = "imu_link";
imu_data.angular_velocity.x = IMU_frame_p->gyro_x;
imu_data.angular_velocity.y = IMU_frame_p->gyro_y;
imu_data.angular_velocity.z = IMU_frame_p->gyro_z;
imu_data.linear_acceleration.x = IMU_frame_p->acc_x;
imu_data.linear_acceleration.y = IMU_frame_p->acc_y;
imu_data.linear_acceleration.z = IMU_frame_p->acc_z;
pub_imu.publish(imu_data);
}
bool SerialPub::odom_del(std::vector<uint8_t>::iterator match_ODOM_){
const ODOM_frame_t * ODOM_frame_p=(const ODOM_frame_t *) &*(match_ODOM_+3);
uint32_t right_enc=ODOM_frame_p->right_encode;
uint32_t left_enc=ODOM_frame_p->left_encode;
//printf("right_enc=%d", right_enc);
//printf("left_enc=%d", left_enc);
//printf("\n");
int32_t R_encodeCdif=right_enc-Pre_right_enc;
int32_t L_encodeCdif=left_enc-Pre_left_enc;
//当编码器启动时，发生跳变时，值为0；3000rpm，线数10000 0.1s=50000线
if(R_encodeCdif>50000&&R_encodeCdif<-50000&&L_encodeCdif>50000&&L_encodeCdif<-50000)
{
R_encodeCdif=0;
L_encodeCdif=0;
}
float distance_sum = 0.5f*(R_encodeCdif+L_encodeCdif);//短时间内，前进距离两侧速度和
float distance_diff = R_encodeCdif-L_encodeCdif;//短时间内，转弯角度为两侧速度差
float Yaw_Angle_Diff = distance_diff * const_angle;// 采样期间走的角度
float Yaw_Angle=Yaw_Angle+Yaw_Angle_Diff;//计算里程计方向角
float Yaw_Angle_1 = Yaw_Angle + 0.5f * Yaw_Angle_Diff;//里程计方向角数据位数变化，用于三角函数计算
odom_X=odom_X+distance_sum*cos(Yaw_Angle_1)*const_frame; //距离*Yaw（偏角） 解析X坐标
odom_Y=odom_Y+distance_sum*sin(Yaw_Angle_1)*const_frame;
// 当前瞬时线速度 w=th/t
float SpeedV_Ang=0,SpeedV_V=0;
if(time_dt!=0)
{
SpeedV_Ang=Yaw_Angle_Diff/time_dt; //角速度
SpeedV_V=(distance_sum*const_frame)/time_dt; //速度=位移/时间； 时间=定时间*Timer
}
if(speedv_cnt<3)
{
speedvsave[speedv_cnt]=SpeedV_V;
speedv_cnt++;
}else
{
speedv_cnt=0;
speedvsave[speedv_cnt]=SpeedV_V;
speedv_cnt++;
}
float Speedv_send=0;
for(int speedvplus=0;speedvplus<3;speedvplus++)
{
Speedv_send=Speedv_send+speedvsave[speedvplus];
}
Speedv_send=Speedv_send/3;
if(anglev_cnt<3)
{
anglevsave[anglev_cnt]=SpeedV_Ang;
anglev_cnt++;
}else
{
anglev_cnt=0;
anglevsave[anglev_cnt]=SpeedV_Ang;
anglev_cnt++;
}
float Anglev_send=0;
for(int anglevplus=0;anglevplus<3;anglevplus++)
{
Anglev_send=Anglev_send+anglevsave[anglevplus];
}
Anglev_send=Anglev_send/3;
if(Yaw_Angle > pi)
{
Yaw_Angle -= pi_2_1;//减2pi
}
else
{
if(Yaw_Angle < -pi)
{
Yaw_Angle+=pi_2_1;//加2pi
}
}
Pre_right_enc=right_enc;
Pre_left_enc=left_enc;
ser_odom.header.stamp.fromNSec(timestamp);
ser_odom.header.frame_id = "odom";
//set the position
ser_odom.pose.pose.position.x = odom_X;
ser_odom.pose.pose.position.y = odom_Y;
ser_odom.pose.pose.position.z = 0.0;
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(Yaw_Angle);//将偏航角转换成四元数
ser_odom.pose.pose.orientation = odom_quat;
//set the velocity
ser_odom.child_frame_id = "base_link";
ser_odom.twist.twist.linear.x = Speedv_send*cos(Yaw_Angle);//线速度
ser_odom.twist.twist.linear.y = Speedv_send*sin(Yaw_Angle);//线速度
ser_odom.twist.twist.angular.z = Anglev_send;//角速度
//载入covariance矩阵 https://blog.csdn.net/ethan_guo/article/details/79635575
if ((Speedv_send==0)&&(Anglev_send==0))//机器人静止和动起来
{
ser_odom.pose.covariance = ODOM_POSE_COVARIANCE2;
ser_odom.twist.covariance = ODOM_TWIST_COVARIANCE2;
}
else
{
ser_odom.pose.covariance = ODOM_POSE_COVARIANCE;
ser_odom.twist.covariance = ODOM_TWIST_COVARIANCE;
}
//publish the message
pub_odom.publish(ser_odom);
}
三、阻塞方式实现
串口驱动程序
#include "serial_drive.h"
int open_serial_port(const char * device, uint32_t baud_rate)
{
int fd = open(device, O_RDWR | O_NOCTTY);
printf("**********************%d\n",fd);
if (fd == -1)
{
perror(device);
return -1;
}
// Flush away any bytes previously read or written.
int result = tcflush(fd, TCIOFLUSH);
if (result)
{
perror("tcflush failed"); // just a warning, not a fatal error
}
// Get the current configuration of the serial port.
struct termios options;
result = tcgetattr(fd, &options);
if (result)
{
perror("tcgetattr failed");
close(fd);
return -1;
}
// Turn off any options that might interfere with our ability to send and
// receive raw binary bytes.
options.c_iflag &= ~(INLCR | IGNCR | ICRNL | IXON | IXOFF);
options.c_oflag &= ~(ONLCR | OCRNL);
options.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
// Set up timeouts: Calls to read() will return as soon as there is
// at least one byte available or when 100 ms has passed.
options.c_cc[VTIME] = 0; //VTIME与VMIN配合使用，是指限定的传输或等待的最长时间 单位：0.1S
options.c_cc[VMIN] = 1;//设置非规范模式下的超时时长和最小字符数：阻塞模式起作用
// This code only supports certain standard baud rates. Supporting
// non-standard baud rates should be possible but takes more work.
switch (baud_rate)
{
case 4800: cfsetospeed(&options, B4800); break;
case 9600: cfsetospeed(&options, B9600); break;
case 19200: cfsetospeed(&options, B19200); break;
case 38400: cfsetospeed(&options, B38400); break;
case 115200: cfsetospeed(&options, B115200); break;
case 230400: cfsetospeed(&options, B230400); break;
default:
fprintf(stderr, "warning: baud rate %u is not supported, using 9600.\n", baud_rate);
cfsetospeed(&options, B9600);
break;
}
cfsetispeed(&options, cfgetospeed(&options));
result = tcsetattr(fd, TCSANOW, &options);
if (result)
{
perror("tcsetattr failed");
close(fd);
return -1;
}
return fd;
}
// Writes bytes to the serial port, returning 0 on success and -1 on failure.
int write_port(int fd, uint8_t * buffer, size_t size)
{
ssize_t result = write(fd, buffer, size);
if (result != (ssize_t)size)
{
perror("failed to write to port");
return -1;
}
return 0;
}
// Reads bytes from the serial port.
// Returns after all the desired bytes have been read, or if there is a
// timeout or other error.
// Returns the number of bytes successfully read into the buffer, or -1 if
// there was an error reading.
ssize_t read_port(int fd, uint8_t * buffer, size_t size)
{
size_t received = 0;
while (received < size)
{
ssize_t r = read(fd, buffer + received, size - received);
if (r < 0)
{
perror("failed to read from port");
return -1;
}
if (r == 0)
{
printf("\rRCV timeout");
}
received += r;
}
return received;
}整体程序
//serial_port.cpp
#include "serial_port.h"
void SerialPub::initialize() {
pps_bit=false;
handshake_bit=false;
revice_bit=false;
timestamp=0;
pre_timestamp=0;
time_dt=0;
speedv_cnt=0;
anglev_cnt=0;
Pre_right_enc=0;
Pre_left_enc=0;
odom_X=0;
odom_Y=0;
AiCloud_bit=0;
Location_bit=0;
frame_buff[FRAME_LEN] = {0};
device=open_serial_port("/dev/ttyGS1", 230400);
std::cout<<"**********************open**************"<<device<<std::endl;
if (device < 0)
{
ROS_INFO_STREAM("Unable to open serial port");
}
else
{
ROS_INFO_STREAM("Serial port opened successfully");
}
}
SerialPub::SerialPub(ros::NodeHandle node, ros::NodeHandle private_nh,std::string serial_name,bool usb_log_) {
//private_nh.param("ser_port", ser_port, std::string("/dev/serial_px30"));
std::cout<<"**************************"<<serial_name<<std::endl;
test_cs=usb_log_;
my_time=new TIME[1];
ros::Rate loop_rate(200); //设置发送数据的频率为100Hz
initialize();
CldPotAi_Sub = node.subscribe("/cloud_point_ai",5, &SerialPub::PCA_Callback,this);
Location_Sub = node.subscribe("/Location_pose",5, &SerialPub::Loc_Callback,this);
pub_gps_fix = node.advertise<sensor_msgs::NavSatFix>("GPS_fix", 5);
pub_odom = node.advertise<nav_msgs::Odometry>("Ser_odom", 5);
pub_imu = node.advertise<sensor_msgs::Imu>("Ser_imu", 5);
}
void Serial_Send(SerialPub *Ser_)
{
std::vector<uint8_t> vec;
std::cout<<"ai serial send"<<std::endl;
uint8_t send_date_Head[40] = {
0xA4,0x59,0x43,//begin
0x04,
0x01,0x01,
0x00,0x00,0x00,0x1C,//length 6-9
0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,//time 10-17
0x00,0x00,0x00,0x00,//state 18-21
0x00,0x00,0x00,0x00,//error 22-25
0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,//date 26-37 location
//34 obs-length uint16_t
0x0D,0x0A};//38-39
uint32_t send_length=0x01;
uint32_t out_status=0;
uint64_t out_time=0;
uint32_t out_error=0;
uint8_t aaaaaa=0x00;
while(ros::ok()){
//vec.clear();
std::this_thread::sleep_for(std::chrono::milliseconds(20));
if(Ser_->handshake_bit)
{
//https://blog.csdn.net/ktigerhero3/article/details/108017281;
///save number
vec.resize(38);
if(Ser_->AiCloud_bit&&Ser_->Location_bit&&Ser_->vec_ai.size()>0)
{
//vec.resize(Ser_->vec_ai.size()+38);
out_status=0x01;
out_error=0x01;
//send_length=40+Ser_->vec_ai.size();
for(int i_loc=0;i_loc<12;i_loc++)//gai i=0
{
vec[26+i_loc]=Ser_->vec_loc[i_loc];
}
vec.insert(vec.begin()+37,Ser_->vec_ai.begin(),Ser_->vec_ai.end());
Ser_->AiCloud_bit=false;
Ser_->Location_bit=false;
}
else if(Ser_->Location_bit){ //Loc short package vec_loc
out_status=0x02;
out_error=0x02;
for(int i_loc=0;i_loc<12;i_loc++)//gai i=0
{
vec[26+i_loc]=Ser_->vec_loc[i_loc];
}
Ser_->Location_bit=false;
}
else if (Ser_->AiCloud_bit&&Ser_->vec_ai.size()>0)
{
vec.resize(Ser_->vec_ai.size()+38);
out_status=0x04;
out_error=0x04;
vec.insert(vec.begin()+37,Ser_->vec_ai.begin(),Ser_->vec_ai.end());
Ser_->AiCloud_bit=false;
}
else
{ // AI + Loc big package vec_loc vec_ai
out_status=0x08;
out_error=0x08;
}
for(int i=0;i<38;i++){
vec[i]=send_date_Head[i];
}//head
out_time=std::chrono::duration_cast<std::chrono::microseconds>
(std::chrono::system_clock::now().time_since_epoch()).count();
vec.push_back(send_date_Head[38]);
vec.push_back(send_date_Head[39]);
vec[vec.size()-3]=aaaaaa++;
send_length=vec.size()-12;
vec[6]=(send_length>>24)&0xFF;
vec[7]=(send_length>>16)&0xFF;
vec[8]=(send_length>>8)&0xFF;
vec[9]=send_length;
vec[10]=out_time;
vec[11]=(out_time>>8)&0xFF;
vec[12]=(out_time>>16)&0xFF;
vec[13]=(out_time>>24)&0xFF;
vec[14]=(out_time>>32)&0xFF;
vec[15]=(out_time>>40)&0xFF;
vec[16]=(out_time>>48)&0xFF;
vec[17]=(out_time>>54)&0xFF;
vec[18]=out_status;
vec[19]=out_status<<8;
vec[20]=out_status<<16;
vec[21]=out_status<<24;
vec[22]=out_error;
vec[23]=out_error<<8;
vec[24]=out_error<<16;
vec[25]=out_error<<24;
ssize_t rtn;
uint8_t send_buf[vec.size()];
for(int send_i=0;send_i<vec.size();send_i++)
{
send_buf[send_i]=vec[send_i];
}
rtn = write_port(Ser_->device, send_buf, sizeof(send_buf));
if(rtn == 0)
{
ROS_INFO_STREAM("serial send success");
}
}
}
}
void SerialPub::Loc_Callback(const std_msgs::UInt8MultiArray::ConstPtr &msg){
Location_bit=true;
for(int i = 0; i < msg->data.size();i++){
vec_loc[i]=msg->data[i];
} //for(int i = 0; i < vec.size();i++){printf("%02x ",vec[i]);}printf("\n");
}
void SerialPub::PCA_Callback(const std_msgs::UInt8MultiArray::ConstPtr &msg){
vec_ai.clear();
AiCloud_bit=true;
for(int i = 0; i < msg->data.size();i++){
vec_ai.push_back(msg->data[i]);
} //for(int i = 0; i < vec.size();i++){printf("%02x ",vec[i]);}printf("\n");
}
SerialPub::~SerialPub() {
delete my_time;
close(device);
}
void SerialPub::listen_ser()
{
uint8_t frame_buff_[5000];
ssize_t rtn=0;
if(handshake_bit)
{
memset(buf_save,0,sizeof(buf_save));
while(1)
{
read(device,buf_save,1);
if(buf_save[00]==0xA4)
{
frame_buff_[0]=0xA4;
read(device,buf_save,1);
if(buf_save[00]==0x59)
{
frame_buff_[1]=0x59;
read(device,buf_save,1);
if(buf_save[00]==0x43)
{
frame_buff_[2]=0x43;
break;
}
}
}
}
rtn = read_port(device, buf_save, 7);
if(rtn>0)
{
for(int buf_i=0;buf_i<7;buf_i++)
{
frame_buff_[buf_i+3]=buf_save[buf_i];
printf("%02x ", frame_buff_[buf_i]);
}
printf("\n");
}
n_size=frame_buff_[9]+(frame_buff_[8]<<8)+(frame_buff_[7]<<16)+(frame_buff_[6]<<24);
std::cout<<n_size <<std::endl;
printf("\n");
rtn = read_port(device, buf_save, n_size+2);//read 0d 0a
for(int buf_i=0;buf_i<n_size+2;buf_i++)// 0d 0a
frame_buff_[buf_i+10]=buf_save[buf_i];
ROS_INFO_STREAM("serial recive");
for(int frame_i=0;frame_i<n_size+12;frame_i++)
{
frame_buff[frame_i]=frame_buff_[frame_i];
}
//handshake_bit=true;
revice_bit=true;
}
}
void Serial_Receive(SerialPub *Ser_)
{
while(ros::ok()){
Ser_->listen_ser();
}
}
void PPS_Receive(SerialPub *Ser_)
{
std::cout<<"run1"<<std::endl;
pps_handle_t handle[4];
int avail_mode[4];
int ret;
std::string pps_str="/dev/pps0";
char dev_pps[20];
pps_str.copy(dev_pps, 8, 0);
*(dev_pps+9)='\0';
strcpy((dev_pps), pps_str.c_str());
std::cout<<"pps"<<dev_pps<<std::endl;
ret = find_source(dev_pps, &handle[0], &avail_mode[0]);//check pps source is opened, set /dev/pps0
if (ret < 0)
exit(EXIT_FAILURE);
printf("ok, found %d source(s), now start fetching data...\n",0);
while(ros::ok()){
ret = fetch_source(0, &handle[0], &avail_mode[0]);//support check a lot pps source block
if (ret < 0 && errno != ETIMEDOUT)
exit(EXIT_FAILURE);
if(ret==0)
{
Ser_->pps_bit=true;
}
}
time_pps_destroy(handle[0]); /**/
}
void SerialPub::Ser_pub(){
char *ret1;
char *ret2;
handshake_bit=true;
uint8_t HandShake[]={0xA4,0x59,0x43,0x04,0x07,0x02,0x00,0x00,0x00,0x01,0x00,0x0D,0x0A};
while (ros::ok())
{
if (revice_bit) {
revice_bit=false;
uint32_t yz_len=frame_buff[9]+(frame_buff[8]<<8)+(frame_buff[7]<<16)+(frame_buff[6]<<24);
if((yz_len==1)&&(handshake_bit==false)){
printf("test handshake\n");
write_port(device, HandShake, sizeof(HandShake));
}
else if (n_size == yz_len) {//验证长度
handshake_bit=true;
std::vector<uint8_t> vec_data_rcv(frame_buff, frame_buff + FRAME_LEN);
ret1 = strstr((char*)frame_buff,(char*)MSG_H);//验证头 遇到00会停止检测，只能验证头
if(ret1!=NULL)
{
//ret2 = strstr((char*)frame_buff, (char*)MSG_E);
if(frame_buff[yz_len+12-1]==0x0A&&frame_buff[yz_len+12-2]==0x0D)//验证尾
{
// qxwz_rtcm_sendGGAWithGGAString(sendbuf);
std::vector<uint8_t>::iterator match_TIME_H = std::search(vec_data_rcv.begin(), vec_data_rcv.end(),MSG_H_TIME, MSG_H_TIME + 3);
if(match_TIME_H!=vec_data_rcv.end()){
time_del(match_TIME_H);
}
else{
ROS_INFO_STREAM("find time false");
}
std::vector<uint8_t>::iterator match_GPS_H = std::search(vec_data_rcv.begin(), vec_data_rcv.end(),MSG_H_GPS, MSG_H_GPS + 3);
if(match_GPS_H!=vec_data_rcv.end()){
gps_del(match_GPS_H);
}
else{
ROS_INFO_STREAM("find rtk false");
}
std::vector<uint8_t>::iterator match_IMU_H = std::search(vec_data_rcv.begin(), vec_data_rcv.end(),MSG_H_IMU, MSG_H_IMU + 3);
if(match_IMU_H!=vec_data_rcv.end()){
imu_del(match_IMU_H);
}
else{
ROS_INFO_STREAM("find imu false");
}
std::vector<uint8_t>::iterator match_ODOM_H = std::search(vec_data_rcv.begin(), vec_data_rcv.end(),MSG_H_ODOM, MSG_H_ODOM + 3);
if((match_ODOM_H!=vec_data_rcv.end())){
odom_del(match_ODOM_H);
}
else{
ROS_INFO_STREAM("find odom false");
}
}
else{
ROS_INFO_STREAM("end false");
}
}
else{
ROS_INFO_STREAM("head false");
}
vec_data_rcv.clear();
}
else{
ROS_INFO_STREAM("length false");
}
}
else {
//ROS_INFO_STREAM("no data");
}
ros::spinOnce();
}
close(device);
}
bool SerialPub::time_del(std::vector<uint8_t>::iterator match_TIME_){
const TIME_frame_t * Time_frame_p=(const TIME_frame_t *) &*(match_TIME_+3);
if(test_cs)
printf("%d/%d/%d\n", Time_frame_p->ucYear,Time_frame_p->ucMonth,Time_frame_p->ucDay);
timestamp= Time_frame_p->MsOneDay;
time_dt=pre_timestamp-timestamp;
pre_timestamp=timestamp;
if(pps_bit){
my_time->year=Time_frame_p->ucYear;
my_time->month=Time_frame_p->ucMonth;
my_time->day=Time_frame_p->ucDay;
if(my_time->year<2000){
my_time->year=2021;
my_time->month=0x0c;
my_time->day=0x08;
}
my_time->hour=Time_frame_p->MsOneDay/1000/60/60;
my_time->minute=Time_frame_p->MsOneDay/1000/60;
my_time->second=(Time_frame_p->MsOneDay+500)/1000;
if(!systime.setSystemTime(my_time))
std::cout<<"settime false"<<std::endl;
pps_bit=false;
}
}
bool SerialPub::gps_del(std::vector<uint8_t>::iterator match_GPS_){
const GPS_frame_t * GPS_frame_p=(const GPS_frame_t *) &*(match_GPS_+3);
const GPS_frame_t1 * GPS_frame_p1=(const GPS_frame_t1 *) &*(match_GPS_+24);
float latstd = GPS_frame_p1->lat_std;
float lonstd = GPS_frame_p1->lon_std;
int levela = (int)GPS_frame_p->level;
//std::cout<<"level="<<levela<<std::endl;
//std::cout<<"latstd="<<latstd<<std::endl;
//std::cout<<"lon="<<lonstd<<std::endl;
float latvar = latstd*latstd;
float lonvar = lonstd*lonstd;
fix.header.stamp.fromNSec(timestamp);
fix.status.status=levela;
fix.altitude=GPS_frame_p->hgt;
fix.latitude=GPS_frame_p->lat;
fix.longitude=GPS_frame_p->lon;
fix.position_covariance={ lonvar, 0, 0,
0, latvar, 0,
0, 0, 0};
fix.header.frame_id = "GPS_fix";
pub_gps_fix.publish(fix);
return true;
}
bool SerialPub::imu_del(std::vector<uint8_t>::iterator match_IMU_){
const IMU_frame_t * IMU_frame_p=(const IMU_frame_t *) &*(match_IMU_+3);
imu_data.header.stamp.fromNSec(timestamp);
imu_data.header.frame_id = "imu_link";
imu_data.angular_velocity.x = IMU_frame_p->gyro_x;
imu_data.angular_velocity.y = IMU_frame_p->gyro_y;
imu_data.angular_velocity.z = IMU_frame_p->gyro_z;
imu_data.linear_acceleration.x = IMU_frame_p->acc_x;
imu_data.linear_acceleration.y = IMU_frame_p->acc_y;
imu_data.linear_acceleration.z = IMU_frame_p->acc_z;
pub_imu.publish(imu_data);
}
bool SerialPub::odom_del(std::vector<uint8_t>::iterator match_ODOM_){
const ODOM_frame_t * ODOM_frame_p=(const ODOM_frame_t *) &*(match_ODOM_+3);
uint32_t right_enc=ODOM_frame_p->right_encode;
uint32_t left_enc=ODOM_frame_p->left_encode;
//printf("right_enc=%d", right_enc);
//printf("left_enc=%d", left_enc);
//printf("\n");
int32_t R_encodeCdif=right_enc-Pre_right_enc;
int32_t L_encodeCdif=left_enc-Pre_left_enc;
//当编码器启动时，发生跳变时，值为0；3000rpm，线数10000 0.1s=50000线
if(R_encodeCdif>50000&&R_encodeCdif<-50000&&L_encodeCdif>50000&&L_encodeCdif<-50000)
{
R_encodeCdif=0;
L_encodeCdif=0;
}
float distance_sum = 0.5f*(R_encodeCdif+L_encodeCdif);//短时间内，前进距离两侧速度和
float distance_diff = R_encodeCdif-L_encodeCdif;//短时间内，转弯角度为两侧速度差
float Yaw_Angle_Diff = distance_diff * const_angle;// 采样期间走的角度
float Yaw_Angle=Yaw_Angle+Yaw_Angle_Diff;//计算里程计方向角
float Yaw_Angle_1 = Yaw_Angle + 0.5f * Yaw_Angle_Diff;//里程计方向角数据位数变化，用于三角函数计算
odom_X=odom_X+distance_sum*cos(Yaw_Angle_1)*const_frame; //距离*Yaw（偏角） 解析X坐标
odom_Y=odom_Y+distance_sum*sin(Yaw_Angle_1)*const_frame;
// 当前瞬时线速度 w=th/t
float SpeedV_Ang=0,SpeedV_V=0;
if(time_dt!=0)
{
SpeedV_Ang=Yaw_Angle_Diff/time_dt; //角速度
SpeedV_V=(distance_sum*const_frame)/time_dt; //速度=位移/时间； 时间=定时间*Timer
}
if(speedv_cnt<3)
{
speedvsave[speedv_cnt]=SpeedV_V;
speedv_cnt++;
}else
{
speedv_cnt=0;
speedvsave[speedv_cnt]=SpeedV_V;
speedv_cnt++;
}
float Speedv_send=0;
for(int speedvplus=0;speedvplus<3;speedvplus++)
{
Speedv_send=Speedv_send+speedvsave[speedvplus];
}
Speedv_send=Speedv_send/3;
if(anglev_cnt<3)
{
anglevsave[anglev_cnt]=SpeedV_Ang;
anglev_cnt++;
}else
{
anglev_cnt=0;
anglevsave[anglev_cnt]=SpeedV_Ang;
anglev_cnt++;
}
float Anglev_send=0;
for(int anglevplus=0;anglevplus<3;anglevplus++)
{
Anglev_send=Anglev_send+anglevsave[anglevplus];
}
Anglev_send=Anglev_send/3;
if(Yaw_Angle > pi)
{
Yaw_Angle -= pi_2_1;//减2pi
}
else
{
if(Yaw_Angle < -pi)
{
Yaw_Angle+=pi_2_1;//加2pi
}
}
Pre_right_enc=right_enc;
Pre_left_enc=left_enc;
ser_odom.header.stamp.fromNSec(timestamp);
ser_odom.header.frame_id = "odom";
//set the position
ser_odom.pose.pose.position.x = odom_X;
ser_odom.pose.pose.position.y = odom_Y;
ser_odom.pose.pose.position.z = 0.0;
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(Yaw_Angle);//将偏航角转换成四元数
ser_odom.pose.pose.orientation = odom_quat;
//set the velocity
ser_odom.child_frame_id = "base_link";
ser_odom.twist.twist.linear.x = Speedv_send*cos(Yaw_Angle);//线速度
ser_odom.twist.twist.linear.y = Speedv_send*sin(Yaw_Angle);//线速度
ser_odom.twist.twist.angular.z = Anglev_send;//角速度
//载入covariance矩阵 https://blog.csdn.net/ethan_guo/article/details/79635575
if ((Speedv_send==0)&&(Anglev_send==0))//机器人静止和动起来
{
ser_odom.pose.covariance = ODOM_POSE_COVARIANCE2;
ser_odom.twist.covariance = ODOM_TWIST_COVARIANCE2;
}
else
{
ser_odom.pose.covariance = ODOM_POSE_COVARIANCE;
ser_odom.twist.covariance = ODOM_TWIST_COVARIANCE;
}
//publish the message
pub_odom.publish(ser_odom);
}
1422
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
