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 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 get_random_route(self):
import random
start_point = {
"arrow_code": self.status.location.arrow_code,
"waypoint_id": self.status.location.waypoint_id,
}
while True:
while True:
goal_arrow_code = random.choice(self.arrow.get_arrow_codes())
goal_waypoint_id = random.choice(self.arrow.get_waypoint_ids(goal_arrow_code))
if goal_waypoint_id != self.status.location.waypoint_id:
break
goal_id = None
goal_points = [{
"goal_id": goal_id,
"arrow_code": goal_arrow_code,
"waypoint_id": goal_waypoint_id,
}]
shortest_routes = self.route.get_shortest_routes(start_point, goal_points, reverse=False)
if 0 == len(shortest_routes):
continue
shortest_route = shortest_routes[goal_id]
shortest_route.pop("cost")
shortest_route.pop("goal_id")
break
return Route.new_route(self.status.location.waypoint_id, goal_waypoint_id, shortest_route.arrow_codes)
def get_distance_from_stopline(self, monitored_route):
monitored_arrow_codes = monitored_route.arrow_codes
distance_from_stopline = SIM_CAR.FLOAT_MAX
self.traffic_signals_lock.acquire()
traffic_signals = deepcopy(self.traffic_signals)
self.traffic_signals_lock.release()
not_green_traffic_signal_route_codes = list(
map(
lambda x: x.route_code,
filter(
lambda x: x.state in
[TRAFFIC_SIGNAL.STATE.YELLOW, TRAFFIC_SIGNAL.STATE.RED],
traffic_signals.values())))
new_monitored_route = None
for i, monitored_arrow_code in enumerate(monitored_arrow_codes):
for not_green_traffic_signal_route_code in not_green_traffic_signal_route_codes:
if monitored_arrow_code in not_green_traffic_signal_route_code:
not_green_traffic_signal_route = Route.decode_route_code(
not_green_traffic_signal_route_code)
if monitored_arrow_code == not_green_traffic_signal_route.arrow_codes[
0]:
waypoint_ids = self.arrow.get_waypoint_ids(
monitored_arrow_code)
if self.status.location.waypoint_id not in waypoint_ids or \
waypoint_ids.index(self.status.location.waypoint_id) <= waypoint_ids.index(
not_green_traffic_signal_route.start_waypoint_id):
new_monitored_route = Route.new_route(
monitored_route.start_waypoint_id,
not_green_traffic_signal_route.
start_waypoint_id,
monitored_arrow_codes[:i + 1])
break
if new_monitored_route is not None:
break
if new_monitored_route is not None:
distance_from_stopline = self.route.get_route_length(
new_monitored_route)
if distance_from_stopline < SIM_CAR.FLOAT_MAX:
logger.info(
"distance_from_stopline {}[m]".format(distance_from_stopline))
return distance_from_stopline
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 get_monitored_route(self, distance=100.0):
if distance <= 0:
return None
arrow_codes = self.status.schedule.route.arrow_codes
arrow_codes = arrow_codes[arrow_codes.index(self.status.location.
arrow_code):]
route = Route.new_route(
self.status.location.waypoint_id,
self.arrow.get_waypoint_ids(
self.status.schedule.route.arrow_codes[-1])[-1], arrow_codes)
return self.route.get_sliced_route(route, distance)
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
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),
Schedule.new_schedule(targets=[target_bus_user],
event=SimBusUser.CONST.TRIGGER.GET_ON,
start_time=start_time,
end_time=start_time + 1),
from pprint import PrettyPrinter
pp = PrettyPrinter(indent=2).pprint
WAYPOINT_FILE = "../../res/waypoint.json"
ARROW_FILE = "../../res/arrow.json"
INTERSECTION_FILE = "../../res/intersection.json"
if __name__ == '__main__':
waypoint = Waypoint()
waypoint.load(WAYPOINT_FILE)
arrow = Arrow(waypoint)
arrow.load(ARROW_FILE)
route = Route()
route.set_waypoint(waypoint)
route.set_arrow(arrow)
intersection = Intersection()
intersection.load(INTERSECTION_FILE)
start_waypoint_id = "9566" # 9232
currentTime = time()
topic = Topic()
topic.set_message(vehicle_message)
schedules = deepcopy(topic.get_template()["schedules"])
schedules[0]["start_time"] = currentTime - 5
schedules[0]["duration"] = 10
parser.add_argument("-WID",
"--start_waypoint_id",
type=str,
default=None,
help="start waypoint id")
args = parser.parse_args()
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)
intersection = Intersection()
intersection.load(args.path_intersection_json)
stop_waypoint_ids = [
"8910",
"8911",
"8912",
"8913",
"8914",
"8915",
"8916",
"8917",
WAYPOINT_FILE = "../../res/waypoint.json"
ARROW_FILE = "../../res/arrow.json"
INTERSECTION_FILE = "../../res/intersection.json"
if __name__ == "__main__":
start_waypoint_id = "9566" # "8809" # "9566" # 9232
waypoint = Waypoint()
waypoint.load(WAYPOINT_FILE)
arrow = Arrow(waypoint)
arrow.load(ARROW_FILE)
route = Route()
route.set_waypoint(waypoint)
route.set_arrow(arrow)
intersection = Intersection()
intersection.load(INTERSECTION_FILE)
current_time = time()
schedules = [
{
"scheduleID": "start",
"startTime": current_time - 5,
"endTime": current_time + 5,
"content": {
"type": "standBy",
parser.add_argument("-R",
"--route_code",
type=str,
default=None,
help="route_code")
args = parser.parse_args()
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)
intersection = Intersection()
intersection.load(args.path_intersection_json)
stop_waypoint_ids = [
"8910",
"8911",
"8912",
"8913",
"8914",
"8915",
"8916",
"8917",
#!/usr/bin/env python
# coding: utf-8
from ams import Waypoint, Arrow, Route
from ams.nodes import FleetManager
WAYPOINT_FILE = "../../res/waypoint.json"
ARROW_FILE = "../../res/arrow.json"
ROUTE_FILE = "../../res/route.json"
if __name__ == '__main__':
waypoint = Waypoint()
waypoint.load(WAYPOINT_FILE)
arrow = Arrow(waypoint)
arrow.load(ARROW_FILE)
route = Route()
route.set_waypoint(waypoint)
route.set_arrow(arrow)
try:
route.load(ROUTE_FILE)
except:
print("no", ROUTE_FILE)
fleetManager = FleetManager(waypoint, arrow, route)
fleetManager.start(host="localhost", port=1883)
def __init__(self):
self.__waypoint = None
self.__arrow = None
self.__route = Route()
class MapMatch(object):
def __init__(self):
self.__waypoint = None
self.__arrow = None
self.__route = Route()
def set_waypoint(self, waypoint):
self.__waypoint = waypoint
self.__route.set_waypoint(self.__waypoint)
def set_arrow(self, arrow):
self.__arrow = arrow
self.__route.set_arrow(self.__arrow)
@staticmethod
def get_similarity_between_poses(pose1, pose2):
similarity = 0.0
similarity -= abs(pose1.position.x - pose2.position.x)
similarity -= abs(pose1.position.y - pose2.position.y)
similarity -= abs(pose1.position.z - pose2.position.z)
if None not in [pose1.orientation, pose2.orientation]:
if None not in [pose1.orientation.rpy, pose2.orientation.rpy]:
if None not in [
pose1.orientation.rpy.roll, pose2.orientation.rpy.roll
]:
similarity -= abs(pose1.orientation.rpy.roll -
pose2.orientation.rpy.roll)
if None not in [
pose1.orientation.rpy.pitch,
pose2.orientation.rpy.pitch
]:
similarity -= abs(pose1.orientation.rpy.pitch -
pose2.orientation.rpy.pitch)
if None not in [
pose1.orientation.rpy.yaw, pose2.orientation.rpy.yaw
]:
similarity -= abs(pose1.orientation.rpy.yaw -
pose2.orientation.rpy.yaw)
if None not in [
pose1.orientation.quaternion, pose2.orientation.quaternion
]:
similarity -= abs(pose1.orientation.quaternion.w -
pose2.orientation.quaternion.w)
similarity -= abs(pose1.orientation.quaternion.x -
pose2.orientation.quaternion.x)
similarity -= abs(pose1.orientation.quaternion.y -
pose2.orientation.quaternion.y)
similarity -= abs(pose1.orientation.quaternion.z -
pose2.orientation.quaternion.z)
return similarity
def get_matched_location_on_route(self, pose, route):
locations = self.__route.get_locations(route)
similarity_max = 0.0
matched_location = None
for location in locations:
pose_on_route = self.__waypoint.get_pose(location.waypoint_id)
similarity = MapMatch.get_similarity_between_poses(
pose, pose_on_route)
if matched_location is None or similarity_max < similarity:
matched_location = location
similarity_max = similarity
return matched_location
def get_matched_location_on_arrows(self, pose, arrow_codes=None):
if arrow_codes is None:
arrow_codes = self.__arrow.get_arrow_codes()
similarity_max = 0.0
matched_location = None
for arrow_code in arrow_codes:
for waypoint_id in self.__arrow.get_waypoint_ids(arrow_code):
pose_on_route = self.__waypoint.get_pose(waypoint_id)
similarity = MapMatch.get_similarity_between_poses(
pose, pose_on_route)
if matched_location is None or similarity_max < similarity:
matched_location = Location.new_location(
waypoint_id, arrow_code)
similarity_max = similarity
return matched_location