jsk_footstep_controller: test_footstep_controller.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # 1. roslaunch hrpsys_gazebo_tutorials gazebo_jaxon_no_controllers.launch 
00004 # 2. rtmlaunch hrpsys_gazebo_tutorials jaxon_hrpsys_bringup.launch KINEMATICS_MODE:=true
00005 # 3. rosrun jsk_footstep_controller footstep-controller.l __name:=footstep_controller
00006 # 4. rosrun jsk_footstep_controller test_footstep_controller.py
00007 
00008 import rospy
00009 import actionlib
00010 import tf
00011 
00012 from jsk_footstep_msgs.msg import Footstep, FootstepArray, ExecFootstepsAction, ExecFootstepsGoal
00013 from geometry_msgs.msg import Pose
00014 # pose ... matrix
00015 
00016 from tf.transformations import *
00017 
00018 def matToMsg(mat):
00019     q = quaternion_from_matrix(mat)
00020     p = translation_from_matrix(mat)
00021     pose = Pose()
00022     pose.orientation.x = q[0]
00023     pose.orientation.y = q[1]
00024     pose.orientation.z = q[2]
00025     pose.orientation.w = q[3]
00026     pose.position.x = p[0]
00027     pose.position.y = p[1]
00028     pose.position.z = p[2]
00029     return pose
00030 
00031 def msgToMat(pose):
00032     return concatenate_matrices(translation_matrix([pose.position.x,
00033                                                     pose.position.y,
00034                                                     pose.position.z]),
00035                                 quaternion_matrix([pose.orientation.x,
00036                                                    pose.orientation.y,
00037                                                    pose.orientation.z,
00038                                                    pose.orientation.w]))
00039 def transRotToMat(trans, rot):
00040     return concatenate_matrices(translation_matrix([trans[0],
00041                                                     trans[1],
00042                                                     trans[2]]),
00043                                 quaternion_matrix([rot[0],
00044                                                    rot[1],
00045                                                    rot[2],
00046                                                    rot[3]]))
00047 
00048 def sendFootstep(footstep):
00049     pub_footsteps.publish(footstep)
00050     goal = ExecFootstepsGoal(footstep=footstep)
00051     client.send_goal(goal)
00052     client.wait_for_result()
00053 
00054 def oneFootstep(origin, leg, x=0, y=0, z=0, roll=0, pitch=0, yaw=0):
00055     msg = Footstep()
00056     msg.leg = leg
00057     translation = [x, y, z]
00058     q = quaternion_from_euler(roll, pitch, yaw)
00059     offset = transRotToMat(translation, q)
00060     target = concatenate_matrices(origin, offset)
00061     print origin
00062     msg.pose = matToMsg(target)
00063     msg.dimensions.x = 0.23
00064     msg.dimensions.y = 0.13
00065     msg.dimensions.z = 0.001
00066     return msg
00067 
00068 def getCurrentLeftLeg():
00069     tf_listener.waitForTransform('lleg_end_coords', 'odom', rospy.Time(0), rospy.Duration(10))
00070     (trans, rot) = tf_listener.lookupTransform('odom', 'lleg_end_coords', rospy.Time(0))
00071     return transRotToMat(trans, rot)
00072 
00073 def currentSpot():
00074     origin = getCurrentLeftLeg()
00075     footsteps = FootstepArray()
00076     footsteps.footsteps = [oneFootstep(origin, Footstep.LEFT, 0, 0),
00077                            oneFootstep(origin, Footstep.RIGHT, 0, -0.2),
00078                            oneFootstep(origin, Footstep.LEFT, 0, 0),
00079                            oneFootstep(origin, Footstep.RIGHT, 0, -0.2),
00080                            oneFootstep(origin, Footstep.LEFT, 0, 0),
00081                            oneFootstep(origin, Footstep.RIGHT, 0, -0.2),
00082                            oneFootstep(origin, Footstep.LEFT, 0, 0),
00083                            oneFootstep(origin, Footstep.RIGHT, 0, -0.2)]
00084     footsteps.header.frame_id = "odom"
00085     footsteps.header.stamp = rospy.Time.now()
00086     sendFootstep(footsteps)
00087 
00088 def straightSteps():
00089     origin = getCurrentLeftLeg()
00090     footsteps = FootstepArray()
00091     footsteps.footsteps = [oneFootstep(origin, Footstep.LEFT, 0, 0),
00092                            oneFootstep(origin, Footstep.RIGHT, 0.2, -0.2),
00093                            oneFootstep(origin, Footstep.LEFT, 0.4, 0),
00094                            oneFootstep(origin, Footstep.RIGHT, 0.6, -0.2),
00095                            oneFootstep(origin, Footstep.LEFT, 0.8, 0),
00096                            oneFootstep(origin, Footstep.RIGHT, 1.0, -0.2),
00097                            oneFootstep(origin, Footstep.LEFT, 1.2, 0),
00098                            oneFootstep(origin, Footstep.RIGHT, 1.2, -0.2)]
00099     footsteps.header.frame_id = "odom"
00100     footsteps.header.stamp = rospy.Time.now()
00101     sendFootstep(footsteps)
00102 
00103 def backSteps():
00104     origin = getCurrentLeftLeg()
00105     footsteps = FootstepArray()
00106     footsteps.footsteps = [oneFootstep(origin, Footstep.LEFT, 0, 0),
00107                            oneFootstep(origin, Footstep.RIGHT, -0.2, -0.2),
00108                            oneFootstep(origin, Footstep.LEFT, -0.4, 0),
00109                            oneFootstep(origin, Footstep.RIGHT, -0.6, -0.2),
00110                            oneFootstep(origin, Footstep.LEFT, -0.8, 0),
00111                            oneFootstep(origin, Footstep.RIGHT, -1.0, -0.2),
00112                            oneFootstep(origin, Footstep.LEFT, -1.2, 0),
00113                            oneFootstep(origin, Footstep.RIGHT, -1.2, -0.2)]
00114     footsteps.header.frame_id = "odom"
00115     footsteps.header.stamp = rospy.Time.now()
00116     sendFootstep(footsteps)
00117     
00118 def stairUp():
00119     origin = getCurrentLeftLeg()
00120     footsteps = FootstepArray()
00121     footsteps.footsteps = [oneFootstep(origin, Footstep.LEFT, 0, 0, 0),
00122                            oneFootstep(origin, Footstep.RIGHT, 0.2, -0.2, 0.0),
00123                            oneFootstep(origin, Footstep.LEFT, 0.4, 0, 0.1),
00124                            oneFootstep(origin, Footstep.RIGHT, 0.6, -0.2, 0.1),
00125                            oneFootstep(origin, Footstep.LEFT, 0.8, 0, 0.2),
00126                            oneFootstep(origin, Footstep.RIGHT, 1.0, -0.2, 0.2),
00127                            oneFootstep(origin, Footstep.LEFT, 1.2, 0, 0.3),
00128                            oneFootstep(origin, Footstep.RIGHT, 1.2, -0.2, 0.3)]
00129     footsteps.header.frame_id = "odom"
00130     footsteps.header.stamp = rospy.Time.now()
00131     sendFootstep(footsteps)
00132     
00133 def stairDown():
00134     origin = getCurrentLeftLeg()
00135     footsteps = FootstepArray()
00136     footsteps.footsteps = [oneFootstep(origin, Footstep.LEFT, 0, 0, 0),
00137                            oneFootstep(origin, Footstep.RIGHT, 0.2, -0.2, 0.0),
00138                            oneFootstep(origin, Footstep.LEFT, 0.4, 0, -0.1),
00139                            oneFootstep(origin, Footstep.RIGHT, 0.6, -0.2, -0.1),
00140                            oneFootstep(origin, Footstep.LEFT, 0.8, 0, -0.2),
00141                            oneFootstep(origin, Footstep.RIGHT, 1.0, -0.2, -0.2),
00142                            oneFootstep(origin, Footstep.LEFT, 1.2, 0, -0.3),
00143                            oneFootstep(origin, Footstep.RIGHT, 1.2, -0.2, -0.3)]
00144     footsteps.header.frame_id = "odom"
00145     footsteps.header.stamp = rospy.Time.now()
00146     sendFootstep(footsteps)
00147 
00148 def slopeUp():
00149     origin = getCurrentLeftLeg()
00150     footsteps = FootstepArray()
00151     footsteps.footsteps = [oneFootstep(origin, Footstep.LEFT, 0, 0, 0),
00152                            oneFootstep(origin, Footstep.RIGHT, 0.2, -0.2, 0.0),
00153                            oneFootstep(origin, Footstep.LEFT, 0.4, 0, 0.1, pitch=-0.2),
00154                            oneFootstep(origin, Footstep.RIGHT, 0.6, -0.2, 0.1, pitch=-0.2),
00155                            oneFootstep(origin, Footstep.LEFT, 0.8, 0, 0.2, pitch=-0.2),
00156                            oneFootstep(origin, Footstep.RIGHT, 1.0, -0.2, 0.2, pitch=-0.2),
00157                            oneFootstep(origin, Footstep.LEFT, 1.2, 0, 0.3, pitch=0.0),
00158                            oneFootstep(origin, Footstep.RIGHT, 1.2, -0.2, 0.3, pitch=0.0)]
00159     footsteps.header.frame_id = "odom"
00160     footsteps.header.stamp = rospy.Time.now()
00161     sendFootstep(footsteps)
00162     
00163 def slopeDown():
00164     origin = getCurrentLeftLeg()
00165     footsteps = FootstepArray()
00166     footsteps.footsteps = [oneFootstep(origin, Footstep.LEFT, 0, 0, 0),
00167                            oneFootstep(origin, Footstep.RIGHT, 0.2, -0.2, 0.0),
00168                            oneFootstep(origin, Footstep.LEFT, 0.4, 0, -0.1, pitch=0.2),
00169                            oneFootstep(origin, Footstep.RIGHT, 0.6, -0.2, -0.1, pitch=0.2),
00170                            oneFootstep(origin, Footstep.LEFT, 0.8, 0, -0.2, pitch=0.2),
00171                            oneFootstep(origin, Footstep.RIGHT, 1.0, -0.2, -0.2, pitch=0.2),
00172                            oneFootstep(origin, Footstep.LEFT, 1.2, 0, -0.3, pitch=0.0),
00173                            oneFootstep(origin, Footstep.RIGHT, 1.2, -0.2, -0.3, pitch=0.0)]
00174     footsteps.header.frame_id = "odom"
00175     footsteps.header.stamp = rospy.Time.now()
00176     sendFootstep(footsteps)
00177 
00178     
00179 if __name__ == "__main__":
00180     rospy.init_node("test_footstep_controller")
00181     tf_listener = tf.TransformListener()
00182     client = actionlib.SimpleActionClient('footstep_controller', ExecFootstepsAction)
00183     pub_footsteps = rospy.Publisher('footstep', FootstepArray, latch=True)
00184     rospy.loginfo("Waiting for footstep controller")
00185     client.wait_for_server()
00186     rospy.loginfo("Found footstep controller")
00187     currentSpot()
00188     straightSteps()
00189     backSteps()
00190     stairUp()
00191     stairDown()
00192     slopeUp()
00193     slopeDown()