Public Member Functions
None	__init__ (self)

float	footprintCost (self, Polygon footprint)

float	lineCost (self, float x0, float x1, float y0, float y1, float step_size=0.5)

tuple[int, int]	worldToMapValidated (self, float wx, float wy)

np.uint8	pointCost (self, int x, int y)

None	setCostmap (self, PyCostmap2D costmap)

float	footprintCostAtPose (self, float x, float y, float theta, Polygon footprint)

Public Attributes
	costmap_

Detailed Description

FootprintCollisionChecker.

FootprintCollisionChecker Class for getting the cost
and checking the collisions of a Footprint

Definition at line 38 of file footprint_collision_checker.py.

Constructor & Destructor Documentation

◆ init()

None nav2_simple_commander.footprint_collision_checker.FootprintCollisionChecker.__init__ ( self )

Initialize the FootprintCollisionChecker Object.

Definition at line 46 of file footprint_collision_checker.py.

References nav2_costmap_2d::ClearCostmapService.costmap_, nav2_costmap_2d::Costmap2D.costmap_, nav2_costmap_2d::Costmap2D::MarkCell.costmap_, nav2_costmap_2d::Costmap2D::PolygonOutlineCells.costmap_, nav2_costmap_2d::Costmap2DPublisher.costmap_, nav2_costmap_2d::CostmapSubscriber.costmap_, nav2_costmap_2d::FootprintCollisionChecker< std::shared_ptr< nav2_costmap_2d::Costmap2D > >.costmap_, nav2_costmap_2d::FootprintCollisionChecker< CostmapT >.costmap_, nav2_costmap_2d::FootprintCollisionChecker< nav2_costmap_2d::Costmap2D * >.costmap_, costmap_queue::CostmapQueue.costmap_, dwb_critics::BaseObstacleCritic.costmap_, dwb_critics::MapGridCritic.costmap_, mppi::critics::CriticFunction.costmap_, mppi::Optimizer.costmap_, mppi::PathHandler.costmap_, nav2_navfn_planner::NavfnPlanner.costmap_, nav2_planner::PlannerServer.costmap_, nav2_regulated_pure_pursuit_controller::CollisionChecker.costmap_, nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController.costmap_, nav2_route::GoalIntentSearch::BreadthFirstSearch.costmap_, nav2_route::GoalIntentSearch::LoSCollisionChecker.costmap_, nav2_route::CostmapScorer.costmap_, nav2_route::CollisionMonitor.costmap_, nav2_simple_commander.footprint_collision_checker.FootprintCollisionChecker.costmap_, nav2_system_tests::PlannerTester.costmap_, theta_star::ThetaStar.costmap_, and nav2_voxel_grid::VoxelGrid::ClearVoxelInMap.costmap_.

Member Function Documentation

◆ footprintCost()

float nav2_simple_commander.footprint_collision_checker.FootprintCollisionChecker.footprintCost	(		self,
		Polygon	footprint
	)

Iterate over all the points in a footprint and check for collision.

Args
----
    footprint (Polygon): The footprint to calculate the collision cost for

Returns
-------
    LETHAL_OBSTACLE (int): If collision was found, 254 will be returned
    footprint_cost (float): The maximum cost found in the footprint points

Definition at line 51 of file footprint_collision_checker.py.

References dwb_critics::ObstacleFootprintCritic.lineCost(), nav2_costmap_2d::FootprintCollisionChecker< nav2_costmap_2d::Costmap2D * >.lineCost(), nav2_costmap_2d::FootprintCollisionChecker< std::shared_ptr< nav2_costmap_2d::Costmap2D > >.lineCost(), nav2_costmap_2d::FootprintCollisionChecker< CostmapT >.lineCost(), nav2_simple_commander.footprint_collision_checker.FootprintCollisionChecker.lineCost(), nav2_simple_commander.costmap_2d.PyCostmap2D.worldToMapValidated(), and nav2_simple_commander.footprint_collision_checker.FootprintCollisionChecker.worldToMapValidated().

Referenced by nav2_simple_commander.footprint_collision_checker.FootprintCollisionChecker.footprintCostAtPose().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ footprintCostAtPose()

float nav2_simple_commander.footprint_collision_checker.FootprintCollisionChecker.footprintCostAtPose	(		self,
		float	x,
		float	y,
		float	theta,
		Polygon	footprint
	)

Get the cost of a footprint at a specific Pose in map coordinates.

Args
----
    x (float): map coordinate X
    y (float): map coordinate Y
    theta (float): absolute rotation angle of the footprint
    footprint (Polygon): the footprint to calculate its cost at the given Pose

Returns
-------
    LETHAL_OBSTACLE (int): If collision was found, 254 will be returned
    footprint_cost (float): The maximum cost found in the footprint points

Definition at line 191 of file footprint_collision_checker.py.

References nav2_costmap_2d::FootprintCollisionChecker< std::shared_ptr< nav2_costmap_2d::Costmap2D > >.footprintCost(), nav2_costmap_2d::FootprintCollisionChecker< nav2_costmap_2d::Costmap2D * >.footprintCost(), nav2_costmap_2d::FootprintCollisionChecker< CostmapT >.footprintCost(), and nav2_simple_commander.footprint_collision_checker.FootprintCollisionChecker.footprintCost().

Here is the call graph for this function:

◆ lineCost()

float nav2_simple_commander.footprint_collision_checker.FootprintCollisionChecker.lineCost	(		self,
		float	x0,
		float	x1,
		float	y0,
		float	y1,
		float	step_size = `0.5`
	)

Iterate over all the points along a line and check for collision.

Args
----
    x0 (float): Abscissa of the initial point in map coordinates
    y0 (float): Ordinate of the initial point in map coordinates
    x1 (float): Abscissa of the final point in map coordinates
    y1 (float): Ordinate of the final point in map coordinates
    step_size (float): Optional, Increments' resolution, defaults to 0.5

Returns
-------
    LETHAL_OBSTACLE (int): If collision was found, 254 will be returned
    line_cost (float): The maximum cost found in the line points

Definition at line 94 of file footprint_collision_checker.py.

References dwb_critics::ObstacleFootprintCritic.pointCost(), nav2_costmap_2d::FootprintCollisionChecker< std::shared_ptr< nav2_costmap_2d::Costmap2D > >.pointCost(), nav2_costmap_2d::FootprintCollisionChecker< nav2_costmap_2d::Costmap2D * >.pointCost(), nav2_costmap_2d::FootprintCollisionChecker< CostmapT >.pointCost(), and nav2_simple_commander.footprint_collision_checker.FootprintCollisionChecker.pointCost().

Referenced by nav2_simple_commander.footprint_collision_checker.FootprintCollisionChecker.footprintCost().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ pointCost()

np.uint8 nav2_simple_commander.footprint_collision_checker.FootprintCollisionChecker.pointCost	(		self,
		int	x,
		int	y
	)

Get the cost of a point in the costmap using map coordinates XY.

Args
----
    mx (int): map coordinate X
    my (int): map coordinate Y

Returns
-------
    np.uint8: cost of a point

Definition at line 155 of file footprint_collision_checker.py.

References nav2_costmap_2d::ClearCostmapService.costmap_, nav2_costmap_2d::Costmap2D.costmap_, nav2_costmap_2d::Costmap2D::MarkCell.costmap_, nav2_costmap_2d::Costmap2D::PolygonOutlineCells.costmap_, nav2_costmap_2d::Costmap2DPublisher.costmap_, nav2_costmap_2d::CostmapSubscriber.costmap_, nav2_costmap_2d::FootprintCollisionChecker< std::shared_ptr< nav2_costmap_2d::Costmap2D > >.costmap_, nav2_costmap_2d::FootprintCollisionChecker< CostmapT >.costmap_, nav2_costmap_2d::FootprintCollisionChecker< nav2_costmap_2d::Costmap2D * >.costmap_, costmap_queue::CostmapQueue.costmap_, dwb_critics::BaseObstacleCritic.costmap_, dwb_critics::MapGridCritic.costmap_, mppi::critics::CriticFunction.costmap_, mppi::Optimizer.costmap_, mppi::PathHandler.costmap_, nav2_navfn_planner::NavfnPlanner.costmap_, nav2_planner::PlannerServer.costmap_, nav2_regulated_pure_pursuit_controller::CollisionChecker.costmap_, nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController.costmap_, nav2_route::GoalIntentSearch::BreadthFirstSearch.costmap_, nav2_route::GoalIntentSearch::LoSCollisionChecker.costmap_, nav2_route::CostmapScorer.costmap_, nav2_route::CollisionMonitor.costmap_, nav2_simple_commander.footprint_collision_checker.FootprintCollisionChecker.costmap_, nav2_system_tests::PlannerTester.costmap_, theta_star::ThetaStar.costmap_, and nav2_voxel_grid::VoxelGrid::ClearVoxelInMap.costmap_.

Referenced by nav2_simple_commander.footprint_collision_checker.FootprintCollisionChecker.lineCost().

Here is the caller graph for this function:

◆ setCostmap()

None nav2_simple_commander.footprint_collision_checker.FootprintCollisionChecker.setCostmap	(		self,
		PyCostmap2D	costmap
	)

Specify which costmap to use.

Args
----
    costmap (PyCostmap2D): costmap to use in the object's methods

Returns
-------
    None

Definition at line 175 of file footprint_collision_checker.py.

References nav2_costmap_2d::ClearCostmapService.costmap_, nav2_costmap_2d::Costmap2D.costmap_, nav2_costmap_2d::Costmap2D::MarkCell.costmap_, nav2_costmap_2d::Costmap2D::PolygonOutlineCells.costmap_, nav2_costmap_2d::Costmap2DPublisher.costmap_, nav2_costmap_2d::CostmapSubscriber.costmap_, nav2_costmap_2d::FootprintCollisionChecker< std::shared_ptr< nav2_costmap_2d::Costmap2D > >.costmap_, nav2_costmap_2d::FootprintCollisionChecker< CostmapT >.costmap_, nav2_costmap_2d::FootprintCollisionChecker< nav2_costmap_2d::Costmap2D * >.costmap_, costmap_queue::CostmapQueue.costmap_, dwb_critics::BaseObstacleCritic.costmap_, dwb_critics::MapGridCritic.costmap_, mppi::critics::CriticFunction.costmap_, mppi::Optimizer.costmap_, mppi::PathHandler.costmap_, nav2_navfn_planner::NavfnPlanner.costmap_, nav2_planner::PlannerServer.costmap_, nav2_regulated_pure_pursuit_controller::CollisionChecker.costmap_, nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController.costmap_, nav2_route::GoalIntentSearch::BreadthFirstSearch.costmap_, nav2_route::GoalIntentSearch::LoSCollisionChecker.costmap_, nav2_route::CostmapScorer.costmap_, nav2_route::CollisionMonitor.costmap_, nav2_simple_commander.footprint_collision_checker.FootprintCollisionChecker.costmap_, nav2_system_tests::PlannerTester.costmap_, theta_star::ThetaStar.costmap_, and nav2_voxel_grid::VoxelGrid::ClearVoxelInMap.costmap_.

◆ worldToMapValidated()

tuple[int, int] nav2_simple_commander.footprint_collision_checker.FootprintCollisionChecker.worldToMapValidated	(		self,
		float	wx,
		float	wy
	)

Get the map coordinate XY using world coordinate XY.

Args
----
    wx (float): world coordinate X
    wy (float): world coordinate Y

Returns
-------
    None: if coordinates are invalid
    tuple of int: mx, my (if coordinates are valid)
    mx (int): map coordinate X
    my (int): map coordinate Y

Definition at line 132 of file footprint_collision_checker.py.

References nav2_costmap_2d::ClearCostmapService.costmap_, nav2_costmap_2d::Costmap2D.costmap_, nav2_costmap_2d::Costmap2D::MarkCell.costmap_, nav2_costmap_2d::Costmap2D::PolygonOutlineCells.costmap_, nav2_costmap_2d::Costmap2DPublisher.costmap_, nav2_costmap_2d::CostmapSubscriber.costmap_, nav2_costmap_2d::FootprintCollisionChecker< std::shared_ptr< nav2_costmap_2d::Costmap2D > >.costmap_, nav2_costmap_2d::FootprintCollisionChecker< CostmapT >.costmap_, nav2_costmap_2d::FootprintCollisionChecker< nav2_costmap_2d::Costmap2D * >.costmap_, costmap_queue::CostmapQueue.costmap_, dwb_critics::BaseObstacleCritic.costmap_, dwb_critics::MapGridCritic.costmap_, mppi::critics::CriticFunction.costmap_, mppi::Optimizer.costmap_, mppi::PathHandler.costmap_, nav2_navfn_planner::NavfnPlanner.costmap_, nav2_planner::PlannerServer.costmap_, nav2_regulated_pure_pursuit_controller::CollisionChecker.costmap_, nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController.costmap_, nav2_route::GoalIntentSearch::BreadthFirstSearch.costmap_, nav2_route::GoalIntentSearch::LoSCollisionChecker.costmap_, nav2_route::CostmapScorer.costmap_, nav2_route::CollisionMonitor.costmap_, nav2_simple_commander.footprint_collision_checker.FootprintCollisionChecker.costmap_, nav2_system_tests::PlannerTester.costmap_, theta_star::ThetaStar.costmap_, and nav2_voxel_grid::VoxelGrid::ClearVoxelInMap.costmap_.