def __init__(self, map_name='fleming', bounds=[-5, -5, 5, 5],
plot_robbers=True, map_display_type='probability',
combined_only=True, publish_to_ROS=False):
# Define map properties
# <>TODO: Clean this up- add seperate map creation function?
self.map_name = map_name
if self.map_name == 'fleming':
#self.bounds = [-9.5, -3.33, 4, 3.68]
self.bounds = [0, 0, 5, 5]
else:
self.bounds = bounds # [x_min,y_min,x_max,y_max] in [m]
self.plot_robbers = plot_robbers
self.outer_bounds = [i * 1.1 for i in self.bounds]
self.origin = [0, 0] # in [m]
# <>TODO: Make display type relative to each robber
self.display_type = map_display_type
self.combined_only = combined_only
# Set up ROS elements if using ROS
self.publish_to_ROS = publish_to_ROS
if publish_to_ROS:
from cv_bridge import CvBridge
import rospy
from sensor_msgs.msg import Image
from std_msgs.msg import String
self.probability_target = 'Roy'
self.bridge = CvBridge()
self.image_pub = rospy.Publisher("python_probability_map", Image,
queue_size=10)
rospy.Subscriber("robot_probability_map", String,
self.change_published_ax)
# Define map elements
self.objects = {} # For dynamic/static map objects (not robbers/cops)
self.areas = {}
self.cops = {}
self.robbers = {}
self.dynamic_elements = []
self.static_elements = []
self.information_elements = []
self.element_dict = {'dynamic': self.dynamic_elements,
'static': self.static_elements,
'information': self.information_elements}
# Define layers
self.shape_layer = ShapeLayer(self.element_dict, bounds=self.bounds)
self.feasible_layer = FeasibleLayer(bounds=self.bounds)
self.particle_layers = {} # One per robber, plus one combined
self.probability_layers = {} # One per robber, plus one combined
# Set up map
if self.map_name == 'fleming':
set_up_fleming(self) # <>TODO: make a generic 'setup map' function
else:
pass
def _setup_layers(self):
# Set up basic layers
self.shape_layers = {}
self.feasible_layers = {}
for ax_name, ax in self.axes.iteritems():
self.shape_layers[ax_name] = ShapeLayer(self.element_dict,
bounds=self.bounds,
ax=ax)
# Set up probability/particle layers
if self.fusion_engine is not None:
filter_ = self.fusion_engine.filters[ax_name]
self.probability_layers[ax_name] = \
ProbabilityLayer(filter_, fig=self.fig, ax=ax,
bounds=self.bounds)
