dynamixel controll client and server

 官网教程：
  http://wiki.ros.org/dynamixel_controllers/Tutorials/Creatingmetacontroller
  http://wiki.ros.org/dynamixel_controllers/Tutorials/Creatingdynamixelactionclient

 action  action是ROS下节点间数据通信的一种方式，这种方式本质也是基于ros消息的，action的基础内容可以先看一下ros官网和古月的博客，可以这样简单理解：action打包了同一行为下的多个消息，使用action可以将你从繁琐的消息订阅与发布编程中解放出来，并且使节点间的信息交互能力大大提升。action是需要配对的。客户端与服务端配对，插座与插头的关系，产生联系的纽带是定义action时指定的action名字。

 roscd my_dynamixel_tutorial/config

== 创建一个sever ==

 新建tilt.yaml将以下粘贴进去：

 

 arm_shoulder_pan_joint:
    controller:
        package: dynamixel_controllers
        module: joint_position_controller
        type: JointPositionController
    joint_name: arm_shoulder_pan_joint
    joint_speed: 1.17
    motor:
        id: 1
        init: 512
        min: 0
        max: 1023

arm_shoulder_lift_joint:
    controller:
        package: dynamixel_controllers
        module: joint_position_controller
        type: JointPositionController
    joint_name: arm_shoulder_lift_joint
    joint_speed: 1.17
    motor:
        id: 2
        init: 512
        min: 0
        max: 1023

arm_elbow_flex_joint:
    controller:
        package: dynamixel_controllers
        module: joint_position_controller
        type: JointPositionController
    joint_name: arm_elbow_flex_joint
    joint_speed: 1.17
    motor:
        id: 3
        init: 512
        min: 0
        max: 1023

arm_wrist_flex_joint:
    controller:
        package: dynamixel_controllers
        module: joint_position_controller
        type: JointPositionController
    joint_name: arm_wrist_flex_joint
    joint_speed: 1.17
    motor:
        id: 4
        init: 512
        min: 0
        max: 1023

gripper_joint:
    controller:
        package: dynamixel_controllers
        module: joint_position_controller
        type: JointPositionController
    joint_name: gripper_joint
    joint_speed: 1.17
    motor:
        id: 5
        init: 512
        min: 0
        max: 1023

 

 

 

 新建一个 joints_trajectory_controller.yaml并把以下粘贴进去：

 

 

 f_arm_controller:
    controller:
        package: dynamixel_controllers
        module: joint_trajectory_action_controller
        type: JointTrajectoryActionController
    joint_trajectory_action_node:
        min_velocity: 0.1
        constraints:
            goal_time: 0.25        

 

 

 这里的f_arm_controller即为需要配对的action的名字。

 新建一个arm_action.launch 并把以下粘贴进去：
 <launch>
    <!-- Start tilt joint controller -->
    <rosparam file="$(find my_dynamixel_tutorial)/config/tilt.yaml" command="load"/>
    <node name="tilt_controller_spawner" pkg="dynamixel_controllers" type="controller_spawner.py"
          args="--manager=dxl_manager
                --port pan_tilt_port

                arm_shoulder_pan_joint
                arm_shoulder_lift_joint
                arm_elbow_flex_joint
                arm_wrist_flex_joint
                gripper_joint"
          output="screen"/>

 

  <!-- Start joints trajectory controller controller -->
    <rosparam file="$(find my_dynamixel_tutorial)/config/joints_trajectory_controller.yaml" command="load"/>
    <node name="controller_spawner_meta" pkg="dynamixel_controllers" type="controller_spawner.py"
          args="--manager=dxl_manager
                --type=meta
                f_arm_controller
                arm_shoulder_pan_joint                       
                arm_shoulder_lift_joint
                arm_elbow_flex_joint
                arm_wrist_flex_joint
                gripper_joint
               "
          output="screen"/>
</launch>

 

 

 

 在这里完成了server的配置，接下来启动sever查看效果
 1. roslaunch my_dynamixel_tutorial controller_manager.launch    启动舵机确保通信
 2. roslaunch my_dynamixel_tutorial start_meta_controller.launch  

 会出现以下：
 [INFO] [WallTime: 1412948063.493818] dxl_USB0 controller_spawner: All services are up, spawning controllers...
 [INFO] [WallTime: 1412948063.556084] Controller joint1_controller successfully started.
 [INFO] [WallTime: 1412948063.578977] meta controller_spawner: All services are up, spawning controllers...
 [WARN] [WallTime: 1412948063.608758] [f_arm_controller] not all dependencies started, still waiting for ['joint4_controller', 'joint5_controller']...
 [INFO] [WallTime: 1412948063.609689]
 [WARN] [WallTime: 1412948063.626055] [f_arm_controller] not all dependencies started, still waiting for ['joint5_controller']...
 [INFO] [WallTime: 1412948063.626438] Controller joint2_controller successfully started.
 [INFO] [WallTime: 1412948063.720389] Controller joint3_controller successfully started.
 [controller_spawner_meta-3] process has finished cleanly
 log file: /home/vitor/.ros/log/d553b226-5081-11e4-ba96-0c84dc3fa970/controller_spawner_meta-3*.log
 [controller_spawner-2] process has finished cleanly
 log file: /home/vitor/.ros/log/d553b226-5081-11e4-ba96-0c84dc3fa970/controller_spawner-2*.log

 

 

 这个时候在controller_manager.launnch终端下可能会有warn，可以不必理会。
 再启动rostopic list会出现以下相关话题：
 /arm_elbow_flex_joint/command
/arm_elbow_flex_joint/state
/arm_shoulder_lift_joint/command
/arm_shoulder_lift_joint/state
/arm_shoulder_pan_joint/command
/arm_shoulder_pan_joint/state
/arm_wrist_flex_joint/command
/arm_wrist_flex_joint/state
/diagnostics
/f_arm_controller/command
/f_arm_controller/follow_joint_trajectory/cancel
/f_arm_controller/follow_joint_trajectory/feedback
/f_arm_controller/follow_joint_trajectory/goal
/f_arm_controller/follow_joint_trajectory/result
/f_arm_controller/follow_joint_trajectory/status
/f_arm_controller/state
/gripper_joint/command
/gripper_joint/state
/motor_states/pan_tilt_port
/rosout
/rosout_agg

 

 这表示f_arm_controller的action server端启动成功。

 

 

== 创建一个client ==

 创建trajectory_client.py并粘贴以下：

 

 

 #!/usr/bin/env python
import roslib
roslib.load_manifest('my_dynamixel_tutorial')
 
import rospy
import actionlib
from std_msgs.msg import Float64
import trajectory_msgs.msg
import control_msgs.msg
from trajectory_msgs.msg import JointTrajectoryPoint
from control_msgs.msg import JointTrajectoryAction, JointTrajectoryGoal, FollowJointTrajectoryAction, FollowJointTrajectoryGoal

 
class Joint:
        def __init__(self, motor_name):
            #arm_name should be f_arm
            self.name = motor_name
            rospy.loginfo(self.name)
            self.jta = actionlib.SimpleActionClient('/'+self.name+'_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
            rospy.loginfo('Waiting for joint trajectory action')
            self.jta.wait_for_server()
            rospy.loginfo('Found joint trajectory action!')

        def move_joint(self, angles):
            goal = FollowJointTrajectoryGoal()
            goal.trajectory.joint_names = ['arm_shoulder_pan_joint', 'arm_shoulder_lift_joint','arm_elbow_flex_joint','arm_wrist_flex_joint', 'gripper_joint']
            point = JointTrajectoryPoint()
            point.positions = angles
            point.time_from_start = rospy.Duration(3)
            goal.trajectory.points.append(point)
            self.jta.send_goal_and_wait(goal)

def main():
            arm = Joint('f_arm')
            arm.move_joint([1.57,1.4,0,0,-0.4])
            arm.move_joint([1.57,1.4,0,0,0.2])

if __name__ == '__main__':
      rospy.init_node('joint_position_tester')
      main()

 

 

 

 使client和server通信：
 确保controller_manager.launch和arm_action.launch已经启动了，并且已经为trajectory_client.py文件赋予权限

 然后运行
  roscd my_dynamixel_tutorial
  python trajectory_client.py

 运行效果：舵机运动到我们所定义的位置：[1.57,1.4,0,0,-0.4]

 ps：

 为文件赋予权限，roscd my_dynamixel_tutorial,然后ls，如果trajectory_client.py文件的颜色是白色的，就执行sudo chmod trajectory_client.py即可，此时ls一下可以看到文件名字变成蓝色的了。

 在trajectory_client.py里面，arm = Joint('f_arm')中的f_arm跟官方教程中的不一样，我做了修改，这样子client才可以跟server配对。