def set_receiver(self, _player_list):
receiver_op_dist_list = []
for receiver in _player_list :
temp_receiver_op_dist_list = []
# the sender is not a receiver candidate
if receiver == self.sender :
receiver_op_dist_list.append(999)
continue
# the distance between the robot and opponents
for op in range(1, 5) : #[1,2,3,4]
op_distance = helper.dist(self.cur_posture[receiver][X], self.cur_posture_op[op][X], self.cur_posture[receiver][Y], self.cur_posture_op[op][Y])
temp_receiver_op_dist_list.append(op_distance)
# save the shortest distance between this robot and one of opponents
receiver_op_dist_list.append(min(temp_receiver_op_dist_list))
receiver_ball_list = []
for r in receiver_op_dist_list :
# if the minimum distance between player and opponent's player is less than 0.5, this robot cannot be receiver
if r < 0.5 or r == 999:
receiver_ball_list.append(999)
continue
id = receiver_op_dist_list.index(r) + 1
receiver_ball_distance = helper.dist(self.cur_ball[X], self.cur_posture[id][X], self.cur_ball[Y], self.cur_posture[id][Y])
receiver_ball_list.append(receiver_ball_distance)
if min(receiver_ball_list) < 999 :
min_id = receiver_ball_list.index(min(receiver_ball_list)) + 1
return min_id
return None
def receive_ball(self):
# if receiver does not exist, do nothing
if self.receiver == None:
return
goal_dist = helper.dist(
4.0, self.cur_posture[self.receiver][X], 0, self.cur_posture[self.receiver][Y])
# if sender is in shoot chance, receiver does nothing(reset)
if self.shoot_chance(self.sender):
self.actions(self.receiver, None)
return
# if receiver is in shoot chance, receiver try to shoot
if self.shoot_chance(self.receiver):
if goal_dist > 0.3 * self.field[X] / 2:
self.actions(self.receiver, 'dribble', refine=True)
return
else:
self.actions(self.receiver, 'kick')
return
# if sender exists
if not self.sender == None:
s2risFace, _ = self.is_facing_target(
self.sender, self.cur_posture[self.receiver][X], self.cur_posture[self.receiver][Y], 4)
r2sisFace, _ = self.is_facing_target(
self.receiver, self.cur_posture[self.sender][X], self.cur_posture[self.sender][Y], 4)
# if sender and receiver directs each other
if s2risFace and r2sisFace:
if self.cur_posture[self.receiver][TH] > 0 or self.cur_posture[self.receiver][TH] < -3:
self.actions(self.receiver, 'follow', [
self.prev_posture[self.receiver][X], self.prev_posture[self.receiver][Y] - 0.5 * self.field[Y]])
return
self.actions(self.receiver, 'follow', [
self.prev_posture[self.receiver][X], self.prev_posture[self.receiver][Y] + 0.5 * self.field[Y]])
return
r_point = self.cur_ball
# if sender exists
if not self.sender == None:
r_point = self.receive_position()
receiver_ball_dist = helper.dist(
self.cur_ball[X], self.cur_posture[self.receiver][X], self.cur_ball[Y], self.cur_posture[self.receiver][Y])
# if ball is close to receiver
if receiver_ball_dist > 0.3 * self.field[X] / 2:
self.actions(self.receiver, 'follow', [
r_point[X], r_point[Y]], refine=True)
return
r2bisFace, _ = self.is_facing_target(
self.receiver, self.cur_ball[X], self.cur_ball[Y], 4)
if not r2bisFace:
self.actions(self.receiver, 'follow', refine=True)
return
# if receiver is moving to our goal area
if self.cur_posture[self.receiver][X] < - 0.8 * self.field[X] / 2:
if self.cur_posture[self.receiver][X] - self.prev_posture[self.receiver][X] < 0:
self.actions(self.receiver, 'backward')
self.actions(self.receiver, 'dribble')
return
def receive_position(self):
step = 5
ball_receiver_dist = helper.dist(self.cur_ball[X],
self.cur_posture[self.receiver][X],
self.cur_ball[Y],
self.cur_posture[self.receiver][Y])
prev_ball_receiver_dist = helper.dist(
self.prev_ball[X], self.prev_posture[self.receiver][X],
self.prev_ball[Y], self.prev_posture[self.receiver][Y])
diff_dist = prev_ball_receiver_dist - ball_receiver_dist
if diff_dist > 0:
step = ball_receiver_dist # diff_dist
step = min(step, 15)
predict_pass_point = self.predict_ball_location(step)
ball_goal_dist = helper.dist(self.cur_ball[X], self.field[X] / 2,
self.cur_ball[Y], 0)
prev_ball_goal_dist = helper.dist(self.prev_ball[X], self.field[X] / 2,
self.prev_ball[Y], 0)
if ball_goal_dist > prev_ball_goal_dist:
predict_pass_point[X] = predict_pass_point[X] - 0.15
return predict_pass_point
def set_pos_parameters(self, id, target_pts, params, mult=1.2):
prev_dist = helper.dist(
self.prev_posture[id][X], target_pts[X], self.prev_posture[id][Y], target_pts[Y])
cur_dist = helper.dist(
self.cur_posture[id][X], target_pts[X], self.cur_posture[id][Y], target_pts[Y])
if cur_dist > prev_dist - 0.02:
params = [params[0] * mult, params[1]
* mult, params[2] * mult, params[3]]
return params
def send_ball(self):
if self.sender == None:
return
goal_dist = helper.dist(4.0, self.cur_posture[self.sender][X], 0,
self.cur_posture[self.sender][Y])
# if the sender has a shoot chance, it tries to shoot
if self.shoot_chance(self.sender):
if goal_dist > 0.3 * self.field[X] / 2:
self.actions(self.sender, 'dribble', refine=True)
return
else:
self.actions(self.sender, 'kick')
return
# if the receiver exists, get the distance between the sender and the receiver
sender_receiver_dist = None
if not self.receiver == None:
sender_receiver_dist = helper.dist(
self.cur_posture[self.sender][X],
self.cur_posture[self.receiver][X],
self.cur_posture[self.sender][Y],
self.cur_posture[self.receiver][Y])
# if the sender is close to the receiver, the sender kicks the ball
if not sender_receiver_dist == None:
if sender_receiver_dist < 0.3 and not self.cur_posture[
self.receiver][TOUCH]:
self.actions(self.sender, 'kick')
return
ift, theta_diff = self.is_facing_target(self.sender, self.cur_ball[X],
self.cur_ball[Y])
if not ift:
# after the sender kicks, it stops
if theta_diff > math.pi * 3 / 4:
self.actions(self.sender, None)
return
else:
self.actions(self.sender, 'follow', refine=True)
return
# if the ball is in front of the sender and sender is moving backward
if self.cur_posture[self.sender][X] < -0.8 * self.field[X] / 2:
if self.cur_posture[self.sender][X] - self.prev_posture[
self.sender][X] < 0:
self.actions(self.sender, 'backward')
self.actions(self.sender, 'dribble', refine=True)
return
def set_sender_condition(self):
for i in range(1, 5):
# if this robot is near the ball, it will be a sender candidate
dist = helper.dist(self.cur_posture[i][X], self.cur_ball[X],
self.cur_posture[i][Y], self.cur_ball[Y])
if dist < 0.5 and self.cur_posture[i][ACTIVE]: return True
return False
def denoise(self):
"""
return a new vehicle with outlier removed
"""
# if len(self.vlist) < 3:
# return self
# new_vlist = [self.vlist[0]]
# for i in xrange(1, len(self.vlist) - 1):
# if dist(self.get_location(i), self.get_location(i-1)) > outlier_dist_thres \
# and dist(self.get_location(i), self.get_location(i+1)) > outlier_dist_thres:
# print ("remove outlier in %s" % self.get_no()), self.get_location(i-1), \
# self.get_location(i), self.get_location(i+1)
# continue
# new_vlist.append(self.vlist[i])
# new_vlist.append(self.vlist[-1])
# Assume self.vlist[0] is not outlier and time is continuous.
if not self.vlist:
return self
new_vlist = [self.vlist[0]]
for i in xrange(1, len(self.vlist)):
if dist(self.get_location(i),
(new_vlist[-1][2], new_vlist[-1][1])) < outlier_dist_thres:
new_vlist.append(self.vlist[i])
return Vehicle(new_vlist, self.get_no())
def length(self):
"""
total distance between adjacent sites,
use lines to link adjacent sites.
"""
if self.total_len == None:
self.total_len = sum(dist(self.get_location(i), self.get_location(i - 1))
for i in xrange(1, len(self)))
return self.total_len
def length(self):
"""
total distance between adjacent sites,
use lines to link adjacent sites.
"""
if self.total_len == None:
self.total_len = sum(
dist(self.get_location(i), self.get_location(i - 1))
for i in xrange(1, len(self)))
return self.total_len
def find_closest_robot(self):
# find the closest defender
min_idx = 0
min_dist = 9999.99
def_dist = 9999.99
all_dist = []
for i in [1, 2]:
measured_dist = helper.dist(self.cur_ball[X],
self.cur_posture[i][X],
self.cur_ball[Y],
self.cur_posture[i][Y])
all_dist.append(measured_dist)
if (measured_dist < min_dist):
min_dist = measured_dist
def_dist = min_dist
min_idx = i
self.def_idx = min_idx
# find the closest forward
min_idx = 0
min_dist = 9999.99
atk_dist = 9999.99
for i in [3, 4]:
measured_dist = helper.dist(self.cur_ball[X],
self.cur_posture[i][X],
self.cur_ball[Y],
self.cur_posture[i][Y])
all_dist.append(measured_dist)
if (measured_dist < min_dist):
min_dist = measured_dist
atk_dist = min_dist
min_idx = i
self.atk_idx = min_idx
# record the robot closer to the ball between the two too
self.closest_order = np.argsort(all_dist) + 1
def set_sender(self, _player_list):
distance_list = []
for sender in _player_list :
predict_ball = self.predict_ball_location(3)
ball_distance = helper.dist(predict_ball[X], self.cur_posture[sender][X], predict_ball[Y], self.cur_posture[sender][Y])
distance_list.append(ball_distance)
# if the distance between ball and sender is less than 1, choose the closest robot as the sender
if min(distance_list) < 1.0 :
return distance_list.index(min(distance_list)) + 1
# otherwise, there is no sender
return None
def subset_match_with_dist(vehicle, start, end, route, grids):
"""
return whether vehicle[start..end] match route.
unlike subset_match, subset_match_with_dist
considers match_dist, is slower but more accurate.
"""
matched_sites_no = set()
route_no = route.get_no()
for i in xrange(start, end + 1):
loc = vehicle.get_location(i)
id_x, id_y = grid_index(loc)
if grids[id_x][id_y].has_key(route_no):
sites_no_set = set(
site_no for site_no in grids[id_x][id_y][route_no]
if dist(route.get_location(site_no), loc) < match_dist)
matched_sites_no |= sites_no_set
return True if len(matched_sites_no) / len(
route) > match_thres_one else False
def split_vehicle_by_route(vehicle, route):
"""
split vehicle according to route
"""
split_ids = []
last_dist = float('inf')
last_id = -split_pts_int - 100
for i in xrange(len(vehicle)):
# choose best split points for current route
# from continuous split points
cur_dist = dist(vehicle.get_location(i), route.get_dest_loc())
if cur_dist < match_dist:
if i - last_id >= split_pts_int:
split_ids.append([i, route.get_no()])
last_dist = cur_dist
elif cur_dist <= last_dist:
split_ids[-1][0] = i
last_dist = cur_dist
last_id = i
return split_ids
def vehicle_site_time(vehicle, start, end, route, grids):
"""
include start, end
return [(site_no, time), (site_no, time)]
"""
times = [None for _ in xrange(route.site_count())]
dists = [100000 for _ in xrange(route.site_count())]
for i in xrange(start, end + 1):
loc = vehicle.get_location(i)
idx, idy = grid_index(loc)
sites = grids[idx][idy].get(route.get_no(), set())
for site in sites:
d = dist(route.site_location(site), vehicle.get_location(i))
if d <= dists[site]:
dists[site] = d
times[site] = vehicle.get_GpsTime(i)
ret_times = []
for site, time in enumerate(times):
if time != None:
ret_times.append((site, time))
return ret_times
def vehicle_site_time(vehicle, start, end, route, grids):
"""
include start, end
return [(site_no, time), (site_no, time)]
"""
times = [None for _ in xrange(route.site_count())]
dists = [100000 for _ in xrange(route.site_count())]
for i in xrange(start, end + 1):
loc = vehicle.get_location(i)
idx, idy = grid_index(loc)
sites = grids[idx][idy].get(route.get_no(), set())
for site in sites:
d = dist(route.site_location(site), vehicle.get_location(i))
if d <= dists[site]:
dists[site] = d
times[site] = vehicle.get_GpsTime(i)
ret_times = []
for site, time in enumerate(times):
if time != None:
ret_times.append((site, time))
return ret_times
def denoise(self):
"""
return a new vehicle with outlier removed
"""
# if len(self.vlist) < 3:
# return self
# new_vlist = [self.vlist[0]]
# for i in xrange(1, len(self.vlist) - 1):
# if dist(self.get_location(i), self.get_location(i-1)) > outlier_dist_thres \
# and dist(self.get_location(i), self.get_location(i+1)) > outlier_dist_thres:
# print ("remove outlier in %s" % self.get_no()), self.get_location(i-1), \
# self.get_location(i), self.get_location(i+1)
# continue
# new_vlist.append(self.vlist[i])
# new_vlist.append(self.vlist[-1])
# Assume self.vlist[0] is not outlier and time is continuous.
if not self.vlist:
return self
new_vlist = [self.vlist[0]]
for i in xrange(1, len(self.vlist)):
if dist(self.get_location(i), (new_vlist[-1][2], new_vlist[-1][1])) < outlier_dist_thres:
new_vlist.append(self.vlist[i])
return Vehicle(new_vlist, self.get_no())
def dock_collide(self, object): """Is the given object in range of self's dock?""" return helper.dist(self.pos,object.pos) < self.dock_radius + object.radius
def match_card_from_vehicles(card, mapper, connector, routes, grids, vehicles,
cvmapper):
"""
find the best match for the card
"""
route_nos = mapper.get_lineid_from_statid(card.get_no()[1])
cand_vehicles_nos = set()
for route_no in route_nos:
cand_vehicles_nos |= connector.get_cands(route_no)
cand_vehicles = [
vehicles.get_vehicle(vehicle_no) for vehicle_no in cand_vehicles_nos
]
card = card.get_simplified_card(between_card_time)
#plt.close('all')
match_results = []
for vehicle in cand_vehicles:
match_num_1 = 0
best_offset = 0
for offset in offsets:
match_num = 0
time_sequences = map(lambda x: x + offset,
card.get_time_sequence())
locations = vehicle.get_locations_at_timestamps(time_sequences)
# x, y = zip(*filter(None, locations))
# plt.figure()
# vehicle.plot(0, len(vehicle))
# plt.plot(x, y, 'gs')
# for route_no in list(route_nos)[:1]:
# route = routes.get_route(route_no)
# route.plot(0, len(route), 'ro')
#
# plt.show()
last_location = (0, 0)
for location in locations:
if location == None:
continue
gid = grid_index(location)
for route_no in route_nos:
tmp_set = grids[gid[0]][gid[1]].get(route_no, set())
if not tmp_set or dist(location,
last_location) <= dist_card_time:
continue
route = routes.get_route(route_no)
min_dist = min(
dist(location, route.get_location(i)) for i in tmp_set)
if min_dist < dist_stop:
match_num += 1
break
last_location = location
if match_num > match_num_1:
match_num_1 = match_num
best_offset = offset
print vehicle.get_no(), match_num_1, '/', len(card), best_offset
match_results.append((match_num_1, vehicle, best_offset))
match_results.sort(key=lambda x: x[0], reverse=True)
if not match_results:
cvmapper.add_map(card.get_no(), None)
return ("No Match", )
cvmapper.add_map(card.get_no(),
(match_results[0][1].get_no(), match_results[0][2]))
if (float(match_results[0][0]) / len(card)) >= min_ratio:
if (len(match_results) == 1 or match_results[0][0] >
(1 + thres_ratio) * match_results[1][0]):
return "Accurate Match", match_results[0][0], '/', len(
card), match_results[0][1].get_no(), match_results[0][2]
else:
return "Inaccurate Match", match_results[0][0], '/', len(
card), match_results[0][1].get_no(), match_results[0][2]
else:
return "Low Match", match_results[0][0], '/', len(
card), match_results[0][1].get_no(), match_results[0][2]
def in_jump_range(self, pilot): return helper.dist(self.pos,pilot.vessel.pos) <= 10 + pilot.vessel.radius
'W_conv2': W_conv2,
'b_conv2': b_conv2
}
init_new_vars_op = tf.initialize_variables(
[W_conv1, b_conv1, W_conv2, b_conv2])
x1 = tf.placeholder(tf.float32, [None, 784], name='x1')
z1 = tf.placeholder(tf.float32, [None, z_dim], name='z1')
gz1 = helper.Generator(z1)
x1_real_conv = helper.random_discriminator(x1, rr, D1)
gz1_real_conv = helper.random_discriminator(gz1, rr, D1)
loss_1 = helper.dist(x1_real_conv, gz1_real_conv, b_size)
loss_2 = helper.dist(gz1_real_conv, x1_real_conv, b_size)
g_loss = tf.cond(tf.less(loss_1, loss_2), lambda: loss_2, lambda: loss_1)
gen_params = tf.get_collection(tf.GraphKeys.TRAINABLE_VARIABLES, "Generator")
step = tf.Variable(0, trainable=False)
rate = tf.train.exponential_decay(step_size, step, 3000, 0.96)
optimizer = tf.train.AdamOptimizer(learning_rate=rate,
beta1=0.5,
beta2=0.9,
name='Adam1')
grads_and_vars = optimizer.compute_gradients(g_loss, var_list=gen_params)
vars = [x[1] for x in grads_and_vars]
gradients = [x[0] for x in grads_and_vars]
player_input = pygame.key.get_pressed()
if player_input[pygame.K_UP] or player_input[pygame.K_w]:
player_y -= player_speed
if player_input[pygame.K_RIGHT] or player_input[pygame.K_d]:
player_x += player_speed
if player_input[pygame.K_DOWN] or player_input[pygame.K_s]:
player_y += player_speed
if player_input[pygame.K_LEFT] or player_input[pygame.K_a]:
player_x -= player_speed
## game logic ##
# place aim reticule
mouse_x, mouse_y = pygame.mouse.get_pos()
aim_x = mouse_x - player_x
aim_y = mouse_y - player_y
mouse_dist = dist(0, 0, aim_x, aim_y)
aim_x = int(aim_x / mouse_dist * 30)
aim_y = int(aim_y / mouse_dist * 30)
# add a new bullet to the list
if fire == True:
bullets.append([player_x, player_y, aim_x // 2, aim_y // 2])
fire = False
# move the bullets, and delete them if they leave the screen
alive_bullets = []
for bullet in bullets:
bullet[0] += bullet[2]
bullet[1] += bullet[3]
if -50 <= bullet[0] <= WIDE + 50 and -50 <= bullet[1] <= HIGH + 50:
alive_bullets.append(bullet)
def in_position(self, id, xTarget, yTarget, tol):
return (helper.dist(self.xDesired, self.cur_posture[id][X],
self.yDesired, self.cur_posture[id][Y]) < tol)
def move_to_circles(self, id, x, y, radius=0.5, speed=1.0, goal_speed=0.0, goal_angle=None, max_velocity = False, kick=True, debug = False):
if max_velocity:
speed = self.max_linear_velocity[id]
max_turn_d_th = 480*DEGTORAD
min_turn_d_th = 90*DEGTORAD
d_t = self.received_frame.time - self.previous_frame.time
if d_t == 0.0:
d_t = 0.2
my_x = self.cur_posture[id][X]
my_y = self.cur_posture[id][Y]
last_x = self.prev_posture[id][X]
last_y = self.prev_posture[id][Y]
dist = helper.dist(my_x, x, my_y, y)
last_dist = helper.dist(last_x, x, last_y, y)
d_dist = (dist - last_dist)/d_t
angle_to_target = self.direction_angle(id, x, y)
my_angle = self.cur_posture[id][TH]
last_angle = self.prev_posture[id][TH]
diff_theta = helper.clamp(my_angle - angle_to_target) # right-facing angle to turn to target
d_theta = (helper.clamp(my_angle - last_angle)) / d_t
direction = 1.0
if abs(diff_theta) > math.pi/2: # drive backwards if easier
direction = -1.0
diff_theta = helper.clamp(diff_theta-math.pi)
if goal_angle != None:
assert type(goal_angle) == type(1.0) or type(goal_angle) == type(1)
# move to the target position at a given orientation (Two turn-circles)
dir_x = math.cos(goal_angle)
dir_y = math.sin(goal_angle)
bot_dir_x = math.cos(my_angle)
bot_dir_y = math.sin(my_angle)
max_corridor_dist = self.axle_length[id]/2 + math.sin(3*DEGTORAD)*dist # Cone shaped approach corridor
tar_to_bot_x = my_x - x
tar_to_bot_y = my_y - y
intersect_dist_tar = (bot_dir_x*tar_to_bot_x + bot_dir_y*tar_to_bot_y) / (bot_dir_x*dir_x + bot_dir_y*dir_y)
intersect_dist_bot = -(dir_x*tar_to_bot_x + dir_y*tar_to_bot_y) / (bot_dir_x*dir_x + bot_dir_y*dir_y)
rel_angle = goal_angle - my_angle
turn_radius = intersect_dist_tar * math.tan(rel_angle/2.0)
if debug:
print(f"int_d_ball: {intersect_dist_tar}")
print(f"int_d_bot : {intersect_dist_bot}")
print(f"rel_angle : {rel_angle}")
print(f"turn_rad : {turn_radius}")
corridor_dist = dir_x*tar_to_bot_y - dir_y*tar_to_bot_x
correct_direction = (dir_x*tar_to_bot_x + dir_y*tar_to_bot_y) >= 0.0
if abs(corridor_dist) > max_corridor_dist or not correct_direction:
# If outside the corridor
x_int_goal = x + dir_x*radius
y_int_goal = y + dir_y*radius
int_dist = helper.dist(my_x, x_int_goal, my_y, y_int_goal)
if int_dist > dist:
x_temp_goal = tar_to_bot_y * corridor_dist
y_temp_goal = -tar_to_bot_x * corridor_dist
l_temp = math.sqrt(x_temp_goal ** 2 + y_temp_goal ** 2)
x_temp_goal /= l_temp
y_temp_goal /= l_temp
x_temp_goal = x + x_temp_goal
y_temp_goal = y + y_temp_goal
self.move_to_circles(id, x_temp_goal, y_temp_goal, radius, speed, goal_speed=speed*0.99, max_velocity=max_velocity, debug=debug)
return
else:
self.move_to_circles(id, x_int_goal, y_int_goal, radius, speed, goal_speed=speed*0.99, max_velocity=max_velocity, debug=debug)
return
if ((abs(diff_theta)*RADTODEG < 2 and dist < radius) or (self.player_state[id] == 'kick')) and d_dist < 0.0 and goal_speed > speed:
# If inside the corridor and close to the target
# Kick
#print("kicking")
self.player_state[id] = 'kick'
targ_speed = dist/radius * speed + (1-dist/radius) * goal_speed
self.set_wheel_velocity(id, targ_speed*direction, targ_speed*direction)
return
else:
self.player_state[id] = None
# just move to the target position. Orientation is not important (Only one turn-circle)
f_dist = Dist(dist)
# Speed
speed_far = speed
speed_small = (speed + goal_speed)/2
speed_zero = goal_speed
# Defuzzy Speed
speed = f_dist['zero'] * speed_zero + f_dist['small'] * speed_small + (f_dist['med'] + f_dist['big'] + f_dist['large']) * speed_far
# Fuzzy diff_theta
# The fuzzy domain Ang is in ° for convenience
f_d_theta = Ang(diff_theta*RADTODEG)
# Use p-d control for the angular velocity
if id == 0:
p = 1
d = 0.12
elif id == 1 or id == 2:
p = 1
d = 0.12
else:
p = 1
d = 0.12
p_d = p*diff_theta + d*d_theta
# In the turn-circle (meaning while diff_theta is not small)
max_circ_radius = dist/4.0*math.cos(diff_theta)
circ_rad = min(max_circ_radius, radius)
#circ_rad = max_circ_radius
if p_d < 0.0:
circ_rad *= -1.0
circ_l, circ_r = self.speed_turn_circle(id, circ_rad*direction, speed*direction)
# On the straight (when diff_theta is small)
straight_rad = 1/(3*math.sin(p_d))
if p_d < 0.0:
straight_rad *= -1.0
stra_l, stra_r = self.speed_turn_circle(id, straight_rad*direction, speed*direction)
# When angle is zero
zero_l, zero_r = speed * direction, speed * direction
# Defuzzy straight and circle radius
used_l = f_d_theta['zero'] * zero_l + f_d_theta['small'] * stra_l + (f_d_theta['med'] + f_d_theta['big'] +f_d_theta['large']) * circ_l
used_r = f_d_theta['zero'] * zero_r + f_d_theta['small'] * stra_r + (f_d_theta['med'] + f_d_theta['big'] +f_d_theta['large']) * circ_r
used_l, used_r = self.limit_speed_to_ang_vel(id, used_l, used_r, min(max_turn_d_th, max(abs(8*diff_theta + 0.05*d_theta), min_turn_d_th)))
used_d_th = (used_l - used_r) / self.axle_length[id]
if debug:
print("__________________________________________")
print(f"id:{id}")
print(f"diff_theta:{diff_theta*RADTODEG}")
print(f"d_theta:{d_theta*RADTODEG}")
print(f"th_zero:{f_d_theta['zero']}, th_small:{f_d_theta['small']}, th_big:{(f_d_theta['med'] + f_d_theta['big'] +f_d_theta['large'])}")
print(f"dist:{dist}, d_dist:{d_dist}")
print(f"d_zero:{f_dist['zero']}, d_small:{f_dist['small']}, d_big:{(f_dist['med'] + f_dist['big'] + f_dist['large'])}")
print(f"speed:{speed}")
print(f"used_l:{used_l}, used_r:{used_r}")
print(f"used_speed:{(used_l + used_r)/2.0}")
print(f"used_d_th:{used_d_th*RADTODEG}")
print(f"direction:{direction}")
self.set_wheel_velocity(id, used_l, used_r)
def match_card_from_vehicles(card, mapper, connector, routes, grids, vehicles, cvmapper):
"""
find the best match for the card
"""
route_nos = mapper.get_lineid_from_statid(card.get_no()[1])
cand_vehicles_nos = set()
for route_no in route_nos:
cand_vehicles_nos |= connector.get_cands(route_no)
cand_vehicles = [vehicles.get_vehicle(vehicle_no)
for vehicle_no in cand_vehicles_nos]
card = card.get_simplified_card(between_card_time)
#plt.close('all')
match_results = []
for vehicle in cand_vehicles:
match_num_1 = 0
best_offset = 0
for offset in offsets:
match_num = 0
time_sequences = map(lambda x: x + offset, card.get_time_sequence())
locations = vehicle.get_locations_at_timestamps(time_sequences)
# x, y = zip(*filter(None, locations))
# plt.figure()
# vehicle.plot(0, len(vehicle))
# plt.plot(x, y, 'gs')
# for route_no in list(route_nos)[:1]:
# route = routes.get_route(route_no)
# route.plot(0, len(route), 'ro')
#
# plt.show()
last_location = (0, 0)
for location in locations:
if location == None:
continue
gid = grid_index(location)
for route_no in route_nos:
tmp_set = grids[gid[0]][gid[1]].get(route_no, set())
if not tmp_set or dist(location, last_location) <= dist_card_time:
continue
route = routes.get_route(route_no)
min_dist = min(dist(location, route.get_location(i)) for i in tmp_set)
if min_dist < dist_stop:
match_num += 1
break
last_location = location
if match_num > match_num_1:
match_num_1 = match_num
best_offset = offset
print vehicle.get_no(), match_num_1, '/', len(card), best_offset
match_results.append((match_num_1, vehicle, best_offset))
match_results.sort(key = lambda x: x[0], reverse = True)
if not match_results:
cvmapper.add_map(card.get_no(), None)
return ("No Match",)
cvmapper.add_map(card.get_no(), (match_results[0][1].get_no(), match_results[0][2]))
if (float(match_results[0][0]) / len(card)) >= min_ratio:
if (len(match_results) == 1 or match_results[0][0] > (1+thres_ratio) * match_results[1][0]):
return "Accurate Match", match_results[0][0], '/', len(card), match_results[0][1].get_no(), match_results[0][2]
else:
return "Inaccurate Match", match_results[0][0], '/', len(card), match_results[0][1].get_no(), match_results[0][2]
else:
return "Low Match", match_results[0][0], '/', len(card), match_results[0][1].get_no(), match_results[0][2]
def dock_range(self, vessel): """Are this and that within each other's dock range?""" return helper.dist(self.pos,vessel.pos) < self.dock_radius + vessel.dock_radius
def collide(self, object): """did self collide with the given space obj?""" return helper.dist(self.pos, object.pos) < self.radius + object.radius
def pass_received(self, idReceiver):
return (helper.dist(self.cur_posture[idReceiver][X], self.cur_ball[X],
self.cur_posture[idReceiver][Y], self.cur_ball[Y])
< 0.2)