官网教程:
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配对。