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)