def get_move_to_branch_point_via_bus_stop_schedules(
self, target_vehicle, target_bus_schedule, start_time,
branch_index):
bus_schedule = self.__bus_schedules[target_bus_schedule.id]
schedules = []
for i in range(0, len(bus_schedule[branch_index].sub)):
targets = [target_vehicle]
if bus_schedule[branch_index].sub[i].targets is not None:
targets.extend(bus_schedule[branch_index].sub[i].targets)
schedules.append(
Schedule.new_schedule(targets,
bus_schedule[branch_index].sub[i].event,
start_time, start_time + 5,
bus_schedule[branch_index].sub[i].route))
start_time += 1000
branch_index = (branch_index +
1) * (branch_index + 1 < len(bus_schedule))
part_index = 0
for i in range(len(bus_schedule[branch_index].common)):
targets = [target_vehicle]
if bus_schedule[branch_index].common[i].targets is not None:
targets.extend(bus_schedule[branch_index].common[i].targets)
schedules.append(
Schedule.new_schedule(
targets, bus_schedule[branch_index].common[i].event,
start_time, start_time + 1000,
bus_schedule[branch_index].common[i].route))
start_time += 1000
if bus_schedule[branch_index].common[
i].event == SIM_BUS.EVENT.MOVE_TO_BRANCH_POINT:
part_index = i
break
return schedules, branch_index, "common", part_index
def get_pickup_schedules(vehicle_id, user_id, pickup_route, current_time):
targets = [
Target.new_target(vehicle_id, SIM_TAXI.NODE_NAME),
Target.new_target(user_id, SIM_TAXI_USER.NODE_NAME)
]
move_for_picking_up_schedule = Schedule.new_schedule(
targets, SIM_TAXI.TRIGGER.MOVE, current_time, current_time + 1000,
pickup_route)
stop_for_picking_up_schedule = Schedule.new_schedule(
targets, SIM_TAXI.TRIGGER.STOP, current_time + 1000,
current_time + 1010,
Route.new_point_route(pickup_route.goal_waypoint_id,
pickup_route.arrow_codes[-1]))
waiting_schedule = Schedule.new_schedule(
targets, SIM_TAXI_USER.TRIGGER.WAIT, current_time,
current_time + 1000,
Route.new_point_route(pickup_route.goal_waypoint_id,
pickup_route.arrow_codes[-1]))
getting_on_schedule = Schedule.new_schedule(
targets, SIM_TAXI_USER.TRIGGER.GET_ON, current_time + 1000,
current_time + 1010,
Route.new_point_route(pickup_route.goal_waypoint_id,
pickup_route.arrow_codes[-1]))
got_on_schedule = Schedule.new_schedule(
targets, SIM_TAXI_USER.TRIGGER.GOT_ON, current_time + 1010,
current_time + 1011,
Route.new_point_route(pickup_route.goal_waypoint_id,
pickup_route.arrow_codes[-1]))
return [move_for_picking_up_schedule, stop_for_picking_up_schedule],\
[waiting_schedule, getting_on_schedule, got_on_schedule]
def after_change_state_publish_new_bus_schedules(self, current_time,
vehicle_id):
target_vehicle = Target.new_target(vehicle_id, SIM_BUS.NODE_NAME)
target_bus_schedule = self.get_unassigned_bus_schedule_target()
vehicle_schedules = []
start_time = current_time
move_to_circular_route_schedule, branch_index, part_type, part_index = \
self.get_move_to_circular_route_schedule(
target_vehicle, target_bus_schedule, start_time)
vehicle_schedules = \
Schedule.get_merged_schedules(vehicle_schedules, [move_to_circular_route_schedule])
start_time = vehicle_schedules[-1].period.end
move_to_branch_point_schedules, branch_index, part_type, part_index = \
self.get_move_to_branch_point_schedules(
target_vehicle, target_bus_schedule, start_time, branch_index, part_type, part_index)
vehicle_schedules = \
Schedule.get_merged_schedules(vehicle_schedules, move_to_branch_point_schedules)
self.vehicle_schedules[vehicle_id][0].period.end = current_time
self.vehicle_schedules[vehicle_id] = \
Schedule.get_merged_schedules(self.vehicle_schedules[vehicle_id], vehicle_schedules)
self.update_relation(target_bus_schedule, vehicle_id)
self.after_change_state_update_last_indices(vehicle_id, branch_index,
part_type, part_index)
self.after_change_state_publish_schedules(vehicle_id)
return True
def get_taxi_schedules(self, user_id, user_status, current_time):
pickup_route, vehicle_id = self.get_pickup_route(user_status)
if pickup_route is None:
return None, None
carry_route = self.get_carry_route(user_status)
if carry_route is None:
return None, None
if self.vehicle_schedules[vehicle_id][
-1].event == SIM_TAXI.STATE.STAND_BY:
current_time = self.vehicle_schedules[vehicle_id][-1].period.start
pickup_schedules, get_on_schedules = self.get_pickup_schedules(
vehicle_id, user_id, pickup_route, current_time)
carry_schedules, get_out_schedules = \
self.get_carry_schedules(vehicle_id, user_id, carry_route, pickup_schedules[-1].period.end)
deploy_schedules = self.get_deploy_schedules(
vehicle_id, carry_route, carry_schedules[-1].period.end)
vehicle_schedules = Schedule.get_merged_schedules(
self.vehicle_schedules[vehicle_id],
pickup_schedules + carry_schedules + deploy_schedules)
user_schedules = Schedule.get_merged_schedules(
self.user_schedules[user_id], get_on_schedules + get_out_schedules)
return vehicle_id, vehicle_schedules, user_schedules
def get_user_request_schedules(self, user_id, current_time):
user_request_schedule = Schedule.new_schedule(
[Target.new_target(user_id, SIM_TAXI_USER.NODE_NAME)],
SIM_TAXI_USER.TRIGGER.REQUEST,
current_time,
current_time + 1,
)
user_schedules = Schedule.get_merged_schedules(
self.user_schedules[user_id], [user_request_schedule])
return user_schedules
def get_deploy_schedules(vehicle_id, carry_route, current_time):
stand_by_schedules = Schedule.new_schedule(
[
Target.new_target(vehicle_id, SIM_TAXI.NODE_NAME),
], SIM_TAXI.TRIGGER.STAND_BY, current_time, current_time + 86400,
Route.new_point_route(carry_route.goal_waypoint_id,
carry_route.arrow_codes[-1]))
return [stand_by_schedules]
def set_trip_schedules(self, trip_schedules):
self.status.trip_schedules = trip_schedules
self.set_schedules([Schedule.new_schedule(
targets=[self.target],
event=USER.TRIGGER.LOG_IN,
start_time=trip_schedules[0].period.start,
end_time=trip_schedules[0].period.end
)])
def get_move_to_branch_point_schedules(self, target_vehicle,
target_bus_schedule, start_time,
branch_index, part_type,
part_index):
bus_schedule = self.__bus_schedules[target_bus_schedule.id]
schedules = []
if part_type in ["main", "sub"]:
for i in range(part_index,
len(bus_schedule[branch_index][part_type])):
targets = [target_vehicle]
if bus_schedule[branch_index][part_type][
i].targets is not None:
targets.extend(
bus_schedule[branch_index][part_type][i].targets)
schedules.append(
Schedule.new_schedule(
targets,
bus_schedule[branch_index][part_type][i].event,
start_time, start_time + 1000,
bus_schedule[branch_index][part_type][i].route))
start_time += 1000
branch_index = (branch_index +
1) * (branch_index + 1 < len(bus_schedule))
part_type = "common"
part_index = 0
if part_type == "common":
for i in range(part_index,
len(bus_schedule[branch_index][part_type])):
targets = [target_vehicle]
if bus_schedule[branch_index][part_type][
i].targets is not None:
targets.extend(
bus_schedule[branch_index][part_type][i].targets)
schedules.append(
Schedule.new_schedule(
targets,
bus_schedule[branch_index][part_type][i].event,
start_time, start_time + 1000,
bus_schedule[branch_index][part_type][i].route))
start_time += 1000
if bus_schedule[branch_index][part_type][
i].event == SIM_BUS.EVENT.MOVE_TO_BRANCH_POINT:
part_index = i
break
return schedules, branch_index, part_type, part_index
def get_move_to_circular_route_schedule(self, target_vehicle,
target_bus_schedule, start_time):
vehicle_status = self.vehicle_statuses[target_vehicle.id]
to_circular_route, branch_index, part_type, part_index = \
self.get_to_circular_route(
vehicle_status.location.waypoint_id, vehicle_status.location.arrow_code, target_bus_schedule.id)
return Schedule.new_schedule(
[target_vehicle], SIM_BUS.EVENT.MOVE_TO_CIRCULAR_ROUTE, start_time, start_time + 1000,
to_circular_route),\
branch_index, part_type, part_index
def update_vehicle_schedules(self, vehicle_statuses):
for vehicle_id, vehicle_status in vehicle_statuses.items():
if vehicle_id not in self.vehicle_schedules:
self.vehicle_schedules[vehicle_id] = [vehicle_status.schedule]
else:
while self.vehicle_schedules[vehicle_id][
0].id != vehicle_status.schedule.id:
self.vehicle_schedules[vehicle_id].pop(0)
dif_time = vehicle_status.schedule.period.start - self.vehicle_schedules[
vehicle_id][0].period.start
self.vehicle_schedules[vehicle_id] = \
Schedule.get_shifted_schedules(self.vehicle_schedules[vehicle_id], dif_time)
def update_user_schedules(self, user_statuses):
for user_id, user_status in user_statuses.items():
if user_id not in self.user_schedules:
self.user_schedules[user_id] = [user_status.schedule]
else:
while self.user_schedules[user_id][
0].id != user_status.schedule.id:
self.user_schedules[user_id].pop(0)
dif_time = user_status.schedule.period.start - self.user_schedules[
user_id][0].period.start
self.user_schedules[user_id] = \
Schedule.get_shifted_schedules(self.user_schedules[user_id], dif_time)
def get_carry_schedules(vehicle_id, user_id, carry_route, current_time):
targets = [
Target.new_target(vehicle_id, SIM_TAXI.NODE_NAME),
Target.new_target(user_id, SIM_TAXI_USER.NODE_NAME)
]
move_for_discharging_schedules = Schedule.new_schedule(
targets, SIM_TAXI.TRIGGER.MOVE, current_time, current_time + 1010,
carry_route)
stop_for_discharging_schedules = Schedule.new_schedule(
targets, SIM_TAXI.TRIGGER.STOP, current_time + 1010,
current_time + 1020,
Route.new_point_route(carry_route.goal_waypoint_id,
carry_route.arrow_codes[-1]))
move_schedule = Schedule.new_schedule(
targets, SIM_TAXI_USER.TRIGGER.MOVE_VEHICLE, current_time,
current_time + 1010, carry_route)
getting_out_schedule = Schedule.new_schedule(
targets, SIM_TAXI_USER.TRIGGER.GET_OUT, current_time + 1010,
current_time + 1020,
Route.new_point_route(carry_route.goal_waypoint_id,
carry_route.arrow_codes[-1]))
got_out_schedule = Schedule.new_schedule(
[Target.new_target(user_id, SIM_TAXI_USER.NODE_NAME)],
SIM_TAXI_USER.TRIGGER.GOT_OUT, current_time + 1020,
current_time + 1021,
Route.new_point_route(carry_route.goal_waypoint_id,
carry_route.arrow_codes[-1]))
log_out_schedule = Schedule.new_schedule(
[Target.new_target(user_id, SIM_TAXI_USER.NODE_NAME)],
USER.TRIGGER.LOG_OUT, current_time + 1021, current_time + 1022,
Route.new_point_route(carry_route.goal_waypoint_id,
carry_route.arrow_codes[-1]))
return [move_for_discharging_schedules, stop_for_discharging_schedules],\
[move_schedule, getting_out_schedule, got_out_schedule, log_out_schedule]
def after_change_state_publish_through_schedules(self, vehicle_id):
target_vehicle = Target.new_target(vehicle_id, SIM_BUS.NODE_NAME)
target_bus_schedule = self.get_assigned_bus_schedule_target(
target_vehicle)
move_to_branch_point_schedules, branch_index, part_type, part_index = \
self.get_move_to_branch_point_schedules(
target_vehicle, target_bus_schedule, self.vehicle_schedules[vehicle_id][-1].period.end,
self.vehicle_last_indices[vehicle_id]["branch_index"], "main", 0)
self.vehicle_schedules[vehicle_id] = Schedule.get_merged_schedules(
self.vehicle_schedules[vehicle_id], move_to_branch_point_schedules)
self.after_change_state_update_last_indices(vehicle_id, branch_index,
part_type, part_index)
self.after_change_state_publish_schedules(vehicle_id)
return True
def __update_schedules(self):
current_time = time()
schedules = []
if self.route_code in self.schedules:
schedules = list(
filter(lambda x: current_time <= x.period.end, self.schedules))
if self.cycle is not None:
if len(schedules) < 3:
if len(schedules) == 0:
start_time = current_time
else:
start_time = schedules[-1].period.end
schedules.append(
Schedule.get_schedule_from_cycle([self.target], self.cycle,
start_time))
self.schedules = schedules
def add_random_schedule(self, current_time, schedules):
synchronize_route_schedule = Schedule.new_schedule(
[self.target],
AUTOWARE.TRIGGER.SYNCHRONIZE_ROUTE, current_time, current_time + 5,
self.route.new_point_route(
self.status.location.waypoint_id,
self.status.location.arrow_code
)
)
random_route = self.get_random_route()
move_schedule = Schedule.new_schedule(
[self.target],
AUTOWARE.TRIGGER.MOVE, synchronize_route_schedule.period.end, synchronize_route_schedule.period.end + 100,
random_route
)
stop_schedule = Schedule.new_schedule(
[self.target],
AUTOWARE.TRIGGER.STOP, move_schedule.period.end, move_schedule.period.end + 10,
self.route.new_point_route(
move_schedule.route.goal_waypoint_id,
move_schedule.route.arrow_codes[-1]
)
)
get_ready_schedule = Schedule.new_schedule(
[self.target],
AUTOWARE.TRIGGER.GET_READY, move_schedule.period.end, move_schedule.period.end + 1,
self.route.new_point_route(
move_schedule.route.goal_waypoint_id,
move_schedule.route.arrow_codes[-1]
)
)
reschedule_schedule = Schedule.new_schedule(
[self.target],
AUTOWARE.TRIGGER.SCHEDULE, move_schedule.period.end, move_schedule.period.end + 1,
self.route.new_point_route(
move_schedule.route.goal_waypoint_id,
move_schedule.route.arrow_codes[-1]
)
)
schedules[:] = Schedule.get_merged_schedules(
schedules,
[
synchronize_route_schedule, move_schedule, stop_schedule, get_ready_schedule, reschedule_schedule
]
)
def load_bus_schedule_json(path_bus_schedule_json):
with open(path_bus_schedule_json, "r") as f:
bus_schedules_json = json.load(f)["busSchedules"]
bus_schedules = {}
for target_bus_schedules in bus_schedules_json:
schedule_branches = []
for schedule_branch in target_bus_schedules["scheduleBranches"]:
common_schedules = []
for common in schedule_branch["common"]:
common_schedules = Schedule.get_merged_schedules(common_schedules, [Schedule.new_schedule(
Target.new_targets(common["targets"]) if "targets" in common else None,
common["event"], None, None,
Route.decode_route_code(common["routeCode"])
)])
main_schedules = []
for main in schedule_branch["main"]:
main_schedules = Schedule.get_merged_schedules(
main_schedules, [Schedule.new_schedule(
Target.new_targets(main["targets"]) if "targets" in main else None,
main["event"], None, None,
Route.decode_route_code(main["routeCode"])
)]
)
sub_schedules = []
for sub in schedule_branch["sub"]:
sub_schedules = Schedule.get_merged_schedules(
sub_schedules, [Schedule.new_schedule(
Target.new_targets(sub["targets"]) if "targets" in sub else None,
sub["event"], None, None,
Route.decode_route_code(sub["routeCode"])
)]
)
schedule_branches.append(
ScheduleBranch.new_schedule_branch(common_schedules, main_schedules, sub_schedules))
bus_schedules[target_bus_schedules["ID"]] = schedule_branches
return bus_schedules
def get_next_schedules(self, schedules, current_time):
schedules.pop(0)
dif_time = current_time - schedules[0].period.start
schedules = Schedule.get_shifted_schedules(schedules, dif_time)
self.status.schedule = schedules[0]
return schedules
bus_stop_ids.remove(start_bus_stop_id)
goal_bus_stop_id = random.choice(bus_stop_ids)
goal_waypoint_id = bus_parkable_spots[goal_bus_stop_id].contact.waypoint_id
goal_arrow_code = bus_parkable_spots[goal_bus_stop_id].contact.arrow_code
bus_user = SimBusUser(_id=args.id if args.id is not None else str(uuid()),
name=args.name,
dt=3.0)
target_bus_user = Target.new_node_target(bus_user)
trip_schedule = Schedule.new_schedule(
[
target_bus_user,
Target.new_target(start_bus_stop_id,
SimBusUser.CONST.TARGET_GROUP.START_BUS_STOP),
Target.new_target(goal_bus_stop_id,
SimBusUser.CONST.TARGET_GROUP.GOAL_BUS_STOP)
], "trip_schedule", start_time, start_time + 9999,
Route.new_route(start_waypoint_id, goal_waypoint_id,
[start_arrow_code, goal_arrow_code]))
bus_user.set_trip_schedules([trip_schedule])
bus_user.set_schedules(
Schedule.new_schedules([
Schedule.new_schedule(targets=[target_bus_user],
event=User.CONST.TRIGGER.LOG_IN,
start_time=start_time,
end_time=start_time + 1),
Schedule.new_schedule(targets=[target_bus_user],
event=SimBusUser.CONST.TRIGGER.WAIT,
start_time=start_time,
end_time=start_time + 1),
"10374",
"9697", "9698", "9699", "9700", "9701", "9702", "9703", "9704", "9705", "9706", "9707", "9708",
"8936", "8937", "8938", "8939", "8940", "8941", "8942", "8943", "8944", "8945", "8946", "8947", "8948", "8949",
"8950", "8951", "8952", "8953", "8954", "8955", "8956", "8957", "8958", "8959", "8960", "8961", "8962", "8963",
"8964", "8965", "8966", "8967", "8968",
]
start_waypoint_id = args.start_waypoint_id
if start_waypoint_id is None:
start_waypoint_id = random.choice(stop_waypoint_ids)
start_arrow_code = arrow.get_arrow_codes_from_waypoint_id(start_waypoint_id)[0]
start_time = time() - 5
stop_waypoint_ids.remove(start_waypoint_id)
goal_waypoint_id = random.choice(stop_waypoint_ids)
goal_arrow_code = arrow.get_arrow_codes_from_waypoint_id(goal_waypoint_id)[0]
taxi_user = SimTaxiUser(
_id=args.id if args.id is not None else str(uuid()),
name=args.name,
dt=3.0
)
trip_schedule = Schedule.new_schedule(
[Target.new_node_target(taxi_user)],
User.CONST.EVENT.TRIP, start_time, start_time+9999,
Route.new_route(start_waypoint_id, goal_waypoint_id, [start_arrow_code, goal_arrow_code])
)
taxi_user.set_trip_schedules([trip_schedule])
taxi_user.start(host=args.host, port=args.port)
"8966",
"8967",
"8968",
]
start_waypoint_id = args.start_waypoint_id
if start_waypoint_id is None:
start_waypoint_id = random.choice(stop_waypoint_ids)
start_arrow_code = arrow.get_arrow_codes_from_waypoint_id(
start_waypoint_id)[0]
current_time = time()
sim_taxi = SimTaxi(_id=args.id if args.id is not None else str(uuid()),
name=args.name,
waypoint=waypoint,
arrow=arrow,
route=route,
intersection=intersection,
dt=0.5)
sim_taxi.set_waypoint_id_and_arrow_code(start_waypoint_id,
start_arrow_code)
sim_taxi.set_velocity(3.0)
sim_taxi.set_schedules([
Schedule.new_schedule([Target.new_node_target(sim_taxi)],
SimTaxi.CONST.TRIGGER.STAND_BY, current_time,
current_time + 100,
Route.new_route(start_waypoint_id,
start_waypoint_id,
[start_arrow_code]))
])
sim_taxi.start(host=args.host, port=args.port)
waypoint.load(args.path_waypoint_json)
arrow = Arrow(waypoint)
arrow.load(args.path_arrow_json)
route = Route()
route.set_waypoint(waypoint)
route.set_arrow(arrow)
current_time = time()
autoware = Autoware(_id=args.id,
name=args.name,
waypoint=waypoint,
arrow=arrow,
route=route,
dt=0.5)
autoware.set_schedules([
Schedule.new_schedule([Target.new_node_target(autoware)],
Autoware.CONST.TRIGGER.LAUNCH, current_time,
current_time + 100, None),
Schedule.new_schedule([Target.new_node_target(autoware)],
Autoware.CONST.TRIGGER.ACTIVATE, current_time,
current_time + 100, None),
Schedule.new_schedule([Target.new_node_target(autoware)],
Autoware.CONST.TRIGGER.SCHEDULE, current_time,
current_time + 1, None)
])
# autoware.set_velocity(3.0)
autoware.start(host=args.host, port=args.port)
if __name__ == "__main__":
waypoint = Waypoint()
waypoint.load(args.path_waypoint_json)
arrow = Arrow(waypoint)
arrow.load(args.path_arrow_json)
route = Route()
route.set_waypoint(waypoint)
route.set_arrow(arrow)
current_time = time()
autoware_taxi = AutowareTaxi(
_id=args.id,
name=args.name,
waypoint=waypoint,
arrow=arrow,
route=route,
dt=0.5
)
autoware_taxi.set_schedules([Schedule.new_schedule(
[Target.new_node_target(autoware_taxi)],
AutowareTaxi.CONST.STATE.STANDBY, current_time, current_time+100,
None
)])
autoware_taxi.start(host=args.host, port=args.port)
"10362", "10363", "10364", "10365", "10366", "10367", "10368", "10369", "10370", "10371", "10372", "10373",
"10374",
"9697", "9698", "9699", "9700", "9701", "9702", "9703", "9704", "9705", "9706", "9707", "9708",
"8936", "8937", "8938", "8939", "8940", "8941", "8942", "8943", "8944", "8945", "8946", "8947", "8948", "8949",
"8950", "8951", "8952", "8953", "8954", "8955", "8956", "8957", "8958", "8959", "8960", "8961", "8962", "8963",
"8964", "8965", "8966", "8967", "8968",
]
start_waypoint_id = args.start_waypoint_id
if start_waypoint_id is None:
start_waypoint_id = random.choice(stop_waypoint_ids)
start_arrow_code = arrow.get_arrow_codes_from_waypoint_id(start_waypoint_id)[0]
current_time = time()
sim_bus = SimBus(
_id=args.id if args.id is not None else str(uuid()),
name=args.name,
waypoint=waypoint,
arrow=arrow,
route=route,
intersection=intersection,
dt=0.5
)
sim_bus.set_waypoint_id_and_arrow_code(start_waypoint_id, start_arrow_code)
sim_bus.set_velocity(3.0)
sim_bus.set_schedules([Schedule.new_schedule(
[Target.new_node_target(sim_bus)],
SimBus.CONST.TRIGGER.STAND_BY, current_time, current_time + 86400,
Route.new_point_route(start_waypoint_id, start_arrow_code)
)])
sim_bus.start(host=args.host, port=args.port)
start_waypoint_id)[0]
current_time = time()
sim_car = SimCar(_id=args.id if args.id is not None else str(uuid()),
name=args.name,
waypoint=waypoint,
arrow=arrow,
route=route,
intersection=intersection,
dt=0.5)
next_start_waypoint_id = start_waypoint_id
schedules = [
Schedule.new_schedule([Target.new_node_target(sim_car)],
SimCar.CONST.TRIGGER.STOP, current_time,
current_time + 5,
Route.new_route(next_start_waypoint_id,
next_start_waypoint_id,
[start_arrow_code]))
]
current_time += 5
if args.route_code is not None:
schedule = Schedule.new_schedule(
[Target.new_node_target(sim_car)], SimCar.CONST.TRIGGER.MOVE,
current_time, current_time + 100,
Route.new_route(next_start_waypoint_id, goal_waypoint_id,
arrow_codes))
schedules = Schedule.get_merged_schedules(schedules, [schedule])
next_start_waypoint_id = arg_route.goal_waypoint_id
else: