|
6 | 6 | import smach_ros
|
7 | 7 |
|
8 | 8 | from controller import Controller
|
| 9 | +from dynamixel_workbench_msgs.srv import SetPID |
9 | 10 |
|
10 | 11 | # Initialize controller
|
11 | 12 | ctrl = Controller()
|
@@ -133,12 +134,17 @@ def execute(self, userdata):
|
133 | 134 | # define state ChangePID
|
134 | 135 | class ChangePID(smach.State):
|
135 | 136 | def __init__(self):
|
136 |
| - smach.State.__init__(self, outcomes=['changed']) |
| 137 | + smach.State.__init__(self, outcomes=['changed', 'not_changed'], input_keys=['P', 'I', 'D']) |
137 | 138 |
|
138 | 139 | def execute(self, userdata):
|
139 |
| - ## Service call to change the PID values |
140 |
| - rospy.loginfo('Executing state ChangePID') |
141 |
| - return 'changed' |
| 140 | + rospy.wait_for_service('SetPID') |
| 141 | + try: |
| 142 | + set_PID = rospy.ServiceProxy('SetPID', SetPID) |
| 143 | + response = set_PID(userdata.P, userdata.I, userdata.D) |
| 144 | + return 'changed' |
| 145 | + except rospy.ServiceException as e: |
| 146 | + rospy.logwarn("Service call failed:{0}".format(e)) |
| 147 | + return 'not_changed' |
142 | 148 |
|
143 | 149 | # define state OpenGripper
|
144 | 150 | class OpenGripper(smach.State):
|
@@ -183,8 +189,11 @@ def main():
|
183 | 189 | transitions={'foundIK':'CHANGEPID'})
|
184 | 190 | # smach.StateMachine.add('MOVEGIVE', MoveGive(),
|
185 | 191 | # transitions={'reached':'CHANGEPID'})
|
| 192 | + userdata.P = 0 |
| 193 | + userdata.I = 0 |
| 194 | + userdata.D = 0 |
186 | 195 | smach.StateMachine.add('CHANGEPID', ChangePID(),
|
187 |
| - transitions={'changed':'OPENGRIPPER'}) |
| 196 | + transitions={'changed':'OPENGRIPPER', 'notchanged': 'CHANGEPID'}) |
188 | 197 | smach.StateMachine.add('OPENGRIPPER', OpenGripper(),
|
189 | 198 | transitions={'opened':'IDLE'})
|
190 | 199 |
|
|
0 commit comments