joy_pose_6d.py
Go to the documentation of this file.
1 from jsk_teleop_joy.plugin.joy_rviz_view_controller import RVizViewController
2 
3 import imp
4 try:
5  imp.find_module("geometry_msgs")
6 except:
7  import roslib; roslib.load_manifest('jsk_teleop_joy')
8 
9 
10 from geometry_msgs.msg import PoseStamped
11 import tf
12 import rospy
13 import numpy
14 import math
15 import tf
16 import numpy
17 import time
18 
19 def signedSquare(val):
20  if val > 0:
21  sign = 1
22  else:
23  sign = -1
24  return val * val * sign
25 
27  '''
28 Usage:
29 Left Analog x/y: translate x/y
30 Up/Down/Right/Left: rotate pitch/roll
31 L1/R2: rotate yaw
32 L2/R2: translate z
33 square: move faster
34 
35 Right Analog x/y: yaw/pitch of camera position (see parent class, RVizViewController)
36 R3(Right Analog button): suppressing buttons/sticks for controlling pose
37  R3 + L2 + R2: enable follow view mode
38 
39 Args:
40 publish_pose [Boolean, default: True]: Publish or not pose
41 frame_id [String, default: map]: frame_id of publishing pose, this is overwritten by parameter, ~frame_id
42 pose [String, default: pose]: topic name for publishing pose
43 set_pose [String, default: set_pose]: topic name for setting pose by topic
44  '''
45  #def __init__(self, name='JoyPose6D', publish_pose=True):
46  def __init__(self, name, args):
47  RVizViewController.__init__(self, name, args)
48  self.pre_pose = PoseStamped()
49  self.pre_pose.pose.orientation.w = 1
50  self.prev_time = rospy.Time.from_sec(time.time())
51  self.publish_pose = self.getArg('publish_pose', True)
52  self.frame_id = self.getArg('frame_id', 'map')
53  if self.publish_pose:
54  self.pose_pub = rospy.Publisher(self.getArg('pose', 'pose'),
55  PoseStamped)
56  self.supportFollowView(True)
57 
58  self.puse_sub = rospy.Subscriber(self.getArg('set_pose', 'set_pose'), PoseStamped, self.setPoseCB)
59  if rospy.has_param('~frame_id'):
60  self.frame_id = rospy.get_param('~frame_id')
62 
63  def setPoseCB(self, pose):
64  pose.header.stamp = rospy.Time(0)
65  self.pre_pose = self.tf_listener.transformPose(self.frame_id, pose)
66 
67  if self.publish_pose:
68  self.pose_pub.publish(self.pre_pose)
69 
70  def joyCB(self, status, history):
71  pre_pose = self.pre_pose
72  if history.length() > 0:
73  latest = history.latest()
74  if status.R3 and status.L2 and status.R2 and not (latest.R3 and latest.L2 and latest.R2):
75  self.followView(not self.followView())
76  if self.control_view:
77  RVizViewController.joyCB(self, status, history)
78  new_pose = PoseStamped()
79  new_pose.header.frame_id = self.frame_id
80  new_pose.header.stamp = rospy.Time(0.0)
81  # move in local
82  if not status.R3:
83  # xy
84  if status.square:
85  scale = 10.0
86  else:
87  dist = status.left_analog_y * status.left_analog_y + status.left_analog_x * status.left_analog_x
88  if dist > 0.9:
89  scale = 20.0
90  else:
91  scale = 60.0
92  x_diff = signedSquare(status.left_analog_y) / scale
93  y_diff = signedSquare(status.left_analog_x) / scale
94  # z
95  if status.L2:
96  z_diff = 0.005
97  elif status.R2:
98  z_diff = -0.005
99  else:
100  z_diff = 0.0
101  if status.square:
102  z_scale = 10.0
103  elif history.all(lambda s: s.L2) or history.all(lambda s: s.R2):
104  z_scale = 4.0
105  else:
106  z_scale = 2.0
107  local_move = numpy.array((x_diff, y_diff,
108  z_diff * z_scale,
109  1.0))
110  else:
111  local_move = numpy.array((0.0, 0.0, 0.0, 1.0))
112  q = numpy.array((pre_pose.pose.orientation.x,
113  pre_pose.pose.orientation.y,
114  pre_pose.pose.orientation.z,
115  pre_pose.pose.orientation.w))
116  xyz_move = numpy.dot(tf.transformations.quaternion_matrix(q),
117  local_move)
118  new_pose.pose.position.x = pre_pose.pose.position.x + xyz_move[0]
119  new_pose.pose.position.y = pre_pose.pose.position.y + xyz_move[1]
120  new_pose.pose.position.z = pre_pose.pose.position.z + xyz_move[2]
121  roll = 0.0
122  pitch = 0.0
123  yaw = 0.0
124  DTHETA = 0.02
125  if not status.R3:
126  if status.L1:
127  if status.square:
128  yaw = yaw + DTHETA * 5
129  elif history.all(lambda s: s.L1):
130  yaw = yaw + DTHETA * 2
131  else:
132  yaw = yaw + DTHETA
133  elif status.R1:
134  if status.square:
135  yaw = yaw - DTHETA * 5
136  elif history.all(lambda s: s.R1):
137  yaw = yaw - DTHETA * 2
138  else:
139  yaw = yaw - DTHETA
140  if status.up:
141  if status.square:
142  pitch = pitch + DTHETA * 5
143  elif history.all(lambda s: s.up):
144  pitch = pitch + DTHETA * 2
145  else:
146  pitch = pitch + DTHETA
147  elif status.down:
148  if status.square:
149  pitch = pitch - DTHETA * 5
150  elif history.all(lambda s: s.down):
151  pitch = pitch - DTHETA * 2
152  else:
153  pitch = pitch - DTHETA
154  if status.right:
155  if status.square:
156  roll = roll + DTHETA * 5
157  elif history.all(lambda s: s.right):
158  roll = roll + DTHETA * 2
159  else:
160  roll = roll + DTHETA
161  elif status.left:
162  if status.square:
163  roll = roll - DTHETA * 5
164  elif history.all(lambda s: s.left):
165  roll = roll - DTHETA * 2
166  else:
167  roll = roll - DTHETA
168  diff_q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
169  new_q = tf.transformations.quaternion_multiply(q, diff_q)
170  new_pose.pose.orientation.x = new_q[0]
171  new_pose.pose.orientation.y = new_q[1]
172  new_pose.pose.orientation.z = new_q[2]
173  new_pose.pose.orientation.w = new_q[3]
174 
175  # publish at 10hz
176  if self.publish_pose:
177  now = rospy.Time.from_sec(time.time())
178  # placement.time_from_start = now - self.prev_time
179  if (now - self.prev_time).to_sec() > 1 / 30.0:
180  self.pose_pub.publish(new_pose)
181  self.prev_time = now
182 
183  self.pre_pose = new_pose
def joyCB(self, status, history)
Definition: joy_pose_6d.py:70
def getArg(self, key, default=None)
Definition: joy_plugin.py:19


jsk_teleop_joy
Author(s): Ryohei Ueda
autogenerated on Sun May 28 2023 03:03:37