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 std::string &parent_namespace="/", const bool &use_sim_time=false, const rclcpp::NodeOptions &options=rclcpp::NodeOptions())
	Constructor for the wrapper. More...

void	init ()
	Common initialization for constructors.

	~Costmap2DROS ()
	A destructor.

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

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

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

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

nav2::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 &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	getCostsCallback (const std::shared_ptr< rmw_request_id_t >, const std::shared_ptr< nav2_msgs::srv::GetCosts::Request > request, const std::shared_ptr< nav2_msgs::srv::GetCosts::Response > response)
	Get the cost at a point in costmap. More...

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

	LifecycleNode (const std::string &node_name, const rclcpp::NodeOptions &options=rclcpp::NodeOptions())
	A lifecycle node constructor with no namespace. More...

template<typename ParamType >
ParamType	declare_or_get_parameter (const std::string &parameter_name, const ParamType &default_value, const ParameterDescriptor &parameter_descriptor=ParameterDescriptor())
	Declares or gets a parameter. If the parameter is already declared, returns its value; otherwise declares it and returns the default value. More...

template<typename MessageT , typename CallbackT >
nav2::Subscription< MessageT >::SharedPtr	create_subscription (const std::string &topic_name, CallbackT &&callback, const rclcpp::QoS &qos=nav2::qos::StandardTopicQoS(), const rclcpp::CallbackGroup::SharedPtr &callback_group=nullptr)
	Create a subscription to a topic using Nav2 QoS profiles and SubscriptionOptions. More...

template<typename MessageT >
nav2::Publisher< MessageT >::SharedPtr	create_publisher (const std::string &topic_name, const rclcpp::QoS &qos=nav2::qos::StandardTopicQoS(), const rclcpp::CallbackGroup::SharedPtr &callback_group=nullptr)
	Create a publisher to a topic using Nav2 QoS profiles and PublisherOptions. More...

template<typename ServiceT >
nav2::ServiceClient< ServiceT >::SharedPtr	create_client (const std::string &service_name, bool use_internal_executor=false)
	Create a ServiceClient to interface with a service. More...

template<typename ServiceT >
nav2::ServiceServer< ServiceT >::SharedPtr	create_service (const std::string &service_name, typename nav2::ServiceServer< ServiceT >::CallbackType cb, rclcpp::CallbackGroup::SharedPtr callback_group=nullptr)
	Create a ServiceServer to host with a service. More...

template<typename ActionT >
nav2::SimpleActionServer< ActionT >::SharedPtr	create_action_server (const std::string &action_name, typename nav2::SimpleActionServer< ActionT >::ExecuteCallback execute_callback, typename nav2::SimpleActionServer< ActionT >::CompletionCallback compl_cb=nullptr, std::chrono::milliseconds server_timeout=std::chrono::milliseconds(500), bool spin_thread=false, const bool realtime=false)
	Create a SimpleActionServer to host with an action. More...

template<typename ActionT >
nav2::ActionClient< ActionT >::SharedPtr	create_action_client (const std::string &action_name, rclcpp::CallbackGroup::SharedPtr callback_group=nullptr)
	Create a ActionClient to call an action using. More...

nav2::LifecycleNode::SharedPtr	shared_from_this ()
	Get a shared pointer of this.

nav2::LifecycleNode::WeakPtr	weak_from_this ()
	Get a shared pointer of this.

nav2::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 parameter change is detected. More...

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

void	register_rcl_preshutdown_callback ()

void	runCleanups ()

Protected Attributes
nav2::Publisher< geometry_msgs::msg::PolygonStamped >::SharedPtr	footprint_pub_

std::unique_ptr< Costmap2DPublisher >	costmap_publisher_

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

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

nav2::Subscription< geometry_msgs::msg::PolygonStamped >::SharedPtr	footprint_stamped_sub_

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

rclcpp::CallbackGroup::SharedPtr	callback_group_

rclcpp::executors::SingleThreadedExecutor::SharedPtr	executor_

std::unique_ptr< nav2::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_

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}

bool	subscribe_to_stamped_footprint_ {false}
	If true, the footprint subscriber expects a PolygonStamped msg.

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_

nav2::ServiceServer< nav2_msgs::srv::GetCosts >::SharedPtr	get_cost_service_

std::unique_ptr< ClearCostmapService >	clear_costmap_service_

OnSetParametersCallbackHandle::SharedPtr	dyn_params_handler

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

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

double	bond_heartbeat_period {0.1}

rclcpp::TimerBase::SharedPtr	autostart_timer_

Additional Inherited Members
Public Types inherited from nav2::LifecycleNode
using	SharedPtr = std::shared_ptr< nav2::LifecycleNode >

using	WeakPtr = std::weak_ptr< nav2::LifecycleNode >

using	SharedConstPointer = std::shared_ptr< const nav2::LifecycleNode >

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 76 of file costmap_2d_ros.hpp.

Constructor & Destructor Documentation

◆ Costmap2DROS() [1/2]

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 61 of file costmap_2d_ros.cpp.

References init(), and is_lifecycle_follower_.

Here is the call graph for this function:

◆ Costmap2DROS() [2/2]

nav2_costmap_2d::Costmap2DROS::Costmap2DROS	(	const std::string &	name,
		const std::string &	parent_namespace = `"/"`,
		const bool &	use_sim_time = `false`,
		const rclcpp::NodeOptions &	options = `rclcpp::NodeOptions()`
	)

explicit

Constructor for the wrapper.

Parameters

name	Name of the costmap ROS node which will also be used as a local namespace
parent_namespace	Absolute namespace of the node hosting the costmap node
use_sim_time	Whether to use simulation or real time

Definition at line 89 of file costmap_2d_ros.cpp.

References init().

Here is the call graph for this function:

Member Function Documentation

◆ dynamicParametersCallback()

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

protected

Callback executed when a parameter change is detected.

Parameters

parameters list of changed parameters

Definition at line 731 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 244 of file costmap_2d_ros.hpp.

References robot_base_frame_.

◆ 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 226 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:

◆ getCostsCallback()

void nav2_costmap_2d::Costmap2DROS::getCostsCallback	(	const std::shared_ptr< rmw_request_id_t >	,
		const std::shared_ptr< nav2_msgs::srv::GetCosts::Request >	request,
		const std::shared_ptr< nav2_msgs::srv::GetCosts::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 842 of file costmap_2d_ros.cpp.

References nav2_costmap_2d::FootprintCollisionChecker< CostmapT >::footprintCostAtPose(), nav2_costmap_2d::Costmap2D::getCost(), transformPoseToGlobalFrame(), 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:

◆ 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 235 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 500 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 271 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 708 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 334 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 283 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 326 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 484 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 & 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 493 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 716 of file costmap_2d_ros.cpp.

References global_frame_, and transform_tolerance_.

Referenced by getCostsCallback().

Here is the caller graph for this function:

Member Data Documentation

◆ map_vis_z_

double nav2_costmap_2d::Costmap2DROS::map_vis_z_ {0}

protected

The height of map, allows to avoid flickering at -0.008

Definition at line 410 of file costmap_2d_ros.hpp.

Referenced by getParameters(), and on_configure().

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

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ Costmap2DROS() [1/2]

◆ Costmap2DROS() [2/2]

Member Function Documentation

◆ dynamicParametersCallback()

◆ getBaseFrameID()

◆ getCostmap()

◆ getCostsCallback()

◆ getGlobalFrameID()

◆ getOrientedFootprint()

◆ getRobotFootprint()

◆ getRobotPose()

◆ getRobotRadius()

◆ getUnpaddedRobotFootprint()

◆ getUseRadius()

◆ setRobotFootprint()

◆ setRobotFootprintPolygon()

◆ transformPoseToGlobalFrame()

Member Data Documentation

◆ map_vis_z_