def test_refuse_mission_because_robot_is_busy(self):
tm = TaskManager(skills=[self.skill_generator()])
response = tm.assign_mission_service_handler(goals=[
self.test_goal_generator(),
self.test_goal_generator(),
self.test_goal_generator()
])
acceptedResponse = MSGConstructor.ActionAcceptedRefusedConstructor(
accepted='True', reasonOfRefusal='None')
self.assertEquals(response, acceptedResponse)
self.assertEquals(len(tm.ongoingTasks), 3)
response = tm.assign_mission_service_handler(goals=[
self.test_goal_generator(),
self.test_goal_generator(),
self.test_goal_generator()
])
refusedResponse = MSGConstructor.ActionAcceptedRefusedConstructor(
accepted='False',
reasonOfRefusal=
"Robot is currently busy with 3 tasks: ['example-skill', 'example-skill', 'example-skill']"
)
self.assertEquals(response, refusedResponse)
self.assertEquals(len(tm.ongoingTasks), 3)
def test_refused_because_empty_goals(self):
tm = TaskManager(skills=[self.skill_generator()])
response = tm.assign_mission_service_handler(goals=[])
refusedResponse = MSGConstructor.ActionAcceptedRefusedConstructor(
accepted='False', reasonOfRefusal='Empty goals!')
self.assertEquals(response, refusedResponse)
self.assertEquals(len(tm.ongoingTasks), 0)
def test_refused_because_different_robotId(self):
tm = TaskManager(skills=[self.skill_generator()])
response = tm.assign_mission_service_handler(
robotId='differentRobotId')
refusedResponse = MSGConstructor.ActionAcceptedRefusedConstructor(
accepted='False', reasonOfRefusal='Different robotId!')
self.assertEquals(response, refusedResponse)
self.assertEquals(len(tm.ongoingTasks), 0)
def test_accepted_simple_mission(self):
tm = TaskManager(skills=[self.skill_generator()])
response = tm.assign_mission_service_handler(
goals=[self.test_goal_generator()])
acceptedResponse = MSGConstructor.ActionAcceptedRefusedConstructor(
accepted='True', reasonOfRefusal='None')
self.assertEquals(response, acceptedResponse)
self.assertEquals(len(tm.ongoingTasks), 1)
def provide_task_status_handler(self, missionId):
# If mission found...
for mission in self.missions:
if mission['missionId'] == missionId:
taskStatus = MSGConstructor.TaskStatusConstructor(
missionId=mission['missionId'],
taskId=mission['taskId'],
statusCode=mission['statusCode'],
statusDescription=mission['statusDescription'],
when=mission['when'])
return taskStatus
# If mission not found
taskStatus = MSGConstructor.TaskStatusConstructor(
missionId='null',
taskId='null',
statusCode=12,
statusDescription='null',
when=rospy.get_rostime())
return taskStatus
def test_refused_bad_syntax_goal(self):
tm = TaskManager(skills=[self.skill_generator()])
response = tm.assign_mission_service_handler(
goals=['example-skill;exampleSkillProperty0'])
refusedResponse = MSGConstructor.ActionAcceptedRefusedConstructor(
accepted='False',
reasonOfRefusal=
'Badly formatted goal (example-skill;exampleSkillProperty0): need more than 1 value to unpack'
)
self.assertEquals(response, refusedResponse)
self.assertEquals(len(tm.ongoingTasks), 0)
def test_execute_mission_called_for_simple_mission(self, mock):
tm = TaskManager(skills=[self.skill_generator()])
response = tm.assign_mission_service_handler(goals=[
self.test_goal_generator(),
self.test_goal_generator(),
self.test_goal_generator()
],
executeMission=True)
self.assertTrue(mock.called)
acceptedResponse = MSGConstructor.ActionAcceptedRefusedConstructor(
accepted='True', reasonOfRefusal='None')
self.assertEquals(response, acceptedResponse)
self.assertEquals(len(tm.ongoingTasks), 3)
def test_accepted_complex_mission(self):
tm = TaskManager(skills=[
self.skill_generator(),
self.skill_generator(
skillName='drive',
skillType='DriveSkill',
skillClass='DriveSkill',
skillProperties=['toObjectId', 'toObjectType'])
])
response = tm.assign_mission_service_handler(goals=[
self.test_goal_generator(),
'drive;toObjectId=testToObjectId;toObjectType=testToObjectType'
])
acceptedResponse = MSGConstructor.ActionAcceptedRefusedConstructor(
accepted='True', reasonOfRefusal='None')
self.assertEquals(response, acceptedResponse)
self.assertEquals(len(tm.ongoingTasks), 2)
def update_mission_status(self, missionId, taskId, statusCode,
statusDescription):
if statusCode in [1, 3, 11]:
rospy.loginfo('[TaskManager] [' + str(self.robotId) + '] [' +
str(missionId) + '] ' + str(statusDescription))
elif statusCode in [4, 12]:
rospy.logerr('[TaskManager] [' + str(self.robotId) + '] [' +
str(missionId) + '] ' + str(statusDescription))
if self.taskStatusPub is not None:
taskStatus = MSGConstructor.TaskStatusConstructor(
missionId=missionId,
taskId=taskId,
statusCode=statusCode,
statusDescription=statusDescription,
when=rospy.get_rostime())
self.taskStatusPub.publish(taskStatus)
for mission in self.missions:
if mission['missionId'] == missionId:
mission['taskId'] = taskId
mission['statusCode'] = statusCode
mission['statusDescription'] = statusDescription
mission['when'] = rospy.get_rostime()
def assign_mission_service_handler(self,
missionId='defaultMissionId',
robotId='defaultRobotId',
goals=[],
priority='NORMAL',
executeMission=False):
if robotId != self.robotId:
rospy.logwarn('[TaskManager] [' + str(self.robotId) +
'] Mission ' + str(missionId) +
' refused: Different robotId!')
return MSGConstructor.ActionAcceptedRefusedConstructor(
accepted='False', reasonOfRefusal='Different robotId!')
if self.missionQueueSize > 0:
if len(self.taskQueue) >= self.missionQueueSize:
rospy.logwarn('[TaskManager] [' + str(self.robotId) +
'] Mission ' + str(missionId) +
' refused: Mission queue is already fulfilled!')
return MSGConstructor.ActionAcceptedRefusedConstructor(
accepted='False',
reasonOfRefusal='Mission queue is already fulfilled!')
#this feature was comented by Ivo/Heber request. #TODO: it should be uncomented to refuse again in the future
# for mission in self.missions:
# if missionId == mission['missionId']:
# rospy.logwarn('[TaskManager] [' + str(self.robotId) + '] Mission ' + str(missionId) + ' refused: Mission already stored!')
# # return MSGConstructor.ActionAcceptedRefusedConstructor(accepted='False', reasonOfRefusal='Mission already stored!')
if len(goals) == 0:
rospy.logwarn('[TaskManager] [' + str(self.robotId) +
'] Mission ' + str(missionId) +
' refused: Empty goals!')
return MSGConstructor.ActionAcceptedRefusedConstructor(
accepted='False', reasonOfRefusal='Empty goals!')
if priority not in self.possiblePriorities:
rospy.logwarn('[TaskManager] [' + str(self.robotId) +
'] Mission ' + str(missionId) +
' refused: Priority is not well defined!')
return MSGConstructor.ActionAcceptedRefusedConstructor(
accepted='False',
reasonOfRefusal='Priority is not well defined!')
missionSkills = []
for goal in goals:
try:
sf = SkillFactory(goal, self.skills)
missionSkills += [sf.skill]
except Exception as e:
rospy.logwarn('[TaskManager] [' + str(self.robotId) +
'] Mission ' + str(missionId) +
' refused: Badly formatted goal (' + str(goal) +
'): ' + str(e))
return MSGConstructor.ActionAcceptedRefusedConstructor(
accepted='False',
reasonOfRefusal='Badly formatted goal (' + str(goal) +
'): ' + str(e))
self.missions.append({
'missionId': missionId,
'taskId': '',
'statusCode': '',
'statusDescription': 'Mission Created'
})
priorityNum = self.possiblePriorities[
priority] #Transforms priority description (LOW, NORMAL or HIGH in a number 0,1,2)
mission = {
'missionId': missionId,
'missionSkills': missionSkills,
'priority': priorityNum,
'executeMission': executeMission
}
#this for loop apends missions in the queue deppendig on its priority
for task in self.taskQueue:
if task['priority'] < priorityNum:
self.taskQueue.insert(self.taskQueue.index(task), mission)
break
else:
self.taskQueue.append(mission)
if self.robotState == 'busy':
rospy.loginfo('[TaskManager] [' + str(self.robotId) +
'] Mission ' + str(missionId) +
' Accepted! Waiting for execution.')
rospy.loginfo('[TaskManager] [' + str(self.robotId) +
'] Mission Queue' +
str([task['missionId'] for task in self.taskQueue]))
return MSGConstructor.ActionAcceptedRefusedConstructor(
accepted='True', reasonOfRefusal='None')
else:
rospy.loginfo('[TaskManager] [' + str(self.robotId) +
'] Mission ' + str(missionId) +
' Accepted! Starting Execution.')
self.queue_execution_handler()
return MSGConstructor.ActionAcceptedRefusedConstructor(
accepted='True', reasonOfRefusal='None')
def cancel_mission_service_handler(self,
missionId=[],
robotId='defaultRobotId'):
if robotId != self.robotId:
rospy.logwarn('[TaskManager] [' + str(self.robotId) +
'] Mission \'' + str(missionId) +
'\' not canceled: Different robotId!')
return MSGConstructor.ActionAcceptedRefusedConstructor(
accepted='False', reasonOfRefusal='Different robotId!')
if missionId == []:
if self.currentActionClient is None and self.taskQueue == []:
rospy.logwarn(
'[TaskManager] [' + str(self.robotId) +
'] No mission was canceled. There are no ongoing or scheduled missions'
)
return MSGConstructor.ActionAcceptedRefusedConstructor(
accepted='False',
reasonOfRefusal='There are no ongoing or scheduled missions'
)
self.currentActionClient.cancel_all_goals()
self.ongoigSkills = []
self.taskQueue = self.taskQueue[:0]
rospy.loginfo('[TaskManager] [' + str(self.robotId) +
'] All Missions Canceled!')
return MSGConstructor.ActionAcceptedRefusedConstructor(
accepted='True', reasonOfRefusal='None')
failedAttempts = 0
failedCancelMissions = []
for mission in missionId:
if mission == self.ongoingMissionId:
self.currentActionClient.cancel_all_goals()
self.ongoigSkills = []
rospy.loginfo('[TaskManager] [' + str(self.robotId) +
'] Mission \'' + str(mission) +
'\' Canceled! It was Already Executing.')
else:
for task in self.taskQueue:
if task['missionId'] == mission:
del self.taskQueue[self.taskQueue.index(task)]
rospy.loginfo('[TaskManager] [' + str(self.robotId) +
'] Mission \'' + str(mission) +
'\' Canceled!')
break
else:
rospy.logwarn(
'[TaskManager] [' + str(self.robotId) +
'] Mission \'' + str(missionId) +
'\' not canceled: The mission is not in the queue!')
failedAttempts += 1
failedCancelMissions.append(mission)
if failedAttempts == 0:
return MSGConstructor.ActionAcceptedRefusedConstructor(
accepted='True', reasonOfRefusal='None')
else:
rospy.logwarn('[TaskManager] [' + str(self.robotId) +
'] Missions ' + str(failedCancelMissions) +
' are not in the queue')
return MSGConstructor.ActionAcceptedRefusedConstructor(
accepted='False',
reasonOfRefusal='Missions ' + str(failedCancelMissions) +
' are not in the queue')