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


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