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