def action(self): r = int(getInput(self.pid, localize('WITCH_LOOK'))) sendOutput(self.pid, localize('WITCH_SELECTED').format(self.remainings[r])) p = int(getInput(self.pid, localize('WITCH_GIVE'))) self.players[p], self.remainings[r] = self.remainings[r], self.players[ p]
def action(self): p = int(getInput(self.pid, localize('ROBBER_SELECT'))) assert p != self.pid self.players[self.pid], self.players[p] = self.players[ p], self.players[self.pid] sendOutput(self.pid, localize('ROBBER_KEEP').format(self.players[self.pid]))
def action(self): wolves = [] for i, player in enumerate(self.players): if isinstance(player, Werewolf): wolves.append(i) if wolves: sendOutput(self.pid, localize('MINION_PARTNER').format(wolves)) else: sendOutput(self.pid, localize('MINION_NO_PARTNER'))
def action(self): partner = None for i, player in enumerate(self.players): if isinstance(player, Mason) and player is not self: partner = i if partner is not None: sendOutput(self.pid, localize('MASON_PARTNER').format(partner)) else: sendOutput(self.pid, localize('MASON_NO_PARTNER'))
def action(self): look = getInput(self.pid, localize('SEER_LOOK')) if len(look.split()) == 1: # check player sendOutput( self.pid, localize('SEER_CHECK_PLAYER').format(look, self.players[int(look)])) else: # check remainings r1, r2 = look.split() sendOutput( self.pid, localize('SEER_CHECK_REMAININGS').format( r1, self.remainings[int(r1)], r2, self.remainings[int(r2)]))
def perform_localization(self): # Get template self.get_template() rospy.loginfo("Performing localization...") rospy.sleep(5) max_score, max_score_loc, rot_angle = localization.localize( self.map_path + ".pgm", self.template_path + ".pgm") rospy.loginfo("max_score: " + str(max_score)) rospy.loginfo("location: " + str(max_score_loc)) rospy.loginfo("angle: " + str(rot_angle)) angle_rad = rot_angle * math.pi / 180 rotation = pyquaternion.Quaternion(axis=[0.0, 0.0, 1.0], radians=angle_rad) quat = Quaternion(rotation[1], rotation[2], rotation[3], rotation[0]) # Launch self navigation node os.system( "gnome-terminal -x roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=/home/darebalogun/Desktop/Turtlebot/turtlebot/frontend-webapp/maps/" + self.name + ".yaml") rospy.loginfo("Estimating Initial Pose...") rospy.sleep(3) # Publish location from localization back to rviz /initialpose pose_publisher = rospy.Publisher("/initialpose", PoseWithCovarianceStamped, queue_size=10) poseWCS = PoseWithCovarianceStamped() poseWCS.header.frame_id = "map" poseWCS.header.stamp = rospy.Time.now() poseWCS.pose.pose.position.x = ( max_score_loc[0] - self.map_size / 2) * self.map_resolution poseWCS.pose.pose.position.y = -1 * \ (max_score_loc[1] - self.map_size/2) * self.map_resolution poseWCS.pose.pose.position.z = 0 poseWCS.pose.pose.orientation = quat poseWCS.pose.covariance = [ 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942 ] rate = rospy.Rate(1) pose_publisher.publish(poseWCS) rate.sleep() pose_publisher.publish(poseWCS) rate.sleep() pose_publisher.publish(poseWCS) # Publish markers on navigation node map self.add_marker_array()
def test_five(self): colors = [['G', 'G', 'G'], ['G', 'R', 'R'], ['G', 'G', 'G']] measurements = ['R', 'R'] motions = [[0, 0], [0, 1]] sensor_right = 1.0 p_move = 1.0 p = localization.localize(colors, measurements, motions, sensor_right, p_move) correct_answer = ([[0.0, 0.0, 0.0], [0.0, 0.0, 1.0], [0.0, 0.0, 0.0]]) self.assertMatricesAlmostEqual(p, correct_answer)
def action(self): wolves = [] for i, player in enumerate(self.players): if isinstance(player, Werewolf) and self.pid != player.pid: wolves.append(i) if wolves: sendOutput(self.pid, localize('WOLF_PARTNER').format(wolves)) else: self.action_only_wolf() if isinstance(self, SeniorWerewolf): self.action_senoir()
def test_six(self): colors = [['G', 'G', 'G'], ['G', 'R', 'R'], ['G', 'G', 'G']] measurements = ['R', 'R'] motions = [[0, 0], [0, 1]] sensor_right = 0.8 p_move = 0.5 p = localization.localize(colors, measurements, motions, sensor_right, p_move) correct_answer = ([[0.0289855072, 0.0289855072, 0.0289855072], [0.0724637681, 0.2898550724, 0.4637681159], [0.0289855072, 0.0289855072, 0.0289855072]]) self.assertMatricesAlmostEqual(p, correct_answer)
def on_draw(delta_time): global vehicles global sensors global active_sensor_ids global DX global DY global DD global counter global ex global ey global mouse_x global mouse_y global problem_string global problems """ Use this function to draw everything to the screen. """ # Start the render. This must happen before any drawing # commands. We do NOT need a stop render command. arcade.start_render() arcade.draw_texture_rectangle(SCREEN_WIDTH // 2, SCREEN_HEIGHT // 2, SCREEN_WIDTH, SCREEN_HEIGHT, BACKGROUND) button_sprites.draw() arcade.draw_circle_filled(mouse_x, mouse_y, 10, color=arcade.color.WHITE) # # plot all sensor (include non-active and active sensor) # # all edges edges = create_edge(sensors, sensors, 200, c=NON_ACTIVE_SENSOR_EDGE_COLOR) render(edges) # all sensors render(sensors) # # initial state # global STATE_INITIAL if STATE_INITIAL: sensors = create_random_nodes(15, c=NON_ACTIVE_SENSOR_COLOR) vehicles = create_random_nodes(10, c=VEHICLE_COLOR) arcade.draw_text("Applay sensor placement algorithm...", SCREEN_WIDTH // 2, SCREEN_HEIGHT // 2, arcade.color.BLACK, 12) is_activate = activate_sensors(sensors, RADIUS) # set sensor colors for i in range(len(is_activate)): sensors[i].c = ACTIVE_SENSOR_COLOR if is_activate[ i] else NON_ACTIVE_SENSOR_COLOR if is_activate[i]: active_sensor_ids.append(i) render(sensors) STATE_INITIAL = False return # # plot active sensors # active_sensors = [ sensors[i] for i in range(len(sensors)) if i in active_sensor_ids ] render(active_sensors) # plot active sensor edge active_edges = create_edge(active_sensors, active_sensors, RADIUS) render(active_edges) if STATE_RUNNING: # # plot vehicle # move(vehicles, delta_time) # plot vehicle moving edges = create_edge(active_sensors, vehicles, RADIUS, c=VEHICLE_EDGE_COLOR) render(edges) # perform estimation arcade.draw_text(problem_string, vehicles[0].x, vehicles[0].y, arcade.color.WHITE, 8) if counter >= 20: problems = generate_problem(vehicles[0], active_sensors, RADIUS) problem_string = "\n".join( ["dis %d loc %d loc %d" % p for p in problems][:len(edges)]) ex, ey = localize(problems, DX, DY) counter = 0 arcade.draw_rectangle_outline(ex + DX / 2, ey + DY / 2, DX * 4, DY * 8, color=ESTIMATION_COLOR) # plot render(vehicles) counter += 1
distanceWhenCorrect = 0 for i in range(0, 10000): total += 1 xya = generate_XYA() print( "Real: ", "X={} ".format(xya[0]) + "Y={} ".format(xya[1]) + "A={} ".format(xya[2])) distances = localization.distances_from_XYA(xya[0] * 1.0, xya[1] * 1.0, xya[2]) for j in range(0, len(distances)): distances[j] = abs(distances[j] + random.uniform(-NOISE_LIMIT, NOISE_LIMIT)) estimate = localization.localize(distances, xya[2]) print("Estimated: X=%f Y=%f" % (estimate[0], estimate[1])) if estimate != None: d = points_distance(estimate, (xya[0], xya[1])) if d > TOLLERANCE: errors += 1 print("ERROR: wrong point") else: distanceWhenCorrect += d else: print("ERROR: None") noneErrors += 1 print("\nNumber of tests:", total) print("Error precentage:", errors / total * 100, "%")
source = [] s_array = [] sourceX = [] sourceZ = [] locationX = [] locationZ = [] sensorPosition =[[.0015, .040],[.048, .0105],[.0025,-.0165],[-.0385,.0065]] mics = np.matrix(sensorPosition).T temperature = 23 for files in range(0,len(sound_files)): source_optitrak, mics_optitrak, times, mic1, mic2, mic3, mic4 = lc.get_sound_data(sound_files[files], optitrak_files[files]) location_alg = lc.localize(times, mic1, mic2, mic3, mic4, temperature, mics) s_array = np.append(s_array,mics_optitrak) sourceX = np.append(sourceX, source_optitrak[0]) sourceZ = np.append(sourceZ, source_optitrak[1]) locationX = np.append(locationX, location_alg[0]) locationZ = np.append(locationZ, location_alg[1]) mic1Position = [s_array[0] + mics[0,0], s_array[1] + mics[1,0]] mic2Position = [s_array[0] + mics[0,1], s_array[1] + mics[1,1]] mic3Position = [s_array[0] + mics[0,2], s_array[1] + mics[1,2]] mic4Position = [s_array[0] + mics[0,3], s_array[1] + mics[1,3]] micPosition = np.array([mic1Position, mic2Position, mic3Position, mic4Position]) plt.plot(micPosition[:,0], micPosition[:,1],'rx')
def action(self): p1, p2 = getInput(self.pid, localize('TROUBLEMAKER_SELECT')).split() p1, p2 = int(p1), int(p2) assert p1 != self.pid and p2 != self.pid self.players[p1], self.players[p2] = self.players[p2], self.players[p1]
def action(self): r = int(getInput(self.pid, localize('DRUNK_SELECT'))) self.players[self.pid], self.remainings[r] = self.remainings[ r], self.players[self.pid]
def action_only_wolf(self): r = int(getInput(self.pid, localize('ONLY_WOLF_LOOK'))) sendOutput(self.pid, localize('ONLY_WOLF_SELECTED').format(self.remainings[r]))
def action_senoir(self): p = int(getInput(self.pid, localize('SENIOR_WOLF_LOOK'))) sendOutput(self.pid, localize('SENIOR_WOLF_SELECTED').format(self.players[p]))
def kill_another(self, votes): toKill = int(getInput(self.pid, localize('HUNTER_KILL').format(votes))) assert toKill < len(self.players) return toKill
def action(self): sendOutput(self.pid, localize('INSOMNIAC_LOOK').format(self.players[self.pid]))