BasePathPlanner

src.python_motion_planning.path_planner.base_path_planner.BasePathPlanner

Bases: ABC

Class for building path planner.

Parameters:

Name	Type	Description	Default
map_	BaseMap	The map which the planner is based on.	required
start	tuple	The start point of the planner in the map coordinate system.	required
goal	tuple	The goal point of the planner in the map coordinate system.	required

Source code in src\python_motion_planning\path_planner\base_path_planner.py

Python

class BasePathPlanner(ABC):
    """
    Class for building path planner.

    Args:
        map_: The map which the planner is based on.
        start: The start point of the planner in the map coordinate system.
        goal: The goal point of the planner in the map coordinate system.
    """
    def __init__(self, map_: BaseMap, start: tuple, goal: tuple) -> None:
        super().__init__()
        self.map_ = map_
        self.map_.update_esdf()
        self.start = start
        self.goal = goal
        self.failed_info = [], {"success": False, "start": None, "goal": None, "length": 0, "cost": 0, "expand": {}}

    def __str__(self) -> str:
        return "Base Path Planner"

    @property
    def dim(self) -> int:
        """
        Get the dimension of the map.

        Returns:
            dim (int): The dimension of the map.
        """
        return self.map_.dim

    @property
    def bounds(self) -> Iterable:
        """
        Get the bounds of the map.

        Returns:
            bounds (Iterable): The bounds of the map.
        """
        return self.map_.bounds

    @abstractmethod
    def plan(self) -> Union[List[Tuple[float, ...]], Dict[str, Any]]:
        """
        Interface for planning.

        Returns:
            path: A list containing the path waypoints
            path_info: A dictionary containing the path information (success, length, cost, expand)
        """
        return self.failed_info

    def get_cost(self, p1: tuple, p2: tuple) -> float:
        """
        Get the cost between two points. (default: distance defined in the map)

        Args:
            p1: Start point.
            p2: Goal point.

        Returns:
            cost: Cost between two points.
        """
        return self.map_.get_distance(p1, p2)

    def get_heuristic(self, point: tuple) -> float:
        """
        Get the heuristic value of the point. (default: cost between current point and goal point)

        Args:
            point: Point.


        Returns:
            heuristic: Heuristic value of the point.
        """
        return self.get_cost(point, self.goal)


    def extract_path(self, closed_list: dict, start: tuple = None, goal: tuple = None) -> Tuple[List[Tuple[float, ...]], float, float]:
        """
        Extract the path based on the CLOSED list.

        Args:
            closed_list: CLOSED list
            start: Start point. (default: self.start)
            goal: Goal point. (default: self.goal)

        Returns:
            path: A list containing the path waypoints
            length: Length of the path
            cost: Cost of the path
        """
        length = 0
        cost = 0

        if start is None:
            start = self.start
        if goal is None:
            goal = self.goal

        node = closed_list.get(goal)
        path = [node.current]

        while node.current != start:
            node_parent = closed_list.get(node.parent)
            length += self.map_.get_distance(node.current, node_parent.current)
            cost += self.get_cost(node.current, node_parent.current)
            node = node_parent
            path.append(node.current)
        path = path[::-1]   # make the order: start -> goal

        return path, length, cost

bounds: Iterable property

Get the bounds of the map.

Returns:

Name	Type	Description
bounds	Iterable	The bounds of the map.

dim: int property

Get the dimension of the map.

Returns:

Name	Type	Description
dim	int	The dimension of the map.

extract_path(closed_list, start=None, goal=None)

Extract the path based on the CLOSED list.

Parameters:

Name	Type	Description	Default
closed_list	dict	CLOSED list	required
start	tuple	Start point. (default: self.start)	None
goal	tuple	Goal point. (default: self.goal)	None

Returns:

Name	Type	Description
path	List[Tuple[float, ...]]	A list containing the path waypoints
length	float	Length of the path
cost	float	Cost of the path

Source code in src\python_motion_planning\path_planner\base_path_planner.py

Python

def extract_path(self, closed_list: dict, start: tuple = None, goal: tuple = None) -> Tuple[List[Tuple[float, ...]], float, float]:
    """
    Extract the path based on the CLOSED list.

    Args:
        closed_list: CLOSED list
        start: Start point. (default: self.start)
        goal: Goal point. (default: self.goal)

    Returns:
        path: A list containing the path waypoints
        length: Length of the path
        cost: Cost of the path
    """
    length = 0
    cost = 0

    if start is None:
        start = self.start
    if goal is None:
        goal = self.goal

    node = closed_list.get(goal)
    path = [node.current]

    while node.current != start:
        node_parent = closed_list.get(node.parent)
        length += self.map_.get_distance(node.current, node_parent.current)
        cost += self.get_cost(node.current, node_parent.current)
        node = node_parent
        path.append(node.current)
    path = path[::-1]   # make the order: start -> goal

    return path, length, cost

get_cost(p1, p2)

Get the cost between two points. (default: distance defined in the map)

Parameters:

Name	Type	Description	Default
p1	tuple	Start point.	required
p2	tuple	Goal point.	required

Returns:

Name	Type	Description
cost	float	Cost between two points.

Source code in src\python_motion_planning\path_planner\base_path_planner.py

Python

def get_cost(self, p1: tuple, p2: tuple) -> float:
    """
    Get the cost between two points. (default: distance defined in the map)

    Args:
        p1: Start point.
        p2: Goal point.

    Returns:
        cost: Cost between two points.
    """
    return self.map_.get_distance(p1, p2)

get_heuristic(point)

Get the heuristic value of the point. (default: cost between current point and goal point)

Parameters:

Name	Type	Description	Default
point	tuple	Point.	required

Returns:

Name	Type	Description
heuristic	float	Heuristic value of the point.

Source code in src\python_motion_planning\path_planner\base_path_planner.py

Python

def get_heuristic(self, point: tuple) -> float:
    """
    Get the heuristic value of the point. (default: cost between current point and goal point)

    Args:
        point: Point.


    Returns:
        heuristic: Heuristic value of the point.
    """
    return self.get_cost(point, self.goal)

plan() abstractmethod

Interface for planning.

Returns:

Name	Type	Description
path	Union[List[Tuple[float, ...]], Dict[str, Any]]	A list containing the path waypoints
path_info	Union[List[Tuple[float, ...]], Dict[str, Any]]	A dictionary containing the path information (success, length, cost, expand)

Source code in src\python_motion_planning\path_planner\base_path_planner.py

Python

@abstractmethod
def plan(self) -> Union[List[Tuple[float, ...]], Dict[str, Any]]:
    """
    Interface for planning.

    Returns:
        path: A list containing the path waypoints
        path_info: A dictionary containing the path information (success, length, cost, expand)
    """
    return self.failed_info