ROS与STM32通信

[imuncle]

昨天刚搞定STM32模拟CH340，今天就实现了ROS的串口通信，果然还是硬件让人头秃，软件开发的资料太丰富了，随便百度就能找到答案。

下面进入正题。

ROS要和STM32通信首先要确定通信协议。我的协议很简单，单片机上有两个LED，如果接收到命令'0x01'，就改变1号LED的亮灭状态，如果收到0x02，就改变2号LED的亮灭状态。另外STM32会返回本次接受到的数据第一位和数据长度。

我的思路是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，具体实现如下：

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()或类似的函数，这些需要敲回车后才能获取到输入，体验感极差。

后面我找到了这个：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文件：

<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>

最终效果