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 __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 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
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
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
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
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
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()
