Esempio n. 1
0
    def __init__(self,
                 name,
                 pose=[0, 0, 90],
                 pose_source='python',
                 web_interface_topic='python',
                 ask_every_ten=False,
                 robber_model='static',
                 other_robot_names={},
                 map_cfg={},
                 mission_planner_cfg={},
                 goal_planner_cfg={},
                 path_planner_cfg={},
                 camera_cfg={},
                 questioner_cfg={},
                 human_cfg={},
                 **kwargs):
        # Use class defaults for kwargs not included
        mp_cfg = Cop.mission_planner_defaults.copy()
        mp_cfg.update(mission_planner_cfg)
        gp_cfg = Cop.goal_planner_defaults.copy()
        gp_cfg.update(goal_planner_cfg)
        pp_cfg = Cop.path_planner_defaults.copy()
        pp_cfg.update(path_planner_cfg)
        q_cfg = Cop.questioner_defaults.copy()
        q_cfg.update(questioner_cfg)

        # Configure fusion and map based on goal planner
        if gp_cfg['type_'] == 'particle':
            fusion_engine_type = 'particle'
            map_display_type = 'particle'
        elif gp_cfg['type_'] == 'MAP':
            fusion_engine_type = 'gauss sum'
            map_display_type = 'probability'
        # TODO: Refrence in yaml instead?
        map_cfg.update({'map_display_type': map_display_type})

        # Superclass and compositional attributes
        super(Cop, self).__init__(name,
                                  pose=pose,
                                  pose_source=pose_source,
                                  create_mission_planner=False,
                                  goal_planner_cfg=gp_cfg,
                                  path_planner_cfg=pp_cfg,
                                  map_cfg=map_cfg,
                                  color_str='darkgreen')

        # Tracking attributes
        self.other_robot_names = other_robot_names
        self.missing_robber_names = self.other_robot_names['robbers']
        self.distracting_robot_names = self.other_robot_names['distractors']
        self.found_robbers = {}

        # Create mission planner
        self.mission_planner = CopMissionPlanner(self, **mp_cfg)

        # Fusion and sensor attributes
        # <>TODO: Fusion Engine owned and refrenced from imaginary robber?
        self.fusion_engine = FusionEngine(fusion_engine_type,
                                          self.missing_robber_names,
                                          self.map.feasible_layer,
                                          robber_model)
        self.sensors = {}
        self.sensors['camera'] = Camera((0, 0, 0),
                                        element_dict=self.map.element_dict,
                                        **camera_cfg)
        self.map.dynamic_elements.append(self.sensors['camera'].viewcone)

        # Add self to map
        self.map.add_cop(self.map_obj)

        # Make others
        self.make_others()

        # Add human sensor after robbers have been made
        self.ask_every_ten = ask_every_ten
        self.sensors['human'] = Human(self.map, **human_cfg)
        self.map.add_human_sensor(self.sensors['human'])
        self.questioner = Questioner(human_sensor=self.sensors['human'],
                                     **q_cfg)
Esempio n. 2
0
class Cop(Robot):
    """The Cop subclass of the generic robot type.

    Cops extend the functionality of basic robots, providing sensing (both
    camera-based and human) and a fusion engine.

    .. image:: img/classes_Cop.png

    Parameters
    ----------
    name : str, optional
        The cop's name (defaults to 'Deckard').
    pose : list of float, optional
        The cop's initial [x, y, theta] (defaults to [0, 0.5, 90]).
    fusion_engine_type : {'particle','gauss_sum'}
        For particle filters or gaussian mixture filters, respectively.
    planner_type: {'simple', 'particle', 'MAP'}
        The cop's own type of planner.
    robber_model: {'stationary', 'random walk', 'clockwise',
      'counterclockwise'}
        The type of planner this cop believes robbers use.

    Attributes
    ----------
    fusion_engine
    planner
    found_robbers : dict
        All robbers found so far.
    sensors : dict
        All sensors owned by the cop.
    mission_statuses : {'searching', 'capturing', 'retired'}
        The possible mission-level statuses of any cop, where:
            * `searching` means the cop is exploring the environment;
            * `capturing` means the cop has detected a robber and is moving
                to capture it;
            * `retired` means all robbers have been captured.

    """
    mission_planner_defaults = {}
    goal_planner_defaults = {'type_': 'particle',
                             'use_target_as_goal': False}
    path_planner_defaults = {'type_': 'direct'}
    questioner_defaults = {}

    def __init__(self,
                 name,
                 pose=[0, 0, 90],
                 pose_source='python',
                 web_interface_topic='python',
                 ask_every_ten=False,
                 robber_model='static',
                 other_robot_names={},
                 map_cfg={},
                 mission_planner_cfg={},
                 goal_planner_cfg={},
                 path_planner_cfg={},
                 camera_cfg={},
                 questioner_cfg={},
                 human_cfg={},
                 **kwargs):
        # Use class defaults for kwargs not included
        mp_cfg = Cop.mission_planner_defaults.copy()
        mp_cfg.update(mission_planner_cfg)
        gp_cfg = Cop.goal_planner_defaults.copy()
        gp_cfg.update(goal_planner_cfg)
        pp_cfg = Cop.path_planner_defaults.copy()
        pp_cfg.update(path_planner_cfg)
        q_cfg = Cop.questioner_defaults.copy()
        q_cfg.update(questioner_cfg)

        # Configure fusion and map based on goal planner
        if gp_cfg['type_'] == 'particle':
            fusion_engine_type = 'particle'
            map_display_type = 'particle'
        elif gp_cfg['type_'] == 'MAP':
            fusion_engine_type = 'gauss sum'
            map_display_type = 'probability'
        # TODO: Refrence in yaml instead?
        map_cfg.update({'map_display_type': map_display_type})

        # Superclass and compositional attributes
        super(Cop, self).__init__(name,
                                  pose=pose,
                                  pose_source=pose_source,
                                  create_mission_planner=False,
                                  goal_planner_cfg=gp_cfg,
                                  path_planner_cfg=pp_cfg,
                                  map_cfg=map_cfg,
                                  color_str='darkgreen')

        # Tracking attributes
        self.other_robot_names = other_robot_names
        self.missing_robber_names = self.other_robot_names['robbers']
        self.distracting_robot_names = self.other_robot_names['distractors']
        self.found_robbers = {}

        # Create mission planner
        self.mission_planner = CopMissionPlanner(self, **mp_cfg)

        # Fusion and sensor attributes
        # <>TODO: Fusion Engine owned and refrenced from imaginary robber?
        self.fusion_engine = FusionEngine(fusion_engine_type,
                                          self.missing_robber_names,
                                          self.map.feasible_layer,
                                          robber_model)
        self.sensors = {}
        self.sensors['camera'] = Camera((0, 0, 0),
                                        element_dict=self.map.element_dict,
                                        **camera_cfg)
        self.map.dynamic_elements.append(self.sensors['camera'].viewcone)

        # Add self to map
        self.map.add_cop(self.map_obj)

        # Make others
        self.make_others()

        # Add human sensor after robbers have been made
        self.ask_every_ten = ask_every_ten
        self.sensors['human'] = Human(self.map, **human_cfg)
        self.map.add_human_sensor(self.sensors['human'])
        self.questioner = Questioner(human_sensor=self.sensors['human'],
                                     **q_cfg)

    def make_others(self):
        # <>TODO: Make generic, so each robot has an idea of all others
        # <>TODO: Move to back to Robot
        """Generate robot objects for all other robots.

        Create personal belief (not necessarily true!) of other robots,
        largely regarding their map positions. Their positions are
        known to the 'self' robot, but this function will be expanded
        in the future to include registration between robots: i.e.,
        optional pose and information sharing instead of predetermined
        sharing.

        """

        # Robot  MapObject
        shape_pts = Point([0, 0, 0]).buffer(iRobotCreate.DIAMETER / 2)\
            .exterior.coords

        # <>TODO: Implement imaginary class for more robust models
        self.missing_robbers = {}
        for name in self.missing_robber_names:
            self.missing_robbers[name] = ImaginaryRobot(name)
            # Add robber objects to map
            self.missing_robbers[name].map_obj = MapObject(name,
                                                           shape_pts[:],
                                                           has_relations=False,
                                                           blocks_camera=False,
                                                           color_str='none')
            # <>TODO: allow no display individually for each robber
            self.map.add_robber(self.missing_robbers[name].map_obj)
            # All will be at 0,0,0 until actually pose is given.
            # init_pose =
            # self.missing_robbers[name].map_obj.move_absolute(init_pose)
        self.distracting_robots = {}
        for name in self.distracting_robot_names:
            self.distracting_robots[name] = ImaginaryRobot(name)
            self.distracting_robots[name].map_obj = MapObject(name,
                                                              shape_pts[:],
                                                              has_relations=False,
                                                              blocks_camera=False,
                                                              color_str='none')
            self.map.add_robot(self.distracting_robots[name].map_obj)
            # <>TODO: Add config similar to plot_robbers in map_cfg
            self.distracting_robots[name].map_obj.visible = False

    def update(self, i=0):
        super(Cop, self).update(i=i)

        # Update sensor and fusion information
        # irobber - Imaginary robber
        for irobber in self.missing_robbers.values():
            point = Point(irobber.pose2D.pose[0:2])
            # Try to visually spot a robber
            if self.sensors['camera'].viewcone.shape.contains(point):
                self.map.found_robber(irobber.map_obj)
                logging.info('{} captured!'.format(irobber.name))
                self.mission_planner.found_robber(irobber.name)
                self.fusion_engine.filters[irobber.name].robber_detected(irobber.pose2D.pose)
                self.found_robbers.update({irobber.name:
                                           self.missing_robbers.pop(irobber.name)})
                self.questioner.remove_target(irobber.name)

            # Update robber's shapes
            else:
                self.missing_robbers[irobber.name].map_obj.move_absolute(
                    irobber.pose2D.pose)

        for idistractor in self.distracting_robots.values():
            point = Point(idistractor.pose2D.pose[0:2])
            # Try to visually spot a robot
            if self.sensors['camera'].viewcone.shape.contains(point):
                logging.info('{} found, but it is not a robber!'
                             .format(idistractor.name))
                if not idistractor.map_obj.visible:
                    idistractor.map_obj.visible = True
                    idistractor.map_obj.color = 'cornflowerblue'

            # Update robber's shapes
            self.distracting_robots[idistractor.name].map_obj.move_absolute(
                idistractor.pose2D.pose)

        # Update probability model
        self.fusion_engine.update(self.pose2D.pose, self.sensors,
                                  self.missing_robbers)

        # Ask a question every 10th step
        if i % 15 == 9 and self.fusion_engine.filter_type == 'gauss sum' and \
           self.ask_every_ten:
            # <>TODO: Key error, make sure target is reassigned.
            priors = {}
            for name, filter_ in self.fusion_engine.filters.iteritems():
                if name == 'combined':
                    continue
                priors[name] = filter_.probability
                priors[name]._discretize(bounds=self.map.bounds,
                                          grid_spacing=0.1)
            self.questioner.ask(priors)