def set_planning_conf(config):
"""change planning config based on navi config"""
planning_conf = planning_config_pb2.PlanningConfig()
proto_utils.get_pb_from_file(PLANNING_CONF_FILE, planning_conf)
planner_type = config.get('PlanningConf', 'planner_type')
if planner_type == "EM":
planning_conf.planner_type = planning_config_pb2.PlanningConfig.EM
if planner_type == "LATTICE":
planning_conf.planner_type = planning_config_pb2.PlanningConfig.LATTICE
if planner_type == "NAVI":
planning_conf.planner_type = planning_config_pb2.PlanningConfig.NAVI
proto_utils.write_pb_to_text_file(planning_conf, PLANNING_CONF_FILE)
def set_planning_conf(config):
"""change planning config based on navi config"""
planning_conf = planning_config_pb2.PlanningConfig()
proto_utils.get_pb_from_file(PLANNING_CONF_FILE, planning_conf)
planner_type = config.get('PlanningConf', 'planner_type')
if planner_type == "EM":
planning_conf.planner_type = planning_config_pb2.PlanningConfig.EM
if planner_type == "LATTICE":
planning_conf.planner_type = planning_config_pb2.PlanningConfig.LATTICE
if planner_type == "NAVI":
planning_conf.planner_type = planning_config_pb2.PlanningConfig.NAVI
proto_utils.write_pb_to_text_file(planning_conf, PLANNING_CONF_FILE)
def set_hmi_conf(config):
"""change hmi conf file based on navi config file"""
hmi_conf = hmi_config_pb2.HMIConfig()
proto_utils.get_pb_from_file(HMI_CONF_FILE, hmi_conf)
perception = config.get('PerceptionConf', 'perception')
navi_mode = hmi_conf.modes["Navigation"]
if 'navigation_camera' in navi_mode.live_modules:
navi_mode.live_modules.remove('navigation_camera')
if 'navigation_perception' in navi_mode.live_modules:
navi_mode.live_modules.remove('navigation_perception')
if 'mobileye' in navi_mode.live_modules:
navi_mode.live_modules.remove('mobileye')
if 'third_party_perception' in navi_mode.live_modules:
navi_mode.live_modules.remove('third_party_perception')
if 'velodyne' in navi_mode.live_modules:
navi_mode.live_modules.remove('velodyne')
if 'perception' in navi_mode.live_modules:
navi_mode.live_modules.remove('perception')
if perception == "CAMERA":
if 'navigation_camera' not in navi_mode.live_modules:
navi_mode.live_modules.insert(0, 'navigation_camera')
if 'navigation_perception' not in navi_mode.live_modules:
navi_mode.live_modules.insert(0, 'navigation_perception')
if perception == "MOBILEYE":
if 'mobileye' not in navi_mode.live_modules:
navi_mode.live_modules.insert(0, 'mobileye')
if 'third_party_perception' not in navi_mode.live_modules:
navi_mode.live_modules.insert(0, 'third_party_perception')
if perception == "VELODYNE64":
if 'velodyne' not in navi_mode.live_modules:
navi_mode.live_modules.insert(0, 'velodyne')
if 'perception' not in navi_mode.live_modules:
navi_mode.live_modules.insert(0, 'perception')
hmi_conf.modes["Navigation"].CopyFrom(navi_mode)
proto_utils.write_pb_to_text_file(hmi_conf, HMI_CONF_FILE)
def set_hmi_conf(config):
"""change hmi conf file based on navi config file"""
hmi_conf = hmi_config_pb2.HMIConfig()
proto_utils.get_pb_from_file(HMI_CONF_FILE, hmi_conf)
perception = config.get('PerceptionConf', 'perception')
navi_mode = hmi_conf.modes["Navigation"]
if 'navigation_camera' in navi_mode.live_modules:
navi_mode.live_modules.remove('navigation_camera')
if 'navigation_perception' in navi_mode.live_modules:
navi_mode.live_modules.remove('navigation_perception')
if 'mobileye' in navi_mode.live_modules:
navi_mode.live_modules.remove('mobileye')
if 'third_party_perception' in navi_mode.live_modules:
navi_mode.live_modules.remove('third_party_perception')
if 'velodyne' in navi_mode.live_modules:
navi_mode.live_modules.remove('velodyne')
if 'perception' in navi_mode.live_modules:
navi_mode.live_modules.remove('perception')
if perception == "CAMERA":
if 'navigation_camera' not in navi_mode.live_modules:
navi_mode.live_modules.insert(0, 'navigation_camera')
if 'navigation_perception' not in navi_mode.live_modules:
navi_mode.live_modules.insert(0, 'navigation_perception')
if perception == "MOBILEYE":
if 'mobileye' not in navi_mode.live_modules:
navi_mode.live_modules.insert(0, 'mobileye')
if 'third_party_perception' not in navi_mode.live_modules:
navi_mode.live_modules.insert(0, 'third_party_perception')
if perception == "VELODYNE64":
if 'velodyne' not in navi_mode.live_modules:
navi_mode.live_modules.insert(0, 'velodyne')
if 'perception' not in navi_mode.live_modules:
navi_mode.live_modules.insert(0, 'perception')
hmi_conf.modes["Navigation"].CopyFrom(navi_mode)
proto_utils.write_pb_to_text_file(hmi_conf, HMI_CONF_FILE)
def run(gmap_key, map_file, utm_zone):
"""
read and process map file
"""
map_pb = map_pb2.Map()
proto_utils.get_pb_from_file(map_file, map_pb)
left_boundaries = []
right_boundaries = []
center_lat = None
center_lon = None
for lane in map_pb.lane:
for curve in lane.left_boundary.curve.segment:
if curve.HasField('line_segment'):
left_boundary = []
for p in curve.line_segment.point:
point = {}
lat, lng = utm2latlon(p.x, p.y, utm_zone)
if center_lat is None:
center_lat = lat
if center_lon is None:
center_lon = lng
point['lat'] = lat
point['lng'] = lng
left_boundary.append(point)
left_boundaries.append(left_boundary)
for curve in lane.right_boundary.curve.segment:
if curve.HasField('line_segment'):
right_boundary = []
for p in curve.line_segment.point:
point = {}
lat, lng = utm2latlon(p.x, p.y, utm_zone)
point['lat'] = lat
point['lng'] = lng
right_boundary.append(point)
right_boundaries.append(right_boundary)
html = generate(gmap_key, left_boundaries, right_boundaries, center_lat,
center_lon)
with open('gmap.html', 'w') as file:
file.write(html)
def seq_publisher(seq_num, period):
"""publisher"""
rospy.init_node('replay_node', anonymous=True)
messages = {}
for topic, msg_type in proto_utils.topic_pb_dict.iteritems():
messages[topic] = {}
filename = str(seq_num) + "_" + topic + ".pb.txt"
print "trying to load pb file:", filename
messages[topic]["publisher"] = rospy.Publisher(
topic, msg_type, queue_size=1)
pb_msg = msg_type()
if not proto_utils.get_pb_from_file(filename, pb_msg):
print topic, " pb is none"
messages[topic]["value"] = pb_msg
rate = rospy.Rate(int(1.0 / period)) # 10hz
while not rospy.is_shutdown():
for topic, module_features in messages:
if module_features["value"] is not None:
module_features["publisher"].publish(module_features["value"])
rate.sleep()
def seq_publisher(seq_num, period):
"""publisher"""
rospy.init_node('replay_node', anonymous=True)
messages = {}
for topic, msg_type in proto_utils.topic_pb_dict.iteritems():
messages[topic] = {}
filename = str(seq_num) + "_" + topic + ".pb.txt"
print "trying to load pb file:", filename
messages[topic]["publisher"] = rospy.Publisher(topic,
msg_type,
queue_size=1)
pb_msg = msg_type()
if not proto_utils.get_pb_from_file(filename, pb_msg):
print topic, " pb is none"
messages[topic]["value"] = pb_msg
rate = rospy.Rate(int(1.0 / period)) # 10hz
while not rospy.is_shutdown():
for topic, module_features in messages:
if module_features["value"] is not None:
module_features["publisher"].publish(module_features["value"])
rate.sleep()
second_last = points[point_num - 2]
last_point = points[point_num - 1]
x_diff = last_point.x - second_last.x
y_diff = last_point.y - second_last.y
t_diff = last_point.t - second_last.t
delta_diff = math.sqrt(x_diff ** 2 + y_diff ** 2)
cur_len = trajectory_length
while cur_len < min_length:
last_point.x += x_diff
last_point.y += y_diff
last_point.t += t_diff
p = points.add()
p.CopyFrom(last_point)
last_point = p
cur_len += delta_diff
return prediction
if __name__ == '__main__':
parser = argparse.ArgumentParser(description="extend prediction trajectory")
parser.add_argument("prediction", action="store", type=str, help="set the prediction file")
parser.add_argument("-p", "--period", action="store", type=float, default=10.0,
help="set the prediction period")
parser.add_argument("-d", "--distance", action="store", type=float, default=70.0,
help="set the prediction distance")
args = parser.parse_args()
prediction_data = proto_utils.get_pb_from_file(args.prediction,
PredictionObstacles())
extended_prediction = extend_prediction(prediction_data, args.distance, args.period)
print extended_prediction
boundary.setMap(map);
}
""")
doc.asis('}\n')
doc.asis('<script src="' + api_url + '"></script>')
self.html = doc.getvalue()
return self.html
if __name__ == "__main__":
import google.protobuf.text_format as text_format
if len(sys.argv) < 2:
print "usage: python map_viewer.py dbmap_file [utm_zone=10]"
sys.exit(0)
map_file = sys.argv[1]
utm_zone = 10
if len(sys.argv) >= 3:
utm_zone = int(sys.argv[2])
dbmap = navigation_pb2.NavigationInfo()
proto_utils.get_pb_from_file(map_file, dbmap)
with open(map_file, 'r') as file_in:
text_format.Merge(file_in.read(), dbmap)
viewer = DBMapViewer(utm_zone)
viewer.add(dbmap)
html = viewer.generate()
with open('dbmap.html', 'w') as f:
f.write(html)
def load(self, map_file_name):
res = proto_utils.get_pb_from_file(map_file_name, self.map_pb)
return res != None
def load(self, map_file_name):
res = proto_utils.get_pb_from_file(map_file_name, self.map_pb)
return res != None
def load_map_data(map_file):
map_pb = map_pb2.Map()
proto_utils.get_pb_from_file(map_file, map_pb)
return map_pb
boundary.setMap(map);
}
""")
doc.asis('}\n')
doc.asis('<script src="' + api_url + '"></script>')
self.html = doc.getvalue()
return self.html
if __name__ == "__main__":
import google.protobuf.text_format as text_format
if len(sys.argv) < 2:
print "usage: python map_viewer.py dbmap_file [utm_zone=10]"
sys.exit(0)
map_file = sys.argv[1]
utm_zone = 10
if len(sys.argv) >= 3:
utm_zone = int(sys.argv[2])
dbmap = navigation_pb2.NavigationInfo()
proto_utils.get_pb_from_file(map_file, dbmap)
with open(map_file, 'r') as file_in:
text_format.Merge(file_in.read(), dbmap)
viewer = DBMapViewer(utm_zone)
viewer.add(dbmap)
html = viewer.generate()
with open('dbmap.html', 'w') as f:
f.write(html)
