3 imp.find_module(
"geometry_msgs")
5 import roslib; roslib.load_manifest(
'jsk_teleop_joy')
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
15 def __init__(self, position=[0,0,0], angles=[0,0,0], leg=0, pose=None):
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]
32 footstep.leg = self.
leg 39 class JoyFootstep(JoyPose6D):
41 args[
'publish_pose'] =
False 42 JoyPose6D.__init__(self, name, args)
43 self.supportFollowView(
True)
46 self.
frame_id = self.getArg(
'frame_id',
'/map')
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)
54 if status.triangle
and not history.latest().triangle:
58 self.pre_pose.pose = self.
footsteps[-1].pose
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
70 current_step = Footstep()
71 current_step.pose = self.pre_pose.pose
72 if status.cross
and not history.latest().cross:
74 current_step.leg = Footstep.LEFT
76 elif status.circle
and not history.latest().circle:
78 current_step.leg = Footstep.RIGHT
82 footsteps.footsteps.extend(self.
footsteps)
83 footsteps.footsteps.append(current_step)