如何给机械臂添加hande机械爪并应用于moveit

首先需要利用moveit的初始启动软件生成带有机械臂的URDF文件，此时打开moveit会发现机械爪为红色，无法控制机械臂。因为机械爪的话题没有发给moveit，提示没有 hande_left_finger_joint。按照如下步骤解决：

1、从源码下载hande机械爪文件:

cd ~/your_ws/src
git clone https://github.com/macs-lab/robotiq_hande_ros_driver.git
cd …
catkin_make

2、更改srv文件

进入文件夹 robotiq_hande_ros_driver/srv/gripper_service.srv
将内容更改为：

int32 position
int32 speed
int32 force
---
int32 response`

3、更改gripper_node.py

主要解决问题：moveit无法接收hande的关节状态信息
接收moveit发出的joint_state话题，将其增加hande_left_finger_joint信息后重新发出。
我这里将机械臂启动launch文件发布的机械臂关节话题名称改为joint_states_ur，重新以joint_states话题发布出去
源码如下：

#!/usr/bin/env python

import rospy 
from std_msgs.msg import Int32
import robotiq_gripper
from robotiq_hande_ros_driver.srv import gripper_service, gripper_serviceResponse
from sensor_msgs.msg import JointState
import threading
import time

class HandEGripper:
    def __init__(self):
        rospy.init_node("hand_e_gripper_node", anonymous=False)
        # get the IP
        ip = rospy.get_param('~robot_ip')
        # initialize the gripper
        self.gripper = robotiq_gripper.RobotiqGripper()
        rospy.loginfo("Connecting to the gripper.....")
        self.gripper.connect(ip, 63352)
        rospy.loginfo("Activating the gripper.....")
        self.gripper.activate(auto_calibrate=False)
        # set up server
        # add_thread = threading.Thread(target = thread_job)
        # add_thread.start()
        self.gripper_server = rospy.Service('gripper_service', gripper_service, self.serverCallback)
        rospy.loginfo("Gripper ready to receive service request...")

    def jointStateCallback(self, msg):
        js = msg
        # js.name[7] = 'hande_left_finger_joint'
        js.name = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint','hande_left_finger_joint']
        xpos=gripper_obj.gripper.get_current_position()
        xpos=xpos*0.0001
        js.position = [msg.position[0],msg.position[1],msg.position[2],msg.position[3],msg.position[4],msg.position[5],xpos]
        # js.name[7]="hande_left_finger_joint"
        # js.postion[7]=xpos
        # print js
        self.joint_state_pub.publish(js)

    def serverCallback(self, request):
        pos = request.position
        speed = request.speed
        force = request.force
        if speed > 255 or speed <=0:
            return(gripper_serviceResponse('invalid speed value. Valid in range (0,255]'))
        if force > 255 or force <=0:
            return(gripper_serviceResponse('invalid force value. Valid in range (0,255]'))
        if pos > 255 or pos < 0:
            return(gripper_serviceResponse('invalid position value. Valid in range [0,255]'))

        rospy.loginfo("moving the gripper. positino = {}, speed={}, force={}".format(pos, speed, force))
        self.gripper.move_and_wait_for_pos(pos, speed, force)
        return(gripper_serviceResponse(1))

if __name__ == '__main__':
    gripper_obj = HandEGripper()
    rospy.spin()

4、通过clien和service控制机械爪：

在终端执行：
rosservice call /gripper_service “position: 10
speed: 10
force: 10”
可以看到机械爪运动