spacenav2pose.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import rospy
00003 import numpy
00004 import math
00005 import tf
00006 import numpy
00007 import time
00008 
00009 from geometry_msgs.msg import PoseStamped
00010 from sensor_msgs.msg import Joy
00011 
00012 class MoveMode:
00013     WorldMode = 1
00014     LocalMode = 2
00015 
00016 class Spacenav2Pose():
00017     def __init__(self):
00018         rospy.init_node("spacenav2pose")
00019         self.pre_pose = PoseStamped()
00020         self.pre_pose.pose.orientation.w = 1
00021         self.prev_time = rospy.Time.from_sec(time.time())
00022         self.prev_joy = None
00023 
00024         self.publish_pose = True
00025         self.move_mode = MoveMode.WorldMode
00026 
00027         self.frame_id = rospy.get_param('~frame_id', '/map')
00028         if self.publish_pose:
00029             self.pose_pub = rospy.Publisher(rospy.get_param('~pose', 'pose'),
00030                                             PoseStamped)
00031         self.pose_sub = rospy.Subscriber(rospy.get_param('~set_pose', 'set_pose'), PoseStamped, self.setPoseCB)
00032 
00033         joy_topic = rospy.get_param('~joy', '/spacenav/joy')
00034         self.joy_sub = rospy.Subscriber(joy_topic, Joy, self.joyCB)
00035         rospy.logwarn("subscribe " + joy_topic)
00036 
00037         self.x_scale = rospy.get_param('~x_scale', 0.03)
00038         self.y_scale = rospy.get_param('~y_scale', 0.03)
00039         self.z_scale = rospy.get_param('~z_scale', 0.03)
00040 
00041         self.r_scale = rospy.get_param('~r_scale', 0.01)
00042         self.p_scale = rospy.get_param('~p_scale', 0.01)
00043         self.y_scale = rospy.get_param('~y_scale', 0.01)
00044 
00045         self.tf_listener = tf.TransformListener()
00046 
00047         rospy.spin()
00048 
00049     def setPoseCB(self, pose):
00050         pose.header.stamp = rospy.Time(0)
00051         self.pre_pose = self.tf_listener.transformPose(self.frame_id, pose)
00052 
00053         if self.publish_pose:
00054             self.pose_pub.publish(self.pre_pose)
00055 
00056     def joyCB(self, joy):
00057         b0 = joy.buttons[0]
00058         b1 = joy.buttons[1]
00059         if self.prev_joy:
00060             # change move mode
00061             if b0 == 1 and self.prev_joy.buttons[0] == 0:
00062                 if self.move_mode == MoveMode.WorldMode:
00063                     self.move_mode = MoveMode.LocalMode
00064                     rospy.logwarn("change mode to local mode")
00065                 else:
00066                     self.move_mode = MoveMode.WorldMode
00067                     rospy.logwarn("change mode to world mode")
00068 
00069         self.prev_joy = joy
00070 
00071         pre_pose = self.pre_pose
00072 
00073         new_pose = PoseStamped()
00074         new_pose.header.frame_id = self.frame_id
00075         new_pose.header.stamp = rospy.Time(0.0)
00076 
00077         # calculate position
00078         q = numpy.array((pre_pose.pose.orientation.x,
00079                          pre_pose.pose.orientation.y,
00080                          pre_pose.pose.orientation.z,
00081                          pre_pose.pose.orientation.w))
00082 
00083         x_diff = joy.axes[0] * self.x_scale
00084         y_diff = joy.axes[1] * self.y_scale
00085         z_diff = joy.axes[2] * self.z_scale
00086         local_move = numpy.array((x_diff, y_diff, z_diff, 1.0))
00087 
00088         if self.move_mode == MoveMode.WorldMode:
00089             move_q = numpy.array((0.0, 0.0, 0.0, 1.0))
00090         else:
00091             move_q = q
00092 
00093         xyz_move = numpy.dot(tf.transformations.quaternion_matrix(move_q),
00094                              local_move)
00095 
00096         new_pose.pose.position.x = pre_pose.pose.position.x + xyz_move[0]
00097         new_pose.pose.position.y = pre_pose.pose.position.y + xyz_move[1]
00098         new_pose.pose.position.z = pre_pose.pose.position.z + xyz_move[2]
00099 
00100         # calculate orientation
00101         roll =  self.r_scale * joy.axes[3]
00102         pitch =  self.p_scale * joy.axes[4]
00103         yaw =  self.y_scale * joy.axes[5]
00104 
00105         diff_q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
00106 
00107         if self.move_mode == MoveMode.WorldMode:
00108             new_q = tf.transformations.quaternion_multiply(diff_q, q)
00109         else:
00110             new_q = tf.transformations.quaternion_multiply(q, diff_q)
00111 
00112         new_pose.pose.orientation.x = new_q[0]
00113         new_pose.pose.orientation.y = new_q[1]
00114         new_pose.pose.orientation.z = new_q[2]
00115         new_pose.pose.orientation.w = new_q[3]
00116 
00117         # publish at 10hz
00118         if self.publish_pose:
00119             now = rospy.Time.from_sec(time.time())
00120             # placement.time_from_start = now - self.prev_time
00121             if (now - self.prev_time).to_sec() > 1 / 30.0:
00122                 self.pose_pub.publish(new_pose)
00123                 self.prev_time = now
00124 
00125         self.pre_pose = new_pose
00126 
00127 
00128 if __name__ == '__main__':
00129     Spacenav2Pose()


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