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)
Ejemplo n.º 2
0
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
Ejemplo n.º 3
0
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
Ejemplo n.º 6
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
        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)
Ejemplo n.º 7
0
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
Ejemplo n.º 8
0
 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]
Ejemplo n.º 9
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"])
Ejemplo n.º 11
0
    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")
Ejemplo n.º 13
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")
Ejemplo n.º 14
0
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()))
Ejemplo n.º 15
0
                    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)')
Ejemplo n.º 16
0
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"]()))
Ejemplo n.º 17
0
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"])
Ejemplo n.º 18
0
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"])
Ejemplo n.º 19
0
 def __init__(self, *args):
     super(GCalTest, self).__init__(*args)
     self.filename = find_resource(PKG, 'test.json')[0]
     rospy.init_node(NAME)