Dofbot机械臂从零部署笔记（2）——ROS之操作实机实时控制机械臂每个关节转动

上节从零试着自己创建了一遍URDF模型，配置了MoveIt，目的是方便给机械臂做轨迹规划。
不过这些都是ROS系统中对机械臂运动的规划模拟，我们先试着把机械臂跑起来！

开发环境和Arm_Lib库

出厂系统中已经为我们部署好了集成开发环境——JupyterLab，直接使用Python来编写机械臂程序。
打开浏览器输入：localhost:8888或者在同一路由器的电脑中输入ip地址:8888进行远程开发：

并且，出厂系统中已经编写好了Python的实机控制库——Arm_Lib，我们可以直接拿来用！
Arm_Lib.py文件在 Home/Dofbot/0.py_install/Arm_Lib 目录下。
比如上图中：

from Arm_Lib import Arm_Device
Arm = Arm_Device()

就实例化了一个机械臂对象，对象可以用各种方法对机械臂进行操作控制，比如Arm.Arm_serial_servo_write6(90, 70, 150, 90, 90, 180, 3000)就可以让机械臂在3000毫秒内运动到6个指定角度。
更多方法可以参考Arm_Lib.py源码和提供给我们的实例进行学习。在 Home/Dofbot文件夹下还有好多使用Arm_Lib驱动机械臂的实例供参考学习。这里不详述了。

使用ROS操作实机——实时控制机械臂每个关节转动

我们先启动上一节部署好的moveit的demo（可以不需要打开rviz）。

roslaunch xiaok_moveit_config demo.launch

我们先临时新建一个终端，输入rostopic list可以看到有个名为/joint_states的topic；
我们接着输入：

rostopic echo /joint_states

这条命令的意思是，显示发送给指定话题的消息。接着会看到：

屏幕中会不停地更新显示发送给/joint_states的msg，里面包含了当前时刻moveit程序中joint的状态。
那么我们就可以利用这个msg，创建一个订阅者Subcriber，接收这个topic中的msg，在回调函数中调用Arm_Lib的Arm_serial_servo_write6_array(joints, time)方法，把msg中各个joint角度的信息传给这个方法，实现实时与moveit中的机械臂同步运动。

我们再试着在终端输入rostopic info /joint_states，可以看到该话题的Publisher之一是joint_state_publisher_gui，就是那个可以调整各个关节角度的小窗口。
（顺便我们还可以看到订阅者Subscribers之一是/move_group，这是moveit相关的topic群的命名空间，可以看到做运动规划也是必须要订阅这个/joint_states关节状态信息的。）

那么我们现在知道可以直接通过moveit程序中joint_state_publisher_gui中各个角度的滚动条来进行角度变换。这样就实现了实时控制机械臂每个关节转动！

程序代码实现

打开JupyterLab，新建一个记事本，输入代码：
（程序源码位于：/home/jetson/dofbot_ws/src/dofbot_moveit/scripts/00_dofbot_move.py）

import rospy
import Arm_Lib
from math import pi
from sensor_msgs.msg import JointState

# 弧度转角度
RA2DE = 180 / pi


def topic(msg):
    # 如果不是该话题的数据直接返回
    if not isinstance(msg, JointState): return
    # 定义关节角度容器,最后一个是夹爪的角度,默认夹爪不动为90.
    joints = [0.0, 0.0, 0.0, 0.0, 0.0, 90.0]
    # 将接收到的弧度[-1.57,1.57]转换成角度[0,180]
    for i in range(5): joints[i] = (msg.position[i] * RA2DE) + 90
    # 调驱动函数
    sbus.Arm_serial_servo_write6_array(joints, 100)


if __name__ == '__main__':
    sbus = Arm_Lib.Arm_Device()
    # ROS节点初始化
    rospy.init_node("ros_dofbot")
    # 创建一个订阅者
    subscriber = rospy.Subscriber("/joint_states", JointState, topic)
    # 设置循环的频率
    rate = rospy.Rate(2)
    # 按照循环频率延时
    rospy.spin()

点击运行，就可以拖动各个角度来实时控制机械臂的关节了！

我们不妨这时再看看rostopic info /joint_states：

看到Subscribers多了一个/ros_dofbot，“ros_dofbot”就是我们创建的节点名。

结束记得点终止运行，并关闭Kernel（Kernel → Shut Down Kernel）。