Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043 PKG = 'hironx_ros_bridge'
00044
00045 try:
00046 import roslib
00047 import hironx_ros_bridge
00048 except:
00049 import roslib; roslib.load_manifest(PKG)
00050 import hironx_ros_bridge
00051
00052 from hironx_ros_bridge import hironx_client as hironx
00053 import rospy, actionlib, math, numpy
00054 import tf
00055 from tf.transformations import quaternion_matrix, euler_from_matrix
00056
00057 import unittest
00058
00059
00060 roslib.load_manifest('pr2_controllers_msgs')
00061 import pr2_controllers_msgs.msg
00062 import trajectory_msgs.msg
00063
00064 from sensor_msgs.msg import JointState
00065 from pr2_controllers_msgs.msg import JointTrajectoryAction
00066 from trajectory_msgs.msg import JointTrajectoryPoint
00067 from hrpsys_ros_bridge.srv import *
00068
00069 import time
00070 import tempfile
00071
00072 class TestHiroROSBridge(unittest.TestCase):
00073
00074 def joint_states_cb(self, msg):
00075 self.joint_states.append(msg)
00076 self.joint_states = self.joint_states[0:3000]
00077
00078 def __init__(self, *args, **kwargs):
00079 super(TestHiroROSBridge, self).__init__(*args, **kwargs)
00080
00081 self.joint_states = []
00082 rospy.init_node('hironx_ros_bridge_test')
00083 self.joint_states_sub = rospy.Subscriber("joint_states", JointState, self.joint_states_cb)
00084 self.filename_base = tempfile.mkstemp()[1]
00085 self.filenames = []
00086
00087 @classmethod
00088 def setUpClass(self):
00089 self.robot = hironx.HIRONX()
00090 self.robot.init()
00091
00092 self.listener = tf.TransformListener()
00093
00094 self.larm = actionlib.SimpleActionClient("/larm_controller/joint_trajectory_action", JointTrajectoryAction)
00095 self.rarm = actionlib.SimpleActionClient("/rarm_controller/joint_trajectory_action", JointTrajectoryAction)
00096 self.torso = actionlib.SimpleActionClient("/torso_controller/joint_trajectory_action", JointTrajectoryAction)
00097 self.head = actionlib.SimpleActionClient("/head_controller/joint_trajectory_action", JointTrajectoryAction)
00098 self.larm.wait_for_server()
00099 self.rarm.wait_for_server()
00100 self.torso.wait_for_server()
00101 self.head.wait_for_server()
00102
00103 rospy.wait_for_service('/SequencePlayerServiceROSBridge/setTargetPose')
00104 self.set_target_pose = rospy.ServiceProxy('/SequencePlayerServiceROSBridge/setTargetPose', OpenHRP_SequencePlayerService_setTargetPose)
00105 rospy.wait_for_service('/SequencePlayerServiceROSBridge/waitInterpolationOfGroup')
00106 self.wait_interpolation_of_group = rospy.ServiceProxy('/SequencePlayerServiceROSBridge/waitInterpolationOfGroup', OpenHRP_SequencePlayerService_waitInterpolationOfGroup)
00107
00108
00109 def tearDown(self):
00110 self.reset_Pose()
00111 True
00112
00113 def setUp(self):
00114 self.reset_Pose()
00115
00116 def reset_Pose(self):
00117 larm_goal = self.goal_LArm()
00118 larm_goal = self.setup_Positions(larm_goal, [[ 0.0, -40.0, -90.0, 0.0, 0.0, 0.0]])
00119 rarm_goal = self.goal_RArm()
00120 rarm_goal = self.setup_Positions(rarm_goal, [[ 0.0, -40.0, -90.0, 0.0, 0.0, 0.0]])
00121 head_goal = self.goal_Head()
00122 head_goal = self.setup_Positions(head_goal, [[0, 20]])
00123 torso_goal = self.goal_Torso()
00124 torso_goal = self.setup_Positions(torso_goal, [[0]])
00125 self.larm.send_goal(larm_goal)
00126 self.rarm.send_goal(rarm_goal)
00127 self.head.send_goal(head_goal)
00128 self.torso.send_goal(torso_goal)
00129 self.larm.wait_for_result()
00130 self.rarm.wait_for_result()
00131 self.head.wait_for_result()
00132 self.torso.wait_for_result()
00133
00134 def goal_LArm(self):
00135 goal = pr2_controllers_msgs.msg.JointTrajectoryGoal()
00136 goal.trajectory.joint_names.append("LARM_JOINT0")
00137 goal.trajectory.joint_names.append("LARM_JOINT1")
00138 goal.trajectory.joint_names.append("LARM_JOINT2")
00139 goal.trajectory.joint_names.append("LARM_JOINT3")
00140 goal.trajectory.joint_names.append("LARM_JOINT4")
00141 goal.trajectory.joint_names.append("LARM_JOINT5")
00142 return goal
00143
00144 def goal_RArm(self):
00145 goal = pr2_controllers_msgs.msg.JointTrajectoryGoal()
00146 goal.trajectory.joint_names.append("RARM_JOINT0")
00147 goal.trajectory.joint_names.append("RARM_JOINT1")
00148 goal.trajectory.joint_names.append("RARM_JOINT2")
00149 goal.trajectory.joint_names.append("RARM_JOINT3")
00150 goal.trajectory.joint_names.append("RARM_JOINT4")
00151 goal.trajectory.joint_names.append("RARM_JOINT5")
00152 return goal
00153
00154 def goal_Torso(self):
00155 goal = pr2_controllers_msgs.msg.JointTrajectoryGoal()
00156 goal.trajectory.joint_names.append("CHEST_JOINT0")
00157 return goal
00158
00159 def goal_Head(self):
00160 goal = pr2_controllers_msgs.msg.JointTrajectoryGoal()
00161 goal.trajectory.joint_names.append("HEAD_JOINT0")
00162 goal.trajectory.joint_names.append("HEAD_JOINT1")
00163 return goal
00164
00165 def setup_Positions(self, goal, positions, tm = 1.0):
00166 for p in positions:
00167 point = trajectory_msgs.msg.JointTrajectoryPoint()
00168 point.positions = [ x * math.pi / 180.0 for x in p]
00169 point.time_from_start = rospy.Duration(tm)
00170 goal.trajectory.points.append(point)
00171 tm += tm
00172 return goal
00173
00174 def check_q_data(self, name):
00175 import math
00176 data = []
00177
00178 name = name+".q"
00179 f = open(name, 'w')
00180 for j in self.joint_states:
00181 current_time = j.header.stamp.to_sec();
00182 current_value = [p*180/math.pi for p in j.position]
00183 data.append([current_time, current_value[5]])
00184 f.write(str(current_time)+' '+' '.join([str(i) for i in current_value])+"\n")
00185 f.close()
00186 self.filenames.append(name)
00187 return data
00188
00189 def write_output_to_pdf (self,name):
00190 import os
00191 cmd = "gnuplot -p -e \"set terminal pdf; set output '"+name+"'; plot "
00192 for name in self.filenames:
00193 cmd += "'"+name+"' using 0:7 title '"+name+"' with lines"
00194 if name != self.filenames[-1]:
00195 cmd += ","
00196 cmd += "\""
00197 os.system(cmd)
00198 return cmd
00199
00200
00201 if __name__ == '__main__':
00202 import rostest
00203 rostest.rosrun(PKG, 'test_hronx_ros_bridge', TestHiroROSBridge)
00204
00205
00206
00207