A ROS wrapper for a 2D Costmap. Handles subscribing to topics that provide observations about obstacles in either the form of PointCloud or LaserScan messages. More...

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

Inheritance diagram for nav2_costmap_2d::Costmap2DROS:

[legend]

Collaboration diagram for nav2_costmap_2d::Costmap2DROS:

[legend]

Public Member Functions
	Costmap2DROS (const rclcpp::NodeOptions &options=rclcpp::NodeOptions())
	Constructor for the wrapper. More...

	Costmap2DROS (const std::string &name, const bool &use_sim_time=false)
	Constructor for the wrapper, the node will be placed in a namespace equal to the node's name. More...

	Costmap2DROS (const std::string &name, const std::string &parent_namespace, const std::string &local_namespace, const bool &use_sim_time)
	Constructor for the wrapper. More...

void	init ()
	Common initialization for constructors.

	~Costmap2DROS ()
	A destructor.

nav2_util::CallbackReturn	on_configure (const rclcpp_lifecycle::State &state) override
	Configure node.

nav2_util::CallbackReturn	on_activate (const rclcpp_lifecycle::State &state) override
	Activate node.

nav2_util::CallbackReturn	on_deactivate (const rclcpp_lifecycle::State &state) override
	Deactivate node.

nav2_util::CallbackReturn	on_cleanup (const rclcpp_lifecycle::State &state) override
	Cleanup node.

nav2_util::CallbackReturn	on_shutdown (const rclcpp_lifecycle::State &state) override
	shutdown node

void	on_rcl_preshutdown () override
	as a child-LifecycleNode : Costmap2DROS may be launched by another Lifecycle Node as a composed module If composed, its parents will handle the shutdown, which includes this module

void	start ()
	Subscribes to sensor topics if necessary and starts costmap updates, can be called to restart the costmap after calls to either stop() or pause()

void	stop ()
	Stops costmap updates and unsubscribes from sensor topics.

void	pause ()
	Stops the costmap from updating, but sensor data still comes in over the wire.

void	resume ()
	Resumes costmap updates.

void	updateMap ()
	Update the map with the layered costmap / plugins.

void	resetLayers ()
	Reset each individual layer.

bool	isCurrent ()
	Same as getLayeredCostmap()->isCurrent().

bool	getRobotPose (geometry_msgs::msg::PoseStamped &global_pose)
	Get the pose of the robot in the global frame of the costmap. More...

bool	transformPoseToGlobalFrame (const geometry_msgs::msg::PoseStamped &input_pose, geometry_msgs::msg::PoseStamped &transformed_pose)
	Transform the input_pose in the global frame of the costmap. More...

std::string	getName () const
	Returns costmap name.

double	getTransformTolerance () const
	Returns the delay in transform (tf) data that is tolerable in seconds.

Costmap2D *	getCostmap ()
	Return a pointer to the "master" costmap which receives updates from all the layers. More...

std::string	getGlobalFrameID ()
	Returns the global frame of the costmap. More...

std::string	getBaseFrameID ()
	Returns the local frame of the costmap. More...

LayeredCostmap *	getLayeredCostmap ()
	Get the layered costmap object used in the node.

geometry_msgs::msg::Polygon	getRobotFootprintPolygon ()
	Returns the current padded footprint as a geometry_msgs::msg::Polygon.

std::vector< geometry_msgs::msg::Point >	getRobotFootprint ()
	Return the current footprint of the robot as a vector of points. More...

std::vector< geometry_msgs::msg::Point >	getUnpaddedRobotFootprint ()
	Return the current unpadded footprint of the robot as a vector of points. More...

void	getOrientedFootprint (std::vector< geometry_msgs::msg::Point > &oriented_footprint)
	Build the oriented footprint of the robot at the robot's current pose. More...

void	setRobotFootprint (const std::vector< geometry_msgs::msg::Point > &points)
	Set the footprint of the robot to be the given set of points, padded by footprint_padding. More...

void	setRobotFootprintPolygon (const geometry_msgs::msg::Polygon::SharedPtr footprint)
	Set the footprint of the robot to be the given polygon, padded by footprint_padding. More...

std::shared_ptr< tf2_ros::Buffer >	getTfBuffer ()

bool	getUseRadius ()
	Get the costmap's use_radius_ parameter, corresponding to whether the footprint for the robot is a circle with radius robot_radius_ or an arbitrarily defined footprint in footprint_. More...

double	getRobotRadius ()
	Get the costmap's robot_radius_ parameter, corresponding to raidus of the robot footprint when it is defined as as circle (i.e. when use_radius_ == true). More...

void	getCostCallback (const std::shared_ptr< rmw_request_id_t >, const std::shared_ptr< nav2_msgs::srv::GetCost::Request > request, const std::shared_ptr< nav2_msgs::srv::GetCost::Response > response)
	Get the cost at a point in costmap. More...

Public Member Functions inherited from nav2_util::LifecycleNode
	LifecycleNode (const std::string &node_name, const std::string &ns="", const rclcpp::NodeOptions &options=rclcpp::NodeOptions())
	A lifecycle node constructor. More...

void	add_parameter (const std::string &name, const rclcpp::ParameterValue &default_value, const std::string &description="", const std::string &additional_constraints="", bool read_only=false)
	Declare a parameter that has no integer or floating point range constraints. More...

void	add_parameter (const std::string &name, const rclcpp::ParameterValue &default_value, const floating_point_range fp_range, const std::string &description="", const std::string &additional_constraints="", bool read_only=false)
	Declare a parameter that has a floating point range constraint. More...

void	add_parameter (const std::string &name, const rclcpp::ParameterValue &default_value, const integer_range int_range, const std::string &description="", const std::string &additional_constraints="", bool read_only=false)
	Declare a parameter that has an integer range constraint. More...

std::shared_ptr< nav2_util::LifecycleNode >	shared_from_this ()
	Get a shared pointer of this.

nav2_util::CallbackReturn	on_error (const rclcpp_lifecycle::State &)
	Abstracted on_error state transition callback, since unimplemented as of 2020 in the managed ROS2 node state machine. More...

void	autostart ()
	Automatically configure and active the node.

void	createBond ()
	Create bond connection to lifecycle manager.

void	destroyBond ()
	Destroy bond connection to lifecycle manager.

Protected Member Functions
void	mapUpdateLoop (double frequency)
	Function on timer for costmap update.

void	getParameters ()
	Get parameters for node.

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

Protected Member Functions inherited from nav2_util::LifecycleNode
void	printLifecycleNodeNotification ()
	Print notifications for lifecycle node.

void	register_rcl_preshutdown_callback ()

void	runCleanups ()

Protected Attributes
rclcpp_lifecycle::LifecyclePublisher< geometry_msgs::msg::PolygonStamped >::SharedPtr	footprint_pub_

std::unique_ptr< Costmap2DPublisher >	costmap_publisher_

std::vector< std::unique_ptr< Costmap2DPublisher > >	layer_publishers_

rclcpp::Subscription< geometry_msgs::msg::Polygon >::SharedPtr	footprint_sub_

rclcpp::Subscription< rcl_interfaces::msg::ParameterEvent >::SharedPtr	parameter_sub_

rclcpp::CallbackGroup::SharedPtr	callback_group_

rclcpp::executors::SingleThreadedExecutor::SharedPtr	executor_

std::unique_ptr< nav2_util::NodeThread >	executor_thread_

std::shared_ptr< tf2_ros::Buffer >	tf_buffer_

std::shared_ptr< tf2_ros::TransformListener >	tf_listener_

std::unique_ptr< LayeredCostmap >	layered_costmap_ {nullptr}

std::string	name_

std::string	parent_namespace_

bool	map_update_thread_shutdown_ {false}

std::atomic< bool >	stop_updates_ {false}

std::atomic< bool >	initialized_ {false}

std::atomic< bool >	stopped_ {true}

std::mutex	_dynamic_parameter_mutex

std::unique_ptr< std::thread >	map_update_thread_
	A thread for updating the map.

rclcpp::Time	last_publish_ {0, 0, RCL_ROS_TIME}

rclcpp::Duration	publish_cycle_ {1, 0}

pluginlib::ClassLoader< Layer >	plugin_loader_ {"nav2_costmap_2d", "nav2_costmap_2d::Layer"}

bool	always_send_full_costmap_ {false}

std::string	footprint_

float	footprint_padding_ {0}

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

int	map_height_meters_ {0}

double	map_publish_frequency_ {0}

double	map_update_frequency_ {0}

int	map_width_meters_ {0}

double	origin_x_ {0}

double	origin_y_ {0}

std::vector< std::string >	default_plugins_

std::vector< std::string >	default_types_

std::vector< std::string >	plugin_names_

std::vector< std::string >	plugin_types_

std::vector< std::string >	filter_names_

std::vector< std::string >	filter_types_

double	resolution_ {0}

std::string	robot_base_frame_
	The frame_id of the robot base.

double	robot_radius_

bool	rolling_window_ {false}
	Whether to use a rolling window version of the costmap.

bool	track_unknown_space_ {false}

double	transform_tolerance_ {0}
	The timeout before transform errors.

double	initial_transform_timeout_ {0}
	The timeout before activation of the node errors.

double	map_vis_z_ {0}
	The height of map, allows to avoid flickering at -0.008.

bool	is_lifecycle_follower_ {true}
	whether is a child-LifecycleNode or an independent node

bool	use_radius_ {false}

std::vector< geometry_msgs::msg::Point >	unpadded_footprint_

std::vector< geometry_msgs::msg::Point >	padded_footprint_

rclcpp::Service< nav2_msgs::srv::GetCost >::SharedPtr	get_cost_service_

std::unique_ptr< ClearCostmapService >	clear_costmap_service_

OnSetParametersCallbackHandle::SharedPtr	dyn_params_handler

Protected Attributes inherited from nav2_util::LifecycleNode
std::unique_ptr< rclcpp::PreShutdownCallbackHandle >	rcl_preshutdown_cb_handle_ {nullptr}

std::unique_ptr< bond::Bond >	bond_ {nullptr}

double	bond_heartbeat_period

rclcpp::TimerBase::SharedPtr	autostart_timer_

Detailed Description

A ROS wrapper for a 2D Costmap. Handles subscribing to topics that provide observations about obstacles in either the form of PointCloud or LaserScan messages.

Definition at line 75 of file costmap_2d_ros.hpp.

Constructor & Destructor Documentation

◆ Costmap2DROS() [1/3]

nav2_costmap_2d::Costmap2DROS::Costmap2DROS ( const rclcpp::NodeOptions & options = rclcpp::NodeOptions() )

explicit

Constructor for the wrapper.

Parameters

options Additional options to control creation of the node.

Definition at line 64 of file costmap_2d_ros.cpp.

References init(), and is_lifecycle_follower_.

Here is the call graph for this function:

◆ Costmap2DROS() [2/3]

nav2_costmap_2d::Costmap2DROS::Costmap2DROS	(	const std::string &	name,
		const bool &	use_sim_time = `false`
	)

explicit

Constructor for the wrapper, the node will be placed in a namespace equal to the node's name.

Parameters

name	Name of the costmap ROS node
use_sim_time	Whether to use simulation or real time

Definition at line 61 of file costmap_2d_ros.cpp.

◆ Costmap2DROS() [3/3]

nav2_costmap_2d::Costmap2DROS::Costmap2DROS	(	const std::string &	name,
		const std::string &	parent_namespace,
		const std::string &	local_namespace,
		const bool &	use_sim_time
	)

explicit

Constructor for the wrapper.

Parameters

name	Name of the costmap ROS node
parent_namespace	Absolute namespace of the node hosting the costmap node
local_namespace	Namespace to append to the parent namespace
use_sim_time	Whether to use simulation or real time

Definition at line 78 of file costmap_2d_ros.cpp.

Member Function Documentation

◆ dynamicParametersCallback()

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

protected

Callback executed when a paramter change is detected.

Parameters

parameters list of changed parameters

Definition at line 726 of file costmap_2d_ros.cpp.

References global_frame_, nav2_costmap_2d::makeFootprintFromRadius(), nav2_costmap_2d::makeFootprintFromString(), nav2_costmap_2d::padFootprint(), robot_base_frame_, setRobotFootprint(), transform_tolerance_, and updateMap().

Referenced by on_activate().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getBaseFrameID()

std::string nav2_costmap_2d::Costmap2DROS::getBaseFrameID ( )

inline

Returns the local frame of the costmap.

Returns: The local frame of the costmap

Definition at line 252 of file costmap_2d_ros.hpp.

References robot_base_frame_.

◆ getCostCallback()

void nav2_costmap_2d::Costmap2DROS::getCostCallback	(	const std::shared_ptr< rmw_request_id_t >	,
		const std::shared_ptr< nav2_msgs::srv::GetCost::Request >	request,
		const std::shared_ptr< nav2_msgs::srv::GetCost::Response >	response
	)

Get the cost at a point in costmap.

Parameters

request	x and y coordinates in map
response	cost of the point

Definition at line 833 of file costmap_2d_ros.cpp.

References nav2_costmap_2d::FootprintCollisionChecker< CostmapT >::footprintCostAtPose(), nav2_costmap_2d::Costmap2D::getCost(), and nav2_costmap_2d::Costmap2D::worldToMap().

Referenced by on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getCostmap()

Costmap2D* nav2_costmap_2d::Costmap2DROS::getCostmap ( )

inline

Return a pointer to the "master" costmap which receives updates from all the layers.

Same as calling getLayeredCostmap()->getCostmap().

Definition at line 234 of file costmap_2d_ros.hpp.

Referenced by nav2_costmap_2d::ClearCostmapService::ClearCostmapService(), and nav2_costmap_2d::ClearCostmapService::clearEntirely().

Here is the caller graph for this function:

◆ getGlobalFrameID()

std::string nav2_costmap_2d::Costmap2DROS::getGlobalFrameID ( )

inline

Returns the global frame of the costmap.

Returns: The global frame of the costmap

Definition at line 243 of file costmap_2d_ros.hpp.

References global_frame_.

Referenced by nav2_costmap_2d::ClearCostmapService::clearAroundPose().

Here is the caller graph for this function:

◆ getOrientedFootprint()

void nav2_costmap_2d::Costmap2DROS::getOrientedFootprint ( std::vector< geometry_msgs::msg::Point > & oriented_footprint )

Build the oriented footprint of the robot at the robot's current pose.

Parameters

oriented_footprint Will be filled with the points in the oriented footprint of the robot

Definition at line 495 of file costmap_2d_ros.cpp.

References getRobotPose(), and nav2_costmap_2d::transformFootprint().

Here is the call graph for this function:

◆ getRobotFootprint()

std::vector<geometry_msgs::msg::Point> nav2_costmap_2d::Costmap2DROS::getRobotFootprint ( )

inline

Return the current footprint of the robot as a vector of points.

This version of the footprint is padded by the footprint_padding_ distance, set in the rosparam "footprint_padding".

The footprint initially comes from the rosparam "footprint" but can be overwritten by dynamic reconfigure or by messages received on the "footprint" topic.

Definition at line 279 of file costmap_2d_ros.hpp.

◆ getRobotPose()

bool nav2_costmap_2d::Costmap2DROS::getRobotPose ( geometry_msgs::msg::PoseStamped & global_pose )

Get the pose of the robot in the global frame of the costmap.

Parameters

global_pose Will be set to the pose of the robot in the global frame of the costmap

Returns: True if the pose was set successfully, false otherwise

Definition at line 703 of file costmap_2d_ros.cpp.

References global_frame_, robot_base_frame_, and transform_tolerance_.

Referenced by getOrientedFootprint(), and updateMap().

Here is the caller graph for this function:

◆ getRobotRadius()

double nav2_costmap_2d::Costmap2DROS::getRobotRadius ( )

inline

Get the costmap's robot_radius_ parameter, corresponding to raidus of the robot footprint when it is defined as as circle (i.e. when use_radius_ == true).

Returns: robot_radius_

Definition at line 342 of file costmap_2d_ros.hpp.

◆ getUnpaddedRobotFootprint()

std::vector<geometry_msgs::msg::Point> nav2_costmap_2d::Costmap2DROS::getUnpaddedRobotFootprint ( )

inline

Return the current unpadded footprint of the robot as a vector of points.

This is the raw version of the footprint without padding.

The footprint initially comes from the rosparam "footprint" but can be overwritten by dynamic reconfigure or by messages received on the "footprint" topic.

Definition at line 291 of file costmap_2d_ros.hpp.

◆ getUseRadius()

bool nav2_costmap_2d::Costmap2DROS::getUseRadius ( )

inline

Get the costmap's use_radius_ parameter, corresponding to whether the footprint for the robot is a circle with radius robot_radius_ or an arbitrarily defined footprint in footprint_.

Returns: use_radius_

Definition at line 334 of file costmap_2d_ros.hpp.

◆ setRobotFootprint()

void nav2_costmap_2d::Costmap2DROS::setRobotFootprint ( const std::vector< geometry_msgs::msg::Point > & points )

Set the footprint of the robot to be the given set of points, padded by footprint_padding.

Should be a convex polygon, though this is not enforced.

First expands the given polygon by footprint_padding_ and then sets padded_footprint_ and calls layered_costmap_->setFootprint(). Also saves the unpadded footprint, which is available from getUnpaddedRobotFootprint().

Definition at line 479 of file costmap_2d_ros.cpp.

References nav2_costmap_2d::padFootprint().

Referenced by dynamicParametersCallback(), on_configure(), and setRobotFootprintPolygon().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ setRobotFootprintPolygon()

void nav2_costmap_2d::Costmap2DROS::setRobotFootprintPolygon ( const geometry_msgs::msg::Polygon::SharedPtr footprint )

Set the footprint of the robot to be the given polygon, padded by footprint_padding.

Should be a convex polygon, though this is not enforced.

First expands the given polygon by footprint_padding_ and then sets padded_footprint_ and calls layered_costmap_->setFootprint(). Also saves the unpadded footprint, which is available from getUnpaddedRobotFootprint().

Definition at line 488 of file costmap_2d_ros.cpp.

References setRobotFootprint(), and nav2_costmap_2d::toPointVector().

Referenced by on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ transformPoseToGlobalFrame()

bool nav2_costmap_2d::Costmap2DROS::transformPoseToGlobalFrame	(	const geometry_msgs::msg::PoseStamped &	input_pose,
		geometry_msgs::msg::PoseStamped &	transformed_pose
	)

Transform the input_pose in the global frame of the costmap.

Parameters

input_pose	pose to be transformed
transformed_pose	pose transformed

Returns: True if the pose was transformed successfully, false otherwise

Definition at line 711 of file costmap_2d_ros.cpp.

References global_frame_, and transform_tolerance_.

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

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp
nav2_costmap_2d/src/costmap_2d_ros.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ Costmap2DROS() [1/3]

◆ Costmap2DROS() [2/3]

◆ Costmap2DROS() [3/3]

Member Function Documentation

◆ dynamicParametersCallback()

◆ getBaseFrameID()

◆ getCostCallback()

◆ getCostmap()

◆ getGlobalFrameID()

◆ getOrientedFootprint()

◆ getRobotFootprint()

◆ getRobotPose()

◆ getRobotRadius()

◆ getUnpaddedRobotFootprint()

◆ getUseRadius()

◆ setRobotFootprint()

◆ setRobotFootprintPolygon()

◆ transformPoseToGlobalFrame()