ROS2中用MoveIt2控制自己的舵机机械手（5）

1.前言

在上一篇【ROS2中用MoveIt2控制自己的舵机机械手（4）】，我们已经实现了上下位机的通讯。由于目前使用的是舵机，MCU(stm32)是无法从舵机那里读取到当前的角度的，因此下位机发给上位机的协议暂时没必要实现，暂时只用printf来调试就够了。
这一篇，我们来实现整个系统的调试与运行。

2.串口连接至虚拟机

我这边用的是虚拟机VMware里面运行了个Ubuntu22.04，因此串口需要连接到虚拟机里面。我用的是usb转串口（CH340芯片），在虚拟机开启的情况下，将串口插入电脑时，虚拟机会提示是否接入到虚拟机，选择接入虚拟机即可。

但是接入后，假如打开我们自己写的Qt程序来试图打开串口，会发现枚举不到我们的串口，此时需要按照这个来进行操作：【Ubuntu22.04 CH340系列串口驱动（没有ttyUSB）问题解决方案】

sudo apt remove brltty

此时重新拔插就可以枚举到这个ttyUSB了。

但是此时假如选择打开这个串口的话，会报[permission denied]的错误，无法打开。这个问题可以参考这里解决：【ubuntu 打开串口时permission denied】

3.MoveIt、Rviz、机器人状态节点

参考 【在ROS2中，通过MoveIt2控制Gazebo中的自定义机械手】、【ROS2中用MoveIt2控制自己的舵机机械手（1）】，利用我们从SolidWorks导出的模型构建基本的MoveIt环境。

4.机器人节点

这次我们的机器人节点是用Qt调用ROS库编写的，详情请参考【ROS2中用MoveIt2控制自己的舵机机械手（1）】，【ROS2中用MoveIt2控制自己的舵机机械手（4）】.
我们创建了一个类MyActionRobot ，里面会发布Action用于对接Moveit，同时会把接收到的MoveIt发送过来的路径点位信息发射出去

再将该信号连接到串口槽函数中，将点位通过串口发送给下位机，让下位机PCA9685驱动电机。

5.效果

整个系统的运行视频可以看这里：【ROS2中用MoveIt2控制自己的舵机机械手】

6.工程代码

整个项目的代码我放在了百度网盘，有兴趣的同学可以下载下来对比博客来慢慢看。
(在上位机代码里，可以看到有一些joint的旋转方向需要翻转，后来我继续研究了一下，发现可以在urdf中进行修正。改正后就不需要在代码里翻转了。各位弄自己的模型时，一定要注意这个问题，否则后面标定的话，会乱套。
同时，solidworks里面马达坐标系的轴方向最好是全部一致，比如都是长边为y、短边为x，z轴朝外。

)

网址：https://pan.baidu.com/s/1oTOQ6x-VMjijK7c428DuKQ，提取码：rjdz。
里面一共三个文件：
MachineArm.zip、myArmTest.zip、myClaw.zip

MachineArm.zip是个stm32cubeIDE的工程，解压后可以用stm32cubeIDE打开。假如用的是别的下位机MCU（51，arduino或者其他的），可以只参考里面的 MachineArm\Core\Inc，Src路径下的pca9685文件。

myArmTest.zip是个Qt工程，我用的Qt版本是Qt5.15.2。里面实现了与下位机的串口通讯+与MoveIt的Action通讯。
代码myactionrobot.cpp中的85行应该为
int64_t duration = time_to_sleep.nanoseconds();
因为这个time_to_sleep是个duration，他的nanoseconds() 、seconds()是同样的意义的，而不是像前面的 time_from_start那样是分开统计汇总的。。。。
另外，myactionrobot.cpp的148行也要改一下。还是这个nanoseconds() 、seconds()的问题。
double timeSec = this->now().seconds(); qint32 sec = timeSec; jointStates.header.stamp.sec = sec; jointStates.header.stamp.nanosec = (timeSec - sec) * 1e9;

myClaw.zip里面是ROS2工程，里面有两个项目：claw_description以及arm_claw。claw_description用来提供urdf文件，arm_claw是用moveit_setup_assistant配合claw_description的urdf生成的项目。
假如想要启动整个项目（下位机假如需要你们用的不是stm32，需要自己根据实际情况调试一下），可以按照以下步骤执行。

6.1上位机启动

a、连接好串口。
b、启动Qt程序：myArmTest，生成机器人节点。

c、cd到myClaw。
d、执行编译

colcon build

e、soure一下，目的是将本目录下的install目录下的东西（的路径）告知ROS系统，然后才可以利用ros2的各种指令对我们的项目进行操作：

source install/setup.bash

f、执行以下命令，启动RobotStates节点

ros2 launch arm_claw my_claw.launch.py

g、重新打开一个命令行，也cd到myClaw目录，也source一下，然后执行以下命令，启动MoveIt

ros2 launch arm_claw my_moveit_rviz.launch.py

如无意外的话，上位机就启动完了。
执行规划的话，还可以在Qt那边看到输出：

6.2 下位机相关

下位机与上位机的通讯是通过串口实现的。下位机目前相当于是单方面接收数据，并没有给上位机发送与轨迹规划有关的数据。所以理论上，这是个开环的控制系统，不要下位机也不影响我们调试上位机的。
但是假如需要下位机执行上位机给出的指令，从而驱动机械手的话，只需要对上位机发送下去的5个角度值做文章就行。

7.总结

假如需要跑完整个项目，是需要一定的基础的。推荐看一下鱼香ROS的ROS2教程【动手学ROS2】，我也是从他的教程入门的。
该机械手在执行MoveIt规划的路径时，会一卡一卡的，主要是因为两方面的原因：1.机械结构松散、2.执行路径时没有进行插补。主要原因是插补、反馈问题，后面再慢慢完善了。
总之，暂时告一段落了。