使用moveit控制真实机械臂

说明：本人控制四自由度机械臂，前面的工作在索引中查看，目前已有实物和实现底层驱动，采用ros系统，通过串口通讯实现了机械臂运动。使用了《古月居》的开源代码，参考《古月居》开源机械臂文件配置。

1.索引：

1. https://blog.csdn.net/sxl_124/article/details/124648968(solidworks导出urdf)
2. https://blog.csdn.net/sxl_124/article/details/126044566(在ROS创建moveit模型)

2.准备：实物（ROS系统、STM32、机器人）、底层驱动代码，串口通讯代码、《古月居》机械臂开源代码。

3.参考：

串口通信：

ROS机器人STM32和ROS通信

【STM32】STM32与PC端、HC-06、ROS进行USART串口通信_K.Fire浑身是肝的博客-CSDN博客_stm32与pc串口通信

moveit配置：

机器人学习必看系列：如何使用moveit控制真实机械臂？_鱼香ROS的博客-CSDN博客_moveit控制真实机械臂

Moveit--控制真实机械臂_cookie909的博客-CSDN博客_topp算法

Simple_MoveIt_Demo: ABB机器人模型，作为MoveIt驱动真实机器人的简洁教程 - Gitee.com（直接在开源项目基础上改）

目录

一、串口通讯实现

二、文件配置

三、代码实现

---

一、串口通讯实现

主要在别人的代码基础上修改。

原理：获取moveit的轨迹路点，然后发送给下位机，下位机执行再将执行的脉冲返回给ROS，随后发布关节状态。

.h文件

#ifndef MBOT_LINUX_SERIAL_H
#define MBOT_LINUX_SERIAL_H

#include <ros/ros.h>
#include <ros/time.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <boost/asio.hpp>
#include <geometry_msgs/Twist.h>

extern void serialInit();
extern void writeSpeed(float w_Servo1, float w_Servo2,float w_Servo3,float w_Servo4,unsigned char w_pump);
extern bool readSpeed(float &dof1,float &dof2,float &dof3,float &dof4 ,unsigned char &walk);
unsigned char getCrc8(unsigned char *ptr, unsigned short len);

#endif

.c文件

#include "wcr_driver/mbot_linux_serial.h"

using namespace std;
using namespace boost::asio;
//串口相关对象
boost::system::error_code err;
boost::asio::io_service iosev;
boost::asio::serial_port sp(iosev);

/********************************************************
            串口发送接收相关常量、变量、共用体对象
********************************************************/
const unsigned char ender[2] = {0x0d, 0x0a};
const unsigned char header[2] = {0x55, 0xaa};

//发送左右轮速控制速度共用体
union sendData
{
	float d;
	unsigned char data[4];
}Servo1,Servo2,Servo3,Servo4;

//接收数据角度共用体（-32767 - +32768）
union receiveData
{
	float d;
	unsigned char data[4];
}angle1,angle2,angle3,angle4;

/********************************************************
函数功能：串口参数初始化
入口参数：无
出口参数：
********************************************************/
void serialInit()
{
    sp.open("/dev/ttyUSB0", err);
    if(err){
        std::cout << "Error: " << err << std::endl;
        std::cout << "请检查您的串口/dev/ttyUSB0，是否已经准备好：\n 1.读写权限是否打开（默认不打开) \n 2.串口名称是否正确" << std::endl;
        return ;
    }
    sp.set_option(serial_port::baud_rate(115200));
    sp.set_option(serial_port::flow_control(serial_port::flow_control::none));
    sp.set_option(serial_port::parity(serial_port::parity::none));
    sp.set_option(serial_port::stop_bits(serial_port::stop_bits::one));
    sp.set_option(serial_port::character_size(8));    
}

/********************************************************
函数功能：将对机器人的脉冲打包发送给下位机
入口参数：舵机脉冲
出口参数：
********************************************************/
void writeSpeed(float w_Servo1, float w_Servo2,float w_Servo3,float w_Servo4,unsigned char w_pump)
{
    unsigned char buf[23] = {0};//
    int i, length = 0;

    Servo1.d  =  w_Servo1;//mm/s
    Servo2.d  =  w_Servo2;
    Servo3.d  =  w_Servo3;
    Servo4.d  =  w_Servo4;

    // 设置消息头
    for(i = 0; i < 2; i++)
        buf[i] = header[i];             //buf[0]  buf[1]
    
    //舵机脉冲
    length = 17;//只算发送的数据，不算size位
    buf[2] = length;                    //buf[2]
    for(i = 0; i < 4; i++)
    {
        buf[i + 3] = Servo1.data[i];  //buf[3] buf[4] buf[5] buf[6]
        buf[i + 7] = Servo2.data[i];  //buf[7] buf[8] buf[9] buf[10]
        buf[i + 11] = Servo3.data[i];  //11~14
        buf[i + 15] = Servo4.data[i];  //15~18
    }
    buf[19]=w_pump;//19

    // 设置校验值、消息尾
    buf[3 + length] = getCrc8(buf, 3 + length);//buf[20]
    buf[3 + length + 1] = ender[0];     //buf[21]
    buf[3 + length + 2] = ender[1];     //buf[22]

    // 通过串口下发数据
    boost::asio::write(sp, boost::asio::buffer(buf));
}
/********************************************************
函数功能：从下位机读取数据
入口参数：舵机脉冲
出口参数：bool
********************************************************/
bool readSpeed(float &dof1,float &dof2,float &dof3,float &dof4, unsigned char &walk)
{
    char i, length = 0;
    unsigned char checkSum;
    unsigned char buf[150]={0};
    //=========================================================
    //此段代码可以读数据的结尾，进而来进行读取数据的头部
    try
    {
        boost::asio::streambuf response;
        boost::asio::read_until(sp, response, "\r\n",err);   
        copy(istream_iterator<unsigned char>(istream(&response)>>noskipws),
        istream_iterator<unsigned char>(),
        buf); 
    }  
    catch(boost::system::system_error &err)
    {
        ROS_ERROR("read_until error");
    } 
    //=========================================================        

    // 检查信息头
    if (buf[0]!= header[0] || buf[1] != header[1])   //buf[0] buf[1]
    {
        ROS_ERROR("Received message header error!");
        return false;
    }
    // 数据长度
    length = buf[2];                                 //buf[2]

    // 检查信息校验值
    checkSum = getCrc8(buf, 3 + length);             //buf[20] 计算得出
    if (checkSum != buf[3 + length])                 //buf[20] 串口接收
    {
        ROS_ERROR("Received data check sum error!");
        return false;
    }    

    // 读取速度值
    for(i = 0; i < 4; i++)
    {
        angle1.data[i]    = buf[i + 3]; //buf[3] buf[4] buf[5] buf[6]
        angle2.data[i]    = buf[i + 7]; //buf[7] buf[8] buf[9] buf[10]
        angle3.data[i]    = buf[i + 11]; //11～14
        angle4.data[i]    = buf[i + 15]; //15～18
    }
    walk   =buf[19];
    dof1   =angle1.d;
    dof2   =angle2.d;
    dof3   =angle3.d;
    dof4   =angle4.d;

    return true;
}
/********************************************************
函数功能：获得8位循环冗余校验值
入口参数：数组地址、长度
出口参数：校验值
********************************************************/
unsigned char getCrc8(unsigned char *ptr, unsigned short len)
{
    unsigned char crc;
    unsigned char i;
    crc = 0;
    while(len--)
    {
        crc ^= *ptr++;
        for(i = 0; i < 8; i++)
        {
            if(crc&0x01)
                crc=(crc>>1)^0x8C;
            else 
                crc >>= 1;
        }
    }
    return crc;
}

调用代码（我这里加了其它控制，要用可以自行修改）

serialInit()

writeSpeed(servo1 , servo2 , servo3 , servo4, pump);

readSpeed(Rece1,Rece2,Rece3,Rece4,walk);

使用方法

查看串口

ls -l /dev/ttyUSB*

 使能串口（ls -l /dev/ttyUSB*执行后显示USB1，则为ttyUSB1）

 sudo chmod 777 /dev/ttyUSB0

二、文件配置

这里只写注意事项，最有效的方法直接上别人的开源：Simple_MoveIt_Demo: ABB机器人模型，作为MoveIt驱动真实机器人的简洁教程 - Gitee.com

文件夹名称应符合大家的共识，养成良好习惯。

1.修改urdf文件夹

        本人要从导出的文件名后加上_description，需要更改几个文件内容。

display.launch

<launch>
  <arg
    name="model" />
  <arg
    name="gui"
    default="True" />
  <param
    name="robot_description"
    textfile="$(find wcr_description)/urdf/wcr.urdf" /><!--修改此处_description-->
  <param
    name="use_gui"
    value="$(arg gui)" />
  <!--<node
    name="joint_state_publisher"
    pkg="joint_state_publisher"
    type="joint_state_publisher" />
  <node
    name="robot_state_publisher"
    pkg="robot_state_publisher"
    type="state_publisher" />
  <node
    name="rviz"
    pkg="rviz"
    type="rviz"
    args="-d $(find wcr)/urdf.rviz" /-->
</launch>

.urdf

包含以下形式的所有代码

      <geometry>
        <mesh
          filename="package://wcr_description/meshes/base_link.STL" />
      </geometry>

CMakeLists.txt

project(wcr_description)

package.xml 

  <name>wcr_description</name>

2.moveit配置文件

config文件夹不用配置

launch文件夹具体配置参考开源机械臂（几乎都要改，不改连通不了控制器），这里只放注意事项。C++代码要和相应的配置文件对应

代码：

    joint_pub_ = nh_.advertise<sensor_msgs::JointState>("/wcr/joint_states", 1);

demo.launch

  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
    <param name="/use_gui" value="$(arg use_gui)"/>
    <rosparam param="source_list">[/wcr/joint_states]</rosparam>
  </node>

代码：

    WcrRobot robot("wcr_controller/follow_joint_trajectory");

ros_controller.yaml

controller_list:
  - name: wcr/wcr_controller
    action_ns: follow_joint_trajectory

三、代码实现

看开源机械臂代码：Simple_MoveIt_Demo: ABB机器人模型，作为MoveIt驱动真实机器人的简洁教程 - Gitee.com

GitHub - Xueming10wu/Simple_MoveIt_Demo at kawasaka_7axies