transformable_joy_configure.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import rospy
4 
5 PKG='sensor_msgs'
6 
7 import imp
8 try:
9  imp.find_module(PKG)
10 except:
11  import roslib;roslib.load_manifest(PKG)
12 
13 from sensor_msgs.msg import Joy
14 from geometry_msgs.msg import *
15 from std_msgs.msg import *
16 
17 x_max = 10
18 y_max = 10
19 z_max = 10
20 r_max = 10
21 sr_max = 10
22 
23 x_small_max = 1
24 y_small_max = 1
25 z_small_max = 1
26 r_small_max = 1
27 sr_small_max = 1
28 
29 MIN_VALUE=0.001
30 
31 def callback(msg):
32  global x_max, y_max, z_max, r_max, sr_max
33  global x_small_max, y_small_max, z_small_max, r_small_max, sr_small_max
34  a = msg.axes
35  b = msg.buttons
36 
37  x_v1,y_v1,z_v1,r_v1,sr_v1 = a[13:]
38  x_v2,y_v2,z_v2,r_v2,sr_v2 = a[0:5]
39 
40  x = Float32()
41  y = Float32()
42  z = Float32()
43  r = Float32()
44  sr= Float32()
45 
46  x.data = x_max * (x_v1+1)/2 + x_small_max * (x_v2+1)/2;
47  y.data = y_max * (y_v1+1)/2 + y_small_max * (y_v2+1)/2;
48  z.data = z_max * (z_v1+1)/2 + z_small_max * (z_v2+1)/2;
49  r.data = r_max * (r_v1+1)/2 + r_small_max * (r_v2+1)/2;
50  sr.data= sr_max * (sr_v1+1)/2 + sr_small_max * (sr_v2+1)/2;
51 
52  x.data = max(MIN_VALUE,x.data)
53  y.data = max(MIN_VALUE,y.data)
54  z.data = max(MIN_VALUE,z.data)
55  r.data = max(MIN_VALUE,r.data)
56  sr.data = max(MIN_VALUE,sr.data)
57 
58  set_x_pub.publish(x)
59  set_y_pub.publish(y)
60  set_z_pub.publish(z)
61  set_r_pub.publish(r)
62  set_sr_pub.publish(sr)
63 
64 if __name__ == "__main__":
65  rospy.init_node("transformable_joy_configure")
66  set_x_pub = rospy.Publisher("set_x", Float32)
67  set_y_pub = rospy.Publisher("set_y", Float32)
68  set_z_pub = rospy.Publisher("set_z", Float32)
69  set_r_pub = rospy.Publisher("set_radius", Float32)
70  set_sr_pub = rospy.Publisher("set_small_radius", Float32)
71 
72  x_max = rospy.get_param("~x_max")
73  y_max = rospy.get_param("~y_max")
74  z_max = rospy.get_param("~z_max")
75  r_max = rospy.get_param("~r_max")
76  sr_max = rospy.get_param("~sr_max")
77 
78  x_small_max = rospy.get_param("~x_small_max")
79  y_small_max = rospy.get_param("~y_small_max")
80  z_small_max = rospy.get_param("~z_small_max")
81  r_small_max = rospy.get_param("~r_small_max")
82  sr_small_max = rospy.get_param("~sr_small_max")
83 
84 
85  s = rospy.Subscriber("input_joy", Joy, callback)
86  rospy.spin()
87 


jsk_interactive_marker
Author(s): furuta
autogenerated on Sat Mar 20 2021 03:03:33