def stopExecution(self):
if (self.taskProcess != None):
if (self.taskProcess.is_alive()):
misc.sendMsg(self.cmdQ, None, 1)
while (True):
if not (self.taskProcess.is_alive()):
break
def OnStop(self, event):
for rId in (self.robotIds):
misc.sendMsg(self.robotCmd[rId], None, 1)
time.sleep(0.1)
def checkTaskProgress(self):
"""checkTaskProgress
Input arguments:
None
Returns:
None
Description:
this method is called from the main bidding/consensus/execution loop
to check the progress of execution by reading the messages from the taskNavigation process"""
# if taskFinish = 1 or 2 ignore stage
# if taskFinish = 0, check for stage
# if stage is changed, make necessary modification and
# send command 1 in stgQ
xNew = yNew = stat = 0
if (self.taskProcess == None):
return -1
else:
if (self.taskProcess.is_alive()):
stat = 0
while (True):
try:
msgNew = self.msgQ.get_nowait()
xNew = msgNew[0]
yNew = msgNew[1]
stage = msgNew[2]
stat = msgNew[3]
except queue.Empty:
break
else:
# print (self.index, xNew, yNew)
self.x.append(xNew)
self.y.append(yNew)
if (stage > self.stage):
print("r[%d] stage:%d prevStage:%d" %
(self.index, stage, self.stage))
# normal task completion
if (stat == 1):
break
# task stopped as per command
elif (stat == 2):
break
elif (stat == 0):
if (stage > self.stage):
print("r[%d] got a stage change" % self.index)
timeNow = time.time()
self.taskEndTime[self.nTaskAllocated -
1][0] = timeNow
self.taskStartTime[self.nTaskAllocated -
1][1] = timeNow
# progress to next stage
self.stage = copy.copy(stage)
# send command 2: OK to progress to next level
misc.sendMsg(self.cmdQ, None, 2)
print(
"robot:checkTaskProgress-1: r[%d] sent stage change ack"
% self.index)
else: # process finished. now reading all data from queue
while (True):
try:
msgNew = self.msgQ.get_nowait()
xNew = msgNew[0]
yNew = msgNew[1]
stage = msgNew[2]
stat = msgNew[3]
except queue.Empty:
break
else:
# print (self.index, xNew, yNew)
self.x.append(xNew)
self.y.append(yNew)
if (stage > self.stage):
print("r[%d] stage:%d prevStage:%d" %
(self.index, stage, self.stage))
# normal task completion
if (stat == 1):
break
# task stopped as per command
elif (stat == 2):
break
elif (stat == 0):
if (stage > self.stage):
# progressed to next stage
self.stage = copy.copy(stage)
# send command 2: OK to progress to next level
misc.sendMsg(self.cmdQ, None, 2)
print(
"robot:checkTaskProgress-2: r[%d] sent stage change ack"
% self.index)
if (stat == 1):
self.taskProcess.join()
self.completeTask(time.time())
# print ("robot[%d] navigation to task[%d] completed: %d" %(self.index, self.taskBidOrder[self.nTaskAllocated-1],time.time()))
return 1
elif (stat == 2):
self.taskProcess.join()
# print ("robot[%d] navigation to task[%d] stopped as per command: %d" %(self.index, self.taskBidOrder[self.nTaskAllocated-1], time.time()))
return 2
else:
return 0
def taskNavigation(self, msgQ, cmdQ, xPrev, yPrev, xFinish, yFinish, stage,
mbGoal, mbState, odomQ):
"""taskNavigation
Input arguments:
msgQ -> message queue back to the main robot process
cmdQ -> command queue to the task process from the main robot process
xPrev -> previous x coordinate of the robot
yPrev -> previous y coordinate of the robot
xFinish -> target x coodinate
yFinish -> target y coordinate
stage -> current execution stage, to send it back to the main robot process
mbGoal -> queue object to pass move_base target locations as tuples (x, y)
Returns:
None
Description:
method to control the navigation of a robot to the target location"""
# task is allocated and the robot has to travel to the task location
# robot moves in the direction of the task from the current location
# keep on track: regularly check the position and orientation with respect to the task location
# obstacle avoidance: may have to move away from planned path to avoid an obstacle
# stop when it reaches the task location
# this is a new process. all methods accessed from this used variables from this function.
taskZone = self.taskZone
print(
"r[%d] started navigation to (%0.2f, %0.2f) from (%0.2f, %0.2f)" %
(self.index, xFinish, yFinish, xPrev, yPrev))
taskFinish = 0
com = 0
prevMsg = None
# wait for some time.
time.sleep(0.1)
xNew, yNew = xPrev + 0, yPrev + 0
taskFinish = self.checkTaskFinish(
xNew, yNew, xFinish, yFinish, taskZone,
mbState) # check task finish status: 1-finished; 0-still running
misc.sendMsg(msgQ, None, [xNew, yNew, stage, taskFinish])
prevMsg = [] + [xNew, yNew, stage, taskFinish]
# get the path to navigate
xyPath = self.getPath(xNew, yNew, xFinish, yFinish)
# (tX,tY) are modified as the coordinates of the nodes in the point in the following loop
# for (tX, tY) in xyPath:
while (len(xyPath) != 0):
# xyPath can be modified with in the loop
# always the first waypoint in the path is the next waypoint
# when a way point is reached that is removed from the path
tX = xyPath[0][0]
tY = xyPath[0][1]
currRoom = self.mapInfo.getRoom(xNew, yNew)
nextRoom = self.mapInfo.getRoom(tX, tY)
if (misc.eq(xFinish, tX) and misc.eq(yFinish, tY)):
taskZone = self.taskZone #*1.5 # not to detect the task object as obstacle
else:
taskZone = self.taskZone
if (taskFinish == 2):
break
self.goalSent = False
self.mbFinished = False
taskFinish = 0
while (taskFinish == 0): # when task not finished
time.sleep(0.1)
# rospy.loginfo ("%s here: tf- %d, gs - %s" %(self.index, taskFinish, self.goalSent))
if not self.goalSent:
goal = ("goal", tX, tY)
mbGoal.put_nowait(goal)
time.sleep(0.1)
self.goalSent = True
time.sleep(0.5)
# odom updates do not reach the subprocesses. receive it using a queue
while (True):
try:
(xNew, yNew) = odomQ.get_nowait()
except queue.Empty:
break
else:
if (prevMsg != [xNew, yNew, stage, taskFinish]):
# send current coordination back
misc.sendMsg(msgQ, None,
[xNew, yNew, stage, taskFinish])
prevMsg = [] + [xNew, yNew, stage, taskFinish]
taskFinish = self.checkTaskFinish(
xNew, yNew, tX, tY, taskZone,
mbState) # check task finish status
if (taskFinish == 1):
xyPath.pop(0)
break
# if a stop task command is received from the main process,
# stop the current execution with a stat return of 2
try:
com = cmdQ.get_nowait(
) # command from main process. 1-> stop current task
except queue.Empty:
pass
else:
if (com == 1):
taskFinish = 2
break
# check the room
tempRoom = self.mapInfo.getRoom(xNew, yNew)
if (tempRoom == None):
tempRoom = -1
if ((tempRoom != currRoom) and (tempRoom != nextRoom)
and (tempRoom != -1)):
taskFinish = 3
break
# replan the path here
if (taskFinish == 3):
print("r[%d] changed xyPath" % (self.index))
# print ("r[%d] was moving from room[%d] to room[%d]. but now room is %d" %(self.index, currRoom, nextRoom, tempRoom))
xyPath = self.getPath(xNew, yNew, xFinish, yFinish)
if (taskFinish == 1):
# => navigation to task location successful
misc.sendMsg(msgQ, None, [xNew, yNew, stage, 0])
return (xNew, yNew, taskFinish)
elif (taskFinish == 2):
# => task has been stopped by command
# return the status
goal = ("cancel", 0.0, 0.0)
mbGoal.put_nowait(goal)
time.sleep(0.1)
misc.sendMsg(msgQ, None, [xPrev, yPrev, stage, 0])
return (xNew, yNew, taskFinish)
# add detailed controls here for longer tasks after reaching the task location.
print("Error!!!, taskFinish = ", taskFinish)
exit()
return 0
def taskExecution(self, msgQ, cmdQ, currTask, xPrev, yPrev, mbGoal,
mbState, odomQ):
"""taskExecution
Input arguments:
msgQ -> message queue back to the main robot process
cmdQ -> command queue to task execution process from main robot process
currTask -> the current task as object
xPrev -> previous x coordinate
yPrev -> previous y coordinate
mbGoal -> queue object to pass target location for navigation as tuple (x, y)
Returns:
None
Description:
method to be run as parallel process for execution of a task"""
tIndex = currTask.index
# navigate to org location of task
# stage 0
tX = currTask.xOrg
tY = currTask.yOrg
stage = 0
(xNew, yNew, taskStat) = self.taskNavigation(msgQ, cmdQ, xPrev, yPrev,
tX, tY, stage, mbGoal,
mbState, odomQ)
if (taskStat == 1):
# => navigation to task location successful
# => a task may not be dropped after this stage
print("r[%d] finished stage 0 of t[%d]" %
(self.index, currTask.index))
if (currTask.taskType == "delivery"):
print("r[%d] going to (%0.2f,%0.2f) for finishing" %
(self.index, currTask.xFinish, currTask.yFinish))
stage = 1
# even if self.stage is set as 1, it will not be reflected in the other side (main robot process) as both are separate processes
# send the status change back to the main process
# msgQ - (xNew, yNew, stage, taskFinish)
misc.sendMsg(msgQ, None, [xNew, yNew, stage, 0])
print("r[%d] sent the stage update" % self.index)
# wait for stage change ack & stop command
# cmdQ - command 2: acknowledge state change, 1- stop execution, 0 - start
stageAck = 0
while (True):
cmds = misc.recvMsg(cmdQ)
for cmd in (cmds):
if (cmd == 1):
print("1 r[%d] received stop command" % self.index)
taskStat = 2
return 2
elif (cmd == 2):
print("1 r[%d] received stage change ack" % self.index)
stageAck = 1
else:
pass
if (stageAck == 1):
break
if (taskStat != 2):
# actual execution based on the task type
if (currTask.taskType == "fall"): # fall detection
taskStat = self.fall(cmdQ, tIndex, mbGoal, mbState, odomQ)
elif (currTask.taskType == "delivery"
): # food / drink / medicine delivery
(xNew, yNew,
taskStat) = self.delivery(msgQ, cmdQ, xPrev, yPrev, tX,
tY, tIndex, mbGoal, mbState,
odomQ)
elif (currTask.taskType == "door"): # door opening
taskStat = self.door(cmdQ, tIndex, mbGoal, mbState, odomQ)
elif (currTask.taskType == "medicine"
): # reminder to take medicine
taskStat = self.medicine(cmdQ, tIndex, mbGoal, mbState,
odomQ)
elif (currTask.taskType == "clean"): # cleaning the floor area
taskStat = self.clean(cmdQ, tIndex, mbGoal, mbState, odomQ)
elif (currTask.taskType == "surveillance"): # surveillance
taskStat = self.surveillance(cmdQ, tIndex, mbGoal, mbState,
odomQ)
else:
taskStat = self.mockTask(cmdQ, tIndex, mbGoal, mbState,
odomQ)
# whether taskStat == 1 or 2, send the current msg with the taskStat
misc.sendMsg(msgQ, None, [xNew, yNew, stage, taskStat])
return taskStat
elif (taskStat == 2):
# => task has been stopped by command
# return the status
misc.sendMsg(msgQ, None, [xNew, yNew, stage, taskStat])
return taskStat
return