其他分享
首页 > 其他分享> > ros学习之串口通信(数据读取),并进行发布

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