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