基于ROS-Serial和Arduino实现控制六轴舵机机械臂
不久之前我在古月学院上架了《软硬结合带你玩转ROS机械臂》的课程，课程里面我们通过Arduino Nano和ROS的结合实现了机械臂在ROS系统下的驱动。有些细心小伙伴已经加到我的QQ开始更深层次的交流学习，不知道你们有没有发现呢~ 在课程当中，我使用的是基于StandardFirmata协议的驱动，我们给Arduino烧录它的示例程序StandardFirmata，通过Python的pyFirmata库来调用硬件，我们在软件系统实际做的，仅仅只是订阅了joint_state，并且通过pyFirmata的方式来驱动我们的Arduino，Arduino实现控制舵机。 StandardFirmata有着很大的局限性，它对于IO的操作确实很方便，但是涉及到通讯部分的话就很难受。pyFirmata并没有操作串口、IIC、SPI相关的接口，pyFirmata的升级版pyMata增加了超声波、IIC等方式，但是在我使用过后仍觉得较为不便。 我喜欢使用Arduino，它封装的库函数对我这种不懂寄存器的程序猿来说，实在是方便。ROS的官方对于Arduino也是有着库函数支持的，那就是ROS Serial，我们可以在使用Arduino的同时，增加ROS的话题机制来实现数据的交互。 在机械臂的课程当中，大家所使用到的驱动板上面有个OLED屏幕，我们先来聊聊这个屏幕该怎么来使用，怎么在ROS下使用。 我在这里使用的是1.3寸IIC接口的OLED屏幕，大家在这里有个注意点，0.96寸的OLED屏幕使用的是SSD_1306的驱动，而1.3寸的OLED屏幕使用的确实SH_1106的驱动。这些驱动文件大家可以自及网上找到，或者在淘宝上面购买的时候找店家要相关的资料即可。我们先来看看实例程序下的屏幕该怎么来使用。
使用ROS-Serial来驱动OLED屏幕
我在这里使用的是1.3寸的OLED屏幕，使用IIC通讯，驱动为SH_1106。我的想法是这样的，我们在ROS下发布一个String类型的话题，这个话题通过ROS-Serial被Arduino订阅到，然后Arduino把这个String类型的字符串通过IIC显示在OLED屏幕上面。 我们了可以先去淘宝或者github找下Arduino驱动SH_1106的libraries，这个方法比你在百度找资料要高效的很多，当然学习教程什么的，我还是建议你去百度寻找相关的资料。我是直接在github搜索sh_1106的，这个办法很有用。
大家可以找的很多资料，使用哪个大家一定要自己去筛选一下，基本上OLED的话Adafruit的还是挺好使的。大家下载一下这个Adafruit_SH1106并且放到自己Arduino的libraries路径下，打开我们的Arduino IDE，就可以从example里面找到它的实例程序。我们先来看下示例程序的内容。
由于内容过多，我只截取部分代码。大家在这里可以看到，我们通过clearDisplay进行清屏，通过display进行显示，在显示和清屏之间，我们加入我们要显示的内容，比如通过setTextSize设置字体大小、通过setTextColor设置背光情况、通过setCursor设置显示的位置、通过println设置显示的内容等等。当然，我们需要声明OLED在使用IIC时候的总线地址，这个是必须的。 我在这里为大家提供一个Arduino的代码，这个代码实现的功能是订阅一个叫做oled的话题，话题类型是String，将话题的内容在OLED屏幕上面进行显示的功能。我们来看看代码。
#include <ros.h> //引用ros头文件
#include <std_msgs/String.h> //引用std_msgs的String类型的数据
#include <Wire.h> //引用IIC协议库
#include <Adafruit_GFX.h> //引用字体取模库
#include <Adafruit_SH1106.h> //引用SH1106驱动库，1.3寸OLED驱动时SH1106
#define OLED_RESET 4 //定义OLED为4针引脚模式
Adafruit_SH1106 display(OLED_RESET); //实例化OLED显示功能
ros::NodeHandle nh; //声明ros句柄
/********************************************************
*函数名称：oled_callback *
*函数类型：void *
*函数功能：当订阅到oled的话题，在OLED屏幕显示订阅到的msg *
*输入参数：msg，订阅到的数据 *
*输出参数：无 *
*********************************************************/
void oled_callback(const std_msgs::String& msg)
{
display.clearDisplay(); //清空当前屏幕内容
display.setTextSize(1); //设置显示字体的大小
display.setTextColor(WHITE); //设置显示字体的样式
display.setCursor(0,0); //设置显示字体的坐标
display.println(msg.data); //设置显示字体的内容
display.display(); //显示
}
//实例化订阅，订阅的话题名为oled，话题消息类型为std_msgs::String，回调函数为oled_callback
ros::Subscriber<std_msgs::String> sub("oled", &oled_callback );
/**************************
*函数名称：setup *
*函数类型：void *
*函数功能：初始化相关硬件 *
*输入参数：无 *
*输出参数：无 *
**************************/
void setup()
{
display.begin(SH1106_SWITCHCAPVCC, 0x3C); //初始化OLED屏幕，IIC默认地址0x3C
display.display(); //显示
delay(2000); //延时2000ms
display.clearDisplay(); //清空当前屏幕内容
display.setTextSize(1); //设置显示字体的大小
display.setTextColor(WHITE); //设置显示字体的样式
display.setCursor(0,0); //设置显示字体的坐标
display.println("Hello, ROS!"); //设置显示字体的内容
display.display(); //显示
delay(2000); //延时2000ms
display.clearDisplay(); //清空当前屏幕内容
nh.initNode(); //初始化节点
nh.subscribe(sub); //订阅
}
/*********************************
*函数名称：loop *
*函数类型：void *
*函数功能：程序执行内容，循环执行 *
*输入参数：无 *
*输出参数：无 *
**********************************/
void loop()
{
nh.spinOnce(); //在Arduino的loop函数，调用nh.spinOnce()，这样所有的ROS回调函数就会被处理
delay(1); //延时1ms
}
代码由三部分组成，我们先来聊聊setup函数。这块是我们初始化的代码，大家可以看到我先是声明了OLED屏幕所在的IIC的地址0x3C，紧跟着控制屏幕进行一次清屏和显示，在这次清屏和显示之间并没有任何要显示的内容，所以屏幕就不会由什么显示。然后是设定字体大小为1，字体为白色（即白色凸显，大家搞个OLED玩一下就知道了），从(0,0)坐标开始显示，要显示的内容为“Hello ROS！”，然后通过display进行显示。显示两秒后，进行清屏，最后是初始化我们的ROS节点。这里有个问题，可能会让大家以为是bug，在最后我们执行的clearDisplay当中，大家会发现屏幕并没清屏，但是随着订阅到的内容的显示，却可以正常显示。而且我们在开始的使用也用了一组display和clearDisplay，这是为什么呢？欢迎大家留言评论做出自己的理解，我们也会在下一篇文章为大家答疑。 然后是我们的loop部分，这部分在Arduino里面是要循环执行的，其实也就是我们C++实现ROS通讯时候是一样的效果。重要的地方在这里。
//实例化订阅，订阅的话题名为oled，话题消息类型为std_msgs::String，回调函数为oled_callback
ros::Subscriber<std_msgs::String> sub("oled", &oled_callback );
我们通过ros节点示例化一个Subscriber类的变量sub，它的类型是std_msgs::String（即std_msgs下的String类型）。它有两个参数，第一个是订阅的话题的名称，我们在这里订阅的话题叫做oled；第二个是订阅到这个话题时候所执行的函数，也就是回调函数。这里和我们在C++写ROS功能包有个区别，就是没有声明当前的节点名称，这个也是我当时懵逼的一点，这是因为我们在ROS下运行的ROS-Serial的时候，节点是由ROS-Serial功能包声明的，叫做serial_node。其实Arduino终归还是一块单片机，我们只是按照ROS的代码格式来写实现了ROS，在这当中ROS-Serial功能包和ros.h等文件已经为我们处理好了大量的工作。 在oled_callback里面，我们传入的参数是const std_msgs::String& msg，而msg的data内容，就是我们需要显示的。在ROS下，订阅的时候数据都在data里面，这个比较“梗”的问题，也是初学者比较容易困惑的。 接下来的任务就简单了！我们调用OLED屏幕显示这一块的功能，把msg的data变量在屏幕显示，就可以实现功能。我们将成宿下载好，运行roscore和rosserial节点，发布一个oled的话题，后面跟着字符串，就可以实现功能。 最最最后，咳咳，拿到我机械臂板子的小伙伴应该直接尝试了，但是悲剧的事情来了~ Arduino Nano的内存不够，程序烧录不进去，这简直是何等的卧槽~ 当我的发现这个Bug的时候，我的内心已经无法用言语来形容和描述了~
当然，这个程序在Arduino Mega上面使用还是没问题的。我猜测可能是ROS的库+字体库导致的内存不足，也有可能是其他的原因。
使用ROS-Serial来驱动机械臂
课程最初的设计就是使用Arduino Nano来搭建一个低成本的ROS Moveit的机械臂，在这里我也建议大家买总线舵机试试自己写一下驱动（也可以在古月居后台回复获取资料，我基于幻尔的机械臂做了ROS的驱动功能包）。在这里，我们逐步来实现使用ROS-Serial来驱动我们的机械臂。 在机械臂课程当中我们提到，我们需要来订阅一个joint_states的话题，通过里面的position获取当前joint的弧度值，最后由舵机执行。这是我写的Arduino驱动代码。
#include "Servo.h" //引用Arduino舵机驱动头文件
#include <ros.h> //引用ros头文件
#include <sensor_msgs/JointState.h> //引用sensor_msgs的JointState类型的数据
ros::NodeHandle nh; //声明ROS句柄
//实例化舵机对象
Servo joint1;
Servo joint2;
Servo joint3;
Servo joint4;
Servo joint5;
Servo joint6;
/****************************************************************************
*函数名称：arm_callback *
*函数类型：void *
*函数功能：当订阅到joint_states话题时，将该话题的positio数据内容给舵机执行 *
*输入参数：data，当前订阅到的joint_states的内容 *
*输出参数：无 *
****************************************************************************/
void arm_callback( const sensor_msgs::JointState& data )
{
//joint_states的position内容是对应joint的弧度值，position的0对应我们机械臂的90°
//-1.57~1.57（弧度值）对应0°~180°（角度值），所以有了下面的计算
//由于计算的结构取整可能会超过0~180的范围，所以用了if...else if ...
int joint1_angle = data.position[0]*360/6.28+90;
if(joint1_angle<0)
joint1_angle = 0;
else if(joint1_angle>180)
joint1_angle = 180;
joint1.write(joint1_angle);
int joint2_angle = data.position[1]*360/6.28+90;
if(joint2_angle<0)
joint2_angle = 0;
else if(joint2_angle>180)
joint2_angle = 180;
joint2.write(joint2_angle);
int joint3_angle = data.position[2]*360/6.28+90;
if(joint3_angle<0)
joint3_angle = 0;
else if(joint3_angle>180)
joint3_angle = 180;
joint3.write(joint3_angle);
int joint4_angle = data.position[4]*360/6.28+90;
if(joint4_angle<0)
joint4_angle = 0;
else if(joint4_angle>180)
joint4_angle = 180;
joint4.write(joint4_angle);
int joint5_angle = data.position[5]*360/6.28+90;
if(joint5_angle<0)
joint5_angle = 0;
else if(joint5_angle>180)
joint5_angle = 180;
joint5.write(joint5_angle);
int joint6_angle = data.position[6]*360/6.28+90;
if(joint6_angle<0)
joint6_angle = 0;
else if(joint6_angle>180)
joint6_angle = 180;
joint6.write(joint6_angle);
}
//实例化订阅，订阅的话题名为joint_states，话题消息类型为sensor_msgs::JointState，回调函数为arm_callback
ros::Subscriber<sensor_msgs::JointState> sub("joint_states", &arm_callback );
/****************************
*函数名称：setup *
*函数类型：void *
*函数功能：初始化相关硬件 *
*输入参数：无 *
*输出参数：无 *
****************************/
void setup()
{
nh.initNode(); //初始化节点
nh.subscribe(sub); //订阅
joint1.attach(3); //joint1在D3引脚
joint2.attach(5); //joint2在D5引脚
joint3.attach(6); //joint3在D6引脚
joint4.attach(9); //joint4在D9引脚
joint5.attach(10); //joint5在D10引脚
joint6.attach(11); //joint6在11引脚
pinMode(13,OUTPUT); //设置D13引脚为输出模式
//D13引脚在Arduino上面默认有个LED
//在我们的驱动板当中D13有连接了一个蜂鸣器
//这里的功能是开机提示
digitalWrite(13,HIGH);
delay(500);
digitalWrite(13,LOW);
delay(500);
digitalWrite(13,HIGH);
delay(500);
digitalWrite(13,LOW);
delay(500);
digitalWrite(13,HIGH);
delay(500);
digitalWrite(13,LOW);
delay(500);
}
/************************************
*函数名称：loop *
*函数类型：void *
*函数功能：程序执行内容，循环执行 *
*输入参数：无 *
*输出参数：无 *
************************************/
void loop()
{
nh.spinOnce(); //在Arduino的loop函数，调用nh.spinOnce()，这样所有的ROS回调函数就会被处理
delay(1); //延时1ms
}
关于setup和loop的内容我就不再重复的叙述，我们直接来看订阅的初始化部分和回调函数。
//实例化订阅，订阅的话题名为joint_states，话题消息类型为sensor_msgs::JointState，回调函数为arm_callback
ros::Subscriber<sensor_msgs::JointState> sub("joint_states", &arm_callback );
这里和我们上面的一样的，只是订阅的话题是joint_states，类型是sensor_msgs::JointState，回调函数为arm_callback。 然后是我们arm_callback回调函数的内容，如下。
/****************************************************************************
*函数名称：arm_callback *
*函数类型：void *
*函数功能：当订阅到joint_states话题时，将该话题的positio数据内容给舵机执行 *
*输入参数：data，当前订阅到的joint_states的内容 *
*输出参数：无 *
****************************************************************************/
void arm_callback( const sensor_msgs::JointState& data )
{
//joint_states的position内容是对应joint的弧度值，position的0对应我们机械臂的90°
//-1.57~1.57（弧度值）对应0°~180°（角度值），所以有了下面的计算
//由于计算的结构取整可能会超过0~180的范围，所以用了if...else if ...
int joint1_angle = data.position[0]*360/6.28+90;
if(joint1_angle<0)
joint1_angle = 0;
else if(joint1_angle>180)
joint1_angle = 180;
joint1.write(joint1_angle);
int joint2_angle = data.position[1]*360/6.28+90;
if(joint2_angle<0)
joint2_angle = 0;
else if(joint2_angle>180)
joint2_angle = 180;
joint2.write(joint2_angle);
int joint3_angle = data.position[2]*360/6.28+90;
if(joint3_angle<0)
joint3_angle = 0;
else if(joint3_angle>180)
joint3_angle = 180;
joint3.write(joint3_angle);
int joint4_angle = data.position[4]*360/6.28+90;
if(joint4_angle<0)
joint4_angle = 0;
else if(joint4_angle>180)
joint4_angle = 180;
joint4.write(joint4_angle);
int joint5_angle = data.position[5]*360/6.28+90;
if(joint5_angle<0)
joint5_angle = 0;
else if(joint5_angle>180)
joint5_angle = 180;
joint5.write(joint5_angle);
int joint6_angle = data.position[6]*360/6.28+90;
if(joint6_angle<0)
joint6_angle = 0;
else if(joint6_angle>180)
joint6_angle = 180;
joint6.write(joint6_angle);
}
这部分的代码就是我在StandardFirmata实现的代码，只是在驱动执行的位置发生变化——由协议的write变成了Arduino的write。我们的数据还是data，只是在这里提的是data里面的position。
咳咳，大家放心，这个代码是可以在Arduino Nano运行的~ 所以我觉得可能就是OLED的字体库太大了，造成的内存不足~
使用ROS-Serial来驱动MPU6050
咳咳，最近开学了还没写，一定出一定出！！！ 在接下来的时间里，我也会就Modbus的数据格式，来给大家继续聊聊ROS的驱动层开发。我从18年跟着古月老师的星火计划开始学习ROS，当时刚刚接触到树莓派和Linux系统，很多内容简直就是天书。我也很庆幸当时参加了星火计划的课程，从这里开始我的机器人生涯~ 即是生涯，也是深渊。搞硬件的就是比较烧钱，这是无可避免的，大学期间为学习的投资才是最好的投资。当时也觉得ROS啃不下来，这不是我这个萌新该接触的等级，对于发展方向和即将要学习的内容一片迷茫~ 当时看到了YY硕的一篇文章机器人工程师学习计划，个人认为这篇文章对现在正迷茫困惑、想要放弃的小伙伴来说，绝对是一盏指路明灯。机器人是一个综合性的学科，机械、电气、单片机、嵌入式、通讯、前端、后端、运维、人工智能，甚至还特别的吃数学和英语，正如同YY说说的，机器人学的东西至少是全栈开发（程序员里面牛掰的存在，可以自行百度这个职位）的三倍。你的方向可以是是机器人，但是你的就业方向可能不是机器人，但是有这些知识储备的你，一定存在着绝对的竞争力吧~
