class TestOnlineQTC(unittest.TestCase):
RESULTS_FILE = find_resource(PKG, 'correct.txt')[0]
BAG_FILE = find_resource(PKG, 'test.bag')[0]
def __init__(self, *args):
super(TestOnlineQTC, self).__init__(*args)
rospy.init_node(NAME)
self.correct = self._load_file()
#print self.correct
self.test = []
self.seq = 0
def _load_file(self):
return json.load(file(self.RESULTS_FILE))
def _callback(self, msg):
self.seq = msg.header.seq
if msg.qtc:
self.test.append(json.loads(msg.qtc[0].qtc_serialised))
def test_online_creator(self):
rospy.Subscriber("/online_qtc_creator/qtc_array",
QTCArray,
callback=self._callback)
with open(os.devnull, 'w') as FNULL:
p = subprocess.Popen("rosbag play -r 2 " + self.BAG_FILE,
stdin=subprocess.PIPE,
shell=True,
stdout=FNULL)
while p.poll() == None:
rospy.sleep(1)
#print self.test
self.assertEqual(self.test, self.correct)
def load_input_data1():
world = World_Trace()
ob = []
from roslib.packages import find_resource
TEST_INPUT_DATA = find_resource(PKG, "data1.csv")[0]
with open(TEST_INPUT_DATA) as f:
reader = csv.DictReader(f)
for idx, row in enumerate(reader):
ob.append(
Object_State(
name=row["o1"],
timestamp=idx + 1,
x=float(row["x1"]),
y=float(row["y1"]),
xsize=float(row["w1"]),
ysize=float(row["l1"]),
)
)
ob.append(
Object_State(
name=row["o2"],
timestamp=idx + 1,
x=float(row["x2"]),
y=float(row["y2"]),
xsize=float(row["w2"]),
ysize=float(row["l2"]),
)
)
world.add_object_state_series(ob)
return world
def load_input_data4():
world = World_Trace()
ob = []
from roslib.packages import find_resource
TEST_INPUT_DATA = find_resource(PKG, 'data4.csv')[0]
with open(TEST_INPUT_DATA) as f:
reader = csv.DictReader(f)
for idx, row in enumerate(reader):
ob.append(
Object_State(name=row['o1'],
timestamp=idx + 1,
x=float(row['x1']),
y=float(row['y1']),
xsize=float(row['w1']),
ysize=float(row['l1'])))
ob.append(
Object_State(name=row['o2'],
timestamp=idx + 1,
x=float(row['x2']),
y=float(row['y2']),
xsize=float(row["w2"]),
ysize=float(row["l2"])))
ob.append(
Object_State(name=row['o3'],
timestamp=idx + 1,
x=float(row['x3']),
y=float(row['y3']),
xsize=float(row["w3"]),
ysize=float(row["l3"])))
world.add_object_state_series(ob)
return world
def load_scenario(self, req):
conf_file = find_resource("test_cases", req.scenario+".yaml")[0]
rospy.loginfo("Reading config file: %s ..." % conf_file)
with open(conf_file,'r') as f:
conf = yaml.load(f)
rospy.loginfo(" ... done")
self._robot_start_pose = conf["robot_start_pose"]["position"]
self._robot_tx, self._robot_ty = self._translate(
self._robot_start_pose["x"],
self._robot_start_pose["y"]
)
self._robot_start_rot = conf["robot_start_pose"]["orientation"]
self._goal = MoveBaseGoal()
self._goal.target_pose.header.frame_id = "map"
self._goal.target_pose.pose.position.x = conf["robot_end_pose"]["position"]["x"]
self._goal.target_pose.pose.position.y = conf["robot_end_pose"]["position"]["y"]
self._goal.target_pose.pose.orientation.w = conf["robot_end_pose"]["orientation"]["w"]
self._goal.target_pose.pose.orientation.z = conf["robot_end_pose"]["orientation"]["z"]
self._human_start_pose = conf["human_start_pose"]["position"]
self._human_tx, self._human_ty = self._translate(
self._human_start_pose["x"],
self._human_start_pose["y"]
)
self._human_start_rot = conf["human_start_pose"]["orientation"]
self._timeout = conf["success_metrics"]["nav_timeout"]
self._min_distance = conf["success_metrics"]["human_distance"]
self._loaded = True
self.reset(None)
return LoadResponse()
def load_input_data4():
world = World_Trace()
ob = []
from roslib.packages import find_resource
TEST_INPUT_DATA = find_resource(PKG, 'data4.csv')[0]
with open(TEST_INPUT_DATA) as f:
reader = csv.DictReader(f)
for idx, row in enumerate(reader):
ob.append(Object_State(
name=row['o1'],
timestamp=idx+1,
x=float(row['x1']),
y=float(row['y1']),
xsize=float(row['w1']),
ysize=float(row['l1'])
))
ob.append(Object_State(
name=row['o2'],
timestamp=idx+1,
x=float(row['x2']),
y=float(row['y2']),
xsize=float(row["w2"]),
ysize=float(row["l2"])
))
ob.append(Object_State(
name=row['o3'],
timestamp=idx+1,
x=float(row['x3']),
y=float(row['y3']),
xsize=float(row["w3"]),
ysize=float(row["l3"])
))
world.add_object_state_series(ob)
return world
def parse(fn):
try:
lines = subprocess.check_output("grep include {0}".format(fn), shell=True)
except:
return
for l in lines.split('\n'):
v = yaml.load(l)
if (v is None):
continue
res = []
pkg = v[0]['include']['pkg']
src = v[0]['include']['file']
try:
res = find_resource(pkg,src)
except:
base_dir = os.path.dirname(fn)
for root, dirs, files in os.walk(base_dir):
if src in files:
res.append(os.path.join(root,src))
if (len(res) == 0):
exit(-1)
path = res[0]
print path
parse(path)
def get_nodes(package, launch_file):
"""Function to get status of all nodes inside a launch file.
Parameters
----------
package : string
This parameter specifies package name of launch file.
launch_file : string
This parameter specifies the name of launch file.
Returns
-------
Launcher_nodes_status : list
The launcher nodes are returned followed by _1 or _0 highlighting
whether they are active or inactive.
"""
try:
launch_file_paths = find_resource(package, launch_file)
except ResourceNotFound as e:
return ['Package not found']
if len(launch_file_paths) == 0:
return ['Launch file not found']
Launcher_node_details = get_launcher_node_details(launch_file_paths)
Launcher_nodes = list(Launcher_node_details.keys())
Running_nodes = get_running_nodes()
Launcher_nodes_status = get_launcher_nodes_status(Launcher_nodes, Running_nodes)
return Launcher_nodes_status
def getPath(self):
from roslib.packages import find_resource
loc = find_resource(self.pkg, self.fname)
if len(loc) < 1:
print "Error: {0} cannot be found in {1}.".format(
self.fname, self.pkg)
exit(-1)
if len(loc) > 1:
print "Warning: more than one {0} exists in {1}.".format(
self.fname, self.pkg)
return loc[0]
def parse(fn):
try:
lines = subprocess.check_output("grep include {0}".format(fn), shell=True)
except:
return
for l in lines.split('\n'):
v = yaml.load(l)
if (v is None):
continue
pkg = v[0]['include']['pkg']
src = v[0]['include']['file']
path = find_resource(pkg, src)[0]
print path
parse(path)
class TestOfflineQTC(unittest.TestCase):
TEST_DIR = rospkg.RosPack().get_path(PKG)+'/tests/data/'
EXECUTABLE = find_resource(PKG, 'offline_qtc_creator.py')[0]
_correct = {
"qtcbs": [[-1, -1], [0, -1], [0, 0], [1, 0], [1, 1]],
"qtccs": [[-1, -1, 0, 0], [-1, -1, 0, 1], [0, -1, 0, 1], [0, -1, 1, 1], [0, 0, 1, 1], [0, 0, 0, 1], [1, 0, 0, 1], [1, 1, 0, 1], [1, 1, 0, 0]],
"qtcbcs": [[-1.0, -1.0, np.NaN, np.NaN], [0.0, -1.0, np.NaN, np.NaN], [0.0, -1.0, 0.0, 1.0], [0.0, -1.0, 1.0, 1.0], [0.0, 0.0, 1.0, 1.0], [0.0, 0.0, 0.0, 1.0], [1.0, 0.0, 0.0, 1.0], [1.0, 1.0, 0.0, 1.0], [1.0, 1.0, np.NaN, np.NaN]]
}
def __init__(self, *args):
super(TestOfflineQTC, self).__init__(*args)
rospy.init_node(NAME)
self.test = []
def _callback(self, msg):
self.seq = msg.header.seq
if msg.qtc:
self.test=json.loads(msg.qtc[0].qtc_serialised)
def test_offline_creator_qtcb(self):
rospy.Subscriber("/offline_qtc_creator/qtc_array", QTCArray, callback=self._callback)
p = subprocess.Popen(self.EXECUTABLE+" _input:="+self.TEST_DIR+" _qsr:=qtcbs", stdin=subprocess.PIPE, shell=True)
while p.poll() == None:
rospy.sleep(1)
# print self.test
self.assertEqual(self.test, self._correct["qtcbs"])
def test_offline_creator_qtcc(self):
rospy.Subscriber("/offline_qtc_creator/qtc_array", QTCArray, callback=self._callback)
p = subprocess.Popen(self.EXECUTABLE+" _input:="+self.TEST_DIR+" _qsr:=qtccs", stdin=subprocess.PIPE, shell=True)
while p.poll() == None:
rospy.sleep(1)
# print self.test
self.assertEqual(self.test, self._correct["qtccs"])
def test_offline_creator_qtcbc(self):
rospy.Subscriber("/offline_qtc_creator/qtc_array", QTCArray, callback=self._callback)
p = subprocess.Popen(self.EXECUTABLE+" _input:="+self.TEST_DIR+" _qsr:=qtcbcs", stdin=subprocess.PIPE, shell=True)
while p.poll() == None:
rospy.sleep(1)
# print self.test
np.testing.assert_equal(self.test, self._correct["qtcbcs"])
def getPath(self, structure):
from roslib.packages import find_resource
loc = []
try:
loc = find_resource(self.pkg, self.fname)
except:
# rospack failed, look in source:
for root, dirs, files in os.walk(structure.getSourcePath()):
if self.fname in files:
loc.append(os.path.join(root, self.fname))
if len(loc) < 1:
print "Error: {0} cannot be found in {1}.".format(
self.fname, self.pkg)
exit(-1)
if len(loc) > 1:
print "Warning: more than one {0} exists in {1}.".format(
self.fname, self.pkg)
return loc[0]
def __init__(self, name):
rospy.loginfo("Starting scenario server")
self.robot = rospy.get_param("~robot", False)
conf_file = find_resource("topological_navigation", "scenario_server.yaml")[0]
rospy.loginfo("Reading config file: %s ..." % conf_file)
with open(conf_file,'r') as f:
conf = yaml.load(f)
self._robot_start_node = conf["robot_start_node"]
self._robot_goal_node = conf["robot_goal_node"]
self._obstacle_node_prefix = conf["obstacle_node_prefix"]
self._obstacle_types = conf["obstacle_types"]
self._timeout = conf["success_metrics"]["nav_timeout"]
rospy.loginfo(" ... done")
self._insert_maps()
self.tf = TransformListener()
rospy.Service("~load", LoadTopoNavTestScenario, self.load_scenario)
self.reset_pose = self.reset_pose_robot if self.robot else self.reset_pose_sim
rospy.Service("~reset", Empty, self.reset)
rospy.Service("~start", RunTopoNavTestScenario, self.start)
rospy.loginfo("All done")
def __init__(self, name):
rospy.loginfo("Starting scenario server")
self.robot = rospy.get_param("~robot", False)
conf_file = find_resource("topological_navigation", "scenario_server.yaml")[0]
rospy.loginfo("Reading config file: %s ..." % conf_file)
with open(conf_file,'r') as f:
conf = yaml.load(f)
self._robot_start_node = conf["robot_start_node"]
self._robot_goal_node = conf["robot_goal_node"]
self._obstacle_node_prefix = conf["obstacle_node_prefix"]
self._obstacle_types = conf["obstacle_types"]
self._timeout = conf["success_metrics"]["nav_timeout"]
rospy.loginfo(" ... done")
self._insert_maps()
self.tf = TransformListener()
rospy.Service("~load", LoadTopoNavTestScenario, self.load_scenario)
self.reset_pose = self.reset_pose_robot if self.robot else self.reset_pose_sim
rospy.Service("~reset", Empty, self.reset)
rospy.Service("~start", RunTopoNavTestScenario, self.start)
rospy.loginfo("All done")
class TestPf(unittest.TestCase):
CROSSING_OBS = find_resource(PKG, 'crossing.obs')[0]
CROSSING_PRED = find_resource(PKG, 'crossing.pred')[0]
PASSBY_OBS = find_resource(PKG, 'passby.obs')[0]
PASSBY_PRED = find_resource(PKG, 'passby.pred')[0]
STATES = find_resource(PKG, 'qtch.states')[0]
filters = map(str, range(5))
def __init__(self, *args):
super(self.__class__, self).__init__(*args)
rospy.init_node(NAME)
self.r = ROSClient()
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_lookup(self, filename):
with open(filename, 'r') as f:
return json.load(f)
def _create_pf(self,
pred,
obs,
states,
num_particles=1000,
uuid="",
starvation_factor=0.1,
ensure_particle_per_state=True,
debug=True):
models = self._create_model(pred, obs)
states = self._load_lookup(states)
d = self.r.call_service(
PfRepRequestCreate(
num_particles=num_particles,
models=models,
state_lookup_table=states,
starvation_factor=starvation_factor,
ensure_particle_per_state=ensure_particle_per_state,
debug=debug,
uuid=uuid if uuid != "" else None))
return d
def _predict(self, uuid, num_steps=5, debug=True):
p = self.r.call_service(
PfRepRequestPredict(uuid=uuid, num_steps=num_steps, debug=debug))
return p
def _update(self, uuid, obs, debug=True):
p = self.r.call_service(
PfRepRequestUpdate(uuid=uuid, observation=obs, debug=debug))
return p
def _list(self):
p = self.r.call_service(PfRepRequestList())
return p
def _remove(self, uuid):
p = self.r.call_service(PfRepRequestRemove(uuid=uuid))
return p
def test1_pf_create(self):
res = []
for uuid in self.filters:
res.append(
self._create_pf(pred=[self.CROSSING_PRED, self.PASSBY_PRED],
obs=[self.CROSSING_OBS, self.PASSBY_OBS],
states=self.STATES,
uuid=uuid))
self.assertEqual(map(str, res), self.filters)
def test2_pf_predict(self):
res = []
for uuid in self.filters:
res.append(self._predict(uuid=uuid, num_steps=1)[0])
self.assertEqual(len(res), len(self.filters))
def test3_pf_update(self):
res = []
for uuid in self.filters:
res.append(self._update(uuid=uuid, obs='-,-,-'))
self.assertEqual(len(res), len(self.filters))
def test4_pf_list(self):
res = self._list()
self.assertEqual(len(res), len(self.filters))
def test5_pf_remove(self):
for uuid in self.filters:
self._remove(uuid)
self.assertFalse(len(self._list()))
print 'Environment variable %s not set' % env_variable
elif keyword == 'find':
package_name = m.split(' ')[1]
try:
package_path = get_pkg_dir(package_name)
substitution = package_path
except InvalidROSPkgException:
print 'Could not find pacakage %s' % package_name
string = string.replace(full_match, substitution)
match = re.search(regex, string)
return {'resolved':string, 'variable':original}
def __str__(self, prefix = ''):
lines = []
lines.append("{}{}: {}".format(prefix, self.type, zip(self.attributes.keys(), self.attributes.values())))
for c in self.children:
lines.append(c.__str__(prefix=prefix+'\t'))
return '\n'.join(lines)
if __name__ == '__main__':
package = 'pr2_moveit_config'
launch_file = 'demo.launch'
launch_file_path = find_resource(package, launch_file)[0]
launch_root = ET.parse(launch_file_path)
r = launch_root.getroot()
m = RoslaunchElement(RoslaunchElement.load_xml(launch_file_path), launch_file_path)
print m
#print m.resolve('$(arg asd)')
class TestQTC(unittest.TestCase):
TEST_FILE = find_resource(PKG, 'qtc.csv')[0]
__duplicate = "duplicate"
__human_robot = ("human", "robot")
__robot_human = ("robot", "human")
__human_duplicate = ("human", __duplicate)
dynamic_args = {
"qtcs": {
"quantisation_factor": 0.01,
"validate": True,
"no_collapse": False,
"distance_threshold": 1.2
},
"for_all_qsrs": {
"qsrs_for": [__human_robot]
}
}
correct = {
"qtcbs": [{'qtcbs': '-,-'}, {'qtcbs': '-,0'}, {'qtcbs': '0,0'}, {'qtcbs': '0,+'}, {'qtcbs': '+,+'}],
"qtcbs_nocollapse": lambda: json.load(open(find_resource(PKG, 'qtcbs_nocollapse.txt')[0],'r')),
"qtccs": [{'qtccs': '-,-,0,0'}, {'qtccs': '-,-,+,0'}, {'qtccs': '-,0,+,0'}, {'qtccs': '-,0,+,+'}, {'qtccs': '0,0,+,+'}, {'qtccs': '0,0,+,0'}, {'qtccs': '0,+,+,0'}, {'qtccs': '+,+,+,0'}, {'qtccs': '+,+,0,0'}],
"qtccs_nocollapse": lambda: json.load(open(find_resource(PKG, 'qtccs_nocollapse.txt')[0],'r')),
"qtcbcs": [{'qtcbcs': '-,-'}, {'qtcbcs': '-,0'}, {'qtcbcs': '-,0,+,0'}, {'qtcbcs': '-,0,+,+'}, {'qtcbcs': '0,0,+,+'}, {'qtcbcs': '0,0,+,0'}, {'qtcbcs': '0,+,+,0'}, {'qtcbcs': '+,+,+,0'}, {'qtcbcs': '+,+'}],
"qtcbcs_nocollapse": lambda: json.load(open(find_resource(PKG, 'qtcbcs_nocollapse.txt')[0],'r')),
}
def __init__(self, *args):
super(TestQTC, self).__init__(*args)
rospy.init_node(NAME)
self.world = World_Trace()
self.world_duplicate = World_Trace()
self._load_file()
def _inverse_correct(self, correct):
ret = []
for e in correct:
k,v = e.items()[0]
try:
ret.append({k: ','.join(np.array(v.split(','))[[1,0,3,2]])})
except IndexError:
ret.append({k: ','.join(np.array(v.split(','))[[1,0]])})
return ret
def _load_file(self):
ob = []
with open(self.TEST_FILE) as csvfile:
reader = csv.DictReader(csvfile)
for idx,row in enumerate(reader):
ob.append(Object_State(
name=row['agent1'],
timestamp=idx+1,
x=float(row['x1']),
y=float(row['y1'])
))
ob.append(Object_State(
name=row['agent2'],
timestamp=idx+1,
x=float(row['x2']),
y=float(row['y2'])
))
ob.append(Object_State(
name=self.__duplicate,
timestamp=idx+1,
x=float(row['x2']),
y=float(row['y2'])
))
self.world.add_object_state_series(ob)
def _request(self, msg):
cln = QSRlib_ROS_Client()
req = cln.make_ros_request_message(msg)
res = cln.request_qsrs(req)
out = pickle.loads(res.data)
foo = {}
for t in out.qsrs.get_sorted_timestamps():
for k, v in zip(out.qsrs.trace[t].qsrs.keys(), out.qsrs.trace[t].qsrs.values()):
try:
foo[k].append(v.qsr)
except KeyError:
foo[k] = [v.qsr]
return foo
def _create_qsr(self, qsr, dynamic_args=None):
dynamic_args = self.dynamic_args if not dynamic_args else dynamic_args
qsrlib_request_message = QSRlib_Request_Message(
which_qsr=qsr,
input_data=self.world,
dynamic_args=dynamic_args
)
return self._request(qsrlib_request_message)
def _to_strings(self, array):
return [x.values()[0] for x in array]
def test_qtcb(self, between=__human_robot, dynamic_args=dynamic_args):
res = self._create_qsr("qtcbs", dynamic_args=dynamic_args)
self.assertEqual(res[','.join(between)], self.correct["qtcbs"])
def test_qtcb_multiple_objects(self):
dynamic_args = deepcopy(self.dynamic_args)
dynamic_args["for_all_qsrs"] = {}
self.test_qtcb(self.__human_duplicate, dynamic_args=dynamic_args)
def test_qtcb_inverse(self):
dynamic_args = deepcopy(self.dynamic_args)
dynamic_args["for_all_qsrs"] = {}
res = self._create_qsr("qtcbs", dynamic_args=dynamic_args)
self.assertEqual(res[','.join(self.__robot_human)], self._inverse_correct(self.correct["qtcbs"]))
def test_qtcb_nocollapse(self):
dynamic_args = deepcopy(self.dynamic_args)
dynamic_args["qtcs"]["validate"] = False
dynamic_args["qtcs"]["no_collapse"] = True
res = self._create_qsr("qtcbs", dynamic_args=dynamic_args)
self.assertEqual(res[','.join(self.__human_robot)], self.correct["qtcbs_nocollapse"]())
def test_qtcb_nocollapse_inverse(self):
dynamic_args = deepcopy(self.dynamic_args)
dynamic_args["qtcs"]["validate"] = False
dynamic_args["qtcs"]["no_collapse"] = True
dynamic_args["for_all_qsrs"] = {}
res = self._create_qsr("qtcbs", dynamic_args=dynamic_args)
self.assertEqual(res[','.join(self.__robot_human)], self._inverse_correct(self.correct["qtcbs_nocollapse"]()))
def test_qtcc(self, between=__human_robot, dynamic_args=dynamic_args):
res = self._create_qsr("qtccs", dynamic_args=dynamic_args)
self.assertEqual(res[','.join(self.__human_robot)], self.correct["qtccs"])
def test_qtcc_multiple_objects(self):
dynamic_args = deepcopy(self.dynamic_args)
dynamic_args["for_all_qsrs"] = {}
self.test_qtcc(self.__human_duplicate, dynamic_args=dynamic_args)
def test_qtcc_inverse(self):
dynamic_args = deepcopy(self.dynamic_args)
dynamic_args["for_all_qsrs"] = {}
res = self._create_qsr("qtccs", dynamic_args=dynamic_args)
self.assertEqual(res[','.join(self.__robot_human)], self._inverse_correct(self.correct["qtccs"]))
def test_qtcc_nocollapse(self):
dynamic_args = deepcopy(self.dynamic_args)
dynamic_args["qtcs"]["validate"] = False
dynamic_args["qtcs"]["no_collapse"] = True
res = self._create_qsr("qtccs", dynamic_args=dynamic_args)
self.assertEqual(res[','.join(self.__human_robot)], self.correct["qtccs_nocollapse"]())
def test_qtcc_nocollapse_inverse(self):
dynamic_args = deepcopy(self.dynamic_args)
dynamic_args["qtcs"]["validate"] = False
dynamic_args["qtcs"]["no_collapse"] = True
dynamic_args["for_all_qsrs"] = {}
res = self._create_qsr("qtccs", dynamic_args=dynamic_args)
self.assertEqual(res[','.join(self.__robot_human)], self._inverse_correct(self.correct["qtccs_nocollapse"]()))
def test_qtcbc(self, between=__human_robot, dynamic_args=dynamic_args):
res = self._create_qsr("qtcbcs", dynamic_args=dynamic_args)
self.assertEqual(res[','.join(self.__human_robot)], self.correct["qtcbcs"])
def test_qtcbc_multiple_objects(self):
dynamic_args = deepcopy(self.dynamic_args)
dynamic_args["for_all_qsrs"] = {}
self.test_qtcbc(self.__human_duplicate, dynamic_args=dynamic_args)
def test_qtcbc_inverse(self):
dynamic_args = deepcopy(self.dynamic_args)
dynamic_args["for_all_qsrs"] = {}
res = self._create_qsr("qtcbcs", dynamic_args=dynamic_args)
self.assertEqual(res[','.join(self.__robot_human)], self._inverse_correct(self.correct["qtcbcs"]))
def test_qtcbc_nocollapse(self):
dynamic_args = deepcopy(self.dynamic_args)
dynamic_args["qtcs"]["validate"] = False
dynamic_args["qtcs"]["no_collapse"] = True
res = self._create_qsr("qtcbcs", dynamic_args=dynamic_args)
self.assertEqual(res[','.join(self.__human_robot)], self.correct["qtcbcs_nocollapse"]())
def test_qtcbc_nocollapse_inverse(self):
dynamic_args = deepcopy(self.dynamic_args)
dynamic_args["qtcs"]["validate"] = False
dynamic_args["qtcs"]["no_collapse"] = True
dynamic_args["for_all_qsrs"] = {}
res = self._create_qsr("qtcbcs", dynamic_args=dynamic_args)
self.assertEqual(res[','.join(self.__robot_human)], self._inverse_correct(self.correct["qtcbcs_nocollapse"]()))
class TestHMM(unittest.TestCase):
QTCB_SAMPLE_TEST_HMM = find_resource(PKG, 'qtcb_sample_test.hmm')[0]
QTCC_SAMPLE_TEST_HMM = find_resource(PKG, 'qtcc_sample_test.hmm')[0]
QTCBC_SAMPLE_TEST_HMM = find_resource(PKG, 'qtcbc_sample_test.hmm')[0]
QTCB_PASSBY_LEFT_HMM = find_resource(PKG, 'qtcb_passby_left.hmm')[0]
QTCC_PASSBY_LEFT_HMM = find_resource(PKG, 'qtcc_passby_left.hmm')[0]
QTCBC_PASSBY_LEFT_HMM = find_resource(PKG, 'qtcbc_passby_left.hmm')[0]
RCC3_TEST_HMM = find_resource(PKG, 'rcc3_test.hmm')[0]
QTCB_QSR = find_resource(PKG, 'qtcb.qsr')[0]
QTCC_QSR = find_resource(PKG, 'qtcc.qsr')[0]
QTCBC_QSR = find_resource(PKG, 'qtcbc.qsr')[0]
RCC3_QSR = find_resource(PKG, 'rcc3.qsr')[0]
correct_samples = {
"qtcb": [[u'--', u'0-', u'+-', u'+0', u'++']],
"qtcc": [[
u'--+-', u'--0-', u'----', u'0---', u'+---', u'+0--', u'++--',
u'---'
]],
"qtcbc": [[u'--', u'----', u'0---', u'+---', u'+0--', u'++--', u'++']]
}
correct_hashsum = {
"qtcb": "3fb65b50d0f7631a300132e8bca9ca13",
"qtcc": "dbf1529cb0b0c90aaebbe7eafe0e9b05",
"qtcbc": "0a3acf7b48c4c1155931442c86317ce4",
"rcc3": "6d5bcc6c44d9b1120c738efa1994a40a"
}
correct_loglikelihoods = {
"qtcb": -2.16887,
"qtcc": -6.07188,
"qtcbc": -2.23475,
"rcc3": -4.15914
}
def __init__(self, *args):
super(TestHMM, self).__init__(*args)
rospy.init_node(NAME)
self.r = ROSClient()
def _create_hmm(self, qsr_file, qsr_type):
with open(qsr_file, 'r') as f:
qsr_seq = json.load(f)
_, d = self.r.call_service(
HMMRepRequestCreate(qsr_seq=qsr_seq, qsr_type=qsr_type))
return d
def _create_sample(self, hmm_file, qsr_type):
with open(hmm_file, 'r') as f:
hmm = f.read()
_, s = self.r.call_service(
HMMRepRequestSample(qsr_type=qsr_type,
xml=hmm,
max_length=10,
num_samples=1))
return s
def _calculate_loglikelihood(self, hmm_file, qsr_file, qsr_type):
with open(qsr_file, 'r') as f:
qsr_seq = json.load(f)
with open(hmm_file, 'r') as f:
hmm = f.read()
q, l = self.r.call_service(
HMMRepRequestLogLikelihood(qsr_type=qsr_type,
xml=hmm,
qsr_seq=qsr_seq))
return round(l, 5)
def _to_strings(self, array):
return [x.values()[0] for x in array]
def test_qtcb_create(self):
res = self._create_hmm(self.QTCB_QSR, 'qtcb')
self.assertEqual(
hashlib.md5(res).hexdigest(), self.correct_hashsum["qtcb"])
def test_qtcb_sample(self):
res = self._create_sample(self.QTCB_SAMPLE_TEST_HMM, 'qtcb')
self.assertEqual(res, self.correct_samples["qtcb"])
def test_qtcb_loglikelihood(self):
res = self._calculate_loglikelihood(self.QTCB_PASSBY_LEFT_HMM,
self.QTCB_QSR, 'qtcb')
self.assertEqual(res, self.correct_loglikelihoods["qtcb"])
def test_qtcc_create(self):
res = self._create_hmm(self.QTCC_QSR, 'qtcc')
self.assertEqual(
hashlib.md5(res).hexdigest(), self.correct_hashsum["qtcc"])
def test_qtcc_sample(self):
res = self._create_sample(self.QTCC_SAMPLE_TEST_HMM, 'qtcc')
self.assertEqual(res, self.correct_samples["qtcc"])
def test_qtcc_loglikelihood(self):
res = self._calculate_loglikelihood(self.QTCC_PASSBY_LEFT_HMM,
self.QTCC_QSR, 'qtcc')
self.assertEqual(res, self.correct_loglikelihoods["qtcc"])
def test_qtcbc_create(self):
res = self._create_hmm(self.QTCBC_QSR, 'qtcbc')
self.assertEqual(
hashlib.md5(res).hexdigest(), self.correct_hashsum["qtcbc"])
def test_qtcbc_sample(self):
res = self._create_sample(self.QTCBC_SAMPLE_TEST_HMM, 'qtcbc')
self.assertEqual(res, self.correct_samples["qtcbc"])
def test_qtcbc_loglikelihood(self):
res = self._calculate_loglikelihood(self.QTCBC_PASSBY_LEFT_HMM,
self.QTCBC_QSR, 'qtcbc')
self.assertEqual(res, self.correct_loglikelihoods["qtcbc"])
def test_rcc3_create(self):
res = self._create_hmm(self.RCC3_QSR, 'rcc3')
self.assertEqual(
hashlib.md5(res).hexdigest(), self.correct_hashsum["rcc3"])
def test_rcc3_sample(self):
res = self._create_sample(self.RCC3_TEST_HMM, 'rcc3')
self.assertTrue(type(res) == list and len(res) > 0)
def test_rcc3_loglikelihood(self):
res = self._calculate_loglikelihood(self.RCC3_TEST_HMM, self.RCC3_QSR,
'rcc3')
self.assertEqual(res, self.correct_loglikelihoods["rcc3"])
class TestHMM(unittest.TestCase):
QTCB_SAMPLE_TEST_HMM = find_resource(PKG, 'qtcb_sample_test.hmm')[0]
QTCC_SAMPLE_TEST_HMM = find_resource(PKG, 'qtcc_sample_test.hmm')[0]
QTCBC_SAMPLE_TEST_HMM = find_resource(PKG, 'qtcbc_sample_test.hmm')[0]
QTCB_PASSBY_LEFT_HMM = find_resource(PKG, 'qtcb_passby_left.hmm')[0]
QTCC_PASSBY_LEFT_HMM = find_resource(PKG, 'qtcc_passby_left.hmm')[0]
QTCBC_PASSBY_LEFT_HMM = find_resource(PKG, 'qtcbc_passby_left.hmm')[0]
RCC3_TEST_HMM = find_resource(PKG, 'rcc3_test.hmm')[0]
QTCB_QSR = find_resource(PKG, 'qtcb.qsr')[0]
QTCC_QSR = find_resource(PKG, 'qtcc.qsr')[0]
QTCBC_QSR = find_resource(PKG, 'qtcbc.qsr')[0]
RCC3_QSR = find_resource(PKG, 'rcc3.qsr')[0]
correct_samples = {
"qtcb": [[u'--', u'0-', u'+-', u'+0', u'++']],
"qtcc": [[
u'--+-', u'--0-', u'----', u'0---', u'+---', u'+0--', u'++--',
u'---'
]],
"qtcbc": [[u'--', u'----', u'0---', u'+---', u'+0--', u'++--', u'++']]
}
correct_hashsum = {
"qtcb": "de1ea75d1b0d6c9ff8249a24583fedb9",
"qtcc": "5d74fb27e53ba00f84014d3be70e2740",
"qtcbc": "f73e2c85f1447a5109d6ab3d5201fb76",
"rcc3": "402c53a1cc004a5f518d0b607eb7ac38"
}
correct_loglikelihoods = {
"qtcb": -2.16887,
"qtcc": -6.07188,
"qtcbc": -2.23475,
"rcc3": -4.15914
}
def __init__(self, *args):
super(TestHMM, self).__init__(*args)
rospy.init_node(NAME)
self.r = ROSClient()
def _create_hmm(self, qsr_file, qsr_type, start_at_zero=True):
with open(qsr_file, 'r') as f:
qsr_seq = json.load(f)
d = self.r.call_service(
HMMRepRequestCreate(qsr_seq=qsr_seq,
qsr_type=qsr_type,
start_at_zero=start_at_zero))
return d
def _create_sample(self, hmm_file, qsr_type):
with open(hmm_file, 'r') as f:
hmm = json.load(f)
s = self.r.call_service(
HMMRepRequestSample(qsr_type=qsr_type,
dictionary=hmm,
max_length=10,
num_samples=1))
return s
def _calculate_loglikelihood(self, hmm_file, qsr_file, qsr_type):
with open(qsr_file, 'r') as f:
qsr_seq = json.load(f)
with open(hmm_file, 'r') as f:
hmm = json.load(f)
l = self.r.call_service(
HMMRepRequestLogLikelihood(qsr_type=qsr_type,
dictionary=hmm,
qsr_seq=qsr_seq))
return round(l, 5)
def _to_strings(self, array):
return [x.values()[0] for x in array]
def test_qtcb_create(self):
res = self._create_hmm(self.QTCB_QSR, 'qtcb')
self.assertEqual(
hashlib.md5(json.dumps(res)).hexdigest(),
self.correct_hashsum["qtcb"])
def test_qtcb_sample(self):
res = self._create_sample(self.QTCB_SAMPLE_TEST_HMM, 'qtcb')
self.assertEqual(res, self.correct_samples["qtcb"])
def test_qtcb_loglikelihood(self):
res = self._calculate_loglikelihood(self.QTCB_PASSBY_LEFT_HMM,
self.QTCB_QSR, 'qtcb')
self.assertEqual(res, self.correct_loglikelihoods["qtcb"])
def test_qtcc_create(self):
res = self._create_hmm(self.QTCC_QSR, 'qtcc')
self.assertEqual(
hashlib.md5(json.dumps(res)).hexdigest(),
self.correct_hashsum["qtcc"])
def test_qtcc_sample(self):
res = self._create_sample(self.QTCC_SAMPLE_TEST_HMM, 'qtcc')
self.assertEqual(res, self.correct_samples["qtcc"])
def test_qtcc_loglikelihood(self):
res = self._calculate_loglikelihood(self.QTCC_PASSBY_LEFT_HMM,
self.QTCC_QSR, 'qtcc')
self.assertEqual(res, self.correct_loglikelihoods["qtcc"])
def test_qtcbc_create(self):
res = self._create_hmm(self.QTCBC_QSR, 'qtcbc')
self.assertEqual(
hashlib.md5(json.dumps(res)).hexdigest(),
self.correct_hashsum["qtcbc"])
def test_qtcbc_sample(self):
res = self._create_sample(self.QTCBC_SAMPLE_TEST_HMM, 'qtcbc')
self.assertEqual(res, self.correct_samples["qtcbc"])
def test_qtcbc_loglikelihood(self):
res = self._calculate_loglikelihood(self.QTCBC_PASSBY_LEFT_HMM,
self.QTCBC_QSR, 'qtcbc')
self.assertEqual(res, self.correct_loglikelihoods["qtcbc"])
def test_rcc3_create(self):
res = self._create_hmm(self.RCC3_QSR, 'rcc3')
self.assertEqual(
hashlib.md5(json.dumps(res)).hexdigest(),
self.correct_hashsum["rcc3"])
def test_rcc3_sample(self):
res = self._create_sample(self.RCC3_TEST_HMM, 'rcc3')
self.assertTrue(type(res) == list and len(res) > 0)
def test_rcc3_loglikelihood(self):
res = self._calculate_loglikelihood(self.RCC3_TEST_HMM, self.RCC3_QSR,
'rcc3')
self.assertEqual(res, self.correct_loglikelihoods["rcc3"])
def __init__(self, *args):
super(GCalTest, self).__init__(*args)
self.filename = find_resource(PKG, 'test.json')[0]
rospy.init_node(NAME)