class ClosestWaypointSubscriber(EventLoop):
def __init__(self, name, host, port):
super(ClosestWaypointSubscriber, self).__init__()
self.autowareSubscribeTopic = Topic()
self.autowareSubscribeTopic.set_id(name)
self.autowareSubscribeTopic.set_root(Autoware.TOPIC.SUBSCRIBE)
self.autowareSubscribeTopic.set_message(autoware_message)
self.__previous_time = time()
self.__period = 1.0 # [sec]
self.connect(host, port)
self.set_main_loop(rospy.spin)
rospy.init_node("ams_closest_waypoint_subscriber", anonymous=True)
self.__ROSSubscriber = message_filters.Subscriber(
Autoware.ROSTOPIC.SUBSCRIBE, Int32)
self.__ROSSubscriber.registerCallback(self.on_message_from_ros,
self.publish)
def on_message_from_ros(self, messageData, publish):
current_time = time()
if self.__period < current_time - self.__previous_time:
self.__previous_time += (1 + int(
(current_time - self.__previous_time) /
self.__period)) * self.__period
message = self.autowareSubscribeTopic.get_template(
)["closest_waypoint"]
message["index"] = messageData.data
payload = self.autowareSubscribeTopic.serialize(message)
publish(self.autowareSubscribeTopic.private + "/closest_waypoint",
payload)
def request_fleet_relations():
topic = Topic()
topic.set_root(FleetManager.TOPIC.SUBSCRIBE)
topic.set_message(fleet_manager_message)
message = topic.get_template()
message["action"] = FleetManager.ACTION.PUBLISH_RELATIONS
mqtt.publish(topic.root, topic.serialize(message))
return api_response(code=200, message={"result": "requested"})
class SimBusFleet(FleetManager):
CONST = SIM_BUS_FLEET
def __init__(self, _id, name, waypoint, arrow, route, spot):
super().__init__(_id, name, waypoint, arrow, route)
self.waypoint = waypoint
self.arrow = arrow
self.route = route
self.spot = spot
self.relation = Relation()
self.__bus_routes = {}
self.__bus_schedules = {}
self.bus_parkable_spots = self.spot.get_spots_of_target_group(
Target.new_node_target(SimBus))
self.vehicle_statuses = self.manager.dict()
self.vehicle_statuses_lock = self.manager.Lock()
self.vehicle_schedules = {}
self.vehicle_last_indices = {}
self.bus_schedules = {}
self.bus_stop_spots = {}
self.state_machines = {}
self.__topicPubVehicleSchedules = Topic()
self.__topicPubVehicleSchedules.set_categories(
FleetManager.CONST.TOPIC.CATEGORIES.SCHEDULES)
self.__topicSubVehicleStatus = Topic()
self.__topicSubVehicleStatus.set_targets(
Target.new_target(None, SIM_BUS.NODE_NAME), None)
self.__topicSubVehicleStatus.set_categories(
Vehicle.CONST.TOPIC.CATEGORIES.STATUS)
self.__topicSubVehicleStatus.set_message(VehicleStatus)
self.set_subscriber(self.__topicSubVehicleStatus,
self.update_vehicle_status)
def __publish_vehicle_schedules(self, vehicle_id, payload):
self.__topicPubVehicleSchedules.set_targets(
self.target, Target.new_target(vehicle_id, SIM_BUS.NODE_NAME))
self.publish(self.__topicPubVehicleSchedules, payload)
def set_bus_schedules(self, bus_schedules):
self.__bus_schedules = bus_schedules
for bus_schedules_id in self.__bus_schedules:
self.relation.add_relation(
Target.new_target(bus_schedules_id,
SIM_BUS_FLEET.TARGET_GROUP.BUS_SCHEDULES),
Target.new_target(None, SIM_BUS.NODE_NAME))
def update_vehicle_status(self, _client, _userdata, topic, payload):
self.vehicle_statuses_lock.acquire()
vehicle_id = self.__topicSubVehicleStatus.get_from_id(topic)
vehicle_status = self.__topicSubVehicleStatus.unserialize(payload)
self.vehicle_statuses[vehicle_id] = vehicle_status
self.vehicle_statuses_lock.release()
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)
def update_relation(self, target_bus_schedule, vehicle_id):
self.relation.remove_relation(
target_bus_schedule, Target.new_target(None, SIM_BUS.NODE_NAME))
self.relation.add_relation(
target_bus_schedule,
Target.new_target(vehicle_id, SIM_BUS.NODE_NAME))
def get_unassigned_bus_schedule_target(self):
targets = Target.new_targets(
self.relation.get_related(
Target.new_target(None, SIM_BUS.NODE_NAME)))
if len(targets) == 0:
return None
return targets[0]
def get_assigned_bus_schedule_target(self, target_vehicle):
return self.relation.get_related(target_vehicle)[0]
def get_to_circular_route(self, start_waypoint_id, start_arrow_code,
bus_schedules_id):
start_point = {
"arrow_code": start_arrow_code,
"waypoint_id": start_waypoint_id
}
goal_points = []
for branch_index, branch in enumerate(
self.__bus_schedules[bus_schedules_id]):
for part_index, schedule in enumerate(branch.common):
if schedule.event == SIM_BUS.EVENT.MOVE_TO_SELECT_POINT:
goal_point = {
"goal_id":
",".join(map(str,
[branch_index, "common", part_index])),
"waypoint_id":
schedule.route.start_waypoint_id,
"arrow_code":
schedule.route.arrow_codes[0],
# "waypoint_id": schedule.route.goal_waypoint_id,
# "arrow_code": schedule.route.arrow_codes[-1],
}
if goal_point not in goal_points:
goal_points.append(goal_point)
shortest_routes = self.route.get_shortest_routes(start_point,
goal_points,
reverse=False)
to_circular_route = min(shortest_routes.items(),
key=lambda x: x[1]["cost"])[1]
to_circular_route.pop("cost")
branch_index, part_type, part_index = to_circular_route.pop(
"goal_id").split(",")
return to_circular_route, int(branch_index), part_type, int(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 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_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
@staticmethod
def get_event_renamed_schedules(schedules):
event_renamed_schedules = deepcopy(schedules)
for i, schedule in enumerate(event_renamed_schedules):
if schedule.event in [
SIM_BUS.EVENT.MOVE_TO_CIRCULAR_ROUTE,
SIM_BUS.EVENT.MOVE_TO_SELECT_POINT,
SIM_BUS.EVENT.MOVE_TO_BRANCH_POINT,
SIM_BUS.EVENT.MOVE_TO_BUS_STOP,
SIM_BUS.EVENT.MOVE_TO_JUNCTION,
SIM_BUS.EVENT.MOVE_TO_PARKING,
]:
event_renamed_schedules[i].event = SIM_BUS.TRIGGER.MOVE
elif schedule.event in [
SIM_BUS.EVENT.STOP_TO_TAKE_UP,
SIM_BUS.EVENT.STOP_TO_DISCHARGE,
SIM_BUS.EVENT.STOP_TO_DISCHARGE_AND_TAKE_UP,
SIM_BUS.EVENT.STOP_TO_PARKING
]:
event_renamed_schedules[i].event = SIM_BUS.TRIGGER.STOP
elif schedule.event in []:
event_renamed_schedules[
i].event = SIM_BUS.TRIGGER.REQUEST_SCHEDULES
return event_renamed_schedules
def get_state_machine(
self, initial_state=SIM_BUS_FLEET.STATE.WAITING_FOR_BUS_STAND_BY):
machine = StateMachine(
states=list(SIM_BUS_FLEET.STATE),
initial=initial_state,
)
machine.add_transitions([
{
"trigger":
SIM_BUS_FLEET.TRIGGER.ASSIGN_BUS_SCHEDULES,
"source":
SIM_BUS_FLEET.STATE.WAITING_FOR_BUS_STAND_BY,
"dest":
SIM_BUS_FLEET.STATE.WAITING_FOR_SCHEDULES_REQUEST,
"conditions": [
self.
condition_schedules_length_and_publish_new_bus_schedules
]
},
{
"trigger":
SIM_BUS_FLEET.TRIGGER.SEND_THROUGH_SCHEDULES,
"source":
SIM_BUS_FLEET.STATE.WAITING_FOR_SCHEDULES_REQUEST,
"dest":
SIM_BUS_FLEET.STATE.WAITING_FOR_SCHEDULES_REQUEST,
"conditions":
[self.condition_vehicle_state_and_publish_through_schedules]
},
{
"trigger":
SIM_BUS_FLEET.TRIGGER.SEND_VIA_SCHEDULES,
"source":
SIM_BUS_FLEET.STATE.WAITING_FOR_SCHEDULES_REQUEST,
"dest":
SIM_BUS_FLEET.STATE.WAITING_FOR_SCHEDULES_REQUEST,
"conditions":
[self.condition_vehicle_state_and_publish_via_schedules]
},
])
return machine
def condition_schedules_length(self, vehicle_id, expected_length):
return len(self.vehicle_schedules[vehicle_id]) == expected_length
@staticmethod
def condition_vehicle_state(vehicle_status, expected_state):
return vehicle_status.state == expected_state
def after_change_state_update_last_indices(self, vehicle_id, branch_index,
part_type, part_index):
self.vehicle_last_indices[vehicle_id] = {
"branch_index": branch_index,
"part_type": part_type,
"part_index": part_index
}
return True
def after_change_state_publish_schedules(self, vehicle_id):
event_renamed_schedules = self.get_event_renamed_schedules(
self.vehicle_schedules[vehicle_id])
payload = self.__topicPubVehicleSchedules.serialize(
event_renamed_schedules)
self.__publish_vehicle_schedules(vehicle_id, payload)
return True
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 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 after_change_state_publish_via_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_via_bus_stop_schedules, branch_index, part_type, part_index = \
self.get_move_to_branch_point_via_bus_stop_schedules(
target_vehicle, target_bus_schedule, self.vehicle_schedules[vehicle_id][-1].period.end,
self.vehicle_last_indices[vehicle_id]["branch_index"])
self.vehicle_schedules[vehicle_id] = Schedule.get_merged_schedules(
self.vehicle_schedules[vehicle_id],
move_to_branch_point_via_bus_stop_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 condition_schedules_length_and_publish_new_bus_schedules(
self, current_time, vehicle_id):
if self.condition_schedules_length(vehicle_id, 1):
self.after_change_state_publish_new_bus_schedules(
current_time, vehicle_id)
return True
return False
def condition_vehicle_state_and_publish_through_schedules(
self, vehicle_id, vehicle_status, expected_state):
if self.condition_vehicle_state(vehicle_status, expected_state):
if self.condition_schedules_length(vehicle_id, 1):
self.after_change_state_publish_through_schedules(vehicle_id)
return True
return False
def condition_vehicle_state_and_publish_via_schedules(
self, vehicle_id, vehicle_status, expected_state):
if self.condition_vehicle_state(vehicle_status, expected_state):
if self.condition_schedules_length(vehicle_id, 1):
self.after_change_state_publish_via_schedules(vehicle_id)
return True
return False
def get_vehicle_statuses_and_lock(self):
self.vehicle_statuses_lock.acquire()
return deepcopy(self.vehicle_statuses)
def set_vehicle_statuses_and_unlock(self, vehicle_statuses):
self.vehicle_statuses.clear()
self.vehicle_statuses.update(vehicle_statuses)
self.vehicle_statuses_lock.release()
def update_status(self):
vehicle_statuses = self.get_vehicle_statuses_and_lock()
self.update_vehicle_schedules(vehicle_statuses)
self.update_state_machines(vehicle_statuses)
self.set_vehicle_statuses_and_unlock(vehicle_statuses)
def update_state_machines(self, vehicle_statuses):
current_time = time()
for vehicle_id, vehicle_status in vehicle_statuses.items():
if vehicle_id not in self.state_machines:
self.state_machines[vehicle_id] = self.get_state_machine()
vehicle_state = self.state_machines[vehicle_id].state
if vehicle_state == SIM_BUS_FLEET.STATE.WAITING_FOR_BUS_STAND_BY:
self.state_machines[vehicle_id].send_circular_route_schedules(
current_time, vehicle_id)
elif vehicle_state == SIM_BUS_FLEET.STATE.WAITING_FOR_SCHEDULES_REQUEST:
if not self.state_machines[vehicle_id].send_through_schedules(
vehicle_id, vehicle_status,
SIM_BUS.STATE.REQUEST_THROUGH_SCHEDULES):
self.state_machines[vehicle_id].send_via_schedules(
vehicle_id, vehicle_status,
SIM_BUS.STATE.REQUEST_VIA_SCHEDULES)
class SimCar(Vehicle):
CONST = SIM_CAR
def __init__(self,
_id,
name,
waypoint,
arrow,
route,
intersection,
dt=1.0):
super().__init__(_id, name, waypoint, arrow, route, dt=dt)
self.state_machine = self.get_state_machine()
self.velocity = None
self.traffic_signals = self.manager.dict()
self.traffic_signals_lock = self.manager.Lock()
self.other_vehicle_locations = self.manager.dict()
self.other_vehicle_locations_lock = self.manager.Lock()
self.intersection = intersection
self.__topicPubLocation = Topic()
self.__topicPubLocation.set_targets(
Target.new_target(self.target.id, SIM_CAR.NODE_NAME))
self.__topicPubLocation.set_categories(
SIM_CAR.TOPIC.CATEGORIES.LOCATION)
self.__topicSubStatus = Topic()
self.__topicSubStatus.set_targets(
Target.new_target(None, SIM_CAR.NODE_NAME), None)
self.__topicSubStatus.set_categories(SIM_CAR.TOPIC.CATEGORIES.LOCATION)
self.__topicSubStatus.set_message(Location)
self.set_subscriber(self.__topicSubStatus,
self.update_other_vehicle_locations)
self.__topicSubTrafficSignalStatus = Topic()
self.__topicSubTrafficSignalStatus.set_targets(
Target.new_target(None, TRAFFIC_SIGNAL.NODE_NAME), None)
self.__topicSubTrafficSignalStatus.set_categories(
TRAFFIC_SIGNAL.TOPIC.CATEGORIES.STATUS)
self.__topicSubTrafficSignalStatus.set_message(TrafficSignalStatus)
self.set_subscriber(self.__topicSubTrafficSignalStatus,
self.update_traffic_signals)
def set_velocity(self, velocity):
self.velocity = velocity
def publish_location(self):
self.status.location.geohash = self.waypoint.get_geohash(
self.status.location.waypoint_id)
payload = self.__topicPubLocation.serialize(self.status.location)
self.publish(self.__topicPubLocation, payload)
def update_traffic_signals(self, _client, _user_data, _topic, payload):
# todo: localize
traffic_signal_status = self.__topicSubTrafficSignalStatus.unserialize(
payload)
self.traffic_signals_lock.acquire()
self.traffic_signals[
traffic_signal_status.route_code] = traffic_signal_status
self.traffic_signals_lock.release()
def update_other_vehicle_locations(self, _client, _user_data, topic,
payload):
# todo: localize
from_id = Topic.get_from_id(topic)
if self.target.id != from_id:
self.other_vehicle_locations_lock.acquire()
self.other_vehicle_locations[
from_id] = self.__topicSubStatus.unserialize(payload)
self.other_vehicle_locations_lock.release()
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 get_distance_from_preceding_vehicle(self, monitored_route):
self.other_vehicle_locations_lock.acquire()
other_vehicle_locations = deepcopy(self.other_vehicle_locations)
self.other_vehicle_locations_lock.release()
monitored_waypoint_ids = self.route.get_waypoint_ids(monitored_route)
distance_from_preceding_vehicle = SIM_CAR.FLOAT_MAX
if self.status.location.arrow_code is not None and 0 < len(
other_vehicle_locations):
other_vehicles_waypoint_ids = list(
map(lambda x: x.waypoint_id, other_vehicle_locations.values()))
for i, monitored_waypoint_id in enumerate(monitored_waypoint_ids):
if monitored_waypoint_id in other_vehicles_waypoint_ids:
distance_from_preceding_vehicle = \
self.route.get_distance_of_waypoints(monitored_waypoint_ids[:i+1])
break
if distance_from_preceding_vehicle < SIM_CAR.FLOAT_MAX:
logger.info("distance_from_preceding_vehicle {}[m]".format(
distance_from_preceding_vehicle))
return distance_from_preceding_vehicle
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_movable_distance(self):
monitored_route = self.get_monitored_route()
if monitored_route is None:
return 0.0
distance_from_preceding_vehicle = self.get_distance_from_preceding_vehicle(
monitored_route)
movable_distance = distance_from_preceding_vehicle - SIM_CAR.LOWER_INTER_VEHICLE_DISTANCE
monitored_route = self.get_monitored_route(movable_distance)
if monitored_route is None:
return 0.0
distance_from_stopline = self.get_distance_from_stopline(
monitored_route)
movable_distance = min(
movable_distance, distance_from_stopline -
SIM_CAR.LOWER_INTER_TRAFFIC_SIGNAL_DISTANCE)
return movable_distance
def update_pose(self):
movable_distance = self.__get_movable_distance()
delta_distance = min(self.velocity * self.dt, movable_distance)
if 0.0 < delta_distance:
self.np_position, self.status.pose.orientation.rpy.yaw,\
self.status.location.arrow_code, self.status.location.waypoint_id = \
self.get_next_pose(delta_distance, self.status.schedule.route)
self.publish_location()
def update_route(self):
index = self.status.schedule.route.arrow_codes.index(
self.status.location.arrow_code)
self.status.schedule.route.arrow_codes[:] = self.status.schedule.route.arrow_codes[
index:]
self.status.schedule.route.start_waypoint_id = self.status.location.waypoint_id
def update_velocity(self):
speed_limit = self.waypoint.get_speed_limit(
self.status.location.waypoint_id)
if self.velocity < speed_limit:
self.velocity += min(SIM_CAR.ACCELERATION_MAX * self.dt,
speed_limit - self.velocity)
elif speed_limit < self.velocity:
self.velocity = speed_limit
return
def get_next_pose(self, delta_distance, route):
position, waypoint_id, arrow_code = self.route.get_moved_position(
self.np_position, delta_distance, route)
yaw = self.arrow.get_yaw(arrow_code, waypoint_id)
return position, yaw, arrow_code, waypoint_id
def update_pose_to_route_start(self):
self.status.location.waypoint_id = self.status.schedule.route.start_waypoint_id
self.status.location.arrow_code = self.status.schedule.route.arrow_codes[
0]
self.np_position = self.waypoint.get_np_position(
self.status.location.waypoint_id)
self.status.pose.orientation.rpy.yaw = \
self.arrow.get_yaw(self.status.location.arrow_code, self.status.location.waypoint_id)
self.velocity = 0.0
def get_state_machine(self, initial_state=SIM_CAR.STATE.STOP):
machine = StateMachine(
states=list(SIM_CAR.STATE),
initial=initial_state,
)
machine.add_transitions([{
"trigger":
SIM_CAR.TRIGGER.MOVE,
"source":
SIM_CAR.STATE.STOP,
"dest":
SIM_CAR.STATE.MOVE,
"conditions": [self.after_state_change_update_schedules]
}, {
"trigger":
SIM_CAR.TRIGGER.MOVE,
"source":
SIM_CAR.STATE.MOVE,
"dest":
SIM_CAR.STATE.MOVE,
"conditions": [self.condition_achieved_and_update_schedules]
}, {
"trigger":
SIM_CAR.TRIGGER.STOP,
"source":
SIM_CAR.STATE.MOVE,
"dest":
SIM_CAR.STATE.STOP,
"conditions": [self.condition_achieved_and_update_schedules]
}])
return machine
def condition_achieved(self):
return self.status.location.waypoint_id == self.status.schedule.route.goal_waypoint_id
def condition_time_limit(self, current_time):
return self.status.schedule.period.end < current_time
def after_state_change_update_schedules(self, current_time, schedules):
schedules[:] = self.get_next_schedules(schedules, current_time)
self.update_pose_to_route_start()
return True
def condition_achieved_and_update_schedules(self, current_time, schedules):
if self.condition_achieved():
self.after_state_change_update_schedules(current_time, schedules)
return True
return False
def condition_time_limit_and_update_schedules(self, current_time,
schedules):
if self.condition_time_limit(current_time):
self.after_state_change_update_schedules(current_time, schedules)
return True
return False
def update_status(self):
schedules = self.get_schedules_and_lock()
if self.state_machine.state == SIM_CAR.STATE.MOVE:
self.update_pose()
self.update_route()
self.update_velocity()
if 1 < len(schedules):
current_time = time()
next_event = schedules[1].event
if next_event == SIM_CAR.TRIGGER.MOVE:
self.state_machine.move(current_time, schedules)
elif next_event == SIM_CAR.TRIGGER.STOP:
self.state_machine.stop(current_time, schedules)
else:
pass
self.set_schedules_and_unlock(schedules)
class User(EventLoop):
class TOPIC(object):
PUBLISH = "pubUser"
SUBSCRIBE = "subUser"
class STATE(object):
LOGIN = "******"
WAITING = "waiting"
GETTING_ON = "gettingOn"
GOT_ON = "gotOn"
MOVING = "moving"
GETTING_OUT = "gettingOut"
GOT_OUT = "gotOut"
class ACTION(object):
WAIT = "wait"
GET_ON = "getOn"
GET_OUT = "getOut"
class EVENT(object):
MOVE_VEHICLE = "moveVehicle"
def __init__(self, name, waypoint, dt=1.0):
super().__init__()
self.topicUserPublish = Topic()
self.topicUserPublish.set_id(self.event_loop_id)
self.topicUserPublish.set_root(User.TOPIC.PUBLISH)
self.topicUserPublish.set_message(user_message)
self.topicUserSubscribe = Topic()
self.topicUserSubscribe.set_id(self.event_loop_id)
self.topicUserSubscribe.set_root(User.TOPIC.SUBSCRIBE)
self.topicUserSubscribe.set_message(user_message)
self.name = name
self.id = self.event_loop_id
self.state = User.STATE.LOGIN
self.event = None
self.action = None
self.dt = dt
self.__start_waypoint_id = None
self.__goal_waypoint_id = None
self.__vehicleID = None
self.__waypoint = waypoint
self.add_on_message_function(self.update_action)
self.add_on_message_function(self.update_event)
self.set_subscriber(self.topicUserSubscribe.private + "/schedules")
self.set_subscriber(self.topicUserSubscribe.private + "/event")
self.set_main_loop(self.__main_loop)
def publish_status(self):
message = self.topicUserPublish.get_template()
message["name"] = self.name
message["state"] = self.state
message["event"] = self.event
message["schedules"][0]["action"] = self.action
message["schedules"][0]["start"][
"waypoint_id"] = self.__start_waypoint_id
message["schedules"][0]["goal"][
"waypoint_id"] = self.__goal_waypoint_id
payload = self.topicUserPublish.serialize(message)
self.publish(self.topicUserPublish.private, payload)
def set_waypoint_at_random(self, waypoint_ids=None):
if waypoint_ids is None:
waypoint_ids = self.__waypoint.get_waypoint_ids()
start_waypoint_id = random.choice(waypoint_ids)
waypoint_ids.remove(start_waypoint_id)
goal_waypoint_id = random.choice(waypoint_ids)
self.set_waypoint(start_waypoint_id, goal_waypoint_id)
def set_waypoint(self, start_waypoint_id, goal_waypoint_id):
self.set_start_waypoint(start_waypoint_id)
self.set_goal_waypoint(goal_waypoint_id)
def set_start_waypoint(self, start_waypoint_id):
self.__start_waypoint_id = start_waypoint_id
def set_goal_waypoint(self, goal_waypoint_id):
self.__goal_waypoint_id = goal_waypoint_id
def update_action(self, _client, _userdata, topic, payload):
# print(topic)
if topic == self.topicUserSubscribe.private + "/schedules":
message = self.topicUserSubscribe.unserialize(payload)
self.action = message["schedules"][0]["action"]
def update_event(self, _client, _userdata, topic, payload):
if topic == self.topicUserSubscribe.private + "/event":
message = self.topicUserSubscribe.unserialize(payload)
self.event = message["event"]
def update_status(self):
# print(self.state, self.event, self.action)
if self.state == User.STATE.LOGIN:
if self.action == User.ACTION.WAIT:
self.state = User.STATE.WAITING
self.action = None
elif self.state == User.STATE.WAITING:
if self.action == User.ACTION.GET_ON:
self.state = User.STATE.GETTING_ON
self.action = None
elif self.state == User.STATE.GETTING_ON:
self.state = User.STATE.GOT_ON
elif self.state == User.STATE.GOT_ON:
if self.event == User.EVENT.MOVE_VEHICLE:
self.state = User.STATE.MOVING
self.event = None
elif self.state == User.STATE.MOVING:
if self.action == User.ACTION.GET_OUT:
self.state = User.STATE.GETTING_OUT
self.action = None
elif self.state == User.STATE.GETTING_OUT:
self.state = User.STATE.GOT_OUT
def __main_loop(self):
self.publish_status()
while self.state != User.STATE.GOT_OUT:
sleep(self.dt)
self.update_status()
self.publish_status()
sleep(2)
traffic_signal = TrafficSignal(
_id=args.id if args.id is not None else str(uuid()),
route_code=args.route_code)
process = Process(target=traffic_signal.start, args=[args.host, args.port])
process.start()
if args.cycle is not None:
sleep(5)
# print("publish cycles")
topicCycle = Topic()
topicCycle.set_targets(
Target.new_target(None, "TrafficSignalCycleSetter"),
traffic_signal.target)
topicCycle.set_categories(TrafficSignal.CONST.TOPIC.CATEGORIES.CYCLE)
mqtt_client.publish(topicCycle.get_path(),
topicCycle.serialize(json.loads(args.cycle)))
if args.schedules is not None:
sleep(5)
# print("publish schedules")
topicSchedules = Topic()
topicSchedules.set_targets(
Target.new_target(None, "TrafficSignalSchedulesSetter"),
traffic_signal.target)
topicSchedules.set_categories(
TrafficSignal.CONST.TOPIC.CATEGORIES.SCHEDULES)
mqtt_client.publish(
topicSchedules.get_path(),
topicSchedules.serialize(json.loads(args.schedules)))
# print("wait join")
class EventLoop(object):
KEEP_ALIVE = 60
class TOPIC(object):
PUBLISH = "pub_event_loop"
SUBSCRIBE = "sub_event_loop"
def __init__(self, _id=None):
self.event_loop_id = _id
if _id is None:
self.event_loop_id = str(uuid())
self.__subscribeTopic = Topic()
self.__subscribeTopic.set_id(self.event_loop_id)
self.__subscribeTopic.set_root(EventLoop.TOPIC.SUBSCRIBE)
self.__subscribeTopic.set_message(event_loop_message)
self.__publishTopic = Topic()
self.__publishTopic.set_id(self.event_loop_id)
self.__publishTopic.set_root(EventLoop.TOPIC.PUBLISH)
self.__publishTopic.set_message(event_loop_message)
self.__subscribers = {}
self.__publishers = {}
self.__client = None
self.__main_loop = None
self.__pid = os.getpid()
self.set_subscriber(self.__subscribeTopic.private)
self.__on_message_functions = []
self.__user_data = None
self.__user_will = None
def __del__(self):
if self.__client is not None:
message = self.__publishTopic.get_template()
message["action"] = "disconnect"
message["pid"] = self.__pid
payload = self.__publishTopic.serialize(message)
self.publish(self.__publishTopic.private, payload)
self.__client.disconnect()
def set_subscriber(self, topic):
self.__subscribers[str(uuid())] = {"topic": topic}
def set_user_data(self, user_data):
self.__user_data = user_data
def add_on_message_function(self, on_message_function):
self.__on_message_functions.append(on_message_function)
def set_main_loop(self, main_loop):
self.__main_loop = main_loop
def set_will(self, topic, payload):
self.__user_will = {"topic": topic, "payload": payload}
def publish(self, topic, payload, qos=0, retain=False):
# payload = self.__subscribeTopic.serialize(message)
self.__client.publish(topic, payload=payload, qos=qos, retain=retain)
def __on_message(self, client, userdata, message_data):
payload = message_data.payload.decode("utf-8")
if self.__subscribeTopic.root in message_data.topic and \
self.__subscribeTopic.get_id(message_data.topic) == self.event_loop_id:
message = self.__subscribeTopic.unserialize(payload)
if message["action"] == "start":
print(self.__subscribeTopic.root, message)
if message["action"] == "kill":
print(self.__subscribeTopic.root, message)
self.end()
if message["action"] == "check":
print(self.__subscribeTopic.root, message)
self.__check()
for onMessageFunction in self.__on_message_functions:
onMessageFunction(client, userdata, message_data.topic, payload)
return True
def connect(self, host, port):
self.__client = mqtt.Client(protocol=mqtt.MQTTv311,
userdata=self.__user_data)
will = self.__user_will
print(will)
if will is None:
message = self.__publishTopic.get_template()
message["action"] = "will"
message["pid"] = self.__pid
payload = self.__publishTopic.serialize(message)
will = {"topic": self.__publishTopic.private, "payload": payload}
self.__client.will_set(will["topic"],
payload=will["payload"],
qos=2,
retain=False)
self.__client.on_message = self.__on_message
self.__client.connect(host=host,
port=port,
keepalive=EventLoop.KEEP_ALIVE)
def start(self, host="localhost", port=1883):
self.connect(host, port)
for subscriber in self.__subscribers.values():
self.__client.subscribe(subscriber["topic"])
message = self.__publishTopic.get_template()
message["time"] = str(int(time()))
message["action"] = "start"
message["pid"] = self.__pid
payload = self.__publishTopic.serialize(message)
self.publish(self.__publishTopic.private, payload)
if self.__main_loop is None:
self.__client.loop_forever()
else:
self.__client.loop_start()
self.__main_loop()
def end(self):
self.__client.loop_stop()
self.__client.disconnect()
os.kill(self.__pid, SIGKILL)
def __check(self):
# todo: main_loop zombie
message = self.__publishTopic.get_template()
message["time"] = str(int(time()))
message["action"] = "ok"
message["pid"] = self.__pid
payload = self.__publishTopic.serialize(message)
self.publish(self.__publishTopic.private, payload)
def get_pid(self):
return self.__pid
class TrafficSignal(EventLoop):
CONST = TRAFFIC_SIGNAL
def __init__(self,
_id,
route_code,
state=TRAFFIC_SIGNAL.STATE.UNKNOWN,
processing_cycle=1.0):
super().__init__(_id)
self.route_code = route_code
self.state = state
self.schedules = []
self.cycle = None
self.__processing_cycle = processing_cycle
self.__check_time = time()
self.__publish_flag = False
self.__topicPubStatus = Topic()
self.__topicPubStatus.set_targets(self.target)
self.__topicPubStatus.set_categories(
TRAFFIC_SIGNAL.TOPIC.CATEGORIES.STATUS)
self.__topicSubSchedules = Topic()
self.__topicSubSchedules.set_targets(None, self.target)
self.__topicSubSchedules.set_categories(
TRAFFIC_SIGNAL.TOPIC.CATEGORIES.SCHEDULES)
self.__topicSubSchedules.set_message(Schedules)
self.set_subscriber(self.__topicSubSchedules, self.update_schedules)
self.__topicSubCycle = Topic()
self.__topicSubCycle.set_targets(None, self.target)
self.__topicSubCycle.set_categories(
TRAFFIC_SIGNAL.TOPIC.CATEGORIES.CYCLE)
self.__topicSubCycle.set_message(Cycle)
self.set_subscriber(self.__topicSubCycle, self.update_cycle)
self.set_main_loop(self.__main_loop)
@staticmethod
def get_status(route_code, state):
return Status.new_data(route_code=route_code, time=time(), state=state)
def publish_status(self):
status = TrafficSignal.get_status(self.route_code, self.state)
payload = self.__topicPubStatus.serialize(status)
self.publish(self.__topicPubStatus, payload)
def update_schedules(self, _client, _userdata, _topic, payload):
self.schedules = self.__topicSubSchedules.unserialize(payload)
self.__publish_flag = True
def update_cycle(self, _client, _userdata, _topic, payload):
self.cycle = self.__topicSubCycle.unserialize(payload)
self.__publish_flag = 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 update_status(self):
if len(self.schedules) == 0:
if self.state is not None:
self.state = TRAFFIC_SIGNAL.STATE.UNKNOWN
self.__publish_flag = True
else:
state = self.schedules[0].event
if self.state != state:
self.state = state
self.__publish_flag = True
def update_check_time(self):
self.__check_time = time() + TRAFFIC_SIGNAL.LOWER_LIMIT_RATE
if 0 < len(self.schedules):
self.__check_time = min(self.__check_time,
self.schedules[0].period.end)
def __main_loop(self):
while True:
if self.__check_time <= time():
self.__update_schedules()
self.update_status()
self.update_check_time()
if self.__publish_flag:
self.publish_status()
self.__check_time = time()
self.__publish_flag = False
sleep(self.__processing_cycle)
class FleetManager(EventLoop):
CONST = FLEET_MANAGER
def __init__(self, _id, name, waypoint, arrow, route, dt=3.0):
super().__init__(_id)
self.status = FleetStatus.new_data(name=name,
time=time(),
state=FLEET_MANAGER.STATE.LOG_IN,
relations={})
self.waypoint = waypoint
self.arrow = arrow
self.route = route
self.relation = Relation()
self.traffic_signals = self.manager.dict()
self.state_machine = None
self.dt = dt
self.__pubTopicStatus = Topic()
self.__pubTopicStatus.set_targets(self.target)
self.__pubTopicStatus.set_categories(
FLEET_MANAGER.TOPIC.CATEGORIES.STATUS)
self.__topicSubTrafficSignalStatus = Topic()
self.__topicSubTrafficSignalStatus.set_targets(
Target.new_target(None, TRAFFIC_SIGNAL.NODE_NAME), None)
self.__topicSubTrafficSignalStatus.set_categories(
TRAFFIC_SIGNAL.TOPIC.CATEGORIES.STATUS)
self.__topicSubTrafficSignalStatus.set_message(TrafficSignalStatus)
self.set_subscriber(self.__topicSubTrafficSignalStatus,
self.update_traffic_signal_status)
self.set_main_loop(self.__main_loop)
def publish_status(self):
self.status.relations = dict(
map(
lambda key:
(Target.get_code(key),
list(map(Target.get_code, self.relation.get_related(key)))),
self.relation.get_keys()))
payload = self.__pubTopicStatus.serialize(self.status)
self.publish(self.__pubTopicStatus, payload)
def update_traffic_signal_status(self, _client, _userdata, _topic,
payload):
traffic_signal = self.__topicSubTrafficSignalStatus.unserialize(
payload)
self.traffic_signals[traffic_signal["route_code"]] = traffic_signal
def update_status(self):
return
def __main_loop(self):
while self.status.state != FLEET_MANAGER.STATE.LOG_OUT:
sleep(self.dt)
self.update_status()
self.publish_status()
return True
class EventLoop(object):
CONST = EVENT_LOOP
def __init__(self, _id):
self.manager = Manager()
self.event_loop_id = _id
self.target = Target.new_target(self.event_loop_id,
self.__class__.__name__)
self.__subscribers = {}
self.__subscribers_lock = self.manager.Lock()
self.__publishers = {}
self.__client = None
self.__main_loop = None
self.__pid = os.getpid()
self.__topicPub = Topic()
self.__topicPub.set_targets(
Target.new_target(self.event_loop_id, EventLoop.__name__))
self.__topicPub.set_categories(EVENT_LOOP.TOPIC.CATEGORIES.RESPONSE)
self.__topicSub = Topic()
self.__topicSub.set_targets(
None, Target.new_target(self.event_loop_id, EventLoop.__name__))
self.__topicSub.set_categories(EVENT_LOOP.TOPIC.CATEGORIES.REQUEST)
self.__topicSub.set_message(EventLoopMessage)
self.set_subscriber(self.__topicSub, self.on_event_loop_message)
self.__user_data = None
self.__user_will = None
def __del__(self):
if self.__client is not None:
self.end()
@staticmethod
def get_message(event, pid):
return EventLoopMessage.new_data(time=time(), event=event, pid=pid)
def set_subscriber(self, topic, callback):
self.__subscribers_lock.acquire()
self.__subscribers[topic.get_path(use_wild_card=True)] = callback
self.__subscribers_lock.release()
def remove_subscriber(self, topic):
self.__subscribers_lock.acquire()
self.__subscribers.pop(topic.get_path(use_wild_card=True))
self.__subscribers_lock.release()
self.__client.unsubscribe(topic.get_path(use_wild_card=True))
def set_user_data(self, user_data):
self.__user_data = user_data
def set_main_loop(self, main_loop):
self.__main_loop = main_loop
def set_will(self, topic, payload):
self.__user_will = {"topic": topic, "payload": payload}
def publish(self, topic, payload, qos=0, retain=False):
self.__client.publish(topic.get_path(),
payload=payload,
qos=qos,
retain=retain)
def subscribe(self):
self.__subscribers_lock.acquire()
subscribe_keys = deepcopy(list(self.__subscribers.keys()))
self.__subscribers_lock.release()
for topic in subscribe_keys:
self.__client.subscribe(topic)
def response(self, request_path, payload, qos=0, retain=False):
response_topic = Topic()
response_topic.set_fix_path(
self.__topicPub.get_response_path(request_path))
self.publish(response_topic, payload, qos, retain)
def __on_connect(self, _client, _userdata, _flags, response_code):
if response_code == 0:
self.subscribe()
else:
logger.warning('connect status {0}'.format(response_code))
def on_event_loop_message(self, _client, _userdata, topic, payload):
event_loop_message = self.__topicSub.unserialize(payload)
if event_loop_message.event == EVENT_LOOP.STATE.START:
pass
if event_loop_message.event == EVENT_LOOP.ACTION.KILL:
self.end()
if event_loop_message.event == EVENT_LOOP.ACTION.CHECK:
self.__check(topic)
def __on_message(self, client, userdata, message_data):
try:
payload = message_data.payload.decode("utf-8")
self.__subscribers_lock.acquire()
for subscriber_path, onMessageFunction in self.__subscribers.items(
):
if Topic.is_path_matched(subscriber_path, message_data.topic):
onMessageFunction(client, userdata, message_data.topic,
payload)
self.__subscribers_lock.release()
except KeyboardInterrupt:
pass
except:
logger.error(traceback.format_exc())
finally:
return True
def ssl_setting(self, ca_path, client_path, key_path):
self.__client.tls_set(ca_path,
certfile=client_path,
keyfile=key_path,
tls_version=PROTOCOL_TLSv1_2)
self.__client.tls_insecure_set(True)
def connect(self,
host,
port,
ca_path=None,
client_path=None,
key_path=None):
self.__client = mqtt.Client(protocol=mqtt.MQTTv311,
userdata=self.__user_data)
if ca_path is not None and client_path is not None and key_path is not None:
self.ssl_setting(ca_path, client_path, key_path)
will = self.__user_will
if will is None:
event_loop_message = EventLoop.get_message(EVENT_LOOP.STATE.WILL,
self.__pid)
payload = self.__topicPub.serialize(event_loop_message)
will = {"topic": self.__topicPub.get_path(), "payload": payload}
self.__client.will_set(will["topic"],
payload=will["payload"],
qos=2,
retain=False)
self.__client.on_connect = self.__on_connect
self.__client.on_message = self.__on_message
self.__client.connect(host=host,
port=port,
keepalive=EVENT_LOOP.KEEP_ALIVE)
def start(self,
host="localhost",
port=1883,
ca_path=None,
client_path=None,
key_path=None):
try:
self.connect(host,
port,
ca_path=ca_path,
client_path=client_path,
key_path=key_path)
event_loop_message = EventLoop.get_message(EVENT_LOOP.STATE.START,
self.__pid)
payload = self.__topicPub.serialize(event_loop_message)
self.publish(self.__topicPub, payload)
if self.__main_loop is None:
self.__client.loop_forever()
else:
self.__client.loop_start()
self.__main_loop()
except KeyboardInterrupt:
pass
except:
logger.error(traceback.format_exc())
else:
pass
finally:
self.end()
pass
def end(self):
event_loop_message = EventLoop.get_message(EVENT_LOOP.STATE.DISCONNECT,
self.__pid)
payload = self.__topicPub.serialize(event_loop_message)
self.publish(self.__topicPub, payload)
if self.__main_loop is not None:
self.__client.loop_stop()
self.__client.disconnect()
self.__client = None
os.kill(self.__pid, SIGKILL)
def __check(self, request_path):
# todo: main_loop zombie
event_loop_message = EventLoop.get_message(EVENT_LOOP.RESPONSE.OK,
self.__pid)
payload = self.__topicPub.serialize(event_loop_message)
self.response(request_path, payload)
def get_pid(self):
return self.__pid
class SimTaxiFleet(FleetManager):
CONST = SIM_TAXI_FLEET
def __init__(self, _id, name, waypoint, arrow, route):
super().__init__(_id, name, waypoint, arrow, route)
self.waypoint = waypoint
self.arrow = arrow
self.route = route
self.user_statuses = self.manager.dict()
self.user_statuses_lock = self.manager.Lock()
self.vehicle_statuses = self.manager.dict()
self.vehicle_statuses_lock = self.manager.Lock()
self.user_schedules = {}
self.vehicle_schedules = {}
self.state_machines = {}
self.__topicPubUserSchedules = Topic()
self.__topicPubUserSchedules.set_categories(
FLEET_MANAGER.TOPIC.CATEGORIES.SCHEDULES)
self.__topicPubVehicleSchedules = Topic()
self.__topicPubVehicleSchedules.set_categories(
FLEET_MANAGER.TOPIC.CATEGORIES.SCHEDULES)
self.__topicSubUserStatus = Topic()
self.__topicSubUserStatus.set_targets(
Target.new_target(None, SIM_TAXI_USER.NODE_NAME), None)
self.__topicSubUserStatus.set_categories(USER.TOPIC.CATEGORIES.STATUS)
self.__topicSubUserStatus.set_message(UserStatus)
self.set_subscriber(self.__topicSubUserStatus, self.update_user_status)
self.__topicSubVehicleStatus = Topic()
self.__topicSubVehicleStatus.set_targets(
Target.new_target(None, SIM_TAXI.NODE_NAME), None)
self.__topicSubVehicleStatus.set_categories(
VEHICLE.TOPIC.CATEGORIES.STATUS)
self.__topicSubVehicleStatus.set_message(VehicleStatus)
self.set_subscriber(self.__topicSubVehicleStatus,
self.update_vehicle_status)
def __publish_user_schedules(self, user_id, payload):
self.__topicPubUserSchedules.set_targets(
self.target, Target.new_target(user_id, SIM_TAXI_USER.NODE_NAME))
self.publish(self.__topicPubUserSchedules, payload)
def __publish_vehicle_schedules(self, vehicle_id, payload):
self.__topicPubVehicleSchedules.set_targets(
self.target, Target.new_target(vehicle_id, SIM_TAXI.NODE_NAME))
self.publish(self.__topicPubVehicleSchedules, payload)
def update_user_status(self, _client, _userdata, topic, payload):
user_id = self.__topicSubUserStatus.get_from_id(topic)
user_status = self.__topicSubUserStatus.unserialize(payload)
self.user_statuses_lock.acquire()
if user_id in self.user_statuses or user_status.state == USER.STATE.LOG_IN:
self.user_statuses[user_id] = user_status
self.user_statuses_lock.release()
def update_vehicle_status(self, _client, _userdata, topic, payload):
vehicle_id = self.__topicSubVehicleStatus.get_from_id(topic)
vehicle_status = self.__topicSubVehicleStatus.unserialize(payload)
self.vehicle_statuses_lock.acquire()
self.vehicle_statuses[vehicle_id] = vehicle_status
self.vehicle_statuses_lock.release()
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 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 get_dispatchable_vehicle_ids(self, user_status):
dispatchable_vehicle_ids = list(
filter(
lambda x: self.waypoint.get_geohash(self.vehicle_schedules[x][
-1].route.goal_waypoint_id)[:SIM_TAXI_FLEET.
DISPATCHABLE_GEOHASH_DIGIT] ==
self.waypoint.get_geohash(user_status.trip_schedules[
0].route.start_waypoint_id)[:SIM_TAXI_FLEET.
DISPATCHABLE_GEOHASH_DIGIT],
self.vehicle_schedules.keys()))
return dispatchable_vehicle_ids
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_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_pickup_route(self, user_status):
start_point = {
"arrow_code": user_status.trip_schedules[0].route.arrow_codes[0],
"waypoint_id":
user_status.trip_schedules[0].route.start_waypoint_id,
}
vehicle_ids = self.get_dispatchable_vehicle_ids(user_status)
if len(vehicle_ids) == 0:
logger.warning("no dispatchable vehicles")
return None, None
goal_points = []
for vehicle_id, goal_waypoint_id, goal_arrow_code in map(
lambda x:
(x, self.vehicle_schedules[x][-1].route.goal_waypoint_id, self.
vehicle_schedules[x][-1].route.arrow_codes[-1]), vehicle_ids):
goal_points.append({
"goal_id": vehicle_id,
"arrow_code": goal_arrow_code,
"waypoint_id": goal_waypoint_id,
})
routes = self.route.get_shortest_routes(start_point,
goal_points,
reverse=True)
if len(routes) == 0:
logger.warning("no pickup_route")
return None, None
pickup_route = min(routes.items(),
key=lambda x: x[1]["cost"] + self.vehicle_schedules[
x[0]][-1].period.end)[1]
vehicle_id = pickup_route.pop("goal_id")
pickup_route.pop("cost")
return pickup_route, vehicle_id
def get_carry_route(self, user_status):
start_point = {
"arrow_code": user_status.trip_schedules[0].route.arrow_codes[0],
"waypoint_id":
user_status.trip_schedules[0].route.start_waypoint_id,
}
goal_points = [{
"goal_id":
None,
"arrow_code":
user_status.trip_schedules[0].route.arrow_codes[-1],
"waypoint_id":
user_status.trip_schedules[0].route.goal_waypoint_id,
}]
routes = self.route.get_shortest_routes(start_point,
goal_points,
reverse=False)
if len(routes) == 0:
logger.warning("cant carry_route")
return None
carry_route = routes[None]
carry_route.pop("cost")
carry_route.pop("goal_id")
return carry_route
def get_deploy_route(self):
pass
@staticmethod
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]
@staticmethod
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]
@staticmethod
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_state_machine(
self, initial_state=SIM_TAXI_FLEET.STATE.WAITING_FOR_USER_LOG_IN):
machine = StateMachine(
states=list(SIM_TAXI_FLEET.STATE),
initial=initial_state,
)
machine.add_transitions([
{
"trigger":
SIM_TAXI_FLEET.TRIGGER.WAIT_USER_REQUEST,
"source":
SIM_TAXI_FLEET.STATE.WAITING_FOR_USER_LOG_IN,
"dest":
SIM_TAXI_FLEET.STATE.WAITING_FOR_USER_REQUEST,
"conditions":
[self.condition_user_state_and_publish_user_request_schedules]
},
{
"trigger":
SIM_TAXI_FLEET.TRIGGER.DISPATCH,
"source":
SIM_TAXI_FLEET.STATE.WAITING_FOR_USER_REQUEST,
"dest":
SIM_TAXI_FLEET.STATE.WAITING_FOR_TAXI_ARRIVE_AT_USER_LOCATION,
"conditions":
[self.condition_user_state_and_publish_new_taxi_schedules]
},
{
"trigger":
SIM_TAXI_FLEET.TRIGGER.NOTICE,
"source":
SIM_TAXI_FLEET.STATE.WAITING_FOR_TAXI_ARRIVE_AT_USER_LOCATION,
"dest":
SIM_TAXI_FLEET.STATE.WAITING_FOR_USER_GETTING_ON,
"conditions":
[self.condition_user_vehicle_state_and_publish_user_schedules]
},
{
"trigger":
SIM_TAXI_FLEET.TRIGGER.WAIT_TAXI_ARRIVAL,
"source":
SIM_TAXI_FLEET.STATE.WAITING_FOR_USER_GETTING_ON,
"dest":
SIM_TAXI_FLEET.STATE.
WAITING_FOR_TAXI_ARRIVE_AT_USER_DESTINATION,
"conditions":
[self.condition_user_state_and_publish_updated_taxi_schedules]
},
{
"trigger":
SIM_TAXI_FLEET.TRIGGER.NOTICE,
"source":
SIM_TAXI_FLEET.STATE.
WAITING_FOR_TAXI_ARRIVE_AT_USER_DESTINATION,
"dest":
SIM_TAXI_FLEET.STATE.WAITING_FOR_USER_GETTING_OUT,
"conditions":
[self.condition_user_vehicle_state_and_publish_user_schedules]
},
])
return machine
def after_state_change_publish_user_schedules(self, user_id):
self.__publish_user_schedules(
user_id,
self.__topicPubUserSchedules.serialize(
self.user_schedules[user_id]))
return True
def after_state_change_publish_vehicle_schedules(self, vehicle_id):
self.__publish_vehicle_schedules(
vehicle_id,
self.__topicPubVehicleSchedules.serialize(
self.vehicle_schedules[vehicle_id]))
return True
def after_state_change_add_relation(self, user_id, vehicle_id):
self.relation.add_relation(
Target.new_target(vehicle_id, SIM_TAXI.NODE_NAME),
Target.new_target(user_id, SIM_TAXI_USER.NODE_NAME))
return True
@staticmethod
def condition_user_state(user_status, expected_state):
return user_status.state == expected_state
def condition_user_state_and_publish_user_request_schedules(
self, user_id, user_status, excepted_state, current_time):
if self.condition_user_state(user_status, excepted_state):
self.user_schedules[user_id] = self.get_user_request_schedules(
user_id, current_time)
self.after_state_change_publish_user_schedules(user_id)
return True
return False
def condition_user_state_and_publish_new_taxi_schedules(
self, user_id, user_status, excepted_state, current_time):
if self.condition_user_state(user_status, excepted_state):
vehicle_id, vehicle_schedules, user_schedules = self.get_taxi_schedules(
user_id, user_status, current_time)
if vehicle_id is not None:
self.vehicle_schedules[vehicle_id] = vehicle_schedules
self.user_schedules[user_id] = user_schedules
self.after_state_change_publish_vehicle_schedules(vehicle_id)
self.after_state_change_publish_user_schedules(user_id)
self.after_state_change_add_relation(user_id, vehicle_id)
return True
return False
def condition_user_state_and_publish_updated_taxi_schedules(
self, user_id, user_status, excepted_state, vehicle_id,
current_time):
if self.condition_user_state(user_status, excepted_state):
self.user_schedules[user_id][0].period.end = current_time
self.vehicle_schedules[vehicle_id][0].period.end = current_time
self.after_state_change_publish_vehicle_schedules(vehicle_id)
self.after_state_change_publish_user_schedules(user_id)
return True
return False
@staticmethod
def condition_user_vehicle_state(user_status, vehicle_status,
expected_states):
return [user_status.state, vehicle_status.state] == expected_states
def condition_user_vehicle_state_and_publish_user_schedules(
self, user_id, user_status, vehicle_status, expected_states,
current_time):
if self.condition_user_vehicle_state(user_status, vehicle_status,
expected_states):
self.user_schedules[user_id][0].period.end = current_time
self.after_state_change_publish_user_schedules(user_id)
return True
return False
def get_user_statuses_and_lock(self):
self.user_statuses_lock.acquire()
return deepcopy(self.user_statuses)
def set_user_statuses_and_unlock(self, user_statuses):
self.user_statuses.clear()
self.user_statuses.update(user_statuses)
self.user_statuses_lock.release()
def get_vehicle_statuses_and_lock(self):
self.vehicle_statuses_lock.acquire()
return deepcopy(self.vehicle_statuses)
def set_vehicle_statuses_and_unlock(self, vehicle_statuses):
self.vehicle_statuses.clear()
self.vehicle_statuses.update(vehicle_statuses)
self.vehicle_statuses_lock.release()
def cleanup_status(self, user_statuses):
user_ids = []
for user_id, user_status in user_statuses.items():
if SIM_TAXI_FLEET.TIMEOUT < time() - user_status.time:
user_ids.append(user_id)
for user_id in user_ids:
self.relation.remove_relations_of(
Target.new_target(user_id, SIM_TAXI_USER.NODE_NAME))
user_statuses.pop(user_id)
self.user_schedules.pop(user_id)
def update_status(self):
user_statuses = self.get_user_statuses_and_lock()
vehicle_statuses = self.get_vehicle_statuses_and_lock()
self.update_user_schedules(user_statuses)
self.update_vehicle_schedules(vehicle_statuses)
self.cleanup_status(user_statuses)
self.update_state_machines(user_statuses, vehicle_statuses)
self.set_user_statuses_and_unlock(user_statuses)
self.set_vehicle_statuses_and_unlock(vehicle_statuses)
def update_state_machines(self, user_statuses, vehicle_statuses):
current_time = time()
remove_user_ids = []
for user_id in user_statuses:
if user_id not in self.state_machines:
self.state_machines[user_id] = self.get_state_machine()
user_status = user_statuses[user_id]
target_vehicles = self.relation.get_related(
Target.new_target(user_id, SIM_TAXI_USER.NODE_NAME))
state = self.state_machines[user_id].state
if len(target_vehicles) == 0:
if state == SIM_TAXI_FLEET.STATE.WAITING_FOR_USER_LOG_IN:
self.state_machines[user_id].wait_user_request(
user_id, user_status, USER.STATE.LOG_IN, current_time)
elif state == SIM_TAXI_FLEET.STATE.WAITING_FOR_USER_REQUEST:
self.state_machines[user_id].dispatch(
user_id, user_status, SIM_TAXI_USER.STATE.CALLING,
current_time)
elif len(target_vehicles) == 1:
vehicle_id = target_vehicles[0].id
if user_id in map(
lambda x: x.id,
self.vehicle_schedules[vehicle_id][0].targets):
vehicle_status = vehicle_statuses[vehicle_id]
if state == SIM_TAXI_FLEET.STATE.WAITING_FOR_TAXI_ARRIVE_AT_USER_LOCATION:
self.state_machines[user_id].notice(
user_id, user_status, vehicle_status, [
SIM_TAXI_USER.STATE.WAITING,
SIM_TAXI.STATE.STOP_FOR_PICKING_UP
], current_time)
elif state == SIM_TAXI_FLEET.STATE.WAITING_FOR_USER_GETTING_ON:
self.state_machines[user_id].wait_taxi_arrival(
user_id, user_status, SIM_TAXI_USER.STATE.GOT_ON,
vehicle_id, current_time)
elif state == SIM_TAXI_FLEET.STATE.WAITING_FOR_TAXI_ARRIVE_AT_USER_DESTINATION:
self.state_machines[user_id].notice(
user_id, user_status, vehicle_status, [
SIM_TAXI_USER.STATE.MOVING,
SIM_TAXI.STATE.STOP_FOR_DISCHARGING
], current_time)
else:
pass
else:
logger.error(
pformat({"target_vehicles length": target_vehicles}))
if state == SIM_TAXI_FLEET.STATE.WAITING_FOR_USER_GETTING_OUT:
if user_status.state in [
SIM_TAXI_USER.STATE.GOT_OUT, USER.STATE.LOG_OUT
]:
remove_user_ids.append(user_id)
for user_id in remove_user_ids:
self.relation.remove_relations_of(
Target.new_target(user_id, SIM_TAXI_USER.NODE_NAME))
self.state_machines.pop(user_id)
user_statuses.pop(user_id)
self.user_schedules.pop(user_id)
logger.info(
pformat({
"fleet":
dict(
map(lambda x: (x[0], x[1].state),
self.state_machines.items())),
"user":
dict(map(lambda x: (x[0], x[1].state), user_statuses.items())),
"vehicle":
dict(
map(lambda x: (x[0], x[1].state),
vehicle_statuses.items())),
}))
class FleetManager(EventLoop):
class ACTION(object):
PUBLISH_RELATIONS = "pub_relations"
class TOPIC(object):
PUBLISH = "pub_fleet_manager"
SUBSCRIBE = "sub_fleet_manager"
def __init__(self, waypoint, arrow, route):
super().__init__()
self.topicUserPublish = Topic()
self.topicUserPublish.set_root(User.TOPIC.PUBLISH)
self.topicUserPublish.set_message(user_message)
self.topicUserSubscribe = Topic()
self.topicUserSubscribe.set_root(User.TOPIC.SUBSCRIBE)
self.topicUserSubscribe.set_message(user_message)
self.topicVehiclePublish = Topic()
self.topicVehiclePublish.set_root(Vehicle.TOPIC.PUBLISH)
self.topicVehiclePublish.set_message(vehicle_message)
self.topicVehicleSubscribe = Topic()
self.topicVehicleSubscribe.set_root(Vehicle.TOPIC.SUBSCRIBE)
self.topicVehicleSubscribe.set_message(vehicle_message)
self.topicTrafficSignalPublish = Topic()
self.topicTrafficSignalPublish.set_root(TrafficSignal.TOPIC.PUBLISH)
self.topicTrafficSignalPublish.set_message(traffic_signal_message)
self.topicFleetManagerPublish = Topic()
self.topicFleetManagerPublish.set_id(self.event_loop_id)
self.topicFleetManagerPublish.set_root(FleetManager.TOPIC.PUBLISH)
self.topicFleetManagerPublish.set_message(fleet_manager_message)
self.topicFleetManagerSubscribe = Topic()
self.topicFleetManagerSubscribe.set_id(self.event_loop_id)
self.topicFleetManagerSubscribe.set_root(FleetManager.TOPIC.SUBSCRIBE)
self.topicFleetManagerSubscribe.set_message(fleet_manager_message)
self.waypoint = waypoint
self.arrow = arrow
self.route = route
self.users = {}
self.vehicles = {}
self.traffic_signals = {}
self.relations = {} # vehicle_id -> user_id, user_id -> vehicle_id
self.add_on_message_function(self.update_user_status)
self.add_on_message_function(self.update_vehicle_status)
self.add_on_message_function(self.update_traffic_signal_status)
self.add_on_message_function(self.response_request)
self.set_subscriber(self.topicUserPublish.all)
self.set_subscriber(self.topicVehiclePublish.all)
self.set_subscriber(self.topicTrafficSignalPublish.all)
self.set_subscriber(self.topicFleetManagerSubscribe.all)
def update_user_status(self, _client, _userdata, topic, payload):
# print("update_user_status", topic)
if self.topicUserPublish.root in topic:
user_id = self.topicUserPublish.get_id(topic)
message = self.topicUserPublish.unserialize(payload)
if message["state"] == User.STATE.LOGIN:
# print("user", User.STATE.LOGIN)
# todo: move to dispatcher class
self.dispatch(user_id, message)
elif message["state"] == User.STATE.WAITING:
pass
# print("user", User.STATE.WAITING)
elif message["state"] == User.STATE.GETTING_ON:
pass
# print("user", User.STATE.GETTING_ON)
elif message["state"] == User.STATE.GOT_ON:
# print("user", User.STATE.GOT_ON)
vehicle_id = self.relations[user_id]
self.vehicles[vehicle_id]["schedules"][0][
"action"] = Vehicle.ACTION.MOVE
payload = self.topicVehicleSubscribe.serialize(
{"schedules": self.vehicles[vehicle_id]["schedules"]})
self.publish(
self.topicVehicleSubscribe.root + "/" + vehicle_id +
"/schedules", payload)
elif message["state"] == User.STATE.MOVING:
pass
# print("user", User.STATE.MOVING)
elif message["state"] == User.STATE.GETTING_OUT:
pass
# print("user", User.STATE.GETTING_OUT)
elif message["state"] == User.STATE.GOT_OUT:
pass
# print("user", User.STATE.GOT_OUT)
else:
print("user", message["state"])
self.users[user_id] = message
def update_vehicle_status(self, _client, _userdata, topic, payload):
if self.topicVehiclePublish.root in topic:
vehicle_id = self.topicVehiclePublish.get_id(topic)
message = self.topicVehiclePublish.unserialize(payload)
# print("update_vehicle_status", topic, message["state"])
if vehicle_id in self.relations:
prev_state = self.vehicles[vehicle_id]["state"]
if message["state"] == SimTaxi.STATE.MOVE_TO_USER:
# print("vehicle", SimTaxi.STATE.MOVE_TO_USER)
if prev_state == SimTaxi.STATE.STANDBY:
user_id = self.relations[vehicle_id]
self.users[user_id]["schedules"][0][
"action"] = User.ACTION.WAIT
self.publish(
self.topicUserSubscribe.root + "/" + user_id +
"/schedules",
self.topicUserSubscribe.serialize({
"schedules":
self.users[user_id]["schedules"]
}))
elif message["state"] == SimTaxi.STATE.STOP_FOR_PICKING_UP:
# print("vehicle", SimTaxi.STATE.STOP_FOR_PICKING_UP)
if prev_state == SimTaxi.STATE.MOVE_TO_USER:
# print("vehicle", SimTaxi.STATE.STOP_FOR_PICKING_UP, SimTaxi.STATE.MOVE_TO_USER)
user_id = self.relations[vehicle_id]
self.users[user_id]["schedules"][0][
"action"] = User.ACTION.GET_ON
self.publish(
self.topicUserSubscribe.root + "/" + user_id +
"/schedules",
self.topicUserSubscribe.serialize({
"schedules":
self.users[user_id]["schedules"]
}))
elif message[
"state"] == SimTaxi.STATE.MOVE_TO_USER_DESTINATION:
# print("vehicle", SimTaxi.STATE.MOVE_TO_USER_DESTINATION)
if prev_state == SimTaxi.STATE.STOP_FOR_PICKING_UP:
# print("vehicle", SimTaxi.STATE.MOVE_TO_USER_DESTINATION, SimTaxi.STATE.STOP_FOR_PICKING_UP)
user_id = self.relations[vehicle_id]
self.users[user_id]["schedules"][0][
"event"] = User.EVENT.MOVE_VEHICLE
self.publish(
self.topicUserSubscribe.root + "/" + user_id +
"/event",
self.topicUserSubscribe.serialize(
{"event": User.EVENT.MOVE_VEHICLE}))
elif message["state"] == SimTaxi.STATE.STOP_FOR_DISCHARGING:
# print("vehicle", SimTaxi.STATE.STOP_FOR_DISCHARGING)
if prev_state == SimTaxi.STATE.MOVE_TO_USER_DESTINATION:
user_id = self.relations[vehicle_id]
self.users[user_id]["schedules"][0][
"action"] = User.ACTION.GET_OUT
self.publish(
self.topicUserSubscribe.root + "/" + user_id +
"/schedules",
self.topicUserSubscribe.serialize({
"schedules":
self.users[user_id]["schedules"]
}))
elif message["state"] == SimTaxi.STATE.MOVE_TO_STANDBY:
pass
# print("vehicle", SimTaxi.STATE.MOVE_TO_STANDBY)
elif message["state"] == SimTaxi.STATE.STANDBY:
pass
# print("vehicle", SimTaxi.STATE.STANDBY)
else:
print("vehicle", message["state"])
# vehicleSchedule = vehicle.pop("schedule")
if vehicle_id not in self.vehicles:
# print("set vehicle", vehicle_id, message["name"])
self.vehicles[vehicle_id] = message
else:
# print("update vehicle", vehicle_id, message["name"])
self.vehicles[vehicle_id].update(message)
def update_traffic_signal_status(self, _client, _userdata, topic, payload):
if self.topicTrafficSignalPublish.root in topic:
message = self.topicTrafficSignalPublish.unserialize(payload)
for route in message["routes"]:
self.traffic_signals[route["route_code"]] = route
def response_request(self, _client, _userdata, topic, payload):
if self.topicFleetManagerSubscribe.root in topic:
message = self.topicFleetManagerSubscribe.unserialize(payload)
if message["action"] == FleetManager.ACTION.PUBLISH_RELATIONS:
self.publish_relations()
def get_dispatchable_vehicles(self):
return dict(
filter(lambda x: x[1]["state"] in [SimTaxi.STATE.STANDBY],
self.vehicles.items()))
def dispatch(self, user_id, user_status):
start_point = {
"arrow_code":
self.arrow.get_arrow_codes_from_waypoint_id(
user_status["schedules"][0]["start"]["waypoint_id"])[0],
"waypoint_id":
user_status["schedules"][0]["start"]["waypoint_id"],
}
vehicles = self.get_dispatchable_vehicles()
if len(vehicles) == 0:
print("no dispatchable vehicles")
return
goal_points = []
for vehicle_id, goal_waypoint_id in map(
lambda x: (x[0], x[1]["location"]["waypoint_id"]),
vehicles.items()):
goal_points.append({
"goal_id":
vehicle_id,
"arrow_code":
self.arrow.get_arrow_codes_from_waypoint_id(goal_waypoint_id)
[0],
"waypoint_id":
goal_waypoint_id,
})
routes = self.route.get_shortest_routes(start_point,
goal_points,
reverse=True)
if len(routes) == 0:
print("no pick_up_route")
return
pick_up_route = min(routes.items(), key=lambda x: x[1]["cost"])[1]
vehicle_id = pick_up_route["goal_id"]
start_point = {
"arrow_code":
self.arrow.get_arrow_codes_from_waypoint_id(
user_status["schedules"][0]["start"]["waypoint_id"])[0],
"waypoint_id":
user_status["schedules"][0]["start"]["waypoint_id"],
}
goal_points = [{
"goal_id":
user_id,
"arrow_code":
self.arrow.get_arrow_codes_from_waypoint_id(
user_status["schedules"][0]["goal"]["waypoint_id"])[0],
"waypoint_id":
user_status["schedules"][0]["goal"]["waypoint_id"],
}]
routes = self.route.get_shortest_routes(start_point,
goal_points,
reverse=False)
if len(routes) == 0:
print("cant carry_route")
return
carry_route = min(routes.items(), key=lambda x: x[1]["cost"])[1]
current_time = time()
vehicle_schedule = deepcopy(
self.topicVehicleSubscribe.get_template()["schedules"][0])
vehicle_schedule.update({
"name": "pickup",
"start_time": current_time,
"duration": 1000,
"action": Vehicle.ACTION.MOVE,
"route": {
"start": {
"waypoint_id": pick_up_route["goal_waypoint_id"],
},
"goal": {
"waypoint_id": pick_up_route["start_waypoint_id"],
},
"arrow_codes": pick_up_route["arrow_codes"],
}
})
self.vehicles[vehicle_id]["schedules"].append(vehicle_schedule)
vehicle_schedule = deepcopy(
self.topicVehicleSubscribe.get_template()["schedules"][0])
vehicle_schedule.update({
"name": "takeOn",
"start_time": current_time + 1000,
"duration": 10,
"action": Vehicle.ACTION.STOP,
})
self.vehicles[vehicle_id]["schedules"].append(vehicle_schedule)
vehicle_schedule = deepcopy(
self.topicVehicleSubscribe.get_template()["schedules"][0])
vehicle_schedule.update({
"name": "carry",
"start_time": current_time + 1010,
"duration": 1000,
"action": Vehicle.ACTION.MOVE,
"route": {
"start": {
"waypoint_id": carry_route["start_waypoint_id"],
},
"goal": {
"waypoint_id": carry_route["goal_waypoint_id"],
},
"arrow_codes": carry_route["arrow_codes"],
}
})
self.vehicles[vehicle_id]["schedules"].append(vehicle_schedule)
vehicle_schedule = deepcopy(
self.topicVehicleSubscribe.get_template()["schedules"][0])
vehicle_schedule.update({
"name": "discharge",
"start_time": current_time + 2010,
"duration": 10,
"action": Vehicle.ACTION.STOP,
})
self.vehicles[vehicle_id]["schedules"].append(vehicle_schedule)
vehicle_schedule = deepcopy(
self.topicVehicleSubscribe.get_template()["schedules"][0])
vehicle_schedule.update({
"name": "standBy",
"start_time": current_time + 2020,
"duration": 86400,
"action": SimTaxi.ACTION.STANDBY,
})
self.vehicles[vehicle_id]["schedules"].append(vehicle_schedule)
payload = self.topicVehicleSubscribe.serialize(
{"schedules": self.vehicles[vehicle_id]["schedules"]})
self.publish(
self.topicVehicleSubscribe.root + "/" + vehicle_id + "/schedules",
payload)
self.relations[user_id] = vehicle_id
self.relations[vehicle_id] = user_id
self.publish_relations()
def publish_relations(self):
message = self.topicFleetManagerPublish.get_template()
message["time"] = time()
message["relations"] = self.relations
payload = self.topicFleetManagerPublish.serialize(message)
self.publish(self.topicFleetManagerPublish.private, payload)
class Autoware(Vehicle):
class ROSTOPIC(object):
PUBLISH = "/based/lane_waypoints_array"
SUBSCRIBE = "/closest_waypoint"
class TOPIC(object):
PUBLISH = "pub_autoware"
SUBSCRIBE = "sub_autoware"
def __init__(self,
name,
waypoint,
arrow,
route,
waypoint_id,
velocity,
schedules=None,
dt=1.0):
super().__init__(name, waypoint, arrow, route, waypoint_id, velocity,
schedules, dt)
self.name = name
self.autowarePublishTopic = Topic()
self.autowarePublishTopic.set_id(self.name)
self.autowarePublishTopic.set_root(Autoware.TOPIC.PUBLISH)
self.autowarePublishTopic.set_message(autoware_message)
self.autowareSubscribeTopic = Topic()
self.autowareSubscribeTopic.set_id(self.name)
self.autowareSubscribeTopic.set_root(Autoware.TOPIC.SUBSCRIBE)
self.autowareSubscribeTopic.set_message(autoware_message)
self.pose_index = 0
self.current_poses = []
self.add_on_message_function(self.set_autoware_pose)
self.set_subscriber(self.autowareSubscribeTopic.private +
"/closest_waypoint")
def set_autoware_pose(self, _client, _userdata, topic, payload):
if topic == self.autowareSubscribeTopic.private + "/closest_waypoint":
message = self.autowareSubscribeTopic.unserialize(payload)
if 0 <= message["index"] < len(self.current_poses):
self.pose_index = message["index"]
print(self.current_poses[self.pose_index])
self.arrow_code = self.current_poses[
self.pose_index]["arrow_code"]
self.waypoint_id = self.current_poses[
self.pose_index]["waypoint_id"]
self.position = self.waypoint.get_position(self.waypoint_id)
self.yaw = self.arrow.get_heading(self.arrow_code,
self.waypoint_id)
else:
print("Lost Autoware.")
def set_autoware_waypoints(self):
waypoints = []
schedule = self.schedules[0]
arrow_waypoint_array = self.route.get_arrow_waypoint_array({
"start_waypoint_id":
schedule["route"]["start"]["waypoint_id"],
"goal_waypoint_id":
schedule["route"]["goal"]["waypoint_id"],
"arrow_codes":
schedule["route"]["arrow_codes"]
})
for arrowWaypoint in arrow_waypoint_array:
waypoint_id = arrowWaypoint["waypoint_id"]
waypoints.append({
"position":
dict(
zip(["x", "y", "z"],
self.waypoint.get_position(waypoint_id))),
"orientation":
dict(
zip(["w", "x", "y", "z"],
axangle2quat([0, 0, 1],
self.waypoint.get_yaw(waypoint_id)))),
"velocity":
2.0
})
if 0 < len(waypoints):
num = min(10, len(waypoints))
for i in range(num - 1, 0, -1):
waypoints[-i]["velocity"] = (
i / num) * waypoints[-i - 1]["velocity"]
self.current_poses = arrow_waypoint_array
payload = self.autowarePublishTopic.serialize(waypoints)
self.publish(self.autowarePublishTopic.private + "/waypoints",
payload)
class Vehicle(EventLoop):
class TOPIC(object):
PUBLISH = "pub_vehicle"
SUBSCRIBE = "sub_vehicle"
class GEO(object):
PUBLISH = "pub_geo_vehicle"
SUBSCRIBE = "sub_geo_vehicle"
class STATE(object):
MOVE = "move"
STOP = "stop"
WILL = "will"
class ACTION(object):
MOVE = "move"
STOP = "stop"
def __init__(self, name, waypoint, arrow, route, waypoint_id, velocity, schedules=None, dt=1.0):
super().__init__()
self.topicVehiclePublish = Topic()
self.topicVehiclePublish.set_id(self.event_loop_id)
self.topicVehiclePublish.set_root(Vehicle.TOPIC.PUBLISH)
self.topicVehiclePublish.set_message(vehicle_message)
self.topicVehicleSubscribe = Topic()
self.topicVehicleSubscribe.set_id(self.event_loop_id)
self.topicVehicleSubscribe.set_root(Vehicle.TOPIC.SUBSCRIBE)
self.topicVehicleSubscribe.set_message(vehicle_message)
self.topicGeoVehiclePublish = Topic()
self.topicGeoVehiclePublish.set_id(self.event_loop_id)
self.topicGeoVehiclePublish.set_root(Vehicle.TOPIC.GEO.PUBLISH)
self.topicGeoVehiclePublish.set_message(geo_vehicle_message)
self.name = name
self.state = Vehicle.STATE.STOP
self.event = None
self.action = None
self.waypoint = waypoint
self.arrow = arrow
self.route = route
self.waypoint_id = waypoint_id
self.position = self.waypoint.get_position(self.waypoint_id)
self.velocity = velocity
self.schedules = schedules
self.dt = dt
self.arrow_code = self.arrow.get_arrow_codes_from_waypoint_id(waypoint_id)[0]
self.position = self.waypoint.get_position(self.waypoint_id)
self.yaw = self.arrow.get_heading(self.arrow_code, self.waypoint_id)
self.add_on_message_function(self.update_schedules)
self.add_on_message_function(self.update_event)
self.set_subscriber(self.topicVehicleSubscribe.private+"/schedules")
self.set_subscriber(self.topicVehicleSubscribe.private+"/event")
self.set_main_loop(self.__main_loop)
def publish_status(self):
xyz = self.position.data[:]
message = self.topicVehiclePublish.get_template()
message["name"] = self.name
message["state"] = self.state
message["event"] = self.event
message["location"]["waypoint_id"] = self.waypoint_id
message["location"]["geohash"] = self.waypoint.get_geohash(self.waypoint_id)
message["location"]["arrow_code"] = self.arrow_code
message["pose"]["position"]["x"] = xyz[0]
message["pose"]["position"]["y"] = xyz[1]
message["pose"]["position"]["z"] = xyz[2]
message["pose"]["orientation"]["yaw"] = self.yaw
message["schedules"] = self.schedules
payload = self.topicVehiclePublish.serialize(message)
self.publish(self.topicVehiclePublish.private, payload)
self.publish(
self.topicGeoVehiclePublish.root+"/"+"/".join(self.waypoint.get_geohash(self.waypoint_id)),
self.event_loop_id)
def update_schedules(self, _client, _userdata, topic, payload):
if topic == self.topicVehicleSubscribe.private+"/schedules":
message = self.topicVehicleSubscribe.unserialize(payload)
self.schedules = message["schedules"]
def update_event(self, _client, _userdata, topic, payload):
if topic == self.topicVehicleSubscribe.private+"/event":
message = self.topicVehicleSubscribe.unserialize(payload)
self.event = message["event"]
def update_status(self):
return
def __main_loop(self):
sleep(1)
self.publish_status()
while self.schedules is not None:
sleep(self.dt)
self.update_status()
self.publish_status()
return True
class Autoware(Vehicle):
CONST = AUTOWARE
def __init__(self, _id, name, waypoint, arrow, route, dt=0.5):
super().__init__(_id, name, waypoint, arrow, route, dt=dt)
self.upper_distance_from_stopline = AUTOWARE.DEFAULT_UPPER_DISTANCE_FROM_STOPLINE
self.state_machine = self.get_state_machine()
self.__map_match = MapMatch()
self.__map_match.set_waypoint(self.waypoint)
self.__map_match.set_arrow(self.arrow)
self.ros_closest_waypoint = None
self.ros_closest_waypoint_lock = self.manager.Lock()
self.ros_current_pose = None
self.ros_current_pose_lock = self.manager.Lock()
self.ros_decisionmaker_states = None
self.ros_decisionmaker_states_lock = self.manager.Lock()
self.current_locations = []
self.traffic_signals = self.manager.dict()
self.traffic_signals_lock = self.manager.Lock()
self.__previous_state_command = None
self.__topicPubBasedLaneWaypointsArray = Topic()
self.__topicPubBasedLaneWaypointsArray.set_targets(
self.target, Target.new_target(self.target.id, AUTOWARE.TOPIC.ROS_NODE_NAME))
self.__topicPubBasedLaneWaypointsArray.set_categories(AUTOWARE.TOPIC.CATEGORIES.BASED_LANE_WAYPOINTS_ARRAY)
self.__topicPubStateCmd = Topic()
self.__topicPubStateCmd.set_targets(
self.target, Target.new_target(self.target.id, AUTOWARE.TOPIC.ROS_NODE_NAME))
self.__topicPubStateCmd.set_categories(AUTOWARE.TOPIC.CATEGORIES.STATE_CMD)
self.__topicPubLightColor = Topic()
self.__topicPubLightColor.set_targets(
self.target, Target.new_target(self.target.id, AUTOWARE.TOPIC.ROS_NODE_NAME))
self.__topicPubLightColor.set_categories(AUTOWARE.TOPIC.CATEGORIES.LIGHT_COLOR)
self.__topicSubCurrentPose = Topic()
self.__topicSubCurrentPose.set_targets(
Target.new_target(self.target.id, AUTOWARE.TOPIC.ROS_NODE_NAME), self.target)
self.__topicSubCurrentPose.set_categories(AUTOWARE.TOPIC.CATEGORIES.CURRENT_POSE)
self.__topicSubCurrentPose.set_message(ROSMessage.CurrentPose)
self.set_subscriber(self.__topicSubCurrentPose, self.update_current_pose)
self.__topicSubClosestWaypoint = Topic()
self.__topicSubClosestWaypoint.set_targets(
Target.new_target(self.target.id, AUTOWARE.TOPIC.ROS_NODE_NAME), self.target)
self.__topicSubClosestWaypoint.set_categories(AUTOWARE.TOPIC.CATEGORIES.CLOSEST_WAYPOINT)
self.__topicSubClosestWaypoint.set_message(ROSMessage.ClosestWaypoint)
self.set_subscriber(self.__topicSubClosestWaypoint, self.update_closest_waypoint)
self.__topicSubDecisionMakerStates = Topic()
self.__topicSubDecisionMakerStates.set_targets(
Target.new_target(self.target.id, AUTOWARE.TOPIC.ROS_NODE_NAME), self.target)
self.__topicSubDecisionMakerStates.set_categories(AUTOWARE.TOPIC.CATEGORIES.DECISION_MAKER_STATES)
self.__topicSubDecisionMakerStates.set_message(ROSMessage.DecisionMakerStates)
self.set_subscriber(self.__topicSubDecisionMakerStates, self.update_decisionmaker_states)
self.__topicSubTrafficSignalStatus = Topic()
self.__topicSubTrafficSignalStatus.set_targets(Target.new_target(None, TRAFFIC_SIGNAL.NODE_NAME))
self.__topicSubTrafficSignalStatus.set_categories(TRAFFIC_SIGNAL.TOPIC.CATEGORIES.STATUS)
self.__topicSubTrafficSignalStatus.set_message(TrafficSignalStatus)
self.set_subscriber(self.__topicSubTrafficSignalStatus, self.update_traffic_signals)
def set_upper_distance_from_stopline(self, distance_from_stopline):
self.upper_distance_from_stopline = distance_from_stopline
def publish_lane_array(self, route):
locations = self.route.get_locations(route)
ros_lane_array = self.get_ros_lane_array_from_locations(locations)
if ros_lane_array is not None:
self.current_locations = locations
payload = self.__topicPubBasedLaneWaypointsArray.serialize(ros_lane_array)
self.publish(self.__topicPubBasedLaneWaypointsArray, payload)
def publish_state_command(self, state_command):
if True:
payload = self.__topicPubStateCmd.serialize(state_command)
self.__previous_state_command = state_command
self.publish(self.__topicPubStateCmd, payload)
def publish_init_state_command(self, decisionmaker_states):
if decisionmaker_states.main_state != AUTOWARE.ROS.DECISION_MAKER_STATES.MAIN.INITIAL:
state_command = ROSMessage.StateCommand.new_data()
state_command.data = AUTOWARE.ROS.STATE_CMD.MAIN.INIT
self.publish_state_command(state_command)
def publish_drive_state_command(self, decisionmaker_states):
if any([
all([
decisionmaker_states.main_state == AUTOWARE.ROS.DECISION_MAKER_STATES.MAIN.MISSION_COMPLETE,
AUTOWARE.ROS.DECISION_MAKER_STATES.BEHAVIOR.WAIT_ORDERS in decisionmaker_states.behavior_state
]),
decisionmaker_states.main_state == AUTOWARE.ROS.DECISION_MAKER_STATES.MAIN.INITIAL
]):
state_command = ROSMessage.StateCommand.new_data()
state_command.data = AUTOWARE.ROS.STATE_CMD.MAIN.DRIVE
self.publish_state_command(state_command)
def publish_stop_state_command(self, decisionmaker_states):
if decisionmaker_states.acc_state != AUTOWARE.ROS.DECISION_MAKER_STATES.ACC.STOP:
state_command = ROSMessage.StateCommand.new_data()
state_command.data = AUTOWARE.ROS.STATE_CMD.SUB.STOP
self.publish_state_command(state_command)
def publish_light_color(self):
monitored_route = self.get_monitored_route()
if monitored_route is None:
traffic_light = AUTOWARE.ROS.TRAFFIC_LIGHT.RED
else:
distance_from_stopline = self.get_distance_from_stopline(monitored_route)
if distance_from_stopline <= self.upper_distance_from_stopline:
traffic_light = AUTOWARE.ROS.TRAFFIC_LIGHT.RED
else:
traffic_light = AUTOWARE.ROS.TRAFFIC_LIGHT.GREEN
header = ROSMessage.Header.get_template()
header.stamp.secs = int(time())
header.stamp.nsecs = int((time() - int(time())) * 1000000000)
payload = self.__topicPubLightColor.serialize(ROSMessage.LightColor.new_data(
header=header,
traffic_light=traffic_light
))
self.publish(self.__topicPubLightColor, payload)
def update_current_pose(self, _client, _userdata, _topic, payload):
self.ros_current_pose_lock.acquire()
self.ros_current_pose = self.__topicSubCurrentPose.unserialize(payload)
self.ros_current_pose_lock.release()
def update_closest_waypoint(self, _client, _userdata, _topic, payload):
self.ros_closest_waypoint_lock.acquire()
self.ros_closest_waypoint = self.__topicSubClosestWaypoint.unserialize(payload)
self.ros_closest_waypoint_lock.release()
def update_decisionmaker_states(self, _client, _userdata, _topic, payload):
self.ros_decisionmaker_states_lock.acquire()
self.ros_decisionmaker_states = self.__topicSubDecisionMakerStates.unserialize(payload)
self.ros_decisionmaker_states_lock.release()
def update_traffic_signals(self, _client, _user_data, _topic, payload):
# todo: localize
traffic_signal_status = self.__topicSubTrafficSignalStatus.unserialize(payload)
self.traffic_signals_lock.acquire()
self.traffic_signals[traffic_signal_status.route_code] = traffic_signal_status
self.traffic_signals_lock.release()
@staticmethod
def get_current_pose_from_ros_current_pose(ros_current_pose):
return Pose.new_data(
position=Position.new_data(**ros_current_pose.pose.position),
orientation=Orientation.new_data(
quaternion=Quaternion.new_data(**ros_current_pose.pose.orientation),
)
)
def update_pose_from_current_pose(self):
self.ros_current_pose_lock.acquire()
ros_current_pose = deepcopy(self.ros_current_pose)
self.ros_current_pose_lock.release()
if ros_current_pose is not None:
current_pose = Autoware.get_current_pose_from_ros_current_pose(ros_current_pose)
self.set_location(
self.__map_match.get_matched_location_on_arrows(current_pose, self.arrow.get_arrow_codes()))
self.remove_subscriber(self.__topicSubCurrentPose)
def update_pose_from_closest_arrow_waypoint(self):
self.ros_closest_waypoint_lock.acquire()
ros_closest_waypoint = deepcopy(self.ros_closest_waypoint)
self.ros_closest_waypoint_lock.release()
if ros_closest_waypoint is not None and \
0 <= ros_closest_waypoint.data < len(self.current_locations):
closest_location = self.current_locations[ros_closest_waypoint.data]
self.set_waypoint_id_and_arrow_code(
closest_location.waypoint_id, closest_location.arrow_code)
def get_ros_lane_array_from_locations(self, locations):
if 0 == len(locations):
return None
ros_lane_array = ROSMessage.LaneArray.new_data()
ros_lane = ROSMessage.Lane.new_data()
for location in locations:
pose = self.waypoint.get_pose(location.waypoint_id)
ros_waypoint = ROSMessage.Waypoint.new_data()
ros_waypoint.pose.pose.position.x = pose.position.x
ros_waypoint.pose.pose.position.y = pose.position.y
ros_waypoint.pose.pose.position.z = pose.position.z
ros_waypoint.pose.pose.orientation.z = pose.orientation.quaternion.z
ros_waypoint.pose.pose.orientation.w = pose.orientation.quaternion.w
ros_waypoint.twist.twist.linear.x = 0.2 * self.waypoint.get_speed_limit(location.waypoint_id)
ros_lane.waypoints.append(ros_waypoint)
ros_lane.header.stamp.secs = int(time()+1)
ros_lane_array.lanes.append(ros_lane)
return ros_lane_array
get_distance_from_stopline = SimCar.get_distance_from_stopline
get_monitored_route = SimCar.get_monitored_route
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 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 get_state_machine(self, initial_state=AUTOWARE.STATE.LAUNCHED):
machine = StateMachine(
states=list(AUTOWARE.STATE),
initial=initial_state,
)
machine.add_transitions([
{
"trigger": AUTOWARE.TRIGGER.ACTIVATE,
"source": AUTOWARE.STATE.LAUNCHED, "dest": AUTOWARE.STATE.STAND_BY,
"conditions": [self.condition_activated_and_update_schedules]
},
{
"trigger": AUTOWARE.TRIGGER.SCHEDULE,
"source": AUTOWARE.STATE.STAND_BY, "dest": AUTOWARE.STATE.SCHEDULE_UPDATED,
"conditions": [self.condition_expected_schedules_length_and_update_schedules]
},
{
"trigger": AUTOWARE.TRIGGER.SYNCHRONIZE_ROUTE,
"source": AUTOWARE.STATE.SCHEDULE_UPDATED, "dest": AUTOWARE.STATE.READY_TO_MOVE,
"conditions": [self.condition_route_synchronized_and_update_schedules]
},
{
"trigger": AUTOWARE.TRIGGER.MOVE,
"source": AUTOWARE.STATE.READY_TO_MOVE, "dest": AUTOWARE.STATE.MOVE,
"conditions": [self.condition_decisionmaker_states_changed_to_drive_and_update_schedules]
},
{
"trigger": AUTOWARE.TRIGGER.STOP,
"source": AUTOWARE.STATE.MOVE, "dest": AUTOWARE.STATE.STOP,
"conditions": [self.condition_achieved_and_update_schedules]
},
{
"trigger": AUTOWARE.TRIGGER.GET_READY,
"source": AUTOWARE.STATE.STOP, "dest": AUTOWARE.STATE.STAND_BY,
"conditions": [self.condition_time_limit_devisionmaker_states_change_to_initial_and_update_schedules]
},
])
return machine
def condition_activated(self, decisionmaker_states):
return self.condition_location() and \
self.condition_decisionmaker_states_changed_to_initial(decisionmaker_states)
def condition_location(self):
return self.status.location is not None
@staticmethod
def condition_decisionmaker_states_changed_to_initial(decisionmaker_states):
if decisionmaker_states is not None:
if decisionmaker_states.main_state == AUTOWARE.ROS.DECISION_MAKER_STATES.MAIN.INITIAL:
return True
@staticmethod
def condition_expected_schedules_length(schedules, expected):
return expected == len(schedules)
condition_time_limit = SimCar.condition_time_limit
@staticmethod
def condition_decisionmaker_states_changed_to_mission_complete(decisionmaker_states):
if decisionmaker_states is not None:
if all([
decisionmaker_states.main_state == AUTOWARE.ROS.DECISION_MAKER_STATES.MAIN.MISSION_COMPLETE,
AUTOWARE.ROS.DECISION_MAKER_STATES.BEHAVIOR.WAIT_ORDERS in decisionmaker_states.behavior_state
]):
return True
return False
def condition_route_synchronized(self, route):
self.publish_lane_array(route)
return True
@staticmethod
def condition_decisionmaker_states_changed_to_drive(decisionmaker_states):
if decisionmaker_states is not None:
if decisionmaker_states.main_state == AUTOWARE.ROS.DECISION_MAKER_STATES.MAIN.DRIVE:
return True
return False
def after_state_change_update_schedules(self, current_time, schedules, _decisionmaker_states=None):
schedules[:] = self.get_next_schedules(schedules, current_time)
return True
def after_state_change_publish_drive(self, decisionmaker_states):
self.publish_drive_state_command(decisionmaker_states)
def condition_activated_and_update_schedules(self, current_time, schedules):
if self.condition_location():
self.after_state_change_update_schedules(current_time, schedules)
return True
else:
if not self.condition_location():
self.update_pose_from_current_pose()
return False
def condition_expected_schedules_length_and_update_schedules(
self, current_time, schedules, expected_schedules_length):
if self.condition_expected_schedules_length(schedules, expected_schedules_length):
self.after_state_change_update_schedules(current_time, schedules)
return True
else:
self.add_random_schedule(current_time, schedules)
return False
def condition_route_synchronized_and_update_schedules(self, current_time, schedules):
if self.condition_route_synchronized(schedules[2].route):
self.after_state_change_update_schedules(current_time, schedules)
return True
else:
return False
def condition_decisionmaker_states_changed_to_drive_and_update_schedules(
self, current_time, schedules, decisionmaker_states):
if self.condition_decisionmaker_states_changed_to_drive(decisionmaker_states):
self.after_state_change_update_schedules(current_time, schedules)
return True
else:
self.publish_drive_state_command(decisionmaker_states)
return False
def condition_achieved_and_update_schedules(self, current_time, schedules, decisionmaker_states):
if self.condition_decisionmaker_states_changed_to_mission_complete(decisionmaker_states):
self.after_state_change_update_schedules(current_time, schedules)
return True
return False
def condition_time_limit_devisionmaker_states_change_to_initial_and_update_schedules(
self, current_time, schedules, decisionmaker_states):
if self.condition_time_limit(current_time):
if True:
self.after_state_change_update_schedules(current_time, schedules)
return True
else:
self.publish_drive_state_command(decisionmaker_states)
return False
def update_status(self):
self.update_pose_from_closest_arrow_waypoint()
if self.status.location is not None and self.status.state == AUTOWARE.STATE.MOVE:
self.publish_light_color()
schedules = self.get_schedules_and_lock()
self.ros_decisionmaker_states_lock.acquire()
decisionmaker_states = deepcopy(self.ros_decisionmaker_states)
self.ros_decisionmaker_states_lock.release()
current_time = time()
next_event = schedules[1].event
if next_event == AUTOWARE.TRIGGER.ACTIVATE:
self.state_machine.activate(current_time, schedules)
elif next_event == AUTOWARE.TRIGGER.SCHEDULE:
self.state_machine.schedule(current_time, schedules, 7)
elif next_event == AUTOWARE.TRIGGER.SYNCHRONIZE_ROUTE:
self.state_machine.synchronize_route(current_time, schedules)
elif next_event == AUTOWARE.TRIGGER.MOVE:
self.state_machine.move(current_time, schedules, decisionmaker_states)
elif next_event == AUTOWARE.TRIGGER.STOP:
self.state_machine.stop(current_time, schedules, decisionmaker_states)
elif next_event == AUTOWARE.TRIGGER.GET_READY:
self.state_machine.get_ready(current_time, schedules, decisionmaker_states)
self.set_schedules_and_unlock(schedules)
"baseTime"]
traffic_signal_cycle_message["period"] = cycle_set[group_name][
"period"]
traffic_signal_cycle_message["phases"] = cycle_set[group_name][
"phases"]
cycles[intersection_id].append(traffic_signal_cycle_message)
sleep(1)
print("publish routes")
for intersection_id, traffic_signal in traffic_signals.items():
# print(topicTrafficSignalSubscribe.root+"/"+traffic_signal["instance"].event_loop_id()+"/routes")
mqtt_client.publish(
topicTrafficSignalSubscribe.root + "/" +
traffic_signal["instance"].event_loop_id + "/routes",
topicTrafficSignalSubscribe.serialize(routes[intersection_id]))
sleep(5)
print("publish cycles")
for intersection_id, traffic_signal in traffic_signals.items():
# print(topicTrafficSignalSubscribe.root+"/"+traffic_signal["instance"].event_loop_id()+"/cycles")
mqtt_client.publish(
topicTrafficSignalSubscribe.root + "/" +
traffic_signal["instance"].event_loop_id + "/cycles",
topicTrafficSignalSubscribe.serialize(cycles[intersection_id]))
sleep(5)
print("publish schedules")
mqtt_client.publish(
class TrafficSignal(EventLoop):
class STATE(object):
GREEN = "green"
YELLOW = "yellow"
RED = "red"
class TOPIC(object):
PUBLISH = "pub_traffic_signal"
SUBSCRIBE = "sub_traffic_signal"
def __init__(self, name, processing_cycle=1.0):
super().__init__()
self.trafficSignalPublishTopic = Topic()
self.trafficSignalPublishTopic.set_id(self.event_loop_id)
self.trafficSignalPublishTopic.set_root(TrafficSignal.TOPIC.PUBLISH)
self.trafficSignalPublishTopic.set_message(traffic_signal_message)
self.trafficSignalSubscribeTopic = Topic()
self.trafficSignalSubscribeTopic.set_id(self.event_loop_id)
self.trafficSignalSubscribeTopic.set_root(
TrafficSignal.TOPIC.SUBSCRIBE)
self.trafficSignalSubscribeTopic.set_message(traffic_signal_message)
self.name = name
self.routes = {}
self.schedules = {}
self.cycles = {}
self.__processing_cycle = processing_cycle
self.__check_time = time()
self.__publish_flag = False
self.add_on_message_function(self.update_routes)
self.add_on_message_function(self.update_schedules)
self.add_on_message_function(self.update_cycles)
self.set_subscriber(self.trafficSignalSubscribeTopic.private +
"/routes")
self.set_subscriber(self.trafficSignalSubscribeTopic.private +
"/schedules")
self.set_subscriber(self.trafficSignalSubscribeTopic.private +
"/cycles")
self.set_main_loop(self.__main_loop)
def publish_status(self):
message = self.trafficSignalPublishTopic.get_template()
message["name"] = self.name
message["time"] = time()
message["routes"] = list(self.routes.values())
payload = self.trafficSignalPublishTopic.serialize(message)
self.publish(self.trafficSignalPublishTopic.private, payload)
def update_routes(self, _client, _userdata, topic, payload):
if topic == self.trafficSignalSubscribeTopic.private + "/routes":
routes = self.trafficSignalSubscribeTopic.unserialize(payload)
self.routes = dict(map(lambda x: (x["route_code"], x), routes))
self.__publish_flag = True
def update_schedules(self, _client, _userdata, topic, payload):
if topic == self.trafficSignalSubscribeTopic.private + "/schedules":
schedules = self.trafficSignalSubscribeTopic.unserialize(payload)
self.schedules = dict(
map(lambda x: (x["route_code"], [x]), schedules))
self.__publish_flag = True
def update_cycles(self, _client, _userdata, topic, payload):
if topic == self.trafficSignalSubscribeTopic.private + "/cycles":
cycles = self.trafficSignalSubscribeTopic.unserialize(payload)
for cycle in cycles:
for route_code in cycle["route_codes"]:
self.cycles[route_code] = cycle
self.__publish_flag = True
@staticmethod
def get_schedule_from_cycle(cycle, start_time):
base_time = cycle["base_time"]
period = cycle["period"]
phase_time = (start_time - base_time) % period
state = None
end_time = start_time
elapse_time = 0
for phase in cycle["phases"]:
elapse_time += phase["duration"]
state = phase["state"]
end_time = start_time + (elapse_time - phase_time)
if phase_time < elapse_time:
break
schedule = {
"state": state,
"start_time": start_time,
"duration": end_time - start_time,
}
return schedule
def __update_schedules(self):
current_time = time()
for route_code, route in self.routes.items():
route_schedules = []
if route_code in self.schedules:
route_schedules = list(
filter(
lambda x: current_time <= x["start_time"] + x[
"duration"], self.schedules[route_code]))
if route_code in self.cycles:
if len(route_schedules) < 3:
if len(route_schedules) == 0:
start_time = current_time
else:
start_time = route_schedules[-1][
"start_time"] + route_schedules[-1]["duration"]
route_schedules.append(
self.get_schedule_from_cycle(self.cycles[route_code],
start_time))
self.schedules[route_code] = route_schedules
def update_status(self):
for route_code, route in self.routes.items():
if len(self.schedules[route_code]) == 0:
if route["state"] is not None:
self.routes[route_code]["state"] = None
self.__publish_flag = True
else:
current_schedule = self.schedules[route_code][0]
state = current_schedule["state"]
if route["state"] != state:
self.routes[route_code]["state"] = state
self.__publish_flag = True
def update_check_time(self):
if len(self.routes) == 0:
return 1
self.__check_time = time() + 600
for route_schedules in self.schedules.values():
if 0 < len(route_schedules):
self.__check_time = min(
self.__check_time, route_schedules[0]["start_time"] +
route_schedules[0]["duration"])
def __main_loop(self):
while True:
if self.__check_time <= time():
self.__update_schedules()
self.update_status()
self.update_check_time()
if self.__publish_flag:
self.publish_status()
self.__check_time = time()
self.__publish_flag = False
sleep(self.__processing_cycle)
class User(EventLoop):
CONST = USER
def __init__(self, _id, name, dt=1.0):
super().__init__(_id)
self.status = UserStatus.new_data(
name=name,
time=time(),
trip_schedules=None,
state=USER.STATE.LOG_IN,
schedule=None
)
self.state_machine = None
self.dt = dt
self.schedules = self.manager.list()
self.schedules_lock = self.manager.Lock()
self.__topicPubStatus = Topic()
self.__topicPubStatus.set_targets(self.target)
self.__topicPubStatus.set_categories(USER.TOPIC.CATEGORIES.STATUS)
self.__topicSubSchedules = Topic()
self.__topicSubSchedules.set_targets(None, self.target)
self.__topicSubSchedules.set_categories(FLEET_MANAGER.TOPIC.CATEGORIES.SCHEDULES)
self.__topicSubSchedules.set_message(Schedules)
self.set_subscriber(self.__topicSubSchedules, self.update_schedules)
self.set_main_loop(self.__main_loop)
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 set_schedules(self, schedules):
self.schedules_lock.acquire()
self.schedules[:] = schedules
self.status.schedule = deepcopy(self.schedules[0])
self.schedules_lock.release()
def publish_status(self):
self.status.time = time()
self.status.state = self.state_machine.state
payload = self.__topicPubStatus.serialize(self.status)
self.publish(self.__topicPubStatus, payload)
def update_status_schedule(self):
pass
def update_schedules(self, _client, _userdata, _topic, payload):
new_schedules = self.__topicSubSchedules.unserialize(payload)
self.schedules_lock.acquire()
index = list(map(lambda x: x.id, new_schedules)).index(self.schedules[0].id)
self.schedules[:] = new_schedules[index:]
self.update_status_schedule()
self.schedules_lock.release()
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
def condition_time_limit(self, current_time, _schedules):
return self.status.schedule.period.end < current_time
def after_state_change_update_schedules(self, current_time, schedules):
schedules[:] = self.get_next_schedules(schedules, current_time)
return True
def after_state_change_update_time_limit(self, current_time, duration):
self.status.schedule.period.start = current_time
self.status.schedule.period.end = current_time + duration
return True
def condition_time_limit_and_update_schedules(self, current_time, schedules):
if self.condition_time_limit(current_time, schedules):
self.after_state_change_update_schedules(current_time, schedules)
return True
return False
def get_schedules_and_lock(self):
self.schedules_lock.acquire()
return deepcopy(self.schedules)
def set_schedules_and_unlock(self, schedules):
self.schedules[:] = schedules
self.schedules_lock.release()
def update_status(self):
schedules = self.get_schedules_and_lock()
self.set_schedules_and_unlock(schedules)
return
def __main_loop(self):
while self.status.state != USER.STATE.LOG_OUT:
sleep(self.dt)
self.update_status()
self.publish_status()
return True
class Vehicle(EventLoop):
CONST = VEHICLE
def __init__(self, _id, name, waypoint, arrow, route, dt=1.0):
super().__init__(_id)
self.status = VehicleStatus.new_data(name=name,
time=time(),
state=VEHICLE.STATE.LOG_IN,
schedule=None,
location=None,
pose=None)
self.waypoint = waypoint
self.arrow = arrow
self.route = route
self.state_machine = None
self.np_position = None
self.dt = dt
self.schedules = self.manager.list()
self.schedules_lock = self.manager.Lock()
self.__topicPubStatus = Topic()
self.__topicPubStatus.set_targets(self.target)
self.__topicPubStatus.set_categories(VEHICLE.TOPIC.CATEGORIES.STATUS)
self.__topicSubSchedules = Topic()
self.__topicSubSchedules.set_targets(None, self.target)
self.__topicSubSchedules.set_categories(
FLEET_MANAGER.TOPIC.CATEGORIES.SCHEDULES)
self.__topicSubSchedules.set_message(Schedules)
self.set_subscriber(self.__topicSubSchedules, self.update_schedules)
self.__topicPubGeotopic = Topic()
self.__topicPubGeotopic.set_targets(self.target, None)
self.set_main_loop(self.__main_loop)
def set_waypoint_id_and_arrow_code(self, waypoint_id, arrow_code):
self.set_location(
Location.new_location(waypoint_id, arrow_code,
self.waypoint.get_geohash(waypoint_id)))
def set_location(self, location):
self.status.location = location
self.update_np_position()
self.status.pose = Pose.new_data(
position=Position.new_position_from_np_position(self.np_position),
orientation=Orientation.new_data(rpy=Rpy.new_data(
yaw=self.arrow.get_yaw(self.status.location.arrow_code,
self.status.location.waypoint_id))))
def update_np_position(self):
self.np_position = self.waypoint.get_np_position(
self.status.location.waypoint_id)
def set_schedules(self, schedules):
self.schedules[:] = schedules
self.status.schedule = deepcopy(self.schedules[0])
def publish_status(self):
self.status.time = time()
self.status.state = self.state_machine.state
if self.status.location is not None:
self.status.location.geohash = self.waypoint.get_geohash(
self.status.location.waypoint_id)
if self.np_position is not None:
self.status.pose.position = Position.new_position_from_np_position(
self.np_position)
payload = self.__topicPubStatus.serialize(self.status)
self.publish(self.__topicPubStatus, payload)
def publish_geotopic(self):
if self.status.location is not None:
self.__topicPubGeotopic.set_categories(
VEHICLE.TOPIC.CATEGORIES.GEOTOPIC +
list(self.status.location.geohash))
self.publish(self.__topicPubGeotopic,
self.__topicPubGeotopic.serialize(self.target))
def update_status_schedule(self):
pass
def update_schedules(self, _client, _userdata, _topic, payload):
new_schedules = self.__topicSubSchedules.unserialize(payload)
index = list(map(lambda x: x.id,
new_schedules)).index(self.status.schedule.id)
self.schedules_lock.acquire()
self.schedules[:] = new_schedules[index:]
self.update_status_schedule()
self.schedules_lock.release()
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
def get_schedules_and_lock(self):
self.schedules_lock.acquire()
return deepcopy(self.schedules)
def set_schedules_and_unlock(self, schedules):
self.schedules[:] = schedules
self.schedules_lock.release()
def update_status(self):
schedules = self.get_schedules_and_lock()
self.set_schedules_and_unlock(schedules)
return
def __main_loop(self):
while self.status.state != VEHICLE.STATE.LOG_OUT:
sleep(self.dt)
self.update_status()
self.publish_status()
self.publish_geotopic()
return True