Go to the documentation of this file.00001
00002
00003 import rospy
00004
00005 PKG='sensor_msgs'
00006
00007 import imp
00008 try:
00009 imp.find_module(PKG)
00010 except:
00011 import roslib;roslib.load_manifest(PKG)
00012
00013 from sensor_msgs.msg import Joy
00014 from geometry_msgs.msg import *
00015 from std_msgs.msg import *
00016 from tf.transformations import quaternion_from_euler
00017 from jsk_rviz_plugins.msg import OverlayText
00018
00019 x_max = 10
00020 y_max = 10
00021 z_max = 10
00022
00023 set_pose_pub = None
00024 prev_b = 0
00025 translate_mode = True
00026 display_separate_mode = True
00027 def callback(msg):
00028 global x_max, y_max, z_max, rx_max, ry_max, rz_max, set_pose_pub
00029 global prev_b, translate_mode
00030
00031 a = msg.axes
00032 b = msg.buttons
00033
00034 if prev_b == 0 and (b[0] == 1 or b[1] == 1):
00035 translate_mode = not translate_mode
00036
00037 if b[0] == 1 or b[1] == 1:
00038 prev_b = 1
00039 else:
00040 prev_b = 0
00041
00042 x_v1,y_v1,z_v1,rx_v1,ry_v1,rz_v1 = a
00043
00044 target_pose = Pose()
00045 if not separate_mode or (separate_mode and translate_mode):
00046 target_pose.position.x = x_v1 * x_max
00047 target_pose.position.y = y_v1 * y_max
00048 target_pose.position.z = z_v1 * z_max
00049
00050 target_pose.orientation.w = 1
00051 if not separate_mode or (separate_mode and not translate_mode):
00052 q = quaternion_from_euler(rx_v1 * rx_max, ry_v1 * ry_max, rz_v1 * rz_max, 'rxyz')
00053 target_pose.orientation.x = q[0]
00054 target_pose.orientation.y = q[1]
00055 target_pose.orientation.z = q[2]
00056 target_pose.orientation.w = q[3]
00057
00058 set_pose_pub.publish(target_pose)
00059 if translate_mode and separate_mode:
00060 publishModeText("TranslateMode")
00061 elif separate_mode:
00062 publishModeText("RotateMode")
00063
00064 def publishModeText(message):
00065 send_text = OverlayText()
00066 send_text.text = message
00067 send_text.top = 100
00068 send_text.left = 0
00069 send_text.width = 300
00070 send_text.height = 50
00071
00072 send_text.bg_color.r = 0.9;
00073 send_text.bg_color.b = 0.9;
00074 send_text.bg_color.g = 0.9;
00075 send_text.bg_color.a = 0.1;
00076 send_text.fg_color.r = 0.3;
00077 send_text.fg_color.g = 0.8;
00078 send_text.fg_color.b = 0.3;
00079 send_text.fg_color.a = 1;
00080 send_text.line_width = 1;
00081 send_text.text_size = 30;
00082 send_text_pub.publish(send_text);
00083
00084
00085 if __name__ == "__main__":
00086 rospy.init_node("transformable_spacenav_configure")
00087 ns = rospy.get_param('~transformable_interactive_server_nodename', '')
00088
00089 x_max = rospy.get_param("~x_max")
00090 y_max = rospy.get_param("~y_max")
00091 z_max = rospy.get_param("~z_max")
00092 rx_max = rospy.get_param("~rx_max")
00093 ry_max = rospy.get_param("~ry_max")
00094 rz_max = rospy.get_param("~rz_max")
00095
00096 separate_mode = rospy.get_param("~separate_mode")
00097 display_separate_mode = rospy.get_param("~display_separate_mode")
00098
00099 set_pose_pub = rospy.Publisher(ns+"/add_pose", Pose)
00100 send_text_pub = rospy.Publisher("separate_mode_text", OverlayText)
00101
00102 s = rospy.Subscriber("input_joy", Joy, callback)
00103 rospy.spin()
00104