def __init__(self, record_file):
self.firstvalid = False
self.logger = Logger.get_logger("RtkRecord")
self.record_file = record_file
self.logger.info("Record file to: " + record_file)
try:
self.file_handler = open(record_file, 'w')
except IOError:
self.logger.error("Open file %s failed" % (record_file))
self.file_handler.close()
sys.exit(1)
self.write(
"x,y,z,speed,acceleration,curvature,"
"curvature_change_rate,time,theta,gear,s,throttle,brake,steering\n"
)
self.localization = localization_pb2.LocalizationEstimate()
self.chassis = chassis_pb2.Chassis()
self.chassis_received = False
self.cars = 0.0
self.startmoving = False
self.terminating = False
self.carcurvature = 0.0
self.prev_carspeed = 0.0
vehicle_config = vehicle_config_pb2.VehicleConfig()
proto_utils.get_pb_from_text_file(
"/apollo/modules/common/data/vehicle_param.pb.txt", vehicle_config)
self.vehicle_param = vehicle_config.vehicle_param
def __init__(self):
self.node = cyber.Node("sendor_calibration_preprocessor")
self.writer = self.node.create_writer("/apollo/dreamview/progress",
preprocess_table_pb2.Progress, 6)
self.config = extractor_config_pb2.DataExtractionConfig()
self.progress = preprocess_table_pb2.Progress()
self.progress.percentage = 0.0
self.progress.log_string = "Preprocessing in progress..."
self.progress.status = preprocess_table_pb2.Status.UNKNOWN
try:
get_pb_from_text_file(FLAGS.config, self.config)
except text_format.ParseError:
print(f'Error: Cannot parse {FLAGS.config} as text proto')
self.records = []
for r in self.config.records.record_path:
self.records.append(str(r))
self.start_timestamp = -1
self.end_timestamp = -1
if self.config.io_config.start_timestamp == "FLOAT_MIN":
self.start_timestamp = np.finfo(np.float32).min
else:
self.start_timestamp = np.float32(
self.config.io_config.start_timestamp)
if self.config.io_config.end_timestamp == "FLOAT_MAX":
self.end_timestamp = np.finfo(np.float32).max
else:
self.end_timestamp = np.float32(
self.config.io_config.end_timestamp)
def __init__(self, record_file, node, speedmultiplier, completepath,
replan):
"""Init player."""
self.firstvalid = False
self.logger = Logger.get_logger(tag="RtkPlayer")
self.logger.info("Load record file from: %s" % record_file)
try:
file_handler = open(record_file, 'r')
except IOError:
self.logger.error("Cannot find file: " + record_file)
file_handler.close()
sys.exit(0)
self.data = genfromtxt(file_handler, delimiter=',', names=True)
file_handler.close()
self.localization = localization_pb2.LocalizationEstimate()
self.chassis = chassis_pb2.Chassis()
self.padmsg = pad_msg_pb2.PadMessage()
self.localization_received = False
self.chassis_received = False
self.planning_pub = node.create_writer('/apollo/planning',
planning_pb2.ADCTrajectory)
self.speedmultiplier = speedmultiplier / 100
self.terminating = False
self.sequence_num = 0
b, a = signal.butter(6, 0.05, 'low')
self.data['acceleration'] = signal.filtfilt(b, a,
self.data['acceleration'])
self.start = 0
self.end = 0
self.closestpoint = 0
self.automode = False
self.replan = (replan == 't')
self.completepath = (completepath == 't')
self.estop = False
self.logger.info("Planning Ready")
vehicle_config = vehicle_config_pb2.VehicleConfig()
proto_utils.get_pb_from_text_file(
"/apollo/modules/common/data/vehicle_param.pb.txt", vehicle_config)
self.vehicle_param = vehicle_config.vehicle_param
def generate_task_config_yaml(self,
task_name,
source_sensor,
dest_sensor,
source_folder,
dest_folder,
out_config_file,
in_config_file=None):
self._task_name = task_name
self._source_sensor = source_sensor
out_data = self.load_sample_yaml_file(task_name=task_name,
sample_file=in_config_file)
out_data['source_sensor'] = source_sensor
out_data['destination_sensor'] = dest_sensor
if task_name not in self._supported_tasks:
raise ValueError(
'does not support the calibration task: {}'.format(task_name))
user_config = os.path.join(ROOT_DIR, 'config',
task_name + '_user.config')
if os.path.exists(user_config):
try:
get_pb_from_text_file(user_config, self._table_info)
except text_format.ParseError:
print(f'Error: Cannot parse {user_config} as text proto')
if self._task_name == 'lidar_to_gnss':
out_data = self._generate_lidar_to_gnss_calibration_yaml(
out_data, source_folder, dest_folder)
elif self._task_name == 'camera_to_lidar':
if source_folder != dest_folder:
raise ValueError(
'camera frame and lidar frame should be in same folder')
out_root_path = os.path.dirname(out_config_file)
self._generate_camera_init_param_yaml(out_root_path, out_data)
out_data = self._generate_camera_to_lidar_calibration_yaml(
out_data)
print(out_data)
try:
with open(out_config_file, 'w') as f:
yaml.safe_dump(out_data, f)
except IOError:
raise ValueError(
'cannot generate the task config yaml file at {}'.format(
out_config_file))
return True
def get_routingdata():
print('Please wait for loading route response data...')
log_dir = os.path.normpath(
os.path.join(os.path.dirname(__file__), '../../../data/log'))
route_data_path = os.path.join(log_dir, 'passage_region_debug.bin')
print("File: %s" % route_data_path)
return proto_utils.get_pb_from_text_file(route_data_path,
routing_pb2.RoutingResponse())
def update():
# setup server url
config = ConfigParser()
CONFIG_FILE = os.path.join(os.path.dirname(__file__), 'config.ini')
config.read(CONFIG_FILE)
ip = config.get('Host', 'ip')
port = config.get('Host', 'port')
url = 'https://' + ip + ':' + port + '/update'
# setup car info
vehicle_info = VehicleInfo()
VEHICLE_INFO_FILE = os.path.join(
os.path.dirname(__file__), 'vehicle_info.pb.txt')
try:
proto_utils.get_pb_from_text_file(VEHICLE_INFO_FILE, vehicle_info)
except IOError:
print("vehicle_info.pb.txt cannot be open file.")
exit()
brand = VehicleInfo.Brand.Name(vehicle_info.brand)
model = VehicleInfo.Model.Name(vehicle_info.model)
vin = vehicle_info.vehicle_config.vehicle_id.vin
car_info = {
"car_type": brand + "." + model,
"tag": sys.argv[1],
"vin": vin,
}
urllib3.disable_warnings()
CERT_FILE = os.path.join(os.path.dirname(__file__), 'ota.cert')
r = requests.post(url, json=car_info, verify=CERT_FILE)
if r.status_code == 200:
print("Update successfully.")
sys.exit(0)
elif r.status_code == 400:
print("Invalid Request.")
else:
print("Cannot connect to server.")
sys.exit(1)
def reorganize_extracted_data(self,
tmp_data_path,
remove_input_data_cache=False):
root_path = os.path.dirname(os.path.normpath(tmp_data_path))
output_path = None
config_yaml = ConfigYaml()
task_name = self.config.io_config.task_name
if task_name == 'lidar_to_gnss':
subfolders = [
x for x in get_subfolder_list(tmp_data_path)
if '_apollo_sensor_' in x or '_localization_pose' in x
]
odometry_subfolders = [
x for x in subfolders if '_odometry' in x or '_pose' in x
]
lidar_subfolders = [x for x in subfolders if '_PointCloud2' in x]
print(lidar_subfolders)
print(odometry_subfolders)
if len(lidar_subfolders) == 0 or len(odometry_subfolders) != 1:
raise ValueError(('one odometry and more than 0 lidar(s)'
'sensor are needed for sensor calibration'))
odometry_subfolder = odometry_subfolders[0]
yaml_list = []
gnss_name = 'novatel'
multi_lidar_out_path = os.path.join(
root_path, 'multi_lidar_to_gnss_calibration')
output_path = multi_lidar_out_path
for lidar in lidar_subfolders:
# get the lidar name from folder name string
lidar_name = Extractor.get_substring(str=lidar,
prefix='_apollo_sensor_',
suffix='_PointCloud2')
# reorganize folder structure: each lidar has its raw data,
# corresponding odometry and configuration yaml file
if not Extractor.process_dir(multi_lidar_out_path, 'create'):
raise ValueError(
f'Failed to create directory: {multi_lidar_out_path}')
lidar_in_path = os.path.join(tmp_data_path, lidar)
lidar_out_path = os.path.join(multi_lidar_out_path, lidar)
if not os.path.exists(lidar_out_path):
shutil.copytree(lidar_in_path, lidar_out_path)
odometry_in_path = os.path.join(tmp_data_path,
odometry_subfolder)
odometry_out_path = os.path.join(multi_lidar_out_path,
odometry_subfolder)
if not os.path.exists(odometry_out_path):
shutil.copytree(odometry_in_path, odometry_out_path)
generated_config_yaml = os.path.join(
tmp_data_path, lidar_name + '_' + 'sample_config.yaml')
config_yaml.generate_task_config_yaml(
task_name=task_name,
source_sensor=lidar_name,
dest_sensor=gnss_name,
source_folder=lidar,
dest_folder=odometry_subfolder,
out_config_file=generated_config_yaml)
print(f'lidar {lidar_name} calibration data and configuration'
' are generated.')
yaml_list.append(generated_config_yaml)
out_data = {
'calibration_task': task_name,
'destination_sensor': gnss_name,
'odometry_file': odometry_subfolder + '/odometry'
}
sensor_files_directory_list = []
source_sensor_list = []
transform_list = []
for i in range(len(yaml_list)):
with open(yaml_list[i], 'r') as f:
data = yaml.safe_load(f)
sensor_files_directory_list.append(
data['sensor_files_directory'])
source_sensor_list.append(data['source_sensor'])
transform_list.append(data['transform'])
out_data['sensor_files_directory'] = sensor_files_directory_list
out_data['source_sensor'] = source_sensor_list
out_data['transform'] = transform_list
out_data['main_sensor'] = source_sensor_list[0]
table = preprocess_table_pb2.PreprocessTable()
user_config = os.path.join(os.path.dirname(__file__), 'config',
'lidar_to_gnss_user.config')
if os.path.exists(user_config):
try:
get_pb_from_text_file(user_config, table)
except text_format.ParseError:
print(f'Error: Cannot parse {user_config} as text proto')
if table.HasField("main_sensor"):
out_data['main_sensor'] = table.main_sensor
multi_lidar_yaml = os.path.join(multi_lidar_out_path,
'sample_config.yaml')
with open(multi_lidar_yaml, 'w') as f:
yaml.safe_dump(out_data, f)
elif task_name == 'camera_to_lidar':
# data selection.
pair_data_folder_name = 'camera-lidar-pairs'
cameras, lidar = select_static_image_pcd(
path=tmp_data_path,
min_distance=5,
stop_times=4,
wait_time=3,
check_range=50,
image_static_diff_threshold=0.005,
output_folder_name=pair_data_folder_name,
image_suffix='.jpg',
pcd_suffix='.pcd')
lidar_name = Extractor.get_substring(str=lidar,
prefix='_apollo_sensor_',
suffix='_PointCloud2')
for camera in cameras:
camera_name = Extractor.get_substring(str=camera,
prefix='_apollo_sensor_',
suffix='_image')
out_path = os.path.join(
root_path,
camera_name + '_to_' + lidar_name + '_calibration')
output_path = out_path
if not Extractor.process_dir(out_path, 'create'):
raise ValueError(f'Failed to create directory: {out_path}')
# reorganize folder structure: each camera has its images,
# corresponding lidar pointclouds, camera initial extrinsics,
# intrinsics, and configuration yaml file
in_pair_data_path = os.path.join(tmp_data_path, camera,
pair_data_folder_name)
out_pair_data_path = os.path.join(out_path,
pair_data_folder_name)
shutil.copytree(in_pair_data_path, out_pair_data_path)
generated_config_yaml = os.path.join(out_path,
'sample_config.yaml')
config_yaml.generate_task_config_yaml(
task_name=task_name,
source_sensor=camera_name,
dest_sensor=lidar_name,
source_folder=None,
dest_folder=None,
out_config_file=generated_config_yaml)
elif task_name == 'radar_to_gnss':
print('not ready. stay tuned')
else:
raise ValueError(
f'Unsupported data extraction task for {task_name}')
if remove_input_data_cache:
print(f'removing the cache at {tmp_data_path}')
os.system(f'rm -rf {tmp_data_path}')
return output_path
def load_open_space_protobuf(filename):
open_space_params = planner_open_space_config_pb2.PlannerOpenSpaceConfig()
proto_utils.get_pb_from_text_file(filename, open_space_params)
return open_space_params
textcoords='offset points',
ha='left',
va='top',
bbox=dict(boxstyle='round,pad=0.5', fc='yellow', alpha=0.3),
arrowprops=dict(arrowstyle='->', connectionstyle='arc3,rad=0'),
alpha=0.8)
if __name__ == "__main__":
if len(sys.argv) < 2:
print("usage: %s <planning.pb.txt> <localization.pb.txt>" %
sys.argv[0])
sys.exit(0)
planning_pb_file = sys.argv[1]
localization_pb_file = sys.argv[2]
planning_pb = proto_utils.get_pb_from_text_file(planning_pb_file,
ADCTrajectory())
localization_pb = proto_utils.get_pb_from_text_file(
localization_pb_file, LocalizationEstimate())
plot_trajectory(planning_pb, plt)
plot_vehicle(localization_pb, plt)
current_t = localization_pb.header.timestamp_sec
trajectory_point = find_closest_traj_point(planning_pb, current_t)
plot_traj_point(planning_pb, trajectory_point, plt)
plt.axis('equal')
plt.show()
def query():
vehicle_info = VehicleInfo()
VEHICLE_INFO_FILE = os.path.join(os.path.dirname(__file__),
'vehicle_info.pb.txt')
try:
proto_utils.get_pb_from_text_file(VEHICLE_INFO_FILE, vehicle_info)
except IOError:
print('vehicle_info.pb.txt cannot be open file.')
sys.exit(1)
# Setup server url
config = ConfigParser()
CONFIG_FILE = os.path.join(os.path.dirname(__file__), 'config.ini')
config.read(CONFIG_FILE)
ip = config.get('Host', 'ip')
port = config.get('Host', 'port')
url = 'https://' + ip + ':' + port + '/query'
# Generate device token
ret = sec_api.sec_upgrade_get_device_token()
if ret[0] is False:
print('Failed to get device token.')
sys.exit(1)
dev_token = ret[1]
# setup car info
brand = VehicleInfo.Brand.Name(vehicle_info.brand)
model = VehicleInfo.Model.Name(vehicle_info.model)
vin = vehicle_info.vehicle_config.vehicle_id.vin
META_FILE = '/apollo/meta.ini'
config.read(META_FILE)
car_info = {
"car_type": brand + "." + model,
"tag": config.get('Release', 'tag'),
"vin": vin,
"token": dev_token
}
urllib3.disable_warnings()
CERT_FILE = os.path.join(os.path.dirname(__file__), 'ota.cert')
r = requests.post(url, json=car_info, verify=CERT_FILE)
if r.status_code == 200:
auth_token = r.json().get("auth_token")
if auth_token == "":
print('Cannot get authorize token!')
sys.exit(1)
else:
token_file_name = os.environ['HOME'] + \
'/.cache/apollo_update/auth_token'
apollo_update = os.path.dirname(token_file_name)
if not os.path.exists(apollo_update):
os.makedirs(apollo_update)
with open(token_file_name, 'w') as token_file:
token_file.write(auth_token)
tag = r.json().get("tag")
print(tag)
sys.exit(0)
elif r.status_code == 204:
print('Release is up to date.')
sys.exit(0)
elif r.status_code == 400:
print('Invalid car type.')
else:
print('Cannot connect to server.')
sys.exit(1)
def load(self, localization_file_name):
self.localization_pb = proto_utils.get_pb_from_text_file(
localization_file_name, LocalizationEstimate())
calibration_table_pb = calibration_table_pb2.ControlCalibrationTable()
speeds = list(speed_table.keys())
speeds.sort()
for speed in speeds:
acc_table = speed_table[speed]
accs = list(acc_table.keys())
accs.sort()
for acc in accs:
cmds = acc_table[acc]
cmd = np.mean(cmds)
item = calibration_table_pb.calibration.add()
item.speed = speed
item.acceleration = acc
item.command = cmd
return calibration_table_pb
if __name__ == '__main__':
if len(sys.argv) != 3:
print("Usage: %s old_control_conf.pb.txt result.csv" % sys.argv[0])
sys.exit(0)
ctl_conf_pb = proto_utils.get_pb_from_text_file(sys.argv[1], ControlConf())
speed_table_dict = load_calibration_raw_data(sys.argv[2])
calibration_table_pb = get_calibration_table_pb(speed_table_dict)
ctl_conf_pb.lon_controller_conf.calibration_table.CopyFrom(
calibration_table_pb)
with open('control_conf.pb.txt', 'w') as f:
f.write(str(ctl_conf_pb))
