pp = PrettyPrinter(indent=2).pprint
WAYPOINT_FILE = "../../res/waypoint.json"
ARROW_FILE = "../../res/arrow.json"
INTERSECTION_FILE = "../../res/intersection.json"
if __name__ == '__main__':
waypoint = Waypoint()
waypoint.load(WAYPOINT_FILE)
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
class MapMatch(object):
def __init__(self):
self.__waypoint = None
self.__arrow = None
self.__route = Route()
def set_waypoint(self, waypoint):
self.__waypoint = waypoint
self.__route.set_waypoint(self.__waypoint)
def set_arrow(self, arrow):
self.__arrow = arrow
self.__route.set_arrow(self.__arrow)
@staticmethod
def get_similarity_between_poses(pose1, pose2):
similarity = 0.0
similarity -= abs(pose1.position.x - pose2.position.x)
similarity -= abs(pose1.position.y - pose2.position.y)
similarity -= abs(pose1.position.z - pose2.position.z)
if None not in [pose1.orientation, pose2.orientation]:
if None not in [pose1.orientation.rpy, pose2.orientation.rpy]:
if None not in [
pose1.orientation.rpy.roll, pose2.orientation.rpy.roll
]:
similarity -= abs(pose1.orientation.rpy.roll -
pose2.orientation.rpy.roll)
if None not in [
pose1.orientation.rpy.pitch,
pose2.orientation.rpy.pitch
]:
similarity -= abs(pose1.orientation.rpy.pitch -
pose2.orientation.rpy.pitch)
if None not in [
pose1.orientation.rpy.yaw, pose2.orientation.rpy.yaw
]:
similarity -= abs(pose1.orientation.rpy.yaw -
pose2.orientation.rpy.yaw)
if None not in [
pose1.orientation.quaternion, pose2.orientation.quaternion
]:
similarity -= abs(pose1.orientation.quaternion.w -
pose2.orientation.quaternion.w)
similarity -= abs(pose1.orientation.quaternion.x -
pose2.orientation.quaternion.x)
similarity -= abs(pose1.orientation.quaternion.y -
pose2.orientation.quaternion.y)
similarity -= abs(pose1.orientation.quaternion.z -
pose2.orientation.quaternion.z)
return similarity
def get_matched_location_on_route(self, pose, route):
locations = self.__route.get_locations(route)
similarity_max = 0.0
matched_location = None
for location in locations:
pose_on_route = self.__waypoint.get_pose(location.waypoint_id)
similarity = MapMatch.get_similarity_between_poses(
pose, pose_on_route)
if matched_location is None or similarity_max < similarity:
matched_location = location
similarity_max = similarity
return matched_location
def get_matched_location_on_arrows(self, pose, arrow_codes=None):
if arrow_codes is None:
arrow_codes = self.__arrow.get_arrow_codes()
similarity_max = 0.0
matched_location = None
for arrow_code in arrow_codes:
for waypoint_id in self.__arrow.get_waypoint_ids(arrow_code):
pose_on_route = self.__waypoint.get_pose(waypoint_id)
similarity = MapMatch.get_similarity_between_poses(
pose, pose_on_route)
if matched_location is None or similarity_max < similarity:
matched_location = Location.new_location(
waypoint_id, arrow_code)
similarity_max = similarity
return matched_location