fullbody_action_node.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- mode: python -*-
00003 
00004 import roslib; roslib.load_manifest('setup_fullbody_controller')
00005 import rospy
00006 import actionlib
00007 
00008 import math
00009 import thread
00010 from threading import Thread
00011 
00012 from geometry_msgs.msg import PointStamped, Point
00013 from trajectory_msgs.msg import *
00014 from pr2_controllers_msgs.msg import *
00015 
00016 class FullbodyAction(object):
00017     def __init__(self, name):
00018         self._action_name = name + '/joint_trajectory_action'
00019         self._trajectories = [] # list of JointTrajectory
00020         self._lockobj = thread.allocate_lock()
00021         self._as = actionlib.SimpleActionServer(self._action_name, JointTrajectoryAction, execute_cb=self.execute_cb)
00022 
00023         # controllers
00024         self._larm_client=actionlib.SimpleActionClient('/l_arm_controller/joint_trajectory_action', JointTrajectoryAction)
00025         self._rarm_client=actionlib.SimpleActionClient('/r_arm_controller/joint_trajectory_action', JointTrajectoryAction)
00026         self._torso_client=actionlib.SimpleActionClient('/torso_controller/joint_trajectory_action', JointTrajectoryAction)
00027         self._head_client=actionlib.SimpleActionClient('/head_traj_controller/head_trajectory_action', JointTrajectoryAction)
00028         self._base_client=actionlib.SimpleActionClient('/base_controller/joint_trajectory_action', JointTrajectoryAction)
00029         self._clients = [self._larm_client, self._rarm_client,
00030                          self._torso_client, self._head_client,
00031                          self._base_client]
00032         for client in self._clients:
00033             client.wait_for_server()
00034 
00035         print 'fullbody action initialized'
00036 
00037     # action methods
00038     def larm(self, traj):
00039         joints = ["l_shoulder_pan_joint",
00040                   "l_shoulder_lift_joint", "l_upper_arm_roll_joint",
00041                   "l_elbow_flex_joint", "l_forearm_roll_joint",
00042                   "l_wrist_flex_joint", "l_wrist_roll_joint"]
00043         return self.joint(self._larm_client, traj, joints)
00044     def rarm(self, traj):
00045         joints = ["r_shoulder_pan_joint",
00046                   "r_shoulder_lift_joint", "r_upper_arm_roll_joint",
00047                   "r_elbow_flex_joint", "r_forearm_roll_joint",
00048                   "r_wrist_flex_joint", "r_wrist_roll_joint"]
00049         return self.joint(self._rarm_client, traj, joints)
00050     def base(self, traj):
00051         joints = ["base_link_x", "base_link_y", "base_link_pan"]
00052         return self.joint(self._base_client, traj, joints)
00053     def torso(self, traj):
00054         joints = ["torso_lift_joint"]
00055         return self.joint(self._torsi_client, traj, joints)
00056     def head(self, traj):
00057         joints = ["head_pan_joint", "head_tilt_joint"]
00058         return self.joint(self._head_client, traj, joints)
00059 
00060     def joint(self, client, traj, joints):
00061         try:
00062             indexes = [y[0] for x in joints for y in enumerate(traj.joint_names) if x == y[1]]
00063             if len(indexes) != len(joints): return
00064 
00065             jgoal = JointTrajectory()
00066             jgoal.header.stamp = traj.header.stamp
00067             jgoal.joint_names = joints
00068             jgoal.points=[JointTrajectoryPoint(
00069                     time_from_start=pt.time_from_start,
00070                     positions=[pt.positions[i] for i in indexes if pt.positions != []],
00071                     velocities=[pt.velocities[i] for i in indexes if pt.velocities != []])
00072                           for pt in traj.points]
00073             client.send_goal(JointTrajectoryGoal(trajectory=jgoal))
00074         except:
00075             print 'error in joint client'
00076 
00077     def execute_cb(self, goal):
00078         self.larm(goal.trajectory)
00079         self.rarm(goal.trajectory)
00080         self.base(goal.trajectory)
00081         self.torso(goal.trajectory)
00082         self.head(goal.trajectory)
00083 
00084         self._as.set_succeeded(JointTrajectoryResult())
00085 
00086 if __name__ == "__main__":
00087     rospy.init_node('fullbody_controller')
00088     action = FullbodyAction(rospy.get_name())
00089     rospy.spin()


jsk_pr2_startup
Author(s):
autogenerated on Wed Sep 16 2015 10:32:17