ROS下使用串口发送数据
#include <stdio.h>
#include <string.h>
#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <ros/package.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <serial/serial.h>
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include <fstream>
#include <iostream>
using namespace std;
serial::Serial ser;
ros::Subscriber serial_sub;
ros::Publisher read_pub;
typedef union{
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));
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];
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::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);
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
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)