def __init__(self, name):
self.isDebug = False
rospy.loginfo("Starting %s ..." % name)
self.client = ROSClient()
qmc = QTCModelCreation()
obs = qmc.create_observation_model(qtc_type=qmc.qtc_types.qtch, start_end=True)
self.lookup = qmc.create_states(qtc_type=qmc.qtc_types.qtch, start_end=True)
hmc = HMMModelCreation()
m = PfModel()
for f in os.listdir(rospy.get_param("~model_dir")):
filename = rospy.get_param("~model_dir") + '/' + f
if f.endswith(".hmm"):
rospy.loginfo("Creating prediction model from: %s", filename)
pred = hmc.create_prediction_model(input=filename)
m.add_model(f.split('.')[0], pred, obs)
elif f.endswith(".rules"):
with open(filename) as fd:
rospy.loginfo("Reading rules from: %s", filename)
# Necessary due to old rule format:
self.rules[f.split('.')[0]] = self._create_proper_qtc_keys(json.load(fd))
self.model = m.get()
visualisation = rospy.get_param("~visualisation")
self.visualisation_colors = visualisation['models']
self.default_color = ColorRGBA(
a=1.0,
r=visualisation['default']['r']/255.,
g=visualisation['default']['g']/255.,
b=visualisation['default']['b']/255.
)
self.pub = rospy.Publisher("~prediction_array", QTCPredictionArray, queue_size=10)
self.markpub = rospy.Publisher("~marker_array", MarkerArray, queue_size=10)
rospy.Subscriber(rospy.get_param("~qtc_topic", "/online_qtc_creator/qtc_array"), QTCArray, self.callback, queue_size=1)
rospy.Subscriber(rospy.get_param("~ppl_topic", "/people_tracker/positions"), PeopleTracker, self.people_callback, queue_size=1)
rospy.loginfo("... all done")
def _create_model(self, obs, pred):
m = PfModel()
for f in pred:
name = f.split('/')[-1].split('.')[0]
with open(f, 'r') as a:
m.add_prediction_matrix(name, np.loadtxt(a))
for f in obs:
name = f.split('/')[-1].split('.')[0]
with open(f, 'r') as a:
m.add_observation_matrix(name, np.loadtxt(a))
return m.get()
def load_files(path):
m = PfModel()
for f in os.listdir(path):
if f.endswith(".pred"):
name = f.split('.')[0]
filename = path + '/' + f
with open(filename, 'r') as a:
m.add_prediction_matrix(name, np.loadtxt(a))
elif f.endswith(".obs"):
name = f.split('.')[0]
filename = path + '/' + f
with open(filename, 'r') as a:
m.add_observation_matrix(name, np.loadtxt(a))
return m.get()
def _create_model(self, obs, pred):
m = PfModel()
for f in pred:
name = f.split('/')[-1].split('.')[0]
with open(f, 'r') as a:
m.add_prediction_matrix(name, np.loadtxt(a))
for f in obs:
name = f.split('/')[-1].split('.')[0]
with open(f, 'r') as a:
m.add_observation_matrix(name, np.loadtxt(a))
return m.get()
def load_files(path):
m = PfModel()
for f in os.listdir(path):
if f.endswith(".pred"):
name = f.split('.')[0]
filename = path + '/' + f
with open(filename, 'r') as a:
m.add_prediction_matrix(name, np.loadtxt(a))
elif f.endswith(".obs"):
name = f.split('.')[0]
filename = path + '/' + f
with open(filename, 'r') as a:
m.add_observation_matrix(name, np.loadtxt(a))
return m.get()
