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