joy_footstep.py
Go to the documentation of this file.
1 import imp
2 try:
3  imp.find_module("geometry_msgs")
4 except:
5  import roslib; roslib.load_manifest('jsk_teleop_joy')
6 
7 
8 from geometry_msgs.msg import PoseStamped, Pose
9 from jsk_teleop_joy.joy_pose_6d import JoyPose6D
10 from jsk_footstep_msgs.msg import FootstepArray, Footstep
11 import tf
12 import rospy
13 
15  def __init__(self, position=[0,0,0], angles=[0,0,0], leg=0, pose=None):
16  self.position = position
17  self.angles = angles
18  self.leg = leg
19  def toROSPose(self):
20  pose = Pose()
21  pose.position.x = self.position[0]
22  pose.position.y = self.position[1]
23  pose.position.z = self.position[2]
24  q = tf.transformations.quaternion_from_euler(angles[0], angles[1], angles[2])
25  pose.orientation.x = q[0]
26  pose.orientation.y = q[1]
27  pose.orientation.z = q[2]
28  pose.orientation.w = q[3]
29  return pose
30  def toROSMsg(self):
31  footstep = Footstep()
32  footstep.leg = self.leg
33  footstep.pose = self.toROSPose()
34  return footstep
35 
36 def FootstepCoordsToROSMsg(footsteps):
37  pass
38 
39 class JoyFootstep(JoyPose6D):
40  def __init__(self, name, args):
41  args['publish_pose'] = False
42  JoyPose6D.__init__(self, name, args)
43  self.supportFollowView(True)
44  self.footstep_pub = rospy.Publisher('/footstep', FootstepArray)
45  self.footsteps = []
46  self.frame_id = self.getArg('frame_id', '/map')
47 
48  def joyCB(self, status, history):
49  JoyPose6D.joyCB(self, status, history)
50  footsteps = FootstepArray()
51  footsteps.header.frame_id = self.frame_id
52  footsteps.header.stamp = rospy.Time(0.0)
53 
54  if status.triangle and not history.latest().triangle:
55  # remove the latest one
56  if len(self.footsteps) >= 2:
57  self.footsteps = self.footsteps[:-1]
58  self.pre_pose.pose = self.footsteps[-1].pose
59  elif len(self.footsteps) == 1:
60  self.footsteps = []
61  if len(self.footsteps) == 0:
62  self.pre_pose.pose.position.x = 0
63  self.pre_pose.pose.position.y = 0
64  self.pre_pose.pose.position.z = 0
65  self.pre_pose.pose.orientation.x = 0
66  self.pre_pose.pose.orientation.y = 0
67  self.pre_pose.pose.orientation.z = 0
68  self.pre_pose.pose.orientation.w = 1
69  # pre_pose -> Footstep
70  current_step = Footstep()
71  current_step.pose = self.pre_pose.pose
72  if status.cross and not history.latest().cross:
73  # left
74  current_step.leg = Footstep.LEFT
75  self.footsteps.append(current_step)
76  elif status.circle and not history.latest().circle:
77  # right
78  current_step.leg = Footstep.RIGHT
79  self.footsteps.append(current_step)
80 
81 
82  footsteps.footsteps.extend(self.footsteps)
83  footsteps.footsteps.append(current_step)
84  self.footstep_pub.publish(footsteps)
jsk_teleop_joy.plugin.joy_footstep.FootstepCoords
Definition: joy_footstep.py:14
jsk_teleop_joy.plugin.joy_footstep.JoyFootstep.__init__
def __init__(self, name, args)
Definition: joy_footstep.py:40
jsk_teleop_joy.plugin.joy_footstep.JoyFootstep.joyCB
def joyCB(self, status, history)
Definition: joy_footstep.py:48
jsk_teleop_joy.plugin.joy_footstep.FootstepCoords.angles
angles
Definition: joy_footstep.py:17
jsk_teleop_joy.plugin.joy_footstep.FootstepCoords.toROSPose
def toROSPose(self)
Definition: joy_footstep.py:19
jsk_teleop_joy.plugin.joy_footstep.FootstepCoords.__init__
def __init__(self, position=[0, 0, 0], angles=[0, 0, 0], leg=0, pose=None)
Definition: joy_footstep.py:15
jsk_teleop_joy.plugin.joy_footstep.JoyFootstep.footsteps
footsteps
Definition: joy_footstep.py:45
jsk_teleop_joy.plugin.joy_footstep.FootstepCoords.position
position
Definition: joy_footstep.py:16
jsk_teleop_joy.plugin.joy_footstep.FootstepCoords.leg
leg
Definition: joy_footstep.py:18
jsk_teleop_joy.plugin.joy_footstep.JoyFootstep.footstep_pub
footstep_pub
Definition: joy_footstep.py:44
jsk_teleop_joy.plugin.joy_footstep.FootstepCoordsToROSMsg
def FootstepCoordsToROSMsg(footsteps)
Definition: joy_footstep.py:36
jsk_teleop_joy.plugin.joy_footstep.JoyFootstep.frame_id
frame_id
Definition: joy_footstep.py:46
jsk_teleop_joy.plugin.joy_footstep.FootstepCoords.toROSMsg
def toROSMsg(self)
Definition: joy_footstep.py:30


jsk_teleop_joy
Author(s): Ryohei Ueda
autogenerated on Wed Jan 24 2024 04:05:49