Exemplo n.º 1
0
    def convert_skeleton_to_world(self, data, use_every=1):
        world = World_Trace()

        joint_states = {
            'head': [],
            'neck': [],
            'torso': [],
            'left_shoulder': [],
            'left_elbow': [],
            'left_hand': [],
            'left_hip': [],
            'left_knee': [],
            'left_foot': [],
            'right_shoulder': [],
            'right_elbow': [],
            'right_hand': [],
            'right_hip': [],
            'right_knee': [],
            'right_foot': []
        }

        for tp, msg in enumerate(data):
            for i in msg.joints:
                o = Object_State(name=i.name, timestamp=tp, x=i.pose.position.x,\
                                y = i.pose.position.y, z=i.pose.position.z, xsize=0.1, ysize=0.1, zsize=0.1)
                joint_states[i.name].append(o)

        for joint_data in joint_states.values():
            world.add_object_state_series(joint_data)
        return world
    def __compute_qsr(self, bb1, bb2):
        if not self.qsr:
            return ""
        ax, ay, bx, by = self.bb1
        cx, cy, dx, dy = self.bb2

        qsrlib = QSRlib()
        world = World_Trace()
        world.add_object_state_series([
            Object_State(name="red",
                         timestamp=0,
                         x=((ax + bx) / 2.0),
                         y=((ay + by) / 2.0),
                         xsize=abs(bx - ax),
                         ysize=abs(by - ay)),
            Object_State(name="yellow",
                         timestamp=0,
                         x=((cx + dx) / 2.0),
                         y=((cy + dy) / 2.0),
                         xsize=abs(dx - cx),
                         ysize=abs(dy - cy))
        ])
        dynamic_args = {"argd": {"qsr_relations_and_values": self.distance}}
        qsrlib_request_message = QSRlib_Request_Message(
            which_qsr=self.qsr, input_data=world, dynamic_args=dynamic_args)
        qsrlib_response_message = qsrlib.request_qsrs(
            req_msg=qsrlib_request_message)

        for t in qsrlib_response_message.qsrs.get_sorted_timestamps():
            foo = ""
            for k, v in zip(
                    qsrlib_response_message.qsrs.trace[t].qsrs.keys(),
                    qsrlib_response_message.qsrs.trace[t].qsrs.values()):
                foo += str(k) + ":" + str(v.qsr) + "; \n"
        return foo
Exemplo n.º 3
0
    def get_world_frame_trace(self, world_objects):
        """Accepts a dictionary of world (soma) objects.
        Adds the position of the object at each timepoint into the World Trace
        Note 'frame' is the actual detection file number. 't' is a timestamp starting from 0.
        't' is used in the World Trace."""

        ob_states = {}
        world = World_Trace()
        for t in self.sorted_timestamps:
            # Joints:
            for joint_id, (x, y, z) in self.map_frame_data[t].items():
                if joint_id not in ob_states.keys():
                    ob_states[joint_id] = [Object_State(name=joint_id, timestamp=t, x=x, y=y, z=z)]
                else:
                    ob_states[joint_id].append(Object_State(name=joint_id, timestamp=t, x=x, y=y, z=z))

            # SOMA objects
            for object, (x, y, z) in world_objects.items():
                if object not in ob_states.keys():
                    ob_states[object] = [Object_State(name=str(object), timestamp=t, x=x, y=y, z=z)]
                else:
                    ob_states[object].append(Object_State(name=str(object), timestamp=t, x=x, y=y, z=z))

            # Robot's position
            (x, y, z) = self.robot_data[t][0]
            if "robot" not in ob_states.keys():
                ob_states["robot"] = [Object_State(name="robot", timestamp=t, x=x, y=y, z=z)]
            else:
                ob_states["robot"].append(Object_State(name="robot", timestamp=t, x=x, y=y, z=z))

        for object_state in ob_states.values():
            world.add_object_state_series(object_state)
        self.map_world = world
    def __init__(self):

        self.world = World_Trace()
        self.options = sorted(QSRlib().qsrs_registry.keys())
        self.calculi = self.options[0]
        # if a value lies between the thresholds set for 'near' and 'far' then the relation is immediately reported as 'far'
        # the values are bound by a 'lesser than, < ' relationship
        self.distance_args = {"Near": 3.0, "Medium": 3.5, "Far": 4.0}
        self.dynamic_args = {
            "argd": {
                "qsr_relations_and_values": self.distance_args
            },
            "for_all_qsrs": {
                'qsrs_for': [('object_1', 'robot'), ('object_2', 'robot')]
            }
        }

        self.relationship_pub = rospy.Publisher("argd_realtionship",
                                                QsrMsg,
                                                queue_size=10)
        self.pose_sub1 = message_filters.Subscriber("extracted_pose1", PoseMsg)
        self.pose_sub2 = message_filters.Subscriber("extracted_pose2", PoseMsg)
        self.cln = QSRlib_ROS_Client()
        # the update rate for the ApproximateTimeSynchronizer is 0.1 and the queue_size is 10
        self.ts = message_filters.ApproximateTimeSynchronizer(
            [self.pose_sub1, self.pose_sub2], 3, 0.1)
        self.ts.registerCallback(self.relations_callback)
Exemplo n.º 5
0
    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()
Exemplo n.º 6
0
    def getQSRState(self, xh0, yh0, xh1, yh1, xr0, yr0, xr1, yr1, td):

        isValid = False
        state = None
        human_os = [
            Object_State(name="human_os", timestamp=0, x=xh0, y=yh0),
            Object_State(name="human_os", timestamp=td, x=xh1, y=yh1)
        ]

        robot_os = [
            Object_State(name="robot_os", timestamp=0, x=xr0, y=yr0),
            Object_State(name="robot_os", timestamp=td, x=xr1, y=yr1)
        ]

        # make some input data
        world = World_Trace()

        world.add_object_state_series(human_os)
        world.add_object_state_series(robot_os)

        # make a QSRlib request message
        qsrlib_request_message = QSRlib_Request_Message(
            self.which_qsr, world, self.dynammic_args)

        # request your QSRs
        qsrlib_response_message = self.qsrlib.request_qsrs(
            req_msg=qsrlib_request_message)

        # should have 1 timestamp
        t = qsrlib_response_message.qsrs.get_sorted_timestamps()
        if len(t) != 1:
            rospy.logerr(
                "Node [" + rospy.get_name() +
                "@getQSRState] : response timestamp message lenght is not 1.")
            return (isValid, state)
        t = t[0]

        # should have one value: human to robot
        v = qsrlib_response_message.qsrs.trace[t].qsrs.values()
        if len(v) != 1:
            rospy.logerr(
                "Node [" + rospy.get_name() +
                "@getQSRState] : response values message lenght is not 1.")
            return (isValid, state)
        v = v[0]
        state = v.qsr.values()
        if len(state) != 1:
            rospy.logerr(
                "Node [" + rospy.get_name() +
                "@getQSRState] : response state message lenght is not 1.")
            return (isValid, state)
        state = state[0]

        # Ok, maybe is valid ...
        isValid = True

        return (isValid, state)
Exemplo n.º 7
0
    def apply_mean_filter(self, window_length=3):
        """Once obtained the joint x,y,z coords.
        Apply a median filter over a temporal window to smooth the joint positions.
        Whilst doing this, create a world Trace object"""

        joints, ob_states = {}, {}
        world = World_Trace()
        window = {}
        filtered_cnt = 0

        for t in self.sorted_timestamps:
            joints[t] = {}

            for joint_id, (x, y, z) in self.skeleton_data[t].items():
                # print "jointID=", joint_id, (x,y,z)
                try:
                    window[joint_id].pop(0)
                except (IndexError, KeyError):
                    window[joint_id] = []

                window[joint_id].append((float(x), float(y), float(z)))
                avg_x, avg_y, avg_z = 0, 0, 0
                for l, point in enumerate(window[joint_id]):
                    avg_x += point[0]
                    avg_y += point[1]
                    avg_z += point[2]

                x, y, z = avg_x / float(l + 1), avg_y / float(
                    l + 1), avg_z / float(l + 1)
                joints[t][joint_id] = (x, y, z)

                #Create a QSRLib format list of Object States (for each joint id)
                if joint_id not in ob_states.keys():
                    ob_states[joint_id] = [
                        Object_State(name=joint_id,
                                     timestamp=filtered_cnt,
                                     x=x,
                                     y=y,
                                     z=z)
                    ]
                else:
                    ob_states[joint_id].append(
                        Object_State(name=joint_id,
                                     timestamp=filtered_cnt,
                                     x=x,
                                     y=y,
                                     z=z))
            filtered_cnt += 1

        # #Add all the joint IDs into the World Trace
        for joint_id, obj in ob_states.items():
            world.add_object_state_series(obj)

        self.filtered_skeleton_data = joints
        self.camera_world = world
    def get_world_frame_trace(self, world_objects):
        """Accepts a dictionary of world (soma) objects.
        Adds the position of the object at each timepoint into the World Trace
        Note 'frame' is the actual detection file number. 't' is a timestamp starting from 0.
        't' is used in the World Trace."""

        ob_states = {}
        world = World_Trace()
        # print ">>", self.map_frame_data.keys()
        for t, frame in enumerate(self.sorted_timestamps):
            #Joints:
            for joint_id, (x, y, z) in self.map_frame_data[frame].items():
                if joint_id not in ob_states.keys():
                    ob_states[joint_id] = [
                        Object_State(name=joint_id, timestamp=t, x=x, y=y, z=z)
                    ]
                else:
                    ob_states[joint_id].append(
                        Object_State(name=joint_id, timestamp=t, x=x, y=y,
                                     z=z))

            # SOMA objects
            for object, (x, y, z) in world_objects.items():
                if object not in ob_states.keys():
                    ob_states[object] = [
                        Object_State(name=str(object),
                                     timestamp=t,
                                     x=x,
                                     y=y,
                                     z=z)
                    ]
                else:
                    ob_states[object].append(
                        Object_State(name=str(object),
                                     timestamp=t,
                                     x=x,
                                     y=y,
                                     z=z))

            # Robot's position
            (x, y, z) = self.robot_data[frame][0]
            if 'robot' not in ob_states.keys():
                ob_states['robot'] = [
                    Object_State(name='robot', timestamp=t, x=x, y=y, z=z)
                ]
            else:
                ob_states['robot'].append(
                    Object_State(name='robot', timestamp=t, x=x, y=y, z=z))

        for object_state in ob_states.values():
            world.add_object_state_series(object_state)
        self.map_world = world
Exemplo n.º 9
0
def isValidState(xh0, yh0, xh1, yh1, xr0, yr0, xr1, yr1, td, which_qsr,
                 dynammic_args, forbidden_states):
    ans = 1

    human_os = [
        Object_State(name="human_os", timestamp=0, x=xh0, y=yh0),
        Object_State(name="human_os", timestamp=td, x=xh1, y=yh1)
    ]

    robot_os = [
        Object_State(name="robot_os", timestamp=0, x=xr0, y=yr0),
        Object_State(name="robot_os", timestamp=td, x=xr1, y=yr1)
    ]

    # make some input data
    world = World_Trace()

    world.add_object_state_series(human_os)
    world.add_object_state_series(robot_os)

    # make a QSRlib request message
    qsrlib_request_message = QSRlib_Request_Message(which_qsr, world,
                                                    dynammic_args)

    # request your QSRs
    qsrlib_response_message = qsrlib.request_qsrs(
        req_msg=qsrlib_request_message)

    # should have 1 timestamp
    t = qsrlib_response_message.qsrs.get_sorted_timestamps()
    if len(t) != 1:
        print("something is wrong!!!!!!!!!!!!! with t")
        sys.exit(1)
    t = t[0]

    # should have one value: human to robot
    v = qsrlib_response_message.qsrs.trace[t].qsrs.values()
    if len(v) != 1:
        print("something is wrong!!!!!!!!!!!!! with v")
        sys.exit(1)
    v = v[0]
    state = v.qsr.values()
    if len(state) != 1:
        print("something is wrong!!!!!!!!!!!!! with state")
        sys.exit(1)
    state = state[0]

    if state in forbidden_states:
        ans = 0
        #print(state)
    return (ans, state)
Exemplo n.º 10
0
def qsr_relation_between(obj1_name, obj2_name, obj1, obj2):
    global frame

    qsrlib = QSRlib()
    options = sorted(qsrlib.qsrs_registry.keys())
    if init_qsr.qsr not in options:
        raise ValueError("qsr not found, keywords: %s" % options)

    world = World_Trace()

    object_types = {obj1_name: obj1_name, obj2_name: obj2_name}

    dynamic_args = {
        "qstag": {
            "object_types": object_types,
            "params": {
                "min_rows": 1,
                "max_rows": 1,
                "max_eps": 3
            }
        },
        "tpcc": {
            "qsrs_for": [(obj1_name, obj2_name)]
        },
        "rcc2": {
            "qsrs_for": [(obj1_name, obj2_name)]
        },
        "rcc4": {
            "qsrs_for": [(obj1_name, obj2_name)]
        },
        "rcc8": {
            "qsrs_for": [(obj1_name, obj2_name)]
        }
    }

    o1 = [Object_State(name=obj1_name, timestamp=frame, x=obj1[0], y=obj1[1], \
      xsize=obj1[2], ysize=obj1[3])]
    o2 = [Object_State(name=obj2_name, timestamp=frame, x=obj2[0], y=obj2[1], \
      xsize=obj2[2], ysize=obj2[3])]
    world.add_object_state_series(o1)
    world.add_object_state_series(o2)

    qsrlib_request_message = QSRlib_Request_Message(which_qsr=init_qsr.qsr, \
      input_data=world, dynamic_args=dynamic_args)
    qsrlib_response_message = qsrlib.request_qsrs(
        req_msg=qsrlib_request_message)
    pretty_print_world_qsr_trace(init_qsr.qsr,
                                 qsrlib_response_message)  #, vis = True)
    qsr_value = find_qsr_value(init_qsr.qsr, qsrlib_response_message)
    return qsr_value
Exemplo n.º 11
0
    def get_world_frame_trace(self, world_objects):
        """Accepts a dictionary of world (soma) objects.
        Adds the position of the object at each timepoint into the World Trace"""

        ob_states = {}
        world = World_Trace()
        for t in self.sorted_timestamps:
            #Joints:
            for joint_id, (x, y, z) in self.map_frame_data[t].items():
                if joint_id not in ob_states.keys():
                    ob_states[joint_id] = [
                        Object_State(name=joint_id, timestamp=t, x=x, y=y, z=z)
                    ]
                else:
                    ob_states[joint_id].append(
                        Object_State(name=joint_id, timestamp=t, x=x, y=y,
                                     z=z))

            # SOMA objects
            for object, (x, y, z) in world_objects.items():
                if object not in ob_states.keys():
                    ob_states[object] = [
                        Object_State(name=str(object),
                                     timestamp=t,
                                     x=x,
                                     y=y,
                                     z=z)
                    ]
                else:
                    ob_states[object].append(
                        Object_State(name=str(object),
                                     timestamp=t,
                                     x=x,
                                     y=y,
                                     z=z))

            # Robot's position
            (x, y, z) = self.robot_data[t][0]
            if 'robot' not in ob_states.keys():
                ob_states['robot'] = [
                    Object_State(name='robot', timestamp=t, x=x, y=y, z=z)
                ]
            else:
                ob_states['robot'].append(
                    Object_State(name='robot', timestamp=t, x=x, y=y, z=z))

        for object_state in ob_states.values():
            world.add_object_state_series(object_state)

        self.map_world = world
Exemplo n.º 12
0
class QSRlib_Request_Message(object):
    def __init__(self, which_qsr="", input_data=None, qsrs_for=[], timestamp_request_made=None,
                 start=0, finish=-1, objects_names=[], include_missing_data=True, qsr_relations_and_values={},
                 future=False, config=None, dynamic_args=None):
        self.future = future
        self.which_qsr = which_qsr
        self.input_data = None
        self.set_input_data(input_data=input_data, start=start, finish=finish, objects_names=objects_names)
        self.qsrs_for = qsrs_for
        self.timestamp_request_made = datetime.now() if timestamp_request_made is None else timestamp_request_made
        self.include_missing_data = include_missing_data
        self.qsr_relations_and_values = qsr_relations_and_values # should be more dynamic
        self.config = config
        self.dynamic_args = dynamic_args

    def make(self, which_qsr, input_data, qsrs_for=[], timestamp_request_made=None, future=None, ini=None,
             dynamic_args=None):
        if future:
            self.future = future
        self.which_qsr = which_qsr
        self.input_data = self.set_input_data(input_data)
        self.qsrs_for = qsrs_for
        self.timestamp_request_made = datetime.now() if timestamp_request_made is None else timestamp_request_made
        self.ini = None
        self.dynamic_args = None

    def set_input_data(self, input_data, start=0, finish=-1, objects_names=[]):
        error = False
        if input_data is None:
            input_data = World_Trace()
        if isinstance(input_data, World_Trace):
            if len(input_data.trace) > 0:
                self.input_data = input_data
            else:
                if self.input_data is not None and len(self.input_data.trace) > 0:
                    print("Reusing previous input data")
                else:
                    print("Warning (QSRlib_Request_Message.set_input_data): It seems you are trying to reuse previous data, but previous data is empty")
                    self.input_data = World_Trace(description="error")
                    error = True
        else:
            print("ERROR (QSRLib_.set_input_data): input data has incorrect type, must be of type 'World_Trace'")
            self.input_data = World_Trace(description="error")
            error = True
        if not error:
            if finish >= 0:
                self.input_data = self.input_data.get_at_timestamp_range(start=start, finish=finish)
            if len(objects_names) > 0:
                self.input_data = self.input_data.get_for_objects(objects_names=objects_names)
Exemplo n.º 13
0
    def ppl_cb(self, msg):
        self.__buffer["human"].append(msg.poses[0])
        self.__buffer["robot"].append(self.robot_pose)
        
        # QTC needs at least two instances in time to work
        ob = []
        if len(self.__buffer["human"]) > 1:
            # Creating the world trace for both agents over all timestamps
            world = World_Trace()
            for idx, (human, robot) in enumerate(zip(self.__buffer["human"], self.__buffer["robot"])):
                ob.append(Object_State(
                    name="human",
                    timestamp=idx,
                    x=human.position.x,
                    y=human.position.y
                ))
                ob.append(Object_State(
                    name="robot",
                    timestamp=idx,
                    x=robot.position.x,
                    y=robot.position.y
                ))

            world.add_object_state_series(ob)

            # Creating the qsr_lib request message
            qrmsg = QSRlib_Request_Message(
                which_qsr="qtccs",
                input_data=world,
                dynamic_args=self.__parameters
            )
            cln = QSRlib_ROS_Client()
            req = cln.make_ros_request_message(qrmsg)
            res = cln.request_qsrs(req)
            out = pickle.loads(res.data)
           
            # Printing the result, publishing might be more useful though ;)
            qsr_array = []
            for t in out.qsrs.get_sorted_timestamps():
                for k, v in out.qsrs.trace[t].qsrs.items():
                    qsr_array.append(v.qsr.values()[0])
            #test    
            self.send_dict(qsr_array)
            
            #print out
           

        rospy.sleep(0.3) # Worst smoothing ever, I'm sure you can do better
Exemplo n.º 14
0
	def convert_skeleton_to_world(self, data, use_every=1):
		world = World_Trace()

		joint_states = {'head' : [], 'neck' : [],  'torso' : [], 'left_shoulder' : [],'left_elbow' : [],
				'left_hand' : [],'left_hip' : [], 'left_knee' : [],'left_foot' : [],'right_shoulder' : [],
				'right_elbow' : [],'right_hand' : [],'right_hip' : [],'right_knee' : [],'right_foot' : []}

		for tp, msg in enumerate(data):
			for i in msg.joints:
				o = Object_State(name=i.name, timestamp=tp, x=i.pose.position.x,\
				                y = i.pose.position.y, z=i.pose.position.z, xsize=0.1, ysize=0.1, zsize=0.1)
				joint_states[i.name].append(o)

		for joint_data in joint_states.values():
			world.add_object_state_series(joint_data)
		return world
Exemplo n.º 15
0
    def apply_median_filter(self, window_size=11, vis=False):
        """Once obtained the joint x,y,z coords.
        Apply a median filter over a temporal window to smooth the joint positions.
        Whilst doing this, create a world Trace object"""

        fx = 525.0
        fy = 525.0
        cx = 319.5
        cy = 239.5

        data, f_data, ob_states = {}, {}, {}
        self.camera_world = World_Trace()
        self.filtered_skeleton_data = {}

        for t in self.sorted_timestamps:
            self.filtered_skeleton_data[t] = {}  # initialise with all timepoints
            for joint_id, (x, y, z) in self.skeleton_data[t].items():
                try:
                    data[joint_id]["x"].append(x)
                    data[joint_id]["y"].append(y)
                    data[joint_id]["z"].append(z)
                except:
                    data[joint_id] = {"x": [x], "y": [y], "z": [z]}

        for joint_id, joint_dic in data.items():
            f_data[joint_id] = {}
            for dim, values in joint_dic.items():
                filtered_values = sp.signal.medfilt(values, window_size)  # filter
                f_data[joint_id][dim] = filtered_values

                if dim is "z" and 0 in filtered_values:
                    print "Z should never be 0. (distance to camera)."
                    vis = True

                if vis and "hand" in joint_id:
                    print joint_id
                    title1 = "input %s position: %s" % (joint_id, dim)
                    title2 = "filtered %s position: %s" % (joint_id, dim)
                    plot_the_results(values, filtered_values, title1, title2)

            # Create a QSRLib format list of Object States (for each joint id)
            for cnt, t in enumerate(self.sorted_timestamps):
                x = f_data[joint_id]["x"][cnt]
                y = f_data[joint_id]["y"][cnt]
                z = f_data[joint_id]["z"][cnt]

                # add the x2d and y2d (using filtered x,y,z data)
                # print self.uuid, t, joint_id, (x,y,z)
                x2d = int(x * fx / z * 1 + cx)
                y2d = int(y * fy / z * -1 + cy)
                self.filtered_skeleton_data[t][joint_id] = (x, y, z, x2d, y2d)  # Kept for Legacy
                try:
                    ob_states[joint_id].append(Object_State(name=joint_id, timestamp=cnt, x=x, y=y, z=z))
                except:
                    ob_states[joint_id] = [Object_State(name=joint_id, timestamp=cnt, x=x, y=y, z=z)]

        # #Add all the joint IDs into the World Trace
        for joint_id, obj in ob_states.items():
            self.camera_world.add_object_state_series(obj)
Exemplo n.º 16
0
    def ppl_cb(self, msg):
        self.__buffer["human"].append(msg.poses[0])
        self.__buffer["robot"].append(self.robot_pose)

        # QTC needs at least two instances in time to work
        ob = []
        if len(self.__buffer["human"]) > 1:
            # Creating the world trace for both agents over all timestamps
            world = World_Trace()
            for idx, (human, robot) in enumerate(
                    zip(self.__buffer["human"], self.__buffer["robot"])):
                ob.append(
                    Object_State(name="human",
                                 timestamp=idx,
                                 x=human.position.x,
                                 y=human.position.y))
                ob.append(
                    Object_State(name="robot",
                                 timestamp=idx,
                                 x=robot.position.x,
                                 y=robot.position.y))

            world.add_object_state_series(ob)

            # Creating the qsr_lib request message
            qrmsg = QSRlib_Request_Message(which_qsr="qtccs",
                                           input_data=world,
                                           dynamic_args=self.__parameters)
            cln = QSRlib_ROS_Client()
            req = cln.make_ros_request_message(qrmsg)
            res = cln.request_qsrs(req)
            out = pickle.loads(res.data)

            # Printing the result, publishing might be more useful though ;)
            qsr_array = []
            for t in out.qsrs.get_sorted_timestamps():
                for k, v in out.qsrs.trace[t].qsrs.items():
                    qsr_array.append(v.qsr.values()[0])
            #test
            self.send_dict(qsr_array)

            #print out

        rospy.sleep(0.3)  # Worst smoothing ever, I'm sure you can do better
Exemplo n.º 17
0
    def _convert_to_world(self,
                          data_dict,
                          quantisation_factor=0,
                          validate=True,
                          no_collapse=False,
                          distance_threshold=np.Inf):
        world = World_Trace()

        agent1 = data_dict["agent1"]
        agent2 = data_dict["agent2"]

        name1 = agent1["name"]
        name2 = agent2["name"]

        x1 = np.array(agent1["x"], dtype=float)
        y1 = np.array(agent1["y"], dtype=float)

        x2 = np.array(agent2["x"], dtype=float)
        y2 = np.array(agent2["y"], dtype=float)

        ob = []
        for idx, (e_x1, e_y1, e_x2, e_y2) in enumerate(zip(x1, y1, x2, y2)):
            ob.append(
                Object_State(name=name1,
                             timestamp=idx,
                             x=e_x1,
                             y=e_y1,
                             quantisation_factor=quantisation_factor,
                             validate=validate,
                             no_collapse=no_collapse,
                             distance_threshold=distance_threshold))
            ob.append(
                Object_State(name=name2,
                             timestamp=idx,
                             x=e_x2,
                             y=e_y2,
                             quantisation_factor=quantisation_factor,
                             validate=validate,
                             no_collapse=no_collapse,
                             distance_threshold=distance_threshold))

        world.add_object_state_series(ob)
        return world
Exemplo n.º 18
0
    def __init__(self):

        self.world = World_Trace()
        self.options = sorted(QSRlib().qsrs_registry.keys())
        self.calculi = self.options[6]
        # quantisation_factor represents the minimum change that should be considered to assume that the object has moved
        self.dynamic_args = {"qtcbs": {"no_collapse":True,"quantisation_factor": 0.01}, "qtcbs": {"qsrs_for": [("object_1","robot"), ("object_2","robot")]}}
        self.prev_timestamp = 0
        self.prev_pose_x1 = self.prev_pose_y1 = 0
        self.prev_pose_x2 = self.prev_pose_y2 = 0
        self.prev_robot_pose_x = self.prev_robot_pose_y = 0

        self.relationship_pub = rospy.Publisher("qtcb_realtionship", QsrMsg, queue_size = 10)
        self.pose_sub1 = message_filters.Subscriber("extracted_pose1",PoseMsg)
        self.pose_sub2 = message_filters.Subscriber("extracted_pose2",PoseMsg)
        self.cln = QSRlib_ROS_Client()
        # the update rate for the ApproximateTimeSynchronizer is 0.1 and the queue_size is 10
        self.ts = message_filters.ApproximateTimeSynchronizer([self.pose_sub1, self.pose_sub2], 10, 0.1)
        self.ts.registerCallback(self.relations_callback)
Exemplo n.º 19
0
    def apply_mean_filter_old(self, window_length=3):
        """Once obtained the joint x,y,z coords.
        Apply a median filter over a temporal window to smooth the joint positions.
        Whilst doing this, create a world Trace object"""

        joints, ob_states = {}, {}
        world = World_Trace()
        window = {}
        filtered_cnt = 0

        for t in self.sorted_timestamps:
            joints[t] = {}

            for joint_id, (x, y, z) in self.skeleton_data[t].items():
                # print "jointID=", joint_id, (x,y,z)
                try:
                    window[joint_id].pop(0)
                except (IndexError, KeyError):
                    window[joint_id] = []

                window[joint_id].append((float(x), float(y), float(z)))
                avg_x, avg_y, avg_z = 0, 0, 0
                for l, point in enumerate(window[joint_id]):
                    avg_x += point[0]
                    avg_y += point[1]
                    avg_z += point[2]

                x, y, z = avg_x / float(l + 1), avg_y / float(l + 1), avg_z / float(l + 1)
                joints[t][joint_id] = (x, y, z)

                # Create a QSRLib format list of Object States (for each joint id)
                if joint_id not in ob_states.keys():
                    ob_states[joint_id] = [Object_State(name=joint_id, timestamp=filtered_cnt, x=x, y=y, z=z)]
                else:
                    ob_states[joint_id].append(Object_State(name=joint_id, timestamp=filtered_cnt, x=x, y=y, z=z))
            filtered_cnt += 1

        # #Add all the joint IDs into the World Trace
        for joint_id, obj in ob_states.items():
            world.add_object_state_series(obj)

        self.filtered_skeleton_data = joints
        self.camera_world = world
    def __compute_qsr(self, bb1, bb2):
        if not self.qsr:
            return ""
        ax, ay, bx, by = self.bb1
        cx, cy, dx, dy = self.bb2
        
        qsrlib = QSRlib()
        world = World_Trace()
        world.add_object_state_series([Object_State(name="red", timestamp=0, x=((ax+bx)/2.0), y=((ay+by)/2.0), xsize=abs(bx-ax), ysize=abs(by-ay)),
            Object_State(name="yellow", timestamp=0, x=((cx+dx)/2.0), y=((cy+dy)/2.0), xsize=abs(dx-cx), ysize=abs(dy-cy))])
        dynamic_args = {"argd": {"qsr_relations_and_values": self.distance}}
        qsrlib_request_message = QSRlib_Request_Message(which_qsr=self.qsr, input_data=world, dynamic_args=dynamic_args)
        qsrlib_response_message = qsrlib.request_qsrs(req_msg=qsrlib_request_message)

        for t in qsrlib_response_message.qsrs.get_sorted_timestamps():
            foo = ""
            for k, v in zip(qsrlib_response_message.qsrs.trace[t].qsrs.keys(),
                            qsrlib_response_message.qsrs.trace[t].qsrs.values()):
                foo += str(k) + ":" + str(v.qsr) + "; \n"
        return foo
Exemplo n.º 21
0
def qsr_message(state_series, objects):
    qsrlib = QSRlib()
    options = sorted(qsrlib.qsrs_registry.keys())
    if init_qsr.qsr not in options:
        raise ValueError("qsr not found, keywords: %s" % options)

    world = World_Trace()

    # Create dynamic arguments for every qsr type
    dynamic_args = dynamic_qsr_arguments(objects)
    
    # Add all object states in the World.
    for o in state_series:
        world.add_object_state_series(o)
    # Create request message
    qsrlib_request_message = QSRlib_Request_Message(which_qsr=init_qsr.qsr, input_data=world, dynamic_args=dynamic_args)

    qsrlib_response_message = qsrlib.request_qsrs(req_msg=qsrlib_request_message)

    pretty_print_world_qsr_trace(init_qsr.qsr, qsrlib_response_message, vis = True)
    def get_world_frame_trace(self, world_objects):
        """Accepts a dictionary of world (soma) objects.
        Adds the position of the object at each timepoint into the World Trace"""
        self.subj_world_trace = {}
        for subj in self.skeleton_map:
            ob_states={}
            world = World_Trace()
            map_frame_data = self.skeleton_map[subj]
            for joint_id in map_frame_data.keys():
                #Joints:
                for t in xrange(self.frames):
                    x = map_frame_data[joint_id][t][0]
                    y = map_frame_data[joint_id][t][1]
                    z = map_frame_data[joint_id][t][2]
                    if joint_id not in ob_states.keys():
                        ob_states[joint_id] = [Object_State(name=joint_id, timestamp=t+1, x=x, y=y, z=z)]
                    else:
                        ob_states[joint_id].append(Object_State(name=joint_id, timestamp=t+1, x=x, y=y, z=z))

                # SOMA objects
            for t in xrange(self.frames):
                for object, (x,y,z) in world_objects.items():
                    if object not in ob_states.keys():
                        ob_states[object] = [Object_State(name=str(object), timestamp=t+1, x=x, y=y, z=z)]
                    else:
                        ob_states[object].append(Object_State(name=str(object), timestamp=t+1, x=x, y=y, z=z))

                # # Robot's position
                # (x,y,z) = self.robot_data[t][0]
                # if 'robot' not in ob_states.keys():
                #     ob_states['robot'] = [Object_State(name='robot', timestamp=t, x=x, y=y, z=z)]
                # else:
                #     ob_states['robot'].append(Object_State(name='robot', timestamp=t, x=x, y=y, z=z))

            for obj, object_state in ob_states.items():
                world.add_object_state_series(object_state)

            # get world trace for each person

            region = self.soma_roi_config[self.waypoint]
            self.subj_world_trace[subj] = self.get_object_frame_qsrs(world, self.objects[region])
    def _convert_to_world(self, data_dict):
        world = World_Trace()

        agent1 = data_dict["agent1"]
        agent2 = data_dict["agent2"]

        name1 = agent1["name"]
        name2 = agent2["name"]

        x1 = np.array(agent1["x"], dtype=float)
        y1 = np.array(agent1["y"], dtype=float)

        x2 = np.array(agent2["x"], dtype=float)
        y2 = np.array(agent2["y"], dtype=float)

        ob = []
        for idx, (e_x1, e_y1, e_x2, e_y2) in enumerate(zip(x1, y1, x2, y2)):
            ob.append(Object_State(name=name1, timestamp=idx, x=e_x1, y=e_y1))
            ob.append(Object_State(name=name2, timestamp=idx, x=e_x2, y=e_y2))

        world.add_object_state_series(ob)
        return world
Exemplo n.º 24
0
    def __init__(self):

        self.world = World_Trace()
        self.options = sorted(QSRlib().qsrs_registry.keys())
        self.calculi = self.options[0]
        # any value in between goes to the upper bound
        self.dynamic_args = {
            "argd": {
                "qsr_relations_and_values": {
                    "Near": 0.36,
                    "Medium": .40,
                    "Far": .45
                }
            }
        }

        self.relationship_pub = rospy.Publisher("argd_realtionship",
                                                QsrMsg,
                                                queue_size=2)
        self.pose_sub1 = rospy.Subscriber("extracted_pose1", PoseMsg,
                                          self.relations_callback)
        self.cln = QSRlib_ROS_Client()
Exemplo n.º 25
0
 def set_input_data(self, input_data, start=0, finish=-1, objects_names=[]):
     error = False
     if input_data is None:
         input_data = World_Trace()
     if isinstance(input_data, World_Trace):
         if len(input_data.trace) > 0:
             self.input_data = input_data
         else:
             if self.input_data is not None and len(self.input_data.trace) > 0:
                 print("Reusing previous input data")
             else:
                 print("Warning (QSRlib_Request_Message.set_input_data): It seems you are trying to reuse previous data, but previous data is empty")
                 self.input_data = World_Trace(description="error")
                 error = True
     else:
         print("ERROR (QSRLib_.set_input_data): input data has incorrect type, must be of type 'World_Trace'")
         self.input_data = World_Trace(description="error")
         error = True
     if not error:
         if finish >= 0:
             self.input_data = self.input_data.get_at_timestamp_range(start=start, finish=finish)
         if len(objects_names) > 0:
             self.input_data = self.input_data.get_for_objects(objects_names=objects_names)
from __future__ import print_function, division
from qsrlib_io.world_trace import Object_State, World_Trace


def print_world_trace(world_trace):
    for t in world_trace.get_sorted_timestamps():
        print("-t:", t)
        for oname, o in world_trace.trace[t].objects.items():
            print("%s\t%f\t%f\t%f\t%f\t%f\t%f" % (oname, o.x, o.y, o.z, o.xsize, o.ysize, o.xsize))

def print_world_state(world_state):
    for oname, o in world_state.objects.items():
            print("%s\t%f\t%f\t%f\t%f\t%f\t%f" % (oname, o.x, o.y, o.z, o.xsize, o.ysize, o.xsize))

if __name__ == "__main__":
    world = World_Trace()
    o1 = [Object_State(name="o1", timestamp=0, x=1., y=1., xsize=5., ysize=8.),
          Object_State(name="o1", timestamp=1, x=1., y=2., xsize=5., ysize=8.),
          Object_State(name="o1", timestamp=2, x=1., y=3., xsize=5., ysize=8.),
          Object_State(name="o1", timestamp=3, x=1., y=4., xsize=5., ysize=8.),
          Object_State(name="o1", timestamp=4, x=1., y=5., xsize=5., ysize=8.)]

    o2 = [Object_State(name="o2", timestamp=0, x=11., y=1., xsize=5., ysize=8.),
          Object_State(name="o2", timestamp=1, x=11., y=2., xsize=5., ysize=8.),
          Object_State(name="o2", timestamp=2, x=11., y=3., xsize=5., ysize=8.),
          Object_State(name="o2", timestamp=3, x=11., y=4., xsize=5., ysize=8.),
          Object_State(name="o2", timestamp=4, x=11., y=5., xsize=5., ysize=8.)]
    world.add_object_state_series(o1)
    world.add_object_state_series(o2)

    # world_new = world.get_at_timestamp_range(2, 3, include_finish=False)
Exemplo n.º 27
0
class qtc_interpreter:
    def __init__(self):

        self.world = World_Trace()
        self.options = sorted(QSRlib().qsrs_registry.keys())
        self.calculi = self.options[6]
        # quantisation_factor represents the minimum change that should be considered to assume that the object has moved
        self.dynamic_args = {"qtcbs": {"no_collapse":True,"quantisation_factor": 0.01}, "qtcbs": {"qsrs_for": [("object_1","robot"), ("object_2","robot")]}}
        self.prev_timestamp = 0
        self.prev_pose_x1 = self.prev_pose_y1 = 0
        self.prev_pose_x2 = self.prev_pose_y2 = 0
        self.prev_robot_pose_x = self.prev_robot_pose_y = 0

        self.relationship_pub = rospy.Publisher("qtcb_realtionship", QsrMsg, queue_size = 10)
        self.pose_sub1 = message_filters.Subscriber("extracted_pose1",PoseMsg)
        self.pose_sub2 = message_filters.Subscriber("extracted_pose2",PoseMsg)
        self.cln = QSRlib_ROS_Client()
        # the update rate for the ApproximateTimeSynchronizer is 0.1 and the queue_size is 10
        self.ts = message_filters.ApproximateTimeSynchronizer([self.pose_sub1, self.pose_sub2], 10, 0.1)
        self.ts.registerCallback(self.relations_callback)

    def relations_callback(self,extracted_pose1,extracted_pose2):
        qtc_relation_msg = QsrMsg()
        print("extracted_pose1",extracted_pose1.pose_vec.position)
        print("marker1", extracted_pose1.marker_id)
        print('==========================================')
        print("extracted_pose2",extracted_pose2.pose_vec.position)
        print("marker2", extracted_pose2.marker_id)
        print("##########################################")


        o1 = [Object_State(name="object_1", timestamp=self.prev_timestamp, x=self.prev_pose_x1, y=self.prev_pose_y1),
              Object_State(name="object_1", timestamp=extracted_pose1.header.stamp.secs, x=extracted_pose1.pose_vec.position.x, y=extracted_pose1.pose_vec.position.z)]
        o2 = [Object_State(name="robot", timestamp=self.prev_timestamp, x=self.prev_robot_pose_x, y=self.prev_robot_pose_y),
              Object_State(name="robot", timestamp=extracted_pose1.header.stamp.secs, x=0, y=0)]
        o3 = [Object_State(name="object_2", timestamp=self.prev_timestamp, x=self.prev_pose_x2, y=self.prev_pose_y2),
              Object_State(name="object_2", timestamp=extracted_pose1.header.stamp.secs, x=extracted_pose2.pose_vec.position.x, y=extracted_pose2.pose_vec.position.z)]
        self.world.add_object_state_series(o1)
        self.world.add_object_state_series(o2)
        self.world.add_object_state_series(o3)


        self.prev_timestamp = extracted_pose1.header.stamp.secs
        self.prev_pose_x1 = extracted_pose1.pose_vec.position.x
        self.prev_pose_y1 = extracted_pose1.pose_vec.position.z
        self.prev_pose_x2 = extracted_pose2.pose_vec.position.x
        self.prev_pose_y2 = extracted_pose2.pose_vec.position.z
        self.prev_robot_pose_x = 0
        self.prev_robot_pose_y = 0


        qsrlib_request_message = QSRlib_Request_Message(which_qsr=self.calculi, input_data=self.world, dynamic_args=self.dynamic_args)
        req = self.cln.make_ros_request_message(qsrlib_request_message)
        res = self.cln.request_qsrs(req)
        qsrlib_response_message = pickle.loads(res.data)


        qtc_relation_msg.selected_qsr = str(self.calculi)
        qtc_relation_msg.marker_id1 = extracted_pose1.marker_id
        qtc_relation_msg.marker_id2 = extracted_pose2.marker_id

        for time in qsrlib_response_message.qsrs.get_sorted_timestamps():
            qtc_relation_msg.message_time = extracted_pose1.header.stamp.secs
            object_key = str(extracted_pose1.header.stamp.secs) + " = "
            value_key = str(extracted_pose1.header.stamp.secs) + " = "

            for key, value in zip(qsrlib_response_message.qsrs.trace[time].qsrs.keys(),
                            qsrlib_response_message.qsrs.trace[time].qsrs.values()):
                            object_key += str(key) + ';'
                            qtc_relation_msg.objects = object_key
                            value_key += str(value.qsr)
                            qtc_relation_msg.relationship = value_key
            self.relationship_pub.publish(qtc_relation_msg)
            print(qtc_relation_msg)
Exemplo n.º 28
0
    def __compute_qsr(self, bb1, bb2):
        if not self.qsr:
            return ""
        ax, ay, bx, by = bb1
        cx, cy, dx, dy = bb2
        
        qsrlib = QSRlib()
        world = World_Trace()

        if self.args.distribution[0] == "weibull":
            weibull_units = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/weibull_units.npy")
            weibull = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/weibull.npy")
            scores = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/scaled_scores.npy")
            timestamp = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/timestamp.npy")

            o1 = []
            o2 = []
            o3 = []

            for ii,unit in tqdm(enumerate(weibull_units)):
                o1.append(Object_State(name="o1", timestamp=timestamp[ii], x=ii, y=int(unit*100), object_type="Unit"))
            for ii,weib in tqdm(enumerate(weibull)):
                o2.append(Object_State(name="o2", timestamp=timestamp[ii], x=ii, y=int(weib*100), object_type="Weibull"))
            for ii,score in tqdm(enumerate(scores)):
                o3.append(Object_State(name="o3", timestamp=timestamp[ii], x=ii, y=int(score*100), object_type="Score"))
        
        elif self.args.distribution[0] == "beta":
            beta_units = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/beta_units.npy")
            beta = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/beta.npy")
            scores = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/scaled_scores.npy")
            timestamp = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/timestamp.npy")

            o1 = []
            o2 = []
            o3 = []

            for ii,unit in tqdm(enumerate(beta_units)):
                o1.append(Object_State(name="o1", timestamp=timestamp[ii], x=ii, y=int(unit*100), object_type="Unit"))
            for ii,beta in tqdm(enumerate(beta)):
                o2.append(Object_State(name="o2", timestamp=timestamp[ii], x=ii, y=int(beta*100), object_type="Beta"))
            for ii,score in tqdm(enumerate(scores)):
                o3.append(Object_State(name="o3", timestamp=timestamp[ii], x=ii, y=int(score*100), object_type="Score"))

        elif self.args.distribution[0] == "cauchy":
            cauchy_units = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/cauchy_units.npy")
            cauchy = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/cauchy.npy")
            scores = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/scaled_scores.npy")
            timestamp = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/timestamp.npy")

            o1 = []
            o2 = []
            o3 = []

            for ii,unit in tqdm(enumerate(cauchy_units)):
                o1.append(Object_State(name="o1", timestamp=timestamp[ii], x=ii, y=int(unit*100), object_type="Unit"))
            for ii,cauc in tqdm(enumerate(cauchy)):
                o2.append(Object_State(name="o2", timestamp=timestamp[ii], x=ii, y=int(cauc*100), object_type="Cauchy"))
            for ii,score in tqdm(enumerate(scores)):
                o3.append(Object_State(name="o3", timestamp=timestamp[ii], x=ii, y=int(score*100), object_type="Score"))

        elif self.args.distribution[0] == "rayleigh":
            rayleigh_units = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/rayleigh_units.npy")
            rayleigh = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/beta.npy")
            scores = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/scaled_scores.npy")
            timestamp = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/timestamp.npy")

            o1 = []
            o2 = []
            o3 = []

            for ii,unit in tqdm(enumerate(rayleigh_units)):
                o1.append(Object_State(name="o1", timestamp=timestamp[ii], x=ii, y=int(unit*100), object_type="Unit"))
            for ii,rayl in tqdm(enumerate(rayleigh)):
                o2.append(Object_State(name="o2", timestamp=timestamp[ii], x=ii, y=int(rayl*100), object_type="Rayleigh"))
            for ii,score in tqdm(enumerate(scores)):
                o3.append(Object_State(name="o3", timestamp=timestamp[ii], x=ii, y=int(score*100), object_type="Score"))

        elif self.args.distribution[0] == "gamma":
            gamma_units = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/gamma_units.npy")
            gamma = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/gamma.npy")
            scores = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/scaled_scores.npy")
            timestamp = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/timestamp.npy")

            o1 = []
            o2 = []
            o3 = []

            for ii,unit in tqdm(enumerate(gamma_units)):
                o1.append(Object_State(name="o1", timestamp=timestamp[ii], x=ii, y=int(unit*100), object_type="Unit"))
            for ii,gam in tqdm(enumerate(gamma)):
                o2.append(Object_State(name="o2", timestamp=timestamp[ii], x=ii, y=int(gam*100), object_type="Gamma"))
            for ii,score in tqdm(enumerate(scores)):
                o3.append(Object_State(name="o3", timestamp=timestamp[ii], x=ii, y=int(score*100), object_type="Score"))
        
        world.add_object_state_series(o1)
        world.add_object_state_series(o2)
        world.add_object_state_series(o3)
        
        # world.add_object_state_series([Object_State(name="red", timestamp=0, x=((ax+bx)/2.0), y=((ay+by)/2.0), xsize=abs(bx-ax), ysize=abs(by-ay)),
        #     Object_State(name="yellow", timestamp=0, x=((cx+dx)/2.0), y=((cy+dy)/2.0), xsize=abs(dx-cx), ysize=abs(dy-cy))])
        dynamic_args = {"argd": {"qsr_relations_and_values": self.distance}}
        qsrlib_request_message = QSRlib_Request_Message(which_qsr=self.qsr, input_data=world, dynamic_args=dynamic_args)
        qsrlib_response_message = qsrlib.request_qsrs(req_msg=qsrlib_request_message)

        for t in qsrlib_response_message.qsrs.get_sorted_timestamps():
            foo = ""
            for k, v in zip(qsrlib_response_message.qsrs.trace[t].qsrs.keys(),
                            qsrlib_response_message.qsrs.trace[t].qsrs.values()):
                foo += str(k) + ":" + str(v.qsr) + "; \n"
        return foo
Exemplo n.º 29
0
class event(object):
    """Event object class"""

    def __init__(self, uuid=None, dir_=None, waypoint=None):

        self.uuid = uuid
        # self.start_frame, self.end_frame = self.get_last_frame()
        # self.waypoint = waypoint
        self.dir = dir_

        self.sorted_timestamps = []
        self.sorted_ros_timestamps = []
        self.bad_timepoints = {}  # use for filtering
        self.skeleton_data = {}  # type: dict[timepoint][joint_id]= (x, y, z, x2d, y2d)
        self.map_frame_data = {}  # type: dict[timepoint][joint_id]= (x, y, z, x2d, y2d)
        self.robot_data = {}  # type: dict[timepoint][joint_id]= ((x, y, z), (roll, pitch, yaw))
        # self.world = World_Trace()

    def apply_median_filter(self, window_size=11, vis=False):
        """Once obtained the joint x,y,z coords.
        Apply a median filter over a temporal window to smooth the joint positions.
        Whilst doing this, create a world Trace object"""

        fx = 525.0
        fy = 525.0
        cx = 319.5
        cy = 239.5

        data, f_data, ob_states = {}, {}, {}
        self.camera_world = World_Trace()
        self.filtered_skeleton_data = {}

        for t in self.sorted_timestamps:
            self.filtered_skeleton_data[t] = {}  # initialise with all timepoints
            for joint_id, (x, y, z) in self.skeleton_data[t].items():
                try:
                    data[joint_id]["x"].append(x)
                    data[joint_id]["y"].append(y)
                    data[joint_id]["z"].append(z)
                except:
                    data[joint_id] = {"x": [x], "y": [y], "z": [z]}

        for joint_id, joint_dic in data.items():
            f_data[joint_id] = {}
            for dim, values in joint_dic.items():
                filtered_values = sp.signal.medfilt(values, window_size)  # filter
                f_data[joint_id][dim] = filtered_values

                if dim is "z" and 0 in filtered_values:
                    print "Z should never be 0. (distance to camera)."
                    vis = True

                if vis and "hand" in joint_id:
                    print joint_id
                    title1 = "input %s position: %s" % (joint_id, dim)
                    title2 = "filtered %s position: %s" % (joint_id, dim)
                    plot_the_results(values, filtered_values, title1, title2)

            # Create a QSRLib format list of Object States (for each joint id)
            for cnt, t in enumerate(self.sorted_timestamps):
                x = f_data[joint_id]["x"][cnt]
                y = f_data[joint_id]["y"][cnt]
                z = f_data[joint_id]["z"][cnt]

                # add the x2d and y2d (using filtered x,y,z data)
                # print self.uuid, t, joint_id, (x,y,z)
                x2d = int(x * fx / z * 1 + cx)
                y2d = int(y * fy / z * -1 + cy)
                self.filtered_skeleton_data[t][joint_id] = (x, y, z, x2d, y2d)  # Kept for Legacy
                try:
                    ob_states[joint_id].append(Object_State(name=joint_id, timestamp=cnt, x=x, y=y, z=z))
                except:
                    ob_states[joint_id] = [Object_State(name=joint_id, timestamp=cnt, x=x, y=y, z=z)]

        # #Add all the joint IDs into the World Trace
        for joint_id, obj in ob_states.items():
            self.camera_world.add_object_state_series(obj)

    def apply_mean_filter_old(self, window_length=3):
        """Once obtained the joint x,y,z coords.
        Apply a median filter over a temporal window to smooth the joint positions.
        Whilst doing this, create a world Trace object"""

        joints, ob_states = {}, {}
        world = World_Trace()
        window = {}
        filtered_cnt = 0

        for t in self.sorted_timestamps:
            joints[t] = {}

            for joint_id, (x, y, z) in self.skeleton_data[t].items():
                # print "jointID=", joint_id, (x,y,z)
                try:
                    window[joint_id].pop(0)
                except (IndexError, KeyError):
                    window[joint_id] = []

                window[joint_id].append((float(x), float(y), float(z)))
                avg_x, avg_y, avg_z = 0, 0, 0
                for l, point in enumerate(window[joint_id]):
                    avg_x += point[0]
                    avg_y += point[1]
                    avg_z += point[2]

                x, y, z = avg_x / float(l + 1), avg_y / float(l + 1), avg_z / float(l + 1)
                joints[t][joint_id] = (x, y, z)

                # Create a QSRLib format list of Object States (for each joint id)
                if joint_id not in ob_states.keys():
                    ob_states[joint_id] = [Object_State(name=joint_id, timestamp=filtered_cnt, x=x, y=y, z=z)]
                else:
                    ob_states[joint_id].append(Object_State(name=joint_id, timestamp=filtered_cnt, x=x, y=y, z=z))
            filtered_cnt += 1

        # #Add all the joint IDs into the World Trace
        for joint_id, obj in ob_states.items():
            world.add_object_state_series(obj)

        self.filtered_skeleton_data = joints
        self.camera_world = world

    def get_world_frame_trace(self, world_objects):
        """Accepts a dictionary of world (soma) objects.
        Adds the position of the object at each timepoint into the World Trace
        Note 'frame' is the actual detection file number. 't' is a timestamp starting from 0.
        't' is used in the World Trace."""

        ob_states = {}
        world = World_Trace()
        for t in self.sorted_timestamps:
            # Joints:
            for joint_id, (x, y, z) in self.map_frame_data[t].items():
                if joint_id not in ob_states.keys():
                    ob_states[joint_id] = [Object_State(name=joint_id, timestamp=t, x=x, y=y, z=z)]
                else:
                    ob_states[joint_id].append(Object_State(name=joint_id, timestamp=t, x=x, y=y, z=z))

            # SOMA objects
            for object, (x, y, z) in world_objects.items():
                if object not in ob_states.keys():
                    ob_states[object] = [Object_State(name=str(object), timestamp=t, x=x, y=y, z=z)]
                else:
                    ob_states[object].append(Object_State(name=str(object), timestamp=t, x=x, y=y, z=z))

            # Robot's position
            (x, y, z) = self.robot_data[t][0]
            if "robot" not in ob_states.keys():
                ob_states["robot"] = [Object_State(name="robot", timestamp=t, x=x, y=y, z=z)]
            else:
                ob_states["robot"].append(Object_State(name="robot", timestamp=t, x=x, y=y, z=z))

        for object_state in ob_states.values():
            world.add_object_state_series(object_state)
        self.map_world = world
Exemplo n.º 30
0
    parser.add_argument("qsr", help="choose qsr: %s" % options, type=str)
    parser.add_argument("-i", "--input", help="file from which to read object states", type=str)
    parser.add_argument("--validate", help="validate state chain. Only QTC", action="store_true")
    parser.add_argument("--quantisation_factor", help="quantisation factor for 0-states in qtc, or 's'-states in mos", type=float)
    parser.add_argument("--no_collapse", help="does not collapse similar adjacent states. Only QTC", action="store_true")
    parser.add_argument("--distance_threshold", help="distance threshold for qtcb <-> qtcc transition. Only QTCBC", type=float)
    parser.add_argument("-c", "--config", help="config file", type=str)
    parser.add_argument("--ros", action="store_true", default=False, help="Use ROS eco-system")
    args = parser.parse_args()

    if args.qsr in options:
        which_qsr = args.qsr
    else:
        raise ValueError("qsr not found, keywords: %s" % options)

    world = World_Trace()

    dynamic_args = {}

    if which_qsr in ["rcc2", "rcc3", "ra"]:
        dynamic_args = {which_qsr: {"quantisation_factor": args.quantisation_factor}}
        o1 = [Object_State(name="o1", timestamp=0, x=1., y=1., xsize=5., ysize=8.),
              Object_State(name="o1", timestamp=1, x=1., y=2., xsize=5., ysize=8.),
              Object_State(name="o1", timestamp=2, x=1., y=3., xsize=5., ysize=8.)]

        o2 = [Object_State(name="o2", timestamp=0, x=11., y=1., xsize=5., ysize=8.),
              Object_State(name="o2", timestamp=1, x=11., y=2., xsize=5., ysize=8.),
              Object_State(name="o2", timestamp=2, x=11., y=3., xsize=5., ysize=8.),
              Object_State(name="o2", timestamp=3, x=11., y=4., xsize=5., ysize=8.)]

        o3 = [Object_State(name="o3", timestamp=0, x=1., y=11., xsize=5., ysize=8.),
        except:
            pass
    h_state_seqs.append(h_state_seq)

    bag.close()
    r_positions.append([r_xs, r_ys])
    h_positions.append([h_xs, h_ys])

# In[61]:

# Test getting QTC_C sequence
bag_no = 0
quantisation_factor = 0.05

world = World_Trace()

h_x = [h_state_seqs[bag_no][i].x for i in range(len(h_state_seqs[bag_no]))]
h_y = [h_state_seqs[bag_no][i].y for i in range(len(h_state_seqs[bag_no]))]

r_x = [r_state_seqs[bag_no][i].x for i in range(len(r_state_seqs[bag_no]))]
r_y = [r_state_seqs[bag_no][i].y for i in range(len(r_state_seqs[bag_no]))]

# In[62]:

# Downsample state series' to 200kHz frequency
h_state_series = pd.DataFrame({
    "x": h_x,
    "y": h_y
},
                              index=[
class argd_interpreter:
    def __init__(self):

        self.world = World_Trace()
        self.options = sorted(QSRlib().qsrs_registry.keys())
        self.calculi = self.options[0]
        # if a value lies between the thresholds set for 'near' and 'far' then the relation is immediately reported as 'far'
        # the values are bound by a 'lesser than, < ' relationship
        self.distance_args = {"Near": 3.0, "Medium": 3.5, "Far": 4.0}
        self.dynamic_args = {
            "argd": {
                "qsr_relations_and_values": self.distance_args
            },
            "for_all_qsrs": {
                'qsrs_for': [('object_1', 'robot'), ('object_2', 'robot')]
            }
        }

        self.relationship_pub = rospy.Publisher("argd_realtionship",
                                                QsrMsg,
                                                queue_size=10)
        self.pose_sub1 = message_filters.Subscriber("extracted_pose1", PoseMsg)
        self.pose_sub2 = message_filters.Subscriber("extracted_pose2", PoseMsg)
        self.cln = QSRlib_ROS_Client()
        # the update rate for the ApproximateTimeSynchronizer is 0.1 and the queue_size is 10
        self.ts = message_filters.ApproximateTimeSynchronizer(
            [self.pose_sub1, self.pose_sub2], 3, 0.1)
        self.ts.registerCallback(self.relations_callback)

    def relations_callback(self, extracted_pose1, extracted_pose2):
        o1 = [
            Object_State(name="object_1",
                         timestamp=extracted_pose1.header.stamp.secs,
                         x=extracted_pose1.pose_vec.position.x,
                         y=extracted_pose1.pose_vec.position.z)
        ]
        o2 = [
            Object_State(name="robot",
                         timestamp=extracted_pose1.header.stamp.secs,
                         x=0,
                         y=0)
        ]
        o3 = [
            Object_State(name="object_2",
                         timestamp=extracted_pose1.header.stamp.secs,
                         x=extracted_pose2.pose_vec.position.x,
                         y=extracted_pose2.pose_vec.position.z)
        ]

        self.world.add_object_state_series(o1)
        self.world.add_object_state_series(o2)
        self.world.add_object_state_series(o3)

        qsrlib_request_message = QSRlib_Request_Message(
            which_qsr=self.calculi,
            input_data=self.world,
            dynamic_args=self.dynamic_args)
        req = self.cln.make_ros_request_message(qsrlib_request_message)
        res = self.cln.request_qsrs(req)
        qsrlib_response_message = pickle.loads(res.data)

        argd_relation_msg = QsrMsg()
        argd_relation_msg.selected_qsr = str(self.calculi)
        argd_relation_msg.marker_id1 = extracted_pose1.marker_id
        argd_relation_msg.marker_id2 = extracted_pose2.marker_id

        for time in qsrlib_response_message.qsrs.get_sorted_timestamps():
            argd_relation_msg.message_time = extracted_pose1.header.stamp.secs
            object_key = str(time) + " = "
            value_key = str(time) + " = "

            for key, value in zip(
                    qsrlib_response_message.qsrs.trace[time].qsrs.keys(),
                    qsrlib_response_message.qsrs.trace[time].qsrs.values()):
                object_key += str(key) + ';'
                argd_relation_msg.objects = object_key
                value_key += str(value.qsr)
                argd_relation_msg.relationship = value_key
            self.relationship_pub.publish(argd_relation_msg)
            print(argd_relation_msg)
Exemplo n.º 33
0
class event(object):
    """Event object class"""
    def __init__(self, uuid, dir_, start, end, label, waypoint=None):

        self.uuid = uuid
        # self.start_frame, self.end_frame = self.get_last_frame()
        # self.waypoint = waypoint
        self.dir = dir_
        self.start_frame = start
        self.end_frame = end
        self.label = label
        self.sorted_timestamps = []
        # self.sorted_ros_timestamps = []
        self.bad_timepoints = {}  #use for filtering
        self.skeleton_data = {
        }  #type: dict[timepoint][joint_id]= (x, y, z, x2d, y2d)
        self.map_frame_data = {
        }  #type: dict[timepoint][joint_id]= (x, y, z, x2d, y2d)
        self.robot_data = {
        }  #type: dict[timepoint][joint_id]= ((x, y, z), (roll, pitch, yaw))
        # self.world = World_Trace()

    def apply_median_filter(self, window_size=11, vis=False):
        """Once obtained the joint x,y,z coords.
        Apply a median filter over a temporal window to smooth the joint positions.
        Whilst doing this, create a world Trace object"""

        fx = 525.0
        fy = 525.0
        cx = 319.5
        cy = 239.5

        data, f_data, ob_states = {}, {}, {}
        self.camera_world = World_Trace()
        self.filtered_skeleton_data = {}

        for t in self.sorted_timestamps:
            self.filtered_skeleton_data[t] = {
            }  # initialise with all timepoints
            for joint_id, (x, y, z) in self.skeleton_data[t].items():
                try:
                    data[joint_id]["x"].append(x)
                    data[joint_id]["y"].append(y)
                    data[joint_id]["z"].append(z)
                except KeyError:
                    data[joint_id] = {"x": [x], "y": [y], "z": [z]}

        for joint_id, joint_dic in data.items():
            f_data[joint_id] = {}
            for dim, values in joint_dic.items():
                filtered_values = sp.signal.medfilt(values,
                                                    window_size)  # filter
                f_data[joint_id][dim] = filtered_values

                if dim is "z" and 0 in filtered_values:
                    print ">> Z should never be 0. (distance to camera)."
                    return False
                    #vis = True

                if vis and "hand" in joint_id:
                    print joint_id
                    title1 = 'input %s position: %s' % (joint_id, dim)
                    title2 = 'filtered %s position: %s' % (joint_id, dim)
                    #plot_the_results(values, filtered_values, title1, title2)

            # Create a QSRLib format list of Object States (for each joint id)
            for cnt, t in enumerate(self.sorted_timestamps):
                x = f_data[joint_id]["x"][cnt]
                y = f_data[joint_id]["y"][cnt]
                z = f_data[joint_id]["z"][cnt]

                # add the x2d and y2d (using filtered x,y,z data)
                # print self.uuid, t, joint_id, (x,y,z)
                try:
                    x2d = int(x * fx / z * 1 + cx)
                    y2d = int(y * fy / z * -1 + cy)
                except ValueError:
                    print "ValueError for %s at frame: %s. t:%s" % (self.uuid,
                                                                    cnt, t)
                    x2d = 0
                    y2d = 0

                self.filtered_skeleton_data[t][joint_id] = (
                    x, y, z, x2d, y2d)  # Kept for Legacy
                try:
                    ob_states[joint_id].append(
                        Object_State(name=joint_id,
                                     timestamp=cnt,
                                     x=x,
                                     y=y,
                                     z=z))
                except KeyError:
                    ob_states[joint_id] = [
                        Object_State(name=joint_id,
                                     timestamp=cnt,
                                     x=x,
                                     y=y,
                                     z=z)
                    ]

        # #Add all the joint IDs into the World Trace
        for joint_id, obj in ob_states.items():
            self.camera_world.add_object_state_series(obj)
        return True

    def apply_mean_filter_old(self, window_length=3):
        """Once obtained the joint x,y,z coords.
        Apply a median filter over a temporal window to smooth the joint positions.
        Whilst doing this, create a world Trace object"""

        joints, ob_states = {}, {}
        world = World_Trace()
        window = {}
        filtered_cnt = 0

        for t in self.sorted_timestamps:
            joints[t] = {}

            for joint_id, (x, y, z) in self.skeleton_data[t].items():
                # print "jointID=", joint_id, (x,y,z)
                try:
                    window[joint_id].pop(0)
                except (IndexError, KeyError):
                    window[joint_id] = []

                window[joint_id].append((float(x), float(y), float(z)))
                avg_x, avg_y, avg_z = 0, 0, 0
                for l, point in enumerate(window[joint_id]):
                    avg_x += point[0]
                    avg_y += point[1]
                    avg_z += point[2]

                x, y, z = avg_x / float(l + 1), avg_y / float(
                    l + 1), avg_z / float(l + 1)
                joints[t][joint_id] = (x, y, z)

                #Create a QSRLib format list of Object States (for each joint id)
                if joint_id not in ob_states.keys():
                    ob_states[joint_id] = [
                        Object_State(name=joint_id,
                                     timestamp=filtered_cnt,
                                     x=x,
                                     y=y,
                                     z=z)
                    ]
                else:
                    ob_states[joint_id].append(
                        Object_State(name=joint_id,
                                     timestamp=filtered_cnt,
                                     x=x,
                                     y=y,
                                     z=z))
            filtered_cnt += 1

        # #Add all the joint IDs into the World Trace
        for joint_id, obj in ob_states.items():
            world.add_object_state_series(obj)

        self.filtered_skeleton_data = joints
        self.camera_world = world

    def get_world_frame_trace(self, world_objects):
        """Accepts a dictionary of world (soma) objects.
        Adds the position of the object at each timepoint into the World Trace
        Note 'frame' is the actual detection file number. 't' is a timestamp starting from 0.
        't' is used in the World Trace."""

        ob_states = {}
        world = World_Trace()
        # print ">>", self.map_frame_data.keys()
        for t, frame in enumerate(self.sorted_timestamps):
            #Joints:
            for joint_id, (x, y, z) in self.map_frame_data[frame].items():
                if joint_id not in ob_states.keys():
                    ob_states[joint_id] = [
                        Object_State(name=joint_id, timestamp=t, x=x, y=y, z=z)
                    ]
                else:
                    ob_states[joint_id].append(
                        Object_State(name=joint_id, timestamp=t, x=x, y=y,
                                     z=z))

            # SOMA objects
            for object, (x, y, z) in world_objects.items():
                if object not in ob_states.keys():
                    ob_states[object] = [
                        Object_State(name=str(object),
                                     timestamp=t,
                                     x=x,
                                     y=y,
                                     z=z)
                    ]
                else:
                    ob_states[object].append(
                        Object_State(name=str(object),
                                     timestamp=t,
                                     x=x,
                                     y=y,
                                     z=z))

            # Robot's position
            (x, y, z) = self.robot_data[frame][0]
            if 'robot' not in ob_states.keys():
                ob_states['robot'] = [
                    Object_State(name='robot', timestamp=t, x=x, y=y, z=z)
                ]
            else:
                ob_states['robot'].append(
                    Object_State(name='robot', timestamp=t, x=x, y=y, z=z))

        for object_state in ob_states.values():
            world.add_object_state_series(object_state)
        self.map_world = world
Exemplo n.º 34
0
    def apply_median_filter(self, window_size=11, vis=False):
        """Once obtained the joint x,y,z coords.
        Apply a median filter over a temporal window to smooth the joint positions.
        Whilst doing this, create a world Trace object"""

        fx = 525.0
        fy = 525.0
        cx = 319.5
        cy = 239.5

        data, f_data, ob_states = {}, {}, {}
        self.camera_world = World_Trace()
        self.filtered_skeleton_data = {}

        for t in self.sorted_timestamps:
            self.filtered_skeleton_data[t] = {
            }  # initialise with all timepoints
            for joint_id, (x, y, z) in self.skeleton_data[t].items():
                try:
                    data[joint_id]["x"].append(x)
                    data[joint_id]["y"].append(y)
                    data[joint_id]["z"].append(z)
                except KeyError:
                    data[joint_id] = {"x": [x], "y": [y], "z": [z]}

        for joint_id, joint_dic in data.items():
            f_data[joint_id] = {}
            for dim, values in joint_dic.items():
                filtered_values = sp.signal.medfilt(values,
                                                    window_size)  # filter
                f_data[joint_id][dim] = filtered_values

                if dim is "z" and 0 in filtered_values:
                    print ">> Z should never be 0. (distance to camera)."
                    return False
                    #vis = True

                if vis and "hand" in joint_id:
                    print joint_id
                    title1 = 'input %s position: %s' % (joint_id, dim)
                    title2 = 'filtered %s position: %s' % (joint_id, dim)
                    #plot_the_results(values, filtered_values, title1, title2)

            # Create a QSRLib format list of Object States (for each joint id)
            for cnt, t in enumerate(self.sorted_timestamps):
                x = f_data[joint_id]["x"][cnt]
                y = f_data[joint_id]["y"][cnt]
                z = f_data[joint_id]["z"][cnt]

                # add the x2d and y2d (using filtered x,y,z data)
                # print self.uuid, t, joint_id, (x,y,z)
                try:
                    x2d = int(x * fx / z * 1 + cx)
                    y2d = int(y * fy / z * -1 + cy)
                except ValueError:
                    print "ValueError for %s at frame: %s. t:%s" % (self.uuid,
                                                                    cnt, t)
                    x2d = 0
                    y2d = 0

                self.filtered_skeleton_data[t][joint_id] = (
                    x, y, z, x2d, y2d)  # Kept for Legacy
                try:
                    ob_states[joint_id].append(
                        Object_State(name=joint_id,
                                     timestamp=cnt,
                                     x=x,
                                     y=y,
                                     z=z))
                except KeyError:
                    ob_states[joint_id] = [
                        Object_State(name=joint_id,
                                     timestamp=cnt,
                                     x=x,
                                     y=y,
                                     z=z)
                    ]

        # #Add all the joint IDs into the World Trace
        for joint_id, obj in ob_states.items():
            self.camera_world.add_object_state_series(obj)
        return True
Exemplo n.º 35
0
    def get_world_frame_trace(self, world_objects):
        """Accepts a dictionary of world (soma) objects.
        Adds the position of the object at each timepoint into the World Trace"""
        self.subj_world_trace = {}
        for subj in self.skeleton_map:
            ob_states = {}
            world = World_Trace()
            map_frame_data = self.skeleton_map[subj]
            for joint_id in map_frame_data.keys():
                #Joints:
                for t in xrange(self.frames):
                    x = map_frame_data[joint_id][t][0]
                    y = map_frame_data[joint_id][t][1]
                    z = map_frame_data[joint_id][t][2]
                    if joint_id not in ob_states.keys():
                        ob_states[joint_id] = [
                            Object_State(name=joint_id,
                                         timestamp=t + 1,
                                         x=x,
                                         y=y,
                                         z=z)
                        ]
                    else:
                        ob_states[joint_id].append(
                            Object_State(name=joint_id,
                                         timestamp=t + 1,
                                         x=x,
                                         y=y,
                                         z=z))

                # SOMA objects
            for t in xrange(self.frames):
                for object, (x, y, z) in world_objects.items():
                    if object not in ob_states.keys():
                        ob_states[object] = [
                            Object_State(name=str(object),
                                         timestamp=t + 1,
                                         x=x,
                                         y=y,
                                         z=z)
                        ]
                    else:
                        ob_states[object].append(
                            Object_State(name=str(object),
                                         timestamp=t + 1,
                                         x=x,
                                         y=y,
                                         z=z))

                # # Robot's position
                # (x,y,z) = self.robot_data[t][0]
                # if 'robot' not in ob_states.keys():
                #     ob_states['robot'] = [Object_State(name='robot', timestamp=t, x=x, y=y, z=z)]
                # else:
                #     ob_states['robot'].append(Object_State(name='robot', timestamp=t, x=x, y=y, z=z))

            for obj, object_state in ob_states.items():
                world.add_object_state_series(object_state)

            # get world trace for each person

            region = self.soma_roi_config[self.waypoint]
            self.subj_world_trace[subj] = self.get_object_frame_qsrs(
                world, self.objects[region])
    mos_qsrs_for = ["o1", "o2"]
    tpcc_qsrs_for = [("o1", "o2", "o3")]

    object_types = {"o1": "Unit", "o2": "Beta", "o1-o2": "Score"}

    if args.qsr in options:
        if args.qsr != "multiple":
            which_qsr = args.qsr
        else:
            which_qsr = multiple
    elif args.qsr == "hardcoded":
        which_qsr = ["qtcbs", "argd", "mos"]
    else:
        raise ValueError("qsr not found, keywords: %s" % options)

    world = World_Trace()

    dynamic_args = {
        "qtcbs": {
            "quantisation_factor": args.quantisation_factor,
            "validate": args.validate,
            "no_collapse": args.no_collapse,
            "qsrs_for": qtcbs_qsrs_for
        },
        "argd": {
            "qsr_relations_and_values": args.distance_threshold,
            "qsrs_for": argd_qsrs_for
        },
        "mos": {
            "qsrs_for": mos_qsrs_for
        },
Exemplo n.º 37
0
class argd_interpreter:
    def __init__(self):

        self.world = World_Trace()
        self.options = sorted(QSRlib().qsrs_registry.keys())
        self.calculi = self.options[0]
        # any value in between goes to the upper bound
        self.dynamic_args = {
            "argd": {
                "qsr_relations_and_values": {
                    "Near": 0.36,
                    "Medium": .40,
                    "Far": .45
                }
            }
        }

        self.relationship_pub = rospy.Publisher("argd_realtionship",
                                                QsrMsg,
                                                queue_size=2)
        self.pose_sub1 = rospy.Subscriber("extracted_pose1", PoseMsg,
                                          self.relations_callback)
        self.cln = QSRlib_ROS_Client()
        # the update rate for the ApproximateTimeSynchronizer is 0.1 and the queue_size is 10

    def relations_callback(self, extracted_pose1):
        print('------------------------')
        print(extracted_pose1)
        print("========================")
        o1 = [
            Object_State(name="object_1",
                         timestamp=extracted_pose1.header.stamp.secs,
                         x=extracted_pose1.pose_vec.position.x,
                         y=extracted_pose1.pose_vec.position.z)
        ]
        o2 = [
            Object_State(name="robot",
                         timestamp=extracted_pose1.header.stamp.secs,
                         x=0,
                         y=0)
        ]

        self.world.add_object_state_series(o1)
        self.world.add_object_state_series(o2)

        qsrlib_request_message = QSRlib_Request_Message(
            which_qsr=self.calculi,
            input_data=self.world,
            dynamic_args=self.dynamic_args)
        req = self.cln.make_ros_request_message(qsrlib_request_message)
        res = self.cln.request_qsrs(req)
        qsrlib_response_message = pickle.loads(res.data)

        argd_relation_msg = QsrMsg()
        argd_relation_msg.selected_qsr = str(self.calculi)
        argd_relation_msg.marker_id1 = extracted_pose1.marker_id

        for time in qsrlib_response_message.qsrs.get_sorted_timestamps():
            argd_relation_msg.message_time = extracted_pose1.header.stamp.nsecs

            for key, value in zip(
                    qsrlib_response_message.qsrs.trace[time].qsrs.keys(),
                    qsrlib_response_message.qsrs.trace[time].qsrs.values()):
                argd_relation_msg.objects = key
                argd_relation_msg.relationship = str(value.qsr)
                self.relationship_pub.publish(argd_relation_msg)
                print(argd_relation_msg)
Exemplo n.º 38
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"]()))