joy_pose_6d.py
Go to the documentation of this file.
00001 from joy_rviz_view_controller import RVizViewController
00002 
00003 import imp
00004 try:
00005   imp.find_module("geometry_msgs")
00006 except:
00007   import roslib; roslib.load_manifest('jsk_teleop_joy')
00008 
00009 
00010 from geometry_msgs.msg import PoseStamped
00011 import tf
00012 import rospy
00013 import numpy
00014 import math
00015 import tf
00016 import numpy
00017 import time
00018 
00019 def signedSquare(val):
00020  if val > 0:
00021    sign = 1
00022  else:
00023    sign = -1
00024  return val * val * sign
00025 
00026 class JoyPose6D(RVizViewController):
00027   '''
00028 Usage:
00029 Left Analog x/y: translate x/y
00030 Up/Down/Right/Left: rotate pitch/roll
00031 L1/R2: rotate yaw
00032 L2/R2: translate z
00033 square: move faster
00034 
00035 Right Analog x/y: yaw/pitch of camera position (see parent class, RVizViewController)
00036 R3(Right Analog button): suppressing buttons/sticks for controlling pose
00037    R3 + L2 + R2: enable follow view mode
00038 
00039 Args:
00040 publish_pose [Boolean, default: True]: Publish or not pose
00041 frame_id [String, default: map]: frame_id of publishing pose, this is overwritten by parameter, ~frame_id
00042 pose [String, default: pose]: topic name for publishing pose
00043 set_pose [String, default: set_pose]: topic name for setting pose by topic
00044   '''
00045   #def __init__(self, name='JoyPose6D', publish_pose=True):
00046   def __init__(self, name, args):
00047     RVizViewController.__init__(self, name, args)
00048     self.pre_pose = PoseStamped()
00049     self.pre_pose.pose.orientation.w = 1
00050     self.prev_time = rospy.Time.from_sec(time.time())
00051     self.publish_pose = self.getArg('publish_pose', True)
00052     self.frame_id = self.getArg('frame_id', 'map')
00053     if self.publish_pose:
00054       self.pose_pub = rospy.Publisher(self.getArg('pose', 'pose'),
00055                                       PoseStamped)
00056     self.supportFollowView(True)
00057 
00058     self.puse_sub = rospy.Subscriber(self.getArg('set_pose', 'set_pose'), PoseStamped, self.setPoseCB)
00059     if rospy.has_param('~frame_id'):
00060       self.frame_id = rospy.get_param('~frame_id')
00061     self.tf_listener = tf.TransformListener()
00062 
00063   def setPoseCB(self, pose):
00064     pose.header.stamp = rospy.Time(0)
00065     self.pre_pose = self.tf_listener.transformPose(self.frame_id, pose)
00066 
00067     if self.publish_pose:
00068       self.pose_pub.publish(self.pre_pose)
00069 
00070   def joyCB(self, status, history):
00071     pre_pose = self.pre_pose
00072     if history.length() > 0:
00073       latest = history.latest()
00074       if status.R3 and status.L2 and status.R2 and not (latest.R3 and latest.L2 and latest.R2):
00075         self.followView(not self.followView())
00076     if self.control_view:
00077       RVizViewController.joyCB(self, status, history)
00078     new_pose = PoseStamped()
00079     new_pose.header.frame_id = self.frame_id
00080     new_pose.header.stamp = rospy.Time(0.0)
00081     # move in local
00082     if not status.R3:
00083       # xy
00084       if status.square:
00085         scale = 10.0
00086       else:
00087         dist = status.left_analog_y * status.left_analog_y + status.left_analog_x * status.left_analog_x
00088         if dist > 0.9:
00089           scale = 20.0
00090         else:
00091           scale = 60.0
00092       x_diff = signedSquare(status.left_analog_y) / scale
00093       y_diff = signedSquare(status.left_analog_x) / scale
00094       # z
00095       if status.L2:
00096         z_diff = 0.005
00097       elif status.R2:
00098         z_diff = -0.005
00099       else:
00100         z_diff = 0.0
00101       if status.square:
00102         z_scale = 10.0
00103       elif history.all(lambda s: s.L2) or history.all(lambda s: s.R2):
00104         z_scale = 4.0
00105       else:
00106         z_scale = 2.0
00107       local_move = numpy.array((x_diff, y_diff,
00108                                 z_diff * z_scale, 
00109                                 1.0))
00110     else:
00111       local_move = numpy.array((0.0, 0.0, 0.0, 1.0))
00112     q = numpy.array((pre_pose.pose.orientation.x,
00113                      pre_pose.pose.orientation.y,
00114                      pre_pose.pose.orientation.z,
00115                      pre_pose.pose.orientation.w))
00116     xyz_move = numpy.dot(tf.transformations.quaternion_matrix(q),
00117                          local_move)
00118     new_pose.pose.position.x = pre_pose.pose.position.x + xyz_move[0]
00119     new_pose.pose.position.y = pre_pose.pose.position.y + xyz_move[1]
00120     new_pose.pose.position.z = pre_pose.pose.position.z + xyz_move[2]
00121     roll = 0.0
00122     pitch = 0.0
00123     yaw = 0.0
00124     DTHETA = 0.02
00125     if not status.R3:
00126       if status.L1:
00127         if status.square:
00128           yaw = yaw + DTHETA * 5
00129         elif history.all(lambda s: s.L1):
00130           yaw = yaw + DTHETA * 2
00131         else:
00132           yaw = yaw + DTHETA
00133       elif status.R1:
00134         if status.square:
00135           yaw = yaw - DTHETA * 5
00136         elif history.all(lambda s: s.R1):
00137           yaw = yaw - DTHETA * 2
00138         else:
00139           yaw = yaw - DTHETA
00140       if status.up:
00141         if status.square:
00142           pitch = pitch + DTHETA * 5
00143         elif history.all(lambda s: s.up):
00144           pitch = pitch + DTHETA * 2
00145         else:
00146           pitch = pitch + DTHETA
00147       elif status.down:
00148         if status.square:
00149           pitch = pitch - DTHETA * 5
00150         elif history.all(lambda s: s.down):
00151           pitch = pitch - DTHETA * 2
00152         else:
00153           pitch = pitch - DTHETA
00154       if status.right:
00155         if status.square:
00156           roll = roll + DTHETA * 5
00157         elif history.all(lambda s: s.right):
00158           roll = roll + DTHETA * 2
00159         else:
00160           roll = roll + DTHETA
00161       elif status.left:
00162         if status.square:
00163           roll = roll - DTHETA * 5
00164         elif history.all(lambda s: s.left):
00165           roll = roll - DTHETA * 2
00166         else:
00167           roll = roll - DTHETA
00168     diff_q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
00169     new_q = tf.transformations.quaternion_multiply(q, diff_q)
00170     new_pose.pose.orientation.x = new_q[0]
00171     new_pose.pose.orientation.y = new_q[1]
00172     new_pose.pose.orientation.z = new_q[2]
00173     new_pose.pose.orientation.w = new_q[3]
00174 
00175     # publish at 10hz
00176     if self.publish_pose:
00177       now = rospy.Time.from_sec(time.time())
00178       # placement.time_from_start = now - self.prev_time
00179       if (now - self.prev_time).to_sec() > 1 / 30.0:
00180         self.pose_pub.publish(new_pose)
00181         self.prev_time = now
00182 
00183     self.pre_pose = new_pose


jsk_teleop_joy
Author(s): Ryohei Ueda
autogenerated on Fri Apr 19 2019 03:45:43