Saved_Code.txt

'''
# line 69 to 91 copied to Drive.py
class Driving(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes = ['outcome_mine', 'outcome_dump', 'outcome_manual'],
                                input_keys = ['x_in'],
                                output_keys = ['x_out'])
             
    # Drive chooses to mine, dump, or go to manual (and execute its function)
    def execute(self, userdata):
        #DRIVE STUFF
        
        userdata.x_out = random.randint(0, 2)

        rospy.loginfo(userdata.x_in)

        if(userdata.x_in == 0):
            return 'outcome_manual'

        elif(userdata.x_in == 1):
            return 'outcome_mine'

        else:
            return 'outcome_dump'
'''
'''
# line 94 to 115 copied to Mining.py file
class Mining(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes = ['outcome_drive', 'outcome_manual', 'outcome_dig'],
                                input_keys = ['x_in'],
                                output_keys = ['x_out'])
             
    def execute(self, userdata):
        #MINE STUFF

        userdata.x_out = random.randint(0, 2)
        rospy.loginfo(userdata.x_in)

        return 'outcome_dig'

        ############################ tripple quote here
        if(userdata.x_in == 0):
           return 'outcome_manual'
        else:
            return 'outcome_drive'
        ############################## tripple quote end here
'''

'''
# line 119 to 137 copied in Dump.py
class Dump(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes = ['outcome_drive', 'outcome_manual', 'outcome_dump'],
                                input_keys = ['x_in'],
                                output_keys = ['x_out'])
             
    def execute(self, userdata):
        #DUMP STUFF

        userdata.x_out = random.randint(0, 2)
        rospy.loginfo(userdata.x_in)

        return 'outcome_dump'
        ############################## tripple qyote here
        if(userdata.x_in == 0):
            return 'outcome_manual'
        else:
            return 'outcome_drive'
        ############################# tripple quote end here
'''

'''
# line 142 to 159 copied to Manual_Control.py file
class Manual_Control(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes = ['outcome_mine', 'outcome_dump', 'outcome_drive'],
                                input_keys = ['x_in'],
                                output_keys = ['x_out'])
             
    def execute(self, userdata):
        #MANUAL STUFF

        userdata.x_out = random.randint(0, 2)
        rospy.loginfo(userdata.x_in)

        if(userdata.x_in == 0):
            return 'outcome_mine'
        elif(userdata.x_in == 1):
            return 'outcome_dump'
        else:
            return 'outcome_drive'
'''
    # S S S S S S S S S S S S
    # START OF SM_SUB_DIGGING
    # S S S S S S S S S S S S
'''
# line 166 to 182 copied to Bl_ON.py file
class BL_ON(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes = ['outcome_next'],
                                input_keys = ['x_in'],
                                output_keys = ['x_out'])
             
    def execute(self, userdata):
        rospy.loginfo("Running --> Sub Digging: BL_ON")

        client = actionlib.SimpleActionClient("/name0/position",odrive_ros.msg.SetpointAction)
        client.wait_for_server()
        goal = odrive_ros.msg.SetpointGoal(setpoint=10)
        client.send_goal(goal,feedback_cb=fdbk_cb)
        client.wait_for_result()
        print(client.get_result())
            
        return 'outcome_next'
'''
'''
# line 186 to 202 copied to BL_Lower.py file
class BL_Lower(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes = ['outcome_next'],
                                input_keys = ['x_in'],
                                output_keys = ['x_out'])
             
    def execute(self, userdata):
        rospy.loginfo("Running --> Sub Digging: BL_Lower")

        client = actionlib.SimpleActionClient("/name0/position",odrive_ros.msg.SetpointAction)
        client.wait_for_server()
        goal = odrive_ros.msg.SetpointGoal(setpoint=10)
        client.send_goal(goal,feedback_cb=fdbk_cb)
        client.wait_for_result()
        print(client.get_result())

        return 'outcome_next'
'''
'''
# line 209 to 225 copied to Dump_Move.py file
class Dump_Move(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes = ['outcome_next'],
                                input_keys = ['x_in'],
                                output_keys = ['x_out'])
             
    def execute(self, userdata):
        rospy.loginfo("Running --> Sub Digging: Dump_Move")

                client = actionlib.SimpleActionClient("/name0/position",odrive_ros.msg.SetpointAction)
        client.wait_for_server()
        goal = odrive_ros.msg.SetpointGoal(setpoint=10)
        client.send_goal(goal,feedback_cb=fdbk_cb)
        client.wait_for_result()
        print(client.get_result())

        return 'outcome_next'
'''

'''
# line 230 to 246 copied to Robot_Lower.py file
class Robot_Lower(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes = ['outcome_next'],
                                input_keys = ['x_in'],
                                output_keys = ['x_out'])
             
    def execute(self, userdata):
        rospy.loginfo("Running --> Sub Digging: Robot_Lower")

                client = actionlib.SimpleActionClient("/name0/position",odrive_ros.msg.SetpointAction)
        client.wait_for_server()
        goal = odrive_ros.msg.SetpointGoal(setpoint=10)
        client.send_goal(goal,feedback_cb=fdbk_cb)
        client.wait_for_result()
        print(client.get_result())

        return 'outcome_next'
'''

'''
# line 253 to 262 copied to Timeout.py file
class Timeout(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes = ['outcome_next'],
                                input_keys = ['x_in'],
                                output_keys = ['x_out'])
'             
    def execute(self, userdata):
        rospy.loginfo("Running --> Sub Digging: Timeout")

        return 'outcome_next'
'''

'''
# line 268 to 284 copied to Dump_Move.py file
class Dump_Stop(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes = ['outcome_next'],
                                input_keys = ['x_in'],
                                output_keys = ['x_out'])
             
    def execute(self, userdata):
        rospy.loginfo("Running --> Sub Digging: Dump_Stop")

        client = actionlib.SimpleActionClient("/name0/position",odrive_ros.msg.SetpointAction)
        client.wait_for_server()
        goal = odrive_ros.msg.SetpointGoal(setpoint=10)
        client.send_goal(goal,feedback_cb=fdbk_cb)
        client.wait_for_result()
        print(client.get_result())

        return 'outcome_next'
'''

'''
# line 290 to 306 copied to BL_Raise.py file
class BL_Raise(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes = ['outcome_next'],
                                input_keys = ['x_in'],
                                output_keys = ['x_out'])
             
    def execute(self, userdata):
        rospy.loginfo("Running --> Sub Digging: BL_Raise")

        client = actionlib.SimpleActionClient("/name0/position",odrive_ros.msg.SetpointAction)
        client.wait_for_server()
        goal = odrive_ros.msg.SetpointGoal(setpoint=10)
        client.send_goal(goal,feedback_cb=fdbk_cb)
        client.wait_for_result()
        print(client.get_result())

        return 'outcome_next'
'''

'''
# line 312 to 328 copied to Return.py file
class Return(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes = ['outcome_return'],
                                input_keys = ['x_in'],
                                output_keys = ['x_out'])
             
    def execute(self, userdata):
        rospy.loginfo("Running --> Sub Digging: Return")
        return 'outcome_return'

    # E E E E E E E E E E E E
    # END OF SM_SUB_DIGGING
    # E E E E E E E E E E E E

    # S S S S S S S S S S S S
    # START OF SM_SUB_DUMPING
    # S S S S S S S S S S S S
'''

'''
# line 334 to 342 copied to Rangefinder_Check.py file
class Rangefinder_Check(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes = ['outcome_next'],
                                input_keys = ['x_in'],
                                output_keys = ['x_out'])
             
    def execute(self, userdata):
        rospy.loginfo("Running --> Sub Digging: Rangefinder_Check")
        return 'outcome_next'
'''

'''
# line to 348 to 364 copied to BL_Move.py file
class BL_Move(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes = ['outcome_next'],
                                input_keys = ['x_in'],
                                output_keys = ['x_out'])
             
    def execute(self, userdata):
        rospy.loginfo("Running --> Sub Digging: BL_Move")

        client = actionlib.SimpleActionClient("/name0/position",odrive_ros.msg.SetpointAction)
        client.wait_for_server()
        goal = odrive_ros.msg.SetpointGoal(setpoint=10)
        client.send_goal(goal,feedback_cb=fdbk_cb)
        client.wait_for_result()
        print(client.get_result())

        return 'outcome_next'
'''

'''
# line 371 to 387 copied to Dump_Rotate.py file
class Dump_Rotate(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes = ['outcome_next'],
                                input_keys = ['x_in'],
                                output_keys = ['x_out'])
             
    def execute(self, userdata):
        rospy.loginfo("Running --> Sub Digging: Dump_Rotate")

        client = actionlib.SimpleActionClient("/name0/position",odrive_ros.msg.SetpointAction)
        client.wait_for_server()
        goal = odrive_ros.msg.SetpointGoal(setpoint=10)
        client.send_goal(goal,feedback_cb=fdbk_cb)
        client.wait_for_result()
        print(client.get_result())

        return 'outcome_next'
'''

'''
# line 393 to 409 copied to Dump_Lower.py file
class Dump_Lower(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes = ['outcome_next'],
                                input_keys = ['x_in'],
                                output_keys = ['x_out'])
             
    def execute(self, userdata):
        rospy.loginfo("Running --> Sub Digging: Dump_Lower")

        client = actionlib.SimpleActionClient("/name0/position",odrive_ros.msg.SetpointAction)
        client.wait_for_server()
        goal = odrive_ros.msg.SetpointGoal(setpoint=10)
        client.send_goal(goal,feedback_cb=fdbk_cb)
        client.wait_for_result()
        print(client.get_result())

        return 'outcome_next'
'''       

'''
# line 415 to 435 copied to BL_Reset.py file
class BL_Reset(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes = ['outcome_next'],
                                input_keys = ['x_in'],
                                output_keys = ['x_out'])
             
    def execute(self, userdata):
        rospy.loginfo("Running --> Sub Digging: BL_reset")

        client = actionlib.SimpleActionClient("/name0/position",odrive_ros.msg.SetpointAction)
        client.wait_for_server()
        goal = odrive_ros.msg.SetpointGoal(setpoint=10)
        client.send_goal(goal,feedback_cb=fdbk_cb)
        client.wait_for_result()
        print(client.get_result())

        return 'outcome_next'

    # E E E E E E E E E E E E
    # END OF SM_SUB_DIGGING
    # E E E E E E E E E E E E
'''