#!/usr/bin/env python3 # stdlib import json import os import sys import argparse import subprocess import itertools from os.path import exists from copy import deepcopy from datetime import date, datetime import matplotlib import atom_core.config_io from atom_core.config_visualization import createDotGraph, createNxGraph import atom_core.drawing # 3rd-party import yaml import numpy import rospy import rospkg import rosbag import jinja2 import matplotlib.pyplot as plt import networkx as nx from pytictoc import TicToc # local packages from atom_core.utilities import atomPrintOK, atomWarn, checkDirectoryExistence, atomError, atomStartupPrint, getJointParentChild, verifyAnchoredSensor from atom_core.naming import generateKey, generateLabeledTopic from atom_core.system import execute from atom_core.rosbag2 import is_rosbag2, convert_rosbag2_to_rosbag1 from colorama import Style, Fore from graphviz import Digraph from urdf_parser_py.urdf import URDF from matplotlib import cm from jinja2 import Environment, FileSystemLoader if __name__ == "__main__": # Parse command line arguments ap = argparse.ArgumentParser() ap.add_argument("-n", "--name", help='package name', type=str, required=True) ap.add_argument("-cfg", "--config_file", help='Specify if you want to configure the calibration package with a specific configuration file. If this flag is not given, the standard config.yml ill be used.', type=str, required=False, default=None) args = vars(ap.parse_args()) # -------------------------------------------------------------------------- # Initial setup # -------------------------------------------------------------------------- tictoc = TicToc() tictoc.tic() package_name = os.path.basename(args['name']) atomStartupPrint('Configuring calibration package ' + Fore.BLUE + package_name + Style.RESET_ALL) rospack = rospkg.RosPack() atom_calibration_path = rospack.get_path('atom_calibration') if not package_name in rospack.list(): # Check if package is under $ROS_PACKAGE_PATH, abort if not atomError('ROS package ' + Fore.BLUE + package_name + Style.RESET_ALL + ' not found under ROS. Are you sure the package in under a directory listed in $ROS_PACKAGE_PATH? Can you run:\n\n' + Fore.BLUE + 'roscd ' + package_name + Style.RESET_ALL + '\n\nPlease fix this before running your package configuration.') package_path = rospack.get_path(package_name) # full path to the package, including its name. package_base_path = os.path.dirname(package_path) # parent path where the package is located rviz_file_template = atom_calibration_path + '/templates/rviz/config.rviz.j2' rviz_playbag = 'playbag.rviz' rviz_set_initial_estimate = 'set_initial_estimate.rviz' rviz_collect_data = 'collect_data.rviz' rviz_calibrate = 'calibrate.rviz' rviz_dataset_playback = 'dataset_playback.rviz' playbag_launch_file = package_path + '/launch/playbag.launch' set_initial_estimate_launch_file = package_path + '/launch/set_initial_estimate.launch' data_collection_launch_file = package_path + '/launch/collect_data.launch' calibrate_launch_file = package_path + '/launch/calibrate.launch' dataset_playback_launch_file = package_path + '/launch/dataset_playback.launch' full_evaluation_launch_file = package_path + '/launch/full_evaluation.launch' # Verify if the standard atom_calibration directories exist, if not create them (#401) checkDirectoryExistence('calibration', package_name, create_if_nonexistent=True) checkDirectoryExistence('launch', package_name, create_if_nonexistent=True) checkDirectoryExistence('rviz', package_name, create_if_nonexistent=True) checkDirectoryExistence('scripts', package_name, create_if_nonexistent=True) checkDirectoryExistence('urdf', package_name, create_if_nonexistent=True) # Template engine1 setup file_loader = FileSystemLoader(atom_calibration_path + '/templates') env = Environment(loader=file_loader, undefined=jinja2.StrictUndefined) env.add_extension('jinja2.ext.do') # Date dt_string = datetime.now().strftime("%d/%m/%Y %H:%M:%S") # -------------------------------------------------------------------------- # Read the config.yml file # -------------------------------------------------------------------------- if args['config_file'] is None: args['config_file'] = package_path + '/calibration/config.yml' else: args['config_file'] = package_path + '/calibration/' + args['config_file'] if not exists(args['config_file']): args['config_file'] = package_path + '/calibration/config.yml' print('Loading config_file ' + Fore.BLUE + str(args['config_file']) + Style.RESET_ALL) config = atom_core.config_io.loadConfig(args['config_file']) # Sensors colormap. Access with: color_map_sensors[idx, :] cm_sensors = cm.Set3(numpy.linspace(0, 1, len(config['sensors'].keys()))) # -------------------------------------------------------------------------- # Read the bag file # -------------------------------------------------------------------------- bag_file, _, bag_file_relative = atom_core.config_io.uriReader(config['bag_file']) if is_rosbag2(bag_file): print('Bagfile ' + Fore.BLUE + bag_file + Style.RESET_ALL + ' is in rosbag2 format. Converting to rosbag1 format ... ', end='') rosbag2_file = convert_rosbag2_to_rosbag1(bag_file) atomPrintOK() if rosbag2_file: print("Converted successfully, destination file: " + Fore.BLUE + rosbag2_file + Style.RESET_ALL) bag_file = rosbag2_file bag_file_relative = bag_file else: atomError('Converting rosbag2 to rosbag1 failed.') print('Loading bagfile ' + bag_file + ' ... ', end='') bag = rosbag.Bag(bag_file) atomPrintOK() bag_info = bag.get_type_and_topic_info() bag_types = bag_info[0] bag_topics = bag_info[1] # Get initial stamp to compute mission time for topic, msg, stamp in bag.read_messages(): bag_initial_stamp = stamp break # -------------------------------------------------------------------------- # Setup the description file # -------------------------------------------------------------------------- description_file, _, _ = atom_core.config_io.uriReader(config['description_file']) description_file_out_initial_estimate = package_path + '/urdf/initial_estimate.urdf.xacro' execute('cp ' + description_file + ' ' + description_file_out_initial_estimate, verbose=False) # Copy the xacro to the initial_estimate file # Check the description file urdf_file = '/tmp/description.urdf' if os.path.exists(urdf_file): # print('Deleting temporary file ' + urdf_file) os.remove(urdf_file) print('Parsing description file ' + Fore.BLUE + description_file + Style.RESET_ALL) xacro_cmd = 'xacro ' + description_file + ' -o ' + urdf_file execute(xacro_cmd, verbose=True) # create tmp urdf file if not os.path.exists(urdf_file): atomError('Could not parse description file ' + Fore.BLUE + description_file + Style.RESET_ALL + '\nYou must manually run command:\n' + Fore.BLUE + xacro_cmd + Style.RESET_ALL + '\nand fix the problem before configuring your calibration package.') description = URDF.from_xml_file(urdf_file) # read the urdf file # -------------------------------------------------------------------------- # Check if modalities are properly created # -------------------------------------------------------------------------- # TODO change lidar to lidar3d and lidar2d to lidar2d; change modality to modality for sensor_key in config['sensors']: topic = config['sensors'][sensor_key]['topic_name'] topic_compressed = topic + '/compressed' if topic in bag_topics: msg_type = bag_info[1][topic].msg_type elif topic_compressed in bag_topics: msg_type = bag_info[1][topic_compressed].msg_type if ('modality' not in config['sensors'][sensor_key]) or ( config['sensors'][sensor_key]['modality'] not in ['rgb', 'depth', 'lidar3d', 'lidar2d', 'imu']): print(Fore.YELLOW + 'Warning: Sensor ID not properly identified for sensor ' + sensor_key + ' with topic ' + topic + "Sensor will be identified by message type." + Style.RESET_ALL) if msg_type == 'sensor_msgs/CompressedImage' or msg_type == 'sensor_msgs/Image': config['sensors'][sensor_key]['modality'] = 'rgb' elif msg_type == 'sensor_msgs/LaserScan': config['sensors'][sensor_key]['modality'] = 'lidar2d' elif msg_type == 'sensor_msgs/PointCloud2': config['sensors'][sensor_key]['modality'] = 'lidar3d' elif msg_type == "sensor_msgs/Imu": config['sensors'][sensor_key]['modality'] = 'imu' else: atomError('Cannot generate rviz configuration for sensor ' + Fore.BLUE + sensor_key + Style.RESET_ALL + ' with topic ' + Fore.BLUE + topic + Style.RESET_ALL) # -------------------------------------------------------------------------- # Create a tf graph to support verifications # -------------------------------------------------------------------------- nx_graph = createNxGraph(args, description, config, bag) # -------------------------------------------------------------------------- # Check if a new bagfile needs to be created # See https://github.com/lardemua/atom/issues/838 # -------------------------------------------------------------------------- transforms_from_bag = [] for edge_key, edge in nx_graph.edges.items(): if edge['from_bag']: t = {'parent': edge['parent'], 'child': edge['child']} transforms_from_bag.append(t) use_atom_bag_file = False # by default we use the original bagfile selected_bag_file_relative = bag_file_relative if transforms_from_bag: atomWarn('The urdf does not contain all the transforms in the bagfile. Atom will use transformations from the urdf description, combined with transformations from the bagfile that do not exist in the urdf. This implies the creation of a new bagfile.') print('Transforms exclusive to the bag file are: ' + str(transforms_from_bag)) # Initial setup use_atom_bag_file = True tictoc_bag_creation = TicToc() tictoc_bag_creation.tic() # TODO should'nt here be used the generateName function? atom_bag_file = bag_file[:-4] + '_atom_filtered.bag' atom_bag_file_relative = bag_file_relative[:-4] + '_atom_filtered.bag' selected_bag_file_relative = atom_bag_file_relative print('Creating atom_filtered bagfile ' + Fore.BLUE + atom_bag_file + Style.RESET_ALL + ' ... please wait ...') bag_out = rosbag.Bag(atom_bag_file, 'w') # Writing the filtered bagfile for topic, msg, stamp, connection_header in bag.read_messages(return_connection_header=True): if ((topic == "/tf" or topic == "/tf_static") and msg.transforms): transforms_to_keep = [] for i in range(len(msg.transforms)): msg_parent = msg.transforms[i].header.frame_id msg_child = msg.transforms[i].child_frame_id for transform_from_bag in transforms_from_bag: if msg_parent == transform_from_bag['parent'] and msg_child == transform_from_bag['child']: transforms_to_keep.append(msg.transforms[i]) break # print('Transformations to keep: ' + str(transforms_to_keep)) msg.transforms = transforms_to_keep bag_out.write(topic, msg, stamp, connection_header=connection_header) bag_out.close() # close the bag file. print('Completed in ' + str(round(tictoc_bag_creation.tocvalue(), 2)) + ' seconds.') # -------------------------------------------------------------------------- # Verifications: Run as much as we can think of to see if something is wrong. Better early than late. # -------------------------------------------------------------------------- print('Running several verifications ... please wait ...') # If anchored sensor exists, it must be one of the existing sensors verifyAnchoredSensor(config['anchored_sensor'], config['sensors']) # Check if sensor names are valid: sensor_keys = list(config['sensors'].keys()) for i in range(len(sensor_keys)): for j in range(i + 1, len(sensor_keys)): key1 = sensor_keys[i] key2 = sensor_keys[j] if key1 in key2: atomError(f"'{key1}' is a string subset of '{key2}', please rename them") elif key2 in key1: atomError(f"'{key2}' is a string subset of '{key1}', please rename them") atomPrintOK() # Check if config sensor topics exist in the bag file (also check for compressed topics) print('Checking for compressed image topics ... ') compressed_topics = {} # a list of compressed topics to decompress in the launch file for sensor_key in config['sensors']: topic = config['sensors'][sensor_key]['topic_name'] if topic not in bag_topics: topic_compressed = topic + '/compressed' if topic_compressed in bag_topics: # Check if the topic is a compressed image topic msg_type = bag_info[1][topic_compressed].msg_type if msg_type == 'sensor_msgs/CompressedImage': # Check if the topic of correct msg_type compressed_topics[topic] = {'topic_compressed': topic_compressed, 'msg_type': msg_type, 'sensor_key': sensor_key} print('Topic ' + Fore.BLUE + topic + Style.RESET_ALL + ' is in compressed format (' + topic_compressed + '). Will setup a decompressor.') else: atomError('Topic ' + Fore.BLUE + topic + Style.RESET_ALL + ' (from sensor ' + Fore.BLUE + sensor_key + ') exists in compressed format in the bag file, but is not of msg_type "sensor_msgs/CompressedImage"' + Style.RESET_ALL) else: raise atomError('Topic ' + Fore.BLUE + topic + Style.RESET_ALL + ' (from sensor ' + Fore.BLUE + sensor_key + Style.RESET_ALL + ') does not exist in the bag file.' + Style.RESET_ALL) # Check if throttled topics exist print('Checking for throttled topics ... ') throttle_topics = {} use_throttle_topics = {} for sensor_key in config['sensors']: topic = config['sensors'][sensor_key]['topic_name'] topic_compressed = topic + '/compressed' if topic_compressed in bag_topics: # Check if the topic is a compressed image topic topic = topic_compressed if config['sensors'][sensor_key]['throttle'] is not None: throttle = config['sensors'][sensor_key]['throttle'] print('Sensor ' + sensor_key + ' throttled to ' + Fore.BLUE + str(config['sensors'][sensor_key]['throttle']) + Style.RESET_ALL + ' Hz.') use_throttle = True else: throttle = None use_throttle = False throttle_topics[topic] = {'throttled_topic': topic, 'sensor_key': sensor_key, 'use_throttle': use_throttle, 'throttle_hz': throttle} # For rgb and depth sensors, check that the camera_info topic exists in the bag file print('Checking for camera_info topic in bagfile ... ', end='') for sensor_key in config['sensors']: # no need to check for other modalities if not config['sensors'][sensor_key]['modality'] == 'rgb' and not config['sensors'][sensor_key]['modality'] == 'depth': continue topic = config['sensors'][sensor_key]['topic_name'] camera_info_topic = os.path.dirname(topic) + '/camera_info' if camera_info_topic not in bag_topics: atomError('Topic ' + Fore.BLUE + camera_info_topic + Style.RESET_ALL + ' (from sensor ' + Fore.BLUE + sensor_key + Style.RESET_ALL + ') does not exist in the bag file. This topic must exist. Aborting configuration!' + Style.RESET_ALL) atomPrintOK() # Verify the existence of all tf frames referred in the config file print('Checking if coordinate frames from config.yml exist in bagfile ... ', end='') frames_to_verify = {'world_link': config['world_link']} for calibration_pattern_key, calibration_pattern in config['calibration_patterns'].items(): frames_to_verify[calibration_pattern_key + '.pattern_link'] = calibration_pattern['parent_link'] for sensor_key in config['sensors']: # Check if link, child_link and parent_link exist in the bagfile. for frame in ['child_link', 'parent_link', 'link']: frames_to_verify[sensor_key + '.' + frame] = config['sensors'][sensor_key][frame] # Search for additional tfs to be estimated if config['additional_tfs'] is not None: # Check if link, child_link and parent_link exist in the bagfile. for additional_tf_key in config['additional_tfs']: for frame in ['child_link', 'parent_link']: frames_to_verify[additional_tf_key + '.' + frame] = config['additional_tfs'][additional_tf_key][frame] # Verify that the parent and child links for each additional_tfs are a direct transformation for additional_tf_key, additional_tf in config['additional_tfs'].items(): edge_in_config = (config['additional_tfs'][additional_tf_key]['parent_link'], config['additional_tfs'][additional_tf_key]['child_link']) if not edge_in_config in nx_graph.edges(): atomError('Additional_tfs ' + Fore.BLUE + additional_tf_key + Style.RESET_ALL + ' parent and child links are ' + Fore.YELLOW + edge_in_config[0] + Fore.BLACK + ' and ' + Fore.YELLOW + edge_in_config[1] + Fore.BLACK + ' but there is no such atomic transformation in the tf tree.' + Style.RESET_ALL) for additional_tf_key, additional_tf in config['additional_tfs'].items(): # Check if frames exits if additional_tf['child_link'] not in nx_graph.nodes: atomError('Additional tf for child link ' + Fore.BLUE + additional_tf['child_link'] + Style.RESET_ALL + ' not found.\nCheck the frames you entered in the config.yaml.') # path between root and additional_tfs data frame print('\nChecking path from ' + config['world_link'] + ' to ' + additional_tf['child_link']) paths = nx.shortest_path(nx_graph, config['world_link'], additional_tf['child_link']) print('Path from ' + Fore.RED + config['world_link'] + Fore.RESET + ' to additional_tfs ' + Fore.LIGHTMAGENTA_EX + additional_tf_key + Fore.RESET + ':') at = '[' for path in paths: if path == config['world_link']: at += Fore.RED + path + Fore.RESET + ', ' elif path == additional_tf['parent_link'] or path == additional_tf['child_link']: at += Fore.BLUE + path + Fore.RESET + ', ' else: for joint in description.joints: # atomic transformations are given by the joints if path == joint.parent or path == joint.child: if joint.type == 'fixed': at += path + ', ' else: at += Fore.GREEN + path + Fore.RESET + ', ' break at = at[:-2] at += ']' print(at) # Checking if all tf frames exist for key, frame in frames_to_verify.items(): if not frame in nx_graph: atomError('Frame ' + Fore.BLUE + frame + Style.RESET_ALL + ' given in config file for ' + Fore.BLUE + key + Style.RESET_ALL + ' does not exist. Check config file ' + Fore.BLUE + args['config_file'] + Style.RESET_ALL) atomPrintOK() # Verify that the parent and child links for each sensor are a direct transformation (#316). print('Checking if transforms in config.yml are direct transformations ... ', end='') for sensor_key, sensor in config['sensors'].items(): edge_in_config = (config['sensors'][sensor_key]['parent_link'], config['sensors'][sensor_key]['child_link']) if not edge_in_config in nx_graph.edges(): atomError('Sensor ' + Fore.BLUE + sensor_key + Style.RESET_ALL + ' parent and child links are ' + Fore.YELLOW + edge_in_config[0] + Style.RESET_ALL + ' and ' + Fore.YELLOW + edge_in_config[1] + Style.RESET_ALL + ' but there is no such atomic transformation in the tf tree.' + Style.RESET_ALL) atomPrintOK() # Verify that the frame_id in the calibration.yaml->sensor->link field is the same as in the published messages (#514). print('Checking consistency of configuration tf frames with the messages on corresponding topic ...', end='') for sensor_key, sensor in config['sensors'].items(): frame_in_config = config['sensors'][sensor_key]['link'] topic_in_config = config['sensors'][sensor_key]['topic_name'] for topic, msg, t in bag.read_messages(topics=[topic_in_config]): if not msg.header.frame_id == frame_in_config: atomError('Sensor ' + Fore.BLUE + sensor_key + Style.RESET_ALL + ' calibration config defined link ' + Fore.YELLOW + frame_in_config + Fore.BLACK + ' but bagfile messages on topic ' + Fore.BLUE + topic_in_config + Fore.BLACK + ' have header.frame_id ' + Fore.YELLOW + msg.header.frame_id + Fore.BLACK + '. These should be the same! Check #514.' + Style.RESET_ALL) break # check consistency only for the first message atomPrintOK() # if nx.is_connected(gx): if nx.is_weakly_connected(nx_graph): print('Checking if TF tree is connected ... ', end='') else: pos = nx.random_layout(nx_graph) edges, weights = zip(*nx.get_edge_attributes(nx_graph, 'weight').items()) # edge_labels = nx.draw_networkx_edge_labels(nx_graph, pos) nx.draw(nx_graph, with_labels=True) plt.show() raise ValueError('TF tree is not connected. Aborting.') atomPrintOK() # Pretty print of transformation chains # TODO How to detect a dynamic transformation using the information in the bagfile's tfs? print( 'Transformations for sensors. Color coding is ' + Fore.RED + 'root link' + Fore.RESET + ', ' + Fore.BLUE + 'to be calibrated' + Fore.RESET + ', ' + Fore.GREEN + 'dynamic transformation' + Fore.RESET + ', ' + Fore.LIGHTMAGENTA_EX + 'sensor data frame' + Fore.RESET + '.') for sensor_key, sensor in config['sensors'].items(): paths = nx.shortest_path(nx_graph, config['world_link'], sensor['link'] ) # path between root and sensor data frame print('Path from ' + Fore.RED + config['world_link'] + Fore.RESET + ' to sensor ' + Fore.LIGHTMAGENTA_EX + sensor_key + Fore.RESET + ':') s = '[' for path in paths: if path == config['world_link']: s += Fore.RED + path + Fore.RESET + ', ' elif path == sensor['parent_link'] or path == sensor['child_link']: s += Fore.BLUE + path + Fore.RESET + ', ' elif path == sensor['link']: s += Fore.LIGHTMAGENTA_EX + path + Fore.RESET + ', ' else: for joint in description.joints: # atomic transformations are given by the joints if path == joint.parent or path == joint.child: if joint.type == 'fixed': s += path + ', ' else: s += Fore.GREEN + path + Fore.RESET + ', ' break s = s[:-2] s += ']' print(s) # Verify that the transformations selected to be optimized are indeed static (#581) for sensor_key, sensor in config['sensors'].items(): parent = sensor['parent_link'] child = sensor['child_link'] edge = nx_graph.get_edge_data(parent, child) # print(edge['type']) DEBUG if not edge['type'] == 'fixed': atomError('Sensor ' + Fore.BLUE + sensor_key + Style.RESET_ALL + ' defines transformation ' + Fore.LIGHTMAGENTA_EX + parent + ' to ' + child + Style.RESET_ALL + ' to be calibrated but this transformation is not static and therefore cannot be calibrated.') # Verify if urdf contains joints defined to be calibrated if config['joints'] is not None: for joint_key, joint in config['joints'].items(): joint_in_urdf = False for urdf_joint in description.joints: if urdf_joint.name == joint_key: joint_in_urdf = True break if not joint_in_urdf: atomError('Defined joint ' + Fore.BLUE + joint_key + Style.RESET_ALL + ' to be calibrated, but it does not exist in the urdf description.\nAre you sure there is a joint in the urdf defined with parent ' + Fore.BLUE + parent + Style.RESET_ALL + ' and child ' + Fore.BLUE + child + Style.RESET_ALL + ' ?') # Verify if joints are of type revolute or prismatic if config['joints'] is not None: for joint_key, joint in config['joints'].items(): joint_in_urdf = False for urdf_joint in description.joints: if urdf_joint.name == joint_key: if not urdf_joint.type in ['revolute', 'prismatic']: atomError('Defined joint ' + Fore.BLUE + joint_key + Style.RESET_ALL + ' to be calibrated, but this is not of type revolute, continuous or prismatic.' + Style.RESET_ALL) # Verify if joints calibrate parameters are valid if config['joints'] is not None: allowed_params = ['origin_x', 'origin_y', 'origin_z', 'origin_roll', 'origin_pitch', 'origin_yaw'] for joint_key, joint in config['joints'].items(): for param in joint['params_to_calibrate']: if not param in allowed_params: atomError('Joint ' + Fore.BLUE + joint_key + Style.RESET_ALL + ' is configured to calibrate parameter ' + Fore.BLUE + param + Style.RESET_ALL + ' which is not known.\nOnly parameters ' + Fore.BLUE + str(allowed_params) + Style.RESET_ALL + ' are allowed.') # Verify if bagfile contains joints defined to be calibrated if not config['joints'] == "": joints_to_check = deepcopy(config['joints']) if joints_to_check: # only if there are joints to check print('Checking if bagfile contains joint to be calibrated in /joint_states topic ... ') # Read first 10 secs of bagfile and search for joint state msgs for topic, msg, stamp in bag.read_messages(): if topic == '/joint_states': for idx, (bag_joint, bag_position) in enumerate(zip(msg.name, msg.position)): for joint_to_check_key, joint_to_check in joints_to_check.items(): if joint_to_check_key == bag_joint: print('Joint ' + Fore.BLUE + joint_to_check_key + Style.RESET_ALL + ' (' + bag_joint + ' in bagfile) found ', end='') atomPrintOK() del joints_to_check[joint_to_check_key] break if (stamp-bag_initial_stamp).to_sec() > 10: # assume in the first 5 secs we have covered all the existing joints break if list(joints_to_check.keys()): atomError('Bagfile /joint_states message do not contain joint ' + str(list(joints_to_check.keys())) + ' (' + bag_joint + ' in bagfile)') # Verify if the joints define to be calibrated do not collide with transforms to be calibrated if config['joints'] is not None: print('Checking if joints to be calibrated do not collide with transformations to be calibrated ... ') for joint_key, joint in config['joints'].items(): parent, child = getJointParentChild(joint_key, description) for sensor_key, sensor in config['sensors'].items(): if sensor['parent_link'] == parent and sensor['child_link'] == child: atomError('Transformation for sensor ' + sensor_key + ' collides with joint ' + joint_key) if config['additional_tfs'] is not None: for additional_tf_key, additional_tf in config['additional_tfs'].items(): if additional_tf['parent_link'] == parent and additional_tf['child_link'] == child: atomError('Additional tf ' + additional_tf_key + ' collides with joint ' + joint_key) # Verify if there are two calibration patterns with the same aruco dictionary # https://github.com/lardemua/atom/issues/784 print('Checking if there are two patterns with the same aruco dictionary ... ', end='') aruco_dictionaries = [] for calibration_pattern_key, calibration_pattern in config['calibration_patterns'].items(): aruco_dictionaries.append(calibration_pattern['dictionary']) duplicates = [x for x in aruco_dictionaries if aruco_dictionaries.count(x) >= 2] if duplicates: atomError('There are two or more calibration patterns with the same aruco dictionary. This is not a valid configuration. Repeated aruco dictionaries are: ' + str(list(set(duplicates)))) atomPrintOK() # Verify if there are equal transformations in the config file # https://github.com/lardemua/atom/issues/895 print("Checking if there transformations in the config file are unique ...") transforms_list = [] for sensor_key, sensor in config['sensors'].items(): transform_key = generateKey(sensor["parent_link"], sensor["child_link"]) transforms_list.append(transform_key) if config['additional_tfs'] is not None: for additional_tf_key, additional_tf in config['additional_tfs'].items(): transform_key = generateKey(additional_tf["parent_link"], additional_tf["child_link"]) transforms_list.append(transform_key) print('List of transformations to optimize is: ' + Fore.BLUE + str(transforms_list) + Style.RESET_ALL) transforms_set = set(transforms_list) if len(transforms_list) != len(transforms_set): tmp_list = [] duplicate_transforms = [] for transform in transforms_list: if transform not in tmp_list: tmp_list.append(transform) else: duplicate_transforms.append(transform) # this method catches the first atomError('Config file has duplicate transform(s) ' + Fore.BLUE + str(duplicate_transforms) + Style.RESET_ALL + '. Invalid configuration.') # NOTE: Development of IMU calibration will progress throughout the calibration pipeline, meaning some of the scripts created during configuration might not work if there is an IMU sensor and depending on the current stage of development. Adding a warning... imu_present = False for sensor_key, sensor in config['sensors'].items(): if sensor['modality'] == "imu": imu_present = True if imu_present == True: atomWarn('There is at least one sensor with modality "imu". At the moment, the calibration of systems involving IMU sensors is in development. As such, some of the scripts generated by this configuration might not be functional.') # -------------------------------------------------------------------------- # Create dot calibration graph (used for printing out a summary) # -------------------------------------------------------------------------- dot_graph = createDotGraph(nx_graph, config) # dot_graph.view() dot_graph.render(filename='summary', directory=package_path + '/calibration', cleanup=True) # dot_graph.view() dot_graph.render(filename='summary', directory=package_path + '/calibration', cleanup=True) # -------------------------------------------------------------------------- # Create the playback launch file # -------------------------------------------------------------------------- launch_file = playbag_launch_file print('Setting up ' + Fore.BLUE + playbag_launch_file + Style.RESET_ALL + ' ...') template = env.get_template('playbag.launch.j2') with open(launch_file, 'wb') as f: output = template.render(c={'filename': os.path.basename(launch_file), 'date': dt_string, 'bag_file': selected_bag_file_relative, 'package_name': package_name, 'rviz_playbag': rviz_playbag, 'use_compressed_topics': bool(compressed_topics), 'compressed_topics': compressed_topics, 'throttle_topics': throttle_topics, 'use_atom_bag_file': use_atom_bag_file, }) f.write(output.encode('utf-8')) # -------------------------------------------------------------------------- # Create the set_initial_estimate launch file # -------------------------------------------------------------------------- launch_file = set_initial_estimate_launch_file print('Setting up ' + Fore.BLUE + launch_file + Style.RESET_ALL + ' ...') template = env.get_template('set_initial_estimate.launch.j2') with open(launch_file, 'wb') as f: output = template.render(c={'filename': os.path.basename(launch_file), 'date': dt_string, 'package_name': package_name, 'rviz_set_initial_estimate': rviz_set_initial_estimate, 'bag_file': selected_bag_file_relative, 'marker_size': 0.5, 'config_file': os.path.basename(args['config_file']) }) f.write(output.encode('utf-8')) # -------------------------------------------------------------------------- # Create the data collection launch file # -------------------------------------------------------------------------- launch_file = data_collection_launch_file print('Setting up ' + Fore.BLUE + launch_file + Style.RESET_ALL + ' ...') # Create a dictionary of the labelers labelers = {} for sensor_key, sensor in config['sensors'].items(): if sensor['modality'] == 'rgb': labelers[sensor_key] = {'command': 'rgb_labeler'} elif sensor['modality'] == 'lidar3d': labelers[sensor_key] = {'command': 'lidar3d_labeler'} elif sensor['modality'] == 'depth': labelers[sensor_key] = {'command': 'depth_labeler'} elif sensor['modality'] == 'imu': # No labeler is needed for an IMU sensor. Check that does not break anything (#1000) -- Diogo V. continue else: atomError('Sensor ' + sensor_key + ' has unknown modality ' + sensor['modality']) template = env.get_template('collect_data.launch.j2') with open(launch_file, 'wb') as f: output = template.render(c={'filename': os.path.basename(launch_file), 'date': dt_string, 'package_name': package_name, 'rviz_collect_data': rviz_collect_data, 'bag_file': selected_bag_file_relative, 'config_file': os.path.basename(args['config_file']), 'labelers': labelers }) f.write(output.encode('utf-8')) # -------------------------------------------------------------------------- # Create the calibrate launch file # -------------------------------------------------------------------------- launch_file = calibrate_launch_file print('Setting up ' + Fore.BLUE + launch_file + Style.RESET_ALL + ' ...') template = env.get_template('calibrate.launch.j2') with open(launch_file, 'wb') as f: output = template.render(c={'filename': os.path.basename(launch_file), 'date': dt_string, 'package_name': package_name, 'rviz_calibrate': rviz_calibrate }) f.write(output.encode('utf-8')) # -------------------------------------------------------------------------- # Create the dataset playback launch file # -------------------------------------------------------------------------- launch_file = dataset_playback_launch_file print('Setting up ' + Fore.BLUE + launch_file + Style.RESET_ALL + ' ...') template = env.get_template('dataset_playback.launch.j2') with open(launch_file, 'wb') as f: output = template.render(c={'filename': os.path.basename(launch_file), 'date': dt_string, 'package_name': package_name, 'bag_file': selected_bag_file_relative, 'rviz_dataset_playback': rviz_dataset_playback }) f.write(output.encode('utf-8')) # -------------------------------------------------------------------------- # Create the full evaluation launch file # -------------------------------------------------------------------------- launch_file = full_evaluation_launch_file print('Setting up ' + Fore.BLUE + launch_file + Style.RESET_ALL + ' ...') # Create a list of all possible combinations of sensor pairs sensor_combinations_list = list(itertools.combinations(config['sensors'].keys(), 2)) # Initialize a dictionary to store sensor combination descriptions sensor_combinations_dict = dict() # Define mappings for filenames, arguments and based on sensor modalities # Added imu sensor pairs to this (#1000) -- Diogo V. modality_to_filename = { ('rgb', 'rgb'): 'rgb_to_rgb', ('lidar3d', 'lidar3d'): 'lidar_to_lidar', ('depth', 'depth'): 'depth_to_depth', ('imu', 'imu'): 'imu_to_imu', ('rgb', 'lidar3d'): 'lidar_to_rgb', ('lidar3d', 'rgb'): 'lidar_to_rgb', ('depth', 'lidar3d'): 'lidar_to_depth', ('lidar3d', 'depth'): 'lidar_to_depth', ('rgb', 'depth'): 'depth_to_rgb', ('depth', 'rgb'): 'depth_to_rgb', ('imu', 'rgb'): 'rgb_to_imu', ('rgb', 'imu'): 'rgb_to_imu', ('imu', 'depth'): 'depth_to_imu', ('depth', 'imu'): 'depth_to_imu', ('imu', 'lidar3d'): 'lidar_to_imu', ('lidar3d', 'imu'): 'lidar_to_imu' } modality_to_args = { 'rgb': '-cs {} ', 'depth': '-ds {} ', 'lidar3d': '-rs {} ', 'imu': '-is {}' } for calibration_pattern_key, calibration_pattern in config['calibration_patterns'].items(): for source, target in sensor_combinations_list: # Initialize a description for the evaluation evaluation_description = { 'filename': None, 'args': None, } # Determine the modalities of the source and target sensors source_modality = config['sensors'][source]['modality'] target_modality = config['sensors'][target]['modality'] modality_combination = (source_modality, target_modality) # Set the filename and args based on the modality combination evaluation_description['filename'] = modality_to_filename[modality_combination] if source_modality == target_modality: evaluation_description['args'] = f'-ss {source} -st {target}' else: evaluation_description['args'] = modality_to_args[source_modality].format(source) evaluation_description['args'] += modality_to_args[target_modality].format(target) # Added imu sensor pairs (#1000) -- Diogo V. if source_modality == target_modality or \ source_modality == 'depth' and target_modality == 'rgb' or \ source_modality == 'lidar3d' and target_modality == 'rgb' or \ source_modality == 'lidar3d' and target_modality == 'depth' or \ source_modality == 'rgb' and target_modality == 'imu' or \ source_modality == 'depth' and target_modality == 'imu' or \ source_modality == 'lidar3d' and target_modality == 'imu': combination_name = source + '_to_' + target elif source_modality == 'rgb' and target_modality == 'depth' or \ source_modality == 'rgb' and target_modality == 'lidar3d' or \ source_modality == 'depth' and target_modality == 'lidar3d' or \ source_modality == 'imu' and target_modality == 'rgb' or \ source_modality == 'imu' and target_modality == 'depth' or \ source_modality == 'imu' and target_modality == 'lidar3d': combination_name = target + '_to_' + source combination_name = combination_name + '_' + calibration_pattern_key # Store the evaluation description in the sensor_combinations_dict sensor_combinations_dict[combination_name] = evaluation_description # Only valid when pattern is fixed if config['calibration_patterns'][calibration_pattern_key]['fixed']: # Create an inter-collection evaluation description inter_collection_evaluation_description = { 'filename': None, 'args': None, } # Set the filename and arguments for inter-collection evaluation inter_collection_evaluation_description['filename'] = 'inter_collection_' + \ evaluation_description['filename'] inter_collection_evaluation_description['args'] = evaluation_description['args'] + \ ' -wf ' + config['world_link'] # Store the inter-collection evaluation description in the sensor_combinations_dict sensor_combinations_dict[f'inter_collection_{combination_name}'] = inter_collection_evaluation_description # Only valid when pattern is fixed if config['calibration_patterns'][calibration_pattern_key]['fixed']: # Create evaluation descriptions for sensors against themselves for sensor in config['sensors'].keys(): evaluation_description = { 'filename': None, 'args': None, } evaluation_description['filename'] = 'inter_collection_' + modality_to_filename[ (config['sensors'][sensor]['modality'], config['sensors'][sensor]['modality'])] evaluation_description['args'] = f'-ss {sensor} -st {sensor} -wf {config["world_link"]}' sensor_combinations_dict['inter_collection_' + sensor + '_to_' + sensor + '_' + calibration_pattern_key] = \ evaluation_description template = env.get_template('full_evaluation.launch.j2') with open(launch_file, 'wb') as f: output = template.render(c={'filename': os.path.basename(launch_file), 'date': dt_string, 'sensor_combinations': sensor_combinations_dict, }) f.write(output.encode('utf-8')) # -------------------------------------------------------------------------- # Create the rviz config core displays (used in several rviz config files) # -------------------------------------------------------------------------- # TODO change rviz fixed_frame according to args['world_link'] core_displays = [] # Create grid, tf and robot model displays rendered = env.get_template('/rviz/Grid.rviz.j2').render(c={'Reference_Frame': config['world_link']}) core_displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) rendered = env.get_template('/rviz/TF.rviz.j2').render(c={'Name': 'TF'}) core_displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) rendered = env.get_template('/rviz/RobotModel.rviz.j2').render(c={'Name': 'RobotModel'}) core_displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) # -------------------------------------------------------------------------- # Create the rviz config file for the playbag # -------------------------------------------------------------------------- print('Setting up ' + Fore.BLUE + rviz_playbag + Style.RESET_ALL + ' ...') rviz = yaml.load(open(rviz_file_template), Loader=yaml.SafeLoader) rviz['Visualization Manager']['Global Options']['Fixed Frame'] = config['world_link'] displays = deepcopy(core_displays) # start with the core displays # Create interactive marker display for moving the sensors rendered = env.get_template('/rviz/InteractiveMarker.rviz.j2').render(c={'Name': 'MoveSensors-InteractiveMarker', 'Update_Topic': 'set_initial_estimate/update'} ) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) # Generate rviz displays according to the sensor types # Iterate alphabetically through the sensors dict keys https://github.com/lardemua/atom/issues/409 for idx, sensor_key in enumerate(sorted(list(config['sensors'].keys()))): color = atom_core.drawing.colormapToRVizColor(cm_sensors[idx, :]) topic = config['sensors'][sensor_key]['topic_name'] topic_compressed = topic + '/compressed' modality = config['sensors'][sensor_key]['modality'] print('\tGenerating rviz displays for sensor ' + Fore.BLUE + sensor_key + Style.RESET_ALL + ' with topic ' + Fore.BLUE + topic + Style.RESET_ALL + ' (' + Fore.CYAN + modality + Style.RESET_ALL + ')') if modality == 'rgb' or modality == 'depth': # Raw image rendered = env.get_template('/rviz/Image.rviz.j2').render(c={'Name': sensor_key + '-Image', 'Image_Topic': topic}) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) # Camera rendered = env.get_template('/rviz/Camera.rviz.j2').render(c={'Name': sensor_key + '-Camera', 'Image_Topic': topic}) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) elif modality == 'lidar2d': # Raw data rendered = env.get_template('/rviz/LaserScan.rviz.j2').render(c={'Name': sensor_key + '-LaserScan', 'Topic': topic, 'Color': color}) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) # elif msg_type == 'sensor_msgs/PointCloud2': elif modality == 'lidar3d': # Raw data rendered = env.get_template('/rviz/PointCloud2.rviz.j2').render(c={'Name': sensor_key + '-PointCloud2', 'Topic': topic, 'Color': color, 'Color_Transformer': 'FlatColor', 'Style': 'Spheres', 'Size__m_': 0.02, 'Alpha': 1}) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) elif modality == 'imu': continue else: atomError('Warning: Cannot generate rviz configuration for sensor ' + sensor_key + ' with topic ' + topic + ' (' + modality + ')' + Style.RESET_ALL) rviz['Visualization Manager']['Displays'] = displays yaml.dump(rviz, open(package_path + '/rviz/' + rviz_playbag, 'w')) # -------------------------------------------------------------------------- # Create the rviz config file for the set_initial_estimate # -------------------------------------------------------------------------- print('Setting up ' + Fore.BLUE + rviz_set_initial_estimate + Style.RESET_ALL + ' ...') rviz = yaml.load(open(rviz_file_template), Loader=yaml.SafeLoader) rviz['Visualization Manager']['Global Options']['Fixed Frame'] = config[ 'world_link'] # set fixed frame to world_link displays = deepcopy(core_displays) # start with the core displays # Create interactive marker display for moving the sensors rendered = env.get_template('/rviz/InteractiveMarker.rviz.j2').render(c={'Name': 'MoveSensors-InteractiveMarker', 'Update_Topic': 'set_initial_estimate/update'} ) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) # Generate rviz displays according to the sensor types # Iterate alphabetically through the sensors dict keys https://github.com/lardemua/atom/issues/409 for idx, sensor_key in enumerate(sorted(list(config['sensors'].keys()))): color = atom_core.drawing.colormapToRVizColor(cm_sensors[idx, :]) topic = config['sensors'][sensor_key]['topic_name'] topic_compressed = topic + '/compressed' # if topic in bag_topics: # msg_type = bag_info[1][topic].msg_type # else: # msg_type = bag_info[1][topic_compressed].msg_type modality = config['sensors'][sensor_key]['modality'] print('\tGenerating rviz displays for sensor ' + Fore.BLUE + sensor_key + Style.RESET_ALL + ' with topic ' + Fore.BLUE + topic + Style.RESET_ALL + ' (' + Fore.CYAN + modality + Style.RESET_ALL + ')') # if msg_type == 'sensor_msgs/CompressedImage' or \ # msg_type == 'sensor_msgs/Image': # add displays for camera sensor # TODO check if depth should be here or not if modality == 'rgb' or modality == 'depth': # Raw image rendered = env.get_template('/rviz/Image.rviz.j2').render(c={'Name': sensor_key + '-Image', 'Image_Topic': topic}) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) # Camera rendered = env.get_template('/rviz/Camera.rviz.j2').render(c={'Name': sensor_key + '-Camera', 'Image_Topic': topic}) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) # Frustum rendered = env.get_template('/rviz/MarkerArray.rviz.j2').render(c={'Name': sensor_key + '-Frustum', 'Marker_Topic': sensor_key + '/frustum'}) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) # elif msg_type == 'sensor_msgs/LaserScan': elif modality == 'lidar2d': # Raw data rendered = env.get_template('/rviz/LaserScan.rviz.j2').render(c={'Name': sensor_key + '-LaserScan', 'Topic': topic, 'Color': color}) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) # elif msg_type == 'sensor_msgs/PointCloud2': elif modality == 'lidar3d': # Raw data rendered = env.get_template('/rviz/PointCloud2.rviz.j2').render(c={'Name': sensor_key + '-PointCloud2', 'Topic': topic, 'Color': color, 'Color_Transformer': 'FlatColor', 'Style': 'Spheres', 'Size__m_': 0.02, 'Alpha': 1}) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) elif modality == 'imu': continue else: atomError('Warning: Cannot generate rviz configuration for sensor ' + sensor_key + ' with topic ' + topic + ' (' + modality + ')' + Style.RESET_ALL) rviz['Visualization Manager']['Displays'] = displays yaml.dump(rviz, open(package_path + '/rviz/' + rviz_set_initial_estimate, 'w')) # -------------------------------------------------------------------------- # Create the rviz config file for the collect_data # -------------------------------------------------------------------------- print('Setting up ' + Fore.BLUE + rviz_collect_data + Style.RESET_ALL + ' ...') rviz = yaml.load(open(rviz_file_template), Loader=yaml.SafeLoader) rviz['Visualization Manager']['Global Options']['Fixed Frame'] = config[ 'world_link'] # set fixed frame to world_link displays = deepcopy(core_displays) # start with the core displays # Add interactive maker display to handle sensor manual labeling rendered = env.get_template('/rviz/InteractiveMarker.rviz.j2').render( c={'Name': 'ManualDataLabeler-InteractiveMarkers', 'Update_Topic': 'data_labeler/update'}) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) # TODO this will be the menu (only one) depth = 0 # Generate rviz displays according to the sensor types # Iterate alphabetically through the sensors dict keys https://github.com/lardemua/atom/issues/409 for idx, sensor_key in enumerate(sorted(list(config['sensors'].keys()))): color = atom_core.drawing.colormapToRVizColor(cm_sensors[idx, :]) topic = config['sensors'][sensor_key]['topic_name'] topic_compressed = topic + '/compressed' # if topic in bag_topics: # msg_type = bag_info[1][topic].msg_type # else: # msg_type = bag_info[1][topic_compressed].msg_type modality = config['sensors'][sensor_key]['modality'] print('\tGenerating rviz displays for sensor ' + Fore.BLUE + sensor_key + Style.RESET_ALL + ' with topic ' + Fore.BLUE + topic + Style.RESET_ALL + ' (' + Fore.CYAN + modality + Style.RESET_ALL + ')') # if msg_type == 'sensor_msgs/CompressedImage' or msg_type == 'sensor_msgs/Image': # displays for camera sensor # TODO check if depth should be here or separated if modality == 'rgb': # Image rendered = env.get_template('/rviz/Image.rviz.j2').render(c={'Name': sensor_key + '-Labels' + '-Image', 'Image_Topic': topic + '/labeled'}) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) # Camera rendered = env.get_template('/rviz/Camera.rviz.j2').render(c={'Name': sensor_key + '-Camera', 'Image_Topic': topic}) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) elif modality == 'depth': depth += 1 rendered = env.get_template('/rviz/Image.rviz.j2').render(c={'Name': sensor_key + '-Labels' + '-Image', 'Image_Topic': topic + '/labeled'}) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) # Camera rendered = env.get_template('/rviz/Camera.rviz.j2').render(c={'Name': sensor_key + '-Camera', 'Image_Topic': topic}) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) rendered = env.get_template('/rviz/InteractiveMarker.rviz.j2').render( c={'Name': sensor_key + '-ManualDataLabeler-InteractiveMarkers', 'Update_Topic': sensor_key + '/data_labeler/update'} ) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) # elif msg_type == 'sensor_msgs/LaserScan': elif modality == 'lidar2d': rendered = env.get_template('/rviz/LaserScan.rviz.j2').render(c={'Name': sensor_key + '-LaserScan', 'Topic': topic, 'Color': color}) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) # Labels rendered = env.get_template('/rviz/PointCloud2.rviz.j2').render(c={'Name': sensor_key + '-Labels-PointCloud2', 'Topic': topic + '/labeled', 'Color': color, 'Color_Transformer': 'FlatColor', 'Style': 'Spheres', 'Size__m_': 0.2, 'Alpha': 0.05}) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) # TODO Add markers to display data clusters rendered = env.get_template('/rviz/InteractiveMarker.rviz.j2').render( c={'Name': sensor_key + '-ManualDataLabeler-InteractiveMarkers', 'Update_Topic': sensor_key + '/data_labeler/update'} ) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) # elif msg_type == 'sensor_msgs/PointCloud2': elif modality == 'lidar3d': # Raw data rendered = env.get_template('/rviz/PointCloud2.rviz.j2').render(c={'Name': sensor_key + '-PointCloud2', 'Topic': topic, 'Color': color, 'Color_Transformer': 'FlatColor', 'Style': 'Spheres', 'Size__m_': 0.02, 'Alpha': 1}) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) # Labeled data rendered = env.get_template('/rviz/PointCloud2.rviz.j2').render(c={'Name': sensor_key + '-Labels-PointCloud2', 'Topic': topic + '/labeled', 'Color': color, 'Color_Transformer': 'FlatColor', 'Style': 'Spheres', 'Size__m_': 0.2, 'Alpha': 0.05}) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) # Interactive marker for the labeler rendered = env.get_template('/rviz/InteractiveMarker.rviz.j2').render( c={'Name': sensor_key + '-ManualDataLabeler-InteractiveMarkers', 'Update_Topic': sensor_key + '/data_labeler/update'} ) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) elif modality == 'imu': continue else: print(Fore.YELLOW + 'Warning: Cannot generate rviz configuration for sensor ' + sensor_key + ' with topic ' + topic + ' (' + modality + ')' + Style.RESET_ALL) if depth != 0: # TODO #411 this is strange here ... # rendered = env.get_template('/rviz/MarkerArray.rviz').render( # c={'Name': 'FrustumVisualizationDepthSensors', # 'Marker_Topic': '/sensor_frustum'}) # displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) pass rviz['Visualization Manager']['Displays'] = displays yaml.dump(rviz, open(package_path + '/rviz/' + rviz_collect_data, 'w')) # -------------------------------------------------------------------------- # Create the rviz config file for the dataset_playback # -------------------------------------------------------------------------- print('Setting up ' + Fore.BLUE + rviz_dataset_playback + Style.RESET_ALL + ' ...') rviz = yaml.load(open(rviz_file_template), Loader=yaml.SafeLoader) rviz['Visualization Manager']['Global Options']['Fixed Frame'] = config[ 'world_link'] # set fixed frame to world_link displays = deepcopy(core_displays) # start with the core displays displays.pop() # remove last core display which is robot model rendered = env.get_template('/rviz/MarkerArray.rviz.j2').render(c={'Name': 'Robot Meshes', 'Marker_Topic': '/dataset_playback/robot_meshes'}) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) # Iterate alphabetically through the sensors dict keys https://github.com/lardemua/atom/issues/409 for idx, sensor_key in enumerate(sorted(list(config['sensors'].keys()))): color = atom_core.drawing.colormapToRVizColor(cm_sensors[idx, :]) topic = config['sensors'][sensor_key]['topic_name'] modality = config['sensors'][sensor_key]['modality'] print('\tGenerating rviz displays for sensor ' + Fore.BLUE + sensor_key + Style.RESET_ALL + ' with topic ' + Fore.BLUE + topic + Style.RESET_ALL + ' (' + Fore.CYAN + modality + Style.RESET_ALL + ')') if modality == 'rgb': # Raw image labeled_topic = generateLabeledTopic(topic, type='2d') rendered = env.get_template('/rviz/Image.rviz.j2').render(c={'Name': sensor_key + '-Labels-Image', 'Image_Topic': labeled_topic}) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) elif modality == 'lidar2d': # Raw data labeled_topic = generateLabeledTopic(topic, type='3d') rendered = env.get_template('/rviz/LaserScan.rviz.j2').render(c={'Name': sensor_key + '-Labels-LaserScan', 'Topic': labeled_topic, 'Color': color}) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) elif modality == 'lidar3d': # Raw data labeled_topic = generateLabeledTopic(topic, type='3d') rendered = env.get_template('/rviz/PointCloud2.rviz.j2').render( c={'Name': sensor_key + '-Labels3d-PointCloud2', 'Topic': labeled_topic, 'Color': color, 'Color_Transformer': 'RGB8', 'Style': 'Spheres', 'Size__m_': 0.02, 'Alpha': 1}) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) elif modality == 'depth': # Raw image labeled_topic = generateLabeledTopic(topic, type='2d') rendered = env.get_template('/rviz/Image.rviz.j2').render(c={'Name': sensor_key + '-Labels-Image', 'Image_Topic': labeled_topic}) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) # 3D Rviz markers of Depth image 3D points corresponding to idxs and idxs_limit labeled_topic = generateLabeledTopic(topic, type='3d') rendered = env.get_template('/rviz/MarkerArray.rviz.j2').render(c={'Name': sensor_key + '-Labels-3D', 'Marker_Topic': labeled_topic}) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) elif modality == 'imu': continue else: print(Fore.YELLOW + 'Warning: Cannot generate rviz configuration for sensor ' + sensor_key + ' with topic ' + topic + ' (' + modality + ')' + Style.RESET_ALL) rviz['Visualization Manager']['Displays'] = displays yaml.dump(rviz, open(package_path + '/rviz/' + rviz_dataset_playback, 'w')) # -------------------------------------------------------------------------- # Create the rviz config file for the calibrate # -------------------------------------------------------------------------- print('Setting up ' + Fore.BLUE + rviz_calibrate + Style.RESET_ALL + ' ...') rviz = yaml.load(open(rviz_file_template), Loader=yaml.SafeLoader) rviz['Visualization Manager']['Global Options']['Fixed Frame'] = config[ 'world_link'] # set fixed frame to world_link displays = deepcopy(core_displays) # start with the core displays displays.pop() # remove the last item, which is the robot model # Add interactive maker display for showing robot meshes rendered = env.get_template('/rviz/MarkerArray.rviz.j2').render( c={'Name': 'RobotMeshes-MarkerArray', 'Marker_Topic': 'calibrate/robot_meshes'}) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) # Add a maker array display for showing the patterns rendered = env.get_template('/rviz/MarkerArray.rviz.j2').render( c={'Name': 'Patterns-MarkerArray', 'Marker_Topic': 'calibrate/patterns'}) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) # Add a maker array display for miscellaneous information rendered = env.get_template('/rviz/MarkerArray.rviz.j2').render( c={'Name': 'Miscellaneous-MarkerArray', 'Marker_Topic': 'calibrate/Miscellaneous'}) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) # Check if 2d lidars exist, if so add laser beams marker modalities = [] for idx, sensor_key in enumerate(config['sensors']): modality = config['sensors'][sensor_key]['modality'] modalities.append(modality) if 'lidar2d' in modalities: # we have 2D lidar data, add laser beams marker # Add interactive maker display for showing laser beams rendered = env.get_template('/rviz/MarkerArray.rviz.j2').render( c={'Name': 'LaserBeams-MarkerArray', 'Marker_Topic': 'calibrate/LaserBeams'} ) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) # Generate rviz displays according to the sensor types # Iterate alphabetically through the sensors dict keys https://github.com/lardemua/atom/issues/409 for idx, sensor_key in enumerate(sorted(list(config['sensors'].keys()))): color = atom_core.drawing.colormapToRVizColor(cm_sensors[idx, :]) topic = config['sensors'][sensor_key]['topic_name'] topic_compressed = topic + '/compressed' # if topic in bag_topics: # msg_type = bag_info[1][topic].msg_type # else: # msg_type = bag_info[1][topic_compressed].msg_type modality = config['sensors'][sensor_key]['modality'] print('\tGenerating rviz displays for sensor ' + Fore.BLUE + sensor_key + Style.RESET_ALL + ' with topic ' + Fore.BLUE + topic + Style.RESET_ALL + ' (' + Fore.CYAN + modality + Style.RESET_ALL + ')') # TODO check if depth should be here or separated if modality == 'rgb': # displays for camera sensor # Image # one image per collection will be published by calibrate, but we only create one Image Display listening to the collection 0 image topic. The user can then change to other collections. labeled_topic = generateLabeledTopic(topic, collection_key='000', type='2d') rendered = env.get_template('/rviz/Image.rviz.j2').render(c={'Name': sensor_key + '-Labels' + '-Image', 'Image_Topic': labeled_topic}) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) elif modality == "depth": # Depth Image with pattern idxs and idxs_limit overlayed labeled_topic = generateLabeledTopic(topic, collection_key='000', type='2d') rendered = env.get_template('/rviz/Image.rviz.j2').render(c={'Name': sensor_key + '-Labels' + '-Image', 'Image_Topic': labeled_topic}) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) # 3D Rviz markers of Depth image 3D points corresponding to idxs and idxs_limit labeled_topic = generateLabeledTopic(topic, type='3d') rendered = env.get_template('/rviz/MarkerArray.rviz.j2').render(c={'Name': sensor_key + '-Labels-3D', 'Marker_Topic': labeled_topic} ) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) elif modality == 'lidar2d': labeled_topic = generateLabeledTopic(topic, type='2d') rendered = env.get_template('/rviz/MarkerArray.rviz.j2').render(c={'Name': sensor_key + '-Labels3D', 'Marker_Topic': labeled_topic} ) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) elif modality == 'lidar3d': # 3D Rviz markers of lidar 3D points corresponding to idxs and idxs_limit labeled_topic = generateLabeledTopic(topic, type='3d') rendered = env.get_template('/rviz/MarkerArray.rviz.j2').render(c={'Name': sensor_key + '-LabeledData', 'Marker_Topic': labeled_topic} ) displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) rviz['Visualization Manager']['Displays'] = displays yaml.dump(rviz, open(package_path + '/rviz/' + rviz_calibrate, 'w')) # Create the transforms_graph json file transforms_graph_file = package_path + '/calibration/transforms_graph.json' print('Saving transforms graph to ' + Fore.BLUE + transforms_graph_file + Style.RESET_ALL) # nx_graph_json = nx.node_link_data(nx_graph, link="edges", source="parent", target="child") nx_graph_json = nx.node_link_data(nx_graph) with open(transforms_graph_file, 'w') as f: json.dump(nx_graph_json, f, indent=2, sort_keys=True) bag.close() # close the bag file. # Print final report print('\nCreated a calibration configuration summary. Use it to see if the calibration is well set up. Located at:\n' + Fore.BLUE + package_path + '/calibration/summary.pdf' + Style.RESET_ALL) print('\nSuccessfully configured calibration package ' + Fore.BLUE + package_name + Style.RESET_ALL + ' in ' + str(round(tictoc.tocvalue(), 2)) + ' seconds.\nYou can use the launch files:') print(Fore.BLUE + 'roslaunch ' + package_name + ' ' + os.path.basename(playbag_launch_file) + Style.RESET_ALL) print(Fore.BLUE + 'roslaunch ' + package_name + ' ' + os.path.basename(set_initial_estimate_launch_file) + Style.RESET_ALL) print(Fore.BLUE + 'roslaunch ' + package_name + ' ' + os.path.basename(data_collection_launch_file) + Style.RESET_ALL) print(Fore.BLUE + 'roslaunch ' + package_name + ' ' + os.path.basename(dataset_playback_launch_file) + Style.RESET_ALL) print(Fore.BLUE + 'roslaunch ' + package_name + ' ' + os.path.basename(calibrate_launch_file) + Style.RESET_ALL) print(Fore.BLUE + 'roslaunch ' + package_name + ' ' + os.path.basename(full_evaluation_launch_file) + Style.RESET_ALL)