Navigation

class Motion_Planning.Navigation.AStarPlanner(robot_size, obstacles_bounds, resolution, x_max=10, y_max=10, enable_plot=False)[source]

AStar Navigation planner

__init__(robot_size, obstacles_bounds, resolution, x_max=10, y_max=10, enable_plot=False)[source]

plan(start_pose, goal_pose)[source]

Find a path from a specified initial position to a goal position in a 2D grid representation

Parameters:

init_base_position (Pose) – The initial position of the robot.
goal_base_pose (Pose) – The goal pose of the robot.

Returns:

The function returns a list of waypoints that form a path from the initial position to the goal position. Each waypoint is a list [x, y] representing a position in the world coordinates.

Note

A* for path searching in map: 10 meter * 10 meter resolution is 0.1 meter a 2D grid: 1 for obstacles

visual()[source]: Visualization of routes generated by A_star navigation algorithm

class Motion_Planning.Navigation.RRTPlanner(robot_size, obstacles_bounds, expand_dis=0.2, path_resolution=0.05, goal_sample_rate=5, max_iter=500, enable_plot=True)[source]

Class for RRT planning

class Node(x, y)[source]

RRT Node

__init__(x, y)[source]

Initializes the RRT node.

Parameters:

x (float) – X-coordinate of the node.
y (float) – Y-coordinate of the node.

__init__(robot_size, obstacles_bounds, expand_dis=0.2, path_resolution=0.05, goal_sample_rate=5, max_iter=500, enable_plot=True)[source]

Initializes the RRT planner.

Parameters:

robot_size (float) – The size of the robot.
obstacles_bounds (list) – List of obstacle boundaries.
expand_dis (float, optional) – The distance to expand the tree. Defaults to 0.2.
path_resolution (float, optional) – The resolution of the path. Defaults to 0.05.
goal_sample_rate (int, optional) – The goal sampling rate. Defaults to 5.
max_iter (int, optional) – The maximum number of iterations. Defaults to 500.
enable_plot (bool, optional) – Flag to enable or disable plotting. Defaults to True.

plan(start_pose, goal_pose)[source]

RRT path planning.

Parameters:

start_pose (Pose) – The starting pose of the robot.
goal_pose (Pose) – The goal pose of the robot.

Returns:

The planned path as a list of points.

Return type:

list

visual()[source]: Visualization of routes generated by RTT navigation algorithm.

steer(from_node, to_node, extend_length=inf)[source]

Steers from one node towards another node.

Parameters:

from_node (Node) – The starting node.
to_node (Node) – The target node.
extend_length (float, optional) – The distance to extend towards the target node. Defaults to infinity.

Returns:

The new node after steering.

Return type:

Node

generate_final_course(goal_ind)[source]

Generates the final course from start to goal.

Parameters:: goal_ind (int) – The index of the goal node.
Returns:: The final path as a list of points.
Return type:: list

calc_dist_to_goal(x, y)[source]

Calculates the distance to the goal.

Parameters:

x (float) – The x-coordinate.
y (float) – The y-coordinate.

Returns:

The distance to the goal.

Return type:

float

get_random_node()[source]

Gets a random node.

Returns:: The random node.
Return type:: Node

static get_nearest_node_index(node_list, rnd_node)[source]

Gets the index of the nearest node.

Parameters:

node_list (list) – The list of nodes.
rnd_node (Node) – The random node.

Returns:

The index of the nearest node.

Return type:

int

static check_if_outside_play_area(node, play_area)[source]

Checks if a node is outside the play area.

Parameters:

node (Node) – The node to check.
play_area (AreaBounds) – The play area.

Returns:

True if the node is inside the play area, False otherwise.

Return type:

bool

check_collision(node)[source]

Checks if a node is in collision.

Parameters:: node (Node) – The node to check.
Returns:: True if the node is not in collision, False otherwise.
Return type:: bool

static calc_distance_and_angle(from_node, to_node)[source]

Calculates the distance and angle between two nodes.

Parameters:

from_node (Node) – The starting node.
to_node (Node) – The target node.

Returns:

The distance and angle between the nodes.

Return type:

tuple

class Motion_Planning.Navigation.PRMPlanner(robot_size, obstacles_bounds, enable_plot=True)[source]

Class for PRM planning

__init__(robot_size, obstacles_bounds, enable_plot=True)[source]

Initializes the PRM planner.

Parameters:

robot_size (float) – The size of the robot.
obstacles_bounds (list) – List of obstacle boundaries.
enable_plot (bool, optional) – Flag to enable or disable plotting. Defaults to True.

class Node(x, y, cost, parent_index)[source]

Node class for Dijkstra search

__init__(x, y, cost, parent_index)[source]

Initializes the Node class.

Parameters:

x (float) – X-coordinate of the node.
y (float) – Y-coordinate of the node.
cost (float) – Cost to reach this node.
parent_index (int) – Index of the parent node.

plan(start_pose, goal_pose)[source]

Finds a path from a specified initial position to a goal position.

Parameters:

start_pose (Pose) – The starting pose of the robot.
goal_pose (Pose) – The goal pose of the robot.

Returns:

The planned path as a list of points.

Return type:

list

visual()[source]: Visualization of routes generated by PRM navigation algorithm.

is_collision(sx, sy, gx, gy)[source]

Checks if there is a collision between two points.

Parameters:

sx (float) – Start x-coordinate.
sy (float) – Start y-coordinate.
gx (float) – Goal x-coordinate.
gy (float) – Goal y-coordinate.

Returns:

True if there is a collision, False otherwise.

Return type:

bool

generate_road_map(sample_x, sample_y)[source]

Generates the road map.

Parameters:

sample_x (list) – X positions of sampled points.
sample_y (list) – Y positions of sampled points.

Returns:

The road map as a list of edges.

Return type:

list

dijkstra_planning(sx, sy, gx, gy, road_map, sample_x, sample_y)[source]

Performs Dijkstra’s algorithm to find the shortest path.

Parameters:

sx (float) – Start x-coordinate.
sy (float) – Start y-coordinate.
gx (float) – Goal x-coordinate.
gy (float) – Goal y-coordinate.
road_map (list) – The road map.
sample_x (list) – X positions of sampled points.
sample_y (list) – Y positions of sampled points.

Returns:

Two lists of path coordinates ([x1, x2, …], [y1, y2, …]).

Return type:

tuple

sample_points(sx, sy, gx, gy)[source]

Samples random points in the environment.

Parameters:

sx (float) – Start x-coordinate.
sy (float) – Start y-coordinate.
gx (float) – Goal x-coordinate.
gy (float) – Goal y-coordinate.

Returns:

Two lists of sampled x and y coordinates.

Return type:

tuple