def announce_target(self, victim_id):
"""
Notify data fusion about the current target.
:param victim_id: The id of the current target/victim.
"""
goal = ChooseVictimGoal(victimId=victim_id)
log.debug('Waiting for the DataFusion action server...')
self.selection.wait_for_server()
log.info('Sending current target #%d', victim_id)
self.selection.send_goal(goal)
log.debug('Waiting for response...')
self.selection.wait_for_result()
status = self.selection.get_state()
result = self.selection.get_result()
verbose_status = ACTION_STATES[status]
if status == GoalStatus.SUCCEEDED:
log.info('Victim #%d, selected as the current target', victim_id)
else:
log.error('Victim selection failed with %s.', verbose_status)
return result.worldModel.victims
def validate_victim(self, victim_id, valid=False, verified=False):
"""
Update the data fusion registry with valid or not victims.
:param :victim_id The victim's ID.
:param :valid True if it was validated from the operator.
:param :verified True if got into the sensor hold state.
"""
goal = ValidateVictimGoal()
goal.victimId = victim_id
goal.victimValid = valid
goal.victimVerified = verified
victim_status = self.classify_target(valid, verified)
log.debug('Waiting for the DataFusion action server...')
self.validation.wait_for_server()
log.info('Validating victim #%d as %s.', victim_id, victim_status)
self.validation.send_goal(goal)
log.debug('Waiting for response...')
self.validation.wait_for_result()
status = self.validation.get_state()
result = self.validation.get_result()
verbose_status = ACTION_STATES[status]
if status == GoalStatus.SUCCEEDED:
log.info('Victim %d validated successfully.', victim_id)
else:
log.error('Validation failure with %s.', verbose_status)
return result.worldModel.victims
def delete_victim(self, victim_id):
"""
Delete a victim from the data fusion registry.
:param :victim_id The ID of the victim to be deleted.
"""
goal = ChooseVictimGoal(victimId=victim_id)
log.debug('Waiting for the DataFusion action server...')
self.deletion.wait_for_server()
log.warning('Deleting victim wth ID -> %d.', victim_id)
self.deletion.send_goal(goal)
log.debug('Waiting for response...')
self.deletion.wait_for_result()
status = self.deletion.get_state()
result = self.deletion.get_result()
verbose_status = ACTION_STATES[status]
if status == GoalStatus.SUCCEEDED:
log.info('Victim deletion succeded!')
else:
log.error('Victim deletion failed with %s.', verbose_status)
return result.worldModel.victims
def send_request(self, target):
""" Sends a validation request to the robot operator.
:param :target A target to be validated.
"""
goal = ValidateVictimGUIGoal()
goal.victimId = target.id
goal.victimFoundx = target.victimPose.pose.position.x
goal.victimFoundy = target.victimPose.pose.position.y
goal.probability = target.probability
goal.sensorIDsFound = target.sensors
log.debug('Waiting for the GUI action server.')
self.client.wait_for_server()
log.info('Sending validation request.')
if self.verbose:
log.debug(target)
self.client.send_goal(goal)
log.info('Waiting for response.')
self.client.wait_for_result()
status = self.client.get_state()
verbose_status = ACTION_STATES[status]
if status == GoalStatus.SUCCEEDED:
log.info('Validation request succeded!')
self.gui_result = self.client.get_result()
return True
else:
log.error('Validation request failed with %s.', verbose_status)
return False
def get_qrs(self):
log.info("Waiting for service %s." % topics.get_world_model)
wait_for_service(topics.get_world_model)
try:
res = ServiceProxy(topics.get_world_model, GetWorldModel)()
return res.worldModel.qrs
except ServiceProxy as e:
log.error(str(e))
return None
def move_base_done(self, status, result):
if self.base_pending.is_set():
if status == GoalStatus.SUCCEEDED:
log.info('Base has reached its destination.')
self.base_pending.clear()
self.dispatcher.emit('move_base.success', result)
elif status in TERMINAL_STATES.keys():
verbose_status = TERMINAL_STATES[status]
log.error('Base has failed to move with %s.', verbose_status)
self.base_pending.clear()
self.dispatcher.emit('move_base.retry', status)
if self.verbose:
log.info('MoveBase goal status: %s', ACTION_STATES[status])
def exploration_done(self, status, result):
# If the explorer is waiting on a goal.
if self.exploration_pending.is_set():
if status == GoalStatus.SUCCEEDED:
log.warning('Exploration has finished.')
self.exploration_pending.clear()
self.dispatcher.emit('exploration.success')
elif status in TERMINAL_STATES.keys():
verbose_status = TERMINAL_STATES[status]
log.error('Exploration has failed with %s.', verbose_status)
self.exploration_pending.clear()
self.dispatcher.emit('exploration.retry')
if self.verbose:
log.debug('Exploration goal status: %s', ACTION_STATES[status])