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