def transcribe(proto_msg):
header = proto_msg.header
seq = "%05d" % header.sequence_num
name = header.module_name
file_path = os.path.join(g_args.out_dir, seq + "_" + name + ".pb.txt")
print('Write proto buffer message to file: %s' % file_path)
proto_utils.write_pb_to_text_file(proto_msg, file_path)
def transcribe(proto_msg):
header = proto_msg.header
seq = "%05d" % header.sequence_num
name = header.module_name
file_path = os.path.join(g_args.out_dir, seq + "_" + name + ".pb.txt")
print file_path
proto_utils.write_pb_to_text_file(proto_msg, file_path)
def ParamEvaluation(params, tunning_object):
proto_utils.write_pb_to_text_file(params, original_file_path)
if tunning_object == "coarse_trajectory":
visualize_flag = False
success, x_out, y_out, phi_out, v_out, a_out, steer_out, planning_time = hybrid_a_star_visualizer.HybridAStarPlan(
visualize_flag)
if not success:
return float('inf')
else:
return planning_time
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 parse_drive_event_file(drive_event_filename, signal_filename):
drive_event = g_message_manager.parse_topic_file("/apollo/drive_event",
drive_event_filename)
if not drive_event:
print("Failed to find localization in %s" % drive_event_filename)
return None
pose = drive_event.location
map_signal = create_signal_proto(pose.position.x, pose.position.y,
pose.heading)
proto_utils.write_pb_to_text_file(map_signal, signal_filename)
return map_signal
def parse_drive_event_file(drive_event_filename, signal_filename):
drive_event = g_message_manager.parse_topic_file("/apollo/drive_event",
drive_event_filename)
if not drive_event:
print("Failed to find localization in %s" % drive_event_filename)
return None
pose = drive_event.location
map_signal = create_signal_proto(pose.position.x, pose.position.y,
pose.heading)
proto_utils.write_pb_to_text_file(map_signal, signal_filename)
return map_signal
def main(args):
drive_event_meta_msg = g_message_manager.get_msg_meta_by_topic(
args.drive_event_topic)
if not drive_event_meta_msg:
print('Unknown drive_event topic name: %s' % args.drive_event_topic)
sys.exit(1)
localization_meta_msg = g_message_manager.get_msg_meta_by_topic(
args.localization_topic)
if not localization_meta_msg:
print('Unknown localization topic name: %s' % args.localization_topic)
sys.exit(1)
cyber.init()
node = cyber.Node("derive_event_node")
node.create_reader(localization_meta_msg.topic,
localization_meta_msg.msg_type, OnReceiveLocalization)
writer = node.create_writer(drive_event_meta_msg.topic,
drive_event_meta_msg.msg_type)
seq_num = 0
while not cyber.is_shutdown():
event_type = input(
"Type in Event Type('d') and press Enter (current time: " +
str(datetime.datetime.now()) + ")\n>")
event_type = event_type.strip()
if len(event_type) != 1 or event_type[0].lower() != 'd':
continue
current_time = cyber_time.Time.now().to_sec()
event_str = None
while not event_str:
event_str = input("Type Event:>")
event_str = event_str.strip()
event_msg = drive_event_meta_msg.msg_type()
event_msg.header.timestamp_sec = current_time
event_msg.header.module_name = 'drive_event'
seq_num += 1
event_msg.header.sequence_num = seq_num
event_msg.header.version = 1
event_msg.event = event_str
if g_localization:
event_msg.location.CopyFrom(g_localization.pose)
writer.write(event_msg)
time_str = datetime.datetime.fromtimestamp(current_time).strftime(
"%Y%m%d%H%M%S")
filename = os.path.join(args.dir, "%s_drive_event.pb.txt" % time_str)
proto_utils.write_pb_to_text_file(event_msg, filename)
print('Logged to rosbag and written to file %s' % filename)
time.sleep(0.1)
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)
parser.add_argument(
"--InputConfig", help="original conf address to be tuned", type=str, default=original_file_path)
parser.add_argument("--OutputConfig", help="tuned conf address",
type=str, default=optimal_file_path)
parser.add_argument("--TunningObject",
help="algorithm to be tuned", type=str, default=tunning_object)
args = parser.parse_args()
original_file_path = args.InputConfig
optimal_file_path = args.OutputConfig
tunning_object = args.TunningObject
if tunning_object != "coarse_trajectory":
print("Algorithm other than coarse_trajectory is not supported")
exit()
param_names_and_range = GetParamsForTunning(tunning_object)
origin_open_space_params = load_open_space_protobuf(original_file_path)
params_lists = RandSampling(
param_names_and_range, origin_open_space_params)
key_to_evaluations = TestingParams(params_lists, tunning_object)
optimal_params, optimal_evaluation = GetOptimalParams(
params_lists, key_to_evaluations)
origin_evaluation = ParamEvaluation(
origin_open_space_params, tunning_object)
print("optimal_evaluation is " + str(optimal_evaluation))
print("origin_evaluation is " + str(origin_evaluation))
improvement_percentage = (
origin_evaluation - optimal_evaluation) / origin_evaluation
print("improvement_percentage is " + str(improvement_percentage))
proto_utils.write_pb_to_text_file(optimal_params, optimal_file_path)
proto_utils.write_pb_to_text_file(
origin_open_space_params, original_file_path)
while not cyber.is_shutdown():
event_type = raw_input(
"Type in Event Type('d') and press Enter (current time: " +
str(datetime.datetime.now()) + ")\n>")
event_type = event_type.strip()
if len(event_type) != 1:
continue
if event_type[0].lower() != 'd':
continue
current_time = time.time()
event_str = None
while not event_str:
event_str = raw_input("Type Event:>")
event_str = event_str.strip()
event_msg = drive_event_meta_msg.msg_type()
event_msg.header.timestamp_sec = current_time
event_msg.header.module_name = "drive_event"
seq_num += 1
event_msg.header.sequence_num = seq_num
event_msg.header.version = 1
event_msg.event = event_str
if g_localization:
event_msg.location.CopyFrom(g_localization.pose)
writer.write(event_msg)
time_str = datetime.datetime.fromtimestamp(current_time).strftime(
"%Y%m%d%H%M%S")
filename = os.path.join(g_args.dir, "%s_drive_event.pb.txt" % time_str)
proto_utils.write_pb_to_text_file(event_msg, filename)
print("logged to rosbag and written to file %s" % filename)
time.sleep(0.1)
seq_num = 0
while not rospy.is_shutdown():
event_type = raw_input(
"Type in Event Type('d') and press Enter (current time: " +
str(datetime.datetime.now()) + ")\n>")
event_type = event_type.strip()
if len(event_type) != 1:
continue
if event_type[0].lower() != 'd':
continue
current_time = time.time()
event_str = None
while not event_str:
event_str = raw_input("Type Event:>")
event_str = event_str.strip()
event_msg = drive_event_meta_msg.msg_type()()
event_msg.header.timestamp_sec = current_time
event_msg.header.module_name = "drive_event"
seq_num += 1
event_msg.header.sequence_num = seq_num
event_msg.header.version = 1
event_msg.event = event_str
if g_localization:
event_msg.location.CopyFrom(g_localization.pose)
pub.publish(event_msg)
time_str = datetime.datetime.fromtimestamp(current_time).strftime(
"%Y%m%d%H%M%S")
filename = os.path.join(g_args.dir, "%s_drive_event.pb.txt" % time_str)
proto_utils.write_pb_to_text_file(event_msg, filename)
print("logged to rosbag and written to file %s" % filename)
