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
00017 x_max = 10
00018 y_max = 10
00019 z_max = 10
00020 r_max = 10
00021 sr_max = 10
00022
00023 x_small_max = 1
00024 y_small_max = 1
00025 z_small_max = 1
00026 r_small_max = 1
00027 sr_small_max = 1
00028
00029 MIN_VALUE=0.001
00030
00031 def callback(msg):
00032 global x_max, y_max, z_max, r_max, sr_max
00033 global x_small_max, y_small_max, z_small_max, r_small_max, sr_small_max
00034 a = msg.axes
00035 b = msg.buttons
00036
00037 x_v1,y_v1,z_v1,r_v1,sr_v1 = a[13:]
00038 x_v2,y_v2,z_v2,r_v2,sr_v2 = a[0:5]
00039
00040 x = Float32()
00041 y = Float32()
00042 z = Float32()
00043 r = Float32()
00044 sr= Float32()
00045
00046 x.data = x_max * (x_v1+1)/2 + x_small_max * (x_v2+1)/2;
00047 y.data = y_max * (y_v1+1)/2 + y_small_max * (y_v2+1)/2;
00048 z.data = z_max * (z_v1+1)/2 + z_small_max * (z_v2+1)/2;
00049 r.data = r_max * (r_v1+1)/2 + r_small_max * (r_v2+1)/2;
00050 sr.data= sr_max * (sr_v1+1)/2 + sr_small_max * (sr_v2+1)/2;
00051
00052 x.data = max(MIN_VALUE,x.data)
00053 y.data = max(MIN_VALUE,y.data)
00054 z.data = max(MIN_VALUE,z.data)
00055 r.data = max(MIN_VALUE,r.data)
00056 sr.data = max(MIN_VALUE,sr.data)
00057
00058 set_x_pub.publish(x)
00059 set_y_pub.publish(y)
00060 set_z_pub.publish(z)
00061 set_r_pub.publish(r)
00062 set_sr_pub.publish(sr)
00063
00064 if __name__ == "__main__":
00065 rospy.init_node("transformable_joy_configure")
00066 set_x_pub = rospy.Publisher("set_x", Float32)
00067 set_y_pub = rospy.Publisher("set_y", Float32)
00068 set_z_pub = rospy.Publisher("set_z", Float32)
00069 set_r_pub = rospy.Publisher("set_radius", Float32)
00070 set_sr_pub = rospy.Publisher("set_small_radius", Float32)
00071
00072 x_max = rospy.get_param("~x_max")
00073 y_max = rospy.get_param("~y_max")
00074 z_max = rospy.get_param("~z_max")
00075 r_max = rospy.get_param("~r_max")
00076 sr_max = rospy.get_param("~sr_max")
00077
00078 x_small_max = rospy.get_param("~x_small_max")
00079 y_small_max = rospy.get_param("~y_small_max")
00080 z_small_max = rospy.get_param("~z_small_max")
00081 r_small_max = rospy.get_param("~r_small_max")
00082 sr_small_max = rospy.get_param("~sr_small_max")
00083
00084
00085 s = rospy.Subscriber("input_joy", Joy, callback)
00086 rospy.spin()
00087