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 __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 __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 __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 __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 __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 __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 __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 __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 __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)
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"})
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 __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 __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 __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 __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 __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 __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)
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)
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 __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 __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)
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
arrow = Arrow(waypoint) arrow.load(ARROW_FILE) route = Route() route.set_waypoint(waypoint) route.set_arrow(arrow) intersection = Intersection() intersection.load(INTERSECTION_FILE) start_waypoint_id = "9566" # 9232 currentTime = time() topic = Topic() topic.set_message(vehicle_message) schedules = deepcopy(topic.get_template()["schedules"]) schedules[0]["start_time"] = currentTime - 5 schedules[0]["duration"] = 10 schedules[0]["action"] = Vehicle.ACTION.STOP # schedules[0]["route"] = None # """ next_start_waypoint_id = start_waypoint_id for i in range(10): startPoint = { "arrow_code": arrow.get_arrow_codes_from_waypoint_id(next_start_waypoint_id)[0], "waypoint_id": next_start_waypoint_id,
args = parser.parse_args() 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()
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
def publish(self, topic, message): 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)
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)
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)