def generate_full_path(self, steps):
"""
Creates a list of nodes associated with each time step
:return:
"""
path = [] # place holder, value will be removed
current_step = self.path_log[-1].get_time()
if not self.waypoints:
# No waypoints provided, robot will just sit still
pos = self.state.get_position()
i = self.get_information_available(pos)
self.waypoints.append(pos)
path.append(Node.init_without_cost(pos, i, current_step))
elif self.state.get_position() != self.waypoints[0]:
# The first state is not the first waypoint, add it in
self.waypoints.insert(0, self.state.get_position())
elif len(self.waypoints) == 1:
pos = self.waypoints[0]
i = self.get_information_available(pos)
path.append(Node.init_without_cost(pos, i, current_step))
for point in range(0, len(self.waypoints) - 1):
temp_path = self.generate_straight_line_path(point, current_step)
temp_path.reverse()
temp_path.pop()
temp_path.extend(path)
path = temp_path
current_step = path[0].get_time()
# temp_path.reverse()
# self.path_log.append(temp_path.pop())
if steps and len(path) < steps:
add_n = steps - len(path)
temp_path = self.generate_stationary_path(path[0], add_n)
temp_path.extend(path)
path = temp_path
self.path = path
def generate_stationary_path(self, node, steps):
k = node.get_time()
pos = node.get_position()
temp_path = []
for _ in range(0, steps):
k += 1
i = self.get_information_available(pos)
temp_path.append(Node.init_without_cost(pos, i, k))
return temp_path
def start(self, workspace):
"""
Set initial conditions for the robot in a specified workspace
:param workspace: Workspace object that describes the environment the agent is working in
:return:
"""
self.set_workspace(workspace)
self.initialize_pdf()
I_0 = self.get_information_available(self.state)
start_node = Node.init_without_cost(self.state.get_position(), I_0, 0)
self.path_log.append(start_node)
self.state_log.append(self.state)
def start(self, workspace):
"""
Set initial conditions for the robot in a specified workspace
:param workspace: Workspace object that describes the environment the agent is working in
:return:
"""
self.set_workspace(workspace)
# self.set_map()
self.set_initial_pdf()
self.update_pdf()
current_step = self.workspace.get_time_step()
i = self.get_information_available(self.state)
start_node = Node.init_without_cost(self.state.get_position(), i, current_step)
self.path_log.append(start_node)
def generate_straight_line_path(self, w_0, k=0):
"""
Could use any type of path planner here. This is just a straight line style implementation
Assume waypoint 1 is the starting location
:return:
"""
start = self.waypoints[w_0]
goal = self.waypoints[w_0 + 1]
# --- Straight line assumption ---
i = self.get_information_available(start)
path = [Node.init_without_cost(start, i, k)]
x = start[0]
y = start[1]
goal_found = False
# North
if y < goal[1]:
while not goal_found:
y += self.cfg["step_size"]
k += 1
i = self.get_information_available((x, y))
path.append(Node.init_without_cost((x, y), i, k))
if (x, y) == goal:
goal_found = True
# East
elif x < goal[0]:
while not goal_found:
x += self.cfg["step_size"]
k += 1
i = self.get_information_available((x, y))
path.append(Node.init_without_cost((x, y), i, k))
if (x, y) == goal:
goal_found = True
# South
elif y > goal[1]:
while not goal_found:
y -= self.cfg["step_size"]
k += 1
i = self.get_information_available((x, y))
path.append(Node.init_without_cost((x, y), i, k))
if (x, y) == goal:
goal_found = True
# West
elif x > goal[0]:
while not goal_found:
x -= self.cfg["step_size"]
k += 1
i = self.get_information_available((x, y))
path.append(Node.init_without_cost((x, y), i, k))
if (x, y) == goal:
goal_found = True
# Error
else:
print("ERROR: Something's wrong with the start or goal position for {}".format(self.name))
return path