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)