def fillOccupancy(self):
"""Function that fill the occupnacy grid on every update
Assumptions:
1. RobotPose is considered (0, 0, 0) to accomodate the laser scan, which produces ranges wrt to the bot
2. The RobotPose in the occupancy grid is (X * scale_factor/2, Y * scale_factor /2, 0)
3. The attribute robotPose is the real pose of the robot wrt to the world Frame,
thus it helps us to calculate the transform for trajectory and pose validity queries
"""
self.grid[:] = 0
ang_min, ang_max, ranges = self.scan
angle_step = (ang_max - ang_min) / len(ranges)
for i, rng in enumerate(ranges):
# Check for obstacles
if np.abs(rng) is not np.inf:
x, y = (
rng * np.cos(ang_min + i * angle_step),
rng * np.sin(ang_max + i * angle_step),
)
newState = self.transform("bot", "map",
RobotState(Point(x, y, 0)))
x_, y_ = newState.position.x, newState.position.y
# Checking if the range is within the grid, to mark them as occupied
if 0 <= x_ < self.grid.shape[0] and 0 <= y_ < self.grid.shape[
1]:
if self.grid[int(x_)][int(-y_ - 1)] != 1:
self.grid[int(x_)][int(-y_ - 1)] = 1
def transform(self, frame1, frame2, rsf1):
"""Transform robotPose from one pose to the other
Args:
frame1 (string) : from the frame (world, bot, map)
frame2 (string) : to the frame (world, bot, map)
rsf1 (gennav.utils.common.RobotState) : RobotState in frame1
Returns:
rsf2 (gennav.utils.common.RobotState) : RobotState in frame2
"""
# TODO: Make it more robust in terms of checking frames
# Check if the required trnasform or the inverse of the transform exists
frame = frame2 + "T" + frame1
frame_inv = frame1 + "T" + frame2
if frame in self.transforms.keys():
t_matrix = self.transforms[frame]["transform"]
elif frame_inv in self.transforms.keys():
t_matrix = np.linalg.inv(self.transforms[frame_inv]["transform"])
else:
raise Exception("Transform for the frames not found")
# Transform using matrix multiplication
pf2 = np.dot(
t_matrix,
np.array([rsf1.position.x, rsf1.position.y, 1]).reshape(3, 1))
rsf2 = RobotState(position=Point(pf2[0].item(), pf2[1].item()))
# Return RobotState
return rsf2
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 __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 astar(graph, start, end, heuristic={}):
"""
Performs A-star search to find the shortest path from start to end
Args:
graph (dict): Dictionary representing the graph where keys are the nodes
and the value is a list of all neighbouring nodes
start (gennav.utils.geometry.Point): Point representing key corresponding
to the start point
end (gennav.utils.geometry.Point): Point representing key corresponding
to the end point
heuristic (dict): Dictionary containing the heuristic values for all the nodes,
if not specified the default heuristic is euclidean distance
Returns:
gennav.utils.Trajectory: The planned path as trajectory
"""
if not (start in graph and end in graph):
path = [RobotState(position=start)]
traj = Trajectory(path)
return traj
open_ = []
closed = []
# calcula]tes heuristic for start if not provided by the user
# pushes the start point in the open_ Priority Queue
start_node = NodeAstar(RobotState(position=start), None)
if len(heuristic) == 0:
start_node.h = math.sqrt((start.x - end.x)**2 + (start.y - end.y)**2)
else:
start_node.h = heuristic[start]
start_node.g = 0
start_node.f = start_node.g + start_node.h
open_.append(start_node)
# performs astar search to find the shortest path
while len(open_) > 0:
open_.sort()
current_node = open_.pop(0)
closed.append(current_node)
# checks if the goal has been reached
if current_node.state.position == end:
path = []
# forms path from closed list
while current_node.parent is not None:
path.append(current_node.state)
current_node = current_node.parent
path.append(start_node.state)
# returns reversed path
path = path[::-1]
traj = Trajectory(path)
return traj
# continues to search for the goal
# makes a list of all neighbours of the current_node
neighbours = graph[current_node.state.position]
# adds them to open_ if they are already present in open_
# checks and updates the total cost for all the neighbours
for node in neighbours:
# creates neighbour which can be pushed to open_ if required
neighbour = NodeAstar(RobotState(position=node), current_node)
# checks if neighbour is in closed
if neighbour in closed:
continue
# calculates weight cost
neighbour.g = (
math.sqrt((node.x - current_node.state.position.x)**2 +
(node.y - current_node.state.position.y)**2) +
current_node.g)
# calculates heuristic for the node if not provided by the user
if len(heuristic) == 0:
neighbour.h = math.sqrt((node.x - end.x)**2 +
(node.y - end.y)**2)
else:
neighbour.h = heuristic[node]
# calculates total cost
neighbour.f = neighbour.g + neighbour.h
# checks if the total cost of neighbour needs to be updated
# if it is presnt in open_ else adds it to open_
flag = 1
for new_node in open_:
if neighbour == new_node and neighbour.f < new_node.f:
new_node = neighbour # lgtm [py/multiple-definition]
flag = 0
break
elif neighbour == new_node and neighbour.f > new_node.f:
flag = 0
break
if flag == 1:
open_.append(neighbour)
# if path doesn't exsist it returns just the start point as the path
path = [RobotState(position=start)]
traj = Trajectory(path)
return traj
def __init__(self, state=RobotState(), parent=None):
Node.__init__(self, state, parent)
self.g = 0 # Distance to start node
self.h = 0 # Distance to goal node
self.f = 0 # Total cost