ROS下使用串口发送数据

ROS下使用串口发送数据

#include 
#include 
#include 
#include 
#include 
#include 
#include 	//数据格式
#include 		//ros串口
#include 
#include 
#include 
#include 
using namespace std;

serial::Serial ser;			//定义一个串口对象
ros::Subscriber serial_sub;		//订阅位姿节点
ros::Publisher read_pub;

typedef union{				//定义一个联合体，用于double数据与16进制的转换
unsigned char cvalue[4];
float fvalue;
}float_union;

static uint8_t s_buffer[11];	//分配静态存储空间

void serial_sub_callback(const geometry_msgs::Twist::ConstPtr& msg)
{
    ROS_INFO("Publish turtle velocity command[%0.2f m/s, %0.2f rad/s]", msg->linear.x, msg->angular.z);
	float_union linear_x,angular_z;
	memset(s_buffer,0,sizeof(s_buffer));		//内存空间初始化为0
    linear_x.fvalue = msg->linear.x;
    angular_z.fvalue = msg->angular.z;
	//数据打包
	s_buffer[0] = 0xff;		//数据的帧头
	s_buffer[1] = 0xfe;
	s_buffer[2] = linear_x.cvalue[0];
	s_buffer[3] = linear_x.cvalue[1];
	s_buffer[4] = linear_x.cvalue[2];
	s_buffer[5] = linear_x.cvalue[3];

    s_buffer[6] = angular_z.cvalue[0];
	s_buffer[7] = angular_z.cvalue[1];
	s_buffer[8] = angular_z.cvalue[2];
	s_buffer[9] = angular_z.cvalue[3];
	//CRC		校验和从有效数据开始取异或即可
	s_buffer[10] = s_buffer[2]^s_buffer[3]^s_buffer[4]^s_buffer[5]^s_buffer[6]^s_buffer[7]^s_buffer[8]^s_buffer[9];
	ser.write(s_buffer,11);
	ROS_INFO("data send successful");
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "serial_node");		//ROS串口节点名称
	ros::NodeHandle my_node;
	serial_sub = my_node.subscribe("/write", 1000, serial_sub_callback); //这里订阅一个写数据的话题，用于接收数据并进行发送。
	read_pub =  my_node.advertise<std_msgs::String>("read", 1000);
	try
	{
	//设置串口属性，并打开串口
		ser.setPort("/dev/ttyUSB0");
		ser.setBaudrate(115200);		//波特率115200
		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())
    {
        ROS_INFO_STREAM("Serial Port opened");
    }
    else
    {
        return -1;
    }
	//指定循环的速率
	ros::Rate loop_rate(50);
	while (ros::ok())
	{
		ros::spinOnce();
		loop_rate.sleep();
	}
    ser.close();
	return 0;
}

参考链接

[1]https://blog.csdn.net/qq_21830903/article/details/91457392