A costmap grid collision checker. More...

#include <nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp>

Inheritance diagram for nav2_smac_planner::GridCollisionChecker:

Collaboration diagram for nav2_smac_planner::GridCollisionChecker:

Public Member Functions
	GridCollisionChecker (std::shared_ptr< nav2_costmap_2d::Costmap2DROS > costmap, unsigned int num_quantizations, rclcpp_lifecycle::LifecycleNode::SharedPtr node)
	A constructor for nav2_smac_planner::GridCollisionChecker for use when regular bin intervals are appropriate. More...

void	setFootprint (const nav2_costmap_2d::Footprint &footprint, const bool &radius, const double &possible_collision_cost)
	A constructor for nav2_smac_planner::GridCollisionChecker for use when irregular bin intervals are appropriate. More...

bool	inCollision (const float &x, const float &y, const float &theta, const bool &traverse_unknown)
	Check if in collision with costmap and footprint at pose. More...

bool	inCollision (const unsigned int &i, const bool &traverse_unknown)
	Check if in collision with costmap and footprint at pose. More...

float	getCost ()
	Get cost at footprint pose in costmap. More...

std::vector< float > &	getPrecomputedAngles ()
	Get the angles of the precomputed footprint orientations. More...

std::shared_ptr< nav2_costmap_2d::Costmap2DROS >	getCostmapROS ()
	Get costmap ros object for inflation layer params. More...

bool	outsideRange (const unsigned int &max, const float &value)
	Check if value outside the range. More...

Public Member Functions inherited from nav2_costmap_2d::FootprintCollisionChecker< nav2_costmap_2d::Costmap2D * >
	FootprintCollisionChecker ()
	A constructor.

	FootprintCollisionChecker (nav2_costmap_2d::Costmap2D * costmap)
	A constructor.

double	footprintCost (const Footprint &footprint)
	Find the footprint cost in oriented footprint.

double	footprintCostAtPose (double x, double y, double theta, const Footprint &footprint)
	Find the footprint cost a a post with an unoriented footprint.

double	lineCost (int x0, int x1, int y0, int y1) const
	Get the cost for a line segment.

bool	worldToMap (double wx, double wy, unsigned int &mx, unsigned int &my)
	Get the map coordinates from a world point.

double	pointCost (int x, int y) const
	Get the cost of a point.

void	setCostmap (nav2_costmap_2d::Costmap2D * costmap)
	Set the current costmap object to use for collision detection.

nav2_costmap_2d::Costmap2D *	getCostmap ()
	Get the current costmap object.

Protected Attributes
std::shared_ptr< nav2_costmap_2d::Costmap2DROS >	costmap_ros_

std::vector< nav2_costmap_2d::Footprint >	oriented_footprints_

nav2_costmap_2d::Footprint	unoriented_footprint_

float	center_cost_

bool	footprint_is_radius_ {false}

std::vector< float >	angles_

float	possible_collision_cost_ {-1}

rclcpp::Logger	logger_ {rclcpp::get_logger("SmacPlannerCollisionChecker")}

rclcpp::Clock::SharedPtr	clock_

Protected Attributes inherited from nav2_costmap_2d::FootprintCollisionChecker< nav2_costmap_2d::Costmap2D * >
nav2_costmap_2d::Costmap2D *	costmap_

Detailed Description

A costmap grid collision checker.

Definition at line 33 of file collision_checker.hpp.

Constructor & Destructor Documentation

◆ GridCollisionChecker()

nav2_smac_planner::GridCollisionChecker::GridCollisionChecker	(	std::shared_ptr< nav2_costmap_2d::Costmap2DROS >	costmap,
		unsigned int	num_quantizations,
		rclcpp_lifecycle::LifecycleNode::SharedPtr	node
	)

A constructor for nav2_smac_planner::GridCollisionChecker for use when regular bin intervals are appropriate.

Parameters

costmap	The costmap to collision check against
num_quantizations	The number of quantizations to precompute footprint
node	Node to extract clock and logger from orientations for to speed up collision checking

Definition at line 20 of file collision_checker.cpp.

Member Function Documentation

◆ getCost()

float nav2_smac_planner::GridCollisionChecker::getCost ( )

Get cost at footprint pose in costmap.

Returns: the cost at the pose in costmap

Definition at line 185 of file collision_checker.cpp.

Referenced by nav2_smac_planner::Node2D::isNodeValid(), nav2_smac_planner::NodeHybrid::isNodeValid(), and nav2_smac_planner::NodeLattice::isNodeValid().

Here is the caller graph for this function:

◆ getCostmapROS()

std::shared_ptr<nav2_costmap_2d::Costmap2DROS> nav2_smac_planner::GridCollisionChecker::getCostmapROS ( )

inline

Get costmap ros object for inflation layer params.

Returns: Costmap ros

Definition at line 114 of file collision_checker.hpp.

◆ getPrecomputedAngles()

std::vector<float>& nav2_smac_planner::GridCollisionChecker::getPrecomputedAngles ( )

inline

Get the angles of the precomputed footprint orientations.

Returns: the ordered vector of angles corresponding to footprints

Definition at line 105 of file collision_checker.hpp.

Referenced by nav2_smac_planner::NodeLattice::isNodeValid().

Here is the caller graph for this function:

◆ inCollision() [1/2]

bool nav2_smac_planner::GridCollisionChecker::inCollision	(	const float &	x,
		const float &	y,
		const float &	theta,
		const bool &	traverse_unknown
	)

Check if in collision with costmap and footprint at pose.

Parameters

x	X coordinate of pose to check against
y	Y coordinate of pose to check against
theta	Angle bin number of pose to check against (NOT radians)
traverse_unknown	Whether or not to traverse in unknown space

Returns: boolean if in collision or not.

Definition at line 104 of file collision_checker.cpp.

References nav2_costmap_2d::FootprintCollisionChecker< nav2_costmap_2d::Costmap2D * >::footprintCost(), nav2_costmap_2d::Costmap2D::getCost(), nav2_costmap_2d::Costmap2D::getSizeInCellsX(), nav2_costmap_2d::Costmap2D::getSizeInCellsY(), nav2_costmap_2d::Costmap2D::mapToWorld(), and outsideRange().

Referenced by nav2_smac_planner::Node2D::isNodeValid(), nav2_smac_planner::NodeHybrid::isNodeValid(), and nav2_smac_planner::NodeLattice::isNodeValid().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ inCollision() [2/2]

bool nav2_smac_planner::GridCollisionChecker::inCollision	(	const unsigned int &	i,
		const bool &	traverse_unknown
	)

Check if in collision with costmap and footprint at pose.

Parameters

i	Index to search collision status of
traverse_unknown	Whether or not to traverse in unknown space

Returns: boolean if in collision or not.

Definition at line 172 of file collision_checker.cpp.

References nav2_costmap_2d::Costmap2D::getCost().

Here is the call graph for this function:

◆ outsideRange()

bool nav2_smac_planner::GridCollisionChecker::outsideRange	(	const unsigned int &	max,
		const float &	value
	)

Check if value outside the range.

Parameters

min	Minimum value of the range
max	Maximum value of the range
value	the value to check if it is within the range

Returns: boolean if in range or not

Definition at line 191 of file collision_checker.cpp.

Referenced by inCollision().

Here is the caller graph for this function:

◆ setFootprint()

void nav2_smac_planner::GridCollisionChecker::setFootprint	(	const nav2_costmap_2d::Footprint &	footprint,
		const bool &	radius,
		const double &	possible_collision_cost
	)

A constructor for nav2_smac_planner::GridCollisionChecker for use when irregular bin intervals are appropriate.

Parameters

costmap	The costmap to collision check against
angles	The vector of possible angle bins to precompute for orientations for to speed up collision checking, in radians

Set the footprint to use with collision checker

Parameters

footprint	The footprint to collision check against
radius	Whether or not the footprint is a circle and use radius collision checking

Definition at line 51 of file collision_checker.cpp.

Referenced by nav2_smac_planner::SmacPlannerHybrid::configure(), nav2_smac_planner::SmacPlannerHybrid::createPlan(), nav2_smac_planner::SmacPlannerLattice::createPlan(), and nav2_smac_planner::SmacPlannerHybrid::dynamicParametersCallback().

Here is the caller graph for this function:

The documentation for this class was generated from the following files:

nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp
nav2_smac_planner/src/collision_checker.cpp

Public Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ GridCollisionChecker()

Member Function Documentation

◆ getCost()

◆ getCostmapROS()

◆ getPrecomputedAngles()

◆ inCollision() [1/2]

◆ inCollision() [2/2]

◆ outsideRange()

◆ setFootprint()