Reads in a speed restriction mask and enables a robot to dynamically adjust speed based on pose in map to slow in dangerous areas. Done via absolute speed setting or percentage of maximum speed. More...

#include <nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp>

Inheritance diagram for nav2_costmap_2d::SpeedFilter:

[legend]

Collaboration diagram for nav2_costmap_2d::SpeedFilter:

[legend]

Public Member Functions
	SpeedFilter ()
	A constructor.

void	initializeFilter (const std::string &filter_info_topic) override
	Initialize the filter and subscribe to the info topic.

void	process (nav2_costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j, const geometry_msgs::msg::Pose &pose) override
	Process the keepout layer at the current pose / bounds / grid.

void	resetFilter () override
	Reset the costmap filter / topic / info.

bool	isActive ()
	If this filter is active.

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

	~CostmapFilter ()
	A destructor.

mutex_t *	getMutex ()
	: returns pointer to a mutex

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

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) final
	Update the costs in the master costmap in the window. More...

void	activate () final
	Activate the layer.

void	deactivate () final
	Deactivate the layer.

void	reset () final
	Reset the layer.

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

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.

virtual void	matchSize ()
	Implement this to make this layer match the size of the parent costmap.

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

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	filterInfoCallback (const nav2_msgs::msg::CostmapFilterInfo::ConstSharedPtr &msg)
	Callback for the filter information.

void	maskCallback (const nav_msgs::msg::OccupancyGrid::ConstSharedPtr &msg)
	Callback for the filter mask.

void	pathCallback (const nav_msgs::msg::Path::ConstSharedPtr &msg)
	Callback for the planned path used by path lookahead.

bool	getSpeedLimitAtPose (const geometry_msgs::msg::Pose &pose, double &speed_limit)
	Look up the speed limit at a single pose in the costmap's global frame. More...

bool	getSpeedLimitFromLookahead (const geometry_msgs::msg::Pose &robot_pose, double lookahead_dist, double &speed_limit)
	Get the speed limit from the path lookahead. More...

Protected Member Functions inherited from nav2_costmap_2d::CostmapFilter
void	enableCallback (const std::shared_ptr< rmw_request_id_t > request_header, const std::shared_ptr< std_srvs::srv::SetBool::Request > request, std::shared_ptr< std_srvs::srv::SetBool::Response > response)
	Costmap filter enabling/disabling callback. More...

bool	transformPose (const std::string global_frame, const geometry_msgs::msg::Pose &global_pose, const std::string mask_frame, geometry_msgs::msg::Pose &mask_pose) const
	: Transforms robot pose from current layer frame to mask frame More...

int8_t	getMaskData (nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask, const unsigned int mx, const unsigned int my) const
	Get the data of a cell in the filter mask. More...

unsigned char	getMaskCost (nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask, const unsigned int mx, const unsigned int &my) const
	Get the cost of a cell in the filter mask. More...

Protected Attributes
nav2::Subscription< nav2_msgs::msg::CostmapFilterInfo >::SharedPtr	filter_info_sub_

nav2::Subscription< nav_msgs::msg::OccupancyGrid >::SharedPtr	mask_sub_

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

nav2::Publisher< nav2_msgs::msg::SpeedLimit >::SharedPtr	speed_limit_pub_

nav2::Publisher< geometry_msgs::msg::PointStamped >::SharedPtr	lookahead_pub_

nav_msgs::msg::OccupancyGrid::ConstSharedPtr	filter_mask_

nav_msgs::msg::Path::ConstSharedPtr	current_path_

std::shared_ptr< nav2_util::OdomSmoother >	odom_smoother_

std::string	global_frame_

double	base_

double	multiplier_

bool	percentage_

double	speed_limit_

double	speed_limit_prev_

double	held_lookahead_dist_

size_t	lookahead_start_idx_

bool	enable_path_lookahead_

double	max_decel_

double	min_lookahead_

double	max_lookahead_

Protected Attributes inherited from nav2_costmap_2d::CostmapFilter
std::string	filter_info_topic_
	: Name of costmap filter info topic

std::string	mask_topic_
	: Name of filter mask topic

tf2::Duration	transform_tolerance_
	: mask_frame->global_frame_ transform tolerance

nav2::ServiceServer< std_srvs::srv::SetBool >::SharedPtr	enable_service_
	: A service to enable/disable costmap filter

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::CostmapFilter
typedef std::recursive_mutex	mutex_t
	: Provide a typedef to ease future code maintenance

Detailed Description

Reads in a speed restriction mask and enables a robot to dynamically adjust speed based on pose in map to slow in dangerous areas. Done via absolute speed setting or percentage of maximum speed.

Definition at line 62 of file speed_filter.hpp.

Member Function Documentation

◆ getSpeedLimitAtPose()

bool nav2_costmap_2d::SpeedFilter::getSpeedLimitAtPose	(	const geometry_msgs::msg::Pose &	pose,
		double &	speed_limit
	)

protected

Look up the speed limit at a single pose in the costmap's global frame.

Parameters

pose	Pose in global_frame_
speed_limit	output: computed speed limit

Returns: true if pose mapped to a valid mask cell, false otherwise

Definition at line 251 of file speed_filter.cpp.

References nav2_costmap_2d::CostmapFilter::getMaskData(), and nav2_costmap_2d::CostmapFilter::transformPose().

Referenced by getSpeedLimitFromLookahead(), and process().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getSpeedLimitFromLookahead()

bool nav2_costmap_2d::SpeedFilter::getSpeedLimitFromLookahead	(	const geometry_msgs::msg::Pose &	robot_pose,
		double	lookahead_dist,
		double &	speed_limit
	)

protected

Get the speed limit from the path lookahead.

Parameters

robot_pose	robot pose
lookahead_dist	lookahead distance
speed_limit	output: strictest speed limit found along the lookahead

Returns: true if the lookahead could be evaluated, false otherwise

Definition at line 313 of file speed_filter.cpp.

References getSpeedLimitAtPose().

Referenced by process().

Here is the call graph for this function:

Here is the caller graph for this function:

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

nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp
nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Additional Inherited Members

Detailed Description

Member Function Documentation

◆ getSpeedLimitAtPose()

◆ getSpeedLimitFromLookahead()