示例#1
0
    def __init__(self, map_name='fleming', bounds=[-5, -5, 5, 5],
                 plot_robbers=True, map_display_type='particle',
                 combined_only=True, publish_to_ROS=False):

        # <>TODO: Move to main?
        self.human_cfg = load_config()['human_interface']

        # Define map properties
        self.map_name = map_name
        # <>TODO: Clean this up- add seperate map creation function?
        if self.map_name == 'fleming':
            self.bounds = [-9.5, -3.33, 4, 3.68]
        else:
            self.bounds = bounds  # [x_min,y_min,x_max,y_max] in [m] useful area

        self.plot_robbers = plot_robbers
        self.outer_bounds = [i * 1.1 for i in self.bounds]
        self.origin = [0, 0]  # in [m]
        self.fig = plt.figure(1, figsize=(14, 10))
        # <>TODO: Make display type relative to each robber
        self.display_type = map_display_type
        self.combined_only = combined_only

        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
示例#2
0
def main(config_file=None):
    # Load configuration files
    cfg = load_config(config_file)
    main_cfg = cfg['main']
    cop_cfg = cfg['cops']
    robber_cfg = cfg['robbers']
    distractor_cfg = cfg['distractors']

    # Set up logging and printing
    logger_level = logging.getLevelName(main_cfg['logging_level'])
    logger_format = '[%(levelname)-7s] %(funcName)-30s %(message)s'
    logging.basicConfig(format=logger_format,
                        level=logger_level,
                        )
    np.set_printoptions(precision=main_cfg['numpy_print_precision'],
                        suppress=True)

    # Set up a ROS node (if using ROS) and link it to Python's logger
    if main_cfg['use_ROS']:
        import rospy
        rospy.init_node(main_cfg['ROS_node_name'], log_level=rospy.DEBUG)
        handler = logging.StreamHandler()
        handler.setFormatter(logging.Formatter(logger_format))
        logging.getLogger().addHandler(handler)

    # Create cops with config params
    cops = {}
    for cop, kwargs in cop_cfg.iteritems():
        cops[cop] = Cop(cop, **kwargs)
        logging.info('{} added to simulation'.format(cop))

    # Create robbers with config params
    robbers = {}
    for robber, kwargs in robber_cfg.iteritems():
        robbers[robber] = Robber(robber, **kwargs)
        logging.info('{} added to simulation'.format(robber))

    # Create distractors with config params
    distractors = {}
    for distractor, kwargs in distractor_cfg.iteritems():
        distractors[distractor] = Distractor(distractor, **kwargs)

    # <>TODO: Replace with message passing
    # Give cops references to the robber's actual poses
    for cop in cops.values():
        for robber_name, robber in robbers.iteritems():
            cop.missing_robbers[robber_name].pose2D = \
                robber.pose2D
        for distrator_name, distractor in distractors.iteritems():
            cop.distracting_robots[distrator_name].pose2D = \
                distractor.pose2D
    # Give robber the cops list of found robots, so they will stop when found
    for robber in robbers.values():
        robber.found_robbers = cops['Deckard'].found_robbers

    # Describe simulation
    cop_names = cops.keys()
    if len(cop_names) > 1:
        cop_str = ', '.join(cop_names[:-1]) + ' and ' + str(cop_names[-1])
    else:
        cop_str = cop_names

    robber_names = robbers.keys()
    if len(robber_names) > 1:
        robber_str = ', '.join(robber_names[:-1]) + ' and ' + \
            str(robber_names[-1])
    else:
        robber_str = robber_names

    logging.info('Simulation started with {} chasing {}.'
                 .format(cop_str, robber_str))

    # Start the simulation
    fig = cops['Deckard'].map.fig
    fusion_engine = cops['Deckard'].fusion_engine
    cops['Deckard'].map.setup_plot(fusion_engine)
    sim_start_time = time.time()

    if cop_cfg['Deckard']['map_cfg']['publish_to_ROS']:
        headless_mode(cops, robbers, distractors, main_cfg, sim_start_time)
    else:
        animated_exploration(fig, cops, robbers, distractors, main_cfg, sim_start_time)