Takes in laser and pointcloud data to populate into 2D costmap. More...

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

Inheritance diagram for nav2_costmap_2d::ObstacleLayer:

[legend]

Collaboration diagram for nav2_costmap_2d::ObstacleLayer:

[legend]

Public Member Functions
	ObstacleLayer ()
	A constructor.

virtual	~ObstacleLayer ()
	A destructor.

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

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

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

virtual void	deactivate ()
	Deactivate the layer.

virtual void	activate ()
	Activate the layer.

virtual void	reset ()
	Reset this costmap.

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

rcl_interfaces::msg::SetParametersResult	dynamicParametersCallback (std::vector< rclcpp::Parameter > parameters)
	Callback executed when a parameter change is detected. More...

void	resetBuffersLastUpdated ()
	triggers the update of observations buffer

void	laserScanCallback (sensor_msgs::msg::LaserScan::ConstSharedPtr message, const std::shared_ptr< nav2_costmap_2d::ObservationBuffer > &buffer)
	A callback to handle buffering LaserScan messages. More...

void	laserScanValidInfCallback (sensor_msgs::msg::LaserScan::ConstSharedPtr message, const std::shared_ptr< nav2_costmap_2d::ObservationBuffer > &buffer)
	A callback to handle buffering LaserScan messages which need filtering to turn Inf values into range_max. More...

void	pointCloud2Callback (sensor_msgs::msg::PointCloud2::ConstSharedPtr message, const std::shared_ptr< nav2_costmap_2d::ObservationBuffer > &buffer)
	A callback to handle buffering PointCloud2 messages. More...

void	addStaticObservation (nav2_costmap_2d::Observation &obs, bool marking, bool clearing)

void	clearStaticObservations (bool marking, bool clearing)

Public Member Functions inherited from nav2_costmap_2d::CostmapLayer
	CostmapLayer ()
	CostmapLayer constructor.

bool	isDiscretized ()
	If layer is discrete.

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

virtual void	clearArea (int start_x, int start_y, int end_x, int end_y, bool invert)
	Clear an are in the costmap with the given dimension if invert, then clear everything except these dimensions.

void	addExtraBounds (double mx0, double my0, double mx1, double my1)

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

virtual	~Layer ()
	A destructor.

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

virtual void	onFootprintChanged ()
	LayeredCostmap calls this whenever the footprint there changes (via LayeredCostmap::setFootprint()). Override to be notified of changes to the robot's footprint.

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...

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

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

void	declareParameter (const std::string &param_name, const rclcpp::ParameterValue &value)
	Convenience functions for declaring ROS parameters.

void	declareParameter (const std::string &param_name, const rclcpp::ParameterType &param_type)
	Convenience functions for declaring ROS parameters.

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

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

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

Public Member Functions inherited from nav2_costmap_2d::Costmap2D
	Costmap2D (unsigned int cells_size_x, unsigned int cells_size_y, double resolution, double origin_x, double origin_y, unsigned char default_value=0)
	Constructor for a costmap. More...

	Costmap2D (const Costmap2D &map)
	Copy constructor for a costmap, creates a copy efficiently. More...

	Costmap2D (const nav_msgs::msg::OccupancyGrid &map)
	Constructor for a costmap from an OccupancyGrid map. More...

Costmap2D &	operator= (const Costmap2D &map)
	Overloaded assignment operator. More...

bool	copyCostmapWindow (const Costmap2D &map, double win_origin_x, double win_origin_y, double win_size_x, double win_size_y)
	Turn this costmap into a copy of a window of a costmap passed in. More...

bool	copyWindow (const Costmap2D &source, unsigned int sx0, unsigned int sy0, unsigned int sxn, unsigned int syn, unsigned int dx0, unsigned int dy0)
	Copies the (x0,y0)..(xn,yn) window from source costmap into a current costmap. More...

	Costmap2D ()
	Default constructor.

virtual	~Costmap2D ()
	Destructor.

unsigned char	getCost (unsigned int mx, unsigned int my) const
	Get the cost of a cell in the costmap. More...

unsigned char	getCost (unsigned int index) const
	Get the cost of a cell in the costmap. More...

void	setCost (unsigned int mx, unsigned int my, unsigned char cost)
	Set the cost of a cell in the costmap. More...

void	mapToWorld (unsigned int mx, unsigned int my, double &wx, double &wy) const
	Convert from map coordinates to world coordinates. More...

void	mapToWorldNoBounds (int mx, int my, double &wx, double &wy) const
	Convert from map coordinates to world coordinates with no bounds checking. More...

bool	worldToMap (double wx, double wy, unsigned int &mx, unsigned int &my) const
	Convert from world coordinates to map coordinates. More...

bool	worldToMapContinuous (double wx, double wy, float &mx, float &my) const
	Convert from world coordinates to map coordinates. More...

void	worldToMapNoBounds (double wx, double wy, int &mx, int &my) const
	Convert from world coordinates to map coordinates without checking for legal bounds. More...

void	worldToMapEnforceBounds (double wx, double wy, int &mx, int &my) const
	Convert from world coordinates to map coordinates, constraining results to legal bounds. More...

unsigned int	getIndex (unsigned int mx, unsigned int my) const
	Given two map coordinates... compute the associated index. More...

void	indexToCells (unsigned int index, unsigned int &mx, unsigned int &my) const
	Given an index... compute the associated map coordinates. More...

unsigned char *	getCharMap () const
	Will return a pointer to the underlying unsigned char array used as the costmap. More...

unsigned int	getSizeInCellsX () const
	Accessor for the x size of the costmap in cells. More...

unsigned int	getSizeInCellsY () const
	Accessor for the y size of the costmap in cells. More...

double	getSizeInMetersX () const
	Accessor for the x size of the costmap in meters. More...

double	getSizeInMetersY () const
	Accessor for the y size of the costmap in meters. More...

double	getOriginX () const
	Accessor for the x origin of the costmap. More...

double	getOriginY () const
	Accessor for the y origin of the costmap. More...

double	getResolution () const
	Accessor for the resolution of the costmap. More...

void	setDefaultValue (unsigned char c)
	Set the default background value of the costmap. More...

unsigned char	getDefaultValue ()
	Get the default background value of the costmap. More...

bool	setConvexPolygonCost (const std::vector< geometry_msgs::msg::Point > &polygon, unsigned char cost_value)
	Sets the cost of a convex polygon to a desired value. More...

bool	getMapRegionOccupiedByPolygon (const std::vector< geometry_msgs::msg::Point > &polygon, std::vector< MapLocation > &polygon_map_region)
	Gets the map region occupied by polygon. More...

void	setMapRegionOccupiedByPolygon (const std::vector< MapLocation > &polygon_map_region, unsigned char new_cost_value)
	Sets the given map region to desired value. More...

void	restoreMapRegionOccupiedByPolygon (const std::vector< MapLocation > &polygon_map_region)
	Restores the corresponding map region using given map region. More...

void	polygonOutlineCells (const std::vector< MapLocation > &polygon, std::vector< MapLocation > &polygon_cells)
	Get the map cells that make up the outline of a polygon. More...

void	convexFillCells (const std::vector< MapLocation > &polygon, std::vector< MapLocation > &polygon_cells)
	Get the map cells that fill a convex polygon. More...

virtual void	updateOrigin (double new_origin_x, double new_origin_y)
	Move the origin of the costmap to a new location.... keeping data when it can. More...

bool	saveMap (std::string file_name)
	Save the costmap out to a pgm file. More...

void	resizeMap (unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y)
	Resize the costmap.

void	resetMap (unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn)
	Reset the costmap in bounds.

void	resetMapToValue (unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn, unsigned char value)
	Reset the costmap in bounds to a value.

unsigned int	cellDistance (double world_dist)
	Given distance in the world... convert it to cells. More...

mutex_t *	getMutex ()

Protected Member Functions
bool	getMarkingObservations (std::vector< nav2_costmap_2d::Observation > &marking_observations) const
	Get the observations used to mark space. More...

bool	getClearingObservations (std::vector< nav2_costmap_2d::Observation > &clearing_observations) const
	Get the observations used to clear space. More...

virtual void	raytraceFreespace (const nav2_costmap_2d::Observation &clearing_observation, double min_x, double min_y, double max_x, double max_y)
	Clear freespace based on one observation. More...

void	updateRaytraceBounds (double ox, double oy, double wx, double wy, double max_range, double min_range, double min_x, double min_y, double max_x, double max_y)
	Process update costmap with raytracing the window bounds.

void	updateFootprint (double robot_x, double robot_y, double robot_yaw, double min_x, double min_y, double max_x, double max_y)
	Clear costmap layer info below the robot's footprint.

Protected Member Functions inherited from nav2_costmap_2d::CostmapLayer
void	updateWithTrueOverwrite (nav2_costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j)

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

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

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

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

void	touch (double x, double y, double min_x, double min_y, double max_x, double max_y)

void	useExtraBounds (double min_x, double min_y, double max_x, double max_y)

CombinationMethod	combination_method_from_int (const int value)
	Converts an integer to a CombinationMethod enum and logs on failure. More...

Protected Member Functions inherited from nav2_costmap_2d::Costmap2D
template<typename data_type >
void	copyMapRegion (data_type source_map, unsigned int sm_lower_left_x, unsigned int sm_lower_left_y, unsigned int sm_size_x, data_type dest_map, unsigned int dm_lower_left_x, unsigned int dm_lower_left_y, unsigned int dm_size_x, unsigned int region_size_x, unsigned int region_size_y)
	Copy a region of a source map into a destination map. More...

virtual void	deleteMaps ()
	Deletes the costmap, static_map, and markers data structures.

virtual void	resetMaps ()
	Resets the costmap and static_map to be unknown space.

virtual void	initMaps (unsigned int size_x, unsigned int size_y)
	Initializes the costmap, static_map, and markers data structures. More...

template<class ActionType >
void	raytraceLine (ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, unsigned int max_length=UINT_MAX, unsigned int min_length=0)
	Raytrace a line and apply some action at each step. More...

Protected Attributes
std::vector< geometry_msgs::msg::Point >	transformed_footprint_

bool	footprint_clearing_enabled_

std::string	global_frame_
	The global frame for the costmap.

double	min_obstacle_height_
	Max Obstacle Height.

double	max_obstacle_height_
	Max Obstacle Height.

laser_geometry::LaserProjection	projector_
	Used to project laser scans into point clouds.

std::vector< std::shared_ptr< message_filters::SubscriberBase< rclcpp_lifecycle::LifecycleNode > > >	observation_subscribers_
	Used for the observation message filters.

std::vector< std::shared_ptr< tf2_ros::MessageFilterBase > >	observation_notifiers_
	Used to make sure that transforms are available for each sensor.

std::vector< std::shared_ptr< nav2_costmap_2d::ObservationBuffer > >	observation_buffers_
	Used to store observations from various sensors.

std::vector< std::shared_ptr< nav2_costmap_2d::ObservationBuffer > >	marking_buffers_
	Used to store observation buffers used for marking obstacles.

std::vector< std::shared_ptr< nav2_costmap_2d::ObservationBuffer > >	clearing_buffers_
	Used to store observation buffers used for clearing obstacles.

rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr	dyn_params_handler_
	Dynamic parameters handler.

std::vector< nav2_costmap_2d::Observation >	static_clearing_observations_

std::vector< nav2_costmap_2d::Observation >	static_marking_observations_

bool	rolling_window_

bool	was_reset_

nav2_costmap_2d::CombinationMethod	combination_method_

Protected Attributes inherited from nav2_costmap_2d::CostmapLayer
bool	has_extra_bounds_

Protected Attributes inherited from nav2_costmap_2d::Layer
LayeredCostmap *	layered_costmap_

std::string	name_

tf2_ros::Buffer *	tf_

rclcpp::CallbackGroup::SharedPtr	callback_group_

nav2::LifecycleNode::WeakPtr	node_

rclcpp::Clock::SharedPtr	clock_

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

bool	current_

bool	enabled_

std::unordered_set< std::string >	local_params_

Protected Attributes inherited from nav2_costmap_2d::Costmap2D
unsigned int	size_x_

unsigned int	size_y_

double	resolution_

double	origin_x_

double	origin_y_

unsigned char *	costmap_

unsigned char	default_value_

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

Detailed Description

Takes in laser and pointcloud data to populate into 2D costmap.

Definition at line 71 of file obstacle_layer.hpp.

Member Function Documentation

◆ dynamicParametersCallback()

rcl_interfaces::msg::SetParametersResult nav2_costmap_2d::ObstacleLayer::dynamicParametersCallback ( std::vector< rclcpp::Parameter > parameters )

Callback executed when a parameter change is detected.

Parameters

event ParameterEvent message

Definition at line 349 of file obstacle_layer.cpp.

References nav2_costmap_2d::CostmapLayer::combination_method_from_int(), max_obstacle_height_, and min_obstacle_height_.

Referenced by onInitialize().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getClearingObservations()

bool nav2_costmap_2d::ObstacleLayer::getClearingObservations ( std::vector< nav2_costmap_2d::Observation > & clearing_observations ) const

protected

Get the observations used to clear space.

Parameters

clearing_observations A reference to a vector that will be populated with the observations

Returns: True if all the observation buffers are current, false otherwise

Definition at line 664 of file obstacle_layer.cpp.

References clearing_buffers_.

Referenced by updateBounds(), and nav2_costmap_2d::VoxelLayer::updateBounds().

Here is the caller graph for this function:

◆ getMarkingObservations()

bool nav2_costmap_2d::ObstacleLayer::getMarkingObservations ( std::vector< nav2_costmap_2d::Observation > & marking_observations ) const

protected

Get the observations used to mark space.

Parameters

marking_observations A reference to a vector that will be populated with the observations

Returns: True if all the observation buffers are current, false otherwise

Definition at line 647 of file obstacle_layer.cpp.

References marking_buffers_.

Referenced by updateBounds(), and nav2_costmap_2d::VoxelLayer::updateBounds().

Here is the caller graph for this function:

◆ laserScanCallback()

void nav2_costmap_2d::ObstacleLayer::laserScanCallback	(	sensor_msgs::msg::LaserScan::ConstSharedPtr	message,
		const std::shared_ptr< nav2_costmap_2d::ObservationBuffer > &	buffer
	)

A callback to handle buffering LaserScan messages.

Parameters

message	The message returned from a message notifier
buffer	A pointer to the observation buffer to update

Definition at line 389 of file obstacle_layer.cpp.

References global_frame_, and projector_.

Referenced by onInitialize().

Here is the caller graph for this function:

◆ laserScanValidInfCallback()

void nav2_costmap_2d::ObstacleLayer::laserScanValidInfCallback	(	sensor_msgs::msg::LaserScan::ConstSharedPtr	message,
		const std::shared_ptr< nav2_costmap_2d::ObservationBuffer > &	buffer
	)

A callback to handle buffering LaserScan messages which need filtering to turn Inf values into range_max.

Parameters

message	The message returned from a message notifier
buffer	A pointer to the observation buffer to update

Definition at line 423 of file obstacle_layer.cpp.

References global_frame_, and projector_.

Referenced by onInitialize().

Here is the caller graph for this function:

◆ pointCloud2Callback()

void nav2_costmap_2d::ObstacleLayer::pointCloud2Callback	(	sensor_msgs::msg::PointCloud2::ConstSharedPtr	message,
		const std::shared_ptr< nav2_costmap_2d::ObservationBuffer > &	buffer
	)

A callback to handle buffering PointCloud2 messages.

Parameters

message	The message returned from a message notifier
buffer	A pointer to the observation buffer to update

Definition at line 466 of file obstacle_layer.cpp.

Referenced by onInitialize().

Here is the caller graph for this function:

◆ raytraceFreespace()

void nav2_costmap_2d::ObstacleLayer::raytraceFreespace	(	const nav2_costmap_2d::Observation &	clearing_observation,
		double *	min_x,
		double *	min_y,
		double *	max_x,
		double *	max_y
	)

protectedvirtual

Clear freespace based on one observation.

Parameters

clearing_observation	The observation used to raytrace
min_x
min_y
max_x
max_y

Reimplemented in nav2_costmap_2d::VoxelLayer.

Definition at line 681 of file obstacle_layer.cpp.

References nav2_costmap_2d::Costmap2D::cellDistance(), nav2_costmap_2d::Costmap2D::getSizeInMetersX(), nav2_costmap_2d::Costmap2D::getSizeInMetersY(), nav2_costmap_2d::Costmap2D::raytraceLine(), nav2_costmap_2d::CostmapLayer::touch(), updateRaytraceBounds(), and nav2_costmap_2d::Costmap2D::worldToMap().

Referenced by updateBounds().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ updateBounds()

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

virtual

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

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.

Reimplemented in nav2_costmap_2d::VoxelLayer.

Definition at line 477 of file obstacle_layer.cpp.

References getClearingObservations(), nav2_costmap_2d::Costmap2D::getIndex(), getMarkingObservations(), nav2_costmap_2d::Costmap2D::getSizeInMetersX(), nav2_costmap_2d::Costmap2D::getSizeInMetersY(), max_obstacle_height_, min_obstacle_height_, raytraceFreespace(), nav2_costmap_2d::CostmapLayer::touch(), updateFootprint(), nav2_costmap_2d::Costmap2D::updateOrigin(), and nav2_costmap_2d::Costmap2D::worldToMap().

Here is the call graph for this function:

◆ updateCosts()

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

virtual

Update the costs in the master costmap in the window.

Parameters

master_grid	The master costmap grid to update
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 587 of file obstacle_layer.cpp.

References nav2_costmap_2d::Max, nav2_costmap_2d::MaxWithoutUnknownOverwrite, nav2_costmap_2d::Overwrite, and nav2_costmap_2d::Costmap2D::setConvexPolygonCost().

Here is the call graph for this function:

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

nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
nav2_costmap_2d/plugins/obstacle_layer.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Additional Inherited Members

Detailed Description

Member Function Documentation

◆ dynamicParametersCallback()

◆ getClearingObservations()

◆ getMarkingObservations()

◆ laserScanCallback()

◆ laserScanValidInfCallback()

◆ pointCloud2Callback()

◆ raytraceFreespace()

◆ updateBounds()

◆ updateCosts()