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)