Open imuncle opened 4 years ago
昨天刚搞定STM32模拟CH340,今天就实现了ROS的串口通信,果然还是硬件让人头秃,软件开发的资料太丰富了,随便百度就能找到答案。
下面进入正题。
ROS要和STM32通信首先要确定通信协议。我的协议很简单,单片机上有两个LED,如果接收到命令'0x01',就改变1号LED的亮灭状态,如果收到0x02,就改变2号LED的亮灭状态。另外STM32会返回本次接受到的数据第一位和数据长度。
0x02
我的思路是ROS读取键盘输入,然后将键盘的输入以键盘码(不是字符串ASCII码)的形式发送给STM32,同时监听STM32的消息。
这里参考的是这篇博客:ROS使用官方包进行串口通信
我的代码如下:
int main(int argc, char **argv) { ros::init(argc, argv, "serial_read_node"); ros::NodeHandle n; ros::Subscriber write_sub = n.subscribe("keyboard", 1000, write_callback); 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 initialized"); } else { return -1; } ros::Rate loop_rate(50); while(ros::ok()) { if(ser.available()) { std_msgs::String result; result.data = ser.read(ser.available()); ROS_INFO_STREAM("Receive from STM32: " << result.data); } ros::spinOnce(); loop_rate.sleep(); } }
代码都挺简单的,注释就不要了吧
从上面的代码也可以看出,我这里订阅了一个keyboard消息,对应的回调函数是write_callback,具体实现如下:
keyboard
write_callback
uint8_t cmd[2] = {0x01, 0x02}; void write_callback(const std_msgs::UInt8::ConstPtr& msg) { ROS_INFO_STREAM("Writing to serial port: " << msg->data); if(msg->data == '1') ser.write(cmd, 1); //发送串口数据 else if(msg->data == '2') ser.write(&cmd[1], 1); else { ROS_INFO_STREAM("Invalid Command!\r\n"); } }
我这里将键盘上的数字1和数字2视为正确指令,其他按键视为错误指令。
键盘的监听这里卡了比较长的时间,因为基本上百度能找到的都是使用getchar()或类似的函数,这些需要敲回车后才能获取到输入,体验感极差。
getchar()
后面我找到了这个:erratic_robot/keyboard.cpp
这个代码里面单独开了一个线程,而且写的比较复杂,我这里简化如下:
#include <termios.h> #include <signal.h> #include <math.h> #include <stdio.h> #include <stdlib.h> #include <sys/poll.h> #include <std_msgs/UInt8.h> #include <ros/ros.h> int kfd = 0; struct termios cooked, raw; void CloseKeyboard(int sig) { tcsetattr(kfd, TCSANOW, &cooked); ROS_INFO_STREAM("Shut Down!"); ros::shutdown(); } int main(int argc, char** argv) { uint8_t c; ros::init(argc,argv,"keuboard_node"); ros::NodeHandle n; ros::Publisher keyboard_pub = n.advertise<std_msgs::UInt8>("keyboard", 1000); tcgetattr(kfd, &cooked); memcpy(&raw, &cooked, sizeof(struct termios)); raw.c_lflag &=~ (ICANON | ECHO); raw.c_cc[VEOL] = 1; raw.c_cc[VEOF] = 2; tcsetattr(kfd, TCSANOW, &raw); puts("Reading from keyboard"); struct pollfd ufd; ufd.fd = kfd; ufd.events = POLLIN; signal(SIGINT, CloseKeyboard); //重定向shutdown函数,ROS默认调用ros::shutdown() while(ros::ok()) { int num; if ((num = poll(&ufd, 1, 250)) < 0) { perror("poll():"); return -1; } else if(num > 0) { if(read(kfd, &c, 1) < 0) { perror("read():"); return -1; } } else { // ROS_INFO_STREAM("Key UP"); continue; } ROS_INFO_STREAM("Key Down: " << c); std_msgs::UInt8 input; input.data = c; keyboard_pub.publish(input); } }
代码就不细说了,很容易看懂。
最后再写个简单的launch文件:
<launch> <node name="serial_read" pkg="com" type="reader" output="screen"/> <node name="serial_key" pkg="com" type="keyboard" output="screen" launch-prefix="gnome-terminal -e"/> </launch>
昨天刚搞定STM32模拟CH340,今天就实现了ROS的串口通信,果然还是硬件让人头秃,软件开发的资料太丰富了,随便百度就能找到答案。
下面进入正题。
ROS要和STM32通信首先要确定通信协议。我的协议很简单,单片机上有两个LED,如果接收到命令'0x01',就改变1号LED的亮灭状态,如果收到
0x02
,就改变2号LED的亮灭状态。另外STM32会返回本次接受到的数据第一位和数据长度。我的思路是ROS读取键盘输入,然后将键盘的输入以键盘码(不是字符串ASCII码)的形式发送给STM32,同时监听STM32的消息。
实现串口通信
这里参考的是这篇博客:ROS使用官方包进行串口通信
我的代码如下:
代码都挺简单的,
注释就不要了吧监听键盘输入
从上面的代码也可以看出,我这里订阅了一个
keyboard
消息,对应的回调函数是write_callback
,具体实现如下:我这里将键盘上的数字1和数字2视为正确指令,其他按键视为错误指令。
键盘的监听这里卡了比较长的时间,因为基本上百度能找到的都是使用
getchar()
或类似的函数,这些需要敲回车后才能获取到输入,体验感极差。后面我找到了这个:erratic_robot/keyboard.cpp
这个代码里面单独开了一个线程,而且写的比较复杂,我这里简化如下:
代码就不细说了,很容易看懂。
添加Launch文件
最后再写个简单的launch文件:
最终效果