更多創客作品，請關注筆者網站園丁鳥，搜集全球極具創意，且有價值的創客作品
ROS機器人知識請關注，diegorobot
業餘時間完成的一款線上統計過程分析工具SPC，及SPC知識分享網站qdo
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
官方的解釋如下
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
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
可以看到消息的結構體中包含了三部分
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")
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.")
在此段代碼中，忽略了控制速度和加速度的設定，因為我們此機械臂的舵機無法控制舵機的速度和加速度，隻要能到達預期控制點就可以了。
未完待續…