ros学习之串口通信(数据读取),并进行发布
作者：互联网
串口参数:
波特率:9600
起始位:1
数据位:8
停止位:1
奇偶校验:无
例如超声波模组地址为0X01,则主机发送
0X55 0XAA 0X01 0X01 checksum
checksum=(帧头+用户地址+指令)&0x00ff=0x01
unsigned char all_data[5] = {0x55,0xAA,0x01,0x01,0x01}; //声明发出指令(帧头(0x55,0xAA)，地址(0x01、0x02、0x03)，指令(0x10)，checksum)
在这里插入代码片
#include <ros/ros.h> //类似 C 语言的 stdio.h
#include <serial/serial.h> //ROS已经内置了的串口包
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include <signal.h>
#include <sensor_msgs/Imu.h>
#include <ros_com_msg/com.h>
#include <ros/duration.h>
#include <string.h>
ros::Publisher imu_pub;
serial::Serial ser; //声明串口对象
bool sendcheck = true ;
unsigned char all_data[5] = {0x55,0xAA,0x01,0x01,0x01}; //声明发出指令(帧头(0x55,0xAA)，地址(0x01、0x02、0x03)，指令(0x10)，checksum)
const int MAX = 100;
int DecArr[MAX] = { 0 };
//十六进制转十进制
int Hex_Conversion_Dec(int aHex)
{
long Dec = 0;
int temp = 0;
int count = 0;
while (0 != aHex)//循环直至aHex的商为零
{
// ROS_INFO("%d------------------------------\n",aHex%16);
temp = aHex;
aHex = aHex / 16;//求商
temp = temp % 16;//取余
DecArr[count++] = temp;//余数存入数组
}
int j = 0;
for (int i = 0; i<count; i++ )
{
if (i < 1)
{
Dec = Dec + DecArr[i];
}
else
{
//16左移4位即16²，左移8位即16³、以此类推。
Dec = (Dec + (DecArr[i]*(16<<j)));
j += 4;
}
}
return Dec ;
}
/*************************************************************************/
//当关闭包时调用，关闭上传
void mySigIntHandler(int sig)
{
ROS_INFO("close the com serial!\n");
ser.close();
ros::shutdown();
}
/*************************************************************************/
void timercallback(const ros::TimerEvent&)
{
ser.write(all_data,5);
// ROS_INFO("write----------\n");
}
void cmd_comCallback(const ros_com_msg::comPtr &msg)
{
ROS_INFO("Subcribe Person Info++++++++++++++++: data1:%d data2:%d data3:%d data4:%d\n",
msg->Data_1, msg->Data_2, msg->Data_3,msg->Data_4);
}
int main(int argc,char **argv)
{
u16 len = 0;
// u16 TempCheck = 0 , check =0;
u8 data[200];
std::string usart_port;
ros::init(argc,argv,"com_talker",ros::init_options::NoSigintHandler); //解析参数，命名节点为 com_talker
ros_com_msg::com com_msg;
ros::NodeHandle nh;
ros::Publisher pub = nh.advertise<ros_com_msg::com>("com_info",1); //创建一个publisher对象，发布名为com_info，队列长度为1，消息类型为ros::com_msg::com。
signal(SIGINT, mySigIntHandler); //把原来ctrl+c中断函数覆盖掉，把信号槽连接到mySigIntHandler保证关闭节点
ros::Timer timer1 = nh.createTimer(ros::Duration (0.25), timercallback); //设置定时器每隔250ms发送数据
ros::Subscriber sub = nh.subscribe("cmd_com",1,cmd_comCallback); //创建subscriber 对象，名为cmd_com,注册回调函数为cmd_velCallback，队列长度为1。
try
{
//设置串口属性，并打开串口
ser.setPort("/dev/ttyUSB0");
ser.setBaudrate(9600);
serial::Timeout to = serial::Timeout::simpleTimeout(1000);
ser.setTimeout(to);
ser.open();
}
catch (serial::IOException& e)
{
ROS_ERROR_STREAM("Unable to open port ");
return -1;
}
//检测串口是否已经打开，并给出提示信息
if(ser.isOpen())
{
ser.flushInput(); //清空输入缓存,把多余的无用数据删除
ROS_INFO_STREAM("cgq Serial Port initialized");
}
else
{
return -1;
}
ros::Rate loop_rate(4); //设置循环的频率为20Hz，50ms
while(ros::ok())
{
ser.read(ReturnRobotData.data,sizeof(ReturnRobotData.data));
u16 TempCheck = 0 , check =0;
char num1 =0 ,num2 =0, num3 = 0,num4 =0;
for(u8 i=0 ; i<sizeof(ReturnRobotData.data)-1; i++)
{
TempCheck += ReturnRobotData.data[i];
check = TempCheck & 0x00ff;
// printf("return data %s\n",ReturnRobotData.data[i]);
}
//验证checksum
// printf("Returnchecksum :%x %x \n",ReturnRobotData.prot.header1,ReturnRobotData.prot.header2);
// printf("Returnchecksum :%x\n",ReturnRobotData.prot.Checksum);
// printf("Returncheck :%X\n",check);
if(ReturnRobotData.prot.header1 == 0x55 && ReturnRobotData.prot.header2 == 0xAA && ReturnRobotData.prot.Checksum == check)
{
ROS_INFO_STREAM("read seriel------------------");
int num1 = ReturnRobotData.prot.Data_1H*256 + ReturnRobotData.prot.Data_1L;
int num2 = ReturnRobotData.prot.Data_2H*256 + ReturnRobotData.prot.Data_2L;
int num3 = ReturnRobotData.prot.Data_3H*256 + ReturnRobotData.prot.Data_3l;
int num4 = ReturnRobotData.prot.Data_4H*256 + ReturnRobotData.prot.Data_4L;
// printf("ultrasonic_num1（0x）:%X ultrasonic_num1（0x）:%X ultrasonic_num3（0x）:%X ultrasonic_num4（0x）:%X\n",
// ReturnRobotData.prot.Data_1H*256 + ReturnRobotData.prot.Data_1L,
// ReturnRobotData.prot.Data_2H*256 + ReturnRobotData.prot.Data_2L,
// ReturnRobotData.prot.Data_3H*256 + ReturnRobotData.prot.Data_3l,
// ReturnRobotData.prot.Data_4H*256 + ReturnRobotData.prot.Data_4L);
printf("ultrasonic_num1（0x）:%X ultrasonic_num1（0x）:%X ultrasonic_num3（0x）:%X ultrasonic_num4（0x）:%X\n",
num1,num2,num3,num4);
//十六进制转化为十进制
int zhuanhuanum1 = Hex_Conversion_Dec(num1);
int zhuanhuanum2 = Hex_Conversion_Dec(num2);
int zhuanhuanum3 = Hex_Conversion_Dec(num3);
int zhuanhuanum4 = Hex_Conversion_Dec(num4);
printf("ultrasonic_num1（mm）:%d ultrasonic_num2（mm）:%d ultrasonic_num3（mm）:%d ultrasonic_num4（mm）:%d \n", zhuanhuanum1,zhuanhuanum2,zhuanhuanum3,zhuanhuanum4);
com_msg.Data_1 = zhuanhuanum1;
com_msg.Data_2 = zhuanhuanum2;
com_msg.Data_3 = zhuanhuanum3;
com_msg.Data_4 = zhuanhuanum4;
pub.publish(com_msg); //发布
ROS_INFO("------------------------------------------------\n");
memset(ReturnRobotData.data,0,sizeof(ReturnRobotData.data));
}
else
{
// ROS_INFO("not return %x %x %x\n",ReturnRobotData.prot.header1,ReturnRobotData.prot.header2,ReturnRobotData.prot.Checksum);
ROS_INFO("not read accuracy");
len = ser.available();
//清空数据残余
if(len > 0)
{
ser.read(data,len);
}
//全部探头打开
}
ros::spinOnce();
loop_rate.sleep(); //按前面设置的4Hz频率将程序挂起
}
return 0;
}
标签：读取,int,com,prot,ReturnRobotData,串口,ros,Data 来源： https://blog.csdn.net/stronger007/article/details/120652696