class ClosestWaypointSubscriber(EventLoop): def __init__(self, name, host, port): super(ClosestWaypointSubscriber, self).__init__() self.autowareSubscribeTopic = Topic() self.autowareSubscribeTopic.set_id(name) self.autowareSubscribeTopic.set_root(Autoware.TOPIC.SUBSCRIBE) self.autowareSubscribeTopic.set_message(autoware_message) self.__previous_time = time() self.__period = 1.0 # [sec] self.connect(host, port) self.set_main_loop(rospy.spin) rospy.init_node("ams_closest_waypoint_subscriber", anonymous=True) self.__ROSSubscriber = message_filters.Subscriber( Autoware.ROSTOPIC.SUBSCRIBE, Int32) self.__ROSSubscriber.registerCallback(self.on_message_from_ros, self.publish) def on_message_from_ros(self, messageData, publish): current_time = time() if self.__period < current_time - self.__previous_time: self.__previous_time += (1 + int( (current_time - self.__previous_time) / self.__period)) * self.__period message = self.autowareSubscribeTopic.get_template( )["closest_waypoint"] message["index"] = messageData.data payload = self.autowareSubscribeTopic.serialize(message) publish(self.autowareSubscribeTopic.private + "/closest_waypoint", payload)
def request_fleet_relations(): topic = Topic() topic.set_root(FleetManager.TOPIC.SUBSCRIBE) topic.set_message(fleet_manager_message) message = topic.get_template() message["action"] = FleetManager.ACTION.PUBLISH_RELATIONS mqtt.publish(topic.root, topic.serialize(message)) return api_response(code=200, message={"result": "requested"})
class LaneArrayPublisher(EventLoop): def __init__(self, name): super(LaneArrayPublisher, self).__init__() self.autowarePublishTopic = Topic() self.autowarePublishTopic.set_id(name) self.autowarePublishTopic.set_root(Autoware.TOPIC.PUBLISH) self.autowarePublishTopic.set_message(autoware_message) self.add_on_message_function(self.publish_to_ros) self.set_subscriber(self.autowarePublishTopic.private+"/waypoints") rospy.init_node("ams_lane_array_publisher", anonymous=True) self.__ROSPublisher = rospy.Publisher(Autoware.ROSTOPIC.PUBLISH, LaneArray) def publish_to_ros(self, client, userdata, topic, payload): if topic == self.autowarePublishTopic.private+"/waypoints": waypointMessages = self.autowarePublishTopic.unserialize(payload) laneArray = LaneArray() # WaypointState.stopline_state = 2 # keep stop lane = Lane() for waypointMessage in waypointMessages: waypoint = Waypoint() waypoint.pose.pose.position.x = waypointMessage["position"]["x"] waypoint.pose.pose.position.y = waypointMessage["position"]["y"] waypoint.pose.pose.position.z = waypointMessage["position"]["z"] waypoint.pose.pose.orientation.z = waypointMessage["orientation"]["z"] waypoint.pose.pose.orientation.w = waypointMessage["orientation"]["w"] waypoint.twist.twist.linear.x = waypointMessage["velocity"] lane.waypoints.append(waypoint) # lane.waypoints[-1].wpstate.stopline_state = 2 laneArray.lanes = [lane] self.__ROSPublisher.publish(laneArray)
self.__client.publish(topic, message) TRAFFIC_SIGNAL_FILE = "../../res/trafficSignal.json" if __name__ == '__main__': host = "localhost" port = 1883 if 1 < len(sys.argv): host = sys.argv[1] if 2 < len(sys.argv): port = int(sys.argv[2]) mqtt_client = MQTTClient(host, port) topicTrafficSignalSubscribe = Topic() topicTrafficSignalSubscribe.set_root(TrafficSignal.TOPIC.SUBSCRIBE) topicTrafficSignalSubscribe.set_message(traffic_signal_message) with open(TRAFFIC_SIGNAL_FILE, "r") as f: traffic_signal_configs = json.load(f) cycle_set = traffic_signal_configs["cycles"] cycle_groups = traffic_signal_configs["cycleGroups"] traffic_signals = {} routes = {} cycles = {} schedules = {} for intersection_id, intersection_cycle_groups in cycle_groups.items(): instance = TrafficSignal(name=intersection_id) process = Process(target=instance.start)
class User(EventLoop): class TOPIC(object): PUBLISH = "pubUser" SUBSCRIBE = "subUser" class STATE(object): LOGIN = "******" WAITING = "waiting" GETTING_ON = "gettingOn" GOT_ON = "gotOn" MOVING = "moving" GETTING_OUT = "gettingOut" GOT_OUT = "gotOut" class ACTION(object): WAIT = "wait" GET_ON = "getOn" GET_OUT = "getOut" class EVENT(object): MOVE_VEHICLE = "moveVehicle" def __init__(self, name, waypoint, dt=1.0): super().__init__() self.topicUserPublish = Topic() self.topicUserPublish.set_id(self.event_loop_id) self.topicUserPublish.set_root(User.TOPIC.PUBLISH) self.topicUserPublish.set_message(user_message) self.topicUserSubscribe = Topic() self.topicUserSubscribe.set_id(self.event_loop_id) self.topicUserSubscribe.set_root(User.TOPIC.SUBSCRIBE) self.topicUserSubscribe.set_message(user_message) self.name = name self.id = self.event_loop_id self.state = User.STATE.LOGIN self.event = None self.action = None self.dt = dt self.__start_waypoint_id = None self.__goal_waypoint_id = None self.__vehicleID = None self.__waypoint = waypoint self.add_on_message_function(self.update_action) self.add_on_message_function(self.update_event) self.set_subscriber(self.topicUserSubscribe.private + "/schedules") self.set_subscriber(self.topicUserSubscribe.private + "/event") self.set_main_loop(self.__main_loop) def publish_status(self): message = self.topicUserPublish.get_template() message["name"] = self.name message["state"] = self.state message["event"] = self.event message["schedules"][0]["action"] = self.action message["schedules"][0]["start"][ "waypoint_id"] = self.__start_waypoint_id message["schedules"][0]["goal"][ "waypoint_id"] = self.__goal_waypoint_id payload = self.topicUserPublish.serialize(message) self.publish(self.topicUserPublish.private, payload) def set_waypoint_at_random(self, waypoint_ids=None): if waypoint_ids is None: waypoint_ids = self.__waypoint.get_waypoint_ids() start_waypoint_id = random.choice(waypoint_ids) waypoint_ids.remove(start_waypoint_id) goal_waypoint_id = random.choice(waypoint_ids) self.set_waypoint(start_waypoint_id, goal_waypoint_id) def set_waypoint(self, start_waypoint_id, goal_waypoint_id): self.set_start_waypoint(start_waypoint_id) self.set_goal_waypoint(goal_waypoint_id) def set_start_waypoint(self, start_waypoint_id): self.__start_waypoint_id = start_waypoint_id def set_goal_waypoint(self, goal_waypoint_id): self.__goal_waypoint_id = goal_waypoint_id def update_action(self, _client, _userdata, topic, payload): # print(topic) if topic == self.topicUserSubscribe.private + "/schedules": message = self.topicUserSubscribe.unserialize(payload) self.action = message["schedules"][0]["action"] def update_event(self, _client, _userdata, topic, payload): if topic == self.topicUserSubscribe.private + "/event": message = self.topicUserSubscribe.unserialize(payload) self.event = message["event"] def update_status(self): # print(self.state, self.event, self.action) if self.state == User.STATE.LOGIN: if self.action == User.ACTION.WAIT: self.state = User.STATE.WAITING self.action = None elif self.state == User.STATE.WAITING: if self.action == User.ACTION.GET_ON: self.state = User.STATE.GETTING_ON self.action = None elif self.state == User.STATE.GETTING_ON: self.state = User.STATE.GOT_ON elif self.state == User.STATE.GOT_ON: if self.event == User.EVENT.MOVE_VEHICLE: self.state = User.STATE.MOVING self.event = None elif self.state == User.STATE.MOVING: if self.action == User.ACTION.GET_OUT: self.state = User.STATE.GETTING_OUT self.action = None elif self.state == User.STATE.GETTING_OUT: self.state = User.STATE.GOT_OUT def __main_loop(self): self.publish_status() while self.state != User.STATE.GOT_OUT: sleep(self.dt) self.update_status() self.publish_status() sleep(2)
class EventLoop(object): KEEP_ALIVE = 60 class TOPIC(object): PUBLISH = "pub_event_loop" SUBSCRIBE = "sub_event_loop" def __init__(self, _id=None): self.event_loop_id = _id if _id is None: self.event_loop_id = str(uuid()) self.__subscribeTopic = Topic() self.__subscribeTopic.set_id(self.event_loop_id) self.__subscribeTopic.set_root(EventLoop.TOPIC.SUBSCRIBE) self.__subscribeTopic.set_message(event_loop_message) self.__publishTopic = Topic() self.__publishTopic.set_id(self.event_loop_id) self.__publishTopic.set_root(EventLoop.TOPIC.PUBLISH) self.__publishTopic.set_message(event_loop_message) self.__subscribers = {} self.__publishers = {} self.__client = None self.__main_loop = None self.__pid = os.getpid() self.set_subscriber(self.__subscribeTopic.private) self.__on_message_functions = [] self.__user_data = None self.__user_will = None def __del__(self): if self.__client is not None: message = self.__publishTopic.get_template() message["action"] = "disconnect" message["pid"] = self.__pid payload = self.__publishTopic.serialize(message) self.publish(self.__publishTopic.private, payload) self.__client.disconnect() def set_subscriber(self, topic): self.__subscribers[str(uuid())] = {"topic": topic} def set_user_data(self, user_data): self.__user_data = user_data def add_on_message_function(self, on_message_function): self.__on_message_functions.append(on_message_function) def set_main_loop(self, main_loop): self.__main_loop = main_loop def set_will(self, topic, payload): self.__user_will = {"topic": topic, "payload": payload} def publish(self, topic, payload, qos=0, retain=False): # payload = self.__subscribeTopic.serialize(message) self.__client.publish(topic, payload=payload, qos=qos, retain=retain) def __on_message(self, client, userdata, message_data): payload = message_data.payload.decode("utf-8") if self.__subscribeTopic.root in message_data.topic and \ self.__subscribeTopic.get_id(message_data.topic) == self.event_loop_id: message = self.__subscribeTopic.unserialize(payload) if message["action"] == "start": print(self.__subscribeTopic.root, message) if message["action"] == "kill": print(self.__subscribeTopic.root, message) self.end() if message["action"] == "check": print(self.__subscribeTopic.root, message) self.__check() for onMessageFunction in self.__on_message_functions: onMessageFunction(client, userdata, message_data.topic, payload) return True def connect(self, host, port): self.__client = mqtt.Client(protocol=mqtt.MQTTv311, userdata=self.__user_data) will = self.__user_will print(will) if will is None: message = self.__publishTopic.get_template() message["action"] = "will" message["pid"] = self.__pid payload = self.__publishTopic.serialize(message) will = {"topic": self.__publishTopic.private, "payload": payload} self.__client.will_set(will["topic"], payload=will["payload"], qos=2, retain=False) self.__client.on_message = self.__on_message self.__client.connect(host=host, port=port, keepalive=EventLoop.KEEP_ALIVE) def start(self, host="localhost", port=1883): self.connect(host, port) for subscriber in self.__subscribers.values(): self.__client.subscribe(subscriber["topic"]) message = self.__publishTopic.get_template() message["time"] = str(int(time())) message["action"] = "start" message["pid"] = self.__pid payload = self.__publishTopic.serialize(message) self.publish(self.__publishTopic.private, payload) if self.__main_loop is None: self.__client.loop_forever() else: self.__client.loop_start() self.__main_loop() def end(self): self.__client.loop_stop() self.__client.disconnect() os.kill(self.__pid, SIGKILL) def __check(self): # todo: main_loop zombie message = self.__publishTopic.get_template() message["time"] = str(int(time())) message["action"] = "ok" message["pid"] = self.__pid payload = self.__publishTopic.serialize(message) self.publish(self.__publishTopic.private, payload) def get_pid(self): return self.__pid
class TrafficSignal(EventLoop): class STATE(object): GREEN = "green" YELLOW = "yellow" RED = "red" class TOPIC(object): PUBLISH = "pub_traffic_signal" SUBSCRIBE = "sub_traffic_signal" def __init__(self, name, processing_cycle=1.0): super().__init__() self.trafficSignalPublishTopic = Topic() self.trafficSignalPublishTopic.set_id(self.event_loop_id) self.trafficSignalPublishTopic.set_root(TrafficSignal.TOPIC.PUBLISH) self.trafficSignalPublishTopic.set_message(traffic_signal_message) self.trafficSignalSubscribeTopic = Topic() self.trafficSignalSubscribeTopic.set_id(self.event_loop_id) self.trafficSignalSubscribeTopic.set_root( TrafficSignal.TOPIC.SUBSCRIBE) self.trafficSignalSubscribeTopic.set_message(traffic_signal_message) self.name = name self.routes = {} self.schedules = {} self.cycles = {} self.__processing_cycle = processing_cycle self.__check_time = time() self.__publish_flag = False self.add_on_message_function(self.update_routes) self.add_on_message_function(self.update_schedules) self.add_on_message_function(self.update_cycles) self.set_subscriber(self.trafficSignalSubscribeTopic.private + "/routes") self.set_subscriber(self.trafficSignalSubscribeTopic.private + "/schedules") self.set_subscriber(self.trafficSignalSubscribeTopic.private + "/cycles") self.set_main_loop(self.__main_loop) def publish_status(self): message = self.trafficSignalPublishTopic.get_template() message["name"] = self.name message["time"] = time() message["routes"] = list(self.routes.values()) payload = self.trafficSignalPublishTopic.serialize(message) self.publish(self.trafficSignalPublishTopic.private, payload) def update_routes(self, _client, _userdata, topic, payload): if topic == self.trafficSignalSubscribeTopic.private + "/routes": routes = self.trafficSignalSubscribeTopic.unserialize(payload) self.routes = dict(map(lambda x: (x["route_code"], x), routes)) self.__publish_flag = True def update_schedules(self, _client, _userdata, topic, payload): if topic == self.trafficSignalSubscribeTopic.private + "/schedules": schedules = self.trafficSignalSubscribeTopic.unserialize(payload) self.schedules = dict( map(lambda x: (x["route_code"], [x]), schedules)) self.__publish_flag = True def update_cycles(self, _client, _userdata, topic, payload): if topic == self.trafficSignalSubscribeTopic.private + "/cycles": cycles = self.trafficSignalSubscribeTopic.unserialize(payload) for cycle in cycles: for route_code in cycle["route_codes"]: self.cycles[route_code] = cycle self.__publish_flag = True @staticmethod def get_schedule_from_cycle(cycle, start_time): base_time = cycle["base_time"] period = cycle["period"] phase_time = (start_time - base_time) % period state = None end_time = start_time elapse_time = 0 for phase in cycle["phases"]: elapse_time += phase["duration"] state = phase["state"] end_time = start_time + (elapse_time - phase_time) if phase_time < elapse_time: break schedule = { "state": state, "start_time": start_time, "duration": end_time - start_time, } return schedule def __update_schedules(self): current_time = time() for route_code, route in self.routes.items(): route_schedules = [] if route_code in self.schedules: route_schedules = list( filter( lambda x: current_time <= x["start_time"] + x[ "duration"], self.schedules[route_code])) if route_code in self.cycles: if len(route_schedules) < 3: if len(route_schedules) == 0: start_time = current_time else: start_time = route_schedules[-1][ "start_time"] + route_schedules[-1]["duration"] route_schedules.append( self.get_schedule_from_cycle(self.cycles[route_code], start_time)) self.schedules[route_code] = route_schedules def update_status(self): for route_code, route in self.routes.items(): if len(self.schedules[route_code]) == 0: if route["state"] is not None: self.routes[route_code]["state"] = None self.__publish_flag = True else: current_schedule = self.schedules[route_code][0] state = current_schedule["state"] if route["state"] != state: self.routes[route_code]["state"] = state self.__publish_flag = True def update_check_time(self): if len(self.routes) == 0: return 1 self.__check_time = time() + 600 for route_schedules in self.schedules.values(): if 0 < len(route_schedules): self.__check_time = min( self.__check_time, route_schedules[0]["start_time"] + route_schedules[0]["duration"]) def __main_loop(self): while True: if self.__check_time <= time(): self.__update_schedules() self.update_status() self.update_check_time() if self.__publish_flag: self.publish_status() self.__check_time = time() self.__publish_flag = False sleep(self.__processing_cycle)
class FleetManager(EventLoop): class ACTION(object): PUBLISH_RELATIONS = "pub_relations" class TOPIC(object): PUBLISH = "pub_fleet_manager" SUBSCRIBE = "sub_fleet_manager" def __init__(self, waypoint, arrow, route): super().__init__() self.topicUserPublish = Topic() self.topicUserPublish.set_root(User.TOPIC.PUBLISH) self.topicUserPublish.set_message(user_message) self.topicUserSubscribe = Topic() self.topicUserSubscribe.set_root(User.TOPIC.SUBSCRIBE) self.topicUserSubscribe.set_message(user_message) self.topicVehiclePublish = Topic() self.topicVehiclePublish.set_root(Vehicle.TOPIC.PUBLISH) self.topicVehiclePublish.set_message(vehicle_message) self.topicVehicleSubscribe = Topic() self.topicVehicleSubscribe.set_root(Vehicle.TOPIC.SUBSCRIBE) self.topicVehicleSubscribe.set_message(vehicle_message) self.topicTrafficSignalPublish = Topic() self.topicTrafficSignalPublish.set_root(TrafficSignal.TOPIC.PUBLISH) self.topicTrafficSignalPublish.set_message(traffic_signal_message) self.topicFleetManagerPublish = Topic() self.topicFleetManagerPublish.set_id(self.event_loop_id) self.topicFleetManagerPublish.set_root(FleetManager.TOPIC.PUBLISH) self.topicFleetManagerPublish.set_message(fleet_manager_message) self.topicFleetManagerSubscribe = Topic() self.topicFleetManagerSubscribe.set_id(self.event_loop_id) self.topicFleetManagerSubscribe.set_root(FleetManager.TOPIC.SUBSCRIBE) self.topicFleetManagerSubscribe.set_message(fleet_manager_message) self.waypoint = waypoint self.arrow = arrow self.route = route self.users = {} self.vehicles = {} self.traffic_signals = {} self.relations = {} # vehicle_id -> user_id, user_id -> vehicle_id self.add_on_message_function(self.update_user_status) self.add_on_message_function(self.update_vehicle_status) self.add_on_message_function(self.update_traffic_signal_status) self.add_on_message_function(self.response_request) self.set_subscriber(self.topicUserPublish.all) self.set_subscriber(self.topicVehiclePublish.all) self.set_subscriber(self.topicTrafficSignalPublish.all) self.set_subscriber(self.topicFleetManagerSubscribe.all) def update_user_status(self, _client, _userdata, topic, payload): # print("update_user_status", topic) if self.topicUserPublish.root in topic: user_id = self.topicUserPublish.get_id(topic) message = self.topicUserPublish.unserialize(payload) if message["state"] == User.STATE.LOGIN: # print("user", User.STATE.LOGIN) # todo: move to dispatcher class self.dispatch(user_id, message) elif message["state"] == User.STATE.WAITING: pass # print("user", User.STATE.WAITING) elif message["state"] == User.STATE.GETTING_ON: pass # print("user", User.STATE.GETTING_ON) elif message["state"] == User.STATE.GOT_ON: # print("user", User.STATE.GOT_ON) vehicle_id = self.relations[user_id] self.vehicles[vehicle_id]["schedules"][0][ "action"] = Vehicle.ACTION.MOVE payload = self.topicVehicleSubscribe.serialize( {"schedules": self.vehicles[vehicle_id]["schedules"]}) self.publish( self.topicVehicleSubscribe.root + "/" + vehicle_id + "/schedules", payload) elif message["state"] == User.STATE.MOVING: pass # print("user", User.STATE.MOVING) elif message["state"] == User.STATE.GETTING_OUT: pass # print("user", User.STATE.GETTING_OUT) elif message["state"] == User.STATE.GOT_OUT: pass # print("user", User.STATE.GOT_OUT) else: print("user", message["state"]) self.users[user_id] = message def update_vehicle_status(self, _client, _userdata, topic, payload): if self.topicVehiclePublish.root in topic: vehicle_id = self.topicVehiclePublish.get_id(topic) message = self.topicVehiclePublish.unserialize(payload) # print("update_vehicle_status", topic, message["state"]) if vehicle_id in self.relations: prev_state = self.vehicles[vehicle_id]["state"] if message["state"] == SimTaxi.STATE.MOVE_TO_USER: # print("vehicle", SimTaxi.STATE.MOVE_TO_USER) if prev_state == SimTaxi.STATE.STANDBY: user_id = self.relations[vehicle_id] self.users[user_id]["schedules"][0][ "action"] = User.ACTION.WAIT self.publish( self.topicUserSubscribe.root + "/" + user_id + "/schedules", self.topicUserSubscribe.serialize({ "schedules": self.users[user_id]["schedules"] })) elif message["state"] == SimTaxi.STATE.STOP_FOR_PICKING_UP: # print("vehicle", SimTaxi.STATE.STOP_FOR_PICKING_UP) if prev_state == SimTaxi.STATE.MOVE_TO_USER: # print("vehicle", SimTaxi.STATE.STOP_FOR_PICKING_UP, SimTaxi.STATE.MOVE_TO_USER) user_id = self.relations[vehicle_id] self.users[user_id]["schedules"][0][ "action"] = User.ACTION.GET_ON self.publish( self.topicUserSubscribe.root + "/" + user_id + "/schedules", self.topicUserSubscribe.serialize({ "schedules": self.users[user_id]["schedules"] })) elif message[ "state"] == SimTaxi.STATE.MOVE_TO_USER_DESTINATION: # print("vehicle", SimTaxi.STATE.MOVE_TO_USER_DESTINATION) if prev_state == SimTaxi.STATE.STOP_FOR_PICKING_UP: # print("vehicle", SimTaxi.STATE.MOVE_TO_USER_DESTINATION, SimTaxi.STATE.STOP_FOR_PICKING_UP) user_id = self.relations[vehicle_id] self.users[user_id]["schedules"][0][ "event"] = User.EVENT.MOVE_VEHICLE self.publish( self.topicUserSubscribe.root + "/" + user_id + "/event", self.topicUserSubscribe.serialize( {"event": User.EVENT.MOVE_VEHICLE})) elif message["state"] == SimTaxi.STATE.STOP_FOR_DISCHARGING: # print("vehicle", SimTaxi.STATE.STOP_FOR_DISCHARGING) if prev_state == SimTaxi.STATE.MOVE_TO_USER_DESTINATION: user_id = self.relations[vehicle_id] self.users[user_id]["schedules"][0][ "action"] = User.ACTION.GET_OUT self.publish( self.topicUserSubscribe.root + "/" + user_id + "/schedules", self.topicUserSubscribe.serialize({ "schedules": self.users[user_id]["schedules"] })) elif message["state"] == SimTaxi.STATE.MOVE_TO_STANDBY: pass # print("vehicle", SimTaxi.STATE.MOVE_TO_STANDBY) elif message["state"] == SimTaxi.STATE.STANDBY: pass # print("vehicle", SimTaxi.STATE.STANDBY) else: print("vehicle", message["state"]) # vehicleSchedule = vehicle.pop("schedule") if vehicle_id not in self.vehicles: # print("set vehicle", vehicle_id, message["name"]) self.vehicles[vehicle_id] = message else: # print("update vehicle", vehicle_id, message["name"]) self.vehicles[vehicle_id].update(message) def update_traffic_signal_status(self, _client, _userdata, topic, payload): if self.topicTrafficSignalPublish.root in topic: message = self.topicTrafficSignalPublish.unserialize(payload) for route in message["routes"]: self.traffic_signals[route["route_code"]] = route def response_request(self, _client, _userdata, topic, payload): if self.topicFleetManagerSubscribe.root in topic: message = self.topicFleetManagerSubscribe.unserialize(payload) if message["action"] == FleetManager.ACTION.PUBLISH_RELATIONS: self.publish_relations() def get_dispatchable_vehicles(self): return dict( filter(lambda x: x[1]["state"] in [SimTaxi.STATE.STANDBY], self.vehicles.items())) def dispatch(self, user_id, user_status): start_point = { "arrow_code": self.arrow.get_arrow_codes_from_waypoint_id( user_status["schedules"][0]["start"]["waypoint_id"])[0], "waypoint_id": user_status["schedules"][0]["start"]["waypoint_id"], } vehicles = self.get_dispatchable_vehicles() if len(vehicles) == 0: print("no dispatchable vehicles") return goal_points = [] for vehicle_id, goal_waypoint_id in map( lambda x: (x[0], x[1]["location"]["waypoint_id"]), vehicles.items()): goal_points.append({ "goal_id": vehicle_id, "arrow_code": self.arrow.get_arrow_codes_from_waypoint_id(goal_waypoint_id) [0], "waypoint_id": goal_waypoint_id, }) routes = self.route.get_shortest_routes(start_point, goal_points, reverse=True) if len(routes) == 0: print("no pick_up_route") return pick_up_route = min(routes.items(), key=lambda x: x[1]["cost"])[1] vehicle_id = pick_up_route["goal_id"] start_point = { "arrow_code": self.arrow.get_arrow_codes_from_waypoint_id( user_status["schedules"][0]["start"]["waypoint_id"])[0], "waypoint_id": user_status["schedules"][0]["start"]["waypoint_id"], } goal_points = [{ "goal_id": user_id, "arrow_code": self.arrow.get_arrow_codes_from_waypoint_id( user_status["schedules"][0]["goal"]["waypoint_id"])[0], "waypoint_id": user_status["schedules"][0]["goal"]["waypoint_id"], }] routes = self.route.get_shortest_routes(start_point, goal_points, reverse=False) if len(routes) == 0: print("cant carry_route") return carry_route = min(routes.items(), key=lambda x: x[1]["cost"])[1] current_time = time() vehicle_schedule = deepcopy( self.topicVehicleSubscribe.get_template()["schedules"][0]) vehicle_schedule.update({ "name": "pickup", "start_time": current_time, "duration": 1000, "action": Vehicle.ACTION.MOVE, "route": { "start": { "waypoint_id": pick_up_route["goal_waypoint_id"], }, "goal": { "waypoint_id": pick_up_route["start_waypoint_id"], }, "arrow_codes": pick_up_route["arrow_codes"], } }) self.vehicles[vehicle_id]["schedules"].append(vehicle_schedule) vehicle_schedule = deepcopy( self.topicVehicleSubscribe.get_template()["schedules"][0]) vehicle_schedule.update({ "name": "takeOn", "start_time": current_time + 1000, "duration": 10, "action": Vehicle.ACTION.STOP, }) self.vehicles[vehicle_id]["schedules"].append(vehicle_schedule) vehicle_schedule = deepcopy( self.topicVehicleSubscribe.get_template()["schedules"][0]) vehicle_schedule.update({ "name": "carry", "start_time": current_time + 1010, "duration": 1000, "action": Vehicle.ACTION.MOVE, "route": { "start": { "waypoint_id": carry_route["start_waypoint_id"], }, "goal": { "waypoint_id": carry_route["goal_waypoint_id"], }, "arrow_codes": carry_route["arrow_codes"], } }) self.vehicles[vehicle_id]["schedules"].append(vehicle_schedule) vehicle_schedule = deepcopy( self.topicVehicleSubscribe.get_template()["schedules"][0]) vehicle_schedule.update({ "name": "discharge", "start_time": current_time + 2010, "duration": 10, "action": Vehicle.ACTION.STOP, }) self.vehicles[vehicle_id]["schedules"].append(vehicle_schedule) vehicle_schedule = deepcopy( self.topicVehicleSubscribe.get_template()["schedules"][0]) vehicle_schedule.update({ "name": "standBy", "start_time": current_time + 2020, "duration": 86400, "action": SimTaxi.ACTION.STANDBY, }) self.vehicles[vehicle_id]["schedules"].append(vehicle_schedule) payload = self.topicVehicleSubscribe.serialize( {"schedules": self.vehicles[vehicle_id]["schedules"]}) self.publish( self.topicVehicleSubscribe.root + "/" + vehicle_id + "/schedules", payload) self.relations[user_id] = vehicle_id self.relations[vehicle_id] = user_id self.publish_relations() def publish_relations(self): message = self.topicFleetManagerPublish.get_template() message["time"] = time() message["relations"] = self.relations payload = self.topicFleetManagerPublish.serialize(message) self.publish(self.topicFleetManagerPublish.private, payload)
class Autoware(Vehicle): class ROSTOPIC(object): PUBLISH = "/based/lane_waypoints_array" SUBSCRIBE = "/closest_waypoint" class TOPIC(object): PUBLISH = "pub_autoware" SUBSCRIBE = "sub_autoware" def __init__(self, name, waypoint, arrow, route, waypoint_id, velocity, schedules=None, dt=1.0): super().__init__(name, waypoint, arrow, route, waypoint_id, velocity, schedules, dt) self.name = name self.autowarePublishTopic = Topic() self.autowarePublishTopic.set_id(self.name) self.autowarePublishTopic.set_root(Autoware.TOPIC.PUBLISH) self.autowarePublishTopic.set_message(autoware_message) self.autowareSubscribeTopic = Topic() self.autowareSubscribeTopic.set_id(self.name) self.autowareSubscribeTopic.set_root(Autoware.TOPIC.SUBSCRIBE) self.autowareSubscribeTopic.set_message(autoware_message) self.pose_index = 0 self.current_poses = [] self.add_on_message_function(self.set_autoware_pose) self.set_subscriber(self.autowareSubscribeTopic.private + "/closest_waypoint") def set_autoware_pose(self, _client, _userdata, topic, payload): if topic == self.autowareSubscribeTopic.private + "/closest_waypoint": message = self.autowareSubscribeTopic.unserialize(payload) if 0 <= message["index"] < len(self.current_poses): self.pose_index = message["index"] print(self.current_poses[self.pose_index]) self.arrow_code = self.current_poses[ self.pose_index]["arrow_code"] self.waypoint_id = self.current_poses[ self.pose_index]["waypoint_id"] self.position = self.waypoint.get_position(self.waypoint_id) self.yaw = self.arrow.get_heading(self.arrow_code, self.waypoint_id) else: print("Lost Autoware.") def set_autoware_waypoints(self): waypoints = [] schedule = self.schedules[0] arrow_waypoint_array = self.route.get_arrow_waypoint_array({ "start_waypoint_id": schedule["route"]["start"]["waypoint_id"], "goal_waypoint_id": schedule["route"]["goal"]["waypoint_id"], "arrow_codes": schedule["route"]["arrow_codes"] }) for arrowWaypoint in arrow_waypoint_array: waypoint_id = arrowWaypoint["waypoint_id"] waypoints.append({ "position": dict( zip(["x", "y", "z"], self.waypoint.get_position(waypoint_id))), "orientation": dict( zip(["w", "x", "y", "z"], axangle2quat([0, 0, 1], self.waypoint.get_yaw(waypoint_id)))), "velocity": 2.0 }) if 0 < len(waypoints): num = min(10, len(waypoints)) for i in range(num - 1, 0, -1): waypoints[-i]["velocity"] = ( i / num) * waypoints[-i - 1]["velocity"] self.current_poses = arrow_waypoint_array payload = self.autowarePublishTopic.serialize(waypoints) self.publish(self.autowarePublishTopic.private + "/waypoints", payload)
class Vehicle(EventLoop): class TOPIC(object): PUBLISH = "pub_vehicle" SUBSCRIBE = "sub_vehicle" class GEO(object): PUBLISH = "pub_geo_vehicle" SUBSCRIBE = "sub_geo_vehicle" class STATE(object): MOVE = "move" STOP = "stop" WILL = "will" class ACTION(object): MOVE = "move" STOP = "stop" def __init__(self, name, waypoint, arrow, route, waypoint_id, velocity, schedules=None, dt=1.0): super().__init__() self.topicVehiclePublish = Topic() self.topicVehiclePublish.set_id(self.event_loop_id) self.topicVehiclePublish.set_root(Vehicle.TOPIC.PUBLISH) self.topicVehiclePublish.set_message(vehicle_message) self.topicVehicleSubscribe = Topic() self.topicVehicleSubscribe.set_id(self.event_loop_id) self.topicVehicleSubscribe.set_root(Vehicle.TOPIC.SUBSCRIBE) self.topicVehicleSubscribe.set_message(vehicle_message) self.topicGeoVehiclePublish = Topic() self.topicGeoVehiclePublish.set_id(self.event_loop_id) self.topicGeoVehiclePublish.set_root(Vehicle.TOPIC.GEO.PUBLISH) self.topicGeoVehiclePublish.set_message(geo_vehicle_message) self.name = name self.state = Vehicle.STATE.STOP self.event = None self.action = None self.waypoint = waypoint self.arrow = arrow self.route = route self.waypoint_id = waypoint_id self.position = self.waypoint.get_position(self.waypoint_id) self.velocity = velocity self.schedules = schedules self.dt = dt self.arrow_code = self.arrow.get_arrow_codes_from_waypoint_id(waypoint_id)[0] self.position = self.waypoint.get_position(self.waypoint_id) self.yaw = self.arrow.get_heading(self.arrow_code, self.waypoint_id) self.add_on_message_function(self.update_schedules) self.add_on_message_function(self.update_event) self.set_subscriber(self.topicVehicleSubscribe.private+"/schedules") self.set_subscriber(self.topicVehicleSubscribe.private+"/event") self.set_main_loop(self.__main_loop) def publish_status(self): xyz = self.position.data[:] message = self.topicVehiclePublish.get_template() message["name"] = self.name message["state"] = self.state message["event"] = self.event message["location"]["waypoint_id"] = self.waypoint_id message["location"]["geohash"] = self.waypoint.get_geohash(self.waypoint_id) message["location"]["arrow_code"] = self.arrow_code message["pose"]["position"]["x"] = xyz[0] message["pose"]["position"]["y"] = xyz[1] message["pose"]["position"]["z"] = xyz[2] message["pose"]["orientation"]["yaw"] = self.yaw message["schedules"] = self.schedules payload = self.topicVehiclePublish.serialize(message) self.publish(self.topicVehiclePublish.private, payload) self.publish( self.topicGeoVehiclePublish.root+"/"+"/".join(self.waypoint.get_geohash(self.waypoint_id)), self.event_loop_id) def update_schedules(self, _client, _userdata, topic, payload): if topic == self.topicVehicleSubscribe.private+"/schedules": message = self.topicVehicleSubscribe.unserialize(payload) self.schedules = message["schedules"] def update_event(self, _client, _userdata, topic, payload): if topic == self.topicVehicleSubscribe.private+"/event": message = self.topicVehicleSubscribe.unserialize(payload) self.event = message["event"] def update_status(self): return def __main_loop(self): sleep(1) self.publish_status() while self.schedules is not None: sleep(self.dt) self.update_status() self.publish_status() return True
class SimCar(Vehicle): LOWER_INTER_VEHICLE_DISTANCE = 10.0 LOWER_INTER_TRAFFIC_SIGNAL_DISTANCE = 2.0 FLOAT_MAX = float_info.max def __init__(self, name, waypoint, arrow, route, intersection, waypoint_id, velocity, schedules=None, dt=1.0): super().__init__(name, waypoint, arrow, route, waypoint_id, velocity, schedules, dt) self.topicTrafficSignalPublish = Topic() self.topicTrafficSignalPublish.set_root(TrafficSignal.TOPIC.PUBLISH) self.topicTrafficSignalPublish.set_message(traffic_signal_message) self.__prev_waypoint_id = waypoint_id self.traffic_signals = {} self.other_vehicles = {} self.intersection = intersection self.add_on_message_function(self.set_other_vehicle_poses) self.add_on_message_function(self.set_traffic_signals) self.set_subscriber(self.topicVehiclePublish.all) self.set_subscriber(self.topicTrafficSignalPublish.all) def set_traffic_signals(self, _client, _user_data, topic, payload): if self.topicTrafficSignalPublish.root in topic: message = self.topicTrafficSignalPublish.unserialize(payload) self.traffic_signals.update( dict(map(lambda x: (x["route_code"], x), message["routes"]))) def set_other_vehicle_poses(self, _client, _user_data, topic, payload): if self.topicVehiclePublish.private not in topic and \ self.topicVehiclePublish.root in topic: vehicle_id = self.topicVehiclePublish.get_id(topic) message = self.topicVehiclePublish.unserialize(payload) # todo: localize self.other_vehicles[vehicle_id] = message def get_monitored_route(self, distance=100.0): if distance <= 0: return None arrow_codes = self.schedules[0]["route"]["arrow_codes"] arrow_codes = arrow_codes[arrow_codes.index(self.arrow_code):] route = { "start_waypoint_id": self.waypoint_id, "goal_waypoint_id": self.arrow.get_waypoint_ids( self.schedules[0]["route"]["arrow_codes"][-1])[-1], "arrow_codes": arrow_codes } return self.route.get_sliced_route(route, distance) def __get_inter_vehicle_distance(self, monitored_route): monitored_waypoint_ids = self.route.get_route_waypoint_ids( monitored_route) inter_vehicle_distance = SimCar.FLOAT_MAX if self.arrow_code is not None and 0 < len(self.other_vehicles): other_vehicles_waypoint_ids = list( map(lambda x: x["location"]["waypoint_id"], self.other_vehicles.values())) for i, monitored_waypoint_id in enumerate(monitored_waypoint_ids): if monitored_waypoint_id in other_vehicles_waypoint_ids: inter_vehicle_distance = self.route.get_distance_of_waypoints( monitored_waypoint_ids[0:i + 1]) break # print("inter_vehicle_distance {}[m]".format(inter_vehicle_distance)) return inter_vehicle_distance def __get_inter_traffic_signal_distance(self, monitored_route): monitored_arrow_codes = monitored_route["arrow_codes"] inter_traffic_signal_distance = SimCar.FLOAT_MAX not_green_traffic_signal_route_codes = list( map( lambda x: x["route_code"], filter( lambda x: x["state"] in [TrafficSignal.STATE.YELLOW, TrafficSignal.STATE.RED], self.traffic_signals.values()))) new_monitored_route = { "start_waypoint_id": monitored_route["start_waypoint_id"], "arrow_codes": None, "goal_waypoint_id": 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: start_waypoint_id, arrow_codes, _ = self.route.split_route_code( not_green_traffic_signal_route_code) if monitored_arrow_code == arrow_codes[0]: waypoint_ids = self.arrow.get_waypoint_ids( monitored_arrow_code) if self.waypoint_id not in waypoint_ids or \ waypoint_ids.index(self.waypoint_id) <= waypoint_ids.index(start_waypoint_id): new_monitored_route[ "goal_waypoint_id"] = start_waypoint_id new_monitored_route[ "arrow_codes"] = monitored_arrow_codes[:i] break if new_monitored_route["arrow_codes"] is not None: break if new_monitored_route["arrow_codes"] is not None: inter_traffic_signal_distance = self.route.get_route_length( new_monitored_route) # print("inter_traffic_signal_distance {}[m]".format(inter_traffic_signal_distance)) return inter_traffic_signal_distance def __get_movable_distance(self): movable_distance = SimCar.FLOAT_MAX if 0 < len(self.schedules): if self.schedules[0]["action"] == Vehicle.ACTION.MOVE: # check inter-vehicle distance monitored_route = self.get_monitored_route() if monitored_route is None: return 0.0 inter_vehicle_distance = self.__get_inter_vehicle_distance( monitored_route) movable_distance = inter_vehicle_distance - SimCar.LOWER_INTER_VEHICLE_DISTANCE # check inter-trafficSignal distance monitored_route = self.get_monitored_route(movable_distance) if monitored_route is None: return 0.0 inter_traffic_signal_distance = self.__get_inter_traffic_signal_distance( monitored_route) movable_distance = min( movable_distance, inter_traffic_signal_distance - SimCar.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.__prev_waypoint_id = self.waypoint_id self.position, self.yaw, self.arrow_code, self.waypoint_id = self.get_next_pose( delta_distance) def is_achieved(self): goal_waypoint_id = self.schedules[0]["route"]["goal"]["waypoint_id"] # get pass waypoint_ids waypoint_ids = [] for arrow_code in self.schedules[0]["route"]["arrow_codes"][0:2]: waypoint_ids.extend(self.arrow.get_waypoint_ids(arrow_code)) pass_waypoint_ids = \ waypoint_ids[waypoint_ids.index(self.__prev_waypoint_id):waypoint_ids.index(self.waypoint_id) + 1] return goal_waypoint_id in pass_waypoint_ids def get_next_pose(self, delta_distance): arrows, to_arrows, _ = self.arrow.get_arrow_codes_to_arrows( self.schedules[0]["route"]["arrow_codes"][0:2]) position, arrow_code = self.arrow.get_advanced_position_in_arrows( self.position, delta_distance, arrows=arrows, next_arrows=to_arrows) waypoint_id, position, _ = self.arrow.get_point_to_arrow( position, arrow_code) heading = self.arrow.get_heading(arrow_code, waypoint_id) return position, heading, arrow_code, waypoint_id def update_status(self): current_time = time() if self.state == Vehicle.STATE.STOP: if self.schedules[0]["action"] == Vehicle.ACTION.MOVE: self.state = Vehicle.STATE.MOVE else: if self.schedules[0]["start_time"] + self.schedules[0][ "duration"] <= current_time: if 1 < len(self.schedules): self.schedules.pop(0) # update next schedule dif_time = current_time - self.schedules[0][ "start_time"] self.schedules[0]["start_time"] += dif_time self.schedules[0]["duration_time"] = dif_time self.state = Vehicle.STATE.MOVE elif self.state == Vehicle.STATE.MOVE: self.update_pose() if self.is_achieved(): self.waypoint_id = self.schedules[0]["route"]["goal"][ "waypoint_id"] self.arrow_code = self.schedules[0]["route"]["goal"][ "arrow_codes"][-1] self.position = self.waypoint.get_position(self.waypoint_id) self.yaw = self.arrow.get_heading(self.arrow_code, self.waypoint_id) self.schedules.pop(0) # update next schedule new_start_time = time() dif_time = new_start_time - self.schedules[0]["start_time"] self.schedules[0]["start_time"] += dif_time self.schedules[0]["duration_time"] = dif_time self.state = Vehicle.STATE.STOP else: arrow_codes = self.schedules[0]["route"]["arrow_codes"] self.schedules[0]["route"]["arrow_codes"] = arrow_codes[ arrow_codes.index(self.arrow_code):]