resolved merge conflicts

Author: ykarmesh

scripts/machine.py

@@ -7,6 +7,7 @@
 
 from controller import Controller
 from geometry_msgs.msg import PoseStamped
+from dynamixel_workbench_msgs.srv import SetPID
 
 # Initialize controller
 ctrl = Controller()
@@ -133,12 +134,19 @@ def execute(self, userdata):
 # define state ChangePID
 class ChangePID(smach.State):
     def __init__(self):
-        smach.State.__init__(self, outcomes=['changed'])
+        smach.State.__init__(self, outcomes=['changed', 'not_changed'], input_keys=['joint_nums','PID'])
 
     def execute(self, userdata):
-        ## Service call to change the PID values
-        rospy.loginfo('Executing state ChangePID')
-        return 'changed'
+        rospy.wait_for_service('SetPID')
+        try:
+            set_PID = rospy.ServiceProxy('SetPID', SetPID)
+            P,I,D = userdata.PID
+            for joint_num in userdata.joint_nums:
+                response = set_PID(joint_num, P, I, D)
+            return 'changed'
+        except rospy.ServiceException as e:
+            rospy.logwarn("Service call failed:{0}".format(e))
+            return 'not_changed'
 
 # define state OpenGripper
 class OpenGripper(smach.State):
@@ -158,6 +166,8 @@ def main():
     # Create a SMACH state machine
     sm = smach.StateMachine(outcomes=['stop'])
     sm.userdata.tool_id = -1
+    sm.userdata.joint_nums = [1,2,3]
+    sm.userdata.PID = [0,0,0]
     sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
     sis.start()
     # Open the container
@@ -183,8 +193,9 @@ def main():
                                transitions={'foundIK':'CHANGEPID'})
         # smach.StateMachine.add('MOVEGIVE', MoveGive(),
         #                        transitions={'reached':'CHANGEPID'})
+
         smach.StateMachine.add('CHANGEPID', ChangePID(),
-                               transitions={'changed':'OPENGRIPPER'})
+                               transitions={'changed':'OPENGRIPPER', 'notchanged': 'CHANGEPID'})
         smach.StateMachine.add('OPENGRIPPER', OpenGripper(),
                                transitions={'opened':'IDLE'})