joy_footstep.py
Go to the documentation of this file.
00001 import imp
00002 try:
00003   imp.find_module("geometry_msgs")
00004 except:
00005   import roslib; roslib.load_manifest('jsk_teleop_joy')
00006 
00007 
00008 from geometry_msgs.msg import PoseStamped, Pose
00009 from joy_pose_6d import JoyPose6D
00010 from jsk_footstep_msgs.msg import FootstepArray, Footstep
00011 import tf
00012 import rospy
00013 
00014 class FootstepCoords():
00015   def __init__(self, position=[0,0,0], angles=[0,0,0], leg=0, pose=None):
00016     self.position = position
00017     self.angles = angles
00018     self.leg = leg
00019   def toROSPose(self):
00020     pose = Pose()
00021     pose.position.x = self.position[0]
00022     pose.position.y = self.position[1]
00023     pose.position.z = self.position[2]
00024     q = tf.transformations.quaternion_from_euler(angles[0], angles[1], angles[2])
00025     pose.orientation.x = q[0]
00026     pose.orientation.y = q[1]
00027     pose.orientation.z = q[2]
00028     pose.orientation.w = q[3]
00029     return pose
00030   def toROSMsg(self):
00031     footstep = Footstep()
00032     footstep.leg = self.leg
00033     footstep.pose = self.toROSPose()
00034     return footstep
00035 
00036 def FootstepCoordsToROSMsg(footsteps):
00037   pass
00038 
00039 class JoyFootstep(JoyPose6D):
00040   def __init__(self, name, args):
00041     args['publish_pose'] = False
00042     JoyPose6D.__init__(self, name, args)
00043     self.supportFollowView(True)
00044     self.footstep_pub = rospy.Publisher('/footstep', FootstepArray)
00045     self.footsteps = []
00046     self.frame_id = self.getArg('frame_id', '/map')
00047     
00048   def joyCB(self, status, history):
00049     JoyPose6D.joyCB(self, status, history)
00050     footsteps = FootstepArray()
00051     footsteps.header.frame_id = self.frame_id
00052     footsteps.header.stamp = rospy.Time(0.0)
00053     
00054     if status.triangle and not history.latest().triangle:
00055       # remove the latest one
00056       if len(self.footsteps) >= 2:
00057         self.footsteps = self.footsteps[:-1]
00058         self.pre_pose.pose = self.footsteps[-1].pose
00059       elif len(self.footsteps) == 1:
00060         self.footsteps = []
00061       if len(self.footsteps) == 0:
00062         self.pre_pose.pose.position.x = 0
00063         self.pre_pose.pose.position.y = 0
00064         self.pre_pose.pose.position.z = 0
00065         self.pre_pose.pose.orientation.x = 0
00066         self.pre_pose.pose.orientation.y = 0
00067         self.pre_pose.pose.orientation.z = 0
00068         self.pre_pose.pose.orientation.w = 1
00069     # pre_pose -> Footstep
00070     current_step = Footstep()
00071     current_step.pose = self.pre_pose.pose
00072     if status.cross and not history.latest().cross:
00073       # left
00074       current_step.leg = Footstep.LEFT
00075       self.footsteps.append(current_step)
00076     elif status.circle and not history.latest().circle:
00077       # right
00078       current_step.leg = Footstep.RIGHT
00079       self.footsteps.append(current_step)
00080     
00081         
00082     footsteps.footsteps.extend(self.footsteps)
00083     footsteps.footsteps.append(current_step)
00084     self.footstep_pub.publish(footsteps)


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