机械臂的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.positionsetCurrentPosition(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 roslibimport rospyfrom ros_arduino_msgs.srv import *from math import pi as PI, degrees, radiansclass 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, actionlibfrom control_msgs.msg import FollowJointTrajectoryActionfrom trajectory_msgs.msg import JointTrajectoryfrom diagnostic_msgs.msg import *from math import pi as PI, degrees, radiansfrom joints import Jointclass 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 Trueif __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>     <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->     <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />     <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>     <!-- load controller_list -->     <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>     <!-- By default, we are not in debug mode -->     <arg name="debug" default="false" />     <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->     <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>     <!-- If needed, broadcast static tf for robot root -->     <node pkg="tf" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 world_frame base_link 100" />     <!-- We do not have a robot connected, so publish fake joint states -->     <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>     <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->     <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 pythonimport rospy, sysimport moveit_commanderfrom control_msgs.msg import GripperCommandclass 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.launchroslaunch ros_arduino_python six_arm_follow_controller.launchroslaunch six_arm_moveit_config six_arm_demo.launchrosrun ros_arduino_python six_arm_moveit_test.py