def __init__(self,
robot_pose=(0, 0, 0),
visible=True,
default_color=cnames['yellow'],
element_dict={},
min_view_dist=0.3,
max_view_dist=1.0):
self.element_dict = element_dict
# Define nominal viewcone
self.min_view_dist = min_view_dist # [m]
self.max_view_dist = max_view_dist # [m]
self.view_angle = math.pi / 2 # [rad]
viewcone_pts = [
(0, 0),
(self.max_view_dist * math.cos(self.view_angle / 2),
self.max_view_dist * math.sin(self.view_angle / 2)),
(self.max_view_dist * math.cos(-self.view_angle / 2),
self.max_view_dist * math.sin(-self.view_angle / 2)),
(0, 0),
]
# Create SoftMax model for camera
self.detection_model = camera_model_2D(self.min_view_dist,
self.max_view_dist)
# Instantiate Sensor superclass object
update_rate = 1 # [hz]
has_physical_dimensions = True
super(Camera, self).__init__(update_rate, has_physical_dimensions)
# Set the ideal and actual viewcones
self.ideal_viewcone = MapObject(
'Ideal viewcone',
viewcone_pts,
visible=False,
blocks_camera=False,
color_str='pink',
pose=robot_pose,
has_relations=False,
centroid_at_origin=False,
)
self.viewcone = MapObject(
'Viewcone',
viewcone_pts,
alpha=0.2,
visible=True,
blocks_camera=False,
color_str='lightyellow',
pose=robot_pose,
has_relations=False,
centroid_at_origin=False,
)
self.view_pose = (0, 0, 0)
# <>TODO: Add in and test an offset of (-0.1,-0.1)
self.offset = (0, 0, 0) # [m] offset (x,y,theta) from center of robot
self._move_viewcone(robot_pose)
# Set up the VB fusion parameters
self.vb = VariationalBayes()
def clear_probability_from_objects(self, target, obj):
logging.info('Clearing probability from {}'.format(obj.name))
fusion_engine = self.robot.fusion_engine
vb = VariationalBayes()
if not hasattr(obj, 'relations'):
obj.define_relations()
if target is not None:
prior = fusion_engine.filters[target].probability
likelihood = obj.relations.binary_models['Inside']
mu, sigma, beta = vb.update(
measurement='Not Inside',
likelihood=likelihood,
prior=prior,
use_LWIS=False,
)
gm = GaussianMixture(beta, mu, sigma)
fusion_engine.filters[target].probability = gm
else:
# Update all but combined filters
for name, filter_ in fusion_engine.filters.iteritems():
if name == 'combined':
pass
else:
prior = filter_.probability
likelihood = obj.relations.binary_models['Inside']
mu, sigma, beta = vb.update(
measurement='Not Inside',
likelihood=likelihood,
prior=prior,
use_LWIS=False,
)
gm = GaussianMixture(beta, mu, sigma)
filter_.probability = gm
def clear_probability_from_objects(self, target, obj):
logging.info('Clearing probability from {}'.format(obj.name))
fusion_engine = self.robot.fusion_engine
vb = VariationalBayes()
if not hasattr(obj, 'relations'):
obj.define_relations()
if target is not None:
prior = fusion_engine.filters[target].probability
likelihood = obj.relations.binary_models['Inside']
mu, sigma, beta = vb.update(measurement='Not Inside',
likelihood=likelihood,
prior=prior,
use_LWIS=False,
)
gm = GaussianMixture(beta, mu, sigma)
fusion_engine.filters[target].probability = gm
else:
# Update all but combined filters
for name, filter_ in fusion_engine.filters.iteritems():
if name == 'combined':
pass
else:
prior = filter_.probability
likelihood = obj.relations.binary_models['Inside']
mu, sigma, beta = vb.update(measurement='Not Inside',
likelihood=likelihood,
prior=prior,
use_LWIS=False,
)
gm = GaussianMixture(beta, mu, sigma)
filter_.probability = gm
def __init__(self,
map_=None,
web_interface_topic='python',
false_alarm_prob=0.2):
self.update_rate = None
self.has_physical_dimensions = False
self.speed_model = speed_model()
self.false_alarm_prob = false_alarm_prob
super(Human, self).__init__(self.update_rate,
self.has_physical_dimensions)
# self.certainties = ['think', 'know']
self.certainties = ['know']
self.positivities = ['is not',
'is'] # <>TODO: oh god wtf why does order matter
self.relations = {
'object': [
'behind',
'in front of',
'left of',
'right of',
'near',
],
'area': ['inside', 'near', 'outside']
}
self.movement_types = ['moving', 'stopped']
self.movement_qualities = ['slowly', 'moderately', 'quickly']
self.groundings = {}
self.groundings['area'] = map_.areas
self.groundings['object'] = {}
for cop_name, cop in map_.cops.iteritems():
# if cop.has_relations:
self.groundings['object'][cop_name] = cop
for object_name, obj in map_.objects.iteritems():
if obj.has_relations:
self.groundings['object'][object_name] = obj
self.target_names = ['nothing', 'a robot'] + map_.robbers.keys()
self.utterance = ''
self.new_update = False
# Set up the VB fusion parameters
self.vb = VariationalBayes()
if web_interface_topic != 'python':
# Subscribe to web interface
print web_interface_topic
import rospy
from std_msgs.msg import String
rospy.Subscriber(web_interface_topic, String, self.callback)
def __init__(self, robot_pose=(0, 0, 0), visible=True,
default_color=cnames['yellow'], element_dict={},
min_view_dist=0.3, max_view_dist=1.0):
self.element_dict = element_dict
# Define nominal viewcone
self.min_view_dist = min_view_dist # [m]
self.max_view_dist = max_view_dist # [m]
self.view_angle = math.pi / 2 # [rad]
viewcone_pts = [(0, 0),
(self.max_view_dist * math.cos(self.view_angle / 2),
self.max_view_dist * math.sin(self.view_angle / 2)),
(self.max_view_dist * math.cos(-self.view_angle / 2),
self.max_view_dist * math.sin(-self.view_angle / 2)),
(0, 0),
]
# Create SoftMax model for camera
self.detection_model = camera_model_2D(self.min_view_dist,
self.max_view_dist)
# Instantiate Sensor superclass object
update_rate = 1 # [hz]
has_physical_dimensions = True
super(Camera, self).__init__(update_rate, has_physical_dimensions)
# Set the ideal and actual viewcones
self.ideal_viewcone = MapObject('Ideal viewcone',
viewcone_pts,
visible=False,
blocks_camera=False,
color_str='pink',
pose=robot_pose,
has_relations=False,
centroid_at_origin=False,
)
self.viewcone = MapObject('Viewcone',
viewcone_pts,
alpha=0.2,
visible=True,
blocks_camera=False,
color_str='lightyellow',
pose=robot_pose,
has_relations=False,
centroid_at_origin=False,
)
self.view_pose = (0, 0, 0)
# <>TODO: Add in and test an offset of (-0.1,-0.1)
self.offset = (0, 0, 0) # [m] offset (x,y,theta) from center of robot
self._move_viewcone(robot_pose)
# Set up the VB fusion parameters
self.vb = VariationalBayes()
def __init__(self, map_=None, web_interface_topic='python',
false_alarm_prob=0.2):
self.update_rate = None
self.has_physical_dimensions = False
self.speed_model = speed_model()
self.false_alarm_prob = false_alarm_prob
super(Human, self).__init__(self.update_rate,
self.has_physical_dimensions)
# self.certainties = ['think', 'know']
self.certainties = ['know']
self.positivities = ['is not', 'is'] # <>TODO: oh god wtf why does order matter
self.relations = {'object': ['behind',
'in front of',
'left of',
'right of',
'near',
],
'area': ['inside',
'near',
'outside'
]}
self.movement_types = ['moving', 'stopped']
self.movement_qualities = ['slowly', 'moderately', 'quickly']
self.groundings = {}
self.groundings['area'] = map_.areas
self.groundings['object'] = {}
for cop_name, cop in map_.cops.iteritems():
# if cop.has_relations:
self.groundings['object'][cop_name] = cop
for object_name, obj in map_.objects.iteritems():
if obj.has_relations:
self.groundings['object'][object_name] = obj
self.target_names = ['nothing', 'a robot'] + map_.robbers.keys()
self.utterance = ''
self.new_update = False
# Set up the VB fusion parameters
self.vb = VariationalBayes()
if web_interface_topic != 'python':
# Subscribe to web interface
print web_interface_topic
import rospy
from std_msgs.msg import String
rospy.Subscriber(web_interface_topic, String, self.callback)
class Human(Sensor):
"""The human sensor, able to provide updates to a fusion engine.
.. image:: img/classes_Human.png
Parameters
----------
shape_layer : ShapeLayer
A layer object providing all the shapes in the map so that the human
sensor can ground its statements.
robber_names : list of str
The list of all robbers, for the human to specify targets.
detection_chance : float
The human sensor's ability to correctly name a target, given that
the target is within the human sensor's view -- that is, P(D=i|x).
Note that this is the detection chance related to the "I think"
statement, which can be increased if the human says "I know" instead.
"""
def __init__(self, map_=None, web_interface_topic='python',
false_alarm_prob=0.2):
self.update_rate = None
self.has_physical_dimensions = False
self.speed_model = speed_model()
self.false_alarm_prob = false_alarm_prob
super(Human, self).__init__(self.update_rate,
self.has_physical_dimensions)
# self.certainties = ['think', 'know']
self.certainties = ['know']
self.positivities = ['is not', 'is'] # <>TODO: oh god wtf why does order matter
self.relations = {'object': ['behind',
'in front of',
'left of',
'right of',
'near',
],
'area': ['inside',
'near',
'outside'
]}
self.movement_types = ['moving', 'stopped']
self.movement_qualities = ['slowly', 'moderately', 'quickly']
self.groundings = {}
self.groundings['area'] = map_.areas
self.groundings['object'] = {}
for cop_name, cop in map_.cops.iteritems():
# if cop.has_relations:
self.groundings['object'][cop_name] = cop
for object_name, obj in map_.objects.iteritems():
if obj.has_relations:
self.groundings['object'][object_name] = obj
self.target_names = ['nothing', 'a robot'] + map_.robbers.keys()
self.utterance = ''
self.new_update = False
# Set up the VB fusion parameters
self.vb = VariationalBayes()
if web_interface_topic != 'python':
# Subscribe to web interface
print web_interface_topic
import rospy
from std_msgs.msg import String
rospy.Subscriber(web_interface_topic, String, self.callback)
def callback(self, msg):
logging.info('I heard {}'.format(msg.data))
self.utterance = msg.data
self.new_update = True
def detect(self, filter_name, type_="particle", particles=None, prior=None):
"""Update a fusion engine's probability from human sensor updates.
Parameters
----------
particles : array_like
The particle list, assuming [x,y,p], where x and y are position
data and p is the particle's associated probability. `None` if not
using particles.
"""
if not self.parse_utterance():
logging.debug('No utterance to parse!')
return
# End detect loop if not the right target
if self.target_name not in ['nothing', 'a robot', filter_name]:
logging.debug('Target {} is not in {} Looking for {}.'
.format(filter_name, self.utterance, self.target_name))
return
if self.relation != '':
self.translate_relation()
self.detection_type = 'position'
elif self.movement_type != '':
self.translate_movement()
self.detection_type = 'movement'
else:
logging.error("No relations or movements found in utterance.")
if type_ == 'particle':
self.detect_particles(particles)
return True
elif type_ == 'gauss sum':
return self.detect_probability(prior)
else:
logging.error('Wrong detection model specified.')
def parse_utterance(self):
""" Parse the input string into workable values.
"""
logging.debug('Parsing: {}'.format(self.utterance))
logging.debug('Groundings: {}'.format(self.groundings))
for str_ in self.certainties:
if str_ in self.utterance:
self.certainty = str_
break
else:
self.certainty = ''
for str_ in self.target_names:
if str_ in self.utterance:
self.target_name = str_
break
else:
self.target_name = ''
for str_ in self.positivities:
if str_ in self.utterance:
self.positivity = str_
break
else:
self.positivity = ''
for str_type in self.relations:
for str_ in self.relations[str_type]:
if str_ in self.utterance:
self.relation = str_
break
else:
continue
break
else:
self.relation = ''
for str_type in self.groundings:
for str_ in self.groundings[str_type].keys():
str_ = str_.lower()
if str_ in self.utterance.lower():
str_ = str_.title()
self.grounding = self.groundings[str_type][str_]
break
else:
continue
break
else:
self.grounding = None
logging.debug('Utterance: {}'.format(self.utterance))
logging.debug('Grounding: {}'.format(self.grounding))
for str_ in self.movement_types:
if str_ in self.utterance:
self.movement_type = str_
logging.info(str_)
break
else:
self.movement_type = ''
for str_ in self.movement_qualities:
if str_ in self.utterance:
self.movement_quality = str_
logging.info(str_)
break
else:
self.movement_quality = ''
if self.utterance == '':
utterance_is_well_formed = False
else:
utterance_is_well_formed = True
return utterance_is_well_formed
def translate_relation(self, relation_type='intrinsic'):
"""Translate the uttered relation to a likelihood label.
"""
if relation_type == 'intrinsic':
# Translate relation to zone label
if self.relation == 'behind':
translated_relation = 'Back'
elif self.relation == 'in front of':
translated_relation = 'Front'
elif self.relation == 'left of':
translated_relation = 'Left'
elif self.relation == 'right of':
translated_relation = 'Right'
elif self.relation in ['inside', 'near', 'outside']:
translated_relation = self.relation.title()
self.relation = translated_relation
elif relation_type == 'relative':
pass # <>TODO: Implement relative relations
def translate_movement(self):
"""Translate the uttered movement to a likelihood label.
"""
# Translate movement to motion model
if self.movement_type == 'stopped':
translated_movement = 'stopped'
elif self.movement_quality == 'slowly':
translated_movement = 'moving slowly'
elif self.movement_quality == 'moderately':
translated_movement = 'moving moderately'
elif self.movement_quality == 'quickly':
translated_movement = 'moving quickly'
self.movement = translated_movement
def detect_particles(self, particles):
"""Update particles based on sensor model.
Parameters
----------
particles : array_like
The particle list, assuming [p,x,y,x_dot,y_dot], where x and y are
position data and p is the particle's associated probability.
"""
if not hasattr(self.grounding, 'relations'):
self.grounding.define_relations()
# <>TODO: include certainty
if self.detection_type == 'position':
label = self.relation
if self.target_name == 'nothing' and self.positivity == 'not':
label = 'a robot'
elif self.target_name == 'nothing' or self.positivity == 'not':
label = 'Not ' + label
for i, particle in enumerate(particles):
state = particle[1:3]
particle[0] *= self.grounding.relations.probability(state=state, class_=label)
elif self.detection_type == 'movement':
label = self.movement
if self.target_name == 'nothing' and self.positivity == 'not':
label = 'a robot'
elif self.target_name == 'nothing' or self.positivity == 'not':
label = 'Not ' + label
for i, particle in enumerate(particles):
state = np.sqrt(particle[3] ** 2 + particle[4] ** 2)
particle[0] *= self.speed_model.probability(state=state, class_=label)
# Renormalize
particles[:, 0] /= sum(particles[:, 0])
def detect_probability(self, prior):
if not hasattr(self.grounding, 'relations') \
or self.grounding.name.lower() == 'deckard':
logging.info("Defining relations because {} didn't have any."
.format(self.grounding.name))
self.grounding.define_relations()
if self.grounding.name.lower() == 'deckard':
if self.relation == 'Right':
self.relation = 'Left'
elif self.relation == 'Left':
self.relation = 'Right'
# Position update
label = self.relation
if self.target_name == 'nothing' and self.positivity == 'is not':
label = 'a robot'
elif self.target_name == 'nothing' or self.positivity == 'is not':
label = 'Not ' + label
likelihood = self.grounding.relations.binary_models[self.relation]
mu, sigma, beta = self.vb.update(measurement=label,
likelihood=likelihood,
prior=prior,
use_LWIS=False,
)
gm = GaussianMixture(beta, mu, sigma)
alpha = self.false_alarm_prob / 2
posterior = prior.combine_gms(gm, alpha)
# Weight based on human possibly being wrong
return posterior
class Camera(Sensor):
"""A conic sensor mounted on a robot.
The camera provides a viewcone from the point of view of the robot which
rescales based on its environment (e.g. if it's in front of a wall).
.. image:: img/classes_Camera.png
Parameters
----------
robot_pose : array_like, optional
The cop's initial [x, y, theta] (defaults to [0, 0, 0]).
visible : bool, optional
Whether or not the view cone is shown. Default is True.
default_color : cnames
Default color to display all camera sensors as. Defaults to yellow.
"""
def __init__(self, robot_pose=(0, 0, 0), visible=True,
default_color=cnames['yellow'], element_dict={},
min_view_dist=0.3, max_view_dist=1.0):
self.element_dict = element_dict
# Define nominal viewcone
self.min_view_dist = min_view_dist # [m]
self.max_view_dist = max_view_dist # [m]
self.view_angle = math.pi / 2 # [rad]
viewcone_pts = [(0, 0),
(self.max_view_dist * math.cos(self.view_angle / 2),
self.max_view_dist * math.sin(self.view_angle / 2)),
(self.max_view_dist * math.cos(-self.view_angle / 2),
self.max_view_dist * math.sin(-self.view_angle / 2)),
(0, 0),
]
# Create SoftMax model for camera
self.detection_model = camera_model_2D(self.min_view_dist,
self.max_view_dist)
# Instantiate Sensor superclass object
update_rate = 1 # [hz]
has_physical_dimensions = True
super(Camera, self).__init__(update_rate, has_physical_dimensions)
# Set the ideal and actual viewcones
self.ideal_viewcone = MapObject('Ideal viewcone',
viewcone_pts,
visible=False,
blocks_camera=False,
color_str='pink',
pose=robot_pose,
has_relations=False,
centroid_at_origin=False,
)
self.viewcone = MapObject('Viewcone',
viewcone_pts,
alpha=0.2,
visible=True,
blocks_camera=False,
color_str='lightyellow',
pose=robot_pose,
has_relations=False,
centroid_at_origin=False,
)
self.view_pose = (0, 0, 0)
# <>TODO: Add in and test an offset of (-0.1,-0.1)
self.offset = (0, 0, 0) # [m] offset (x,y,theta) from center of robot
self._move_viewcone(robot_pose)
# Set up the VB fusion parameters
self.vb = VariationalBayes()
def update_viewcone(self, robot_pose):
"""Update the camera's viewcone position and scale.
Parameters
----------
robot_pose : array_like, optional
The robot's currentl [x, y, theta].
shape_layer : ShapeLayer
A layer object providing all the shapes in the map for the camera
to rescale its viewcone.
"""
self._move_viewcone(robot_pose)
self._rescale_viewcone(robot_pose)
# Translate detection model
self.detection_model.move(self.view_pose)
def _move_viewcone(self, robot_pose):
"""Move the viewcone based on the robot's pose
Parameters
----------
robot_pose : array_like, optional
The robot's currentl [x, y, theta].
"""
pose = (robot_pose[0] + self.offset[0],
robot_pose[1] + self.offset[1],
robot_pose[2]
)
# Reset the view shape
self.viewcone.shape = self.ideal_viewcone.shape
transform = tuple(np.subtract(pose, self.view_pose))
self.ideal_viewcone.move_relative(transform,
rotation_pt=self.view_pose[0:2])
self.viewcone.move_relative(transform, rotation_pt=self.view_pose[0:2])
self.view_pose = pose
def _rescale_viewcone(self, robot_pose):
"""Rescale the viewcone based on intersecting map objects.
Parameters
----------
robot_pose : array_like, optional
The robot's currentl [x, y, theta].
"""
scale = self.max_view_dist
blocking_shapes = []
possible_elements = []
try:
possible_elements += self.element_dict['static']
except:
logging.debug('No static elements')
try:
possible_elements += self.element_dict['dynamic']
except:
logging.debug('No dynamic elements')
for element in possible_elements:
if element.blocks_camera:
blocking_shapes.append(element.shape)
for shape in blocking_shapes:
if self.viewcone.shape.intersects(shape):
# <>TODO: Use shadows instead of rescaling viewcone
# calculate shadows for all shapes touching viewcone
# origin = self.viewcone.project(map_object.shape)
# shadow = affinity.scale(...) #map portion invisible to the view
# self.viewcone = self.viewcone.difference(shadow)
distance = Point(self.view_pose[0:2]).distance(shape)
scale_ = distance / self.max_view_dist * 1.3 # <>TODO: why the 1.3?
if scale_ < scale:
scale = scale_
self.viewcone.shape = affinity.scale(self.ideal_viewcone.shape,
xfact=scale,
yfact=scale,
origin=self.view_pose[0:2])
# else:
# self.viewcone.shape = self.ideal_viewcone.shape
def detect(self, filter_type, particles=None, prior=None):
"""Update a fusion engine's probability from camera detections.
Parameters
----------
filter_type : {'particle','gauss sum'}
The type of filter to update.
particles : array_like, optional
The particle list, assuming [x,y,p], where x and y are position
data and p is the particle's associated probability.
"""
if filter_type == 'particle':
self._detect_particles(particles)
else:
posterior = self._detect_probability(prior)
return posterior
def _detect_particles(self, particles):
"""Update particles based on sensor model.
particles : array_like
The particle list, assuming [x,y,p], where x and y are position
data and p is the particle's associated probability.
"""
# Update particle probabilities in view cone frame
for i, particle in enumerate(particles):
if self.viewcone.shape.contains(Point(particle[1:3])):
particles[i, 0] *= (1 - self.detection_model
.probability(state=particle[1:3], class_='Detection'))
# Renormalize
particles[:, 0] /= sum(particles[:, 0])
def _detect_probability(self, prior):
mu, sigma, beta = self.vb.update(measurement='No Detection',
likelihood=self.detection_model,
prior=prior,
use_LWIS=True,
poly=self.detection_model.poly
)
gm = GaussianMixture(beta, mu, sigma)
gm.camera_viewcone = self.detection_model.poly # for plotting
return gm
class Camera(Sensor):
"""A conic sensor mounted on a robot.
The camera provides a viewcone from the point of view of the robot which
rescales based on its environment (e.g. if it's in front of a wall).
.. image:: img/classes_Camera.png
Parameters
----------
robot_pose : array_like, optional
The cop's initial [x, y, theta] (defaults to [0, 0, 0]).
visible : bool, optional
Whether or not the view cone is shown. Default is True.
default_color : cnames
Default color to display all camera sensors as. Defaults to yellow.
"""
def __init__(self,
robot_pose=(0, 0, 0),
visible=True,
default_color=cnames['yellow'],
element_dict={},
min_view_dist=0.3,
max_view_dist=1.0):
self.element_dict = element_dict
# Define nominal viewcone
self.min_view_dist = min_view_dist # [m]
self.max_view_dist = max_view_dist # [m]
self.view_angle = math.pi / 2 # [rad]
viewcone_pts = [
(0, 0),
(self.max_view_dist * math.cos(self.view_angle / 2),
self.max_view_dist * math.sin(self.view_angle / 2)),
(self.max_view_dist * math.cos(-self.view_angle / 2),
self.max_view_dist * math.sin(-self.view_angle / 2)),
(0, 0),
]
# Create SoftMax model for camera
self.detection_model = camera_model_2D(self.min_view_dist,
self.max_view_dist)
# Instantiate Sensor superclass object
update_rate = 1 # [hz]
has_physical_dimensions = True
super(Camera, self).__init__(update_rate, has_physical_dimensions)
# Set the ideal and actual viewcones
self.ideal_viewcone = MapObject(
'Ideal viewcone',
viewcone_pts,
visible=False,
blocks_camera=False,
color_str='pink',
pose=robot_pose,
has_relations=False,
centroid_at_origin=False,
)
self.viewcone = MapObject(
'Viewcone',
viewcone_pts,
alpha=0.2,
visible=True,
blocks_camera=False,
color_str='lightyellow',
pose=robot_pose,
has_relations=False,
centroid_at_origin=False,
)
self.view_pose = (0, 0, 0)
# <>TODO: Add in and test an offset of (-0.1,-0.1)
self.offset = (0, 0, 0) # [m] offset (x,y,theta) from center of robot
self._move_viewcone(robot_pose)
# Set up the VB fusion parameters
self.vb = VariationalBayes()
def update_viewcone(self, robot_pose):
"""Update the camera's viewcone position and scale.
Parameters
----------
robot_pose : array_like, optional
The robot's currentl [x, y, theta].
shape_layer : ShapeLayer
A layer object providing all the shapes in the map for the camera
to rescale its viewcone.
"""
self._move_viewcone(robot_pose)
self._rescale_viewcone(robot_pose)
# Translate detection model
self.detection_model.move(self.view_pose)
def _move_viewcone(self, robot_pose):
"""Move the viewcone based on the robot's pose
Parameters
----------
robot_pose : array_like, optional
The robot's currentl [x, y, theta].
"""
pose = (robot_pose[0] + self.offset[0], robot_pose[1] + self.offset[1],
robot_pose[2])
# Reset the view shape
self.viewcone.shape = self.ideal_viewcone.shape
transform = tuple(np.subtract(pose, self.view_pose))
self.ideal_viewcone.move_relative(transform,
rotation_pt=self.view_pose[0:2])
self.viewcone.move_relative(transform, rotation_pt=self.view_pose[0:2])
self.view_pose = pose
def _rescale_viewcone(self, robot_pose):
"""Rescale the viewcone based on intersecting map objects.
Parameters
----------
robot_pose : array_like, optional
The robot's currentl [x, y, theta].
"""
scale = self.max_view_dist
blocking_shapes = []
possible_elements = []
try:
possible_elements += self.element_dict['static']
except:
logging.debug('No static elements')
try:
possible_elements += self.element_dict['dynamic']
except:
logging.debug('No dynamic elements')
for element in possible_elements:
if element.blocks_camera:
blocking_shapes.append(element.shape)
for shape in blocking_shapes:
if self.viewcone.shape.intersects(shape):
# <>TODO: Use shadows instead of rescaling viewcone
# calculate shadows for all shapes touching viewcone
# origin = self.viewcone.project(map_object.shape)
# shadow = affinity.scale(...) #map portion invisible to the view
# self.viewcone = self.viewcone.difference(shadow)
distance = Point(self.view_pose[0:2]).distance(shape)
scale_ = distance / self.max_view_dist * 1.3 # <>TODO: why the 1.3?
if scale_ < scale:
scale = scale_
self.viewcone.shape = affinity.scale(self.ideal_viewcone.shape,
xfact=scale,
yfact=scale,
origin=self.view_pose[0:2])
# else:
# self.viewcone.shape = self.ideal_viewcone.shape
def detect(self, filter_type, particles=None, prior=None):
"""Update a fusion engine's probability from camera detections.
Parameters
----------
filter_type : {'particle','gauss sum'}
The type of filter to update.
particles : array_like, optional
The particle list, assuming [x,y,p], where x and y are position
data and p is the particle's associated probability.
"""
if filter_type == 'particle':
self._detect_particles(particles)
else:
posterior = self._detect_probability(prior)
return posterior
def _detect_particles(self, particles):
"""Update particles based on sensor model.
particles : array_like
The particle list, assuming [x,y,p], where x and y are position
data and p is the particle's associated probability.
"""
# Update particle probabilities in view cone frame
for i, particle in enumerate(particles):
if self.viewcone.shape.contains(Point(particle[1:3])):
particles[i, 0] *= (1 - self.detection_model.probability(
state=particle[1:3], class_='Detection'))
# Renormalize
particles[:, 0] /= sum(particles[:, 0])
def _detect_probability(self, prior):
mu, sigma, beta = self.vb.update(measurement='No Detection',
likelihood=self.detection_model,
prior=prior,
use_LWIS=True,
poly=self.detection_model.poly)
gm = GaussianMixture(beta, mu, sigma)
gm.camera_viewcone = self.detection_model.poly # for plotting
return gm
class Human(Sensor):
"""The human sensor, able to provide updates to a fusion engine.
.. image:: img/classes_Human.png
Parameters
----------
shape_layer : ShapeLayer
A layer object providing all the shapes in the map so that the human
sensor can ground its statements.
robber_names : list of str
The list of all robbers, for the human to specify targets.
detection_chance : float
The human sensor's ability to correctly name a target, given that
the target is within the human sensor's view -- that is, P(D=i|x).
Note that this is the detection chance related to the "I think"
statement, which can be increased if the human says "I know" instead.
"""
def __init__(self,
map_=None,
web_interface_topic='python',
false_alarm_prob=0.2):
self.update_rate = None
self.has_physical_dimensions = False
self.speed_model = speed_model()
self.false_alarm_prob = false_alarm_prob
super(Human, self).__init__(self.update_rate,
self.has_physical_dimensions)
# self.certainties = ['think', 'know']
self.certainties = ['know']
self.positivities = ['is not',
'is'] # <>TODO: oh god wtf why does order matter
self.relations = {
'object': [
'behind',
'in front of',
'left of',
'right of',
'near',
],
'area': ['inside', 'near', 'outside']
}
self.movement_types = ['moving', 'stopped']
self.movement_qualities = ['slowly', 'moderately', 'quickly']
self.groundings = {}
self.groundings['area'] = map_.areas
self.groundings['object'] = {}
for cop_name, cop in map_.cops.iteritems():
# if cop.has_relations:
self.groundings['object'][cop_name] = cop
for object_name, obj in map_.objects.iteritems():
if obj.has_relations:
self.groundings['object'][object_name] = obj
self.target_names = ['nothing', 'a robot'] + map_.robbers.keys()
self.utterance = ''
self.new_update = False
# Set up the VB fusion parameters
self.vb = VariationalBayes()
if web_interface_topic != 'python':
# Subscribe to web interface
print web_interface_topic
import rospy
from std_msgs.msg import String
rospy.Subscriber(web_interface_topic, String, self.callback)
def callback(self, msg):
logging.info('I heard {}'.format(msg.data))
self.utterance = msg.data
self.new_update = True
def detect(self,
filter_name,
type_="particle",
particles=None,
prior=None):
"""Update a fusion engine's probability from human sensor updates.
Parameters
----------
particles : array_like
The particle list, assuming [x,y,p], where x and y are position
data and p is the particle's associated probability. `None` if not
using particles.
"""
if not self.parse_utterance():
logging.debug('No utterance to parse!')
return
# End detect loop if not the right target
if self.target_name not in ['nothing', 'a robot', filter_name]:
logging.debug('Target {} is not in {} Looking for {}.'.format(
filter_name, self.utterance, self.target_name))
return
if self.relation != '':
self.translate_relation()
self.detection_type = 'position'
elif self.movement_type != '':
self.translate_movement()
self.detection_type = 'movement'
else:
logging.error("No relations or movements found in utterance.")
if type_ == 'particle':
self.detect_particles(particles)
return True
elif type_ == 'gauss sum':
return self.detect_probability(prior)
else:
logging.error('Wrong detection model specified.')
def parse_utterance(self):
""" Parse the input string into workable values.
"""
logging.debug('Parsing: {}'.format(self.utterance))
logging.debug('Groundings: {}'.format(self.groundings))
for str_ in self.certainties:
if str_ in self.utterance:
self.certainty = str_
break
else:
self.certainty = ''
for str_ in self.target_names:
if str_ in self.utterance:
self.target_name = str_
break
else:
self.target_name = ''
for str_ in self.positivities:
if str_ in self.utterance:
self.positivity = str_
break
else:
self.positivity = ''
for str_type in self.relations:
for str_ in self.relations[str_type]:
if str_ in self.utterance:
self.relation = str_
break
else:
continue
break
else:
self.relation = ''
for str_type in self.groundings:
for str_ in self.groundings[str_type].keys():
str_ = str_.lower()
if str_ in self.utterance.lower():
str_ = str_.title()
self.grounding = self.groundings[str_type][str_]
break
else:
continue
break
else:
self.grounding = None
logging.debug('Utterance: {}'.format(self.utterance))
logging.debug('Grounding: {}'.format(self.grounding))
for str_ in self.movement_types:
if str_ in self.utterance:
self.movement_type = str_
logging.info(str_)
break
else:
self.movement_type = ''
for str_ in self.movement_qualities:
if str_ in self.utterance:
self.movement_quality = str_
logging.info(str_)
break
else:
self.movement_quality = ''
if self.utterance == '':
utterance_is_well_formed = False
else:
utterance_is_well_formed = True
return utterance_is_well_formed
def translate_relation(self, relation_type='intrinsic'):
"""Translate the uttered relation to a likelihood label.
"""
if relation_type == 'intrinsic':
# Translate relation to zone label
if self.relation == 'behind':
translated_relation = 'Back'
elif self.relation == 'in front of':
translated_relation = 'Front'
elif self.relation == 'left of':
translated_relation = 'Left'
elif self.relation == 'right of':
translated_relation = 'Right'
elif self.relation in ['inside', 'near', 'outside']:
translated_relation = self.relation.title()
self.relation = translated_relation
elif relation_type == 'relative':
pass # <>TODO: Implement relative relations
def translate_movement(self):
"""Translate the uttered movement to a likelihood label.
"""
# Translate movement to motion model
if self.movement_type == 'stopped':
translated_movement = 'stopped'
elif self.movement_quality == 'slowly':
translated_movement = 'moving slowly'
elif self.movement_quality == 'moderately':
translated_movement = 'moving moderately'
elif self.movement_quality == 'quickly':
translated_movement = 'moving quickly'
self.movement = translated_movement
def detect_particles(self, particles):
"""Update particles based on sensor model.
Parameters
----------
particles : array_like
The particle list, assuming [p,x,y,x_dot,y_dot], where x and y are
position data and p is the particle's associated probability.
"""
if not hasattr(self.grounding, 'relations'):
self.grounding.define_relations()
# <>TODO: include certainty
if self.detection_type == 'position':
label = self.relation
if self.target_name == 'nothing' and self.positivity == 'not':
label = 'a robot'
elif self.target_name == 'nothing' or self.positivity == 'not':
label = 'Not ' + label
for i, particle in enumerate(particles):
state = particle[1:3]
particle[0] *= self.grounding.relations.probability(
state=state, class_=label)
elif self.detection_type == 'movement':
label = self.movement
if self.target_name == 'nothing' and self.positivity == 'not':
label = 'a robot'
elif self.target_name == 'nothing' or self.positivity == 'not':
label = 'Not ' + label
for i, particle in enumerate(particles):
state = np.sqrt(particle[3]**2 + particle[4]**2)
particle[0] *= self.speed_model.probability(state=state,
class_=label)
# Renormalize
particles[:, 0] /= sum(particles[:, 0])
def detect_probability(self, prior):
if not hasattr(self.grounding, 'relations') \
or self.grounding.name.lower() == 'deckard':
logging.info(
"Defining relations because {} didn't have any.".format(
self.grounding.name))
self.grounding.define_relations()
if self.grounding.name.lower() == 'deckard':
if self.relation == 'Right':
self.relation = 'Left'
elif self.relation == 'Left':
self.relation = 'Right'
# Position update
label = self.relation
if self.target_name == 'nothing' and self.positivity == 'is not':
label = 'a robot'
elif self.target_name == 'nothing' or self.positivity == 'is not':
label = 'Not ' + label
likelihood = self.grounding.relations.binary_models[self.relation]
mu, sigma, beta = self.vb.update(
measurement=label,
likelihood=likelihood,
prior=prior,
use_LWIS=False,
)
gm = GaussianMixture(beta, mu, sigma)
alpha = self.false_alarm_prob / 2
posterior = prior.combine_gms(gm, alpha)
# Weight based on human possibly being wrong
return posterior