Commneted action points for manipulation pipeline.

Author: hnsheth07

scripts/Controller.py renamed to scripts/controller.py

@@ -38,14 +38,14 @@ def __init__(self):
         self.IK_ORIENTATION_TOLERANCE = np.pi/9
         self.MIN_CLOSING_GAP = 0.002
 
-    def set_camera_angles(self, pan_rad, tilt_rad):
+    def set_camera_angles(self, angles):
         pan_msg = Float64()
-        pan_msg.data = pan_rad
+        pan_msg.data = angles[0]
         rospy.loginfo('Going to camera pan: {} rad'.format(pan_rad))
         self.pan_pub.publish(pan_msg)
 
         tilt_msg = Float64()
-        tilt_msg.data = tilt_rad
+        tilt_msg.data = angles[1]
         rospy.loginfo('Going to camera tilt: {} rad'.format(tilt_rad))
         self.tilt_pub.publish(tilt_msg)
         rospy.sleep(4)
@@ -107,17 +107,16 @@ def compute_ik(self, target_pose):
 
     # Goes to the position given by FRAME and grabs the object from the top
     def go_to_pose(self, FRAME):
-        while not rospy.is_shutdown():
-            try:
-                self.tf_listener.waitForTransform(self.BOTTOM_PLATE_LINK, FRAME, rospy.Time.now(), rospy.Duration(5.0))
-                P, Q = self.tf_listener.lookupTransform(self.BOTTOM_PLATE_LINK, FRAME, rospy.Time(0))
-                break
-            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
-                print(e)
-    
+        try:
+            self.tf_listener.waitForTransform(self.BOTTOM_PLATE_LINK, FRAME, rospy.Time.now(), rospy.Duration(5.0))
+            P, Q = self.tf_listener.lookupTransform(self.BOTTOM_PLATE_LINK, FRAME, rospy.Time(0))
+        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
+            rospy.logwarn(e)
+            return False
+
         O = tf.transformations.euler_from_quaternion(Q)
         Q = tf.transformations.quaternion_from_euler(0, np.pi/2, O[2])
-        
+
         poses = [Pose(Point(P[0], P[1], P[2]+0.20), Quaternion(Q[0], Q[1], Q[2], Q[3])),
                  Pose(Point(P[0], P[1], P[2]+0.15), Quaternion(Q[0], Q[1], Q[2], Q[3]))]
 
@@ -135,7 +134,7 @@ def go_to_pose(self, FRAME):
                 rospy.sleep(8)
             else:
                 return False
-        self.close_gripper()
+        return True
 
     # checks if the object was grasped or not
     def check_grasp(self):

scripts/machine.py

@@ -5,13 +5,19 @@
 import smach
 import smach_ros
 
+from controller import Controller
+
+# Initialize controller
+ctrl = Controller()
 # define state Idle
 class Idle(smach.State):
     def __init__(self):
         smach.State.__init__(self, outcomes=['gotToolInput'], input_keys=['tool_id'], output_keys=['tool_id'])
 
     def execute(self, userdata):
         userdata.tool_id = -1
+        ctrl.set_camera_angles(ctrl.HOME_POS_CAMERA_01)
+        ctrl.set_arm_joint_angles(ctrl.HOME_POS_MANIPULATOR_01)
         rospy.loginfo('Executing state IDLE')
         while(True):
             # TODO: Ask for Input
@@ -21,149 +27,176 @@ def execute(self, userdata):
 
         # Return success
         return 'gotToolInput'
-        
+
 
 
 # define state FindTool
 class FindTool(smach.State):
     def __init__(self):
-        smach.State.__init__(self, outcomes=['foundTool'], input_keys=['tool_id'])
+        smach.State.__init__(self, outcomes=['foundTool'], input_keys=['tool_id'], output_keys=['frame_name'])
 
     def execute(self, userdata):
         rospy.loginfo('Executing state FINDTOOL')
         print(userdata.tool_id)
+        ## TODO: change the name of the variable frame_name
+        userdata.frame_name = "random"
         return 'foundTool'
-        
+
 # define state IK1
 class IK1(smach.State):
     def __init__(self):
-        smach.State.__init__(self, outcomes=['noIK','foundIK'])
+        smach.State.__init__(self, outcomes=['noIK','foundIK'], input_keys=['frame_name'])
 
     def execute(self, userdata):
+        success = ctrl.go_to_pose(userdata.frame_name)
         rospy.loginfo('Executing state IK1')
-        return 'foundIK'
+        return 'foundIK' if success else 'noIK'
 
 # define state Move
-class Move(smach.State):
-    def __init__(self):
-        smach.State.__init__(self, outcomes=['reached'])
+# class Move(smach.State):
+#     def __init__(self):
+#         smach.State.__init__(self, outcomes=['reached'])
 
-    def execute(self, userdata):
-        rospy.loginfo('Executing state IK1')
-        return 'reached'
+#     def execute(self, userdata):
+#         rospy.loginfo('Executing state IK1')
+#         return 'reached'
 
 # define state Grab
 class Grab(smach.State):
     def __init__(self):
         smach.State.__init__(self, outcomes=['grabSuccess','grabFailure'])
 
     def execute(self, userdata):
+        ctrl.close_gripper()
         rospy.loginfo('Executing state IK1')
-        return 'grabSuccess'
+        if ctrl.check_grasp():
+            return 'grabSuccess'
+        else:
+            return 'grabFailure'
 
 # define state MoveHome2
 class MoveHome2(smach.State):
     def __init__(self):
         smach.State.__init__(self, outcomes=['reached'])
 
     def execute(self, userdata):
+        ctrl.set_camera_angles(ctrl.HOME_POS_CAMERA_02)
+        ctrl.set_arm_joint_angles(ctrl.HOME_POS_MANIPULATOR_02)
         rospy.loginfo('Executing state MoveHome2')
         return 'reached'
 
 # define state OreintCamera
-class OreintCamera(smach.State):
-    def __init__(self):
-        smach.State.__init__(self, outcomes=['reached'])
+# class OrientCamera(smach.State):
+#     def __init__(self):
+#         smach.State.__init__(self, outcomes=['reached'])
 
-    def execute(self, userdata):
-        rospy.loginfo('Executing state OreintCamera')
-        return 'reached'
+#     def execute(self, userdata):
+#         rospy.loginfo('Executing state OrientCamera')
+#         return 'reached'
 
 # define state FindAttention
 class FindAttention(smach.State):
+
     def __init__(self):
-        smach.State.__init__(self, outcomes=['giveTool'])
+        smach.State.__init__(self, outcomes=['giveTool'], output_keys=['frame_name'])
 
     def execute(self, userdata):
+        ## Call service to get the frame id of the hand centroid
+        ## TODO: change the name of the variable frame_name
+        userdata.frame_name = "random"
         rospy.loginfo('Executing state FindAttention')
         return 'giveTool'
 
 # define state IK2
 class IK2(smach.State):
     def __init__(self):
-        smach.State.__init__(self, outcomes=['foundIK'])
+        smach.State.__init__(self, outcomes=['foundIK'], input_keys=['frame_name'])
 
     def execute(self, userdata):
+        ## Wait till IK not found. Change tolerance and call compute IK again
+        ## Break go to pose into find pose and compute IK functions
+        ## Make a different function with gripper pointing upwards and call it from here
+        success = ctrl.go_to_pose(userdata.frame_name)
+        # if not success
         rospy.loginfo('Executing state IK2')
         return 'foundIK'
 
-# define state MoveGive
-class MoveGive(smach.State):
-    def __init__(self):
-        smach.State.__init__(self, outcomes=['reached'])
+# # define state MoveGive
+# class MoveGive(smach.State):
+#     def __init__(self):
+#         smach.State.__init__(self, outcomes=['reached'])
 
-    def execute(self, userdata):
-        rospy.loginfo('Executing state MoveGive')
-        return 'reached'
+#     def execute(self, userdata):
+#         rospy.loginfo('Executing state MoveGive')
+#         return 'reached'
 
 # define state ChangePID
 class ChangePID(smach.State):
     def __init__(self):
-        smach.State.__init__(self, outcomes=['reached'])
+        smach.State.__init__(self, outcomes=['changed'])
 
     def execute(self, userdata):
+        ## Service call to change the PID values
         rospy.loginfo('Executing state ChangePID')
-        return 'reached'
+        return 'changed'
 
 # define state OpenGripper
 class OpenGripper(smach.State):
     def __init__(self):
-        smach.State.__init__(self, outcomes=['reached'])
+        smach.State.__init__(self, outcomes=['opened'])
 
     def execute(self, userdata):
+        ## Detect when to Open gripper
+        self.open_gripper()
         rospy.loginfo('Executing state OpenGripper')
-        return 'reached'
+        return 'opened'
 
 def main():
     rospy.init_node('attention_bot')
 
+
     # Create a SMACH state machine
     sm = smach.StateMachine(outcomes=['stop'])
     sm.userdata.tool_id = -1
-
+    sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
+    sis.start()
     # Open the container
     with sm:
         # Add states to the container
-        smach.StateMachine.add('IDLE', Idle(), 
+        smach.StateMachine.add('IDLE', Idle(),
                                transitions={'gotToolInput':'FINDTOOL'})
-        smach.StateMachine.add('FINDTOOL', FindTool(), 
+        smach.StateMachine.add('FINDTOOL', FindTool(),
                                transitions={'foundTool':'IK1'})
-        smach.StateMachine.add('IK1', IK1(), 
-                               transitions={'noIK':'stop','foundIK':'MOVE'})
-        smach.StateMachine.add('MOVE', Move(), 
-                               transitions={'reached':'GRAB'})
-        smach.StateMachine.add('GRAB', Grab(), 
+        smach.StateMachine.add('IK1', IK1(),
+                               transitions={'noIK':'stop','foundIK':'GRAB'})
+        # smach.StateMachine.add('MOVE', Move(),
+        #                        transitions={'reached':'GRAB'})
+        smach.StateMachine.add('GRAB', Grab(),
                                transitions={'grabSuccess':'MOVEHOME2','grabFailure':'FINDTOOL'})
-        smach.StateMachine.add('MOVEHOME2', MoveHome2(), 
-                               transitions={'reached':'ORIENTCAMERA'})
-        smach.StateMachine.add('ORIENTCAMERA', OreintCamera(), 
+        smach.StateMachine.add('MOVEHOME2', MoveHome2(),
                                transitions={'reached':'ATTENTIONSEEKER'})
-        smach.StateMachine.add('ATTENTIONSEEKER', FindAttention(), 
+        # smach.StateMachine.add('ORIENTCAMERA', OrientCamera(),
+        #                        transitions={'reached':'ATTENTIONSEEKER'})
+        smach.StateMachine.add('ATTENTIONSEEKER', FindAttention(),
                                transitions={'giveTool':'IK2'})
-        smach.StateMachine.add('IK2', IK2(), 
-                               transitions={'foundIK':'MOVEGIVE'})
-        smach.StateMachine.add('MOVEGIVE', MoveGive(), 
-                               transitions={'reached':'CHANGEPID'})
-        smach.StateMachine.add('CHANGEPID', ChangePID(), 
-                               transitions={'reached':'OPENGRIPPER'})
-        smach.StateMachine.add('OPENGRIPPER', OpenGripper(), 
-                               transitions={'reached':'IDLE'})
-        
+        smach.StateMachine.add('IK2', IK2(),
+                               transitions={'foundIK':'CHANGEPID'})
+        # smach.StateMachine.add('MOVEGIVE', MoveGive(),
+        #                        transitions={'reached':'CHANGEPID'})
+        smach.StateMachine.add('CHANGEPID', ChangePID(),
+                               transitions={'changed':'OPENGRIPPER'})
+        smach.StateMachine.add('OPENGRIPPER', OpenGripper(),
+                               transitions={'opened':'IDLE'})
+
+
 
     # Execute SMACH plan
     outcome = sm.execute()
+    sis.stop()
+
+
 
 
 
 if __name__ == '__main__':
-    main()
+    main()