test_hironx_ros_bridge.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 
00004 # Software License Agreement (BSD License)
00005 #
00006 # Copyright (c) 2014, JSK Lab, University of Tokyo
00007 # Copyright (c) 2014, TORK
00008 # All rights reserved.
00009 #
00010 # Redistribution and use in source and binary forms, with or without
00011 # modification, are permitted provided that the following conditions
00012 # are met:
00013 #
00014 #  * Redistributions of source code must retain the above copyright
00015 #    notice, this list of conditions and the following disclaimer.
00016 #  * Redistributions in binary form must reproduce the above
00017 #    copyright notice, this list of conditions and the following
00018 #    disclaimer in the documentation and/or other materials provided
00019 #    with the distribution.
00020 #  * Neither the name of JSK Lab, University of Tokyo, TORK, nor the
00021 #    names of its contributors may be used to endorse or promote products
00022 #    derived from this software without specific prior written permission.
00023 #
00024 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00025 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00026 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00027 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00028 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00029 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00030 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00031 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00033 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00034 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035 # POSSIBILITY OF SUCH DAMAGE.
00036 
00037 # 20170403 ***DO NOT*** refer to this file from any testcases. Instead, use
00038 #          https://github.com/start-jsk/rtmros_hironx/blob/indigo-devel/hironx_ros_bridge/src/hironx_ros_bridge/testutil/test_hironx_ros_bridge.py
00039 #          which this file is a mere duplicate of at the time of writing. See
00040 #          https://github.com/start-jsk/rtmros_hironx/pull/490 and relevant
00041 #          tickets/PRs for further info.
00042 
00043 PKG = 'hironx_ros_bridge'
00044 # rosbuild needs load_manifest
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 # for catkin compiled environment, pr2_controller_msgs is not catkinized
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 # unittest.main()
00201 if __name__ == '__main__':
00202     import rostest
00203     rostest.rosrun(PKG, 'test_hronx_ros_bridge', TestHiroROSBridge)
00204 
00205 
00206 
00207 


hironx_ros_bridge
Author(s): Kei Okada , Isaac I.Y. Saito
autogenerated on Thu Feb 21 2019 03:42:37