test_rosbridge.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # -*- coding: utf-8 -*-
3 
4 # Software License Agreement (BSD License)
5 #
6 # Copyright (c) 2014, JSK Lab, University of Tokyo
7 # Copyright (c) 2014, TORK
8 # All rights reserved.
9 #
10 # Redistribution and use in source and binary forms, with or without
11 # modification, are permitted provided that the following conditions
12 # are met:
13 #
14 # * Redistributions of source code must retain the above copyright
15 # notice, this list of conditions and the following disclaimer.
16 # * Redistributions in binary form must reproduce the above
17 # copyright notice, this list of conditions and the following
18 # disclaimer in the documentation and/or other materials provided
19 # with the distribution.
20 # * Neither the name of JSK Lab, University of Tokyo, TORK, nor the
21 # names of its contributors may be used to endorse or promote products
22 # derived from this software without specific prior written permission.
23 #
24 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 # POSSIBILITY OF SUCH DAMAGE.
36 
37 PKG = 'hironx_ros_bridge'
38 # rosbuild needs load_manifest
39 try:
40  import roslib
41  import hironx_ros_bridge
42 except:
43  import roslib; roslib.load_manifest(PKG)
44  import hironx_ros_bridge
45 
46 from hironx_ros_bridge import hironx_client as hironx
47 import rospy, actionlib, math, numpy
48 import tf
49 from tf.transformations import quaternion_matrix, euler_from_matrix
50 
51 import unittest
52 
53 # for catkin compiled environment, pr2_controller_msgs is not catkinized
54 try:
55  roslib.load_manifest('pr2_controllers_msgs')
56  import pr2_controllers_msgs.msg
57 except:
58  pass
59 import trajectory_msgs.msg
60 
61 from sensor_msgs.msg import JointState
62 try:
63  from pr2_controllers_msgs.msg import JointTrajectoryAction
64  from pr2_controllers_msgs.msg import JointTrajectoryGoal
65  joint_trajectory_action = 'joint_trajectory_action'
66  print("from pr2_controllers_msgs.msg import JointTrajectoryAction")
67 except:
68  from control_msgs.msg import FollowJointTrajectoryAction as JointTrajectoryAction
69  from control_msgs.msg import FollowJointTrajectoryGoal as JointTrajectoryGoal
70  joint_trajectory_action = 'follow_joint_trajectory_action'
71  print("from control_msgs.msg import FollowJointTrajectoryAction as JointTrajectoryAction")
72 from trajectory_msgs.msg import JointTrajectoryPoint
73 from hrpsys_ros_bridge.srv import *
74 
75 import time
76 import tempfile
77 
78 class TestHiroROSBridge(unittest.TestCase):
79 
80  def joint_states_cb(self, msg):
81  self.joint_states.append(msg)
82  self.joint_states = self.joint_states[0:3000]
83 
84  def __init__(self, *args, **kwargs):
85  super(TestHiroROSBridge, self).__init__(*args, **kwargs)
86  #
87  self.joint_states = []
88  rospy.init_node('hironx_ros_bridge_test')
89  self.joint_states_sub = rospy.Subscriber("joint_states", JointState, self.joint_states_cb)
90  self.filename_base = tempfile.mkstemp()[1]
91  self.filenames = []
92 
93  @classmethod
94  def setUpClass(self):
95  self.robot = hironx.HIRONX()
96  self.robot.init()
97 
99 
100  self.larm = actionlib.SimpleActionClient("/larm_controller/" + joint_trajectory_action, JointTrajectoryAction)
101  self.rarm = actionlib.SimpleActionClient("/rarm_controller/" + joint_trajectory_action, JointTrajectoryAction)
102  self.torso = actionlib.SimpleActionClient("/torso_controller/" + joint_trajectory_action, JointTrajectoryAction)
103  self.head = actionlib.SimpleActionClient("/head_controller/" + joint_trajectory_action, JointTrajectoryAction)
104  self.larm.wait_for_server()
105  self.rarm.wait_for_server()
106  self.torso.wait_for_server()
107  self.head.wait_for_server()
108 
109  rospy.wait_for_service('/SequencePlayerServiceROSBridge/setTargetPose')
110  self.set_target_pose = rospy.ServiceProxy('/SequencePlayerServiceROSBridge/setTargetPose', OpenHRP_SequencePlayerService_setTargetPose)
111  rospy.wait_for_service('/SequencePlayerServiceROSBridge/waitInterpolationOfGroup')
112  self.wait_interpolation_of_group = rospy.ServiceProxy('/SequencePlayerServiceROSBridge/waitInterpolationOfGroup', OpenHRP_SequencePlayerService_waitInterpolationOfGroup)
113 
114 
115  def tearDown(self):
116  self.reset_Pose()
117  True
118 
119  def setUp(self):
120  self.reset_Pose()
121 
122  def reset_Pose(self):
123  larm_goal = self.goal_LArm()
124  larm_goal = self.setup_Positions(larm_goal, [[ 0.0, -40.0, -90.0, 0.0, 0.0, 0.0]])
125  rarm_goal = self.goal_RArm()
126  rarm_goal = self.setup_Positions(rarm_goal, [[ 0.0, -40.0, -90.0, 0.0, 0.0, 0.0]])
127  head_goal = self.goal_Head()
128  head_goal = self.setup_Positions(head_goal, [[0, 20]])
129  torso_goal = self.goal_Torso()
130  torso_goal = self.setup_Positions(torso_goal, [[0]])
131  self.larm.send_goal(larm_goal)
132  self.rarm.send_goal(rarm_goal)
133  self.head.send_goal(head_goal)
134  self.torso.send_goal(torso_goal)
135  self.larm.wait_for_result()
136  self.rarm.wait_for_result()
137  self.head.wait_for_result()
138  self.torso.wait_for_result()
139 
140  def goal_LArm(self):
141  goal = JointTrajectoryGoal()
142  goal.trajectory.joint_names.append("LARM_JOINT0")
143  goal.trajectory.joint_names.append("LARM_JOINT1")
144  goal.trajectory.joint_names.append("LARM_JOINT2")
145  goal.trajectory.joint_names.append("LARM_JOINT3")
146  goal.trajectory.joint_names.append("LARM_JOINT4")
147  goal.trajectory.joint_names.append("LARM_JOINT5")
148  return goal
149 
150  def goal_RArm(self):
151  goal = JointTrajectoryGoal()
152  goal.trajectory.joint_names.append("RARM_JOINT0")
153  goal.trajectory.joint_names.append("RARM_JOINT1")
154  goal.trajectory.joint_names.append("RARM_JOINT2")
155  goal.trajectory.joint_names.append("RARM_JOINT3")
156  goal.trajectory.joint_names.append("RARM_JOINT4")
157  goal.trajectory.joint_names.append("RARM_JOINT5")
158  return goal
159 
160  def goal_Torso(self):
161  goal = JointTrajectoryGoal()
162  goal.trajectory.joint_names.append("CHEST_JOINT0")
163  return goal
164 
165  def goal_Head(self):
166  goal = JointTrajectoryGoal()
167  goal.trajectory.joint_names.append("HEAD_JOINT0")
168  goal.trajectory.joint_names.append("HEAD_JOINT1")
169  return goal
170 
171  def setup_Positions(self, goal, positions, tm = 1.0):
172  for p in positions:
173  point = trajectory_msgs.msg.JointTrajectoryPoint()
174  point.positions = [ x * math.pi / 180.0 for x in p]
175  point.time_from_start = rospy.Duration(tm)
176  goal.trajectory.points.append(point)
177  tm += tm
178  return goal
179 
180  def check_q_data(self, name):
181  import math
182  data = []
183 
184  name = name+".q"
185  f = open(name, 'w')
186  for j in self.joint_states:
187  current_time = j.header.stamp.to_sec();
188  current_value = [p*180/math.pi for p in j.position]
189  data.append([current_time, current_value[5]])
190  f.write(str(current_time)+' '+' '.join([str(i) for i in current_value])+"\n")
191  f.close()
192  self.filenames.append(name)
193  return data
194 
195  def write_output_to_pdf (self,name):
196  import os
197  cmd = "gnuplot -p -e \"set terminal pdf; set output '"+name+"'; plot "
198  for name in self.filenames:
199  cmd += "'"+name+"' using 0:7 title '"+name+"' with lines"
200  if name != self.filenames[-1]:
201  cmd += ","
202  cmd += "\""
203  os.system(cmd)
204  return cmd
205 
206 # unittest.main()
207 if __name__ == '__main__':
208  import rostest
209  rostest.rosrun(PKG, 'test_hronx_ros_bridge', TestHiroROSBridge)
210 
211 
212 
213 


hironx_ros_bridge
Author(s): Kei Okada , Isaac I.Y. Saito
autogenerated on Mon Feb 28 2022 23:45:15