Skip to content

Commit b4e24f5

Browse files
committed
Added function for SETPID service
1 parent f044159 commit b4e24f5

File tree

1 file changed

+14
-5
lines changed

1 file changed

+14
-5
lines changed

scripts/machine.py

Lines changed: 14 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@
66
import smach_ros
77

88
from controller import Controller
9+
from dynamixel_workbench_msgs.srv import SetPID
910

1011
# Initialize controller
1112
ctrl = Controller()
@@ -133,12 +134,17 @@ def execute(self, userdata):
133134
# define state ChangePID
134135
class ChangePID(smach.State):
135136
def __init__(self):
136-
smach.State.__init__(self, outcomes=['changed'])
137+
smach.State.__init__(self, outcomes=['changed', 'not_changed'], input_keys=['P', 'I', 'D'])
137138

138139
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'
142148

143149
# define state OpenGripper
144150
class OpenGripper(smach.State):
@@ -183,8 +189,11 @@ def main():
183189
transitions={'foundIK':'CHANGEPID'})
184190
# smach.StateMachine.add('MOVEGIVE', MoveGive(),
185191
# transitions={'reached':'CHANGEPID'})
192+
userdata.P = 0
193+
userdata.I = 0
194+
userdata.D = 0
186195
smach.StateMachine.add('CHANGEPID', ChangePID(),
187-
transitions={'changed':'OPENGRIPPER'})
196+
transitions={'changed':'OPENGRIPPER', 'notchanged': 'CHANGEPID'})
188197
smach.StateMachine.add('OPENGRIPPER', OpenGripper(),
189198
transitions={'opened':'IDLE'})
190199

0 commit comments

Comments
 (0)