joy_footstep_planner_demo.py
Go to the documentation of this file.
00001 import rospy
00002 
00003 import imp
00004 try:
00005   imp.find_module("actionlib")
00006 except:
00007   import roslib; roslib.load_manifest('jsk_teleop_joy')
00008 
00009 import actionlib
00010 from joy_pose_6d import JoyPose6D
00011 from jsk_footstep_msgs.msg import PlanFootstepsAction, PlanFootstepsGoal, Footstep, FootstepArray
00012 from std_msgs.msg import UInt8
00013 from geometry_msgs.msg import Pose
00014 import tf
00015 from tf.transformations import *
00016 import numpy
00017 
00018 def poseToMatrix(pose):
00019     mat = quaternion_matrix(numpy.array((pose.orientation.x,
00020                                          pose.orientation.y,
00021                                          pose.orientation.z,
00022                                          pose.orientation.w)))
00023                                          
00024     mat[0, 3] = pose.position.x
00025     mat[1, 3] = pose.position.y
00026     mat[2, 3] = pose.position.z
00027     return mat
00028 def matrixToPose(mat):
00029     q = quaternion_from_matrix(mat)
00030     pose = Pose()
00031     pose.orientation.x = q[0]
00032     pose.orientation.y = q[1]
00033     pose.orientation.z = q[2]
00034     pose.orientation.w = q[3]
00035     pose.position.x = mat[0, 3]
00036     pose.position.y = mat[1, 3]
00037     pose.position.z = mat[2, 3]
00038     return pose
00039     
00040 
00041 class JoyFootstepPlannerDemo(JoyPose6D):
00042   def __init__(self, name, args):
00043     JoyPose6D.__init__(self, name, args)
00044     self.supportFollowView(True)
00045     self.frame_id = self.getArg('frame_id', '/map')
00046     self.lleg_frame_id = self.getArg('lleg_frame_id', '/lfsensor')
00047     self.rleg_frame_id = self.getArg('rleg_frame_id', '/rfsensor')
00048     self.br = tf.TransformBroadcaster()
00049     self.lleg_pose = Pose()
00050     self.lleg_pose.position.y = 0.1
00051     self.lleg_pose.orientation.w = 1.0
00052     self.rleg_pose = Pose()
00053     self.rleg_pose.position.y = -0.1
00054     self.rleg_pose.orientation.w = 1.0
00055     self.command_pub = rospy.Publisher('/menu_command', UInt8)
00056   def joyCB(self, status, history):
00057     JoyPose6D.joyCB(self, status, history)
00058     # broad cast tf
00059     self.br.sendTransform((self.lleg_pose.position.x, self.lleg_pose.position.y, self.lleg_pose.position.z),
00060                      (self.lleg_pose.orientation.x, self.lleg_pose.orientation.y, self.lleg_pose.orientation.z, self.lleg_pose.orientation.w),
00061                      rospy.Time.now(),
00062                      self.lleg_frame_id,
00063                      self.frame_id)
00064     self.br.sendTransform((self.rleg_pose.position.x, self.rleg_pose.position.y, self.rleg_pose.position.z),
00065                      (self.rleg_pose.orientation.x, self.rleg_pose.orientation.y, self.rleg_pose.orientation.z, self.rleg_pose.orientation.w),
00066                      rospy.Time.now(),
00067                      self.rleg_frame_id,
00068                      self.frame_id)
00069 
00070     latest = history.latest()
00071     if not latest:
00072       return
00073     if status.triangle and not latest.triangle:
00074       self.command_pub.publish(UInt8(1))
00075     elif status.cross and not latest.cross:
00076       self.command_pub.publish(UInt8(2))
00077     if status.circle and not latest.circle:
00078       base_mat = poseToMatrix(self.pre_pose.pose)
00079       lleg_offset = Pose()
00080       lleg_offset.position.y = 0.1
00081       lleg_offset.orientation.w = 1.0
00082       rleg_offset = Pose()
00083       rleg_offset.position.y = -0.1
00084       rleg_offset.orientation.w = 1.0
00085       
00086       left_offset_mat = poseToMatrix(lleg_offset)
00087       right_offset_mat = poseToMatrix(rleg_offset)
00088       new_lleg_mat = numpy.dot(base_mat, left_offset_mat)
00089       new_rleg_mat = numpy.dot(base_mat, right_offset_mat)
00090       self.lleg_pose = matrixToPose(new_lleg_mat)
00091       self.rleg_pose = matrixToPose(new_rleg_mat)
00092       # update the position of tf
00093       # self.pre_pose
00094     


jsk_teleop_joy
Author(s): Ryohei Ueda
autogenerated on Wed Jul 19 2017 02:54:50