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)
