def __call__(self):
"""
Randomly sample point in area while sampling goal point at a specified rate.
Return:
gennav.utils.RobotState: The sampled robot state.
"""
if random.random() > self.goal_sample_p:
new_point = Point()
new_point.x = random.uniform(self.min_x, self.max_x)
new_point.y = random.uniform(self.min_y, self.max_y)
return RobotState(position=new_point)
else:
return self.goal
def uniform_random_sampler(sample_area):
"""
Randomly sample point in sample area
Args:
sample_area(tuple): area to sample point in (min and max)
Return:
Randomly selected point as a Point(gennav.utils.geometry.Point).
"""
new_point = Point()
new_point.x = random.uniform(sample_area[0], sample_area[1])
new_point.y = random.uniform(sample_area[0], sample_area[1])
return new_point
def __call__(self):
"""Randomly sample point in area while sampling goal point at a specified rate.
Return:
gennav.utils.RobotState: The sampled robot state.
"""
if random.random() > self.goal_sample_p:
theta = 2 * pi * random.random()
u = random.random() * self.r
new_point = Point()
new_point.x = self.centre.x + u * cos(theta)
new_point.y = self.centre.y + u * sin(theta)
return RobotState(position=new_point)
else:
return self.goal
def uniform_random_circular_sampler(r):
"""Randomly samples point in a circular region of radius r around the origin.
Args:
r: radius of the circle.
Return:
Randomly selected point as Point(gennav.utils.geometry.Point).
"""
# Generate a random angle theta.
theta = 2 * pi * random.random()
u = random.random() + random.random()
if u > 1:
a = 2 - u
else:
a = u
new_point = Point()
new_point.x = r * a * cos(theta)
new_point.y = r * a * sin(theta)
return new_point
def uniform_adjustable_random_sampler(sample_area, goal, goal_sample_rate):
"""
Randomly sample point in area while sampling goal point at a specified rate.
Args:
sample_area(tuple): area to sample point in (min and max)
goal(gennav.utils.geometry.Point):Point representing goal point.
goal_sample_rate: number between 0 and 1 specifying how often
to sample the goal point.
Return:
Randomly selected point as a Point(gennav.utils.geometry.Point).
"""
if random.random() > goal_sample_rate:
new_point = Point()
new_point.x = random.uniform(sample_area[0], sample_area[1])
new_point.y = random.uniform(sample_area[0], sample_area[1])
return new_point
else:
return goal
def plan(self, start, goal, env):
"""Plans path from start to goal avoiding obstacles.
Args:
start_point(gennav.utils.RobotState): with start point coordinates.
end_point (gennav.utils.RobotState): with end point coordinates.
env: (gennav.envs.Environment) Base class for an envrionment.
Returns:
gennav.utils.Trajectory: The planned path as trajectory
dict: Dictionary containing additional information like the node_list
"""
# Check if start and goal states are obstacle free
if not env.get_status(start):
raise InvalidStartState(start,
message="Start state is in obstacle.")
if not env.get_status(goal):
raise InvalidGoalState(goal, message="Goal state is in obstacle.")
# Initialize start and goal nodes
start_node = Node(state=start)
goal_node = Node(state=goal)
# Initialize node_list with start
node_list = [start_node]
# Loop to keep expanding the tree towards goal if there is no direct connection
traj = Trajectory(path=[start, goal])
if not env.get_traj_status(traj):
while True:
# Sample random state in specified area
rnd_state = self.sampler()
# Find nearest node to the sampled point
distance_list = [
compute_distance(node.state.position, rnd_state.position)
for node in node_list
]
try:
# for python2
nearest_node_index = min(xrange(len(distance_list)),
key=distance_list.__getitem__)
except NameError:
# for python 3
nearest_node_index = distance_list.index(
min(distance_list))
nearest_node = node_list[nearest_node_index]
# Create new point in the direction of sampled point
theta = math.atan2(
rnd_state.position.y - nearest_node.state.position.y,
rnd_state.position.x - nearest_node.state.position.x,
)
new_point = Point()
new_point.x = (nearest_node.state.position.x +
self.expand_dis * math.cos(theta))
new_point.y = (nearest_node.state.position.y +
self.expand_dis * math.sin(theta))
# Check whether new point is inside an obstacles
if not env.get_status(RobotState(position=new_point)):
continue
else:
new_node = Node.from_coordinates(new_point)
new_node.parent = nearest_node
node_list.append(new_node)
# Check if goal has been reached or if there is direct connection to goal
distance_to_goal = compute_distance(goal_node.state.position,
new_node.state.position)
traj = Trajectory(path=[RobotState(position=new_point), goal])
if distance_to_goal < self.expand_dis or env.get_traj_status(
traj):
goal_node.parent = node_list[-1]
node_list.append(goal_node)
print("Goal reached!")
break
else:
goal_node.parent = start_node
node_list = [start_node, goal_node]
# Construct path by traversing backwards through the tree
path = []
last_node = node_list[-1]
while last_node.parent is not None:
path.append(last_node.state)
last_node = last_node.parent
path.append(start)
info_dict = {}
info_dict["node_list"] = node_list
path = Trajectory(path[::-1])
if len(path.path) == 1:
raise PathNotFound(path, message="Path contains only one state")
return (path, info_dict)
def plan(self, start, goal, env):
"""Plans path from start to goal avoiding obstacles.
Args:
start_point(gennav.utils.RobotState): with start point coordinates.
end_point (gennav.utils.RobotState): with end point coordinates.
env: (gennav.envs.Environment) Base class for an envrionment.
Returns:
gennav.utils.Trajectory: The planned path as trajectory
"""
# Initialize start and goal nodes
start_node = Node(state=start)
goal_node = Node(state=goal)
# Initialize node_list with start
node_list = [start_node]
# Loop to keep expanding the tree towards goal if there is no direct connection
traj = Trajectory(path=[start, goal])
if not env.get_traj_status(traj):
while True:
# Sample random point in specified area
rnd_point = self.sampler(self.sample_area, goal.position,
self.goal_sample_rate)
# Find nearest node to the sampled point
distance_list = [(node.state.position.x - rnd_point.x)**2 +
(node.state.position.y - rnd_point.y)**2 +
(node.state.position.z - rnd_point.z)**2
for node in node_list]
try:
# for python2
nearest_node_index = min(xrange(len(distance_list)),
key=distance_list.__getitem__)
except NameError:
# for python 3
nearest_node_index = distance_list.index(
min(distance_list))
nearest_node = node_list[nearest_node_index]
# Create new point in the direction of sampled point
theta = math.atan2(
rnd_point.y - nearest_node.state.position.y,
rnd_point.x - nearest_node.state.position.x,
)
new_point = Point()
new_point.x = (nearest_node.state.position.x +
self.expand_dis * math.cos(theta))
new_point.y = (nearest_node.state.position.y +
self.expand_dis * math.sin(theta))
# Check whether new point is inside an obstacles
if not env.get_status(RobotState(position=new_point)):
continue
else:
new_node = Node.from_coordinates(new_point)
new_node.parent = nearest_node
node_list.append(new_node)
# Check if goal has been reached or if there is direct connection to goal
del_x, del_y, del_z = (
new_node.state.position.x - goal_node.state.position.x,
new_node.state.position.y - goal_node.state.position.y,
new_node.state.position.z - goal_node.state.position.z,
)
distance_to_goal = math.sqrt(del_x**2 + del_y**2 + del_z**2)
traj = Trajectory(path=[RobotState(position=new_point), goal])
if distance_to_goal < self.expand_dis or env.get_traj_status(
traj):
goal_node.parent = node_list[-1]
node_list.append(goal_node)
print("Goal reached!")
break
else:
goal_node.parent = start_node
node_list = [start_node, goal_node]
# Construct path by traversing backwards through the tree
path = []
last_node = node_list[-1]
while node_list[node_list.index(last_node)].parent is not None:
node = node_list[node_list.index(last_node)]
path.append(RobotState(position=node.state.position))
last_node = node.parent
path.append(start)
path = Trajectory(list(reversed(path)))
return path