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)
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)
class User(EventLoop):
class TOPIC(object):
PUBLISH = "pubUser"
SUBSCRIBE = "subUser"
class STATE(object):
LOGIN = "******"
WAITING = "waiting"
GETTING_ON = "gettingOn"
GOT_ON = "gotOn"
MOVING = "moving"
GETTING_OUT = "gettingOut"
GOT_OUT = "gotOut"
class ACTION(object):
WAIT = "wait"
GET_ON = "getOn"
GET_OUT = "getOut"
class EVENT(object):
MOVE_VEHICLE = "moveVehicle"
def __init__(self, name, waypoint, dt=1.0):
super().__init__()
self.topicUserPublish = Topic()
self.topicUserPublish.set_id(self.event_loop_id)
self.topicUserPublish.set_root(User.TOPIC.PUBLISH)
self.topicUserPublish.set_message(user_message)
self.topicUserSubscribe = Topic()
self.topicUserSubscribe.set_id(self.event_loop_id)
self.topicUserSubscribe.set_root(User.TOPIC.SUBSCRIBE)
self.topicUserSubscribe.set_message(user_message)
self.name = name
self.id = self.event_loop_id
self.state = User.STATE.LOGIN
self.event = None
self.action = None
self.dt = dt
self.__start_waypoint_id = None
self.__goal_waypoint_id = None
self.__vehicleID = None
self.__waypoint = waypoint
self.add_on_message_function(self.update_action)
self.add_on_message_function(self.update_event)
self.set_subscriber(self.topicUserSubscribe.private + "/schedules")
self.set_subscriber(self.topicUserSubscribe.private + "/event")
self.set_main_loop(self.__main_loop)
def publish_status(self):
message = self.topicUserPublish.get_template()
message["name"] = self.name
message["state"] = self.state
message["event"] = self.event
message["schedules"][0]["action"] = self.action
message["schedules"][0]["start"][
"waypoint_id"] = self.__start_waypoint_id
message["schedules"][0]["goal"][
"waypoint_id"] = self.__goal_waypoint_id
payload = self.topicUserPublish.serialize(message)
self.publish(self.topicUserPublish.private, payload)
def set_waypoint_at_random(self, waypoint_ids=None):
if waypoint_ids is None:
waypoint_ids = self.__waypoint.get_waypoint_ids()
start_waypoint_id = random.choice(waypoint_ids)
waypoint_ids.remove(start_waypoint_id)
goal_waypoint_id = random.choice(waypoint_ids)
self.set_waypoint(start_waypoint_id, goal_waypoint_id)
def set_waypoint(self, start_waypoint_id, goal_waypoint_id):
self.set_start_waypoint(start_waypoint_id)
self.set_goal_waypoint(goal_waypoint_id)
def set_start_waypoint(self, start_waypoint_id):
self.__start_waypoint_id = start_waypoint_id
def set_goal_waypoint(self, goal_waypoint_id):
self.__goal_waypoint_id = goal_waypoint_id
def update_action(self, _client, _userdata, topic, payload):
# print(topic)
if topic == self.topicUserSubscribe.private + "/schedules":
message = self.topicUserSubscribe.unserialize(payload)
self.action = message["schedules"][0]["action"]
def update_event(self, _client, _userdata, topic, payload):
if topic == self.topicUserSubscribe.private + "/event":
message = self.topicUserSubscribe.unserialize(payload)
self.event = message["event"]
def update_status(self):
# print(self.state, self.event, self.action)
if self.state == User.STATE.LOGIN:
if self.action == User.ACTION.WAIT:
self.state = User.STATE.WAITING
self.action = None
elif self.state == User.STATE.WAITING:
if self.action == User.ACTION.GET_ON:
self.state = User.STATE.GETTING_ON
self.action = None
elif self.state == User.STATE.GETTING_ON:
self.state = User.STATE.GOT_ON
elif self.state == User.STATE.GOT_ON:
if self.event == User.EVENT.MOVE_VEHICLE:
self.state = User.STATE.MOVING
self.event = None
elif self.state == User.STATE.MOVING:
if self.action == User.ACTION.GET_OUT:
self.state = User.STATE.GETTING_OUT
self.action = None
elif self.state == User.STATE.GETTING_OUT:
self.state = User.STATE.GOT_OUT
def __main_loop(self):
self.publish_status()
while self.state != User.STATE.GOT_OUT:
sleep(self.dt)
self.update_status()
self.publish_status()
sleep(2)
class EventLoop(object):
KEEP_ALIVE = 60
class TOPIC(object):
PUBLISH = "pub_event_loop"
SUBSCRIBE = "sub_event_loop"
def __init__(self, _id=None):
self.event_loop_id = _id
if _id is None:
self.event_loop_id = str(uuid())
self.__subscribeTopic = Topic()
self.__subscribeTopic.set_id(self.event_loop_id)
self.__subscribeTopic.set_root(EventLoop.TOPIC.SUBSCRIBE)
self.__subscribeTopic.set_message(event_loop_message)
self.__publishTopic = Topic()
self.__publishTopic.set_id(self.event_loop_id)
self.__publishTopic.set_root(EventLoop.TOPIC.PUBLISH)
self.__publishTopic.set_message(event_loop_message)
self.__subscribers = {}
self.__publishers = {}
self.__client = None
self.__main_loop = None
self.__pid = os.getpid()
self.set_subscriber(self.__subscribeTopic.private)
self.__on_message_functions = []
self.__user_data = None
self.__user_will = None
def __del__(self):
if self.__client is not None:
message = self.__publishTopic.get_template()
message["action"] = "disconnect"
message["pid"] = self.__pid
payload = self.__publishTopic.serialize(message)
self.publish(self.__publishTopic.private, payload)
self.__client.disconnect()
def set_subscriber(self, topic):
self.__subscribers[str(uuid())] = {"topic": topic}
def set_user_data(self, user_data):
self.__user_data = user_data
def add_on_message_function(self, on_message_function):
self.__on_message_functions.append(on_message_function)
def set_main_loop(self, main_loop):
self.__main_loop = main_loop
def set_will(self, topic, payload):
self.__user_will = {"topic": topic, "payload": payload}
def publish(self, topic, payload, qos=0, retain=False):
# payload = self.__subscribeTopic.serialize(message)
self.__client.publish(topic, payload=payload, qos=qos, retain=retain)
def __on_message(self, client, userdata, message_data):
payload = message_data.payload.decode("utf-8")
if self.__subscribeTopic.root in message_data.topic and \
self.__subscribeTopic.get_id(message_data.topic) == self.event_loop_id:
message = self.__subscribeTopic.unserialize(payload)
if message["action"] == "start":
print(self.__subscribeTopic.root, message)
if message["action"] == "kill":
print(self.__subscribeTopic.root, message)
self.end()
if message["action"] == "check":
print(self.__subscribeTopic.root, message)
self.__check()
for onMessageFunction in self.__on_message_functions:
onMessageFunction(client, userdata, message_data.topic, payload)
return True
def connect(self, host, port):
self.__client = mqtt.Client(protocol=mqtt.MQTTv311,
userdata=self.__user_data)
will = self.__user_will
print(will)
if will is None:
message = self.__publishTopic.get_template()
message["action"] = "will"
message["pid"] = self.__pid
payload = self.__publishTopic.serialize(message)
will = {"topic": self.__publishTopic.private, "payload": payload}
self.__client.will_set(will["topic"],
payload=will["payload"],
qos=2,
retain=False)
self.__client.on_message = self.__on_message
self.__client.connect(host=host,
port=port,
keepalive=EventLoop.KEEP_ALIVE)
def start(self, host="localhost", port=1883):
self.connect(host, port)
for subscriber in self.__subscribers.values():
self.__client.subscribe(subscriber["topic"])
message = self.__publishTopic.get_template()
message["time"] = str(int(time()))
message["action"] = "start"
message["pid"] = self.__pid
payload = self.__publishTopic.serialize(message)
self.publish(self.__publishTopic.private, payload)
if self.__main_loop is None:
self.__client.loop_forever()
else:
self.__client.loop_start()
self.__main_loop()
def end(self):
self.__client.loop_stop()
self.__client.disconnect()
os.kill(self.__pid, SIGKILL)
def __check(self):
# todo: main_loop zombie
message = self.__publishTopic.get_template()
message["time"] = str(int(time()))
message["action"] = "ok"
message["pid"] = self.__pid
payload = self.__publishTopic.serialize(message)
self.publish(self.__publishTopic.private, payload)
def get_pid(self):
return self.__pid
class TrafficSignal(EventLoop):
class STATE(object):
GREEN = "green"
YELLOW = "yellow"
RED = "red"
class TOPIC(object):
PUBLISH = "pub_traffic_signal"
SUBSCRIBE = "sub_traffic_signal"
def __init__(self, name, processing_cycle=1.0):
super().__init__()
self.trafficSignalPublishTopic = Topic()
self.trafficSignalPublishTopic.set_id(self.event_loop_id)
self.trafficSignalPublishTopic.set_root(TrafficSignal.TOPIC.PUBLISH)
self.trafficSignalPublishTopic.set_message(traffic_signal_message)
self.trafficSignalSubscribeTopic = Topic()
self.trafficSignalSubscribeTopic.set_id(self.event_loop_id)
self.trafficSignalSubscribeTopic.set_root(
TrafficSignal.TOPIC.SUBSCRIBE)
self.trafficSignalSubscribeTopic.set_message(traffic_signal_message)
self.name = name
self.routes = {}
self.schedules = {}
self.cycles = {}
self.__processing_cycle = processing_cycle
self.__check_time = time()
self.__publish_flag = False
self.add_on_message_function(self.update_routes)
self.add_on_message_function(self.update_schedules)
self.add_on_message_function(self.update_cycles)
self.set_subscriber(self.trafficSignalSubscribeTopic.private +
"/routes")
self.set_subscriber(self.trafficSignalSubscribeTopic.private +
"/schedules")
self.set_subscriber(self.trafficSignalSubscribeTopic.private +
"/cycles")
self.set_main_loop(self.__main_loop)
def publish_status(self):
message = self.trafficSignalPublishTopic.get_template()
message["name"] = self.name
message["time"] = time()
message["routes"] = list(self.routes.values())
payload = self.trafficSignalPublishTopic.serialize(message)
self.publish(self.trafficSignalPublishTopic.private, payload)
def update_routes(self, _client, _userdata, topic, payload):
if topic == self.trafficSignalSubscribeTopic.private + "/routes":
routes = self.trafficSignalSubscribeTopic.unserialize(payload)
self.routes = dict(map(lambda x: (x["route_code"], x), routes))
self.__publish_flag = True
def update_schedules(self, _client, _userdata, topic, payload):
if topic == self.trafficSignalSubscribeTopic.private + "/schedules":
schedules = self.trafficSignalSubscribeTopic.unserialize(payload)
self.schedules = dict(
map(lambda x: (x["route_code"], [x]), schedules))
self.__publish_flag = True
def update_cycles(self, _client, _userdata, topic, payload):
if topic == self.trafficSignalSubscribeTopic.private + "/cycles":
cycles = self.trafficSignalSubscribeTopic.unserialize(payload)
for cycle in cycles:
for route_code in cycle["route_codes"]:
self.cycles[route_code] = cycle
self.__publish_flag = True
@staticmethod
def get_schedule_from_cycle(cycle, start_time):
base_time = cycle["base_time"]
period = cycle["period"]
phase_time = (start_time - base_time) % period
state = None
end_time = start_time
elapse_time = 0
for phase in cycle["phases"]:
elapse_time += phase["duration"]
state = phase["state"]
end_time = start_time + (elapse_time - phase_time)
if phase_time < elapse_time:
break
schedule = {
"state": state,
"start_time": start_time,
"duration": end_time - start_time,
}
return schedule
def __update_schedules(self):
current_time = time()
for route_code, route in self.routes.items():
route_schedules = []
if route_code in self.schedules:
route_schedules = list(
filter(
lambda x: current_time <= x["start_time"] + x[
"duration"], self.schedules[route_code]))
if route_code in self.cycles:
if len(route_schedules) < 3:
if len(route_schedules) == 0:
start_time = current_time
else:
start_time = route_schedules[-1][
"start_time"] + route_schedules[-1]["duration"]
route_schedules.append(
self.get_schedule_from_cycle(self.cycles[route_code],
start_time))
self.schedules[route_code] = route_schedules
def update_status(self):
for route_code, route in self.routes.items():
if len(self.schedules[route_code]) == 0:
if route["state"] is not None:
self.routes[route_code]["state"] = None
self.__publish_flag = True
else:
current_schedule = self.schedules[route_code][0]
state = current_schedule["state"]
if route["state"] != state:
self.routes[route_code]["state"] = state
self.__publish_flag = True
def update_check_time(self):
if len(self.routes) == 0:
return 1
self.__check_time = time() + 600
for route_schedules in self.schedules.values():
if 0 < len(route_schedules):
self.__check_time = min(
self.__check_time, route_schedules[0]["start_time"] +
route_schedules[0]["duration"])
def __main_loop(self):
while True:
if self.__check_time <= time():
self.__update_schedules()
self.update_status()
self.update_check_time()
if self.__publish_flag:
self.publish_status()
self.__check_time = time()
self.__publish_flag = False
sleep(self.__processing_cycle)
class FleetManager(EventLoop):
class ACTION(object):
PUBLISH_RELATIONS = "pub_relations"
class TOPIC(object):
PUBLISH = "pub_fleet_manager"
SUBSCRIBE = "sub_fleet_manager"
def __init__(self, waypoint, arrow, route):
super().__init__()
self.topicUserPublish = Topic()
self.topicUserPublish.set_root(User.TOPIC.PUBLISH)
self.topicUserPublish.set_message(user_message)
self.topicUserSubscribe = Topic()
self.topicUserSubscribe.set_root(User.TOPIC.SUBSCRIBE)
self.topicUserSubscribe.set_message(user_message)
self.topicVehiclePublish = Topic()
self.topicVehiclePublish.set_root(Vehicle.TOPIC.PUBLISH)
self.topicVehiclePublish.set_message(vehicle_message)
self.topicVehicleSubscribe = Topic()
self.topicVehicleSubscribe.set_root(Vehicle.TOPIC.SUBSCRIBE)
self.topicVehicleSubscribe.set_message(vehicle_message)
self.topicTrafficSignalPublish = Topic()
self.topicTrafficSignalPublish.set_root(TrafficSignal.TOPIC.PUBLISH)
self.topicTrafficSignalPublish.set_message(traffic_signal_message)
self.topicFleetManagerPublish = Topic()
self.topicFleetManagerPublish.set_id(self.event_loop_id)
self.topicFleetManagerPublish.set_root(FleetManager.TOPIC.PUBLISH)
self.topicFleetManagerPublish.set_message(fleet_manager_message)
self.topicFleetManagerSubscribe = Topic()
self.topicFleetManagerSubscribe.set_id(self.event_loop_id)
self.topicFleetManagerSubscribe.set_root(FleetManager.TOPIC.SUBSCRIBE)
self.topicFleetManagerSubscribe.set_message(fleet_manager_message)
self.waypoint = waypoint
self.arrow = arrow
self.route = route
self.users = {}
self.vehicles = {}
self.traffic_signals = {}
self.relations = {} # vehicle_id -> user_id, user_id -> vehicle_id
self.add_on_message_function(self.update_user_status)
self.add_on_message_function(self.update_vehicle_status)
self.add_on_message_function(self.update_traffic_signal_status)
self.add_on_message_function(self.response_request)
self.set_subscriber(self.topicUserPublish.all)
self.set_subscriber(self.topicVehiclePublish.all)
self.set_subscriber(self.topicTrafficSignalPublish.all)
self.set_subscriber(self.topicFleetManagerSubscribe.all)
def update_user_status(self, _client, _userdata, topic, payload):
# print("update_user_status", topic)
if self.topicUserPublish.root in topic:
user_id = self.topicUserPublish.get_id(topic)
message = self.topicUserPublish.unserialize(payload)
if message["state"] == User.STATE.LOGIN:
# print("user", User.STATE.LOGIN)
# todo: move to dispatcher class
self.dispatch(user_id, message)
elif message["state"] == User.STATE.WAITING:
pass
# print("user", User.STATE.WAITING)
elif message["state"] == User.STATE.GETTING_ON:
pass
# print("user", User.STATE.GETTING_ON)
elif message["state"] == User.STATE.GOT_ON:
# print("user", User.STATE.GOT_ON)
vehicle_id = self.relations[user_id]
self.vehicles[vehicle_id]["schedules"][0][
"action"] = Vehicle.ACTION.MOVE
payload = self.topicVehicleSubscribe.serialize(
{"schedules": self.vehicles[vehicle_id]["schedules"]})
self.publish(
self.topicVehicleSubscribe.root + "/" + vehicle_id +
"/schedules", payload)
elif message["state"] == User.STATE.MOVING:
pass
# print("user", User.STATE.MOVING)
elif message["state"] == User.STATE.GETTING_OUT:
pass
# print("user", User.STATE.GETTING_OUT)
elif message["state"] == User.STATE.GOT_OUT:
pass
# print("user", User.STATE.GOT_OUT)
else:
print("user", message["state"])
self.users[user_id] = message
def update_vehicle_status(self, _client, _userdata, topic, payload):
if self.topicVehiclePublish.root in topic:
vehicle_id = self.topicVehiclePublish.get_id(topic)
message = self.topicVehiclePublish.unserialize(payload)
# print("update_vehicle_status", topic, message["state"])
if vehicle_id in self.relations:
prev_state = self.vehicles[vehicle_id]["state"]
if message["state"] == SimTaxi.STATE.MOVE_TO_USER:
# print("vehicle", SimTaxi.STATE.MOVE_TO_USER)
if prev_state == SimTaxi.STATE.STANDBY:
user_id = self.relations[vehicle_id]
self.users[user_id]["schedules"][0][
"action"] = User.ACTION.WAIT
self.publish(
self.topicUserSubscribe.root + "/" + user_id +
"/schedules",
self.topicUserSubscribe.serialize({
"schedules":
self.users[user_id]["schedules"]
}))
elif message["state"] == SimTaxi.STATE.STOP_FOR_PICKING_UP:
# print("vehicle", SimTaxi.STATE.STOP_FOR_PICKING_UP)
if prev_state == SimTaxi.STATE.MOVE_TO_USER:
# print("vehicle", SimTaxi.STATE.STOP_FOR_PICKING_UP, SimTaxi.STATE.MOVE_TO_USER)
user_id = self.relations[vehicle_id]
self.users[user_id]["schedules"][0][
"action"] = User.ACTION.GET_ON
self.publish(
self.topicUserSubscribe.root + "/" + user_id +
"/schedules",
self.topicUserSubscribe.serialize({
"schedules":
self.users[user_id]["schedules"]
}))
elif message[
"state"] == SimTaxi.STATE.MOVE_TO_USER_DESTINATION:
# print("vehicle", SimTaxi.STATE.MOVE_TO_USER_DESTINATION)
if prev_state == SimTaxi.STATE.STOP_FOR_PICKING_UP:
# print("vehicle", SimTaxi.STATE.MOVE_TO_USER_DESTINATION, SimTaxi.STATE.STOP_FOR_PICKING_UP)
user_id = self.relations[vehicle_id]
self.users[user_id]["schedules"][0][
"event"] = User.EVENT.MOVE_VEHICLE
self.publish(
self.topicUserSubscribe.root + "/" + user_id +
"/event",
self.topicUserSubscribe.serialize(
{"event": User.EVENT.MOVE_VEHICLE}))
elif message["state"] == SimTaxi.STATE.STOP_FOR_DISCHARGING:
# print("vehicle", SimTaxi.STATE.STOP_FOR_DISCHARGING)
if prev_state == SimTaxi.STATE.MOVE_TO_USER_DESTINATION:
user_id = self.relations[vehicle_id]
self.users[user_id]["schedules"][0][
"action"] = User.ACTION.GET_OUT
self.publish(
self.topicUserSubscribe.root + "/" + user_id +
"/schedules",
self.topicUserSubscribe.serialize({
"schedules":
self.users[user_id]["schedules"]
}))
elif message["state"] == SimTaxi.STATE.MOVE_TO_STANDBY:
pass
# print("vehicle", SimTaxi.STATE.MOVE_TO_STANDBY)
elif message["state"] == SimTaxi.STATE.STANDBY:
pass
# print("vehicle", SimTaxi.STATE.STANDBY)
else:
print("vehicle", message["state"])
# vehicleSchedule = vehicle.pop("schedule")
if vehicle_id not in self.vehicles:
# print("set vehicle", vehicle_id, message["name"])
self.vehicles[vehicle_id] = message
else:
# print("update vehicle", vehicle_id, message["name"])
self.vehicles[vehicle_id].update(message)
def update_traffic_signal_status(self, _client, _userdata, topic, payload):
if self.topicTrafficSignalPublish.root in topic:
message = self.topicTrafficSignalPublish.unserialize(payload)
for route in message["routes"]:
self.traffic_signals[route["route_code"]] = route
def response_request(self, _client, _userdata, topic, payload):
if self.topicFleetManagerSubscribe.root in topic:
message = self.topicFleetManagerSubscribe.unserialize(payload)
if message["action"] == FleetManager.ACTION.PUBLISH_RELATIONS:
self.publish_relations()
def get_dispatchable_vehicles(self):
return dict(
filter(lambda x: x[1]["state"] in [SimTaxi.STATE.STANDBY],
self.vehicles.items()))
def dispatch(self, user_id, user_status):
start_point = {
"arrow_code":
self.arrow.get_arrow_codes_from_waypoint_id(
user_status["schedules"][0]["start"]["waypoint_id"])[0],
"waypoint_id":
user_status["schedules"][0]["start"]["waypoint_id"],
}
vehicles = self.get_dispatchable_vehicles()
if len(vehicles) == 0:
print("no dispatchable vehicles")
return
goal_points = []
for vehicle_id, goal_waypoint_id in map(
lambda x: (x[0], x[1]["location"]["waypoint_id"]),
vehicles.items()):
goal_points.append({
"goal_id":
vehicle_id,
"arrow_code":
self.arrow.get_arrow_codes_from_waypoint_id(goal_waypoint_id)
[0],
"waypoint_id":
goal_waypoint_id,
})
routes = self.route.get_shortest_routes(start_point,
goal_points,
reverse=True)
if len(routes) == 0:
print("no pick_up_route")
return
pick_up_route = min(routes.items(), key=lambda x: x[1]["cost"])[1]
vehicle_id = pick_up_route["goal_id"]
start_point = {
"arrow_code":
self.arrow.get_arrow_codes_from_waypoint_id(
user_status["schedules"][0]["start"]["waypoint_id"])[0],
"waypoint_id":
user_status["schedules"][0]["start"]["waypoint_id"],
}
goal_points = [{
"goal_id":
user_id,
"arrow_code":
self.arrow.get_arrow_codes_from_waypoint_id(
user_status["schedules"][0]["goal"]["waypoint_id"])[0],
"waypoint_id":
user_status["schedules"][0]["goal"]["waypoint_id"],
}]
routes = self.route.get_shortest_routes(start_point,
goal_points,
reverse=False)
if len(routes) == 0:
print("cant carry_route")
return
carry_route = min(routes.items(), key=lambda x: x[1]["cost"])[1]
current_time = time()
vehicle_schedule = deepcopy(
self.topicVehicleSubscribe.get_template()["schedules"][0])
vehicle_schedule.update({
"name": "pickup",
"start_time": current_time,
"duration": 1000,
"action": Vehicle.ACTION.MOVE,
"route": {
"start": {
"waypoint_id": pick_up_route["goal_waypoint_id"],
},
"goal": {
"waypoint_id": pick_up_route["start_waypoint_id"],
},
"arrow_codes": pick_up_route["arrow_codes"],
}
})
self.vehicles[vehicle_id]["schedules"].append(vehicle_schedule)
vehicle_schedule = deepcopy(
self.topicVehicleSubscribe.get_template()["schedules"][0])
vehicle_schedule.update({
"name": "takeOn",
"start_time": current_time + 1000,
"duration": 10,
"action": Vehicle.ACTION.STOP,
})
self.vehicles[vehicle_id]["schedules"].append(vehicle_schedule)
vehicle_schedule = deepcopy(
self.topicVehicleSubscribe.get_template()["schedules"][0])
vehicle_schedule.update({
"name": "carry",
"start_time": current_time + 1010,
"duration": 1000,
"action": Vehicle.ACTION.MOVE,
"route": {
"start": {
"waypoint_id": carry_route["start_waypoint_id"],
},
"goal": {
"waypoint_id": carry_route["goal_waypoint_id"],
},
"arrow_codes": carry_route["arrow_codes"],
}
})
self.vehicles[vehicle_id]["schedules"].append(vehicle_schedule)
vehicle_schedule = deepcopy(
self.topicVehicleSubscribe.get_template()["schedules"][0])
vehicle_schedule.update({
"name": "discharge",
"start_time": current_time + 2010,
"duration": 10,
"action": Vehicle.ACTION.STOP,
})
self.vehicles[vehicle_id]["schedules"].append(vehicle_schedule)
vehicle_schedule = deepcopy(
self.topicVehicleSubscribe.get_template()["schedules"][0])
vehicle_schedule.update({
"name": "standBy",
"start_time": current_time + 2020,
"duration": 86400,
"action": SimTaxi.ACTION.STANDBY,
})
self.vehicles[vehicle_id]["schedules"].append(vehicle_schedule)
payload = self.topicVehicleSubscribe.serialize(
{"schedules": self.vehicles[vehicle_id]["schedules"]})
self.publish(
self.topicVehicleSubscribe.root + "/" + vehicle_id + "/schedules",
payload)
self.relations[user_id] = vehicle_id
self.relations[vehicle_id] = user_id
self.publish_relations()
def publish_relations(self):
message = self.topicFleetManagerPublish.get_template()
message["time"] = time()
message["relations"] = self.relations
payload = self.topicFleetManagerPublish.serialize(message)
self.publish(self.topicFleetManagerPublish.private, payload)
class Autoware(Vehicle):
class ROSTOPIC(object):
PUBLISH = "/based/lane_waypoints_array"
SUBSCRIBE = "/closest_waypoint"
class TOPIC(object):
PUBLISH = "pub_autoware"
SUBSCRIBE = "sub_autoware"
def __init__(self,
name,
waypoint,
arrow,
route,
waypoint_id,
velocity,
schedules=None,
dt=1.0):
super().__init__(name, waypoint, arrow, route, waypoint_id, velocity,
schedules, dt)
self.name = name
self.autowarePublishTopic = Topic()
self.autowarePublishTopic.set_id(self.name)
self.autowarePublishTopic.set_root(Autoware.TOPIC.PUBLISH)
self.autowarePublishTopic.set_message(autoware_message)
self.autowareSubscribeTopic = Topic()
self.autowareSubscribeTopic.set_id(self.name)
self.autowareSubscribeTopic.set_root(Autoware.TOPIC.SUBSCRIBE)
self.autowareSubscribeTopic.set_message(autoware_message)
self.pose_index = 0
self.current_poses = []
self.add_on_message_function(self.set_autoware_pose)
self.set_subscriber(self.autowareSubscribeTopic.private +
"/closest_waypoint")
def set_autoware_pose(self, _client, _userdata, topic, payload):
if topic == self.autowareSubscribeTopic.private + "/closest_waypoint":
message = self.autowareSubscribeTopic.unserialize(payload)
if 0 <= message["index"] < len(self.current_poses):
self.pose_index = message["index"]
print(self.current_poses[self.pose_index])
self.arrow_code = self.current_poses[
self.pose_index]["arrow_code"]
self.waypoint_id = self.current_poses[
self.pose_index]["waypoint_id"]
self.position = self.waypoint.get_position(self.waypoint_id)
self.yaw = self.arrow.get_heading(self.arrow_code,
self.waypoint_id)
else:
print("Lost Autoware.")
def set_autoware_waypoints(self):
waypoints = []
schedule = self.schedules[0]
arrow_waypoint_array = self.route.get_arrow_waypoint_array({
"start_waypoint_id":
schedule["route"]["start"]["waypoint_id"],
"goal_waypoint_id":
schedule["route"]["goal"]["waypoint_id"],
"arrow_codes":
schedule["route"]["arrow_codes"]
})
for arrowWaypoint in arrow_waypoint_array:
waypoint_id = arrowWaypoint["waypoint_id"]
waypoints.append({
"position":
dict(
zip(["x", "y", "z"],
self.waypoint.get_position(waypoint_id))),
"orientation":
dict(
zip(["w", "x", "y", "z"],
axangle2quat([0, 0, 1],
self.waypoint.get_yaw(waypoint_id)))),
"velocity":
2.0
})
if 0 < len(waypoints):
num = min(10, len(waypoints))
for i in range(num - 1, 0, -1):
waypoints[-i]["velocity"] = (
i / num) * waypoints[-i - 1]["velocity"]
self.current_poses = arrow_waypoint_array
payload = self.autowarePublishTopic.serialize(waypoints)
self.publish(self.autowarePublishTopic.private + "/waypoints",
payload)
class Vehicle(EventLoop):
class TOPIC(object):
PUBLISH = "pub_vehicle"
SUBSCRIBE = "sub_vehicle"
class GEO(object):
PUBLISH = "pub_geo_vehicle"
SUBSCRIBE = "sub_geo_vehicle"
class STATE(object):
MOVE = "move"
STOP = "stop"
WILL = "will"
class ACTION(object):
MOVE = "move"
STOP = "stop"
def __init__(self, name, waypoint, arrow, route, waypoint_id, velocity, schedules=None, dt=1.0):
super().__init__()
self.topicVehiclePublish = Topic()
self.topicVehiclePublish.set_id(self.event_loop_id)
self.topicVehiclePublish.set_root(Vehicle.TOPIC.PUBLISH)
self.topicVehiclePublish.set_message(vehicle_message)
self.topicVehicleSubscribe = Topic()
self.topicVehicleSubscribe.set_id(self.event_loop_id)
self.topicVehicleSubscribe.set_root(Vehicle.TOPIC.SUBSCRIBE)
self.topicVehicleSubscribe.set_message(vehicle_message)
self.topicGeoVehiclePublish = Topic()
self.topicGeoVehiclePublish.set_id(self.event_loop_id)
self.topicGeoVehiclePublish.set_root(Vehicle.TOPIC.GEO.PUBLISH)
self.topicGeoVehiclePublish.set_message(geo_vehicle_message)
self.name = name
self.state = Vehicle.STATE.STOP
self.event = None
self.action = None
self.waypoint = waypoint
self.arrow = arrow
self.route = route
self.waypoint_id = waypoint_id
self.position = self.waypoint.get_position(self.waypoint_id)
self.velocity = velocity
self.schedules = schedules
self.dt = dt
self.arrow_code = self.arrow.get_arrow_codes_from_waypoint_id(waypoint_id)[0]
self.position = self.waypoint.get_position(self.waypoint_id)
self.yaw = self.arrow.get_heading(self.arrow_code, self.waypoint_id)
self.add_on_message_function(self.update_schedules)
self.add_on_message_function(self.update_event)
self.set_subscriber(self.topicVehicleSubscribe.private+"/schedules")
self.set_subscriber(self.topicVehicleSubscribe.private+"/event")
self.set_main_loop(self.__main_loop)
def publish_status(self):
xyz = self.position.data[:]
message = self.topicVehiclePublish.get_template()
message["name"] = self.name
message["state"] = self.state
message["event"] = self.event
message["location"]["waypoint_id"] = self.waypoint_id
message["location"]["geohash"] = self.waypoint.get_geohash(self.waypoint_id)
message["location"]["arrow_code"] = self.arrow_code
message["pose"]["position"]["x"] = xyz[0]
message["pose"]["position"]["y"] = xyz[1]
message["pose"]["position"]["z"] = xyz[2]
message["pose"]["orientation"]["yaw"] = self.yaw
message["schedules"] = self.schedules
payload = self.topicVehiclePublish.serialize(message)
self.publish(self.topicVehiclePublish.private, payload)
self.publish(
self.topicGeoVehiclePublish.root+"/"+"/".join(self.waypoint.get_geohash(self.waypoint_id)),
self.event_loop_id)
def update_schedules(self, _client, _userdata, topic, payload):
if topic == self.topicVehicleSubscribe.private+"/schedules":
message = self.topicVehicleSubscribe.unserialize(payload)
self.schedules = message["schedules"]
def update_event(self, _client, _userdata, topic, payload):
if topic == self.topicVehicleSubscribe.private+"/event":
message = self.topicVehicleSubscribe.unserialize(payload)
self.event = message["event"]
def update_status(self):
return
def __main_loop(self):
sleep(1)
self.publish_status()
while self.schedules is not None:
sleep(self.dt)
self.update_status()
self.publish_status()
return True