ROS串口通信

　　身处机器人行业，不想一直只做低端的单片机控制，老是待在舒适区，所以一直都想学一下ROS系统，但看了几个月资料后，感觉还是云里雾里，似懂非懂，感念似乎都很清楚，但要实际去做，却又感觉无从下手。

　　于是想先找点眼前马上能用的着东西来实验一下，串口无疑是最合适的，来个ROS串口通信，就简单的跟单片机通通信也是不错的，反正其他难的也还做不了；

　　当然之前看的资料也没白看，系统安装、基本概念、基本操作都得心里有底，因为Linux基础并不是很好，所以还是比较畏惧敲命令的方式的，不管在Linux系统上做什么，我都还是很乐意去找相应IDE，ROS也不例外，官网上教程全是命令行，痛苦了一阵子之后，实在觉得没必要回到原始人状态，写代码就不用说了，光是那一堆依赖都能让人崩溃，于是网上一顿搜，发现ROS开发也是有IDE的，那个欣喜之情啊，简直溢于言表；马上下载安装上了，用了一段时间，发现这工具有很多bug，有时候莫名其妙的错误，但无妨，至少不用再自己去手动添加依赖了，我心足以！

　　下面详细介绍如何利用RoboWare-Studio创建一个可以跟串口通信的小例子；

　　1、创建工作空间，打开Studio创建一个工作区就行了，工作区中的src文件夹会自动创建，编译一下；

　　2、在src文件夹上右键选择添加C++节点，输入节点名和相应的依赖即可；依赖后面可以随时改；

　　3、创建节点后，里面的src文件夹自动创建，下一步就可以创建C++文件了，如下部分，是串口操作：

#include <ros/ros.h>

#include <serial/serial.h> //ROS已经内置了的串口包

#include <std_msgs/String.h>

#include <std_msgs/Empty.h>

#include <stm32f407/tdata.h>

#include "std_msgs/UInt8.h"

#include "std_msgs/UInt8MultiArray.h"

serial::Serial ser; //声明串口对象

//回调函数

void datasend_callback(const stm32f407::tdata::ConstPtr &frm)

{

int len = frm->data.size();

printf("Send:%02d 0x", len);

for (int i = 0; i < len; i++)

{

// printf("%02X ", frm->data[i]);

printf("%02X ", frm->data.at(i));

}

printf("\r\n");

ser.write(frm->data); //发送串口数据

}

int main (int argc, char** argv)

{

//初始化节点

ros::init(argc, argv, "serial_readwrite_node");

//声明节点句柄

ros::NodeHandle nh;

static int len;

stm32f407::tdata RecvData;

//订阅主题，并配置回调函数

ros::Subscriber send_sub = nh.subscribe("datasend", 1000, datasend_callback);

//发布主题

ros::Publisher read_pub = nh.advertise<stm32f407::tdata>("datarecv", 1000);

try

{

//设置串口属性，并打开串口

ser.setPort("/dev/ttyUSB1");

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(10);

while(ros::ok())

{

len = ser.available();

if(len>0)

{

std_msgs::UInt8MultiArray serial_data;

// len = ser.available();

ser.read(serial_data.data,len) ;

printf("Recv:%02d 0x", len);

RecvData.data.clear();//清除上一次的数据

for (size_t i = 0; i < len; i++)

{

// ROS_INFO("%02X", serial_data.data[i]);

printf("%02X ", serial_data.data[i]);

RecvData.data.push_back(serial_data.data[i]);

}

printf("\r\n");

read_pub.publish(RecvData);

}

//处理ROS的信息，比如订阅消息,并调用回调函数

ros::spinOnce();

loop_rate.sleep();

}

}

 

　　其中操作非常简单，定义了一个订阅句柄，用来接收其他节点发布的数据，然后在回调当中，用串口直接发送出去，另外定义一个发布句柄，在主循环不断扫描串口，读取数据，

如果有读到数据，则发布出去，其他节点订阅后既可查看单片机上发的数据；

　　其中无论订阅还是发布都使用到了一个自定义消息，用来规定要传输数据的格式，添加Msg文件夹后，再添加msg文件，内容如下：

　　这便是两个话题对应消息格式；

　　上面这个节点主要用作直接对硬件串口进行收发，它只管收发，并不理会收发的数据是什么，具体流程为，如果接收其他节点发布的消息，则将之原样发送到串口，如何

扫描到串口有数据，则将读到的数据发布到话题上；

　　所以如果我们要发特定数据并对接收到的做数据处理，则需添加另外一个节点，名称定为datapro，内容如下：

#include "ros/ros.h"

#include "std_msgs/String.h"

#include <sstream>

#include <stm32f407/tdata.h>

#include "CRC.h"

//回调函数

void datarecv_callback(const stm32f407::tdata::ConstPtr &frm)

{

int len = frm->data.size();

// std::cout << "frm->data.size=" << cnt << std::endl;

// ROS_INFO_STREAM("Writing to serial port" << msg->data);

// ROS_INFO_STREAM("Send: " << frm->data);

// printf("Recv:%ld 0x", cnt);

printf("Recv:%02d 0x", len);

for (int i = 0; i < len; i++)

{

// printf("%02X ", frm->data[i]);

printf("%02X ", frm->data.at(i));

}

printf("\r\n");

}

int main(int argc, char **argv)

{

ros::init(argc, argv, "datapro");

ros::NodeHandle nh;

//订阅主题，并配置回调函数

ros::Subscriber recv_sub = nh.subscribe("datarecv", 1000, datarecv_callback);

ros::Publisher send_pub = nh.advertise<stm32f407::tdata>("datasend", 1000);

ros::Rate loop_rate(1);//10hz

int len=0;

stm32f407::tdata SendData;

CRC cc_crc;

//复位 A6 B7 01 02 04 00 B7 1B 02 A6 B7 01 02 04 05 B7 E4 F7

uint8_t Buffer[9] ={0xA6, 0xB7, 0x01, 0x02, 0x04, 0x05, 0xB7, 0xE4, 0xF7};

uint16_t CrcSum=0;

CrcSum = cc_crc.CRC16_ccitt(Buffer,0,7);

// printf("0x%04X\r\n",CrcSum);

Buffer[7] = (uint8_t)(CrcSum>>8);

Buffer[8] = (uint8_t)CrcSum;

SendData.data.clear();

for (size_t i = 0; i < 9; i++)

{

SendData.data.push_back(Buffer[i]);

}

 

// SendData.data.push_back(0xA6);

// SendData.data.push_back(0xB7);

// SendData.data.push_back(0x01);

// SendData.data.push_back(0x02);

// SendData.data.push_back(0x04);

// SendData.data.push_back(0x05);

// SendData.data.push_back(0xB7);

// SendData.data.push_back(0xE4);

// SendData.data.push_back(0xF7);

// frame_pub.publish(SendData);

while (ros::ok())

{

len = SendData.data.size();

printf("Send:%02d 0x", len);

for (int i = 0; i < len; i++)

{

// printf("%02X ", SendData.data[i]);

printf("%02X ", SendData.data.at(i));

}

printf("\r\n");

 

send_pub.publish(SendData);

ros::spinOnce();

loop_rate.sleep();

}

return 0;

}

 

　　该节点的作用就是发布特定格式的数据到发送话题上，以及接收上一个节点发布的话题消息；逻辑很简单，其中需要注意的是在添加CRC校验时，添加：C++类，输入类名称后

会有几个选项，如下：

　　　　　　　　　　　　

　　　　之前一直没搞懂他们的区别，都是选的“加入到新的可执行文件中”，如何选这一项，则改类的代码会单独形成一个可执行文件，在运行时需要单独开启，所以如果两个

功能相对独立，则可以选一项，但是如果代码是调用关系，比如上面的CRC类实际上是被datapro调用的，所以这里我们选择第二项会更好，运行时我们只需要驱动datapro就

可以使用CRC类功能；

　　另外Studio工具有一个bug是，添加C++类时，会报错，提示找不到头文件，这问题折腾了很久，后来发现需要把自动生成的头文件，提到上一级目标才可编译通过，即将

头文件直接到include文件夹下。

　　另一个小提醒就是，每次添加新文件或新类时，最好点一下刷新按钮，有时候看不到添加的类文件，需要手动刷新一下才会出来；