spacenav2pose.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 import rospy
3 import numpy
4 import math
5 import tf
6 import numpy
7 import time
8 
9 from geometry_msgs.msg import PoseStamped
10 from sensor_msgs.msg import Joy
11 
12 class MoveMode:
13  WorldMode = 1
14  LocalMode = 2
15 
16 class Spacenav2Pose():
17  def __init__(self):
18  rospy.init_node("spacenav2pose")
19  self.pre_pose = PoseStamped()
20  self.pre_pose.pose.orientation.w = 1
21  self.prev_time = rospy.Time.from_sec(time.time())
22  self.prev_joy = None
23 
24  self.publish_pose = True
25  self.move_mode = MoveMode.WorldMode
26 
27  self.frame_id = rospy.get_param('~frame_id', '/map')
28  if self.publish_pose:
29  self.pose_pub = rospy.Publisher(rospy.get_param('~pose', 'pose'),
30  PoseStamped)
31  self.pose_sub = rospy.Subscriber(rospy.get_param('~set_pose', 'set_pose'), PoseStamped, self.setPoseCB)
32 
33  joy_topic = rospy.get_param('~joy', '/spacenav/joy')
34  self.joy_sub = rospy.Subscriber(joy_topic, Joy, self.joyCB)
35  rospy.logwarn("subscribe " + joy_topic)
36 
37  self.x_scale = rospy.get_param('~x_scale', 0.03)
38  self.y_scale = rospy.get_param('~y_scale', 0.03)
39  self.z_scale = rospy.get_param('~z_scale', 0.03)
40 
41  self.r_scale = rospy.get_param('~r_scale', 0.01)
42  self.p_scale = rospy.get_param('~p_scale', 0.01)
43  self.y_scale = rospy.get_param('~y_scale', 0.01)
44 
45  self.tf_listener = tf.TransformListener()
46 
47  rospy.spin()
48 
49  def setPoseCB(self, pose):
50  pose.header.stamp = rospy.Time(0)
51  self.pre_pose = self.tf_listener.transformPose(self.frame_id, pose)
52 
53  if self.publish_pose:
54  self.pose_pub.publish(self.pre_pose)
55 
56  def joyCB(self, joy):
57  b0 = joy.buttons[0]
58  b1 = joy.buttons[1]
59  if self.prev_joy:
60  # change move mode
61  if b0 == 1 and self.prev_joy.buttons[0] == 0:
62  if self.move_mode == MoveMode.WorldMode:
63  self.move_mode = MoveMode.LocalMode
64  rospy.logwarn("change mode to local mode")
65  else:
66  self.move_mode = MoveMode.WorldMode
67  rospy.logwarn("change mode to world mode")
68 
69  self.prev_joy = joy
70 
71  pre_pose = self.pre_pose
72 
73  new_pose = PoseStamped()
74  new_pose.header.frame_id = self.frame_id
75  new_pose.header.stamp = rospy.Time(0.0)
76 
77  # calculate position
78  q = numpy.array((pre_pose.pose.orientation.x,
79  pre_pose.pose.orientation.y,
80  pre_pose.pose.orientation.z,
81  pre_pose.pose.orientation.w))
82 
83  x_diff = joy.axes[0] * self.x_scale
84  y_diff = joy.axes[1] * self.y_scale
85  z_diff = joy.axes[2] * self.z_scale
86  local_move = numpy.array((x_diff, y_diff, z_diff, 1.0))
87 
88  if self.move_mode == MoveMode.WorldMode:
89  move_q = numpy.array((0.0, 0.0, 0.0, 1.0))
90  else:
91  move_q = q
92 
93  xyz_move = numpy.dot(tf.transformations.quaternion_matrix(move_q),
94  local_move)
95 
96  new_pose.pose.position.x = pre_pose.pose.position.x + xyz_move[0]
97  new_pose.pose.position.y = pre_pose.pose.position.y + xyz_move[1]
98  new_pose.pose.position.z = pre_pose.pose.position.z + xyz_move[2]
99 
100  # calculate orientation
101  roll = self.r_scale * joy.axes[3]
102  pitch = self.p_scale * joy.axes[4]
103  yaw = self.y_scale * joy.axes[5]
104 
105  diff_q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
106 
107  if self.move_mode == MoveMode.WorldMode:
108  new_q = tf.transformations.quaternion_multiply(diff_q, q)
109  else:
110  new_q = tf.transformations.quaternion_multiply(q, diff_q)
111 
112  new_pose.pose.orientation.x = new_q[0]
113  new_pose.pose.orientation.y = new_q[1]
114  new_pose.pose.orientation.z = new_q[2]
115  new_pose.pose.orientation.w = new_q[3]
116 
117  # publish at 10hz
118  if self.publish_pose:
119  now = rospy.Time.from_sec(time.time())
120  # placement.time_from_start = now - self.prev_time
121  if (now - self.prev_time).to_sec() > 1 / 30.0:
122  self.pose_pub.publish(new_pose)
123  self.prev_time = now
124 
125  self.pre_pose = new_pose
126 
127 
128 if __name__ == '__main__':
129  Spacenav2Pose()
msg

jsk_teleop_joy
Author(s): Ryohei Ueda
autogenerated on Fri May 14 2021 02:52:11