def __init__(self):
rospy.init_node('occ_map_saver')
path_prefix = rospy.get_param("~path_prefix", "map")
self._file_prefix = rospy.get_param('~file_prefix', default="map")
timestamp = datetime.now().strftime('%y%m%d_%H%M%S')
default_path = os.path.join(os.path.join(os.path.expanduser('~')),
'Desktop')
default_path = os.path.join(default_path, 'OccMap')
default_path = os.path.join(default_path,
timestamp + "_" + path_prefix)
save_dir = rospy.get_param("~save_dir", default_path)
save_dir = os.path.expanduser(save_dir)
save_dir = os.path.expandvars(save_dir)
save_dir = os.path.normpath(save_dir)
if not os.path.exists(save_dir):
mkdir_p(save_dir)
self._save_dir = save_dir
rospy.Subscriber("map", OccupancyGrid, self._save_map, queue_size=1)
rospy.spin()
def __init__(self):
rospy.init_node("data_collector")
def_file_path = path.join("~", "Desktop", "Experiments")
file_path = rospy.get_param("~file_path", def_file_path)
file_prefix = rospy.get_param("~file_prefix", "error_data")
mapping_suffix = rospy.get_param("~mapping_suffix", "_map")
localization_suffix = rospy.get_param("~localization_suffix", "_loc")
if not path.exists(file_path):
mkdir_p(file_path)
filename = path.join(file_path, file_prefix)
mapping_filename = filename + mapping_suffix
mapping_filename = path.expanduser(mapping_filename)
mapping_filename = path.expandvars(mapping_filename)
localization_filename = filename + localization_suffix
localization_filename = path.expanduser(localization_filename)
localization_filename = path.expandvars(localization_filename)
self._mapping_filename = mapping_filename
self._localization_filename = localization_filename
self._current_err_file = self._mapping_filename
self._fs = ','
self._rs = '\n'
self._errors = {
"tra": [],
"rot": [],
"tot": [],
}
rospy.Subscriber("doLocOnly", Bool, self._loc_callback)
rospy.Subscriber("tra_err",
Float64,
self._err_callback,
callback_args="tra")
rospy.Subscriber("rot_err",
Float64,
self._err_callback,
callback_args="rot")
rospy.Subscriber("tot_err",
Float64,
self._err_callback,
callback_args="tot")
rospy.on_shutdown(self._shutdown_callback)
rospy.spin()
def start(self):
env = os.environ.copy()
if self._log_dir != "":
if not path.exists(self._log_dir):
mkdir_p(self._log_dir)
env['ROS_LOG_DIR'] = self._log_dir
try:
self._proc = subprocess.Popen(['roscore'],
env=env,
stdout=subprocess.PIPE)
self._pid = self._proc.pid
self._wait_for_core()
except OSError as e:
sys.stderr.write('ROSCore could not be started.')
raise e
self._is_running = True
path_files = [set(c) for c in path_files]
for file_filter in chc_file_filters:
avl_options[file_filter] = path_files[file_filters[file_filter]
['index']]
for file_filter in chc_file_filters:
if arg_dict[file_filter] is None:
file_filters[file_filter]['values'] = sorted(
list(avl_options[file_filter]))
else:
file_filters[file_filter]['values'] = sorted(
list(arg_dict[file_filter]))
if not path.exists(out_path):
mkdir_p(out_path)
FS = ","
LS = "\n"
extension = "csv"
data_header = ["Experiment_Num"]
data = []
max_experiments = -1
plt.ioff()
combination_filters = sorted(file_filters.keys(),
key=lambda x: file_filters[x]['index'])
combination_filters.remove(args.combine_by)
args_dict = dict(arg.split(":=") for arg in args)
path_save = get_arg(args_dict, 'path_save', None, add_if_not_in=False)
if path_save is None:
exp_pfx = get_arg(args_dict, 'exp_pfx', 'exp')
ts = get_arg(args_dict, 'ts', datetime.now().strftime('%y%m%d_%H%M%S'))
mm = get_arg(args_dict, 'mm', 'rfl', valid_vals=['rfl', 'dec'])
pw = get_arg(args_dict, 'pw', 'cmh', valid_vals=['cmh', 'ml', 'fsm'])
path_save = path.join(path.expanduser("~"), "Desktop", "Experiments", "IndividualExperiments",
"{}_{}_{}_{}".format(exp_pfx, ts, mm, pw))
# Create path for log files
log_path = path.join(path_save, "log")
if not path.exists(log_path):
mkdir_p(log_path)
# Create a symlink to the latest experiment
link_dest = path.join(path_save, "..", "latest")
if path.islink(link_dest):
os.remove(link_dest)
os.symlink(path_save, link_dest)
# Setup environment with path for logging
env = os.environ.copy()
env['ROS_LOG_DIR'] = log_path
cmd = "roslaunch map_simulator experiment.launch"
if args_dict:
cmd += " " + " ".join(["{}:=\"{}\"".format(k, v) for k, v in args_dict.items()])
def __init__(self):
"""
Initialize the PoseErrorCalculator object, the ROS node,
get the parameters and keep the node alive (spin)
"""
rospy.init_node('pose_error_calc')
# Error Accumulators
self._cum_trans_err = 0
self._cum_rot_err = 0
self._cum_tot_err = 0
# Buffers for current and last step's transformation matrices
self._curr_gt_mb_pose = None
self._curr_sl_mo_pose = None
self._curr_sl_ob_pose = None
self._last_gt_mb_pose = None
self._last_sl_ob_pose = None
# relative rotation quaternion (starting as zero-rotation)
self._last_rot_err_q = quaternion_from_euler(0, 0, 0)
# and info
self._curr_seq = None
self._curr_ts = None
self._curr_processed = False
self._last_seq = None
self._last_ts = None
self._last_processed = False
self._first_pose = True
self._loc_only_recvd = False
# Weight factor for rotational error
self._lambda = rospy.get_param("~lambda", 0.1)
# Publish and save boolean options
self._publish_error = rospy.get_param("~pub_err", True)
self._log_error = rospy.get_param("log_err", True)
if not (self._publish_error or self._log_error):
rospy.logerr(
"Neither publishing nor logging. Why call me then? Exiting.")
rospy.signal_shutdown(
"Nothing to do. Shutting down. Check _pub_err and _log_err parameters."
)
# TF Frames
self._map_frame = rospy.get_param("~map_frame", "map")
self._odom_frame = rospy.get_param("~odom_frame", "odom")
self._base_frame = rospy.get_param("~base_frame", "base_link")
self._gt_odom_frame = rospy.get_param("~gt_odom_frame", "GT/odom")
self._gt_base_frame = rospy.get_param("~gt_base_frame", "GT/base_link")
# CSV Log Path parameters
default_path = os.path.join("~", "Desktop")
default_path = os.path.join(default_path, "FMP_logs")
log_dir = rospy.get_param("~log_dir", default_path)
log_dir = os.path.expandvars(os.path.expanduser(log_dir))
# CSV Log File parameters
err_prefix = rospy.get_param("~err_prefix", "pose_err")
timestamp = datetime.datetime.now().strftime("%y%m%d_%H%M%S")
default_err_file = err_prefix + '_' + timestamp + '.csv'
default_err_file = os.path.join(log_dir, default_err_file)
default_loc_err_file = err_prefix + '_loc_' + timestamp + '.csv'
default_loc_err_file = os.path.join(log_dir, default_loc_err_file)
self._err_file = rospy.get_param("~err_file", default_err_file)
self._loc_err_file = rospy.get_param("~loc_err_file",
default_loc_err_file)
self._current_err_file = self._err_file
# CSV row and column delimiters
self._newline = rospy.get_param("~newline", "\n")
self._delim = rospy.get_param("~delim", ",")
if self._log_error:
if not os.path.exists(log_dir):
mkdir_p(log_dir)
self._init_log()
# Subscribers / Publishers
rospy.Subscriber("/tf", TFMessage, self._tf_callback)
rospy.Subscriber("/doLocOnly", BoolMessage, self._loc_only_callback)
rospy.on_shutdown(self._shutdown_callback)
if self._publish_error:
self._tra_err_pub = rospy.Publisher("tra_err",
Float64,
latch=True,
queue_size=1)
self._rot_err_pub = rospy.Publisher("rot_err",
Float64,
latch=True,
queue_size=1)
self._tot_err_pub = rospy.Publisher("tot_err",
Float64,
latch=True,
queue_size=1)
rospy.spin()
