preset_reconfigure.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # coding: utf-8
3 
4 import roslib
5 import rospy
6 import dynamic_reconfigure.client
7 from std_msgs.msg import UInt8
8 
9 class PRESET_RECONFIGURE:
10  def __init__( self ):
11  rospy.loginfo("Wait reconfig server...")
12 
13  self.joint_list = [
14  {"control":"/sciurus17/controller1/joints","joint":"r_arm_joint1"}, \
15  {"control":"/sciurus17/controller1/joints","joint":"r_arm_joint2"}, \
16  {"control":"/sciurus17/controller1/joints","joint":"r_arm_joint3"}, \
17  {"control":"/sciurus17/controller1/joints","joint":"r_arm_joint4"}, \
18  {"control":"/sciurus17/controller1/joints","joint":"r_arm_joint5"}, \
19  {"control":"/sciurus17/controller1/joints","joint":"r_arm_joint6"}, \
20  {"control":"/sciurus17/controller1/joints","joint":"r_arm_joint7"}, \
21  {"control":"/sciurus17/controller2/joints","joint":"l_arm_joint1"}, \
22  {"control":"/sciurus17/controller2/joints","joint":"l_arm_joint2"}, \
23  {"control":"/sciurus17/controller2/joints","joint":"l_arm_joint3"}, \
24  {"control":"/sciurus17/controller2/joints","joint":"l_arm_joint4"}, \
25  {"control":"/sciurus17/controller2/joints","joint":"l_arm_joint5"}, \
26  {"control":"/sciurus17/controller2/joints","joint":"l_arm_joint6"}, \
27  {"control":"/sciurus17/controller2/joints","joint":"l_arm_joint7"}, \
28  {"control":"/sciurus17/controller3/joints","joint":"neck_pitch_joint"},\
29  {"control":"/sciurus17/controller3/joints","joint":"neck_yaw_joint"}, \
30  {"control":"/sciurus17/controller3/joints","joint":"waist_yaw_joint"}, \
31  ]
32 
33  self.preset_init = []
34  for i in range(17):
35  self.preset_init.append( { "p_gain": 800, "i_gain": 0, "d_gain": 0 } )
36 
37  self.preset_1 = [ { "name":"r_arm_joint1", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
38  { "name":"r_arm_joint2", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
39  { "name":"r_arm_joint3", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
40  { "name":"r_arm_joint4", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
41  { "name":"r_arm_joint5", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
42  { "name":"r_arm_joint6", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
43  { "name":"r_arm_joint7", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
44  { "name":"l_arm_joint1", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
45  { "name":"l_arm_joint2", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
46  { "name":"l_arm_joint3", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
47  { "name":"l_arm_joint4", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
48  { "name":"l_arm_joint5", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
49  { "name":"l_arm_joint6", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
50  { "name":"l_arm_joint7", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
51  { "name":"neck_pitch_joint","p_gain": 10, "i_gain": 0, "d_gain": 0 },\
52  { "name":"neck_yaw_joint", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
53  { "name":"waist_yaw_joint", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
54  ]
55 
56  self.preset_2 = [ { "name":"r_arm_joint1", "p_gain": 800, "i_gain": 200, "d_gain": 0 },\
57  { "name":"r_arm_joint2", "p_gain": 800, "i_gain": 200, "d_gain": 0 },\
58  { "name":"r_arm_joint3", "p_gain": 800, "i_gain": 200, "d_gain": 0 },\
59  { "name":"r_arm_joint4", "p_gain": 800, "i_gain": 200, "d_gain": 0 },\
60  { "name":"r_arm_joint5", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
61  { "name":"r_arm_joint6", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
62  { "name":"r_arm_joint7", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
63  { "name":"l_arm_joint1", "p_gain": 800, "i_gain": 200, "d_gain": 0 },\
64  { "name":"l_arm_joint2", "p_gain": 800, "i_gain": 200, "d_gain": 0 },\
65  { "name":"l_arm_joint3", "p_gain": 800, "i_gain": 200, "d_gain": 0 },\
66  { "name":"l_arm_joint4", "p_gain": 800, "i_gain": 200, "d_gain": 0 },\
67  { "name":"l_arm_joint5", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
68  { "name":"l_arm_joint6", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
69  { "name":"l_arm_joint7", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
70  { "name":"neck_pitch_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
71  { "name":"neck_yaw_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
72  { "name":"waist_yaw_joint", "p_gain": 1500, "i_gain": 0, "d_gain": 0 },\
73  ]
74 
75  self.preset_3 = [ { "name":"r_arm_joint1", "p_gain": 800, "i_gain": 200, "d_gain": 0 },\
76  { "name":"r_arm_joint2", "p_gain": 800, "i_gain": 200, "d_gain": 0 },\
77  { "name":"r_arm_joint3", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
78  { "name":"r_arm_joint4", "p_gain": 800, "i_gain": 100, "d_gain": 0 },\
79  { "name":"r_arm_joint5", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
80  { "name":"r_arm_joint6", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
81  { "name":"r_arm_joint7", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
82  { "name":"l_arm_joint1", "p_gain": 800, "i_gain": 200, "d_gain": 0 },\
83  { "name":"l_arm_joint2", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
84  { "name":"l_arm_joint3", "p_gain": 800, "i_gain": 200, "d_gain": 0 },\
85  { "name":"l_arm_joint4", "p_gain": 800, "i_gain": 100, "d_gain": 0 },\
86  { "name":"l_arm_joint5", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
87  { "name":"l_arm_joint6", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
88  { "name":"l_arm_joint7", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
89  { "name":"neck_pitch_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
90  { "name":"neck_yaw_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
91  { "name":"waist_yaw_joint", "p_gain": 1500, "i_gain": 0, "d_gain": 0 },\
92  ]
93 
94  self.preset_free_right_arm = [
95  { "name":"r_arm_joint1", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
96  { "name":"r_arm_joint2", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
97  { "name":"r_arm_joint3", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
98  { "name":"r_arm_joint4", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
99  { "name":"r_arm_joint5", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
100  { "name":"r_arm_joint6", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
101  { "name":"r_arm_joint7", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
102  { "name":"l_arm_joint1", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
103  { "name":"l_arm_joint2", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
104  { "name":"l_arm_joint3", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
105  { "name":"l_arm_joint4", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
106  { "name":"l_arm_joint5", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
107  { "name":"l_arm_joint6", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
108  { "name":"l_arm_joint7", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
109  { "name":"neck_pitch_joint","p_gain": 800, "i_gain": 0, "d_gain": 0 },\
110  { "name":"neck_yaw_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
111  { "name":"waist_yaw_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
112  ]
113 
114  self.preset_free_left_arm = [
115  { "name":"r_arm_joint1", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
116  { "name":"r_arm_joint2", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
117  { "name":"r_arm_joint3", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
118  { "name":"r_arm_joint4", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
119  { "name":"r_arm_joint5", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
120  { "name":"r_arm_joint6", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
121  { "name":"r_arm_joint7", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
122  { "name":"l_arm_joint1", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
123  { "name":"l_arm_joint2", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
124  { "name":"l_arm_joint3", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
125  { "name":"l_arm_joint4", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
126  { "name":"l_arm_joint5", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
127  { "name":"l_arm_joint6", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
128  { "name":"l_arm_joint7", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
129  { "name":"neck_pitch_joint","p_gain": 800, "i_gain": 0, "d_gain": 0 },\
130  { "name":"neck_yaw_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
131  { "name":"waist_yaw_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
132  ]
133 
134  self.preset_free_two_arms = [
135  { "name":"r_arm_joint1", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
136  { "name":"r_arm_joint2", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
137  { "name":"r_arm_joint3", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
138  { "name":"r_arm_joint4", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
139  { "name":"r_arm_joint5", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
140  { "name":"r_arm_joint6", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
141  { "name":"r_arm_joint7", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
142  { "name":"l_arm_joint1", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
143  { "name":"l_arm_joint2", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
144  { "name":"l_arm_joint3", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
145  { "name":"l_arm_joint4", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
146  { "name":"l_arm_joint5", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
147  { "name":"l_arm_joint6", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
148  { "name":"l_arm_joint7", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
149  { "name":"neck_pitch_joint","p_gain": 800, "i_gain": 0, "d_gain": 0 },\
150  { "name":"neck_yaw_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
151  { "name":"waist_yaw_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
152  ]
153 
154  self.preset_list = []
155  self.preset_list.append( self.preset_init )
156  self.preset_list.append( self.preset_1 )
157  self.preset_list.append( self.preset_2 )
158  self.preset_list.append( self.preset_3 )
159  self.preset_list.append( self.preset_free_right_arm)
160  self.preset_list.append( self.preset_free_left_arm)
161  self.preset_list.append( self.preset_free_two_arms)
162 
163  self.reconfigure = []
164  for joint in self.joint_list:
165  self.reconfigure.append( {"client":dynamic_reconfigure.client.Client( joint["control"]+"/"+joint["joint"],timeout=10 ), \
166  "joint:":joint["joint"]} )
167  rospy.loginfo("Wait sub...")
168  self.subscribe = rospy.Subscriber("preset_gain_no", UInt8, self.preset_no_callback)
169  rospy.loginfo("Init finished.")
170 
171  def preset_no_callback(self, no):
172  joint_no = 0
173  for conf in self.reconfigure:
174  conf["client"].update_configuration( {"position_p_gain":self.preset_list[no.data][joint_no]["p_gain"],"position_i_gain":self.preset_list[no.data][joint_no]["i_gain"],"position_d_gain":self.preset_list[no.data][joint_no]["d_gain"]} )
175  joint_no = joint_no + 1
176 
177 if __name__ == '__main__':
178  rospy.init_node('preset_reconfigure')
179  pr = PRESET_RECONFIGURE()
180  rospy.spin()
preset_reconfigure.PRESET_RECONFIGURE.preset_free_two_arms
preset_free_two_arms
プリセット定義 - 両手脱力 ###
Definition: preset_reconfigure.py:134
preset_reconfigure.PRESET_RECONFIGURE.preset_3
preset_3
プリセット定義 - 3 ###
Definition: preset_reconfigure.py:75
preset_reconfigure.PRESET_RECONFIGURE
Definition: preset_reconfigure.py:9
preset_reconfigure.PRESET_RECONFIGURE.preset_2
preset_2
プリセット定義 - 2 ###
Definition: preset_reconfigure.py:56
preset_reconfigure.PRESET_RECONFIGURE.__init__
def __init__(self)
Definition: preset_reconfigure.py:10
preset_reconfigure.PRESET_RECONFIGURE.preset_1
preset_1
プリセット定義 - 1.Free ###
Definition: preset_reconfigure.py:37
preset_reconfigure.PRESET_RECONFIGURE.preset_init
preset_init
プリセット定義 - 初期値 ###
Definition: preset_reconfigure.py:33
preset_reconfigure.PRESET_RECONFIGURE.preset_free_left_arm
preset_free_left_arm
プリセット定義 - 左手脱力 ###
Definition: preset_reconfigure.py:114
preset_reconfigure.PRESET_RECONFIGURE.subscribe
subscribe
Definition: preset_reconfigure.py:168
preset_reconfigure.PRESET_RECONFIGURE.joint_list
joint_list
Dynamic Reconfigure 接続先一覧 ###.
Definition: preset_reconfigure.py:13
preset_reconfigure.PRESET_RECONFIGURE.preset_list
preset_list
プリセットデータリスト ###
Definition: preset_reconfigure.py:154
preset_reconfigure.PRESET_RECONFIGURE.preset_free_right_arm
preset_free_right_arm
プリセット定義 - 右手脱力 ###
Definition: preset_reconfigure.py:94
preset_reconfigure.PRESET_RECONFIGURE.reconfigure
reconfigure
Definition: preset_reconfigure.py:163
preset_reconfigure.PRESET_RECONFIGURE.preset_no_callback
def preset_no_callback(self, no)
Definition: preset_reconfigure.py:171

sciurus17_control
Author(s): Hiroyuki Nomura
autogenerated on Sun Oct 2 2022 02:21:42