Example #1
0
    def __init__(self, node_name):

        # initialize DTROS parent class
        super(RescueAgentNode, self).__init__(node_name=node_name)

        # inialize attributes
        self.veh_name = rospy.get_param("~distressed_veh")
        self.veh_id = int(''.join([x for x in self.veh_name if x.isdigit()]))
        self.activated = False
        self.autobot_info = AutobotInfo(self.veh_id)
        # map file
        map_file_path = os.path.join(
            "/duckietown-world/src/duckietown_world/data/gd1",
            "maps/{}.yaml".format(os.environ["MAP_NAME"]),
        )
        self.map = SimpleMap(map_file_path)

        self.current_car_cmd = Twist2DStamped()
        self.current_car_cmd.v = 0
        self.current_car_cmd.omega = 0

        self.controller_counter = 0
        self.v_ref = V_REF_CONTROLLER

        self.desired_pos = None
        self.desired_heading = None

        # closed loop controller
        self.controller = StuckController(k_P=KP_CONTROLLER,
                                          k_I=KI_CONTROLLER,
                                          c1=C1_CONTROLLER,
                                          c2=C2_CONTROLLER)
        self.tileType = None

        # Subscriber
        self.sub_distress_classification = rospy.Subscriber(
            "/{}/distress_classification".format(self.veh_name), String,
            self.cb_distress_classification)

        self.sub_autobot_info = rospy.Subscriber(
            "/{}/autobot_info".format(self.veh_name), AutobotInfoMsg,
            self.cb_autobot_info)

        # Publishers
        self.pub_rescueDone = rospy.Publisher("{}/rescueDone/".format(
            self.veh_name),
                                              BoolStamped,
                                              queue_size=1)
        self.pub_car_cmd = rospy.Publisher(
            "/{}/lane_recovery_node/car_cmd".format(self.veh_name),
            Twist2DStamped,
            queue_size=1,
        )
        self.pub_rescueStopped = rospy.Publisher("{}/rescueStopped/".format(
            self.veh_name),
                                                 BoolStamped,
                                                 queue_size=1)
Example #2
0
    def __init__(self, node_name):

        # initialize DTROS parent class
        super(RescueCenterNode, self).__init__(node_name=node_name)

        # List of current bots
        self.veh_list = list()
        # Dictionary with up to date information on all bots
        self.id_dict = dict()

        ## parameters
        # threshold distance to consider bot not moving ()
        self.parameters['~dist_thres'] = None
        # number of windows to use for filtering, currently not used
        self.updateParameters()

        # Subscribe to topics online localization
        self.sub_markers = rospy.Subscriber("/cslam_markers",
                                            MarkerArray,
                                            self.cbLocalization,
                                            queue_size=30)
        # for simple-localization
        self.sub_simpleLoc = rospy.Subscriber("/simple_loc_bots",
                                              Float64MultiArray,
                                              self.cbSimpleLoc,
                                              queue_size=30)

        # Prepare for bot-wise topics
        self.pub_trigger = dict()
        self.pub_rescue_classfication = dict()
        self.sub_fsm_states = dict()
        self.sub_rescueDone = dict()
        self.pub_autobot_info = dict()
        self.sub_rescue_stopped = dict()

        # build simpleMap, use the map specified when running container
        map_file_path = os.path.join(
            "/duckietown-world/src/duckietown_world/data/gd1",
            "maps/{}.yaml".format(os.environ["MAP_NAME"]),
        )
        self.map = SimpleMap(map_file_path)
        self.map.display_raw_debug()

        # which bots are being monitored
        self.bot_whitelist = set([])
        self.log("Rescue center monitoring list: {}".format(
            list(self.bot_whitelist)))

        self.log("Rescue Center is initialized!")
Example #3
0
    def setup_level(self):
        self.current_level = self.levels[self.level_num]
        start_state = self.current_level["start_state"]
        #Add the game actors
        actors = {}
        actors["cat"] = Cat("cat", start_state['player'][0],
                            start_state['player'][1])
        for i in range(len(start_state["mice"])):
            m = start_state["mice"][i]
            name = "mouse %d" % i
            actors[name] = Mouse(name, m[0], m[1])

        self.level_map = SimpleMap(self.current_level["map_object"], actors,
                                   self.assets)
        self.level_map.draw_map()

        self.state = STATE.RUN_LEVEL
Example #4
0
    def setup_level(self):
        self.current_level = self.levels[self.level_num]
        start_state = self.current_level["start_state"]
        #Add the game actors
        actors = {}
        actors["cat"] = Cat("cat", start_state['player'][0], start_state['player'][1])
        for i in range (len(start_state["mice"])):
            m = start_state["mice"][i]
            name = "mouse %d" % i
            actors[name] = Mouse(name, m[0], m[1])

        self.level_map = SimpleMap(self.current_level["map_object"], actors, self.assets)
        self.level_map.draw_map()

        self.state = STATE.RUN_LEVEL
Example #5
0
class RescueCenterNode(DTROS):
    """Rescue Center Node

    - Monitors all bots currenty visible on the map and classifies if they are in distress or not
    - Responsible for launching and alarming Rescue Agents 

    Args:
        node_name (): a unique, descriptive name for the node that ROS will use

    Attributes:
        - veh_list (list): A list of all vehicles visible on the map
        - veh_dict (dist): A dict of all vehicles visible on the map (key=id, value=AutobotInfo)
        - parameters: Used to pass certain parameters to the node via rosparam
        - map: A map of type SimpleMap(), necessary for classification
        - The following dictionaries to store the bot_wise subcribers and publishers 
        for communication between rescue center and rescue agents:
            - pub_trigger
            - pub_rescue_classfication
            - sub_fsm_states
            - sub_rescueDone
            - pub_autobot_info
            - sub_rescue_stopped

    Subscriber:
        - sub_markers: /cslam_markers (:obj:`MarkerArray`): 
            Rviz markers from the online localization systems
        - sub_simpleLoc: /simple_loc_bots (:obj: `Float64MultiArray`): 
            Position and heading of the bots from the SimpleLoc system
        - sub_fsm_states[veh_id]: /autobot{}/fsm_mode (:obj:`FSMstate`)
            FSM state from every duckiebot
        - sub_rescueDone[veh_id]: /rescue/rescue_agents/autobot{}/rescueDone (:obj:`BoolStamped`)
            Used to get the signal from the rescue agent, that a rescue operation is done
        - sub_rescueStopped[veh_id]: /rescue/rescue_agents/autobot{}/rescueStopped (:obj:`BoolStamped`)
            Used to get the signal from the rescue agent, that a rescue operation has been stopped, without being finished
            (if monitoring is deactivated during rescue operation)

    Publisher:
        - pub_trigger[veh_id]: /autobot{}/recovery_mode (:obj:`BoolStamped`): 
            Triggers the recovery mode for a certain bot (FSM state change) and activates its rescue agent 
        - pub_rescue_classfication[veh_id]: ~/autobot{}/distress_classification (:obj: `String`):
            Gives the result of the latest classification to each rescue agent
        - pub_autobot_info[veh_id]: /autobot{}/autobot_info (:obj: `AutobotInfoMsg`):
            Passes all collected information about a duckiebot to the rescue agent
    """
    def __init__(self, node_name):

        # initialize DTROS parent class
        super(RescueCenterNode, self).__init__(node_name=node_name)

        # List of current bots
        self.veh_list = list()
        # Dictionary with up to date information on all bots
        self.id_dict = dict()

        ## parameters
        # threshold distance to consider bot not moving ()
        self.parameters['~dist_thres'] = None
        # number of windows to use for filtering, currently not used
        self.updateParameters()

        # Subscribe to topics online localization
        self.sub_markers = rospy.Subscriber("/cslam_markers",
                                            MarkerArray,
                                            self.cbLocalization,
                                            queue_size=30)
        # for simple-localization
        self.sub_simpleLoc = rospy.Subscriber("/simple_loc_bots",
                                              Float64MultiArray,
                                              self.cbSimpleLoc,
                                              queue_size=30)

        # Prepare for bot-wise topics
        self.pub_trigger = dict()
        self.pub_rescue_classfication = dict()
        self.sub_fsm_states = dict()
        self.sub_rescueDone = dict()
        self.pub_autobot_info = dict()
        self.sub_rescue_stopped = dict()

        # build simpleMap, use the map specified when running container
        map_file_path = os.path.join(
            "/duckietown-world/src/duckietown_world/data/gd1",
            "maps/{}.yaml".format(os.environ["MAP_NAME"]),
        )
        self.map = SimpleMap(map_file_path)
        self.map.display_raw_debug()

        # which bots are being monitored
        self.bot_whitelist = set([])
        self.log("Rescue center monitoring list: {}".format(
            list(self.bot_whitelist)))

        self.log("Rescue Center is initialized!")

    def updateSubscriberPublisher(self, veh_id):
        """Updates Subscriber and Publisher dictionaries, if new duckiebot is added
        
        Args: 
            veh_id: ID of the new duckiebot spotted by the localization system (int)
        """

        # 1. Publisher: Trigger
        self.pub_trigger[veh_id] = rospy.Publisher(
            "/autobot{}/recovery_mode".format(veh_id),
            BoolStamped,
            queue_size=1,
        )
        # 2. Publisher: Distress classification
        self.pub_rescue_classfication[veh_id] = rospy.Publisher(
            "~/autobot{}/distress_classification".format(veh_id),
            String,
            queue_size=1,
        )
        # 3. Subscriber: FSM state
        self.sub_fsm_states[veh_id] = rospy.Subscriber(
            "/autobot{}/fsm_mode".format(veh_id),
            FSMState,
            self.cbFSM,
            callback_args=veh_id,
        )
        # 4. Subscriber: EverythingOK from Rescue_agend
        self.sub_rescueDone[veh_id] = rospy.Subscriber(
            "/rescue/rescue_agents/autobot{}/rescueDone".format(veh_id),
            BoolStamped,
            self.cbRescueDone,
            callback_args=veh_id,
        )
        # 5. Publisher: AutobotInfo Msg
        self.pub_autobot_info[veh_id] = rospy.Publisher(
            "/autobot{}/autobot_info".format(veh_id),
            AutobotInfoMsg,
            queue_size=5,
        )

        # 6. Subscriber: Rescue stopped (not finished)
        self.sub_rescue_stopped[veh_id] = rospy.Subscriber(
            "/rescue/rescue_agents/autobot{}/rescueStopped/".format(veh_id),
            BoolStamped,
            self.cbRescueStopped,
            callback_args=veh_id,
        )

    def cbRescueStopped(self, msg, veh_id):
        """Callback of RescueStopped:
            - gets triggered, if rescue_agent has stopped rescue, but rescue is not finished yet. 
            (happens, if autobot is stopped being monitored during a rescue operation)
    
        Args: 
            msg: ROS message (BoolStamped)
            veh_id: autobot ID
        """
        if msg.data:
            # Note: this should always be the case
            self.log("Received \"rescue stop\" for bot <{}>".format(veh_id))
            self.id_dict[veh_id].in_rescue = False
            self.id_dict[veh_id].last_moved = self.id_dict[veh_id].timestamp

    def cbFSM(self, msg, veh_id):
        """Callback when the fsm state of a bot changed
            
            Args: 
                msg: ROS message (Float64Array)
                veh_id: ID of duckiebot
            """

        if veh_id in self.id_dict:
            self.log(
                "Duckiebot [{}] FSM state changed from <{}> to <{}>".format(
                    veh_id, self.id_dict[veh_id].fsm_state, msg.state))
            self.id_dict[veh_id].fsm_state = msg.state

    def cbSimpleLoc(self, msg):
        """Callback of simpleLoc: 
                - stores position and heading from simple Localization into the corresponding AutobotInfo() dictionary
                (Note: simpleLoc publishes for one duckiebot at a time)
                - determines msg timestamp as the time, where the duckiebot has moved last 
                (Note: simpleLoc only publishes, if duckiebot moved)
                - publishes autobotInfo to corresponding duckiebot
        
        Args: 
            msg: ROS message (Float64MultiArray)
        """

        veh_id = int(msg.data[0])
        if veh_id in self.id_dict:
            # make sure duckiebot has already been spotted by global localization
            # (need it, in case simpleLoc msg comes in before the first /cslam msg)
            self.id_dict[veh_id].positionSimple = (msg.data[1], msg.data[2])
            self.id_dict[veh_id].headingSimple = msg.data[3]
            self.id_dict[veh_id].current_pos = (msg.data[1], msg.data[2])
            self.id_dict[veh_id].current_heading = msg.data[3]
        self.id_dict[veh_id].last_movedSimple = msg.data[4]  # in s
        self.pub_autobot_info[veh_id].publish(
            self.id_dict[veh_id].autobotInfo2Msg())

    def cbRescueDone(self, msg, veh_id):
        """Callback of RescueAgent, which signals that the rescue operation is over
         - rescue center will take over from rescue_agent
         - trigger state change from "Recovery" to "Lane Following" 
        
        Args: 
            msg: ROS message (Float64Array)
            veh_id: ID of duckiebot
        """

        if msg.data == True:
            self.id_dict[veh_id].in_rescue = False
            self.id_dict[veh_id].last_moved = self.id_dict[veh_id].timestamp
            msg = BoolStamped()
            msg.data = False
            self.pub_trigger[veh_id].publish(msg)  # turns on lane following

    def cbLocalization(self, msg):
        """ Callback when receiving online localization results:
            Checks if there are any new bots on the map
            saves position/heading/lastMoved into corresponding autobotInfo
            calls the Classifier for each bot that is currently not in rescue operation and for which rescue is activated

        Args: 
            msg: ROS visualization message (MarkerArray)
        """

        for m in msg.markers:

            # care only about duckiebots
            if m.ns != "duckiebots":
                continue

            idx = m.id
            # Append new bots to veh_list and launch a rescue_agent for each
            if not idx in self.veh_list:
                self.veh_list.append(idx)
                self.id_dict[idx] = AutobotInfo(idx)
                # create relevant topic handlers for this bot
                self.updateSubscriberPublisher(idx)
                # create rescue agent for this bot
                with open(os.devnull, 'w') as fp:
                    subprocess.Popen([
                        "roslaunch", "rescue_center", "rescue_agent.launch",
                        "botID:={}".format(idx)
                    ],
                                     stdout=fp)
                # wait until regard node has launched
                sleep(3)
                self.log("Launched rescue agent for bot <{}>".format(idx))

            # Store position from localization system in AutoboInfo()
            self.id_dict[idx].update_from_marker(m)

            # Filter position and update last_moved time stamp
            self.id_dict[idx].update_filtered(self.parameters['~dist_thres'])
            # publish autobot_info to rescue_agent
            self.pub_autobot_info[idx].publish(
                self.id_dict[idx].autobotInfo2Msg())

            # Check, if duckiebot is still monitored:
            add_bot = rospy.get_param('~add_duckiebot')
            rm_bot = rospy.get_param('~remove_duckiebot')
            rospy.set_param('~add_duckiebot', -1)
            rospy.set_param('~remove_duckiebot', -1)
            whitelist_updated = False

            # asked to add, not yet added
            if add_bot in self.id_dict and add_bot not in self.bot_whitelist:
                # append to whitelist
                self.bot_whitelist.add(add_bot)
                whitelist_updated = True
                # activate rescue system for this bot
                self.id_dict[add_bot].classificationActivated = True
                # to prevent time_diff being triggered
                self.id_dict[add_bot].last_moved = m.header.stamp.to_sec()
                self.id_dict[add_bot].last_movedSimple = m.header.stamp.to_sec(
                )
            if rm_bot in self.id_dict and rm_bot in self.bot_whitelist:
                # remove from whitelist
                self.bot_whitelist.discard(rm_bot)
                whitelist_updated = True
                # deactivate rescue system
                self.id_dict[rm_bot].classificationActivated = False

            if whitelist_updated:
                self.log("Rescue center monitoring list: {}".format(
                    list(self.bot_whitelist)))

            # continue, if we don't want to classify that duckiebot
            if not self.id_dict[idx].classificationActivated:
                continue
            # continue, if duckiebot is in rescue
            if self.id_dict[idx].in_rescue:
                continue

            # Classify duckiebot
            rescue_class = self.id_dict[idx].classifier(self.map)
            self.id_dict[idx].rescue_class = rescue_class

            self.log(
                "[{}] ".format(idx) +
                "not moved for: {}, ".format(self.id_dict[idx].time_diff) +
                "onRoad: {}, ".format(self.id_dict[idx].onRoad) +
                "delta_phi: {}, ".format(self.id_dict[idx].delta_phi) +
                "tile_type: {}".format(self.id_dict[idx].tile_type))

            if rescue_class.value != 0:
                # publish rescue_class
                self.log("[{}] in distress <{}>".format(idx, rescue_class))
                msg = BoolStamped()
                msg.data = True
                # turn duckiebot into rescue mode
                self.pub_trigger[idx].publish(msg)
                # notify rescue agent of the classification result
                self.pub_rescue_classfication[idx].publish(
                    str(rescue_class.value))
                # note down so can skip the next time
                self.id_dict[idx].in_rescue = True
Example #6
0
class Game(object):
    @staticmethod
    def asset(name):
        return os.path.join(ASSET_BASE, name)

    def __init__(self, name=DEFAULT_GAME_NAME, levels=DEFAULT_LEVEL_FILE):
        pygame.init()

        #Setup frames per second clock
        self.fpsclock = pygame.time.Clock()
        #Setup the main display surface
        self.display_surface = pygame.display.set_mode((WINWIDTH, WINHEIGHT))
        pygame.display.set_caption(name)

        #Setup the fonts
        self.basic_font = pygame.font.Font(DEFAULT_BASIC_FONT, 18)

        self.assets = {
            'uncovered goal': pygame.image.load(Game.asset('RedSelector.png')),
            'covered goal': pygame.image.load(Game.asset('Selector.png')),
            'star': pygame.image.load(Game.asset('Star.png')),
            'corner': pygame.image.load(Game.asset('Wall_Block_Tall.png')),
            'wall': pygame.image.load(Game.asset('Wood_Block_Tall.png')),
            'inside floor': pygame.image.load(Game.asset('Plain_Block.png')),
            'outside floor': pygame.image.load(Game.asset('Grass_Block.png')),
            'title': pygame.image.load(Game.asset('the_path_title.png')),
            'solved': pygame.image.load(Game.asset('star_solved.png')),
            'princess': pygame.image.load(Game.asset('princess.png')),
            'cat': pygame.image.load(Game.asset('cat.png')),
            'mouse': pygame.image.load(Game.asset('mouse.png')),
            'catgirl': pygame.image.load(Game.asset('catgirl.png')),
            'horngirl': pygame.image.load(Game.asset('horngirl.png')),
            'pinkgirl': pygame.image.load(Game.asset('pinkgirl.png')),
            'rock': pygame.image.load(Game.asset('Rock.png')),
            'short tree': pygame.image.load(Game.asset('Tree_Short.png')),
            'tall tree': pygame.image.load(Game.asset('Tree_Tall.png')),
            'ugly tree': pygame.image.load(Game.asset('Tree_Ugly.png'))
        }

        self.levels = []
        self.level_map = None
        self.read_levels_file(levels)
        self.main_character = 'cat'
        #Starting Level
        self.level_num = 0
        self.current_level = None
        self.state = STATE.START
        self.manual_mode = True

    def terminate(self):
        pygame.quit()
        sys.exit()

    def loop(self):
        if self.state == STATE.START:
            self.start_screen()

        elif self.state == STATE.SETUP_LEVEL:
            self.setup_level()

        elif self.state == STATE.RUN_LEVEL:
            self.run_level()

    def read_levels_file(self, filepath):
        if not os.path.exists(filepath):
            filepath = Game.asset(filepath)
        assert os.path.exists(
            filepath), "Cannot find the level file: %s" % (filepath)

        map_file = open(filepath, 'r')
        #Each Level must end with a blank line
        content = map_file.readlines() + ['\r\n']
        map_file.close()

        levels = []  #Contains a list of levels
        level_num = 0
        map_text_lines = []  #Contains the lines for a single level's map
        map_object = []  #The map objects made from the data in map text lines

        for line_num in range(len(content)):
            #Process each line
            line = content[line_num].rstrip('\r\n')
            if ';' in line:
                #Remove comments
                line = line[:line.find(';')]

            if line != '':
                #Usable Map Data
                map_text_lines.append(line)

            #Finished with data
            elif line == '' and len(map_text_lines) > 0:
                #Encountered a blank line, this is the end of a level map
                #Convert the text into an actual level
                max_width = -1
                for i in range(len(map_text_lines)):
                    if len(map_text_lines[i]) > max_width:
                        #Adjust width
                        max_width = len(map_text_lines[i])

                #Add spaces to the end of the shorter rows. This ensures the map is
                #rectangular
                for i in range(len(map_text_lines)):
                    map_text_lines[i] += ' ' * (max_width -
                                                len(map_text_lines[i]))

                #Convert map text lines to a map object
                for x in range(len(map_text_lines[0])):
                    map_object.append([])

                #Convert list of string to 2D Matrix of objects
                for y in range(len(map_text_lines)):
                    for x in range(max_width):
                        map_object[x].append(map_text_lines[y][x])

                #Loop through the spaes in a map and find the @, ., and $
                #Characters for the starting game state
                startx = None
                starty = None
                goals = []
                mice = []
                for x in range(max_width):
                    for y in range(len(map_object[x])):
                        if map_object[x][y] in ('@', '+'):
                            startx = x
                            starty = y
                        if map_object[x][y] in ('.', '+', '*'):
                            # '.' is goal, '*' is star & goal
                            goals.append((x, y))
                        if map_object[x][y] in ('$', '*'):
                            mice.append((x, y))

                #Sanity Checks
                assert startx != None and starty != None, "Level %s (around line %s) in %s is missing a '@' or '+' to make the start point." % (
                    level_num + 1, line_num, filepath)
                #assert len(goals) > 0, 'level %s (around line %s) in %s must have at least one goal.' % (level_num + 1, line_num, filepath)
                #assert len(mice) <= len(goals), 'level %s (around line %s) in %s is impossible to solve. It has %s goals but ony %s mice.' % (level_num + 1, line_num, filepath, len(goals), len(mice))

                game_state = {
                    'player': (startx, starty),
                    'step_counter': 0,
                    'mice': mice
                }

                level_object = {
                    'width': max_width,
                    'height': len(map_object),
                    'map_object': map_object,
                    'goals': goals,
                    'start_state': game_state,
                    'state': None
                }

                self.levels.append(level_object)

                map_text_lines = []
                map_object = []
                game_state = {}
                level_num += 1

    def start_screen(self):
        """
        Display the start screen
        """
        title_rect = self.assets['title'].get_rect()
        top_coord = 50
        title_rect.top = top_coord
        title_rect.centerx = HALF_WINWIDTH
        top_coord += title_rect.height

        # Unfortunately, Pygame's font & text system only shows one line at
        # a time, so we can't use strings with \n newline characters in them.
        # So we will use a list with each line in it.
        instruction_text = [
            'Path... or not, it\'s up to you...',
            'Arrow keys to move, WASD for camera control, P to change character.',
            'Backspace to reset level, Esc to quit.',
            'N for next level, B to go back a level.'
        ]

        #Start Drawing a blank color to the entire window
        self.display_surface.fill(BGCOLOR)
        self.display_surface.blit(self.assets["title"], title_rect)

        #Position and draw the text
        for i in range(len(instruction_text)):
            inst_surf = self.basic_font.render(instruction_text[i], 1,
                                               TEXTCOLOR)
            inst_rect = inst_surf.get_rect()
            top_coord += 10  # 10 Pixels will go in between each line of text
            inst_rect.top = top_coord
            inst_rect.centerx = HALF_WINWIDTH
            top_coord += inst_rect.height
            self.display_surface.blit(inst_surf, inst_rect)

        while True:
            #Loop till the user presses something
            for event in pygame.event.get():
                if event.type == QUIT:
                    self.terminate()
                elif event.type == KEYDOWN:
                    if event.key == K_ESCAPE:
                        self.terminate()
                    self.state = STATE.SETUP_LEVEL
                    return

            pygame.display.update()
            self.fpsclock.tick()

#Level Specific Functions

    def run_level(self):
        key_press = False
        player_move_to = None
        state = self.current_level['state']
        for event in pygame.event.get():  # event handling loop
            if event.type == QUIT:
                # Player clicked the "X" at the corner of the window.
                self.terminate()

            if event.type == KEYDOWN:
                key_press = True
                if event.key == K_m:
                    self.manual_mode = not self.manual_mode
                if self.manual_mode:
                    if event.key == K_LEFT:
                        player_move_to = DIRECTIONS.LEFT
                    if event.key == K_RIGHT:
                        player_move_to = DIRECTIONS.RIGHT
                    if event.key == K_UP:
                        player_move_to = DIRECTIONS.UP
                    if event.key == K_DOWN:
                        player_move_to = DIRECTIONS.DOWN

        redraw_map = False

        actors = self.level_map.get_actors()
        if self.manual_mode:
            if player_move_to is not None:
                #print "Move:"
                actors[self.main_character].move(player_move_to)
                self.level_map.draw_map()
            '''
            #if player_move_to is not None:
                #cat = self.players["cat"]
                #moved = cat.make_move(self.players["cat"], player_move_to)
            
                #if moved:
                #    state['step_counter'] += 1
                #    self.draw_map() 
            '''

            self.display_surface.fill(BGCOLOR_MANUAL)

        else:
            #Use the automatic movement
            self.display_surface.fill(BGCOLOR)
            for name in actors:
                actors[name].process(None)
                self.level_map.draw_map()

        map_surface = self.level_map.get_level_surface()
        map_surface_rect = map_surface.get_rect()
        map_surface_rect.center = (HALF_WINWIDTH, HALF_WINHEIGHT)

        self.display_surface.blit(map_surface, map_surface_rect)

        pygame.display.update()
        self.fpsclock.tick()

    def setup_level(self):
        self.current_level = self.levels[self.level_num]
        start_state = self.current_level["start_state"]
        #Add the game actors
        actors = {}
        actors["cat"] = Cat("cat", start_state['player'][0],
                            start_state['player'][1])
        for i in range(len(start_state["mice"])):
            m = start_state["mice"][i]
            name = "mouse %d" % i
            actors[name] = Mouse(name, m[0], m[1])

        self.level_map = SimpleMap(self.current_level["map_object"], actors,
                                   self.assets)
        self.level_map.draw_map()

        self.state = STATE.RUN_LEVEL
Example #7
0
class Game(object):

    @staticmethod
    def asset(name):
        return os.path.join(ASSET_BASE, name)

    def __init__(self, name = DEFAULT_GAME_NAME, levels = DEFAULT_LEVEL_FILE):
        pygame.init()

        #Setup frames per second clock
        self.fpsclock = pygame.time.Clock()
        #Setup the main display surface
        self.display_surface = pygame.display.set_mode((WINWIDTH, WINHEIGHT))
        pygame.display.set_caption(name)

        #Setup the fonts
        self.basic_font = pygame.font.Font(DEFAULT_BASIC_FONT, 18)

        self.assets = { 'uncovered goal': pygame.image.load(Game.asset('RedSelector.png')),
                        'covered goal':   pygame.image.load(Game.asset('Selector.png')),
                        'star':           pygame.image.load(Game.asset('Star.png')),
                        'corner':         pygame.image.load(Game.asset('Wall_Block_Tall.png')),
                        'wall':           pygame.image.load(Game.asset('Wood_Block_Tall.png')),
                        'inside floor':   pygame.image.load(Game.asset('Plain_Block.png')),
                        'outside floor':  pygame.image.load(Game.asset('Grass_Block.png')),
                        'title':          pygame.image.load(Game.asset('the_path_title.png')),
                        'solved':         pygame.image.load(Game.asset('star_solved.png')),
                        'princess':       pygame.image.load(Game.asset('princess.png')),
                        'cat':            pygame.image.load(Game.asset('cat.png')),
                        'mouse':          pygame.image.load(Game.asset('mouse.png')),
                        'catgirl':        pygame.image.load(Game.asset('catgirl.png')),
                        'horngirl':       pygame.image.load(Game.asset('horngirl.png')),
                        'pinkgirl':       pygame.image.load(Game.asset('pinkgirl.png')),
                        'rock':           pygame.image.load(Game.asset('Rock.png')),
                        'short tree':     pygame.image.load(Game.asset('Tree_Short.png')),
                        'tall tree':      pygame.image.load(Game.asset('Tree_Tall.png')),
                        'ugly tree':      pygame.image.load(Game.asset('Tree_Ugly.png'))}


        self.levels = []
        self.level_map = None
        self.read_levels_file(levels)
        self.main_character = 'cat'
        #Starting Level
        self.level_num = 0
        self.current_level = None
        self.state = STATE.START
        self.manual_mode = True

    def terminate(self):
        pygame.quit()
        sys.exit()

    def loop(self):
        if self.state == STATE.START:
            self.start_screen()

        elif self.state == STATE.SETUP_LEVEL:
            self.setup_level()

        elif self.state == STATE.RUN_LEVEL:
            self.run_level()

    def read_levels_file(self, filepath):
        if not os.path.exists(filepath):
            filepath = Game.asset(filepath)
        assert os.path.exists(filepath), "Cannot find the level file: %s" % (filepath)

        map_file = open(filepath, 'r')
        #Each Level must end with a blank line
        content = map_file.readlines() + ['\r\n']
        map_file.close()

        levels = []             #Contains a list of levels
        level_num = 0
        map_text_lines = []     #Contains the lines for a single level's map
        map_object = []         #The map objects made from the data in map text lines

        for line_num in range(len(content)):
            #Process each line
            line = content[line_num].rstrip('\r\n')
            if ';' in line:
                #Remove comments
                line = line[:line.find(';')]

            if line != '':
                #Usable Map Data
                map_text_lines.append(line)

            #Finished with data
            elif line == '' and len(map_text_lines) > 0:
                #Encountered a blank line, this is the end of a level map
                #Convert the text into an actual level
                max_width = -1
                for i in range(len(map_text_lines)):
                    if len(map_text_lines[i]) > max_width:
                        #Adjust width
                        max_width = len(map_text_lines[i])

                #Add spaces to the end of the shorter rows. This ensures the map is
                #rectangular
                for i in range(len(map_text_lines)):
                    map_text_lines[i] += ' ' * (max_width - len(map_text_lines[i]))

                #Convert map text lines to a map object
                for x in range(len(map_text_lines[0])):
                    map_object.append([])

                #Convert list of string to 2D Matrix of objects
                for y in range(len(map_text_lines)):
                    for x in range(max_width):
                        map_object[x].append(map_text_lines[y][x])

                #Loop through the spaes in a map and find the @, ., and $
                #Characters for the starting game state
                startx = None
                starty = None
                goals = []
                mice = []
                for x in range(max_width):
                    for y in range(len(map_object[x])):
                        if map_object[x][y] in ('@', '+'):
                            startx = x
                            starty = y
                        if map_object[x][y] in ('.', '+', '*'):
                            # '.' is goal, '*' is star & goal
                            goals.append((x, y))
                        if map_object[x][y] in ('$', '*'):
                            mice.append((x, y))

                #Sanity Checks
                assert startx != None and starty != None, "Level %s (around line %s) in %s is missing a '@' or '+' to make the start point." %(level_num + 1, line_num, filepath)
                #assert len(goals) > 0, 'level %s (around line %s) in %s must have at least one goal.' % (level_num + 1, line_num, filepath)
                #assert len(mice) <= len(goals), 'level %s (around line %s) in %s is impossible to solve. It has %s goals but ony %s mice.' % (level_num + 1, line_num, filepath, len(goals), len(mice))

                game_state = {  'player':       (startx, starty),
                                'step_counter': 0,
                                'mice':        mice}

                level_object = {'width':        max_width,
                                'height':       len(map_object),
                                'map_object':   map_object,
                                'goals':        goals,
                                'start_state':  game_state,
                                'state':        None}

                self.levels.append(level_object)

                map_text_lines = []
                map_object = []
                game_state = {}
                level_num += 1

    def start_screen(self):
        """
        Display the start screen
        """
        title_rect = self.assets['title'].get_rect()
        top_coord = 50
        title_rect.top = top_coord
        title_rect.centerx = HALF_WINWIDTH
        top_coord += title_rect.height

        # Unfortunately, Pygame's font & text system only shows one line at
        # a time, so we can't use strings with \n newline characters in them.
        # So we will use a list with each line in it.
        instruction_text = ['Path... or not, it\'s up to you...',
                            'Arrow keys to move, WASD for camera control, P to change character.',
                            'Backspace to reset level, Esc to quit.',
                            'N for next level, B to go back a level.']

        #Start Drawing a blank color to the entire window
        self.display_surface.fill(BGCOLOR)
        self.display_surface.blit(self.assets["title"], title_rect)

        #Position and draw the text
        for i in range(len(instruction_text)):
            inst_surf = self.basic_font.render(instruction_text[i], 1, TEXTCOLOR)
            inst_rect = inst_surf.get_rect()
            top_coord += 10 # 10 Pixels will go in between each line of text
            inst_rect.top = top_coord
            inst_rect.centerx = HALF_WINWIDTH
            top_coord += inst_rect.height
            self.display_surface.blit(inst_surf, inst_rect)

        while True:
            #Loop till the user presses something
            for event in pygame.event.get():
                if event.type == QUIT:
                    self.terminate()
                elif event.type == KEYDOWN:
                    if event.key == K_ESCAPE:
                        self.terminate()
                    self.state = STATE.SETUP_LEVEL
                    return

            pygame.display.update()
            self.fpsclock.tick()

#Level Specific Functions
    def run_level(self):
        key_press = False
        player_move_to = None
        state = self.current_level['state']
        for event in pygame.event.get(): # event handling loop
            if event.type == QUIT:
                # Player clicked the "X" at the corner of the window.
                self.terminate()

            if event.type == KEYDOWN:
                key_press = True
                if event.key == K_m:
                    self.manual_mode = not self.manual_mode
                if self.manual_mode:
                    if event.key == K_LEFT:
                        player_move_to = DIRECTIONS.LEFT
                    if event.key == K_RIGHT:
                        player_move_to = DIRECTIONS.RIGHT
                    if event.key == K_UP:
                        player_move_to = DIRECTIONS.UP
                    if event.key == K_DOWN:
                        player_move_to = DIRECTIONS.DOWN

        redraw_map = False

        actors = self.level_map.get_actors()
        if self.manual_mode:
            if player_move_to is not None:
                #print "Move:"
                actors[self.main_character].move(player_move_to)
                self.level_map.draw_map()
            '''
            #if player_move_to is not None:
                #cat = self.players["cat"]
                #moved = cat.make_move(self.players["cat"], player_move_to)
            
                #if moved:
                #    state['step_counter'] += 1
                #    self.draw_map() 
            '''
            
            self.display_surface.fill(BGCOLOR_MANUAL)


        else:
            #Use the automatic movement
            self.display_surface.fill(BGCOLOR)
            for name in actors:
                actors[name].process(None)
                self.level_map.draw_map()

        map_surface = self.level_map.get_level_surface()
        map_surface_rect = map_surface.get_rect()
        map_surface_rect.center = (HALF_WINWIDTH, HALF_WINHEIGHT)

        self.display_surface.blit(map_surface, map_surface_rect)

        pygame.display.update()
        self.fpsclock.tick()

    def setup_level(self):
        self.current_level = self.levels[self.level_num]
        start_state = self.current_level["start_state"]
        #Add the game actors
        actors = {}
        actors["cat"] = Cat("cat", start_state['player'][0], start_state['player'][1])
        for i in range (len(start_state["mice"])):
            m = start_state["mice"][i]
            name = "mouse %d" % i
            actors[name] = Mouse(name, m[0], m[1])

        self.level_map = SimpleMap(self.current_level["map_object"], actors, self.assets)
        self.level_map.draw_map()

        self.state = STATE.RUN_LEVEL
Example #8
0
class RescueAgentNode(DTROS):
    """Rescue Agent Node

    - takes care of rescue operation, once duckiebot has been classified as "distressed" (from rescue center).
    - a "Rescue Agent" is launched for every duckiebot, which has been spotted by the localization system
    (in passive mode for non-distressed duckiebots)

    Args:
        node_name (): a unique, descriptive name for the node that ROS will use

    Attributes:
        - veh_name (str): name of the vehicle, e.g. autobot27
        - veh_id (int): ID of the vehicle, e.g. 27
        - activated (boolean): rescue agent sends rescue commands to duckiebot, if and only if self.activated == True
        - autobot_info(:obj:`AutobotInfo`): object, where all current information of the duckiebot is saved (e.g. position, heading, etc.)
        - map (:obj:`SimpleMap`): map object providing functions for calculating desired position and heading,
                                reads in YAML file --> works for other maps as well

        - v_ref(float): backward velocity during closed loop rescue
        - controller_counter (int): counting up, after each rescue cmd. Stops after maximum number of rescue cmds
        - desired_pos(tuple (float, float)): current desired position for rescue operation
        - desired_heading (float): current desired heading for rescue operation (-180 DEG --> 180 DEG, 0 DEG facing positive x-Direction)
        - current_car_cmd (:obj:`Twist2DStamped`): car command, which is being sent to the duckiebot
        - tile_type (str):  tile type, the duckiebot is driving/standing on: e.g. "asphalt", "curve", etc.

    Subscriber:
        - sub_distress_classification: /[self.veh_name]/distress_classification (:obj:`String`):
            distress classification from rescue center as rescue_class.value (int converted to String)
        - sub_autobot_info: /[self.veh_name]/autobot_info (:obj: `AutobotInfoMsg`):
            AutobotInfo message from rescue_center

    Publisher:
        - pub_rescueDone: [self.veh_name]/rescueDone/ (:obj:`BoolStamped`):
            publishes, if rescue operation has been finished
            1. to FSM node, True triggers state transition to LANE_FOLLOWING
            2. to rescue_center: True activates monitoring of duckiebot
        - pub_car_cmd: /[self.veh_name]/lane_recovery_node/car_cmd (:obj: `Twist2DStamped`):
            publishes rescue commands to car_cmd_switch_node
        - pub_test: /rescue_agents/test: test publisher to monitor certain values for debugging
    """
    def __init__(self, node_name):

        # initialize DTROS parent class
        super(RescueAgentNode, self).__init__(node_name=node_name)

        # inialize attributes
        self.veh_name = rospy.get_param("~distressed_veh")
        self.veh_id = int(''.join([x for x in self.veh_name if x.isdigit()]))
        self.activated = False
        self.autobot_info = AutobotInfo(self.veh_id)
        # map file
        map_file_path = os.path.join(
            "/duckietown-world/src/duckietown_world/data/gd1",
            "maps/{}.yaml".format(os.environ["MAP_NAME"]),
        )
        self.map = SimpleMap(map_file_path)

        self.current_car_cmd = Twist2DStamped()
        self.current_car_cmd.v = 0
        self.current_car_cmd.omega = 0

        self.controller_counter = 0
        self.v_ref = V_REF_CONTROLLER

        self.desired_pos = None
        self.desired_heading = None

        # closed loop controller
        self.controller = StuckController(k_P=KP_CONTROLLER,
                                          k_I=KI_CONTROLLER,
                                          c1=C1_CONTROLLER,
                                          c2=C2_CONTROLLER)
        self.tileType = None

        # Subscriber
        self.sub_distress_classification = rospy.Subscriber(
            "/{}/distress_classification".format(self.veh_name), String,
            self.cb_distress_classification)

        self.sub_autobot_info = rospy.Subscriber(
            "/{}/autobot_info".format(self.veh_name), AutobotInfoMsg,
            self.cb_autobot_info)

        # Publishers
        self.pub_rescueDone = rospy.Publisher("{}/rescueDone/".format(
            self.veh_name),
                                              BoolStamped,
                                              queue_size=1)
        self.pub_car_cmd = rospy.Publisher(
            "/{}/lane_recovery_node/car_cmd".format(self.veh_name),
            Twist2DStamped,
            queue_size=1,
        )
        self.pub_rescueStopped = rospy.Publisher("{}/rescueStopped/".format(
            self.veh_name),
                                                 BoolStamped,
                                                 queue_size=1)

    def cb_autobot_info(self, msg):
        """Callback function triggered by self.sub_autobot_info:
                saves msg into self.autobot_info

            Args:
            - msg (:obj:`AutobotInfoMsg`): received message

            Parameters: None

            Returns: None
        """
        self.autobot_info.timestamp = msg.timestamp
        self.autobot_info.fsm_state = msg.fsm_state
        self.autobot_info.filtered = (msg.filtered[0], msg.filtered[1])
        self.autobot_info.last_moved = msg.last_moved
        self.autobot_info.last_movedSimple = msg.last_movedSimple
        self.autobot_info.in_rescue = msg.in_rescue
        self.autobot_info.onRoad = msg.onRoad

        self.autobot_info.current_heading = msg.current_heading
        self.autobot_info.current_pos = (msg.current_pos[0],
                                         msg.current_pos[1])

        self.autobot_info.classificationActivated = msg.classificationActivated

    def cb_distress_classification(self, msg):
        """Callback function triggered by self.sub_distress_classification:
                - stops duckiebot, saves distress type in self.autobot_info and
                  triggers rescue operation

            Args:
            - msg (:obj:`AutobotInfoMsg`): received message

            Parameters: None

            Returns: None
        """
        distress_type_num = int(msg.data)
        if distress_type_num > 0:
            # NOTE: this should always be the case
            # since rescue_center_node only publishes, if rescue_class > 0

            # Added for AMoD class demo. Stop a little longer for LEDs to change
            if (rospy.get_param("~demo_mode")):
                rate = rospy.Rate(5)  # 5Hz
                counter = 0
                delay_time = 5 * 3  # 3 seconds
                while counter <= delay_time:
                    self.stopDuckiebot()
                    counter += 1
                    rate.sleep()

            self.stopDuckiebot()
            print("[{}] distressed at: pos = {}, phi = {}".format(
                self.veh_id, self.autobot_info.current_pos,
                self.autobot_info.current_heading))
            self.activated = True
            self.autobot_info.rescue_class = Distress(distress_type_num)
            print("[{}] Distressed class: {}".format(
                self.veh_id, self.autobot_info.rescue_class))

    def readyForLF(self,
                   recalculateDesired=True,
                   tol_angle=TOL_ANGLE_IN_DEG,
                   tol_pos=TOL_POSITION_IN_M):
        """Checks, if duckiebot is ready for lane following (after rescue operation)

            Args: None

            Parameters:
            - recalculateDesired (Boolean):
                if True, function will recalculate desired position/heading and check tolerance based on those
                if False, function will use last desired position(heading)
            - tol_angle (float): allowed angle tolerance (+/-)
            - tol_pos (float): allowed position tolerance (+/-)

            Returns:
            - (Boolean): True, iff ready for lane following
        """
        # for debugging: /rescue/rescue_agents/rescue_agent_[self.vehID]/everythingOK
        debug_param = rospy.get_param('~everythingOK')
        if debug_param:
            return True

        if self.autobot_info.rescue_class == Distress.STUCK_IN_INTERSECTION:
            if self.tileType == '4way' or self.tileType == '3way':
                return False

        # check,if duckiebot is back in lane
        if recalculateDesired:
            desired_pos = self.map.pos_to_ideal_position(
                self.autobot_info.current_pos)
            desired_heading = self.map.pos_to_ideal_heading(desired_pos)
        else:
            desired_pos = self.desired_pos
            desired_heading = self.desired_heading
        delta_phi = abs(self.autobot_info.current_heading - desired_heading)
        delta_phi = min(delta_phi, 360 - delta_phi)
        delta_d = math.sqrt(
            (desired_pos[0] - self.autobot_info.current_pos[0])**2 +
            (desired_pos[1] - self.autobot_info.current_pos[1])**2)
        print("delta_phi: {}, delta_d: {}".format(delta_phi, delta_d))
        if delta_d < tol_pos and delta_phi < tol_angle:
            return True

        # if duckiebot is not ready for LF
        return False

    def stopDuckiebot(self):
        """Stops duckiebot

            Args: None

            Parameters: None

            Returns: None
        """
        car_cmd = Twist2DStamped()
        car_cmd.v = 0
        car_cmd.omega = 0
        self.current_car_cmd = car_cmd
        self.pub_car_cmd.publish(self.current_car_cmd)

    def goBackToLF(self):
        """Duckiebot goes back to lane following

            Args: None
        
            Parameters: None

            Returns: None
        """
        self.activated = False
        self.autobot_info.rescue_class = Distress.NORMAL_OPERATION
        msg = BoolStamped()
        msg.data = True
        self.pub_rescueDone.publish(msg)
        self.controller_counter = 0
        rospy.set_param('/rescue/rescue_center/trigger_rescue', False)

    def run(self):
        """Main loop of ROS node

            Args: None

            Parameters: None

            Returns: None
        """
        # publish rate
        rate = rospy.Rate(4)  # 10Hz

        # --- MAIN LOOP FOR RESCUE CONTROL -- #
        while not rospy.is_shutdown():
            if self.activated:
                # in rescue operation

                # monitoring is deactivated
                if self.autobot_info.classificationActivated == False:
                    self.activated = False
                    self.stopDuckiebot()
                    print(
                        "[{}] Monitoring deactivated in rescue: stop duckiebot"
                    )
                    msg = BoolStamped()
                    msg.data = True
                    self.pub_rescueStopped.publish(msg)
                    self.controller_counter = 0
                    continue

                # --- GET CURRENT POSITION ON MAP ---#
                # self.updatePositionAndHeading()
                self.tileType = self.map.pos_to_semantic(
                    self.autobot_info.current_pos)
                print("[{}] Current: pos = {}, phi = {}".format(
                    self.veh_id, self.autobot_info.current_pos,
                    self.autobot_info.current_heading))

                # --- CALCULATE DESIRED POSITION ---#
                desired_pos = self.map.pos_to_ideal_position(
                    self.autobot_info.current_pos,
                    heading=self.autobot_info.current_heading)
                # check, if at bad position
                if desired_pos == float('inf') or desired_pos == None:
                    # inf: out of map, None: out of map and no good entry point found
                    # No good rescue point found: e.g. on asphalt next to curves
                    print(
                        '[{}] no good point found or out of map! Going backwards now...'
                        .format(self.veh_id))
                    self.current_car_cmd.v = -self.v_ref
                    self.current_car_cmd.omega = 0
                    self.pub_car_cmd.publish(self.current_car_cmd)
                    sleep(T_EXECUTION)
                    # stop car
                    self.stopDuckiebot()
                    sleep(T_STOP)
                    continue

                # Check, if duckiebot drove into intersection: 4-way or 3-way:
                if self.tileType == '4way' or self.tileType == '3way':
                    if self.autobot_info.rescue_class != Distress.STUCK_IN_INTERSECTION:
                        print(
                            "[{}] drove into Intersection, check, if ready for LF"
                            .format(self.veh_id))
                        self.stopDuckiebot()
                        if self.desired_pos:
                            if self.readyForLF(recalculateDesired=False):
                                print("[{}] Ready for LF".format(self.veh_id))
                                self.goBackToLF()
                                # TODO: implement else case: go forward
                            else:
                                self.autobot_info.rescue_class = Distress.STUCK_IN_INTERSECTION
                            continue

                # --- CALCULATE DESIRED HEADING--- #
                self.desired_pos = desired_pos  # need this, because I want to use the old desired_pos in case of an intersection
                self.desired_heading = self.map.pos_to_ideal_heading(
                    self.desired_pos)
                print("[{}] Desired: pos = {}, phi = {}".format(
                    self.veh_id, self.desired_pos, self.desired_heading))

                # --- EXECUTE CONTROL COMMANDS---#
                if self.controller_counter < NUMBER_RESCUE_CMDS:
                    # Calculate controller output
                    v_out, omega_out = self.controller.getControlOutput(
                        self.autobot_info.current_pos,
                        self.autobot_info.current_heading,
                        self.desired_pos,
                        self.desired_heading,
                        v_ref=self.v_ref,
                        dt_last=T_EXECUTION)
                    print("[{}]: v = {}, omega = {}".format(
                        self.veh_id, v_out, omega_out))
                    # Send cmd to duckiebot
                    self.current_car_cmd.v = v_out
                    self.current_car_cmd.omega = omega_out
                    self.pub_car_cmd.publish(self.current_car_cmd)
                    sleep(T_EXECUTION)
                    # stop car
                    self.stopDuckiebot()
                    sleep(T_STOP)
                    self.controller_counter += 1
                    print("Finished {} control action".format(
                        self.controller_counter + 1))
                # --- CHECK IF RESCUE IS FINISHED --- #
                else:
                    print(
                        "[{}] Finished rescue attempt. Check, if ok...".format(
                            self.veh_id))
                    if self.readyForLF(recalculateDesired=False):
                        print("[{}] Ready for LF".format(self.veh_id))
                        self.goBackToLF()
                    else:
                        self.controller_counter = 0
                        print(
                            "[{}] Duckiebot still not ready for LF. New rescue attempt."
                            .format(self.veh_id))
            rate.sleep()