def cancel_all_goals(self):
log.debug('Waiting for the Explorer action server...')
self.client.wait_for_server()
log.info('Canceling all goals on Explorer.')
self.exploration_pending.clear()
self.client.cancel_all_goals()
sleep(3)
def cancel_all_goals(self):
log.debug('Waiting for the Navigation action server...')
self.client.wait_for_server()
log.info('Canceling all goals on Navigation.')
self.base_pending.clear()
self.client.cancel_all_goals()
sleep(3)
def move_base(self, target):
""" Move base to a point of interest.
:param :target A PoseStamped point of interest.
"""
roll, pitch, yaw = euler_from_quaternion([
target.pose.orientation.x, target.pose.orientation.y,
target.pose.orientation.z, target.pose.orientation.w
])
transformend_orientation = quaternion_from_euler(roll, pitch, yaw + pi)
target.pose.orientation.x = transformend_orientation[0]
target.pose.orientation.y = transformend_orientation[1]
target.pose.orientation.z = transformend_orientation[2]
target.pose.orientation.w = transformend_orientation[3]
target.pose.position.z = 0
goal = MoveBaseGoal(target_pose=target)
self.target_pose = target
log.debug('Waiting for Navigation action server...')
self.client.wait_for_server()
log.info('Sending MoveBase goal.')
log.debug('\n' + str(target))
self.base_pending.set()
self.client.send_goal(goal,
feedback_cb=self.base_feedback,
done_cb=self.move_base_done)
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 explore(self, exploration_type=DoExplorationGoal.TYPE_DEEP):
goal = DoExplorationGoal(exploration_type=exploration_type)
log.debug('Waiting for the Explorer action server...')
self.client.wait_for_server()
log.info('Sending new exploration goal.')
self.exploration_pending.set()
self.client.send_goal(goal, done_cb=self.exploration_done)
def visit_qr(self, qr_id):
msg = VisitQRActionGoal()
msg.goal.qrId = qr_id
log.debug('Waiting for the DataFusion action server...')
self.visit.wait_for_server()
log.warning('Deleting QR wth ID -> %d.', qr_id)
self.visit.send_goal(msg.goal)
def scan(self):
""" The end effector starts scanning. """
goal = MoveEndEffectorGoal()
goal.command = MoveEndEffectorGoal.SCAN
log.debug('Waiting for the EndEffector action server.')
self.client.wait_for_server()
log.info('Sending SCAN goal.')
self.client.send_goal(goal)
def point_to(self, target, center='/pi_camera_frame'):
""" Points end effector to a target.
:param target: The vicitim frame ID.
:param center: The center of the frame the we will use.
"""
goal = MoveEndEffectorGoal()
goal.command = MoveEndEffectorGoal.TRACK
goal.point_of_interest = target
goal.center_point = center
log.debug('Waiting for the EndEffector action server.')
self.client.wait_for_server()
log.info('Sending TRACK goal.')
self.client.send_goal(goal)
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])
def slowly_point_to(self, target, center='/pi_camera_frame'):
"""
Points end effector to a target, slow enough so the image
is staying still.
:param target: The victim frame ID.
:param center: The center of the frame the we will use.
"""
goal = MoveEndEffectorGoal()
goal.command = MoveEndEffectorGoal.LAX_TRACK
goal.point_of_interest = target
goal.center_point = center
log.debug('Waiting for the EndEffector action server.')
self.client.wait_for_server()
log.info('Sending LAX TRACK goal.')
self.client.send_goal(goal)
sleep(5)
def cancel_all_goals(self):
log.debug('Waiting for the EndEffector action server...')
self.client.wait_for_server()
log.info('Canceling all goals on EndEffector.')
self.client.cancel_all_goals()
sleep(3)
def base_feedback(self, pose):
self.current_pose = pose.base_position
self.dispatcher.emit('move_base.feedback', pose, self.target_pose)
if self.verbose:
log.debug('Current pose updated.')
log.debug(self.current_pose)
