class DialogSteamapiKey: steamapi_window: QDialog def is_valid_steampi_key(self, key): if len(key) == 32: return True return False def steamapi_key_dialog(self): self.steamapi_window = QDialog() self.steamapi_window.setWindowTitle(_("Set steamapi key")) self.steamapi_window.setWindowIcon(self.switcher_logo) layout = QVBoxLayout() self.steamapi_window.setLayout(layout) text_label = QLabel( _("Used for getting avatars. Get yours from <a href='https://steamcommunity.com/dev/apikey'>steam</a>" )) apikey_edit = QLineEdit() save_button = QPushButton(_("Save")) text_label.setOpenExternalLinks(True) apikey_edit.setText(self.switcher.settings.get("steam_api_key")) layout.addWidget(text_label) layout.addWidget(apikey_edit) layout.addWidget(save_button) def save_enabled(): save_button.setEnabled( self.is_valid_steampi_key(apikey_edit.text())) def save(): self.switcher.settings["steam_api_key"] = apikey_edit.text() self.switcher.settings_write() self.steamapi_window.hide() if self.switcher.first_run: self.import_accounts_dialog() save_enabled() apikey_edit.textChanged.connect(lambda: save_enabled()) save_button.clicked.connect(lambda: save()) self.steamapi_window.show()
class SpecificWorker(GenericWorker): def __init__(self, proxy_map, fullscreen, startup_check=False): super(SpecificWorker, self).__init__(proxy_map) os.system('bash ../joystick.sh &') os.system('bash ../simulator.sh &') self.timer.timeout.connect(self.compute) self.TEST_MODE = False #True self.i_frame = 0 if self.TEST_MODE: self.model = SocNavAPI('.', None) self.img_mutex = threading.RLock() self.img = None self.threshold = 0.4 self.robot_coordinates = [0, 0] ### self.window = QWidget() self.button = QPushButton("enter") self.layout = QVBoxLayout() ### self.goal_coordinates = [100, 100] self.Period = 100 self.timer.start(self.Period) self.data = [] self.goal_data = [] self.wall_data = [] self.interaction_data = [] self.people_data = [] self.object_data = [] self.updates_list = [] self.initial_positions = [] self.initial_positions_objects = [] self.initial_flag = 0 self.data_json = [] self.speed_command = [0., 0., 0.] self.relations_dic = {} self.configGui = Ui_confDialog() self.configWindow = QDialog() self.configGui.setupUi(self.configWindow) if fullscreen != "True": self.WINDOW_WIDTH = 1042 self.WINDOW_HEIGHT = 819 self.setFixedSize(self.WINDOW_WIDTH, self.WINDOW_HEIGHT) self.save_interval = 0.1 # self.last_saved_time = time.time() self.new_goal = False self.on_a_mission = True self.started_mission = False self.ui.configuration.clicked.connect(self.configuration_slot) self.ui.regenerate.clicked.connect(self.regenerate_slot) self.ui.quit.clicked.connect(self.quit_slot) self.configGui.nhumans_min.valueChanged.connect( self.min_nhumans_changed) self.configGui.nhumans_max.valueChanged.connect( self.max_nhumans_changed) self.configGui.nwandhumans_min.valueChanged.connect( self.min_nwandhumans_changed) self.configGui.nwandhumans_max.valueChanged.connect( self.max_nwandhumans_changed) self.configGui.ntables_min.valueChanged.connect( self.min_ntables_changed) self.configGui.ntables_max.valueChanged.connect( self.max_ntables_changed) self.configGui.nplants_min.valueChanged.connect( self.min_nplants_changed) self.configGui.nplants_max.valueChanged.connect( self.max_nplants_changed) self.configGui.nrelations_min.valueChanged.connect( self.min_nrelations_changed) self.configGui.nrelations_max.valueChanged.connect( self.max_nrelations_changed) self.settings = QSettings("dataset", "xxx") contributor = self.settings.value("contributor", "default") self.ui.contributor.setText(contributor) self.ui.contributor.textChanged.connect(self.contributor_changed) if fullscreen == 'True': self.showFullScreen() @QtCore.Slot() def contributor_changed(self): self.settings.setValue("contributor", self.ui.contributor.text()) @QtCore.Slot() def configuration_slot(self, active): if active: self.configWindow.show() else: self.configWindow.hide() @QtCore.Slot() def regenerate_slot(self): print('regenerate') self.omnirobot_proxy.setSpeedBase(0, 0, 0) self.i_frame = 0 self.updates_list = [] self.initial_positions = [] self.initial_positions_objects = [] self.initial_flag = 0 self.info() self.simulator_proxy.regenerate(scene=self.relations_dic) self.new_goal = False self.on_a_mission = True self.started_mission = False @QtCore.Slot() def quit_slot(self): os.system('pwd') os.system('bash ../kill.sh &') @QtCore.Slot() def keyPressEvent(self, event): if event.type() == QtCore.QEvent.KeyPress: if event.key() == QtCore.Qt.Key_Q: self.quit_slot() elif event.key() == QtCore.Qt.Key_R: self.regenerate_slot() @QtCore.Slot() def min_nhumans_changed(self, val): if val > self.configGui.nhumans_max.value(): self.configGui.nhumans_max.setValue(val) @QtCore.Slot() def max_nhumans_changed(self, val): if val < self.configGui.nhumans_min.value(): self.configGui.nhumans_min.setValue(val) @QtCore.Slot() def min_nwandhumans_changed(self, val): if val > self.configGui.nwandhumans_max.value(): self.configGui.nwandhumans_max.setValue(val) @QtCore.Slot() def max_nwandhumans_changed(self, val): if val < self.configGui.nwandhumans_min.value(): self.configGui.nwandhumans_min.setValue(val) @QtCore.Slot() def min_ntables_changed(self, val): if val > self.configGui.ntables_max.value(): self.configGui.ntables_max.setValue(val) @QtCore.Slot() def max_ntables_changed(self, val): if val < self.configGui.ntables_min.value(): self.configGui.ntables_min.setValue(val) @QtCore.Slot() def min_nplants_changed(self, val): if val > self.configGui.nplants_max.value(): self.configGui.nplants_max.setValue(val) @QtCore.Slot() def max_nplants_changed(self, val): if val < self.configGui.nplants_min.value(): self.configGui.nplants_min.setValue(val) @QtCore.Slot() def min_nrelations_changed(self, val): if val > self.configGui.nrelations_max.value(): self.configGui.nrelations_max.setValue(val) @QtCore.Slot() def max_nrelations_changed(self, val): if val < self.configGui.nrelations_min.value(): self.configGui.nrelations_min.setValue(val) def __del__(self): print('SpecificWorker destructor') self.quit_slot() def setParams(self, params): return True @QtCore.Slot() def compute(self): self.show_image() if self.TEST_MODE: self.predict_from_network() else: self.collect_data() def predict_from_network(self): if not self.on_a_mission or not self.new_goal: return proximity = math.sqrt( sum([(a - b)**2 for a, b in zip(self.robot_coordinates, self.goal_coordinates) ])) if proximity <= self.threshold: self.omnirobot_proxy.setSpeedBase(0, 0, 0) # self.regenerate_slot() return if time.time() - self.last_saved_time > self.save_interval: self.last_saved_time = time.time() temp_obj_list, temp_inter_list, temp_people_list, temp_goal_list, temp_wall_list = self.get_scene( ) data = { "timestamp": time.time(), "objects": temp_obj_list, "people": temp_people_list, "walls": temp_wall_list, "goal": temp_goal_list, "command": self.speed_command, "interaction": temp_inter_list } self.updates_list.insert(0, data) i = -1 new_data = [data] for i in range(1, len(self.updates_list)): if new_data[-1]['timestamp'] - self.updates_list[i][ 'timestamp'] >= FRAMES_INTERVAL: new_data.append(self.updates_list[i]) if len(new_data) == N_INTERVALS: break if len(new_data) >= 1: graph = GenerateDataset(new_data, 'run', '1', i_frame=self.i_frame) results = self.model.predictOneGraph(graph)[0] adv = results[0].item() * 3.5 rot = results[2].item() * 4. self.omnirobot_proxy.setSpeedBase(adv, 0, rot) # if len(new_data)==N_INTERVALS and i+1 < len(self.updates_list): # for r in range(i+1, len(self.updates_list)): # self.updates_list.pop(i+1) print(results) self.i_frame += 1 def collect_data(self): if not self.on_a_mission or not self.new_goal or not self.started_mission: return self.omnirobot_proxy.setSpeedBase(*self.speed_command) control_flag = 0 proximity = math.sqrt( sum([(a - b)**2 for a, b in zip(self.robot_coordinates, self.goal_coordinates) ])) if time.time() - self.last_saved_time > self.save_interval: self.last_saved_time = time.time() temp_obj_list, temp_inter_list, temp_people_list, temp_goal_list, temp_wall_list = self.get_scene( ) if len(temp_obj_list) != 0 or len(temp_people_list) != 0 or len( temp_wall_list) != 0: self.updates_list.append({ "timestamp": time.time(), "objects": temp_obj_list, "people": temp_people_list, "walls": temp_wall_list, "goal": temp_goal_list, "command": self.speed_command, "interaction": temp_inter_list }) self.data_json = self.updates_list if proximity <= self.threshold: msgBox = QtWidgets.QMessageBox() msgBox.setText("Robot found the Goal") msgBox.setInformativeText("Do you want to save the data?") msgBox.setStandardButtons(QtWidgets.QMessageBox.Save | QtWidgets.QMessageBox.Discard) msgBox.setDefaultButton(QtWidgets.QMessageBox.Save) self.new_goal = False self.on_a_mission = False self.show_image() ret = msgBox.exec_() if ret == QMessageBox.Save: time_str = '{0:%Y-%m-%d__%H:%M:%S}'.format( datetime.datetime.now()) with open(f'../{self.ui.contributor.text()}__{time_str}.json', 'w') as fp: json.dump(self.data_json, fp, indent=4, sort_keys=True) self.updates_list = [] self.initial_positions = [] self.initial_positions_objects = [] self.initial_flag = 0 self.regenerate_slot() def get_scene(self): temp_obj_list = [] temp_inter_list = [] temp_people_list = [] temp_goal_list = [] temp_wall_list = [] for inter in self.interaction_data: temp_inter_dic = {} temp_inter_dic['src'] = inter.idSrc temp_inter_dic['relation'] = inter.type temp_inter_dic['dst'] = inter.idDst temp_inter_list.append(temp_inter_dic) if self.initial_flag == 0: for _, obj in enumerate(self.object_data): self.initial_positions_objects.append( [obj.x, obj.y, obj.angle]) for _, person in enumerate(self.people_data): self.initial_positions.append( [person.x, person.y, person.angle]) #print("INITIAL POSITIONS >>>>",self.initial_positions) self.initial_flag = 1 for pos, obj in enumerate(self.object_data): if obj.id != -2: temp_obj_dic = {} temp_obj_dic["id"] = obj.id temp_obj_dic["x"] = obj.x temp_obj_dic["y"] = obj.y temp_obj_dic["a"] = obj.angle temp_obj_dic[ "vx"] = self.initial_positions_objects[pos][0] - obj.x temp_obj_dic[ "vy"] = self.initial_positions_objects[pos][1] - obj.y temp_obj_dic[ "va"] = self.initial_positions_objects[pos][2] - obj.angle temp_obj_dic["size_x"] = abs(obj.bbx2 - obj.bbx1) temp_obj_dic["size_y"] = abs(obj.bby2 - obj.bby1) self.initial_positions_objects[pos][0] = obj.x self.initial_positions_objects[pos][1] = obj.y self.initial_positions_objects[pos][2] = obj.angle temp_obj_list.append(temp_obj_dic) for pos, person in enumerate(self.people_data): temp_person_dic = {} temp_person_dic["id"] = person.id temp_person_dic["x"] = person.x temp_person_dic["y"] = person.y temp_person_dic["a"] = person.angle temp_person_dic["vx"] = self.initial_positions[pos][0] - person.x temp_person_dic["vy"] = self.initial_positions[pos][1] - person.y temp_person_dic[ "va"] = self.initial_positions[pos][2] - person.angle self.initial_positions[pos][0] = person.x self.initial_positions[pos][1] = person.y self.initial_positions[pos][2] = person.angle temp_people_list.append(temp_person_dic) for wall in self.wall_data: temp_wall_dic = {} temp_wall_dic["x1"] = wall.x1 temp_wall_dic["y1"] = wall.y1 temp_wall_dic["x2"] = wall.x2 temp_wall_dic["y2"] = wall.y2 temp_wall_list.append(temp_wall_dic) temp_goal_dic = { "x": self.goal_coordinates[0], "y": self.goal_coordinates[1] } temp_goal_list.append(temp_goal_dic) return temp_obj_list, temp_inter_list, temp_people_list, temp_goal_list, temp_wall_list def show_image(self): self.img_mutex.acquire() img = copy.deepcopy(self.img) self.img_mutex.release() if img is None: return if not self.TEST_MODE: if not self.on_a_mission: img[:, :, 1] = img[:, :, 2] // 3 img[:, :, 2] = img[:, :, 1] // 3 elif self.on_a_mission and not self.started_mission: img[:, :, 0] = img[:, :, 0] // 3 img[:, :, 2] = img[:, :, 2] // 3 self.ui.label.setPixmap( QPixmap( QImage(img.data, img.shape[1], img.shape[0], QtGui.QImage.Format_RGB888))) # # SUBSCRIPTION to gotobjects method from ObjectDetector interface # def ObjectDetector_gotobjects(self, lst): # #get robots position self.object_data = lst for obj in lst: if obj.id == -2: self.robot_coordinates = [obj.x, obj.y] # #pass # # SUBSCRIPTION to gotinteractions method from InteractionDetector interface # def InteractionDetector_gotinteractions(self, lst): self.interaction_data = lst # # SUBSCRIPTION to gotpeople method from PeopleDetector interface # def PeopleDetector_gotpeople(self, lst): self.people_data = lst # # SUBSCRIPTION to gotwalls method from WallDetector interface # def WallDetector_gotwalls(self, lst): self.wall_data = lst # # SUBSCRIPTION to newsequence method from ByteSequencePublisher interface # def ByteSequencePublisher_newsequence(self, bs): # print('GOT NEW BYTE SEQUENCE', bs) # bs = np.load(bs, allow_pickle=True) self.img_mutex.acquire() self.img = pickle.loads(bs) self.img_mutex.release() # # SUBSCRIPTION to goalupdated method from GoalPublisher interface # def GoalPublisher_goalupdated(self, goal): if self.on_a_mission: self.new_goal = True self.goal_data = goal self.goal_coordinates = [goal.x, goal.y] #pass def JoystickAdapter_sendData(self, data): if self.on_a_mission: self.started_mission = True adv = -3.5 * data.axes[1].value # m/s rot = 4.0 * data.axes[0].value # rad/s self.speed_command = [adv, 0, rot] self.omnirobot_proxy.setSpeedBase(adv, 0., rot) def info(self): dic = { "min_humans": self.configGui.nhumans_min.value(), "max_humans": self.configGui.nhumans_max.value(), "min_wandering_humans": self.configGui.nwandhumans_min.value(), "max_wandering_humans": self.configGui.nwandhumans_max.value(), "min_plants": self.configGui.nplants_min.value(), "max_plants": self.configGui.nplants_max.value(), "min_tables": self.configGui.ntables_min.value(), "max_tables": self.configGui.ntables_max.value(), "min_relations": self.configGui.nrelations_min.value(), "max_relations": self.configGui.nrelations_max.value() } self.relations_dic = json.dumps(dic)
class SpecificWorker(GenericWorker): def __init__(self, proxy_map, fullscreen, startup_check=False): super(SpecificWorker, self).__init__(proxy_map) # os.system('bash ../joystick.sh &') os.system('bash ../simulator.sh &') self.timer.timeout.connect(self.compute) self.gaussian_spaces = GaussianSpaces() self.img_mutex = threading.RLock() self.img = None self.proximity_threshold = 0.4 self.robot_coordinates = [0,0] ### self.window = QWidget() self.button = QPushButton("enter") self.layout = QVBoxLayout() ### self.goal_coordinates = [100,100] self.Period = 100 self.timer.start(self.Period) self.data = [] self.goal_data = [] self.wall_data = [] self.interaction_data = [] self.people_data = [] self.object_data = [] self.updates_list = [] self.data_json = [] self.speed_command = [0., 0., 0.] self.relations_dic = {} #Metrics self.init_metrics = True self.initial_time = 0. self.navigation_time = 0. self.last_robot_position = [] self.distance_traveled = 0. self.interaction_areas = [] self.configGui = Ui_confDialog() self.configWindow = QDialog() self.configGui.setupUi(self.configWindow) if fullscreen != "True": self.WINDOW_WIDTH = 1042 self.WINDOW_HEIGHT = 819 self.setFixedSize(self.WINDOW_WIDTH,self.WINDOW_HEIGHT) self.save_interval = 0.1 # self.last_saved_time = time.time() self.new_goal = False self.on_a_mission = True self.started_mission = False self.ui.configuration.clicked.connect(self.configuration_slot) self.ui.regenerate.clicked.connect(self.regenerate_slot) self.ui.quit.clicked.connect(self.quit_slot) self.ui.save_metrics.clicked.connect(self.save_metrics) self.configGui.nhumans_min.valueChanged.connect(self.min_nhumans_changed) self.configGui.nhumans_max.valueChanged.connect(self.max_nhumans_changed) self.configGui.nwandhumans_min.valueChanged.connect(self.min_nwandhumans_changed) self.configGui.nwandhumans_max.valueChanged.connect(self.max_nwandhumans_changed) self.configGui.ntables_min.valueChanged.connect(self.min_ntables_changed) self.configGui.ntables_max.valueChanged.connect(self.max_ntables_changed) self.configGui.nplants_min.valueChanged.connect(self.min_nplants_changed) self.configGui.nplants_max.valueChanged.connect(self.max_nplants_changed) self.configGui.nrelations_min.valueChanged.connect(self.min_nrelations_changed) self.configGui.nrelations_max.valueChanged.connect(self.max_nrelations_changed) if fullscreen == 'True': self.showFullScreen() @QtCore.Slot() def configuration_slot(self, active): if active: self.configWindow.show() else: self.configWindow.hide() @QtCore.Slot() def regenerate_slot(self): print('regenerate') self.omnirobot_proxy.setSpeedBase(0, 0, 0) self.i_frame = 0 self.updates_list = [] self.initial_positions = [] self.initial_positions_objects = [] self.initial_flag=0 self.info() self.simulator_proxy.regenerate(scene=self.relations_dic) self.new_goal = False self.on_a_mission = True self.started_mission = False self.init_metrics = True @QtCore.Slot() def quit_slot(self): os.system('pwd') os.system('bash ../kill.sh &') @QtCore.Slot() def keyPressEvent(self, event): if event.type() == QtCore.QEvent.KeyPress: if event.key() == QtCore.Qt.Key_Q: self.quit_slot() elif event.key() == QtCore.Qt.Key_R: self.regenerate_slot() @QtCore.Slot() def min_nhumans_changed(self, val): if val>self.configGui.nhumans_max.value(): self.configGui.nhumans_max.setValue(val) @QtCore.Slot() def max_nhumans_changed(self, val): if val<self.configGui.nhumans_min.value(): self.configGui.nhumans_min.setValue(val) @QtCore.Slot() def min_nwandhumans_changed(self, val): if val>self.configGui.nwandhumans_max.value(): self.configGui.nwandhumans_max.setValue(val) @QtCore.Slot() def max_nwandhumans_changed(self, val): if val<self.configGui.nwandhumans_min.value(): self.configGui.nwandhumans_min.setValue(val) @QtCore.Slot() def min_ntables_changed(self, val): if val>self.configGui.ntables_max.value(): self.configGui.ntables_max.setValue(val) @QtCore.Slot() def max_ntables_changed(self, val): if val<self.configGui.ntables_min.value(): self.configGui.ntables_min.setValue(val) @QtCore.Slot() def min_nplants_changed(self, val): if val>self.configGui.nplants_max.value(): self.configGui.nplants_max.setValue(val) @QtCore.Slot() def max_nplants_changed(self, val): if val<self.configGui.nplants_min.value(): self.configGui.nplants_min.setValue(val) @QtCore.Slot() def min_nrelations_changed(self, val): if val>self.configGui.nrelations_max.value(): self.configGui.nrelations_max.setValue(val) @QtCore.Slot() def max_nrelations_changed(self, val): if val<self.configGui.nrelations_min.value(): self.configGui.nrelations_min.setValue(val) def __del__(self): os.system('kill -15 `ps ax |grep simulator | grep config | awk \'{print $1}\'`') # os.system('kill -15 `ps ax |grep joystick | grep config | awk \'{print $1}\'`') print('SpecificWorker destructor') self.quit_slot() def setParams(self, params): return True @QtCore.Slot() def compute(self): self.show_image() if not self.on_a_mission or not self.new_goal: return proximity = math.sqrt(self.goal_coordinates[0] ** 2 + self.goal_coordinates[1] ** 2) if proximity <= self.proximity_threshold: self.on_a_mission = False self.update_metrics() scenario = self.get_scene() d = SNGNN2DData() d.gridSize = scenario.shape[0] d.areaSize = 800. d.g = scenario.reshape([1,d.gridSize*d.gridSize])[0] d.g = d.g.tolist() self.sngnn2d_proxy.gotgrid(d) def update_metrics(self): robot_position = [ [r.x, r.y] for r in self.object_data if r.id==-2] robot_angle = [ r.angle for r in self.object_data if r.id==-2] if self.init_metrics: self.initial_time = time.time() self.last_time = self.initial_time self.distance_to_goal = math.sqrt(self.goal_coordinates[0] ** 2 + self.goal_coordinates[1] ** 2) self.last_robot_position = robot_position[0] self.last_robot_angle = robot_angle[0] self.distance_traveled = 0. self.cum_heading_changes1 = 0. self.cum_heading_changes2 = 0. self.minimum_distance_to_human = 100. self.number_of_intrusions_intimate = 0 self.number_of_intrusions_personal = 0 self.number_of_intrusions_in_interactions = 0 self.number_of_steps = 0 self.area_computed = False self.room_area = 0 self.init_metrics = False if not self.area_computed: if self.wall_data: room = [] for wall in self.wall_data: room.append([wall.x1, wall.y1]) room.append([self.wall_data[-1].x2, self.wall_data[-1].y2]) self.room_area = Polygon(room).area self.area_computed = True self.number_of_steps += 1 current_time = time.time() self.navigation_time = current_time-self.initial_time self.distance_traveled += math.sqrt(sum([(a - b) ** 2 for a, b in zip(self.last_robot_position, robot_position[0])])) self.last_robot_position = robot_position[0] angle_diff = math.pi - math.fabs(math.fabs(robot_angle[0]-self.last_robot_angle)-math.pi) if angle_diff < 0: angle_diff = 2*math.pi + angle_diff if angle_diff > 0: self.cum_heading_changes1 += angle_diff/(current_time-self.last_time) self.cum_heading_changes2 += angle_diff self.last_robot_angle = robot_angle[0] self.last_time = current_time min_dist_to_h = 100 for h in self.people_data: dist_to_h = math.sqrt(h.x ** 2 + h.y ** 2) if dist_to_h < min_dist_to_h: min_dist_to_h = dist_to_h if min_dist_to_h < self.minimum_distance_to_human: self.minimum_distance_to_human = min_dist_to_h if min_dist_to_h <= 0.45: self.number_of_intrusions_intimate +=1 if min_dist_to_h <= 1.2: self.number_of_intrusions_personal +=1 for inter_area in self.interaction_areas: if inter_area.contains(Point(0.,0.)): self.number_of_intrusions_in_interactions += 1 # print("room area", self.room_area, "navigation time", self.navigation_time, "distance", self.distance_traveled, "chc", self.cum_heading_changes, # "minimum distance", self.minimum_distance_to_human,"number of instrusions", self.number_of_intrusions, "number of steps", self.number_of_steps) @QtCore.Slot() def save_metrics(self): time_str = '{0:%Y-%m-%d - %H:%M:%S}'.format(datetime.datetime.now()) proximity = math.sqrt(self.goal_coordinates[0] ** 2 + self.goal_coordinates[1] ** 2) if proximity<=0.5: self.goal_reached = True else: self.goal_reached = False metrics = {"date" : time_str, "room_area" : self.room_area, "number_of_humans" : len(self.people_data), "number_of_objects" : len(self.object_data)-1, "number_of_interactions" : len(self.interaction_data), "distance_to_goal" : self.distance_to_goal, "navigation_time" : self.navigation_time, "distance_traveled" : self.distance_traveled, "chc1" : self.cum_heading_changes1, "chc2" : self.cum_heading_changes2, "minimum_distance_to_human" : self.minimum_distance_to_human, "number_of_intrusions_intimate" : self.number_of_intrusions_intimate, "number_of_intrusions_personal" : self.number_of_intrusions_personal, "number_of_intrusions_in_interactions" : self.number_of_intrusions_in_interactions, "number_of_steps" : self.number_of_steps, "goal_reached": self.goal_reached} all_metrics = [] fname = 'metrics_gaussians.json' if os.path.isfile(fname): with open(fname, 'r') as fp: all_metrics = json.load(fp) all_metrics.append(metrics) with open(fname, 'w') as fp: json.dump(all_metrics, fp, indent=4) print("saving metrics") def get_scene(self): initial_time = time.time() wGrid = 50 wCell = 8. entity_position = dict() self.interaction_areas = [] personsIDs = [] persons = [] for h in self.people_data: if h.x<4. and h.x>-4. and h.y<4. and h.y>-4. : personsIDs.append(h.id) p = PersonFields(h.x*1000., h.y*1000., h.angle+math.pi/2.) persons.append(p) entity_position[h.id] = (h.x, h.y) objectsIDs = [] objects = [] for o in self.object_data: if o.x<4. and o.x>-4. and o.y<4. and o.y>-4. : objectsIDs.append(o.id) obj = ObjectFields(o.x*1000., o.y*1000., o.angle+math.pi/2., 3.) objects.append(obj) entity_position[o.id] = (o.x, o.y) personGroups = [] objectInterGroup = [] for inter in self.interaction_data: newGroup = [] if inter.idSrc in personsIDs: if inter.idDst in personsIDs or inter.idDst in objectsIDs: p1 = entity_position[inter.idSrc] p2 = entity_position[inter.idDst] V = (p2[0] - p1[0], p2[1] - p1[1]) lV = math.sqrt(V[0] ** 2 + V[1] ** 2) Vn = (-V[1]/lV, V[0]/lV) inter_area = [] inter_area.append((p1[0]-Vn[0]*0.25, p1[1]-Vn[1]*0.25)) inter_area.append((p1[0]+Vn[0]*0.25, p1[1]+Vn[1]*0.25)) inter_area.append((p2[0]+Vn[0]*0.25, p2[1]+Vn[1]*0.25)) inter_area.append((p2[0]-Vn[0]*0.25, p2[1]-Vn[1]*0.25)) self.interaction_areas.append(Polygon(inter_area)) if inter.idDst in personsIDs: personSrc = persons[personsIDs.index(inter.idSrc)] newGroup.append(personSrc) personDst = persons[personsIDs.index(inter.idDst)] newGroup.append(personDst) personGroups.append(newGroup) personsIDs.remove(inter.idSrc) persons.remove(personSrc) personsIDs.remove(inter.idDst) persons.remove(personDst) elif inter.idDst in objectsIDs: personSrc = persons[personsIDs.index(inter.idSrc)] objectDst = objects[objectsIDs.index(inter.idDst)] objectDst.angle = personSrc.angle - math.pi objectDst.space = math.sqrt((objectDst.x-personSrc.x) ** 2 + (objectDst.z-personSrc.z) ** 2)/1000. + 0.5 objectInterGroup.append((personSrc, objectDst)) for ip in persons: personGroups.append([ip]) next_time1 = time.time() scenario = np.full((wGrid, wGrid), 1., np.float32) for g in personGroups: intimate = [] personal = [] social = [] intimate, personal, social = self.gaussian_spaces.getAllPersonalSpaces(g, False) group_scenario = np.full((wGrid, wGrid), 1., np.float32) for g in social: polyline = [] for p in g: polyline.append([int(p[0]*wGrid/wCell+wGrid/2), int(-p[1]*wGrid/wCell+wGrid/2)]) cvpoly = np.array(polyline) cv2.fillPoly(group_scenario, [cvpoly], 0.62) for g in personal: polyline = [] for p in g: polyline.append([int(p[0]*wGrid/wCell+wGrid/2), int(-p[1]*wGrid/wCell+wGrid/2)]) cvpoly = np.array(polyline) cv2.fillPoly(group_scenario, [cvpoly], 0.53) for g in intimate: polyline = [] for p in g: polyline.append([int(p[0]*wGrid/wCell+wGrid/2), int(-p[1]*wGrid/wCell+wGrid/2)]) cvpoly = np.array(polyline) cv2.fillPoly(group_scenario, [cvpoly], 0.) scenario = cv2.min(scenario, group_scenario) for p, o in objectInterGroup: inter_polyline = self.gaussian_spaces.getObjectInteraction([p], [o], True, False) group_scenario = np.full((wGrid, wGrid), 1., np.float32) for g in inter_polyline: polyline = [] print() for p in g: polyline.append([int(p[0]*wGrid/wCell+wGrid/2), int(-p[1]*wGrid/wCell+wGrid/2)]) cvpoly = np.array(polyline) cv2.fillPoly(group_scenario, [cvpoly], 0.62) scenario = cv2.min(scenario, group_scenario) scenario = cv2.resize(scenario, (100,100), cv2.INTER_CUBIC) next_time2 = time.time() print("time init", next_time1-initial_time, "time gaussians", next_time2-next_time1) #cv2.imshow('scenario', scenario) print("number of interactions", len(self.interaction_data)) return scenario def show_image(self): self.img_mutex.acquire() img = copy.deepcopy(self.img) self.img_mutex.release() if img is None: return self.ui.label.setPixmap(QPixmap(QImage(img.data, img.shape[1], img.shape[0], QtGui.QImage.Format_RGB888))) # # SUBSCRIPTION to gotobjects method from ObjectDetector interface # def ObjectDetector_gotobjects(self, lst): # #get robots position self.object_data = lst for obj in lst: if obj.id == -2: self.robot_coordinates = [obj.x, obj.y] # #pass # # SUBSCRIPTION to gotinteractions method from InteractionDetector interface # def InteractionDetector_gotinteractions(self, lst): self.interaction_data = lst # # SUBSCRIPTION to gotpeople method from PeopleDetector interface # def PeopleDetector_gotpeople(self, lst): self.people_data = lst # # SUBSCRIPTION to gotwalls method from WallDetector interface # def WallDetector_gotwalls(self, lst): self.wall_data = lst # # SUBSCRIPTION to newsequence method from ByteSequencePublisher interface # def ByteSequencePublisher_newsequence(self, bs): # print('GOT NEW BYTE SEQUENCE', bs) # bs = np.load(bs, allow_pickle=True) self.img_mutex.acquire() self.img = pickle.loads(bs) self.img_mutex.release() # # SUBSCRIPTION to goalupdated method from GoalPublisher interface # def GoalPublisher_goalupdated(self, goal): if self.on_a_mission: self.new_goal = True self.goal_data = goal self.goal_coordinates = [goal.x, goal.y] #pass def JoystickAdapter_sendData(self, data): if self.on_a_mission: self.started_mission = True adv = -3.5*data.axes[1].value # m/s rot = 4.0*data.axes[0].value # rad/s self.speed_command = [adv, 0, rot] self.omnirobot_proxy.setSpeedBase(adv, 0., rot) def info(self): dic = {"min_humans":self.configGui.nhumans_min.value(), "max_humans":self.configGui.nhumans_max.value(), "min_wandering_humans":self.configGui.nwandhumans_min.value(), "max_wandering_humans":self.configGui.nwandhumans_max.value() ,"min_plants":self.configGui.nplants_min.value(), "max_plants":self.configGui.nplants_max.value(), "min_tables":self.configGui.ntables_min.value(), "max_tables":self.configGui.ntables_max.value(), "min_relations":self.configGui.nrelations_min.value(), "max_relations":self.configGui.nrelations_max.value()} self.relations_dic= json.dumps(dic)
class SpecificWorker(GenericWorker): def __init__(self, proxy_map, fullscreen, startup_check=False): super(SpecificWorker, self).__init__(proxy_map) # os.system('bash ../joystick.sh &') os.system('bash ../simulator.sh &') self.timer.timeout.connect(self.compute) self.sngnn = SNGNN2D( os.path.join(os.path.dirname(__file__), '../../..')) self.img_mutex = threading.RLock() self.img = None self.proximity_threshold = 0.4 self.robot_coordinates = [0, 0] ### self.window = QWidget() self.button = QPushButton("enter") self.layout = QVBoxLayout() ### self.goal_coordinates = [100, 100] self.Period = 100 self.timer.start(self.Period) self.data = [] self.goal_data = [] self.wall_data = [] self.interaction_data = [] self.people_data = [] self.object_data = [] self.updates_list = [] self.data_json = [] self.speed_command = [0., 0., 0.] self.relations_dic = {} #Metrics self.init_metrics = True self.initial_time = 0. self.navigation_time = 0. self.last_robot_position = [] self.distance_traveled = 0. self.interaction_areas = [] self.configGui = Ui_confDialog() self.configWindow = QDialog() self.configGui.setupUi(self.configWindow) if fullscreen != "True": self.WINDOW_WIDTH = 1042 self.WINDOW_HEIGHT = 819 self.setFixedSize(self.WINDOW_WIDTH, self.WINDOW_HEIGHT) self.save_interval = 0.1 # self.last_saved_time = time.time() self.new_goal = False self.on_a_mission = True self.started_mission = False self.ui.configuration.clicked.connect(self.configuration_slot) self.ui.regenerate.clicked.connect(self.regenerate_slot) self.ui.quit.clicked.connect(self.quit_slot) self.ui.save_metrics.clicked.connect(self.save_metrics) self.configGui.nhumans_min.valueChanged.connect( self.min_nhumans_changed) self.configGui.nhumans_max.valueChanged.connect( self.max_nhumans_changed) self.configGui.nwandhumans_min.valueChanged.connect( self.min_nwandhumans_changed) self.configGui.nwandhumans_max.valueChanged.connect( self.max_nwandhumans_changed) self.configGui.ntables_min.valueChanged.connect( self.min_ntables_changed) self.configGui.ntables_max.valueChanged.connect( self.max_ntables_changed) self.configGui.nplants_min.valueChanged.connect( self.min_nplants_changed) self.configGui.nplants_max.valueChanged.connect( self.max_nplants_changed) self.configGui.nrelations_min.valueChanged.connect( self.min_nrelations_changed) self.configGui.nrelations_max.valueChanged.connect( self.max_nrelations_changed) if fullscreen == 'True': self.showFullScreen() @QtCore.Slot() def configuration_slot(self, active): if active: self.configWindow.show() else: self.configWindow.hide() @QtCore.Slot() def regenerate_slot(self): print('regenerate') self.omnirobot_proxy.setSpeedBase(0, 0, 0) self.i_frame = 0 self.updates_list = [] self.initial_positions = [] self.initial_positions_objects = [] self.initial_flag = 0 self.info() self.simulator_proxy.regenerate(scene=self.relations_dic) self.new_goal = False self.on_a_mission = True self.started_mission = False self.init_metrics = True @QtCore.Slot() def quit_slot(self): os.system('pwd') os.system('bash ../kill.sh &') @QtCore.Slot() def keyPressEvent(self, event): if event.type() == QtCore.QEvent.KeyPress: if event.key() == QtCore.Qt.Key_Q: self.quit_slot() elif event.key() == QtCore.Qt.Key_R: self.regenerate_slot() @QtCore.Slot() def min_nhumans_changed(self, val): if val > self.configGui.nhumans_max.value(): self.configGui.nhumans_max.setValue(val) @QtCore.Slot() def max_nhumans_changed(self, val): if val < self.configGui.nhumans_min.value(): self.configGui.nhumans_min.setValue(val) @QtCore.Slot() def min_nwandhumans_changed(self, val): if val > self.configGui.nwandhumans_max.value(): self.configGui.nwandhumans_max.setValue(val) @QtCore.Slot() def max_nwandhumans_changed(self, val): if val < self.configGui.nwandhumans_min.value(): self.configGui.nwandhumans_min.setValue(val) @QtCore.Slot() def min_ntables_changed(self, val): if val > self.configGui.ntables_max.value(): self.configGui.ntables_max.setValue(val) @QtCore.Slot() def max_ntables_changed(self, val): if val < self.configGui.ntables_min.value(): self.configGui.ntables_min.setValue(val) @QtCore.Slot() def min_nplants_changed(self, val): if val > self.configGui.nplants_max.value(): self.configGui.nplants_max.setValue(val) @QtCore.Slot() def max_nplants_changed(self, val): if val < self.configGui.nplants_min.value(): self.configGui.nplants_min.setValue(val) @QtCore.Slot() def min_nrelations_changed(self, val): if val > self.configGui.nrelations_max.value(): self.configGui.nrelations_max.setValue(val) @QtCore.Slot() def max_nrelations_changed(self, val): if val < self.configGui.nrelations_min.value(): self.configGui.nrelations_min.setValue(val) def __del__(self): os.system( 'kill -15 `ps ax |grep simulator | grep config | awk \'{print $1}\'`' ) # os.system('kill -15 `ps ax |grep joystick | grep config | awk \'{print $1}\'`') print('SpecificWorker destructor') self.quit_slot() def setParams(self, params): return True @QtCore.Slot() def compute(self): self.show_image() if not self.on_a_mission or not self.new_goal: return proximity = math.sqrt(self.goal_coordinates[0]**2 + self.goal_coordinates[1]**2) if proximity <= self.proximity_threshold: self.on_a_mission = False self.update_metrics() scenario = self.get_scene() estimation = self.sngnn.predict(scenario, 0) d = SNGNN2DData() d.gridSize = estimation.shape[0] d.areaSize = 800. d.g = estimation.reshape([1, d.gridSize * d.gridSize])[0] d.g = d.g.tolist() self.sngnn2d_proxy.gotgrid(d) def update_metrics(self): robot_position = [[r.x, r.y] for r in self.object_data if r.id == -2] robot_angle = [r.angle for r in self.object_data if r.id == -2] if self.init_metrics: self.initial_time = time.time() self.last_time = self.initial_time self.distance_to_goal = math.sqrt(self.goal_coordinates[0]**2 + self.goal_coordinates[1]**2) self.last_robot_position = robot_position[0] self.last_robot_angle = robot_angle[0] self.distance_traveled = 0. self.cum_heading_changes1 = 0. self.cum_heading_changes2 = 0. self.minimum_distance_to_human = 100. self.number_of_intrusions_intimate = 0 self.number_of_intrusions_personal = 0 self.number_of_intrusions_in_interactions = 0 self.number_of_steps = 0 self.area_computed = False self.room_area = 0 self.init_metrics = False if not self.area_computed: if self.wall_data: room = [] for wall in self.wall_data: room.append([wall.x1, wall.y1]) room.append([self.wall_data[-1].x2, self.wall_data[-1].y2]) self.room_area = Polygon(room).area self.area_computed = True self.number_of_steps += 1 current_time = time.time() self.navigation_time = current_time - self.initial_time self.distance_traveled += math.sqrt( sum([(a - b)**2 for a, b in zip(self.last_robot_position, robot_position[0]) ])) self.last_robot_position = robot_position[0] angle_diff = math.pi - math.fabs( math.fabs(robot_angle[0] - self.last_robot_angle) - math.pi) if angle_diff < 0: angle_diff = 2 * math.pi + angle_diff if angle_diff > 0: self.cum_heading_changes1 += angle_diff / (current_time - self.last_time) self.cum_heading_changes2 += angle_diff self.last_robot_angle = robot_angle[0] self.last_time = current_time min_dist_to_h = 100 for h in self.people_data: dist_to_h = math.sqrt(h.x**2 + h.y**2) if dist_to_h < min_dist_to_h: min_dist_to_h = dist_to_h if min_dist_to_h < self.minimum_distance_to_human: self.minimum_distance_to_human = min_dist_to_h if min_dist_to_h <= 0.45: self.number_of_intrusions_intimate += 1 if min_dist_to_h <= 1.2: self.number_of_intrusions_personal += 1 for inter_area in self.interaction_areas: if inter_area.contains(Point(0., 0.)): self.number_of_intrusions_in_interactions += 1 # print("room area", self.room_area, "navigation time", self.navigation_time, "distance", self.distance_traveled, "chc", self.cum_heading_changes, # "minimum distance", self.minimum_distance_to_human,"number of instrusions", self.number_of_intrusions, "number of steps", self.number_of_steps) @QtCore.Slot() def save_metrics(self): time_str = '{0:%Y-%m-%d - %H:%M:%S}'.format(datetime.datetime.now()) proximity = math.sqrt(self.goal_coordinates[0]**2 + self.goal_coordinates[1]**2) if proximity <= 0.5: self.goal_reached = True else: self.goal_reached = False metrics = { "date": time_str, "room_area": self.room_area, "number_of_humans": len(self.people_data), "number_of_objects": len(self.object_data) - 1, "number_of_interactions": len(self.interaction_data), "distance_to_goal": self.distance_to_goal, "navigation_time": self.navigation_time, "distance_traveled": self.distance_traveled, "chc1": self.cum_heading_changes1, "chc2": self.cum_heading_changes2, "minimum_distance_to_human": self.minimum_distance_to_human, "number_of_intrusions_intimate": self.number_of_intrusions_intimate, "number_of_intrusions_personal": self.number_of_intrusions_personal, "number_of_intrusions_in_interactions": self.number_of_intrusions_in_interactions, "number_of_steps": self.number_of_steps, "goal_reached": self.goal_reached } all_metrics = [] fname = 'metrics_sngnn2D.json' if os.path.isfile(fname): with open(fname, 'r') as fp: all_metrics = json.load(fp) all_metrics.append(metrics) with open(fname, 'w') as fp: json.dump(all_metrics, fp, indent=4) print("saving metrics") def get_scene(self): scenario = SNScenario() humansIds = [] newIds = dict() entity_position = dict() self.interaction_areas = [] room = [] for wall in self.wall_data: room.append([wall.x1, wall.y1]) if self.wall_data: room.append([self.wall_data[-1].x2, self.wall_data[-1].y2]) scenario.add_room(room) cur_id = 1 for h in self.people_data: humansIds.append(h.id) if True: #h.x<4. and h.x>-4. and h.y<4. and h.y>-4. : sngnn2d_human = graph2imageUp.Human(cur_id, 100. * h.x, -100. * h.y, 90. + rad_to_deg(h.angle)) scenario.add_human(sngnn2d_human) newIds[h.id] = cur_id entity_position[h.id] = (h.x, h.y) cur_id += 1 for o in self.object_data: if o.id != -2: if True: #o.x<4. and o.x>-4. and o.y<4. and o.y>-4. : sngnn2d_obj = graph2imageUp.Object( cur_id, 100. * o.x, -100. * o.y, 90. + rad_to_deg(o.angle)) scenario.add_object(sngnn2d_obj) newIds[o.id] = cur_id entity_position[o.id] = (o.x, o.y) cur_id += 1 print("interactions", len(self.interaction_data)) for inter in self.interaction_data: if inter.idSrc in newIds.keys() and inter.idDst in newIds.keys(): scenario.add_interaction( [newIds[inter.idSrc], newIds[inter.idDst]]) p1 = entity_position[inter.idSrc] p2 = entity_position[inter.idDst] V = (p2[0] - p1[0], p2[1] - p1[1]) lV = math.sqrt(V[0]**2 + V[1]**2) Vn = (-V[1] / lV, V[0] / lV) inter_area = [] inter_area.append((p1[0] - Vn[0] * 0.25, p1[1] - Vn[1] * 0.25)) inter_area.append((p1[0] + Vn[0] * 0.25, p1[1] + Vn[1] * 0.25)) inter_area.append((p2[0] + Vn[0] * 0.25, p2[1] + Vn[1] * 0.25)) inter_area.append((p2[0] - Vn[0] * 0.25, p2[1] - Vn[1] * 0.25)) self.interaction_areas.append(Polygon(inter_area)) if inter.idDst in humansIds: scenario.add_interaction( [newIds[inter.idDst], newIds[inter.idSrc]]) #print(scenario) #print(self.interaction_data) return scenario def show_image(self): self.img_mutex.acquire() img = copy.deepcopy(self.img) self.img_mutex.release() if img is None: return self.ui.label.setPixmap( QPixmap( QImage(img.data, img.shape[1], img.shape[0], QtGui.QImage.Format_RGB888))) # # SUBSCRIPTION to gotobjects method from ObjectDetector interface # def ObjectDetector_gotobjects(self, lst): # #get robots position self.object_data = lst for obj in lst: if obj.id == -2: self.robot_coordinates = [obj.x, obj.y] # #pass # # SUBSCRIPTION to gotinteractions method from InteractionDetector interface # def InteractionDetector_gotinteractions(self, lst): self.interaction_data = lst # # SUBSCRIPTION to gotpeople method from PeopleDetector interface # def PeopleDetector_gotpeople(self, lst): self.people_data = lst # # SUBSCRIPTION to gotwalls method from WallDetector interface # def WallDetector_gotwalls(self, lst): self.wall_data = lst # # SUBSCRIPTION to newsequence method from ByteSequencePublisher interface # def ByteSequencePublisher_newsequence(self, bs): # print('GOT NEW BYTE SEQUENCE', bs) # bs = np.load(bs, allow_pickle=True) self.img_mutex.acquire() self.img = pickle.loads(bs) self.img_mutex.release() # # SUBSCRIPTION to goalupdated method from GoalPublisher interface # def GoalPublisher_goalupdated(self, goal): if self.on_a_mission: self.new_goal = True self.goal_data = goal self.goal_coordinates = [goal.x, goal.y] #pass def JoystickAdapter_sendData(self, data): if self.on_a_mission: self.started_mission = True adv = -3.5 * data.axes[1].value # m/s rot = 4.0 * data.axes[0].value # rad/s self.speed_command = [adv, 0, rot] self.omnirobot_proxy.setSpeedBase(adv, 0., rot) def info(self): dic = { "min_humans": self.configGui.nhumans_min.value(), "max_humans": self.configGui.nhumans_max.value(), "min_wandering_humans": self.configGui.nwandhumans_min.value(), "max_wandering_humans": self.configGui.nwandhumans_max.value(), "min_plants": self.configGui.nplants_min.value(), "max_plants": self.configGui.nplants_max.value(), "min_tables": self.configGui.ntables_min.value(), "max_tables": self.configGui.ntables_max.value(), "min_relations": self.configGui.nrelations_min.value(), "max_relations": self.configGui.nrelations_max.value() } self.relations_dic = json.dumps(dic)
class DialogAccount: account_dialog_window: QDialog def account_dialog(self, new_account=False): self.account_dialog_window = QDialog(self) self.account_dialog_window.setMinimumSize(300, 125) if self.account_dialog_window.isVisible(): self.account_dialog_window.hide() # Main layout dialog_layout = QVBoxLayout() self.account_dialog_window.setLayout(dialog_layout) account_name_edit = QLineEdit() comment_edit = QLineEdit() comment_edit.setPlaceholderText(_("Comment")) steam_skin_select = QComboBox() steam_skin_select.addItems(self.switcher.steam_skins) if new_account: user = {} self.account_dialog_window.setWindowTitle(_("Add account")) self.submit_button = QPushButton(_("Add")) self.submit_button.setDisabled(True) else: login_name_selected = self.accounts_list.currentItem().data(5) user = self.switcher.settings["users"].get(login_name_selected, {}) self.account_dialog_window.setWindowTitle( _("Edit account {0}").format(login_name_selected)) self.submit_button = QPushButton(_("Edit")) account_name_edit.setText(login_name_selected) comment_edit.setText(user.get("comment")) steam_skin_select_index = steam_skin_select.findText( user.get("steam_skin", _("default"))) if steam_skin_select_index != -1: steam_skin_select.setCurrentIndex(steam_skin_select_index) else: steam_skin_select.setCurrentIndex(1) def submit_enabled(item): if 3 < len(item) < 32: self.submit_button.setEnabled(True) else: self.submit_button.setEnabled(False) account_name_edit.setPlaceholderText(_("Login name")) account_name_edit.textChanged.connect(submit_enabled) close_button = QPushButton(_("Close")) dialog_layout.addWidget(account_name_edit) dialog_layout.addWidget(comment_edit) dialog_layout.addWidget(steam_skin_select) def update_user(u: dict) -> dict: u["comment"] = comment_edit.text() u["steam_skin"] = steam_skin_select.currentText() return u self.submit_button.clicked.connect(lambda: self.save_account( account_name_edit.text(), update_user(user), login_name_selected if not new_account else None)) close_button.clicked.connect(self.account_dialog_window.close) buttons = QHBoxLayout() buttons.addWidget(self.submit_button) buttons.addWidget(close_button) dialog_layout.addLayout(buttons) self.account_dialog_window.show()