机械臂的moveit驱动

经过一周的纠结,这里终于做好了,还是基础太薄.所以非常有必要总结一下.
moveit作为一个很好的机械臂路径规划工具，大大降低了机械臂的开发的难度，很多功能都可以在模拟环境下测试运行，如前面博客中讲到的，但要让真实的机械臂能够按照moveit规划好的路径动起来，就需要开发连接机械臂和moveit的驱动代码.

1.驱动的原理

上图为通讯原理
首先，moveit把计算的结果通过Ros action的方式发送给driver，driver调用Ros_arduino_bridge的servo_write 服务发送各个关节舵机的控制指令给Arduino uno控制板
其次，同时Driver也要通过调用Ros_arduino_bridge的servo_read服务读取各个关节的舵机状态，通过joint_state消息的方式发送给moveit，供moveit进行路径规划计算。

2.关于硬件连接

主控制板:arduino uno r3, 16路pwm控制板.IIC接口

 Gnd------------------Gnd
 +5V------------------VCC
 A4-------------------SDA
 A5-------------------SCL   

3.Arduino程序修改

没有基础控制器,只是响使用它来控制舵机，编辑ROSArduinoBridge sketch，在文件的最前面，将这两行：

#define USE_BASE
//#undef USE_BASE

改成这样:

//#define USE_BASE
#undef USE_BASE

NOTE:还需要将文件encoder_driver.ino中的这一行注释掉：

#include "MegaEncoderCounter.h"

编译并上传代码

4.创建控制器配置文件six_arm_controllers.yaml

在moveit assistant产生的配置文件目录的config子目录下，创建配置文件six_arm_controllers.yaml，此文件告诉moveit将与哪个的action通讯.
six_arm_controllers.yaml代码如下:

controller_list:
//控制器的ros topic——arm_controller/follow_joint_trajectory
  - name: arm_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory//在driver中要声明的action service类型
    default: true
    joints:
      - arm_base_to_arm_round_joint_stevo0
      - shoulder_2_to_arm_joint_stevo1
      - big_arm_round_to_joint_stevo2
      - arm_joint_stevo2_to_arm_joint_stevo3
      - wrist_to_arm_joint_stevo4
      - arm_joint_stevo4_to_arm_joint_stevo5

这里为机械臂定义了控制器arm_controller,告诉moveit机械臂通讯的topic是arm_controller/follow_joint_trajectory.

5 机械臂控制的FollowController

扩展ros_arduino_bridge的功能来实现FollowController，在ros_arduino_python目录下创建joints.py和six_arm_follow_controller.py

1)joint.py

封装了关节舵机控制的代码，主要功能函数：

__init__类初始化代码
setCurrentPosition(self):通过调用/arduino/servo_write服务发送舵机的控制命令，把舵机的位置设置为self.position
setCurrentPosition(self, position):
通过调用/arduino/servo_write服务发送舵机的控制命令，把舵机的位置设置为moveit给出的舵机位置
mapToServoPosition(self,position):将moveit给出的舵机位置，转换为机械臂舵机实际对应的位置，此功能需要标定后，才能准确控制机械臂的位置。

#!/usr/bin/env python

# Copyright (c) 2017-2018 diego. 
# All right reserved.
#

## @file joints.py the jonit clase support functions for joints.

## @brief Joints hold current values.
import roslib
import rospy
from ros_arduino_msgs.srv import *
from math import pi as PI, degrees, radians
class Joint:

    ## @brief Constructs a Joint instance.
    ##
    ## @param servoNum The servo id for this joint.
    ## 
    ## @param name The joint name.
    ## 
    ## @param name The servo control range.
    def __init__(self, name, servoNum, max, min, servo_max, servo_min, inverse):
        self.name = name//关节名称
        self.servoNum=servoNum//对应的舵机编号
        self.max=max//舵机最大角度：0
        self.min=min//舵机最小角度：180
        self.servo_max=servo_max
        self.servo_min=servo_min
        self.inverse=inverse

        self.position = 0.0
        self.velocity = 0.0

    ## @brief Set the current position.
    def setCurrentPosition(self):
        rospy.wait_for_service('/arduino/servo_write')
    try:
            servo_write=rospy.ServiceProxy('/arduino/servo_write',ServoWrite)
            servo_write(self.servoNum,self.position)
        except rospy.ServiceException, e:
            print "Service call failed: %s"%e   

    ## @brief Set the current position.
    ##
    ## @param position The current position. 
    def setCurrentPosition(self, position):
        rospy.wait_for_service('/arduino/servo_write')
    try:
        #if self.servoNum==2:
            servo_write=rospy.ServiceProxy('/arduino/servo_write',ServoWrite)
            rospy.loginfo("%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%")
        rospy.loginfo("before mapping--- the servo id:"+str(self.servoNum)+" position is "+str(position))
            self.mapToServoPosition(position)
            rospy.loginfo("after mapping---- the servo id:"+str(self.servoNum)+" self.position is "+str(self.position))             
            servo_write(self.servoNum,radians(self.position))

        except rospy.ServiceException, e:
            print "Service call failed: %s"%e

    ## @brief map to  Servo Position.
    ##
    ## @param position The current position.        
    def mapToServoPosition(self,position):
        per=(position-self.min)/(self.max-self.min)
        if not self.inverse:            
            self.position=self.servo_min+per*(self.servo_max-self.servo_min)
        else:
            self.position=self.servo_max-per*(self.servo_max-self.servo_min)

2) six_arm_follow_controller.py

机械臂控制器类，接收moveit的控制命令，转换为关节舵机的实际控制指令，驱动机械臂的运动。

#!/usr/bin/env python
# Copyright (c) 2017-2018 williamar. 
# All right reserved.

import rospy, actionlib

from control_msgs.msg import FollowJointTrajectoryAction
from trajectory_msgs.msg import JointTrajectory
from diagnostic_msgs.msg import *
from math import pi as PI, degrees, radians
from joints import Joint
class FollowController:
// 此类是驱动的核心代码
    def __init__(self, name):
        self.name = name

        rospy.init_node(self.name)

 //  初始化化ros note，设置节点名称，刷新频率为50hz
        # rates
        self.rate = 50.0

//初始化机械臂的关节，并把关节放入joints列表中，方便后续操作   
        # Arm jonits
        self.arm_base_to_arm_round_joint_stevo0=Joint('arm_base_to_arm_round_joint_stevo0',0,1.5797,-1.5707,130,0,False)
        self.shoulder_2_to_arm_joint_stevo1=Joint('shoulder_2_to_arm_joint_stevo1',1,1.5707,-0.1899,115,45,False)
        self.big_arm_round_to_joint_stevo2=Joint('big_arm_round_to_joint_stevo2',2,2.5891,1,100,20,False)
        self.arm_joint_stevo2_to_arm_joint_stevo3=Joint('arm_joint_stevo2_to_arm_joint_stevo3',3,1.5707,-1.5707,130,0,False)
        self.wrist_to_arm_joint_stevo4=Joint('wrist_to_arm_joint_stevo4',4,1.5707,-1.5707,130,0,False)
        self.arm_joint_stevo4_to_arm_joint_stevo5=Joint('arm_joint_stevo4_to_arm_joint_stevo5',5,1.5707,-1.5707,130,0,True)



        self.joints=[self.arm_base_to_arm_round_joint_stevo0,
        self.shoulder_2_to_arm_joint_stevo1,
        self.big_arm_round_to_joint_stevo2,
        self.arm_joint_stevo2_to_arm_joint_stevo3,
        self.wrist_to_arm_joint_stevo4,
        self.arm_joint_stevo4_to_arm_joint_stevo5]


        # action server
        self.server = actionlib.SimpleActionServer('arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction, execute_cb=self.actionCb, auto_start=False)
        self.server.start()
        rospy.spin()
        rospy.loginfo("Started FollowController")


    def actionCb(self, goal):
    print("****************actionCb*********************")    
        rospy.loginfo(self.name + ": Action goal recieved.")
        traj = goal.trajectory

        if set(self.joints) != set(traj.joint_names):
            for j in self.joints:
                if j.name not in traj.joint_names:
                    msg = "Trajectory joint names does not match action controlled joints." + str(traj.joint_names)
                    rospy.logerr(msg)
                    self.server.set_aborted(text=msg)
                    return
            rospy.logwarn("Extra joints in trajectory")

        if not traj.points:
            msg = "Trajectory empy."
            rospy.logerr(msg)
            self.server.set_aborted(text=msg)
            return

        try:
            indexes = [traj.joint_names.index(joint.name) for joint in self.joints]
        except ValueError as val:
            msg = "Trajectory invalid."
            rospy.logerr(msg)
            self.server.set_aborted(text=msg)
            return

        if self.executeTrajectory(traj):   
            self.server.set_succeeded()
        else:
            self.server.set_aborted(text="Execution failed.")

        rospy.loginfo(self.name + ": Done.")     

    def executeTrajectory(self, traj):
        rospy.loginfo("Executing trajectory")
        rospy.logdebug(traj)
        # carry out trajectory
        try:
            indexes = [traj.joint_names.index(joint.name) for joint in self.joints]
        except ValueError as val:
            rospy.logerr("Invalid joint in trajectory.")
            return False

        # get starting timestamp, MoveIt uses 0, need to fill in
        start = traj.header.stamp
        if start.secs == 0 and start.nsecs == 0:
            start = rospy.Time.now()

        r = rospy.Rate(self.rate)
    for point in traj.points:            
            desired = [ point.positions[k] for k in indexes ]
            for i in indexes:
                #self.joints[i].position=desired[i]
                self.joints[i].setCurrentPosition(desired[i])

            while rospy.Time.now() + rospy.Duration(0.01) < start:
                rospy.sleep(0.01)
        return True


if __name__=='__main__':
    try:
        rospy.loginfo("start followController...")
        FollowController('follow_Controller')
    except rospy.ROSInterruptException:
        rospy.loginfo('Failed to start followController...')

6 launch启动文件

moveit assistant会生成一个launch文件夹，修改后测试.
1)3.1修改six_arm_moveit_controller_manager.launch.xml

此文件是moveit assistant自动生成的，但其中内容是空的，我增加如下内容，告诉moveit，启动Controller的配置文件位置

<launch>
     
     <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
     <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
     
     <rosparam file="$(find six_arm_moveit_config)/config/six_arm_controllers.yaml"/>
launch>

3.2创建six_arm_demo.launch文件

在moveit的launch文件夹下创建six_arm_demo.launch文件，并添加如下内容

<launch>

     <master auto="start"/>

     <node name="arduino" pkg="ros_arduino_python" type="arduino_node.py" output="screen">
         <rosparam file="$(find ros_arduino_python)/config/my_arduino_params.yaml" command="load" />
     node>
     
     <arg name="debug" default="false" />

     
     <include file="$(find six_arm_moveit_config)/launch/planning_context.launch">
         <arg name="load_robot_description" value="true"/>
     include>

     <node name="arduino_follow_controller" pkg="ros_arduino_python" type="six_arm_follow_controller.py" output="screen">
     node>

     
     <node pkg="tf" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 world_frame base_link 100" />

     
     <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
        <param name="/use_gui" value="false"/>
        <rosparam param="/source_list">[/move_group/fake_controller_joint_states]rosparam>
     node>

     
     <include file="$(find six_arm_moveit_config)/launch/move_group.launch">
     include>
launch>

这里主要是作为一个测试使用，所以joint_states使用了假数据，在调用joint_state_publisher时我们把source list设置为/move_group/fake_controller_joint_states

执行如下代码来启动moveit

roslaunch six_arm_moveit_config six_arm_demo.launch

这时候我们可以通过控制台输入moveit命令来控制机械臂，我们也可以用代码来调用moveit接口来控制机械臂，下面我们来写一段测试代码来.
4.moveit控制测试代码

在ros_arduino_python目录下创建diego_moveit_test.py，代码如下

#!/usr/bin/env python

import rospy, sys
import moveit_commander
from control_msgs.msg import GripperCommand

class MoveItDemo:
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        # Initialize the ROS node
        rospy.init_node('moveit_demo', anonymous=True)


        # Connect to the arm move group
        arm = moveit_commander.MoveGroupCommander('arm')

        # Get the name of the end-effector link
        end_effector_link = arm.get_end_effector_link()

        # Display the name of the end_effector link
        rospy.loginfo("The end effector link is: " + str(end_effector_link))

        # Set a small tolerance on joint angles
        arm.set_goal_joint_tolerance(0.001)

        # Start the arm target in "resting" pose stored in the SRDF file
        arm.set_named_target('arm_default_pose')

        # Plan a trajectory to the goal configuration
        traj = arm.plan()

        # Execute the planned trajectory
        arm.execute(traj)

        # Pause for a moment
        rospy.sleep(1)         

        # Cleanly shut down MoveIt
        moveit_commander.roscpp_shutdown()

        # Exit the script
        moveit_commander.os._exit(0)

if __name__ == "__main__":
    try:
        MoveItDemo()
    except rospy.ROSInterruptException:
        pass

在测试代码中，我们连接到group arm，并命令机械臂运行到姿势 arm_default_pose，我们运行如下命令启动测试代码

rosrun ros_arduino_python six_arm_moveit_test.py

启动后可以看到机械臂马上响应到arm_default_pose的位置。

7 遇到的问题

软件是参考大神的,基本没啥大问题,就是语法吖,没有启动action的问题.启动步骤:

roslaunch six_arm_moveit_config  demo.launch
新终端: 
roslaunch ros_arduino_python arduino.launch
roslaunch ros_arduino_python six_arm_follow_controller.launch
roslaunch six_arm_moveit_config six_arm_demo.launch
rosrun ros_arduino_python six_arm_moveit_test.py