在ROS下的Arduino舵机机械臂控制

经过前面对ROS和Arduino的学习与例程编写，又用了一些时间学习了URDF和Moveit，结合之前所学的内容编写了程序以实现ROS系统对Arduino的通信控制。

从SW导出URDF模型

利用Solidworks三维建模 ，导出URDF文件可以参考合工大的这篇文章，值得注意的是如果需要在机械臂末端添加摄像头即eye in hand系统的话，还要再新建一个link使其固定于上一个link中，便于后续摄像头坐标系与机械臂末端坐标系的重合。如下图所示：

此外建立URDF模型所使用的坐标系和基准轴需要改为英文标注，否则会导致urdf文件生成错误。此处导出的是ros功能包文件夹，最好使用小写字母与下划线组合的方式命名，将文件夹拷贝至ros工作空间的src文件夹内，需要对工作空间进行catkin_make才可以使用Moveit!助手生成Moveit!的文件。

Moveit!的学习与使用

使用助手生成配置文件之前需要在src文件夹下创建新的文件夹用于存储生成的文件，使用MoveIt配置助手生成MoveIt配置文件可以参考这篇文章，如果出现无法识别urdf文件的情况需要检查上节所提到的坐标系和基准轴命名，文件夹的命名和是否对所在的工作空间进行catkin_make。如果没有将工作空间加入source，则需要在建立新终端的时候source一遍才可以使用，这里建议参考这篇文章永久设置source一劳永逸。使用助手生成配置文件之前需要在src文件夹下创建新的文件夹用于存储生成的文件。

建立完Moveit!配置文件后可以使用以下命令查看urdf文件是否可用，假设配置文件夹名称为moveit_config：

roslaunch moveit_config demo.launch

运行后会弹出包含着机械臂模型和moveit功能的rviz界面，此时点击左下角功能框的planning可以生成随机坐标，并求出当前位置到目标位置的逆解，如下图所示：

点击execute可以将关节信息（角度速度和力矩）输出到/joint states话题中，下面将利用这个话题中的数据对实际机械臂进行控制。可以利用rostopic下echo和info等命令查询关节信息的内容发布者和发布频率等，如下图所示：

ROS控制现实机械臂

如何将/joint states中的数据传输到Arduino开发板中是这个任务的重要环节，这里我为了简化开发流程采用的是串口通信的方式，传输“$F1000T1000!”这样的字符串使得第六个关节的PWM信号为1000，“T1000”代表从当前位置运动到pwm1000所需要的时间为1s，如要提高机械臂的精度此处PWM信号与舵机旋转角度需要做标定，此处为了快速搭建平台默认关节1-5舵机的旋转角度为270°即±135°，关节6舵机旋转角度为180°。我这里采用的是发布和订阅的通信方式，考虑到数据传输的实时性应该采用服务端与客户端的方法更加。

先建立PWM.msg用于传递PWM信号，由于uint8的范围是0-256且PWM信号为500-2500，所以此处使用uint16的数据类型，代码如下：

PWM.msg

uint16 a
uint16 b
uint16 c
uint16 d
uint16 e
uint16 f

随后编写get_data程序获取joint_states中的位置信息，并对其进行转化发布PWM消息，代码如下：

get_data.cpp

#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <myserial/PWM.h>

ros::Publisher pub;
int pwm[6];

//回调函数处理jointstate中position数据，且发布pwm消息
void dataCallback(const sensor_msgs::JointState joint){
    myserial::PWM pwm_data;
    pwm[0]=(1000000/2355)*(-1)*joint.position[0]+1500;
    pwm[1]=(1000000/2355)*(-1)*joint.position[1]+1500;
    pwm[2]=(1000000/2355)*joint.position[2]+1500;
    pwm[3]=(1000000/2355)*joint.position[3]+1500;
    pwm[4]=(1000000/2355)*(-1)*joint.position[4]+1500;
    pwm[5]=(100000/157)*(-1)*joint.position[5]+1500;
    for(int i=0;i < 6;i++){    
        ROS_INFO("joint%d:data is %d!",(i+1),pwm[i]);
    }
    pwm_data.a=pwm[0];
    pwm_data.b=pwm[1];
    pwm_data.c=pwm[2];
    pwm_data.d=pwm[3];
    pwm_data.e=pwm[4];
    pwm_data.f=pwm[5];
    pub.publish(pwm_data);
}

int main(int argc, char** argv){
    //初始化节点
    ros::init(argc, argv, "get_data");
    //创建句柄
    ros::NodeHandle nh;
    //订阅moveit发布的关节信息，发布处理后的pwm信息
    pub = nh.advertise<myserial::PWM>("/pwm",100);
    ros::Subscriber sub = nh.subscribe("/joint_states",10,dataCallback);
    ros::spin();
    return 0;
}

最后根据所学的串口知识获取get_data所传输出来的PWM消息，串口通信的规则延续了机械臂的初始规则，如果对开发板的串口通信规则进行更改，可以优化串口之间传输的数据大小，具体代码如下：

serial_port.cpp

#include <ros/ros.h>
#include <serial/serial.h>
#include <iostream>
#include <myserial/PWM.h>

//创建一个全局serial类
serial::Serial sp;

void pwmCallback(const myserial::PWM& msg){
    char temp[72];
    sprintf(temp,"$A%dT0100!$B%dT0100!$C%dT0100!$D%dT0100!$E%dT0100!$F%dT0100!",msg.a,msg.b,msg.c,msg.d,msg.e,msg.f);
    std::string data1 = temp;
    ROS_INFO_STREAM("Writing to serial port:" << data1);
    sp.write(data1);
}

int main(int argc, char** argv)
{
    //初始化节点
    ros::init(argc, argv, "serial_port");
    //创建句柄
    ros::NodeHandle nh;
    //创建timeout
    serial::Timeout to = serial::Timeout::simpleTimeout(1000);
    //设置要打开的串口名称
    sp.setPort("/dev/ttyUSB0");
    //设置串口通信的波特率
    sp.setBaudrate(115200);
    //串口设置timeout
    sp.setTimeout(to);
 
    try
    {
        //打开串口
        sp.open();
    }
    catch(serial::IOException& e)
    {
        ROS_ERROR_STREAM("Unable to open port.");
        return -1;
    }
    
    //判断串口是否打开成功
    if(sp.isOpen())
    {
        ROS_INFO_STREAM("/dev/ttyUSB0 is opened.");
    }
    else
    {
        return -1;
    }
    
    //订阅转换后的PWM信息
    ros::Subscriber sub = nh.subscribe("/pwm",10,pwmCallback);

    //查询一次PWM话题队列
    //ros::spinOnce();
    
    //循环查询回调函数
    ros::spin();
    
    //关闭串口
    sp.close();
 
    return 0;
}

以上所有的发布和订阅需要频率相同，且机械臂的运动指令也需要与频率相同，此处频率为10则需要T0100作为运动时间信号，最后完成cmakelist文件的配置后，返回工作空间catkin_make，若没有报错说明程序编写正确。此处具体编译过程参考古月老师的ROS21讲内容。最后使用以下命令进行实验：

roslaunch moveit_confit demo.launch
rosrun package_name get_data
rosrun package_name serial_port

通过rviz中的控制组件，execute后现实的电机会像屏幕显示的一样做逆解运动，如下所示。

ROS控制现实舵机机械臂

结语

正常的流程应该是先在gazebo中做仿真模拟，随后可以将仿真所得转移到实际机械臂中，但由于舵机机械臂危险度相对较低且时间紧迫，这里直接将Moveit的控制映射到实际机械臂中。在运行代码的时候最好先试一个舵机的效果，若没问题则可以将所有线接好进行实验。后续会对realsense相机的使用进行学习，尽快搭好控制算法所需要的平台。

~转载注明出处@帕里亚