ROS串口数据读取发送

最新推荐文章于 2024-08-30 17:26:38 发布

任逍遥的博客

最新推荐文章于 2024-08-30 17:26:38 发布

阅读量1.1w

点赞数 8

本文链接：https://blog.csdn.net/m0_37598942/article/details/80713512

版权

记录一下

包含串口数据HEX形式的读取，发送；读取后通过节点发送消息

/*
 * myserialnode.cpp
 * 
 */
#include "ros/ros.h"
#include <serial/serial.h>
#include "crc.h"
#include "global.h"
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include "std_msgs/UInt8.h"
#include "std_msgs/UInt8MultiArray.h"
serial::Serial ros_ser;
crc::CRC cc_crc;
global::GLOBAL cc_global;

void callback(const std_msgs::String::ConstPtr& msg)
{
    ROS_INFO_STREAM("Write to serial port:" << msg->data);
    ros_ser.write(msg->data);
}


int main (int argc, char** argv)
{
    static int counter;
    static int smx=0;
    static uint8_t s_buffer[100];
    int i;
    int tst1=1,tst2=2;
    static int p;
    unsigned int test_crc;

    std_msgs::UInt8MultiArray  r_buffer;
    ros::init(argc, argv, "myserialnode");
    ros::NodeHandle cc_setial1;
    //订阅主题command
    ros::Subscriber command_sub = cc_setial1.subscribe("command", 1000, callback);
    //发布主题sensor
    ros::Publisher sensor_pub = cc_setial1.advertise<std_msgs::UInt8MultiArray>("sensor", 1000);

    try
    {
        ros_ser.setPort("/dev/ttyUSB0");
        ros_ser.setBaudrate(38400);
        serial::Timeout to = serial::Timeout::simpleTimeout(1000);
        ros_ser.setTimeout(to);
        ros_ser.open();
    }
    catch (serial::IOException& e)
    {
        ROS_ERROR_STREAM("Unable to open port ");
        return -1;
    }
    if(ros_ser.isOpen())
    {
        ROS_INFO_STREAM("Serial Port opened");
    }
    else
    {
        return -1;
    }
    ros::Rate loop_rate(100);
    while(ros::ok())
    {
        ros::spinOnce();
        memset(s_buffer,0,sizeof(s_buffer));
          //数据打包
        s_buffer[0] = 0x00;
        s_buffer[1] = 0x11;
        //VA
        s_buffer[2] = 0x01;
        s_buffer[3] = 0X01;
        //s_buffer[4] = 0X00;
        s_buffer[5] = 0;
        s_buffer[6] = 0;
        s_buffer[7] = 0;
        s_buffer[9] =0;
        s_buffer[10] = 0;
        s_buffer[11] = 0;
        s_buffer[12] = 0;
        s_buffer[13] = 0;
        s_buffer[14] = 0;
        s_buffer[15] = 0;
        s_buffer[16] = 0;
        s_buffer[17] = 0;

        counter++;
        if(counter>=10)
        {
            counter=0;
            cc_global.global_counter++;
            if(cc_global.global_counter>100)
                cc_global.global_counter=0;
            s_buffer[4]=cc_global.global_counter;
            s_buffer[18]=cc_crc.sum(s_buffer,1,5);
            test_crc= cc_crc.CRC16(&s_buffer[1],4);
            ros_ser.write(s_buffer,19);
            ROS_INFO("serial sends: 0x%x 0x%x 0X%x", s_buffer[4],s_buffer[18],test_crc);
            cc_crc.sayHello();     
        }
        if(ros_ser.available())
        {
            ROS_INFO_STREAM("Reading from serial port");
            //std_msgs::String serial_data;
            std_msgs::UInt8MultiArray  serial_data;
            //获取串口数据
            p=ros_ser.available();
            ros_ser.read (serial_data .data,p) ;
            ROS_INFO("serial  heard: %x ", p);
            r_buffer.data.resize(p+1);
            r_buffer.data[0]=p;
            for(i=0; i<p; i++)
            {
                r_buffer.data[i+1]=serial_data .data[i];
            }
            sensor_pub.publish( r_buffer);
        }

        loop_rate.sleep();
    }
}

接受函数

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/UInt8MultiArray.h"  

#include "std_msgs/MultiArrayLayout.h"
#include "std_msgs/MultiArrayDimension.h"
#include "std_msgs/Int32MultiArray.h"
#include "crc.h"
//回调函数
crc::CRC cc_crc;
class Listener
{
public:
  void callback(const std_msgs::UInt8MultiArray::ConstPtr& r_buffer);
};
void Listener::callback(const std_msgs::UInt8MultiArray::ConstPtr& r_buffer)
{
  if(r_buffer->data[1]==0x7e)
 {
 
  //ROS_INFO("I heard:");//, serial_data->data);
   ROS_INFO("listener heard: %x %d ",r_buffer->data[0],cc_crc.cc11);
   }
   else
   {
     ROS_INFO("wrong");//, serial_data->data);
   }
   
}
//void callback(const std_msgs::String::ConstPtr& msg)
//{
// ROS_INFO("listener got: [%s]", msg->data.c_str());
//}
int main(int argc, char **argv)
{
 ros::init(argc, argv, "listener");
 ros::NodeHandle n;
 //订阅主题
// ros::Subscriber sub = n.subscribe("sensor", 1000, callback);
  Listener listener;
  ros::Subscriber sub = n.subscribe("sensor", 1000, &Listener::callback, &listener);
      //ros::Publisher sensor_pub = n.advertise<std_msgs::UInt8MultiArray>("sensor", 1000);
  
 ros::spin();
 return 0;
}

ros中cmake

cmake_minimum_required(VERSION 2.8.3)
project( my_serial_node )

# Find catkin
 find_package(catkin REQUIRED COMPONENTS
    roscpp
    serial
    std_msgs
    )
 
 catkin_package(
   CATKIN_DEPENDS
     serial
     std_msgs
 )
#增加自定义头文件
  include_directories(
  ${catkin_INCLUDE_DIRS}
  include/my_serial_node
 )
 
 
 set(serial_SRCS
      src/crc.cpp 
     
)
 #增加静态库
     add_library(${PROJECT_NAME} ${serial_SRCS}
       )
  
  
add_executable( myserialnode src/myserialnode.cpp )
target_link_libraries( myserialnode  
					${catkin_LIBRARIES} 
					${PROJECT_NAME}
				  )

add_executable(talker src/talker.cpp)
target_link_libraries(talker ${catkin_LIBRARIES} ${PROJECT_NAME})

add_executable(listener src/listener.cpp)
target_link_libraries(listener ${catkin_LIBRARIES} ${PROJECT_NAME})

add_executable(listen_cmd src/listen_cmd.cpp)
target_link_libraries(listen_cmd ${catkin_LIBRARIES} ${PROJECT_NAME})

自定义的类

#include "crc.h"
#include <iostream>
#include "std_msgs/UInt8.h"
#include "ros/ros.h"
#include <stdio.h>

using crc::CRC;
static const unsigned char chCRCHTalbe[] =                                // CRC 高位字节值表
{
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
    0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40
};

static const unsigned char chCRCLTalbe[] =                                // CRC 低位字节值表
{
    0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7,
    0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E,
    0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9,
    0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC,
    0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,
    0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32,
    0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D,
    0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38,
    0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF,
    0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,
    0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1,
    0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4,
    0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB,
    0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA,
    0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,
    0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0,
    0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97,
    0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E,
    0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89,
    0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
    0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83,
    0x41, 0x81, 0x80, 0x40
};


CRC::CRC(void) //构造函数,不能带返回值
{

}
CRC::~CRC(void) //构造函数,不能带返回值
{

}

void CRC::sayHello(void)
{
    ROS_INFO_STREAM("HELLO........................... ");
}

uint8_t CRC::sum(uint8_t *buf,uint8_t i,uint8_t number)
{
    uint8_t  sum1;
    uint8_t  p;
    //sum1=buf[i];
    for(p=0; p<number; p++)
    {
        sum1+=buf[i+p];
    }
    sum1=~sum1;
    return sum1;
}



unsigned int CRC::CRC16(unsigned char* pchMsg, unsigned int wDataLen)
{
    unsigned int chCRCHi = 0xFF; // 高CRC字节初始化
    unsigned int chCRCLo = 0xFF; // 低CRC字节初始化
    unsigned long int wIndex;            // CRC循环中的索引

    while (wDataLen--)
    {
        // 计算CRC
        wIndex = chCRCLo ^ *pchMsg++ ;
        chCRCLo = chCRCHi ^ chCRCHTalbe[wIndex];
        chCRCHi = chCRCLTalbe[wIndex] ;
    }

    return ((chCRCHi << 8) | chCRCLo) ;
}

头文件

/*
 * crc.h
 * 
 * Copyright 2018 cc <>
 * 
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
 * MA 02110-1301, USA.
 * 
 * 
 */
#ifndef _CRC_H_
#define _CRC_H_

#include <iostream>
#include "std_msgs/UInt8.h"
#include <stdio.h>
//#include < stdint.h>
namespace crc
{
class CRC
{

public:
	CRC();//构造函数
	~CRC();//构造函数
    int cc11;
    void sayHello(void);
    uint8_t sum(uint8_t *buf,uint8_t i,uint8_t number);
    unsigned int CRC16(unsigned char* pchMsg, unsigned int wDataLen);
};



}
#endif