def des_activate_agentCam_timed(self): """ :description Add and remove target from target_representation_list for given time """ for agent in self.information_simulation.agentCams_list: if agent.t_add[agent.number_of_time_passed] <= constants.get_time() <= \ agent.t_del[agent.number_of_time_passed] and not agent.is_active: agent.is_active = True if agent not in self.active_AgentCams_list: self.active_AgentCams_list.append(agent) if agent not in self.agentCams_representation_list: self.agentCams_representation_list.append(agent) agent.log_main.info("Agent %d is activated at %.02f s" % (agent.id, constants.get_time())) elif constants.get_time() > agent.t_del[ agent.number_of_time_passed] and agent.is_active: if agent.number_of_time_passed < len(agent.t_add) - 1: agent.number_of_time_passed = agent.number_of_time_passed + 1 agent.is_active = False self.active_AgentCams_list.remove(agent) agent.log_main.info("Agent %d is desactivated at %.02f s" % (agent.id, constants.get_time()))
def no_targets_rotation_behaviour(configuration, camera): # points located at the intersection of the arc defining the end of the field of view and the # two lines at +- beta/2 defining the width of the field of view field_edge_points = camera.get_edge_points_world_frame() # time constraint to give time for the camera to move back in the room dt = constants.get_time() - camera.last_swipe_direction_change in_room = are_all_points_in_room(field_edge_points) if in_room: configuration.alpha += camera.swipe_angle_direction * camera.swipe_delta_alpha return if dt > camera.dt_next_swipe_direction_change: # change direction camera.swipe_angle_direction *= -1 camera.last_swipe_direction_change = constants.get_time() # find amount of time to turn before both edges get back in the room intersections_fov_with_room = intersection_fov_room(camera) # chose the correct point of intersection point_of_intersection = chose_point_of_intersection(intersections_fov_with_room, camera.swipe_angle_direction) # get angle from camera current position to intersection point found angle_intersection_point = -math.atan2(point_of_intersection[1] - camera.yc, point_of_intersection[0] - camera.xc) # calculate the lenght of the arc between furthest point and point of intersectionff angle_edge_intersection_len = math.fabs(angle_intersection_point - (camera.alpha - camera.beta / 2)) camera.dt_next_swipe_direction_change = angle_edge_intersection_len / camera.v_alpha_max else: configuration.alpha += camera.swipe_angle_direction * camera.swipe_delta_alpha
def random_movement_behaviour_based_on_time(configuration, camera): # if enough time has passed, randomly generate a new position delta_time = constants.get_time() - camera.last_swipe_position_change if delta_time > constants.DELTA_TIME_CHANGE_POSITION_RANDOM_MOVEMENT: configuration.x = random.uniform(0, constants.ROOM_DIMENSION_X) if camera.camera_type == mobileCam.MobileCameraType.FREE: configuration.y = random.uniform(0, constants.ROOM_DIMENSION_Y) camera.last_swipe_position_change = constants.get_time() camera.last_swipe_configuration = configuration else: configuration.x = camera.last_swipe_configuration.x if camera.camera_type == mobileCam.MobileCameraType.FREE: configuration.y = camera.last_swipe_configuration.y
def send_message_heartbeat(self): """ :description 1. Message without meaning, used to inform other agents that the agent is alive """ delta_time = constants.get_time() - self.time_last_heartbeat_sent if delta_time > constants.TIME_BTW_HEARTBEAT: m = MessageCheckACKNACK(constants.get_time(), self.id, self.signature, "heartbeat", "Hi there !") "Message send to every agent define in the room" self.broadcast_message(m, BroadcastTypes.ALL) self.time_last_heartbeat_sent = constants.get_time()
def send_message_DKF_info(self, target_id): """ :description Send a message containing the information needed for the distribution of the Kalman Filter. :param (int) target_id -- id of tracked target for which we're sending the info """ # only transmit information that contains enough information if self.memory.innovation_smaller_than_bound(target_id): print("innovation too low") return dkf_info_string = self.memory.get_DKF_info_string(target_id) # message containing the DKF information to send message = MessageCheckACKNACK( constants.get_time(), self.id, self.signature, MessageTypeAgentCameraInteractingWithRoom.INFO_DKF, dkf_info_string, target_id) # send the message to every other agent self.broadcast_message(message, BroadcastTypes.AGENT_CAM) # add message to the list if not already inside cdt = self.info_message_to_send.is_message_with_same_message(message) if not cdt: self.info_message_to_send.add_message(message)
def send_message_itemEstimator(self, itemEstimation, receivers=None): """ :description 1. Create a message based on a TargetEstimator 2. Place it in the list message_to_send :param 1. (TargetEstimator) targetEstimator -- TargetEstimator, see the class 2. (list) receivers -- [[receiver_id,receiver_signature], ... ], data to tell to whom to send the message. """ s = itemEstimation.to_string() s = s.replace("\n", "") s = s.replace(" ", "") message_type = MessageTypeAgentInteractingWithRoom.ITEM_ESTIMATION if itemEstimation.item_type == ItemType.AgentRepresentationEstimation: message_type = MessageTypeAgentInteractingWithRoom.AGENT_ESTIMATION if itemEstimation.item_type == ItemType.TargetRepresentationEstimation: message_type = MessageTypeAgentInteractingWithRoom.TARGET_ESTIMATION reference_to_target = itemEstimation.item.id m = MessageCheckACKNACK(constants.get_time(), self.id, self.signature, message_type, s, reference_to_target) self.broadcast_message(m, BroadcastTypes.AGENT_CAM, receivers)
def main(self): run = True reset = False do_next = True do_previous = False """Event loo^p""" while run: constants.time.sleep(constants.TIME_BTW_FRAME) if reset: self.reset() reset = False """Events related to target agent ...""" self.room.add_del_target_timed() self.room.des_activate_agentCam_timed() self.room.des_activate_camera_agentCam_timed() """GUI related actions options - see GUI_option.py file push r - reset the simulatio from the start push s - to take a screen_shot from the window GUI """ if constants.USE_GUI: if constants.USE_dynamic_analysis_simulated_room: region = self.dynamic_region region.init(constants.NUMBER_OF_POINT_DYNAMIC_ANALYSIS) self.dynamic_region.compute_all_map( constants.NUMBER_OF_POINT_DYNAMIC_ANALYSIS) else: region = self.static_region self.static_region.compute_all_map( constants.NUMBER_OF_POINT_STATIC_ANALYSIS, True) self.myGUI.updateGUI(self.room, region, self.link_agent_target.link_camera_target) (run, reset, do_next, do_previous) = self.myGUI.GUI_option.getGUI_Info() """Closing the simulation options""" if constants.get_time( ) > constants.TIME_STOP and constants.USE_GUI: run = False pygame.quit() elif constants.get_time() > constants.TIME_STOP: run = False self.clear() return (do_previous, do_next)
def sent_to_at_time(self, agent_id): new_agent_time = AgentTime(agent_id) try: old_agent_time_index = self.agentTime_list.index(new_agent_time) old_agent_time = self.agentTime_list[old_agent_time_index] old_agent_time.time = constants.get_time() except ValueError: self.agentTime_list.append(new_agent_time)
def assimilate(self, dkf_info_string, timestamp): if constants.DATA_TO_SEND != constants.AgentCameraCommunicationBehaviour.DKF: warnings.warn( "assimilating data even though DKF not defined in constants") if constants.USE_TIMESTAMP_FOR_ASSIMILATION: self.filter.assimilate(dkf_info_string, timestamp) else: self.filter.assimilate(dkf_info_string, constants.get_time())
def send_message_track_loose_target(self, track_loose_choice, target_id): if track_loose_choice == MessageTypeAgentCameraInteractingWithRoom.TRACKING_TARGET or track_loose_choice == MessageTypeAgentCameraInteractingWithRoom.LOSING_TARGET: print(track_loose_choice) message = MessageCheckACKNACK(constants.get_time(), self.id, self.signature, track_loose_choice, "", target_id) self.log_target_tracked.info(message.to_string()) self.broadcast_message(message, BroadcastTypes.AGENT_CAM)
def add_del_target_timed(self): """ :description Add and remove target from target_representation_list for given time """ for target in self.information_simulation.target_list: if target.t_add[target.number_of_time_passed] <= constants.get_time() <= \ target.t_del[target.number_of_time_passed] and not target.is_on_the_map: target.is_on_the_map = True self.target_representation_list.append(target) elif constants.get_time() > target.t_del[ target.number_of_time_passed] and target.is_on_the_map: if target.number_of_time_passed < len(target.t_add) - 1: target.number_of_time_passed = target.number_of_time_passed + 1 target.is_on_the_map = False self.target_representation_list.remove(target)
def receive_message_DKF_info(self, message): """ :description Receive a message contaning the information needed for the distribtion of the Kalman Filter. When received, the filter associated with the tracked target is informed and can assimilate the new data. :param message: instance of Message class. """ info_string = message.message concerned_target_id = int(message.item_ref) if not info_string == "[]": # if message not empty self.memory.process_DKF_info(concerned_target_id, info_string, constants.get_time())
def des_activate_camera_agentCam_timed(self): """ :description Add and remove target from target_representation_list for given time """ for agent in self.information_simulation.agentCams_list: camera = agent.camera if camera.t_add[camera.number_of_time_passed] <= constants.get_time() <= \ camera.t_del[camera.number_of_time_passed] and not camera.is_active: camera.is_active = True agent.log_main.info( "camera of a agent %d is activated at %.02f s" % (agent.id, constants.get_time())) elif constants.get_time() > camera.t_del[ camera.number_of_time_passed] and camera.is_active: if camera.number_of_time_passed < len(camera.t_add) - 1: camera.number_of_time_passed = camera.number_of_time_passed + 1 camera.is_active = False agent.log_main.info( "camera of a agent %d is desactivated at %.02f s" % (agent.id, constants.get_time()))
def compute_all_map(self, factor=3, save=False): """ :param - factor, room discretation from 1 to infinite. The bigger the number, the faster the computation will be (less precision). :return - Fill all the table that contains information from the maps """ if self.update_active_cams(): if constants.SAVE_DATA and save: np.savetxt( constants.ResultsPath.SAVE_LOAD_DATA_STATIC_REGION + "region_updated %.02f s" % constants.get_time(), self.minimum_id_in_view) np.savetxt( constants.ResultsPath.SAVE_LOAD_DATA_STATIC_REGION + "coverage_updated %.02f s" % constants.get_time(), self.coverage) (self.minimum_id_in_view, self.minimum_dist_in_view) = self.define_region_covered_by_cams() self.coverage = self.define_region_covered_by_numberOfCams() self.define_coverage_percentarge()
def random_movement_behaviour_based_on_position(configuration, camera): # if the camera is close to the randomly generated position, generate a new one delta_x_square = math.pow(camera.xc - camera.last_swipe_configuration.x, 2) delta_y_square = math.pow(camera.yc - camera.last_swipe_configuration.y, 2) delta_position = math.pow(delta_x_square + delta_y_square, .5) if delta_position < constants.DELTA_POS_CHANGE_POSITION_RANDOM_MOVEMENT: configuration.x = random.uniform(0, constants.ROOM_DIMENSION_X) if camera.camera_type == mobileCam.MobileCameraType.FREE: configuration.y = random.uniform(0, constants.ROOM_DIMENSION_Y) camera.last_swipe_position_change = constants.get_time() camera.last_swipe_configuration = configuration else: configuration.x = camera.last_swipe_configuration.x if camera.camera_type == mobileCam.MobileCameraType.FREE: configuration.y = camera.last_swipe_configuration.y
def handle_hearbeat(self): self.send_message_heartbeat() for heartbeat in self.hearbeat_tracker.agent_heartbeat_list: if heartbeat.is_to_late(): agent_to_suppress = -1 for agent in self.room_representation.agentCams_representation_list: if agent.id == heartbeat.agent_id: agent_to_suppress = agent break if not agent_to_suppress == -1 and agent_to_suppress.is_active: agent_to_suppress.is_active = False self.log_main.info( "Agent : " + str(agent_to_suppress.id) + "at: %.02fs is not connected anymore, last heartbeat : %.02f s" % (constants.get_time(), heartbeat.heartbeat_list[-1]))
def send_message_ack_nack(self, message, type_message): """ :description 1. Reply to a mesage using a ack or a nack ack = OK, nack = not OK :param 1. (Message) message -- Response according this message 2. (string) type_message -- "ack","nack" either OK or not OK """ m = MessageCheckACKNACK(constants.get_time(), self.id, self.signature, type_message, message.signature, message.targetRef) m.add_receiver(message.sender_id, message.sender_signature) self.info_message_to_send.add_message(m)
def update_target_based_on_memory(self, Target_TargetEstimator): """ :description Use agent's TargetEstimator to modify the RoomRepresentation :param 1. (list) Target_TargetEstimator -- see file memory, class Target_TargetEstimator """ for target_detected_id in Target_TargetEstimator.items_discovered: is_in_RoomRepresentation = False all_TargetEstimator_for_target_id = Target_TargetEstimator.get_item_list( target_detected_id) last_TargetEstimator = all_TargetEstimator_for_target_id[-1] for target in self.target_representation_list: if target.id == target_detected_id: is_in_RoomRepresentation = True target.xc = last_TargetEstimator.item.xc target.yc = last_TargetEstimator.item.yc target.type = last_TargetEstimator.item.target_type if not target.type == TargetType.FIX: target.alpha = last_TargetEstimator.item.alpha # TODO - replace variance on estimation ''' target.variance_on_estimation = last_TargetEstimator.variance_on_estimation if last_TargetEstimator.variance_on_estimation is not None: norm_variance_pos = np.sqrt( np.square(last_TargetEstimator.variance_on_estimation[0]) + np.square( last_TargetEstimator.variance_on_estimation[1])) else: norm_variance_pos = 0.1 ''' target.evaluate_target_confidence( 0.1, constants.get_time() - last_TargetEstimator.time_stamp) break if not is_in_RoomRepresentation: self.add_targetRepresentation( last_TargetEstimator.item.id, last_TargetEstimator.item.xc, last_TargetEstimator.item.yc, last_TargetEstimator.item.radius, last_TargetEstimator.item.target_type)
def __init__(self, id=None, xc=None, yc=None, alpha=None, beta=None, trajectory=None, field_depth=None, color=None, t_add=None, t_del=None, type=None, vx_vy_min=None, vx_vy_max=None, v_alpha_min=None, v_alpha_max=None, delta_beta=None, v_beta_min=None, v_beta_max=None): Camera.__init__(self, id, xc, yc, alpha, beta, field_depth, color, t_add, t_del) camera_attributes_not_to_txt = self.attributes_not_to_txt MobileCameraRepresentation.__init__(self, id, xc, yc, alpha, beta, field_depth, type, color) self.attributes_not_to_txt += [elem for elem in camera_attributes_not_to_txt if elem not in self.attributes_not_to_txt] self.attributes_not_to_txt += ["coeff_field", "coeff_std_position", "coeff_std_speed", "coeff_std_acc", "swipe_angle_direction", "swipe_delta_alpha", "last_swipe_direction_change", "dt_next_swipe_direction_change", "last_swipe_configuration", "last_swipe_position_change","beta_min","beta_max"] """Limit the variation""" self.vx_vy_min = vx_vy_min self.vx_vy_max = vx_vy_max self.v_alpha_min = v_alpha_min self.v_alpha_max = v_alpha_max self.v_beta_min = v_beta_min self.v_beta_max = v_beta_max self.delta_beta = delta_beta """Zoom""" self.coeff_field = constants.COEFF_VARIATION_FROM_FIELD_DEPTH self.coeff_std_position = constants.COEFF_STD_VARIATION_MEASURMENT_ERROR_POSITION self.coeff_std_speed = constants.COEFF_STD_VARIATION_MEASURMENT_ERROR_SPEED self.coeff_std_acc = constants.COEFF_STD_VARIATION_MEASURMENT_ERROR_ACCELERATION """Trajectory""" self.trajectory = TrajectoryPlaner(trajectory) """Variables for the swipe""" self.swipe_angle_direction = 1 self.swipe_delta_alpha = 0.2 self.last_swipe_direction_change = constants.get_time() self.dt_next_swipe_direction_change = -10 self.last_swipe_position_change = -10 from src.multi_agent.tools.configuration import Configuration self.last_swipe_configuration = Configuration(None, None, random.uniform(0, constants.ROOM_DIMENSION_X), random.uniform(0, constants.ROOM_DIMENSION_Y), 1, 1, self.field_depth, False) self.default_parameters()
def __init__(self, id, type_agent, t_add, t_del, color=0): super().__init__(id, type_agent, t_add, t_del, color) "Attibutes" self.memory = Memory(self.id) self.room_representation = room.RoomRepresentation(self.color) self.hearbeat_tracker = HeartbeatCounterAllAgent( self.id, self.signature, self.log_main) "Create his own thread" self.thread_is_running = 1 self.main_thread = None self.table_target_agent_lastTimeSent = AllItemSendToAtTime() self.table_agent_agent_lastTimeSent = AllItemSendToAtTime() self.time_last_heartbeat_sent = constants.get_time() self.log_room = create_logger(constants.ResultsPath.LOG_AGENT, "Room + memory", self.id)
def move_all_targets_thread(self): time_old = constants.time.time() while self.targets_moving: constants.time.sleep(constants.TIME_BTW_TARGET_MOVEMENT) delta_time = constants.time.time() - time_old self.link_agent_target.update_link_camera_target() self.link_agent_target.compute_link_camera_target() for target in self.room.target_representation_list: target.save_position() constants.time_when_target_are_moved = constants.get_time() self.exact_data_target.add_create_itemEstimation( constants.time_when_target_are_moved, -1, -1, copy.copy(target)) move_Target(target, delta_time) # theoritical calculation time_old = constants.time.time()
def getGUI_Info(self): self.position_mouse_all.append([pygame.mouse.get_pos()]) for event in pygame.event.get(): type_event = event.type if type_event == KEYDOWN: if event.key == K_ESCAPE: pygame.quit() return False, False, False, False if event.key == K_r: return True, True, True, False if event.key == K_s: print("screenshot") pygame.image.save( self.screen, constants.ResultsPath.DATA_SCREENSHOT + "/screenshot_at_%.02fs.png" % constants.get_time()) if event.key == K_n: return False, False, True, False if event.key == K_p: return False, False, False, True if event.key == K_v: self.show_virtual_cam = not self.show_virtual_cam if event.key == K_o: self.show_point_to_reach = not self.show_point_to_reach if event.key == K_f: self.show_field_cam = not self.show_field_cam if event.key == K_g: self.show_grid = not self.show_grid elif type_event == MOUSEMOTION and event.buttons[ 0] == 1: # déplacement + boutton enfoncer pass # print("Déplacement") #pour utiliser des boutton elif type_event == MOUSEMOTION: pass #self.update_mouse_position() elif type_event == MOUSEBUTTONUP: self.position_mouse_pressed.append([pygame.mouse.get_pos()]) return True, False, True, False
def add_heartbeat(self, m, log=None): if m.messageType == MessageTypeAgentInteractingWithRoom.HEARTBEAT: new_heartbeat_counter = HeartbeatCounter(m.sender_id, m.sender_signature, self.max_delay) try: old_heartbeat_counter_index = self.agent_heartbeat_list.index( new_heartbeat_counter) old_heartbeat_counter = self.agent_heartbeat_list[ old_heartbeat_counter_index] old_heartbeat_counter.add_heartbeat(m) log.debug("Receive heartbeat from agent: %d at time %.02f " % (m.sender_id, constants.get_time())) except ValueError: new_heartbeat_counter.add_heartbeat(m) self.agent_heartbeat_list.append(new_heartbeat_counter) if log is not None: log.info("Add a new heartbeat counter for agent: " + str(m.sender_id))
def send_message_timed_itemEstimator(self, item_estimator, delta_time, receivers=None): cdt_message_not_to_old = ( (constants.get_time() - item_estimator.time_stamp) <= constants.TRESH_TIME_TO_SEND_MEMORY) if cdt_message_not_to_old: table_time_sent = None if item_estimator.item_type == ItemType.TargetRepresentationEstimation: table_time_sent = self.table_target_agent_lastTimeSent elif item_estimator.item_type == ItemType.AgentRepresentationEstimation: table_time_sent = self.table_agent_agent_lastTimeSent send_message_to_agent = [] if receivers is None: receivers = [] if len(receivers) == 0: for agent in self.room_representation.agentCams_representation_list: if agent.id != self.id: if table_time_sent.should_sent_item_to_agent( item_estimator.item.id, agent.id, delta_time): table_time_sent.sent_to_at_time( item_estimator.item.id, agent.id) send_message_to_agent.append( (agent.id, agent.signature)) else: for receiver in receivers: if table_time_sent.should_sent_item_to_agent( item_estimator.item.id, receiver[0], delta_time): table_time_sent.sent_to_at_time( item_estimator.item.id, receiver[0]) send_message_to_agent.append( (receiver[0], receiver[1])) if len(send_message_to_agent) > 0: self.send_message_itemEstimator(item_estimator, send_message_to_agent)
def __init__(self, camera, t_add=-1, t_del=-1): if t_add == -1 or t_del == -1: t_add = [0] t_del = [constants.TIME_STOP] self.camera = camera self.virtual_camera = None super().__init__(AgentCam.number_agentCam_created, AgentType.AGENT_CAM, t_add, t_del, camera.color) self.camera_controller = None self.link_target_agent = None self.memory_of_objectives = [] self.memory_of_position_to_reach = [] self.log_execution = create_logger(constants.ResultsPath.LOG_AGENT, "Execution time", self.id) self.log_target_tracked = create_logger( constants.ResultsPath.LOG_AGENT, "Number of time a target is tracked", self.id) self.log_controller = create_logger(constants.ResultsPath.LOG_AGENT, "Controller", self.id) AgentCam.number_agentCam_created += 1 # targets to be tracked by this agent self.targets_to_track = [] # count how many times a target is tracked self.table_all_target_number_times_seen = AllTargetNUmberTimesSeen( self.id, self.log_target_tracked) # dictionnary to hold the internal priority of the targets (different from the target priority in # TargetEstimator). # This priority is used to modelize the fact that a target already tracked should be # prioritized compared to a target that just came into the field of vision. self.priority_dict = {} self.init_no_target_behaviour = False self.last_time_no_target_behaviour_init = constants.get_time()
def update_agent_based_on_memory(self, Agent_AgentEstimator): """ :description Use agent's TargetEstimator to modify the RoomRepresentation :param 1. (list) Target_TargetEstimator -- see file memory, class Target_TargetEstimator """ for agent_detected_id in Agent_AgentEstimator.items_discovered: is_in_RoomRepresentation = False all_TargetEstimator_for_target_id = Agent_AgentEstimator.get_item_list( agent_detected_id) itemEstimation = all_TargetEstimator_for_target_id[-1] for agent in self.agentCams_representation_list: camera = agent.camera_representation if agent.id == agent_detected_id: is_in_RoomRepresentation = True #agent.is_active = itemEstimation.item.is_active agent.evaluate_agent_confidence( 0.001, constants.get_time() - itemEstimation.time_stamp) camera.xc = itemEstimation.item.camera_representation.xc camera.yc = itemEstimation.item.camera_representation.yc # self.item_speeds = [0, 0] # [ camera.vx, camera.vy] # self.item_acceleration = [0, 0] # [ camera.ax, camera.ay] camera.camera_type = itemEstimation.item.camera_representation.camera_type camera.alpha = itemEstimation.item.camera_representation.alpha camera.beta = itemEstimation.item.camera_representation.beta camera.field_depth = itemEstimation.item.camera_representation.field_depth camera.is_active = itemEstimation.item.camera_representation.is_active break if not is_in_RoomRepresentation: self.agentCams_representation_list.append(itemEstimation.item)
def draw_room(self, tab, draw_time=False): if draw_time: label = self.font.render("time = %.02f s" % constants.get_time(), 10, WHITE) self.screen.blit(label, (self.x_offset, self.y_offset - 30)) pygame.draw.rect( self.screen, BLACK, (self.x_offset + tab[0] - 10, self.y_offset + tab[1] - 10, self.scale_x * tab[2] + 20, self.scale_y * tab[3] + 20)) pygame.draw.rect( self.screen, WHITE, (self.x_offset + tab[0] - 6, self.y_offset + tab[1] - 6, self.scale_x * tab[2] + 12, self.scale_y * tab[3] + 12)) pygame.draw.rect(self.screen, BLACK, (self.x_offset + tab[0], self.y_offset + tab[1], self.scale_x * tab[2], self.scale_y * tab[3])) if self.GUI_option.show_grid: self.draw_grid() self.draw_axis()
def to_string(self, agent_id): s = "agent %d time %.02f \n" % (agent_id, constants.get_time()) for tracker in self.tracker_list: s += tracker.to_string() return s
def find_configuration_for_targets(self, targets, used_for_movement=True): """ :description: Checks if a configuration can be found where all targets are seen. :param targets: target representations :param used_for_movement: if False, don't do the last non-virtual "get_configuration" :return: Configuration object if exists, None otherwise """ tracked_targets_room_representation = self.construct_room_representation_for_a_given_target_list( targets) virtual_configuration = \ self.compute_virtual_configuration(tracked_targets_room_representation.target_representation_list) better_config_found = False self.virtual_camera = copy.deepcopy(self.camera) self.virtual_camera.set_configuration(virtual_configuration) if not check_configuration_all_target_are_seen( self.virtual_camera, tracked_targets_room_representation): self.log_main.debug( "Configuration not found at time %.02f, starting variation on this config" % constants.get_time()) virtual_configuration.is_valid = False return virtual_configuration # configuration found, but bad score if virtual_configuration.configuration_score < constants.MIN_CONFIGURATION_SCORE: # Check around if there is a better config optimal_configuration = virtual_configuration.variation_on_configuration_found( self.virtual_camera) self.virtual_camera.set_configuration(optimal_configuration) if check_configuration_all_target_are_seen( self.virtual_camera, tracked_targets_room_representation): better_config_found = True # if the agent actually wants to move if used_for_movement: real_configuration = \ get_configuration_based_on_seen_target(self.camera, tracked_targets_room_representation.target_representation_list, self.room_representation, PCA_track_points_possibilites.MEANS_POINTS, self.memory_of_objectives, self.memory_of_position_to_reach, False) if better_config_found: real_configuration.x = optimal_configuration.x real_configuration.y = optimal_configuration.y real_configuration.is_valid = True return real_configuration # if the agent doesn't want to move if better_config_found: better_config = virtual_configuration.variation_on_configuration_found( self.virtual_camera) better_config.is_valid = True return better_config else: virtual_configuration.is_valid = True return virtual_configuration
def move_based_on_config(self, configuration, last_time_move): if configuration is None: self.log_main.debug("no config to move at time %.02f" % constants.get_time()) return constants.get_time() """Computing the vector field""" configuration.compute_vector_field_for_current_position( self.camera.xc, self.camera.yc) """Setting values to the PI controller""" self.camera_controller.set_targets(configuration.x, configuration.y, configuration.alpha, configuration.beta) """Define the values measured""" x_mes = self.camera.xc # + error si on veut ajouter ici y_mes = self.camera.yc alpha_mes = self.camera_controller.alpha_controller.bound_alpha_btw_minus_pi_plus_pi( self.camera.alpha) beta_mes = self.camera.beta if self.camera.camera_type == mobCam.MobileCameraType.RAIL: "1 D" x_mes = self.camera.trajectory.distance y_mes = 0 """Find the command to apply""" (x_command, y_command, alpha_command, beta_command) = self.camera_controller.get_command( x_mes, y_mes, alpha_mes, beta_mes) self.log_controller.info( "time:%.02f,x_mes:%.02f,y_mes:%.02f,alpha_mes:%.02f,beta_mes:%.02f,x_target:%.02f,y_target:%.02f,alpha_target:%.02f,beta_target:%.02f,x_command:%.02f,y_command:%.02f,alpha_command:%.02f,beta_command:%.02f" % (constants.get_time(), x_mes, y_mes, alpha_mes, beta_mes, configuration.x, configuration.y, configuration.alpha, configuration.beta, x_command, y_command, alpha_command, beta_command)) if constants.AGENT_MOTION_CONTROLLER == constants.AgentCameraController.Controller_PI: "We keep it has computed above" pass elif constants.AGENT_MOTION_CONTROLLER == constants.AgentCameraController.Vector_Field_Method: "The position command is here adapted with the vector field" x_controller = self.camera_controller.position_controller.x_controller y_controller = self.camera_controller.position_controller.y_controller x_command = x_controller.born_command(configuration.vector_field_x) y_command = y_controller.born_command(configuration.vector_field_y) else: print("error in move target, controller type not found") """Apply the command""" if constants.get_time() - last_time_move < 0: print("problem time < 0 : %.02f s" % constants.get_time()) return last_time_move else: dt = constants.get_time() - last_time_move if dt > 0.2: # If the dt is to large, the controller can not handle it anymore, the solution is to decompose dt in # smaller values self.camera.rotate(alpha_command, 0.2) self.camera.zoom(beta_command, 0.2) self.camera.move(x_command, y_command, 0.2) return self.move_based_on_config(configuration, last_time_move + 0.2) else: self.camera.rotate(alpha_command, dt) self.camera.zoom(beta_command, dt) self.camera.move(x_command, y_command, dt) return constants.get_time()