def __init__(self, node_id, types, duckietown_graph, movable=False):
self.node_id = node_id
self.node_type, self.node_number = node_id.split("_")
self.types = types
self.duckietown_graph = duckietown_graph
self.time_stamps_to_indices = dict()
self.last_time_stamp = None
self.node_lock = g2oGB.ControlableLock()
self.movable = movable
if (self.node_type not in self.types):
print("Registering node of type %s unkown in types = " %
self.node_type + str(self.types))
def __init__(self,
initial_duckiebot_dict={},
initial_watchtower_dict={},
initial_april_dict={},
initial_floor_april_tags="",
retro_interpolate=True,
using_priors=True,
stocking_time=None,
priors_filename=None,
default_variance=None,
result_folder="/tmp"):
# Initialize pose graph.
self.graph = g2oGB.g2oGraphBuilder()
# Define node types.
self.types = ["duckiebot", "watchtower", "apriltag"]
# Define movable node types.
self.movable = ["duckiebot"]
# Initialize chi2
self.chi2 = 0.0
# Set retro-interpolate mode as inputted.
self.retro_interpolate = retro_interpolate
# Load the initial floor April tags if given an input file name
self.lock = g2oGB.ControlableLock()
self.node_dict = dict()
self.result_folder = result_folder
self.stocking_time = stocking_time
self.last_cleaning = 0.0
self.using_priors = using_priors
self.default_variance = default_variance
if(priors_filename is not None):
self.priors = PriorHandler(priors_filename)
if (initial_floor_april_tags != ""):
self.load_initial_floor_april_tags(initial_floor_april_tags)
print("init done")
def __init__(self,
initial_duckiebot_dict={},
initial_watchtower_dict={},
initial_april_dict={},
initial_floor_april_tags="",
retro_interpolate=True,
stocking_time=None):
# Initialize pose graph.
self.graph = g2oGB.g2oGraphBuilder()
# Define node types.
self.types = ["duckiebot", "watchtower", "apriltag"]
# Initialize first-level dictionary of timestamp_local_indices by
# associating each node type with the respective input dictionary of
# IDs.
initial_dicts = [
initial_duckiebot_dict, initial_watchtower_dict, initial_april_dict
]
self.timestamp_local_indices = dict(zip(self.types, initial_dicts))
# Define movable node types.
self.movable = ["duckiebot"]
# Initialize first-level dictionary of num_local_indices_assigned.
self.num_local_indices_assigned = dict()
# Initialize first-level dictionary of last_time_stamp.
self.last_time_stamp = dict()
# Initialize first-level dictionary of first odometry time stamps.
self.first_odometry_time_stamp = dict()
# Initialize first-level dictionary of last odometry time stamps.
self.last_odometry_time_stamp = dict()
# Initialize chi2
self.chi2 = 0.0
# Set retro-interpolate mode as inputted.
self.retro_interpolate = retro_interpolate
# Load the initial floor April tags if given an input file name
self.lock = g2oGB.ControlableLock()
if (initial_floor_april_tags != ""):
self.load_initial_floor_april_tags(initial_floor_april_tags)
self.stocking_time = stocking_time
self.last_cleaning = 0.0