def __init__(self):
self._transform_listener = tf.TransformListener()
self._joint_state_listener = JointStateListener()
self._battery_monitor = BatteryMonitor()
self._query_functions = {
'get_transform':
QueryFunction(self.get_transform, [str, str]),
'transform_point_to_frame':
QueryFunction(self.transform_point_to_frame, [Point, str, str]),
'point_within_bbox':
QueryFunction(self.point_within_bbox, [Point, Point, Vector3]),
'is_charging':
QueryFunction(self.is_charging, []),
'and':
QueryFunction(self.logical_and, [bool, bool]),
'or':
QueryFunction(self.logical_or, [bool, bool])
}
def __init__(self):
self._transform_listener = tf.TransformListener()
self._joint_state_listener = JointStateListener()
self._battery_monitor = BatteryMonitor()
self._query_functions = {
'get_transform' : QueryFunction(self.get_transform, [str, str]),
'transform_point_to_frame' : QueryFunction(self.transform_point_to_frame, [Point, str, str]),
'point_within_bbox' : QueryFunction(self.point_within_bbox, [Point, Point, Vector3]),
'is_charging' : QueryFunction(self.is_charging, []),
'and' : QueryFunction(self.logical_and, [bool, bool]),
'or' : QueryFunction(self.logical_or, [bool, bool])
}
class QueryEngine:
def __init__(self):
self._transform_listener = tf.TransformListener()
self._joint_state_listener = JointStateListener()
self._battery_monitor = BatteryMonitor()
self._query_functions = {
'get_transform':
QueryFunction(self.get_transform, [str, str]),
'transform_point_to_frame':
QueryFunction(self.transform_point_to_frame, [Point, str, str]),
'point_within_bbox':
QueryFunction(self.point_within_bbox, [Point, Point, Vector3]),
'is_charging':
QueryFunction(self.is_charging, []),
'and':
QueryFunction(self.logical_and, [bool, bool]),
'or':
QueryFunction(self.logical_or, [bool, bool])
}
def get_query_functions(self):
return self._query_functions
def evaluate_query(self, query, args):
rospy.loginfo('Query: "%s"' % query)
# parse the query
parsed_query = parse_query(query)
rospy.loginfo('Parsed query: "%s"' % parsed_query)
# create a map of argument names -> values
arg_dict = {}
for arg in args:
arg_dict[arg.name] = arg.value
# evaluate the query
res = self.evaluate_expression(parsed_query, arg_dict)
# convert the result of the query into JSON
return to_json(res)
def evaluate_expression(self, expression, arg_dict):
if isinstance(expression, QueryFunctionCall):
return self.evaluate_function(expression, arg_dict)
else:
raise ValueError('Unknown expression type')
def evaluate_function(self, func_call, arg_dict):
func_name = func_call.name
rospy.loginfo('Evaluating function %s' % func_name)
if func_name not in self._query_functions:
raise ValueError('Unknown query function %s' % func_name)
func = self._query_functions[func_name]
# assuming for now that arg is a variable, not the result of another expression
args = []
for arg_i, arg in enumerate(func_call.args):
if isinstance(arg, QueryFunctionCall):
# this argument is another function, recurse and evaluate it
arg_val = self.evaluate_function(arg, arg_dict)
args.append(arg_val)
elif isinstance(arg, str):
# this argument is a variable (as json). convert it to a ROS msg type
try:
arg_json_str = arg_dict[arg]
except KeyError:
raise ValueError('No value specified for query arg %s' %
arg)
rospy.loginfo(' arg %s: %s' % (arg, arg_json_str))
# lookup the class for this argument, using the QueryFunction declaration
arg_class = func.arg_types[arg_i]
# convert the value for this argument from JSON to a ROS message
args.append(from_json(arg_json_str, arg_class()))
else:
raise ValueError('Improper argument type for function')
# evaluate the function
return func.func(*args)
def get_transform(self, current_frame, desired_frame):
try:
(trans, rot) = self._transform_listener.lookupTransform(
current_frame, desired_frame, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException):
raise
return Transform(Vector3(*trans), Quaternion(*rot))
def transform_point_to_frame(self, point, current_frame, desired_frame):
point_arr_h = np.array([point.x, point.y, point.z, 1.0])
transform_arr = conv.transform_to_array(
self.get_transform(current_frame, desired_frame))
new_point_arr_h = np.dot(transform_arr, point_arr_h)
new_point_arr = new_point_arr_h[:3] / new_point_arr_h[3]
return Point(*new_point_arr)
def is_charging(self):
return self._battery_monitor.is_charging()
def logical_and(self, arg1, arg2):
return arg1 and arg2
def logical_or(self, arg1, arg2):
return arg1 or arg2
def point_within_bbox(self, point, bbox_origin, bbox_size):
'''
Args:
point - geometry_msgs.msg.Point
bbox_origin - geometry_msgs.msg.Point
bbox_size - geometry_msgs.msg.Vector3
Returns:
(bool) - whether point is within bounding box.
'''
return ((point.x >= bbox_origin.x) and (point.y >= bbox_origin.y)
and (point.z >= bbox_origin.z)
and (point.x <= (bbox_origin.x + bbox_size.x))
and (point.y <= (bbox_origin.y + bbox_size.y))
and (point.z <= (bbox_origin.z + bbox_size.z)))
class QueryEngine:
def __init__(self):
self._transform_listener = tf.TransformListener()
self._joint_state_listener = JointStateListener()
self._battery_monitor = BatteryMonitor()
self._query_functions = {
'get_transform' : QueryFunction(self.get_transform, [str, str]),
'transform_point_to_frame' : QueryFunction(self.transform_point_to_frame, [Point, str, str]),
'point_within_bbox' : QueryFunction(self.point_within_bbox, [Point, Point, Vector3]),
'is_charging' : QueryFunction(self.is_charging, []),
'and' : QueryFunction(self.logical_and, [bool, bool]),
'or' : QueryFunction(self.logical_or, [bool, bool])
}
def get_query_functions(self):
return self._query_functions
def evaluate_query(self, query, args):
rospy.loginfo('Query: "%s"' % query)
# parse the query
parsed_query = parse_query(query)
rospy.loginfo('Parsed query: "%s"' % parsed_query)
# create a map of argument names -> values
arg_dict = {}
for arg in args:
arg_dict[arg.name] = arg.value
# evaluate the query
res = self.evaluate_expression(parsed_query, arg_dict)
# convert the result of the query into JSON
return to_json(res)
def evaluate_expression(self, expression, arg_dict):
if isinstance(expression, QueryFunctionCall):
return self.evaluate_function(expression, arg_dict)
else:
raise ValueError('Unknown expression type')
def evaluate_function(self, func_call, arg_dict):
func_name = func_call.name
rospy.loginfo('Evaluating function %s' % func_name)
if func_name not in self._query_functions:
raise ValueError('Unknown query function %s' % func_name)
func = self._query_functions[func_name]
# assuming for now that arg is a variable, not the result of another expression
args = []
for arg_i, arg in enumerate(func_call.args):
if isinstance(arg, QueryFunctionCall):
# this argument is another function, recurse and evaluate it
arg_val = self.evaluate_function(arg, arg_dict)
args.append(arg_val)
elif isinstance(arg, str):
# this argument is a variable (as json). convert it to a ROS msg type
try:
arg_json_str = arg_dict[arg]
except KeyError:
raise ValueError('No value specified for query arg %s' % arg)
rospy.loginfo(' arg %s: %s' % (arg, arg_json_str))
# lookup the class for this argument, using the QueryFunction declaration
arg_class = func.arg_types[arg_i]
# convert the value for this argument from JSON to a ROS message
args.append(from_json(arg_json_str, arg_class()))
else:
raise ValueError('Improper argument type for function')
# evaluate the function
return func.func(*args)
def get_transform(self, current_frame, desired_frame):
try:
(trans, rot) = self._transform_listener.lookupTransform(
current_frame, desired_frame, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException):
raise
return Transform(Vector3(*trans), Quaternion(*rot))
def transform_point_to_frame(self, point, current_frame, desired_frame):
point_arr_h = np.array([point.x, point.y, point.z, 1.0])
transform_arr = conv.transform_to_array(self.get_transform(current_frame, desired_frame))
new_point_arr_h = np.dot(transform_arr, point_arr_h)
new_point_arr = new_point_arr_h[:3] / new_point_arr_h[3]
return Point(*new_point_arr)
def is_charging(self):
return self._battery_monitor.is_charging()
def logical_and(self, arg1, arg2):
return arg1 and arg2
def logical_or(self, arg1, arg2):
return arg1 or arg2
def point_within_bbox(self, point, bbox_origin, bbox_size):
'''
Args:
point - geometry_msgs.msg.Point
bbox_origin - geometry_msgs.msg.Point
bbox_size - geometry_msgs.msg.Vector3
Returns:
(bool) - whether point is within bounding box.
'''
return ((point.x >= bbox_origin.x) and
(point.y >= bbox_origin.y) and
(point.z >= bbox_origin.z) and
(point.x <= (bbox_origin.x + bbox_size.x)) and
(point.y <= (bbox_origin.y + bbox_size.y)) and
(point.z <= (bbox_origin.z + bbox_size.z)))
