joy_footstep_planner_demo.py
Go to the documentation of this file.
1 import rospy
2 
3 import imp
4 try:
5  imp.find_module("actionlib")
6 except:
7  import roslib; roslib.load_manifest('jsk_teleop_joy')
8 
9 import actionlib
10 from jsk_teleop_joy.joy_pose_6d import JoyPose6D
11 from jsk_footstep_msgs.msg import PlanFootstepsAction, PlanFootstepsGoal, Footstep, FootstepArray
12 from std_msgs.msg import UInt8
13 from geometry_msgs.msg import Pose
14 import tf
15 from tf.transformations import *
16 import numpy
17 
18 def poseToMatrix(pose):
19  mat = quaternion_matrix(numpy.array((pose.orientation.x,
20  pose.orientation.y,
21  pose.orientation.z,
22  pose.orientation.w)))
23 
24  mat[0, 3] = pose.position.x
25  mat[1, 3] = pose.position.y
26  mat[2, 3] = pose.position.z
27  return mat
28 def matrixToPose(mat):
29  q = quaternion_from_matrix(mat)
30  pose = Pose()
31  pose.orientation.x = q[0]
32  pose.orientation.y = q[1]
33  pose.orientation.z = q[2]
34  pose.orientation.w = q[3]
35  pose.position.x = mat[0, 3]
36  pose.position.y = mat[1, 3]
37  pose.position.z = mat[2, 3]
38  return pose
39 
40 
41 class JoyFootstepPlannerDemo(JoyPose6D):
42  def __init__(self, name, args):
43  JoyPose6D.__init__(self, name, args)
44  self.supportFollowView(True)
45  self.frame_id = self.getArg('frame_id', '/map')
46  self.lleg_frame_id = self.getArg('lleg_frame_id', '/lfsensor')
47  self.rleg_frame_id = self.getArg('rleg_frame_id', '/rfsensor')
49  self.lleg_pose = Pose()
50  self.lleg_pose.position.y = 0.1
51  self.lleg_pose.orientation.w = 1.0
52  self.rleg_pose = Pose()
53  self.rleg_pose.position.y = -0.1
54  self.rleg_pose.orientation.w = 1.0
55  self.command_pub = rospy.Publisher('/menu_command', UInt8)
56  def joyCB(self, status, history):
57  JoyPose6D.joyCB(self, status, history)
58  # broad cast tf
59  self.br.sendTransform((self.lleg_pose.position.x, self.lleg_pose.position.y, self.lleg_pose.position.z),
60  (self.lleg_pose.orientation.x, self.lleg_pose.orientation.y, self.lleg_pose.orientation.z, self.lleg_pose.orientation.w),
61  rospy.Time.now(),
62  self.lleg_frame_id,
63  self.frame_id)
64  self.br.sendTransform((self.rleg_pose.position.x, self.rleg_pose.position.y, self.rleg_pose.position.z),
65  (self.rleg_pose.orientation.x, self.rleg_pose.orientation.y, self.rleg_pose.orientation.z, self.rleg_pose.orientation.w),
66  rospy.Time.now(),
67  self.rleg_frame_id,
68  self.frame_id)
69 
70  latest = history.latest()
71  if not latest:
72  return
73  if status.triangle and not latest.triangle:
74  self.command_pub.publish(UInt8(1))
75  elif status.cross and not latest.cross:
76  self.command_pub.publish(UInt8(2))
77  if status.circle and not latest.circle:
78  base_mat = poseToMatrix(self.pre_pose.pose)
79  lleg_offset = Pose()
80  lleg_offset.position.y = 0.1
81  lleg_offset.orientation.w = 1.0
82  rleg_offset = Pose()
83  rleg_offset.position.y = -0.1
84  rleg_offset.orientation.w = 1.0
85 
86  left_offset_mat = poseToMatrix(lleg_offset)
87  right_offset_mat = poseToMatrix(rleg_offset)
88  new_lleg_mat = numpy.dot(base_mat, left_offset_mat)
89  new_rleg_mat = numpy.dot(base_mat, right_offset_mat)
90  self.lleg_pose = matrixToPose(new_lleg_mat)
91  self.rleg_pose = matrixToPose(new_rleg_mat)
92  # update the position of tf
93  # self.pre_pose
94 
jsk_teleop_joy.plugin.joy_footstep_planner_demo.JoyFootstepPlannerDemo.rleg_pose
rleg_pose
Definition: joy_footstep_planner_demo.py:52
jsk_teleop_joy.plugin.joy_footstep_planner_demo.JoyFootstepPlannerDemo.lleg_pose
lleg_pose
Definition: joy_footstep_planner_demo.py:49
transformations
jsk_teleop_joy.plugin.joy_footstep_planner_demo.JoyFootstepPlannerDemo.__init__
def __init__(self, name, args)
Definition: joy_footstep_planner_demo.py:42
jsk_teleop_joy.plugin.joy_footstep_planner_demo.JoyFootstepPlannerDemo.br
br
Definition: joy_footstep_planner_demo.py:48
tf::TransformBroadcaster
jsk_teleop_joy.plugin.joy_footstep_planner_demo.poseToMatrix
def poseToMatrix(pose)
Definition: joy_footstep_planner_demo.py:18
jsk_teleop_joy.plugin.joy_footstep_planner_demo.JoyFootstepPlannerDemo.frame_id
frame_id
Definition: joy_footstep_planner_demo.py:45
jsk_teleop_joy.plugin.joy_footstep_planner_demo.JoyFootstepPlannerDemo.rleg_frame_id
rleg_frame_id
Definition: joy_footstep_planner_demo.py:47
jsk_teleop_joy.plugin.joy_footstep_planner_demo.JoyFootstepPlannerDemo.joyCB
def joyCB(self, status, history)
Definition: joy_footstep_planner_demo.py:56
jsk_teleop_joy.plugin.joy_footstep_planner_demo.JoyFootstepPlannerDemo
Definition: joy_footstep_planner_demo.py:41
jsk_teleop_joy.plugin.joy_footstep_planner_demo.JoyFootstepPlannerDemo.command_pub
command_pub
Definition: joy_footstep_planner_demo.py:55
jsk_teleop_joy.plugin.joy_footstep_planner_demo.JoyFootstepPlannerDemo.lleg_frame_id
lleg_frame_id
Definition: joy_footstep_planner_demo.py:46
jsk_teleop_joy.plugin.joy_footstep_planner_demo.matrixToPose
def matrixToPose(mat)
Definition: joy_footstep_planner_demo.py:28


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