示例#1
0
    def execute(self, userdata):
        global MESCommand
        global position
        global r
        global debugWait
        global path
        global nextPath
#        r.sleep()
        rospy.loginfo("currCommand: " + str(MESCommand))
        rospy.loginfo("currPath: " + str(path))    
        rospy.loginfo("currNextPath: " + str(nextPath))
        #If nothing new received from MES, stay free

        rospy.sleep(debugWait)


        while MESCommand == mes_mobile_command.COMMAND_WAIT:
            rospy.loginfo("in COMMAND_WAIT while loop")
            rospy.loginfo("currCommand: " + str(MESCommand))
            rospy.loginfo("currPath: " + str(path))    
            rospy.loginfo("currNextPath: " + str(nextPath))

            #sleep for 5 seconds
            rospy.loginfo('command was wait. 5 sec')
            status = mes_mobile_status()
            status.header.stamp = rospy.Time.now()
            status.state = mes_mobile_status.STATE_FREE
            status.position = position 
            pubStatus.publish(status)
            rospy.sleep(debugWait)
 
        rospy.loginfo("out of COMMAND_WAIT while loop")

        #If command received from MES server, Go to NavigateInCoordinateZone
        if MESCommand == mes_mobile_command.COMMAND_NAVIGATE:
            #TODO:Publish state change to OEE topic
            status = mes_mobile_status()
            status.header.stamp = rospy.Time.now()
            status.state = mes_mobile_status.STATE_WORKING
            status.position = position 
            pubStatus.publish(status)
            
            path = nextPath
            #Reset MESCommand to zero
                #whenever this state, "free", a status have been sent,
                #and a new command will be set by the callback function
            MESCommand = mes_mobile_command.COMMAND_WAIT
            return 'outcome2'  
示例#2
0
    def execute(self, userdata):
        global position
        global doneNavigating
        global r
        global debugWait
        global path
        global nextPath
        global cameFromLoadOff
#        r.sleep()
        
       
       
        if path == 'Line':
            #TODO: Is it really Independent of whether in LoadOn or LoadOff zone?
            self.driveForwardToLine()
        else:
            goal = self.loadOnOrOff()
            if goal == 'LoadOn':
                self.driveBackwardsToLoadOn()
                cameFromLoadOff = False
                #change to StateFreeAtLoadZone                
#                return 'outcome2' 
            else:
                self.driveBackwardsToLoadOff()
                cameFromLoadOff = True
                #change to StateFreeAtLoadZone
#                return 'outcome2'
           
           
        rospy.sleep(debugWait)  
#        while doneNavigating == False:
#            rospy.sleep(1)
#        doneNavigating = False
            ###############################################
            #TODO: Handle how to wait for tip. 
                #Maybe don't wait at all...            
            ###################################################
        
        #Change to StateFreMeAtLine
        #TODO: OBS: Normally, StateFreeAtLineZone assumes that the robot is
        # at the start of the line zone....
        position = path #here it is "Line"
        status = mes_mobile_status()  
        status.header.stamp = rospy.Time.now()           
        status.state = mes_mobile_status.STATE_FREE
        status.position = position 
        pubStatus.publish(status)     
        
        if path == 'Line':
            path = nextPath
            return 'outcome1'
        #else, determine if in LoadOn or LoadOff zone
        else:
            path = nextPath
            return 'outcome2'
示例#3
0
    def execute(self, userdata):
        global position
        global r
        global doneNavigating
        global debugWait
        global path
        global nextPath
        global poseFromMarkerClient
#        r.sleep()

        
        #goal assumed reached when navigateToGoal Returns.         
        
        wayPoints = self.getWayPointListFromStringCommand(path)
        
        # On the way to the line we want to make sure we are located correctly, so we use the LPS
        if path == 'Line' or path == 'RampIn':
            self.getLPSFix()
        
        #Call Rudis Functions with list for coordinates, called wayPoints. 
        #TODO: Should this function be a ROS action?        
            #simply sends coordinates to other node. This wakes up 
        self.navigateToGoal(wayPoints)
        
#        while doneNavigating == False:
#            rospy.loginfo('still navigating')            
#            rospy.sleep(5)
        
        #if goal is lineZone, change state to StateFreeAtLineZone           
        position = path
        status = mes_mobile_status()
        status.header.stamp = rospy.Time.now()
        status.state = mes_mobile_status.STATE_FREE
        status.position = position
        pubStatus.publish(status)
       
        if path == 'Line':
            path = nextPath
            return 'outcome1'            
        #else, change to StateFreeAtCoordinateZone
        else:
            path = nextPath
            return 'outcome2'
示例#4
0
    def execute(self, userdata):
        global position
        global path
        global nextPath
        global r
        global doneNavigating
        global debugWait
#        r.sleep()
        global cameFromLoadOff
        global latestRobotCell

        if self.atStartOfLine == True:
            rospy.loginfo("navigateToLongStretch")
            self.navigateToLongStretch()
            self.atStartOfLine = False

        if path == 'FloorIn':
            rospy.loginfo("navigateToFloorIn")

            self.navigateToFloorIn()
            self.atStartOfLine = True
        else:
            #rospy.loginfo("navigateToQR")
            if cameFromLoadOff == False:
                QRId = self.getQRId()
                latestRobotCell = QRId
                #TODO: QRId for endOfLine = 0!!! Maybe???
                self.navigateToQR(QRId)
               
               
                if self.loadOnOrOff() == "LoadOn":
                    self.followLineToCross()
                    cameFromLoadOff = False #not necessary
                else:
                     cameFromLoadOff = True
            else:
                self.followLineToCross()
                
                    
            #if to 
            self.turn90DegRight()
            
        
            
        
        #TODO: What if QR not found? Try again or error?
        
#        while doneNavigating == False:
#            rospy.sleep(5)
            
        position = path
        status = mes_mobile_status()
        status.header.stamp = rospy.Time.now()
        status.state = mes_mobile_status.STATE_WORKING
        status.position = position 
        pubStatus.publish(status)
       
        ##################################################################
        #TODO: Handle that navigation should continue in coordinate or load zone.
        #even without coming from a free state
        #DONE in coordinatezone, because path is set to FloorIn, so robot will navigate to center of this zone.
        #########################################################################
        
        #if at end of line, go to state StateNavigateInCoordinateZone, with way point = centerOfFloorInZone
        if path == 'FloorIn':
            
            #change to StateNavigateInCoordinateZone
            return 'outcome1'

        else:
            
            #in order to back up to LoadOff
            #TODO: should be left() when LoadOn!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
            #Change to StateNavigateInLoadZone              
            return 'outcome2'