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 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
eventlet.monkey_patch() app = Flask(__name__) with app.app_context(): app.waypoint = Waypoint() app.waypoint.load(args.path_waypoint_json) app.arrow = Arrow() app.arrow.load(args.path_arrow_json) app.intersection = Intersection() app.intersection.load(args.path_intersection_json) app.topics = {} topic = Topic() topic.set_targets(Target.new_target(None, SimTaxiUser.__name__), None) topic.set_categories(User.CONST.TOPIC.CATEGORIES.STATUS) app.topics["user"] = topic.get_path(use_wild_card=True) topic = Topic() topic.set_targets(Target.new_target(None, SimTaxi.__name__), None) topic.set_categories(Vehicle.CONST.TOPIC.CATEGORIES.STATUS) app.topics["vehicle"] = topic.get_path(use_wild_card=True) topic = Topic() topic.set_targets(Target.new_target(None, SimTaxiFleet.__name__), None) topic.set_categories(FleetManager.CONST.TOPIC.CATEGORIES.STATUS) app.topics["fleet_manager"] = topic.get_path(use_wild_card=True) topic = Topic() topic.set_targets(Target.new_target(None, TrafficSignal.__name__), None) topic.set_categories(TrafficSignal.CONST.TOPIC.CATEGORIES.STATUS)
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 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)
mqtt_client = MQTTClient(args.host, args.port) 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)))
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 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 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 SimBus(SimCar): CONST = SIM_BUS def __init__(self, _id, name, waypoint, arrow, route, intersection, dt=1.0): super().__init__(_id, name, waypoint, arrow, route, intersection, dt=dt) self.state_machine = self.get_state_machine() self.user_statuses = self.manager.dict() self.user_statuses_lock = self.manager.Lock() self.__topicSubUserStatus = Topic() self.__topicSubUserStatus.set_targets( Target.new_target(None, SIM_BUS_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) 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_status.state in [USER.STATE.LOG_OUT]: if user_id in self.user_statuses: self.user_statuses.pop(user_id) else: self.user_statuses[user_id] = user_status self.user_statuses_lock.release() def get_state_machine(self, initial_state=SIM_BUS.STATE.STAND_BY): machine = StateMachine( states=list(SIM_BUS.STATE), initial=initial_state, ) machine.add_transitions([ { "trigger": SIM_BUS.TRIGGER.MOVE, "source": SIM_BUS.STATE.STAND_BY, "dest": SIM_BUS.STATE.MOVE_TO_CIRCULAR_ROUTE, "conditions": [self.condition_time_limit_and_update_schedules] }, { "trigger": SIM_BUS.TRIGGER.MOVE, "source": SIM_BUS.STATE.MOVE_TO_CIRCULAR_ROUTE, "dest": SIM_BUS.STATE.MOVE_TO_SELECT_POINT, "conditions": [self.condition_achieved_and_update_schedules] }, { "trigger": SIM_BUS.TRIGGER.MOVE, "source": SIM_BUS.STATE.MOVE_TO_SELECT_POINT, "dest": SIM_BUS.STATE.MOVE_TO_BRANCH_POINT, "conditions": [self.condition_achieved_and_update_schedules_event] }, { "trigger": SIM_BUS.TRIGGER.REQUEST_SCHEDULES, "source": SIM_BUS.STATE.MOVE_TO_BRANCH_POINT, "dest": SIM_BUS.STATE.REQUEST_THROUGH_SCHEDULES, "conditions": [self.condition_no_need_to_via_and_update_schedue_event] }, { "trigger": SIM_BUS.TRIGGER.REQUEST_SCHEDULES, "source": SIM_BUS.STATE.MOVE_TO_BRANCH_POINT, "dest": SIM_BUS.STATE.REQUEST_VIA_SCHEDULES, "conditions": [self.condition_need_to_via_and_update_schedue_event] }, { "trigger": SIM_BUS.TRIGGER.MOVE, "source": SIM_BUS.STATE.REQUEST_THROUGH_SCHEDULES, "dest": SIM_BUS.STATE.MOVE_TO_JUNCTION, "conditions": [ self. condition_schedules_length_achieved_and_update_schedules ] }, { "trigger": SIM_BUS.TRIGGER.MOVE, "source": SIM_BUS.STATE.REQUEST_VIA_SCHEDULES, "dest": SIM_BUS.STATE.MOVE_TO_BUS_STOP, "conditions": [ self. condition_schedules_length_achieved_and_update_schedules ] }, { "trigger": SIM_BUS.TRIGGER.STOP, "source": SIM_BUS.STATE.MOVE_TO_BUS_STOP, "dest": SIM_BUS.STATE.STOP_FOR_DISCHARGING_AND_TAKING_UP, "conditions": [self.condition_achieved_and_update_schedules] }, { "trigger": SIM_BUS.TRIGGER.MOVE, "source": SIM_BUS.STATE.STOP_FOR_DISCHARGING_AND_TAKING_UP, "dest": SIM_BUS.STATE.MOVE_TO_JUNCTION, "conditions": [self.condition_time_limit_and_update_schedules] }, { "trigger": SIM_BUS.TRIGGER.MOVE, "source": SIM_BUS.STATE.MOVE_TO_JUNCTION, "dest": SIM_BUS.STATE.MOVE_TO_SELECT_POINT, "conditions": [self.condition_achieved_and_update_schedules] }, { "trigger": SIM_BUS.TRIGGER.STAND_BY, "source": SIM_BUS.STATE.STOP_FOR_DISCHARGING_AND_TAKING_UP, "dest": SIM_BUS.STATE.STAND_BY, "conditions": [self.condition_time_limit_and_update_schedules] }, ]) return machine @staticmethod def condition_need_to_via(schedules, user_statuses): target_bus_stops = Target.get_same_group_targets_in_targets( "spot", schedules[0].targets) for target_bus_stop in target_bus_stops: waiting_user_statuses = list( filter(lambda x: x.state == SIM_BUS_USER.STATE.WAITING, user_statuses.values())) for user_status in waiting_user_statuses: target_waiting_bus_stop = Target.get_same_group_targets_in_targets( SIM_BUS_USER.TARGET_GROUP.START_BUS_STOP, user_status.trip_schedules[0].targets)[0] if target_bus_stop.id == target_waiting_bus_stop.id: return True stop_requested_user_statuses = list( filter( lambda x: x.schedule.event == SIM_BUS_USER.TRIGGER. REQUEST_STOP, user_statuses.values())) for user_status in stop_requested_user_statuses: target_waiting_bus_stop = Target.get_same_group_targets_in_targets( SIM_BUS_USER.TARGET_GROUP.GOAL_BUS_STOP, user_status.trip_schedules[0].targets)[0] if target_bus_stop.id == target_waiting_bus_stop.id: return True return False def condition_no_need_to_via(self, schedules, user_statuses): return not self.condition_need_to_via(schedules, user_statuses) @staticmethod def after_state_change_update_schedule_event(schedules, new_event): schedules[0].event = new_event return True def condition_achieved_and_update_schedules_event(self, current_time, schedules): if self.condition_achieved(): self.after_state_change_update_schedules(current_time, schedules) self.after_state_change_update_schedule_event( schedules, SIM_BUS.TRIGGER.REQUEST_SCHEDULES) return True return False def condition_no_need_to_via_and_update_schedue_event( self, schedules, user_statuses): if self.condition_no_need_to_via(schedules, user_statuses): self.after_state_change_update_schedule_event( schedules, SIM_BUS.TRIGGER.MOVE) return True return False def condition_need_to_via_and_update_schedue_event(self, schedules, user_statuses): if self.condition_need_to_via(schedules, user_statuses): self.after_state_change_update_schedule_event( schedules, SIM_BUS.TRIGGER.MOVE) return True return False def condition_schedules_length_achieved_and_update_schedules( self, current_time, schedules): if 1 < len(schedules): if self.condition_achieved(): self.after_state_change_update_schedules( current_time, schedules) 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 update_status_schedule(self): if self.status.schedule.event == SIM_BUS.TRIGGER.STAND_BY: self.status.schedule.period.end = self.schedules[0].period.end def update_status(self): schedules = self.get_schedules_and_lock() user_statuses = self.get_user_statuses_and_lock() if self.state_machine.state in [ SIM_BUS.STATE.MOVE_TO_CIRCULAR_ROUTE, SIM_BUS.STATE.MOVE_TO_SELECT_POINT, SIM_BUS.STATE.MOVE_TO_BRANCH_POINT, SIM_BUS.STATE.REQUEST_THROUGH_SCHEDULES, SIM_BUS.STATE.REQUEST_VIA_SCHEDULES, SIM_BUS.STATE.MOVE_TO_BUS_STOP, SIM_BUS.STATE.MOVE_TO_JUNCTION, ]: 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_BUS.TRIGGER.MOVE: self.state_machine.move(current_time, schedules) elif next_event == SIM_BUS.TRIGGER.STOP: self.state_machine.stop(current_time, schedules) elif next_event == SIM_BUS.TRIGGER.STAND_BY: self.state_machine.stand_by(current_time, schedules) else: pass elif 1 == len(schedules): current_event = schedules[0].event if current_event == SIM_BUS.TRIGGER.REQUEST_SCHEDULES: self.state_machine.request_schedules(schedules, user_statuses) self.set_user_statuses_and_unlock(user_statuses) self.set_schedules_and_unlock(schedules)
eventlet.monkey_patch() app = Flask(__name__) with app.app_context(): app.waypoint = Waypoint() app.waypoint.load(args.path_waypoint_json) app.arrow = Arrow() app.arrow.load(args.path_arrow_json) app.intersection = Intersection() app.intersection.load(args.path_intersection_json) app.topics = {} topic = Topic() topic.set_targets(Target.new_target(None, SimCar.__name__), None) topic.set_categories(SimCar.CONST.TOPIC.CATEGORIES.STATUS) app.topics["vehicle"] = topic.get_path(use_wild_card=True) topic = Topic() topic.set_targets(Target.new_target(None, TrafficSignal.__name__), None) topic.set_categories(TrafficSignal.CONST.TOPIC.CATEGORIES.STATUS) app.topics["traffic_signal"] = topic.get_path(use_wild_card=True) pp(app.topics) CORS(app) app.config['MQTT_BROKER_URL'] = env["MQTT_BROKER_HOST"] app.config['MQTT_BROKER_PORT'] = int(env["MQTT_BROKER_PORT"]) # app.config['MQTT_KEEPALIVE'] = 5 # app.config['MQTT_TLS_ENABLED'] = False
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 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
class SimBusUser(User): CONST = SIM_BUS_USER def __init__(self, _id, name, dt=1.0): super().__init__(_id, name, dt) self.state_machine = self.get_state_machine() self.target_start_bus_stop = None self.target_goal_bus_stop = None self.vehicle_id = None self.vehicle_statuses = self.manager.dict() self.vehicle_statuses_lock = self.manager.Lock() self.__topicSubVehicleStatus = Topic() self.__topicSubVehicleStatus.set_targets( Target.new_target(None, SIM_BUS.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 update_vehicle_status(self, _client, _userdata, topic, payload): vehicle_id = self.__topicSubVehicleStatus.get_from_id(topic) self.vehicle_statuses_lock.acquire() self.vehicle_statuses[ vehicle_id] = self.__topicSubVehicleStatus.unserialize(payload) self.vehicle_statuses_lock.release() def get_state_machine(self, initial_state=USER.STATE.LOG_IN): machine = StateMachine( states=list(USER.STATE) + list(SIM_BUS_USER.STATE), initial=initial_state, ) machine.add_transitions([ { "trigger": SIM_BUS_USER.TRIGGER.WAIT, "source": USER.STATE.LOG_IN, "dest": SIM_BUS_USER.STATE.WAITING, "conditions": [self.after_state_change_update_schedules] }, { "trigger": SIM_BUS_USER.TRIGGER.GET_ON, "source": SIM_BUS_USER.STATE.WAITING, "dest": SIM_BUS_USER.STATE.GETTING_ON, "conditions": [self.condition_bus_arrived_at_start_and_update_schedules] }, { "trigger": SIM_BUS_USER.TRIGGER.GOT_ON, "source": SIM_BUS_USER.STATE.GETTING_ON, "dest": SIM_BUS_USER.STATE.GOT_ON, "conditions": [self.condition_time_limit_and_update_schedules] }, { "trigger": SIM_BUS_USER.TRIGGER.MOVE_VEHICLE, "source": SIM_BUS_USER.STATE.GOT_ON, "dest": SIM_BUS_USER.STATE.MOVING, "conditions": [self.condition_bus_moving_and_update_schedules] }, { "trigger": SIM_BUS_USER.TRIGGER.REQUEST_STOP, "source": SIM_BUS_USER.STATE.MOVING, "dest": SIM_BUS_USER.STATE.READY_TO_GET_OUT, "conditions": [ self. condition_bus_approached_target_bus_stop_and_update_schedules ] }, { "trigger": SIM_BUS_USER.TRIGGER.GET_OUT, "source": SIM_BUS_USER.STATE.READY_TO_GET_OUT, "dest": SIM_BUS_USER.STATE.GETTING_OUT, "conditions": [self.condition_bus_arrived_at_goal_and_update_schedules] }, { "trigger": SIM_BUS_USER.TRIGGER.GOT_OUT, "source": SIM_BUS_USER.STATE.GETTING_OUT, "dest": SIM_BUS_USER.STATE.GOT_OUT, "conditions": [self.condition_time_limit_and_update_schedules] }, { "trigger": USER.TRIGGER.LOG_OUT, "source": SIM_BUS_USER.STATE.GOT_OUT, "dest": USER.STATE.LOG_OUT, "conditions": [self.after_state_change_update_schedules] }, ]) return machine @staticmethod def condition_bus_arrived_at_target_bus_stop(target_bus_stop, vehicle_status): targets_vehicle_statuses = list( filter(lambda x: Target.is_same_id(x, target_bus_stop), vehicle_status.schedule.targets)) if 0 < len(targets_vehicle_statuses): if vehicle_status.schedule.event == SIM_BUS.TRIGGER.STOP: return True return False def get_vehicle_id_arrived_at_target_bus_stop(self, target_bus_stop, vehicle_statuses): for vehicle_id, vehicle_status in vehicle_statuses.items(): if self.condition_bus_arrived_at_target_bus_stop( target_bus_stop, vehicle_status): return vehicle_id return None @staticmethod def condition_bus_moving(vehicle_statuse): return vehicle_statuse.state in [ SIM_BUS.STATE.MOVE_TO_SELECT_POINT, SIM_BUS.STATE.REQUEST_THROUGH_SCHEDULES, SIM_BUS.STATE.REQUEST_VIA_SCHEDULES, SIM_BUS.STATE.MOVE_TO_BRANCH_POINT, SIM_BUS.STATE.MOVE_TO_BUS_STOP, SIM_BUS.STATE.MOVE_TO_JUNCTION, SIM_BUS.STATE.MOVE_TO_PARKING, ] @staticmethod def condition_bus_approached_target_bus_stop(vehicle_status, target_bus_stop): return 0 < len( list( filter(lambda x: Target.is_same_id(target_bus_stop, x), vehicle_status.schedule.targets))) @staticmethod def after_state_change_update_schedule_target(vehicle_id, schedules): for i, schedule in enumerate(schedules): if schedule.event in [ SIM_BUS_USER.TRIGGER.GOT_ON, SIM_BUS_USER.TRIGGER.MOVE_VEHICLE, SIM_BUS_USER.TRIGGER.REQUEST_STOP, SIM_BUS_USER.TRIGGER.GET_OUT, SIM_BUS_USER.TRIGGER.GOT_OUT, ]: schedules[i].targets.append( Target.new_target(vehicle_id, SIM_BUS.NODE_NAME)) return True def condition_bus_arrived_at_start_and_update_schedules( self, current_time, target_bus_stop, vehicle_statuses, schedules, duration): vehicle_id = self.get_vehicle_id_arrived_at_target_bus_stop( target_bus_stop, vehicle_statuses) if vehicle_id is not None: self.after_state_change_update_schedules(current_time, schedules) self.after_state_change_update_time_limit(current_time, duration) self.after_state_change_update_schedule_target( vehicle_id, schedules) return True return False def condition_bus_moving_and_update_schedules(self, current_time, schedules, vehicle_status): if self.condition_bus_moving(vehicle_status): self.after_state_change_update_schedules(current_time, schedules) return True return False def condition_bus_approached_target_bus_stop_and_update_schedules( self, current_time, schedules, vehicle_status): if self.condition_bus_approached_target_bus_stop( vehicle_status, self.target_goal_bus_stop): self.after_state_change_update_schedules(current_time, schedules) return True return False def condition_bus_arrived_at_goal_and_update_schedules( self, current_time, target_bus_stop, vehicle_status, schedules, duration): if self.condition_bus_arrived_at_target_bus_stop( target_bus_stop, vehicle_status): self.after_state_change_update_schedules(current_time, schedules) self.after_state_change_update_time_limit(current_time, duration) 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() @staticmethod def get_vehicle_id_in_schedule(schedule): vehicle_ids = list( map( lambda x: x.id, filter(lambda x: x.group == SIM_BUS.NODE_NAME, schedule.targets))) if 1 == len(vehicle_ids): return vehicle_ids[0] else: return None def update_status(self): if self.target_start_bus_stop is None: if self.status.trip_schedules is not None: self.target_start_bus_stop = \ list(filter( lambda x: x.group == SIM_BUS_USER.TARGET_GROUP.START_BUS_STOP, self.status.trip_schedules[0].targets) )[0] self.target_goal_bus_stop = \ list(filter( lambda x: x.group == SIM_BUS_USER.TARGET_GROUP.GOAL_BUS_STOP, self.status.trip_schedules[0].targets) )[0] schedules = self.get_schedules_and_lock() vehicle_statuses = self.get_vehicle_statuses_and_lock() current_time = time() if 1 < len(schedules): next_event = schedules[1].event vehicle_id = self.get_vehicle_id_in_schedule(schedules[1]) vehicle_status = None if vehicle_id is None else vehicle_statuses[ vehicle_id] if next_event == SIM_BUS_USER.TRIGGER.WAIT: self.state_machine.wait(current_time, schedules) elif next_event == SIM_BUS_USER.TRIGGER.GET_ON: self.state_machine.get_on(current_time, self.target_start_bus_stop, vehicle_statuses, schedules, 3) elif next_event == SIM_BUS_USER.TRIGGER.GOT_ON: self.state_machine.got_on(current_time, schedules) elif next_event == SIM_BUS_USER.TRIGGER.MOVE_VEHICLE: self.state_machine.move_vehicle(current_time, schedules, vehicle_status) elif next_event == SIM_BUS_USER.TRIGGER.REQUEST_STOP: self.state_machine.request_stop(current_time, schedules, vehicle_status) elif next_event == SIM_BUS_USER.TRIGGER.GET_OUT: self.state_machine.get_out(current_time, self.target_goal_bus_stop, vehicle_status, schedules, 3) elif next_event == SIM_BUS_USER.TRIGGER.GOT_OUT: self.state_machine.got_out(current_time, schedules) elif next_event == USER.TRIGGER.LOG_OUT: self.state_machine.log_out(current_time, schedules) else: pass self.set_vehicle_statuses_and_unlock(vehicle_statuses) self.set_schedules_and_unlock(schedules)