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)

超声波传感器发送指令周期需大于等于250ms。

在这里插入代码片
#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>
#include "ros_com_msg.h"
#include "ros_com_msg/com.h"          //要用到 msg 中定义的数据类型

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);   //设置循环的频率为4Hz，250ms

   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;
}

其中 ros::Rate loop_rate(4)和loop_rate.sleep()需要同时用才可以控制发布频率。
ros::Rate loop_rate写在循环外部，loop_rate.sleep写在循环内部。
ros::spinOnce()的作用是集中处理本节点的所有回调函数。当程序运行到spinOnce()时，程序到相应的topic订阅缓存区查看是否存在消息，如果有消息，则将消息传入回调函数执行回调函数;如果没有消息则继续向后执行。spinOnce()并不是等待到一个消息才向后执行，而是查看有没有消息。

运行成功截图com.msg放进msg(功能包ros_com_msg下的msg文件）
com.msg的代码

int16 Data_1
int16 Data_2
int16 Data_3
int16 Data_4

ros_com_msg.h的代码

#ifndef ROS_COM_MSG_H
#define ROS_COM_MSG_H

#include <ros/ros.h>

#define HEADER    0x55AA		//数据头

typedef unsigned char      u8;
typedef unsigned short int u16;
typedef short int          s16;
typedef unsigned       int u32;


union ReturnRobotData
{
   u8 data[13];
   struct
   {
      u8 header1;
      u8 header2;
      u8 Add;
      u8 Cmd;
      u8 Data_1H;
      u8 Data_1L;
      u8 Data_2H;
      u8 Data_2L;
      u8 Data_3H;
      u8 Data_3l;
      u8 Data_4H;
      u8 Data_4L;
      u8 Checksum;
   }prot;
}ReturnRobotData;

#endif // ROS_COM_MSG_H

在CMakeList.txt 添加编译选项

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
  serial
  geometry_msgs
)
-----------------------------------------------
## Generate messages in the 'msg' folder
 add_message_files(
   FILES
   com.msg
)
-----------------------------------------------
## Generate added messages and services with any dependencies listed here
 generate_messages(
   DEPENDENCIES
   std_msgs
 )
-----------------------------------------------
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES ros_com_msg
#  CATKIN_DEPENDS roscpp std_msgs
#  DEPENDS system_lib
#  message_runtime
CATKIN_DEPENDS message_runtime roscpp  std_msgs
)

在package.xml添加功能包依赖

  <build_depend>message_generation</build_depend>
  <exec_depend>message_runtime</exec_depend>
  
  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>std_msgs</exec_depend>