ROS机器人Diego 1#制作（十九）diego机器人的moveit驱动 - 无网不进

ROS机器人Diego 1#制作（十九）diego机器人的moveit驱动

moveit作为一个很好的机械臂路径规划工具，大大降低了机械臂的开发的难度，很多功能都可以在模拟环境下测试运行，如前面博客中讲到的，但要让真实的机器人能够按照moveit规划好的路径动起来，就需要开发连接机器人和moveit的驱动代码，这一篇我们就介绍一下如何开发针对diego的驱动。

1.首先介绍一下驱动的原理

上图为通讯原理 
首先，moveit把计算的结果通过Ros action的方式发送给driver，driver调用Ros_arduino_bridge的servor_write server发送各个关节舵机的控制指令给Arduino uno控制板 
其次，同时Driver也要通过调用Ros_arduino_bridge的servo_read服务读取各个关节的舵机状态，通过joint_state消息的方式发送给moveit，供moveit进行路径规划计算。 
在前面的博文中ros_arduino_bridge和arduino uno相应的修改都已经介绍过，这里就不在说明，主要的工作就是在driver上

2.控制器配置文件diego_controllers.yaml 
根据moveit官方的说明我们需要针对我们机械臂的控制器配置文件，并把其放在moveit assistant产生的配置文件目录的config子目录下，我这里配置文件起名为diego_controllers.yaml 
 
配置文件代码如下：

controller_list:
  - name: left_arm_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - left_shoulder_stevo_to_axis
      - left_shoulder_stevo_lift_to_axis
      - left_big_arm_up_to_axis
      - left_small_arm_up_to_axis
      - left_wrist_run_stevo_to_axis
  - name: rigth_arm_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - right_shoulder_stevo_to_axis
      - right_shoulder_stevo_lift_to_axis
      - right_big_arm_up_to_axis
      - right_small_arm_up_to_axis
      - right_wrist_run_stevo_to_axis
  - name: right_gripper_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - right_hand_run_stevo_to_right_hand_run_stevo_axis
  - name: left_gripper_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - left_hand_run_stevo_to_left_hand_run_stevo_axis

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27
• 28
• 29
• 30
• 31
• 32
• 33

官方的解释如下 
The parameters are: 
name: The name of the controller. (See debugging information below for important notes). 
action_ns: The action namespace for the controller. (See debugging information below for important notes). 
type: The type of action being used (here FollowJointTrajectory). 
default: The default controller is the primary controller chosen by MoveIt! for communicating with a particular set of joints. 
joints: Names of all the joints that are being addressed by this interface.

通俗点理解/name/action_ns就是对应控制器的ros topic, diego配置文件中对于左臂的ros_topic就是/left_arm_controller/follow_joint_trajectory

type就是我们在drive中要声明的action service类型，在diego的driver中需要提供FollowJointTrajectoryAction接收moveit action client发送来的消息

3.joint.py关节类

from ros_arduino_msgs.srv import *
class Joint:

    ## @brief Constructs a Joint instance.
    ##
    ## @param servoNum The servo number.
    ## 
    ## @param name The joint name.
    def __init__(self, name, servoNum, range):
        self.name = name #关节名称
        self.servoNum=servoNum #对应的舵机编号
        self.range=range #舵机的控制范围，这里是0~180度

        self.position = 0.0 
        self.velocity = 0.0
        self.last = rospy.Time.now()
        ## @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   

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24

4.action server 控制器文件follow_controller.py 
follow_controller就是主要的驱动文件

4.1 JointTrajectory msg 
驱动的核心其实就是follow_controller对JointTrajectory msg的处理，所以这里先介绍一下JointTrajectory msg，只要理解了JointTrajectory msg，其实驱动还是比较容易的。 
在命令执行，如下命令就可以显示了JointTrajectory msg的结构

$ rosmsg show JointTrajectory

• 1

可以看到消息的结构体中包含了三部分 
a. header 
这是Ros的标准消息头这里就不多介绍了 
b. joint_names 
这是所有关节名称的数组 
c.JointTrajectoryPoint 
这部分是驱动的关键，这个数组记录了机械臂从一种姿势到另外一种姿势所经过的路径点，moveit所产生的姿势路径是通过这些point点描述出来的，也就是我们驱动中要控制每个关节的舵机都按照这些point点进行运动，每个point又是由一个结构体构成： 
positions这是一个float64的数组，记录每个point的时候舵机应该到达的角度，这里是弧度为单位的，比如说是6自由度的那每个Point的这个positions字段中应该包含六个数值[1.57,0,2,0.2,3,0.12],也就是我们舵机控制范围是180度，那这里面的取值范围就是0~π 
velocities这个数组记录了每个关节运动的速度 
accelerations这个数组记录每个关节运动的加速度 
effort这个参数不知道中文应该如何翻译，可以不用 
d.**time_from**_start这个参数是指定从头部的timestamp开始算起多长时间要达到这个点的位置

4.2 follow_controller的初始化代码 
初始化代码主要就是初始化joints列表，同时启动action Server

    def __init__(self, name):
        self.name = name

        # rates
        self.rate = 20.0

        # left Arm jonits list
        self.left_shoulder_stevo_to_axis=Joint(left_shoulder_stevo_to_axis,6,PI)
        self.left_shoulder_stevo_lift_to_axis=Joint(left_shoulder_stevo_lift_to_axis,7,PI)
        self.left_big_arm_up_to_axis=Joint(left_big_arm_up_to_axis,8,PI)
        self.left_small_arm_up_to_axis=Joint(left_small_arm_up_to_axis,9,PI)
        self.left_wrist_run_stevo_to_axis=Joint(left_wrist_run_stevo_to_axis,10,PI)

        self.joints=list()
        self.joints.append(left_shoulder_stevo_to_axis)
        self.joints.append(left_shoulder_stevo_lift_to_axis)
        self.joints.append(left_big_arm_up_to_axis)
        self.joints.append(left_small_arm_up_to_axis)
        self.joints.append(left_wrist_run_stevo_to_axis)


        # left hand joint
        self.left_hand_run_stevo_to_left_hand_run_stevo_axis=Joint(left_hand_run_stevo_to_left_hand_run_stevo_axis,11,PI)
        self.joints.append(left_hand_run_stevo_to_left_hand_run_stevo_axis)

        # right Arm jonits
        self.right_shoulder_stevo_to_axis=Joint(right_shoulder_stevo_to_axis,0,PI)
        self.right_shoulder_stevo_lift_to_axis=Joint(right_shoulder_stevo_lift_to_axis,1,PI)
        self.right_big_arm_up_to_axis=Joint(right_big_arm_up_to_axis,2,PI)
        self.right_small_arm_up_to_axis=Joint(right_small_arm_up_to_axis,3,PI)
        self.right_wrist_run_stevo_to_axis=Joint(right_wrist_run_stevo_to_axis,4,PI)

        self.joints.append(right_shoulder_stevo_to_axis)
        self.joints.append(right_shoulder_stevo_lift_to_axis)
        self.joints.append(right_big_arm_up_to_axis)
        self.joints.append(right_small_arm_up_to_axis)
        self.joints.append(right_wrist_run_stevo_to_axis)

        # left hand joint
        self.right_hand_run_stevo_to_right_hand_run_stevo_axis=Joint(right_hand_run_stevo_to_right_hand_run_stevo_axis,5,PI)
        self.joints.append(right_hand_run_stevo_to_right_hand_run_stevo_axis)        

        # set the left arm back to the resting position
        rospy.loginfo("set the left arm back to the resting position")
        self.left_shoulder_stevo_to_axis.setCurrentPosition(PI/2)
        self.left_shoulder_stevo_lift_to_axis.setCurrentPosition(PI/2)
        self.left_big_arm_up_to_axis.setCurrentPosition(PI/2)
        self.left_small_arm_up_to_axis.setCurrentPosition(PI/2)
        self.left_wrist_run_stevo_to_axis.setCurrentPosition(PI/2)
        # set the right arm back to the resting position
        rospy.loginfo("set the right arm back to the resting position")
        self.right_shoulder_stevo_to_axis.setCurrentPosition(PI/2)
        self.right_shoulder_stevo_lift_to_axis.setCurrentPosition(PI/2)
        self.right_big_arm_up_to_axis.setCurrentPosition(PI/2)
        self.right_small_arm_up_to_axis.setCurrentPosition(PI/2)
        self.right_wrist_run_stevo_to_axis.setCurrentPosition(PI/2)
        # set the left hand back to the resting position
        rospy.loginfo("set the left hand back to the resting position")
        self.left_hand_run_stevo_to_left_hand_run_stevo_axis.setCurrentPosition(PI/2)
        # set the right hand back to the resting position
        rospy.loginfo("set the right hand back to the resting position")
        self.right_hand_run_stevo_to_right_hand_run_stevo_axis.setCurrentPosition(PI/2)

        # action server
        self.server = actionlib.SimpleActionServer(\'follow_joint_trajectory\', FollowJointTrajectoryAction, execute_cb=self.actionCb, auto_start=True)
        rospy.loginfo("Started FollowController")

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27
• 28
• 29
• 30
• 31
• 32
• 33
• 34
• 35
• 36
• 37
• 38
• 39
• 40
• 41
• 42
• 43
• 44
• 45
• 46
• 47
• 48
• 49
• 50
• 51
• 52
• 53
• 54
• 55
• 56
• 57
• 58
• 59
• 60
• 61
• 62
• 63
• 64
• 65
• 66

4.3 .actionCb函数 
在初始化代码中Action Service的回调函数是actionCb，也就是收到msg后就会调用这个函数，对于节点舵机的控制也就是在这个函数中实现，代码的实现原理见下面的代码注释

    def actionCb(self, goal):
        rospy.loginfo(self.name + ": Action goal recieved.")
        traj = goal.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]#按照joints列表的顺序对traj的数据进行排序，把排序数据放到indexes中
        except ValueError as val:
            msg = "Trajectory invalid."
            rospy.logerr(msg)
            self.server.set_aborted(text=msg)
            return

        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()#发送舵机的控制命令

            while rospy.Time.now() + rospy.Duration(0.01) < start:#如果当前时间小于舵机这个点预期完成时间，则等待
                rospy.sleep(0.01)                        

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

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27
• 28
• 29
• 30
• 31
• 32
• 33

在此段代码中，忽略了控制速度和加速度的设置，因为我们此机械臂的舵机无法控制舵机的速度和加速度，只要能到达预期控制点就可以了。

未完待续…

版权声明：本文为博主原创文章，未经博主允许不得转载。