def get_map_frame_qsrs(file, world_trace, dynamic_args):
"""create QSRs between the person and the robot in map frame"""
qsrs_for = [('robot', 'torso')]
dynamic_args['qtcbs'] = {
"qsrs_for": qsrs_for,
"quantisation_factor": 0.0,
"validate": False,
"no_collapse": True
}
dynamic_args["qstag"] = {
"params": {
"min_rows": 1,
"max_rows": 1,
"max_eps": 3
}
}
qsrlib = QSRlib()
req = QSRlib_Request_Message(which_qsr="qtcbs",
input_data=world_trace,
dynamic_args=dynamic_args)
qsr_map_frame = qsrlib.request_qsrs(req_msg=req)
print " ", file, "episodes = "
for i in qsr_map_frame.qstag.episodes:
print i
return qsr_map_frame
def get_object_frame_qsrs(self, world_trace, objects):
joint_types = {
'left_hand': 'hand',
'right_hand': 'hand',
'head-torso': 'tpcc-plane'
}
joint_types_plus_objects = joint_types.copy()
for object in objects:
generic_object = "_".join(object.split("_")[:-1])
joint_types_plus_objects[object] = generic_object
#print joint_types_plus_objects
"""create QSRs between the person's joints and the soma objects in map frame"""
qsrs_for = []
for ob in objects:
qsrs_for.append((str(ob), 'left_hand'))
qsrs_for.append((str(ob), 'right_hand'))
#qsrs_for.append((str(ob), 'torso'))
dynamic_args = {}
# dynamic_args['argd'] = {"qsrs_for": qsrs_for, "qsr_relations_and_values": {'Touch': 0.25, 'Near': 0.5, 'Ignore': 10}}
# dynamic_args['qtcbs'] = {"qsrs_for": qsrs_for, "quantisation_factor": 0.05, "validate": False, "no_collapse": True} # Quant factor is effected by filters to frame rate
# dynamic_args["qstag"] = {"object_types": joint_types_plus_objects, "params": {"min_rows": 1, "max_rows": 1, "max_eps": 2}}
dynamic_args['argd'] = {
"qsrs_for": qsrs_for,
"qsr_relations_and_values": {
'Touch': 0.5,
'Near': 0.75,
'Medium': 1.5,
'Ignore': 10
}
}
# dynamic_args['argd'] = {"qsrs_for": qsrs_for, "qsr_relations_and_values": {'Touch': 0.2, 'Ignore': 10}}
dynamic_args['qtcbs'] = {
"qsrs_for": qsrs_for,
"quantisation_factor": 0.01,
"validate": False,
"no_collapse": True
} # Quant factor is effected by filters to frame rate
dynamic_args["qstag"] = {
"object_types": joint_types_plus_objects,
"params": {
"min_rows": 1,
"max_rows": 1,
"max_eps": 2
}
}
qsrlib = QSRlib()
req = QSRlib_Request_Message(which_qsr=["argd", "qtcbs"],
input_data=world_trace,
dynamic_args=dynamic_args)
#req = QSRlib_Request_Message(which_qsr="argd", input_data=world_trace, dynamic_args=dynamic_args)
ret = qsrlib.request_qsrs(req_msg=req)
# for ep in ret.qstag.episodes:
# print ep
#
return ret
def __init__(self):
# ................................................................
# read ros parameters
self.loadROSParams()
# ................................................................
# Other config params and atributes
self.closest_human_pose = None
self.prev_closest_human_pose = None
# qsrlib data...
self.qsrlib = QSRlib()
# TODO: add as parameter
self.which_qsr = 'qtccs'
# TODO: add as parameter
self.dynammic_args = {
"qtccs": {
"no_collapse": True,
"quantisation_factor": 0.01,
"validate": False,
"qsrs_for": [("human_os", "robot_os")]
}
}
# sensible default values ...
self.closest_human_pose = None
# ................................................................
# start ros subs/pubs/servs....
self.initROS()
rospy.loginfo("Node [" + rospy.get_name() + "] entering spin...")
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
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 __init__(self, *args):
super(AbstractClass_UnitTest, self).__init__(*args)
self._unique_id = ""
self._worlds = {"data1": load_input_data1(),
"data2": load_input_data2(),
"data3": load_input_data3(),
"data4": load_input_data4(),
"data2_first100": load_input_data2_first100(),
"data3_first100": load_input_data3_first100(),
"data4_first100": load_input_data4_first100()}
self._qsrlib = QSRlib()
def get_object_frame_qsrs(file, world_trace, objects, joint_types,
dynamic_args):
"""create QSRs between the person's joints and the soma objects in map frame"""
qsrs_for = []
for ob in objects:
qsrs_for.append((str(ob), 'left_hand'))
#qsrs_for.append((str(ob), 'right_hand'))
#qsrs_for.append((str(ob), 'torso'))
dynamic_args['argd'] = {
"qsrs_for": qsrs_for,
"qsr_relations_and_values": {
'Touch': 0.5,
'Near': 0.75,
'Medium': 1.5,
'Ignore': 10
}
}
# dynamic_args['argd'] = {"qsrs_for": qsrs_for, "qsr_relations_and_values": {'Touch': 0.2, 'Ignore': 10}}
dynamic_args['qtcbs'] = {
"qsrs_for": qsrs_for,
"quantisation_factor": 0.05,
"validate": False,
"no_collapse": True
} # Quant factor is effected by filters to frame rate
dynamic_args["qstag"] = {
"object_types": joint_types,
"params": {
"min_rows": 1,
"max_rows": 1,
"max_eps": 2
}
}
qsrlib = QSRlib()
req = QSRlib_Request_Message(which_qsr=["argd", "qtcbs"],
input_data=world_trace,
dynamic_args=dynamic_args)
# req = QSRlib_Request_Message(which_qsr="argd", input_data=world_trace, dynamic_args=dynamic_args)
qsr_object_frame = qsrlib.request_qsrs(req_msg=req)
for ep in qsr_object_frame.qstag.episodes:
print ep
for cnt, h in zip(qsr_object_frame.qstag.graphlets.histogram,
qsr_object_frame.qstag.graphlets.code_book):
print "\n", cnt, h, qsr_object_frame.qstag.graphlets.graphlets[h]
return qsr_object_frame
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
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 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 __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()
def __init__(self, node_name="qsr_lib"):
"""Constructor.
:param node_name: The QSRlib ROS server node name.
:type node_name: str
"""
self.qsrlib = QSRlib()
""":class:`QSRlib <qsrlib.qsrlib.QSRlib>`: QSRlib main object."""
self.node_name = node_name
"""str: QSRlib ROS server node name."""
rospy.init_node(self.node_name)
self.service_topic_names = {"request": self.node_name + "/request"}
"""dict: Holds the service topic names."""
self.srv_qsrs_request = rospy.Service(
self.service_topic_names["request"], RequestQSRs,
self.handle_request_qsrs)
"""rospy.impl.tcpros_service.Service: QSRlib ROS service."""
rospy.loginfo("QSRlib_ROS_Server up and running, listening to: %s" %
self.service_topic_names["request"])
def get_joint_frame_qsrs(file, world_trace, joint_types, dynamic_args):
qsrs_for = [('head', 'torso',
ob) if ob not in ['head', 'torso'] and ob != 'head-torso' else
() for ob in joint_types.keys()]
dynamic_args['tpcc'] = {"qsrs_for": qsrs_for}
dynamic_args["qstag"] = {
"object_types": joint_types,
"params": {
"min_rows": 1,
"max_rows": 1,
"max_eps": 3
}
}
qsrlib = QSRlib()
req = QSRlib_Request_Message(which_qsr="tpcc",
input_data=world_trace,
dynamic_args=dynamic_args)
qsr_joints_frame = qsrlib.request_qsrs(req_msg=req)
# for i in qsr_joints_frame.qstag.episodes:
# print i
return qsr_joints_frame
def test(self):
QSRlib(help=False)
# arguments
which_qsr = 'qtcbcs'
state_space = QSR_QTC_BC_Simplified().all_possible_relations
states_len = [len(si.replace(',', '')) for si in state_space]
dynammic_args = {
"qtcbcs": {
"no_collapse": True,
"quantisation_factor": 0.01,
"validate": False,
"qsrs_for": [("human_os", "robot_os")]
}
}
# create a QSRlib object if there isn't one already
qsrlib = QSRlib()
qsr_types = qsrlib.qsrs_registry.keys()
# Print available qsr models
# qsrlib._QSRlib__print_qsrs_available()
# Print available states
# qsrlib._QSRlib__print_qsrs_available()
## Inputs.
# time increment we are studying
td = 0.5
#- Human state vector: position (xh,yh,hh), and speed (vh,wh)
xh0 = 0.
yh0 = 1.
def worker_qsrs(chunk):
(file_, path, config) = chunk
#print "\n", path, file_
e = utils.load_e(path, file_)
dynamic_args = {}
try:
dynamic_args['argd'] = config['argd_args']
dynamic_args['qtcbs'] = config['qtcbs_args']
dynamic_args["qstag"] = {"params": config['qstag_args']}
dynamic_args['filters'] = {
"median_filter": {
"window": config['qsr_mean_window']
}
} # This has been updated since ECAI paper.
except KeyError:
print "check argd, qtcbs, qstag parameters in config file"
joint_types = {
'head': 'head',
'torso': 'torso',
'left_hand': 'hand',
'right_hand': 'hand',
'left_knee': 'knee',
'right_knee': 'knee',
'left_shoulder': 'shoulder',
'right_shoulder': 'shoulder',
'head-torso': 'tpcc-plane'
}
object_types = joint_types.copy()
for ob, pos in e.objects.items():
try:
generic_object = "_".join(ob.split("_")[:-1])
object_types[ob] = generic_object
# print "object generic", generic_object
except:
print "didnt add:", object
dynamic_args["qstag"]["object_types"] = object_types
# for i,j in object_types.items():
# print ">", i,j
"""1. CREATE QSRs FOR joints & Object """
qsrlib = QSRlib()
qsrs_for = []
for ob, pos in e.objects.items():
qsrs_for.append((str(ob), 'left_hand'))
qsrs_for.append((str(ob), 'right_hand'))
qsrs_for.append((str(ob), 'torso'))
dynamic_args['argd']["qsrs_for"] = qsrs_for
dynamic_args['qtcbs']["qsrs_for"] = qsrs_for
# for (i,j) in qsrs_for:
# print ">", (i,j)
req = QSRlib_Request_Message(config['which_qsr'],
input_data=e.map_world,
dynamic_args=dynamic_args)
e.qsr_object_frame = qsrlib.request_qsrs(req_msg=req)
# print ">", e.qsr_object_frame.qstag.graphlets.histogram
# for i in e.qsr_object_frame.qstag.episodes:
# print i
# sys.exit(1)
"""2. CREATE QSRs for joints - TPCC"""
# print "TPCC: ",
# # e.qsr_joint_frame = get_joint_frame_qsrs(file, e.camera_world, joint_types, dynamic_args)
# qsrs_for = [('head', 'torso', ob) if ob not in ['head', 'torso'] and ob != 'head-torso' else () for ob in joint_types.keys()]
# dynamic_args['tpcc'] = {"qsrs_for": qsrs_for}
# dynamic_args["qstag"]["params"] = {"min_rows": 1, "max_rows": 2, "max_eps": 4}
# qsrlib = QSRlib()
# req = QSRlib_Request_Message(which_qsr="tpcc", input_data=e.camera_world, dynamic_args=dynamic_args)
# e.qsr_joints_frame = qsrlib.request_qsrs(req_msg=req)
# # pretty_print_world_qsr_trace("tpcc", e.qsr_joints_frame)
# # print e.qsr_joints_frame.qstag.graphlets.histogram
utils.save_event(e, "Learning/QSR_Worlds")
str(qsrlib_response_message.req_received_at) + " and finished at " +
str(qsrlib_response_message.req_finished_at))
print("---")
print("timestamps:", qsrlib_response_message.qsrs.get_sorted_timestamps())
print("Response is:")
for t in qsrlib_response_message.qsrs.get_sorted_timestamps():
foo = str(t) + ": "
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) + "; "
print(foo)
if __name__ == "__main__":
options = sorted(QSRlib().qsrs_registry.keys()) + ["multiple"]
multiple = options[:]
multiple.remove("multiple")
multiple.remove("argd")
multiple.remove("argprobd")
multiple.remove("ra")
multiple.remove("mwe")
parser = argparse.ArgumentParser()
parser.add_argument("qsr",
help="choose qsr: %s" % options,
type=str,
default='qtcbs')
parser.add_argument("--ros",
action="store_true",
default=False,
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
def __init__(self):
# ................................................................
# read ros parameters
self.loadROSParams()
# ................................................................
# Other config params and atributes
self.data_lock = Lock()
# states from mpc node
self.state = None
self.prev_state = None
# timestamp to get speeds...
self.prev_time = None
# qsrlib data...
self.qsrlib = QSRlib()
# TODO: add as parameter
self.which_qsr = 'qtccs'
# TODO: add as parameter
self.dynammic_args = {
"qtccs": {
"no_collapse": True,
"quantisation_factor": 0.01,
"validate": False,
"qsrs_for": [("human_os", "robot_os")]
}
}
# TODO: add as parameter
# these should be valid which_qsr states ...
self.state_forbidden = ['-,-,-,-']
# sensible default values ...
self.closest_human_pose = None
self.closest_human_twist = None
self.constraint_v = 0
self.constraint_w = 0
self.robot_x = 0
self.robot_y = 0
self.robot_h = 0
self.robot_v = 0
self.robot_w = 0
# TODO: add as parameter
self.allowed_min_v = 0
self.allowed_max_v = 0
self.allowed_min_w = 0
self.allowed_max_w = 0
# What possible speeds we can achieve
self.vr_space_inc = np.linspace(-self.max_vr, self.max_vr,
self.num_speed_points)
self.wr_space_inc = np.linspace(-self.max_wr, self.max_wr,
self.num_speed_points)
# ................................................................
# start ros subs/pubs/servs....
self.initROS()
rospy.loginfo("Node [" + rospy.get_name() + "] entering spin...")
r = rospy.Rate(2)
while not rospy.is_shutdown():
self.updateVisuals()
self.updateConstraints()
self.sendNewConstraints()
r.sleep()