Costmap layer that inflates obstacles asymmetrically relative to the global path, biasing the navigable corridor toward one side. More...

#include <nav2_costmap_2d/include/nav2_costmap_2d/asymmetric_inflation_layer.hpp>

Inheritance diagram for nav2_costmap_2d::AsymmetricInflationLayer:

[legend]

Collaboration diagram for nav2_costmap_2d::AsymmetricInflationLayer:

[legend]

Public Member Functions
void	onInitialize () override
	Initialization process of layer on startup.

void	activate () override
	Activate the layer; registers the parameter validation/update callbacks.

void	deactivate () override
	Deactivate the layer; removes the parameter validation/update callbacks.

void	updateBounds (double robot_x, double robot_y, double robot_yaw, double min_x, double min_y, double max_x, double max_y) override
	Update the bounds of the master costmap by this layer's update dimensions. More...

void	updateCosts (nav2_costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j) override
	Update the costs in the master costmap. More...

void	matchSize () override
	Match the size of the master costmap.

Public Member Functions inherited from nav2_costmap_2d::InflationLayer
	InflationLayer ()
	A constructor.

	~InflationLayer ()
	A destructor.

void	onInitialize () override
	Initialization process of layer on startup.

void	deactivate () override
	Deactivate the layer.

void	activate () override
	Activate the layer.

void	updateBounds (double robot_x, double robot_y, double robot_yaw, double min_x, double min_y, double max_x, double max_y) override
	Update the bounds of the master costmap by this layer's update dimensions. More...

void	updateCosts (nav2_costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j) override
	Update the costs in the master costmap in the window. More...

void	matchSize () override
	Match the size of the master costmap.

bool	isClearable () override
	If clearing operations should be processed on this layer or not.

void	reset () override
	Reset this costmap.

unsigned char	computeCost (double distance) const override
	Given a distance, compute a cost. More...

mutex_t *	getMutex () override
	Get the mutex of the inflation information.

double	getCostScalingFactor () override
	Get the cost scaling factor. More...

double	getInflationRadius () override
	Get the inflation radius. More...

Public Member Functions inherited from nav2_costmap_2d::Layer
	Layer ()
	A constructor.

virtual	~Layer ()
	A destructor.

void	initialize (LayeredCostmap parent, std::string name, nav2::TransformBuffer tf, const nav2::LifecycleNode::WeakPtr &node, rclcpp::CallbackGroup::SharedPtr callback_group)
	Initialization process of layer on startup.

std::string	getName () const
	Get the name of the costmap layer.

bool	isCurrent () const
	Check to make sure all the data in the layer is up to date. If the layer is not up to date, then it may be unsafe to plan using the data from this layer, and the planner may need to know. More...

void	setCurrent (bool current)
	Set whether the data in the layer is up to date. More...

bool	isEnabled () const
	Gets whether the layer is enabled.

const std::vector< geometry_msgs::msg::Point > &	getFootprint () const
	Convenience function for layered_costmap_->getFootprint().

std::string	getFullName (const std::string &param_name)
	Convenience functions for declaring ROS parameters.

std::string	joinWithParentNamespace (const std::string &topic)

Protected Member Functions
void	onFootprintChanged () override
	Process updates on footprint changes to the inflation layer.

void	globalPathCallback (const nav_msgs::msg::Path::ConstSharedPtr msg)
	Callback for incoming global path messages.

std::vector< AsymmetricPathSegment >	extractLocalPathSegments (nav2_costmap_2d::Costmap2D &master_grid)
	Extract global path segments that overlap the local costmap window. More...

Side	computeObstacleSide (double cx, double cy, const std::vector< size_t > &candidates, const std::vector< AsymmetricPathSegment > &local_path_segments)
	Classify an obstacle cell as left (+1), right (-1), or neutral (0) relative to the closest path segment. More...

std::unordered_map< uint64_t, std::vector< size_t > >	buildPathSpatialHash (const std::vector< AsymmetricPathSegment > &local_path_segments)
	Build a spatial hash mapping 2D bucket keys to path segment indices. More...

MatrixXfRM	seedDistanceMap (nav2_costmap_2d::Costmap2D &master_grid, int roi_min_i, int roi_min_j, int roi_width, int roi_height, const std::unordered_map< uint64_t, std::vector< size_t >> &spatial_hash, const std::vector< AsymmetricPathSegment > &local_path_segments)
	Build a distance map seeded from disfavored-side obstacle boundary cells. More...

void	applyInflation (unsigned char *master_array, const MatrixXfRM &distance_map, int min_i, int min_j, int max_i, int max_j, int roi_min_i, int roi_min_j, unsigned int size_x)
	Apply disfavored-side costs from a distance map with max(old, new) semantics. More...

void	computeAsymmetricCaches ()
	Pre-compute cost_lut_disfavored_ using c_side (the smaller per-side scaling factor)

rcl_interfaces::msg::SetParametersResult	validateParameterUpdatesCallback (const std::vector< rclcpp::Parameter > &parameters)
	Validate parameter updates (pre-set callback). Returns success/failure without mutating any state.

void	updateParametersCallback (const std::vector< rclcpp::Parameter > &parameters)
	Apply parameter updates (post-set callback) after they have been validated. Recomputes caches when geometry parameters change.

Protected Member Functions inherited from nav2_costmap_2d::InflationLayer
void	applyInflation (unsigned char *master_array, const MatrixXfRM &distance_map, int min_i, int min_j, int max_i, int max_j, int roi_min_i, int roi_min_j, unsigned int size_x)
	Apply inflation costs from distance map to costmap. More...

void	onFootprintChanged () override
	Process updates on footprint changes to the inflation layer.

unsigned int	cellDistance (double world_dist)
	Convert world distance to cell distance.

void	computeCaches ()
	Generate cost lookup table for distance to cost mapping.

int	getOptimalThreadCount ()
	Determine optimal thread count based on system resources. More...

rcl_interfaces::msg::SetParametersResult	validateParameterUpdatesCallback (const std::vector< rclcpp::Parameter > &parameters)
	Validate incoming parameter updates before applying them. This callback is triggered when one or more parameters are about to be updated. It checks the validity of parameter values and rejects updates that would lead to invalid or inconsistent configurations. More...

void	updateParametersCallback (const std::vector< rclcpp::Parameter > &parameters)
	Apply parameter updates after validation This callback is executed when parameters have been successfully updated. It updates the internal configuration of the node with the new parameter values. More...

void	updateInscribedRadius ()
	Update inscribed_radius_ from custom_inscribed_radius_ or the footprint, logging a warning when the custom value is in use.

Protected Attributes
double	cost_scaling_factor_left_
	Exponential decay rate for cells on the LEFT side of the path.

double	cost_scaling_factor_right_
	Exponential decay rate for cells on the RIGHT side of the path.

double	goal_distance_threshold_
	Distance to goal where asymmetry disables to prevent docking oscillations.

std::string	plan_topic_

tf2::Duration	transform_tolerance_
	Timeout for path-frame TF lookups.

double	current_robot_x_ {0.0}

double	current_robot_y_ {0.0}

bool	asymmetry_active_ {false}

std::vector< unsigned char >	cost_lut_disfavored_
	Cost LUT for the disfavored side, built with c_side (the smaller per-side scaling factor)

nav2::Subscription< nav_msgs::msg::Path >::SharedPtr	path_sub_

nav_msgs::msg::Path::ConstSharedPtr	latest_global_path_

std::mutex	path_mutex_

Protected Attributes inherited from nav2_costmap_2d::InflationLayer
double	inflation_radius_

double	inscribed_radius_

double	custom_inscribed_radius_

double	cost_scaling_factor_

bool	inflate_unknown_

bool	inflate_around_unknown_

unsigned int	cell_inflation_radius_

int	num_threads_

double	resolution_

std::vector< unsigned char >	cost_lut_

double	last_min_x_

double	last_min_y_

double	last_max_x_

double	last_max_y_

bool	need_reinflation_

mutex_t *	access_

rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr	post_set_params_handler_

rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr	on_set_params_handler_

Protected Attributes inherited from nav2_costmap_2d::Layer
LayeredCostmap *	layered_costmap_

std::string	name_

nav2::TransformBuffer *	tf_

rclcpp::CallbackGroup::SharedPtr	callback_group_

nav2::LifecycleNode::WeakPtr	node_

rclcpp::Clock::SharedPtr	clock_

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

std::atomic_bool	current_

bool	enabled_

Additional Inherited Members
Public Types inherited from nav2_costmap_2d::InflationLayerInterface
typedef std::recursive_mutex	mutex_t

Static Public Member Functions inherited from nav2_costmap_2d::InflationLayerInterface
static std::shared_ptr< InflationLayerInterface >	getInflationLayer (std::shared_ptr< nav2_costmap_2d::Costmap2DROS > &costmap_ros, const std::string layer_name="")
	Get the inflation layer from a costmap, checking for both InflationLayer and LegacyInflationLayer implementations. More...

Static Protected Attributes inherited from nav2_costmap_2d::InflationLayer
static constexpr int	COST_LUT_PRECISION = 100

Detailed Description

Costmap layer that inflates obstacles asymmetrically relative to the global path, biasing the navigable corridor toward one side.

Inherits the distance-transform based InflationLayer, which writes the symmetric baseline first. This layer then classifies each lethal obstacle as left (+1), right (-1), or neutral (0) relative to the path and runs a second distance transform seeded from only the disfavored-side boundary cells. Falls back to the inherited symmetric inflation when no path is available, the goal is nearby, or the per-side scaling factors are equal.

Definition at line 59 of file asymmetric_inflation_layer.hpp.

Member Function Documentation

◆ applyInflation()

void nav2_costmap_2d::AsymmetricInflationLayer::applyInflation	(	unsigned char *	master_array,
		const MatrixXfRM &	distance_map,
		int	min_i,
		int	min_j,
		int	max_i,
		int	max_j,
		int	roi_min_i,
		int	roi_min_j,
		unsigned int	size_x
	)

protected

Apply disfavored-side costs from a distance map with max(old, new) semantics.

Mirrors InflationLayer::applyInflation() but uses cost_lut_disfavored_ (built with c_side) instead of the parent's cost_lut_ (built with c_max). Costs are applied only within the originally requested update window.

Parameters

master_array	Raw costmap data pointer.
distance_map	Result of distanceTransform2D() on the disfavored seed map.
min_i	Left edge of the write window.
min_j	Bottom edge of the write window.
max_i	Right edge of the write window.
max_j	Top edge of the write window.
roi_min_i	Left edge of the padded ROI used to build distance_map.
roi_min_j	Bottom edge of the padded ROI used to build distance_map.
size_x	Full costmap width (cells), for index arithmetic.

Definition at line 594 of file asymmetric_inflation_layer.cpp.

◆ buildPathSpatialHash()

std::unordered_map< uint64_t, std::vector< size_t > > nav2_costmap_2d::AsymmetricInflationLayer::buildPathSpatialHash ( const std::vector< AsymmetricPathSegment > & local_path_segments )

protected

Build a spatial hash mapping 2D bucket keys to path segment indices.

Uses inflation_radius_ as the bucket size so each bucket spans exactly one inflation radius. Each segment is inserted into every bucket whose padded AABB it overlaps, enabling O(1) nearest-segment queries during the disfavored-cell seeding phase.

Parameters

local_path_segments Original path segments in costmap-frame world coordinates.

Returns: Hash map from packed (bucket_x, bucket_y) key to segment index list.

Definition at line 478 of file asymmetric_inflation_layer.cpp.

◆ computeObstacleSide()

Side nav2_costmap_2d::AsymmetricInflationLayer::computeObstacleSide	(	double	cx,
		double	cy,
		const std::vector< size_t > &	candidates,
		const std::vector< AsymmetricPathSegment > &	local_path_segments
	)

protected

Classify an obstacle cell as left (+1), right (-1), or neutral (0) relative to the closest path segment.

For each candidate segment, performs an AABB rejection pass followed by an exact perpendicular-distance and cross-product computation to determine side. Obstacles whose nearest perpendicular distance exceeds the inflation radius are returned as 0 (neutral -> symmetric inflation).

Parameters

cx	World x-coordinate of the obstacle cell (metres).
cy	World y-coordinate of the obstacle cell (metres).
candidates	Segment indices whose padded AABB may contain (cx, cy).
local_path_segments	Original path segments in costmap-frame world coordinates.

Returns: +1 (left of path), -1 (right of path), or 0 (neutral / beyond inflation radius).

Definition at line 329 of file asymmetric_inflation_layer.cpp.

◆ extractLocalPathSegments()

std::vector< AsymmetricPathSegment > nav2_costmap_2d::AsymmetricInflationLayer::extractLocalPathSegments ( nav2_costmap_2d::Costmap2D & master_grid )

protected

Extract global path segments that overlap the local costmap window.

Transforms the path inside the local costmap window into the costmap frame via TF2. Returns an empty vector when the robot is within goal_distance_threshold_ of the goal, which disables asymmetry near the goal to prevent docking oscillations.

Parameters

master_grid Reference costmap used to filter points to the current window.

Returns: Path segments in costmap-frame world coordinates; empty disables asymmetry.

Definition at line 206 of file asymmetric_inflation_layer.cpp.

References nav2_costmap_2d::Costmap2D::getOriginX(), nav2_costmap_2d::Costmap2D::getOriginY(), nav2_costmap_2d::Costmap2D::getResolution(), nav2_costmap_2d::Costmap2D::getSizeInCellsX(), and nav2_costmap_2d::Costmap2D::getSizeInCellsY().

Here is the call graph for this function:

◆ seedDistanceMap()

MatrixXfRM nav2_costmap_2d::AsymmetricInflationLayer::seedDistanceMap	(	nav2_costmap_2d::Costmap2D &	master_grid,
		int	roi_min_i,
		int	roi_min_j,
		int	roi_width,
		int	roi_height,
		const std::unordered_map< uint64_t, std::vector< size_t >> &	spatial_hash,
		const std::vector< AsymmetricPathSegment > &	local_path_segments
	)

protected

Build a distance map seeded from disfavored-side obstacle boundary cells.

Initialises a MatrixXfRM (roi_height × roi_width) to DT_INF, then sets disfavored boundary cells to 0.0f. The caller passes the result to DistanceTransform::distanceTransform2D() then applyInflation().

Parameters

master_grid	Costmap used for coordinate and cost lookups.
roi_min_i	Left edge of the padded ROI (cells).
roi_min_j	Bottom edge of the padded ROI (cells).
roi_width	Width of the padded ROI (cells).
roi_height	Height of the padded ROI (cells).
spatial_hash	Segment lookup structure from buildPathSpatialHash().
local_path_segments	Original path segments in costmap-frame world coordinates.

Returns: Distance map with 0.0f at disfavored seeds and DT_INF elsewhere.

Definition at line 518 of file asymmetric_inflation_layer.cpp.

References nav2_costmap_2d::Costmap2D::getCharMap(), nav2_costmap_2d::Costmap2D::getIndex(), nav2_costmap_2d::Costmap2D::getSizeInCellsX(), nav2_costmap_2d::Costmap2D::getSizeInCellsY(), and nav2_costmap_2d::Costmap2D::mapToWorld().

Here is the call graph for this function:

◆ updateBounds()

void nav2_costmap_2d::AsymmetricInflationLayer::updateBounds	(	double	robot_x,
		double	robot_y,
		double	robot_yaw,
		double *	min_x,
		double *	min_y,
		double *	max_x,
		double *	max_y
	)

overridevirtual

Update the bounds of the master costmap by this layer's update dimensions.

Stores the robot position for goal-proximity checks in updateCosts(), then delegates the actual inflation bound expansion to InflationLayer.

Parameters

robot_x	X pose of robot
robot_y	Y pose of robot
robot_yaw	Robot orientation
min_x	X min map coord of the window to update
min_y	Y min map coord of the window to update
max_x	X max map coord of the window to update
max_y	Y max map coord of the window to update

Implements nav2_costmap_2d::Layer.

Definition at line 184 of file asymmetric_inflation_layer.cpp.

◆ updateCosts()

void nav2_costmap_2d::AsymmetricInflationLayer::updateCosts	(	nav2_costmap_2d::Costmap2D &	master_grid,
		int	min_i,
		int	min_j,
		int	max_i,
		int	max_j
	)

overridevirtual

Update the costs in the master costmap.

First writes the inherited symmetric inflation baseline. Runs a second distance-transform pass seeded from disfavored-side boundary cells when a valid path is available and the two per-side decay rates differ. Otherwise leaves the symmetric baseline unchanged.

Parameters

master_grid	The master costmap grid to update
min_i	X min cell index of the window to update
min_j	Y min cell index of the window to update
max_i	X max cell index of the window to update
max_j	Y max cell index of the window to update