def __init__(self, name='JoyPose6D', publish_pose=True):
RVizViewController.__init__(self, name)
self.pre_pose = PoseStamped()
self.pre_pose.pose.orientation.w = 1
self.publish_pose = publish_pose
if self.publish_pose:
self.pose_pub = rospy.Publisher('pose', PoseStamped)
self.support_follow_view = True
self.frame_id = rospy.get_param('~frame_id', '/map')
def __init__(self, name='JoyPose6D', publish_pose=True):
RVizViewController.__init__(self, name)
self.pre_pose = PoseStamped()
self.pre_pose.pose.orientation.w = 1
self.publish_pose = publish_pose
if self.publish_pose:
self.pose_pub = rospy.Publisher('pose', PoseStamped)
self.support_follow_view = True
self.frame_id = rospy.get_param('~frame_id', '/map')
def __init__(self, name, args):
RVizViewController.__init__(self, name, args)
self.pre_pose = PoseStamped()
self.pre_pose.pose.orientation.w = 1
self.publish_pose = self.getArg('publish_pose', True)
self.frame_id = self.getArg('frame_id', '/map')
if self.publish_pose:
self.pose_pub = rospy.Publisher(self.getArg('pose', 'pose'),
PoseStamped)
self.supportFollowView(True)
self.puse_sub = rospy.Subscriber('set_pose', PoseStamped, self.setPoseCB)
self.frame_id = rospy.get_param('~frame_id', '/map')
self.tf_listener = tf.TransformListener()
def __init__(self, name, args):
RVizViewController.__init__(self, name, args)
self.pre_pose = PoseStamped()
self.pre_pose.pose.orientation.w = 1
self.prev_time = rospy.Time.from_sec(time.time())
self.publish_pose = self.getArg('publish_pose', True)
self.frame_id = self.getArg('frame_id', '/map')
if self.publish_pose:
self.pose_pub = rospy.Publisher(self.getArg('pose', 'pose'),
PoseStamped)
self.supportFollowView(True)
self.puse_sub = rospy.Subscriber(self.getArg('set_pose', 'set_pose'),
PoseStamped, self.setPoseCB)
self.frame_id = rospy.get_param('~frame_id', '/map')
self.tf_listener = tf.TransformListener()
def joyCB(self, status, history):
pre_pose = self.pre_pose
if history.length() > 0:
latest = history.latest()
if status.R3 and status.L2 and status.R2 and not (latest.R3 and latest.L2 and latest.R2):
self.follow_view = not self.follow_view
RVizViewController.joyCB(self, status, history)
new_pose = PoseStamped()
new_pose.header.frame_id = self.frame_id
new_pose.header.stamp = rospy.Time(0.0)
# move in local
if not status.R3:
# xy
if status.square:
scale = 10.0
else:
dist = status.left_analog_y * status.left_analog_y + status.left_analog_x * status.left_analog_x
if dist > 0.9:
scale = 20.0
else:
scale = 60.0
x_diff = signedSquare(status.left_analog_y) / scale
y_diff = signedSquare(status.left_analog_x) / scale
# z
if status.L2:
z_diff = 0.005
elif status.R2:
z_diff = -0.005
else:
z_diff = 0.0
if status.square:
z_scale = 10.0
elif history.all(lambda s: s.L2) or history.all(lambda s: s.R2):
z_scale = 4.0
else:
z_scale = 2.0
local_move = numpy.array((x_diff, y_diff,
z_diff * z_scale,
1.0))
else:
local_move = numpy.array((0.0, 0.0, 0.0, 1.0))
q = numpy.array((pre_pose.pose.orientation.x,
pre_pose.pose.orientation.y,
pre_pose.pose.orientation.z,
pre_pose.pose.orientation.w))
xyz_move = numpy.dot(tf.transformations.quaternion_matrix(q),
local_move)
new_pose.pose.position.x = pre_pose.pose.position.x + xyz_move[0]
new_pose.pose.position.y = pre_pose.pose.position.y + xyz_move[1]
new_pose.pose.position.z = pre_pose.pose.position.z + xyz_move[2]
roll = 0.0
pitch = 0.0
yaw = 0.0
DTHETA = 0.02
if not status.R3:
if status.L1:
if status.square:
yaw = yaw + DTHETA * 5
elif history.all(lambda s: s.L1):
yaw = yaw + DTHETA * 2
else:
yaw = yaw + DTHETA
elif status.R1:
if status.square:
yaw = yaw - DTHETA * 5
elif history.all(lambda s: s.R1):
yaw = yaw - DTHETA * 2
else:
yaw = yaw - DTHETA
if status.up:
if status.square:
pitch = pitch + DTHETA * 5
elif history.all(lambda s: s.up):
pitch = pitch + DTHETA * 2
else:
pitch = pitch + DTHETA
elif status.down:
if status.square:
pitch = pitch - DTHETA * 5
elif history.all(lambda s: s.down):
pitch = pitch - DTHETA * 2
else:
pitch = pitch - DTHETA
if status.right:
if status.square:
roll = roll + DTHETA * 5
elif history.all(lambda s: s.right):
roll = roll + DTHETA * 2
else:
roll = roll + DTHETA
elif status.left:
if status.square:
roll = roll - DTHETA * 5
elif history.all(lambda s: s.left):
roll = roll - DTHETA * 2
else:
roll = roll - DTHETA
diff_q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
new_q = tf.transformations.quaternion_multiply(q, diff_q)
new_pose.pose.orientation.x = new_q[0]
new_pose.pose.orientation.y = new_q[1]
new_pose.pose.orientation.z = new_q[2]
new_pose.pose.orientation.w = new_q[3]
if self.publish_pose:
self.pose_pub.publish(new_pose)
self.pre_pose = new_pose
def joyCB(self, status, history):
pre_pose = self.pre_pose
if history.length() > 0:
latest = history.latest()
if status.R3 and status.L2 and status.R2 and not (
latest.R3 and latest.L2 and latest.R2):
self.followView(not self.followView())
RVizViewController.joyCB(self, status, history)
new_pose = PoseStamped()
new_pose.header.frame_id = self.frame_id
new_pose.header.stamp = rospy.Time(0.0)
# move in local
if not status.R3:
# xy
if status.square:
scale = 10.0
else:
dist = status.left_analog_y * status.left_analog_y + status.left_analog_x * status.left_analog_x
if dist > 0.9:
scale = 20.0
else:
scale = 60.0
x_diff = signedSquare(status.left_analog_y) / scale
y_diff = signedSquare(status.left_analog_x) / scale
# z
if status.L2:
z_diff = 0.005
elif status.R2:
z_diff = -0.005
else:
z_diff = 0.0
if status.square:
z_scale = 10.0
elif history.all(lambda s: s.L2) or history.all(lambda s: s.R2):
z_scale = 4.0
else:
z_scale = 2.0
local_move = numpy.array((x_diff, y_diff, z_diff * z_scale, 1.0))
else:
local_move = numpy.array((0.0, 0.0, 0.0, 1.0))
q = numpy.array(
(pre_pose.pose.orientation.x, pre_pose.pose.orientation.y,
pre_pose.pose.orientation.z, pre_pose.pose.orientation.w))
xyz_move = numpy.dot(tf.transformations.quaternion_matrix(q),
local_move)
new_pose.pose.position.x = pre_pose.pose.position.x + xyz_move[0]
new_pose.pose.position.y = pre_pose.pose.position.y + xyz_move[1]
new_pose.pose.position.z = pre_pose.pose.position.z + xyz_move[2]
roll = 0.0
pitch = 0.0
yaw = 0.0
DTHETA = 0.02
if not status.R3:
if status.L1:
if status.square:
yaw = yaw + DTHETA * 5
elif history.all(lambda s: s.L1):
yaw = yaw + DTHETA * 2
else:
yaw = yaw + DTHETA
elif status.R1:
if status.square:
yaw = yaw - DTHETA * 5
elif history.all(lambda s: s.R1):
yaw = yaw - DTHETA * 2
else:
yaw = yaw - DTHETA
if status.up:
if status.square:
pitch = pitch + DTHETA * 5
elif history.all(lambda s: s.up):
pitch = pitch + DTHETA * 2
else:
pitch = pitch + DTHETA
elif status.down:
if status.square:
pitch = pitch - DTHETA * 5
elif history.all(lambda s: s.down):
pitch = pitch - DTHETA * 2
else:
pitch = pitch - DTHETA
if status.right:
if status.square:
roll = roll + DTHETA * 5
elif history.all(lambda s: s.right):
roll = roll + DTHETA * 2
else:
roll = roll + DTHETA
elif status.left:
if status.square:
roll = roll - DTHETA * 5
elif history.all(lambda s: s.left):
roll = roll - DTHETA * 2
else:
roll = roll - DTHETA
diff_q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
new_q = tf.transformations.quaternion_multiply(q, diff_q)
new_pose.pose.orientation.x = new_q[0]
new_pose.pose.orientation.y = new_q[1]
new_pose.pose.orientation.z = new_q[2]
new_pose.pose.orientation.w = new_q[3]
# publish at 10hz
if self.publish_pose:
now = rospy.Time.from_sec(time.time())
# placement.time_from_start = now - self.prev_time
if (now - self.prev_time).to_sec() > 1 / 30.0:
self.pose_pub.publish(new_pose)
self.prev_time = now
self.pre_pose = new_pose