Goal Checker plugin with two tolerance tiers: a tight desired tolerance and a looser coarse tolerance. The robot is considered to have reached the goal if: (1) it reaches within the desired (tight) tolerance, OR (2) it is within the coarse tolerance AND the robot's velocity is below a stopped threshold for a configurable number of consecutive cycles, indicating it is no longer making useful progress toward the goal. More...

#include <nav2_controller/include/nav2_controller/plugins/adaptive_tolerance_goal_checker.hpp>

Inheritance diagram for nav2_controller::AdaptiveToleranceGoalChecker:

[legend]

Collaboration diagram for nav2_controller::AdaptiveToleranceGoalChecker:

[legend]

Public Member Functions
	AdaptiveToleranceGoalChecker ()
	Construct a new Progress Goal Checker object.

	~AdaptiveToleranceGoalChecker ()
	Destroy the Progress Goal Checker object.

void	initialize (const nav2::LifecycleNode::WeakPtr &parent, const std::string &plugin_name, const std::shared_ptr< nav2_costmap_2d::Costmap2DROS > costmap_ros) override
	Initialize the goal checker. More...

void	reset () override
	Reset the goal checker state.

bool	isGoalReached (const geometry_msgs::msg::Pose &query_pose, const geometry_msgs::msg::Pose &goal_pose, const geometry_msgs::msg::Twist &velocity, const nav_msgs::msg::Path &transformed_global_plan) override
	Check if the goal is reached. More...

bool	isGoalXYReached (const geometry_msgs::msg::Pose &query_pose, const geometry_msgs::msg::Pose &goal_pose, const geometry_msgs::msg::Twist &velocity, const nav_msgs::msg::Path &transformed_global_plan) override
	Check if XY goal position has been reached (without considering yaw) More...

bool	getTolerances (geometry_msgs::msg::Pose &pose_tolerance, geometry_msgs::msg::Twist &vel_tolerance, double &path_length_tolerance) override
	Get the position and velocity tolerances. More...

Protected Types
enum class	XyAcceptanceReason { NONE , FINE_TOLERANCE , COARSE_TOLERANCE_FINISH_LINE , COARSE_TOLERANCE_STOPPED_STAGNATION , COARSE_TOLERANCE_DISTANCE_STAGNATION }
	Reason that the XY component of the goal was accepted.

Protected Member Functions
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...

Static Protected Member Functions
static std::string	toString (XyAcceptanceReason reason)
	Convert XY acceptance reason to string. More...

Protected Attributes
nav2::LifecycleNode::WeakPtr	node_

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

double	fine_xy_goal_tolerance_

double	fine_xy_goal_tolerance_sq_

double	coarse_xy_goal_tolerance_

double	coarse_xy_goal_tolerance_sq_

double	xy_goal_tolerance_buffer_

double	fine_xy_goal_tolerance_reset_sq_

double	coarse_xy_goal_tolerance_reset_sq_

double	yaw_goal_tolerance_

double	path_length_tolerance_

bool	stateful_

bool	symmetric_yaw_tolerance_

double	trans_stopped_velocity_

double	rot_stopped_velocity_

int	required_stagnation_cycles_

bool	check_xy_

bool	in_tolerance_zone_

int	stopped_stagnation_count_

int	distance_stagnation_count_

double	best_distance_sq_

double	approach_dx_

double	approach_dy_

XyAcceptanceReason	xy_acceptance_reason_

std::mutex	mutex_

rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr	post_set_params_handler_

rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr	on_set_params_handler_

std::string	plugin_name_

Additional Inherited Members
Public Types inherited from nav2_core::GoalChecker
typedef std::shared_ptr< nav2_core::GoalChecker >	Ptr

Detailed Description

Goal Checker plugin with two tolerance tiers: a tight desired tolerance and a looser coarse tolerance. The robot is considered to have reached the goal if: (1) it reaches within the desired (tight) tolerance, OR (2) it is within the coarse tolerance AND the robot's velocity is below a stopped threshold for a configurable number of consecutive cycles, indicating it is no longer making useful progress toward the goal.

Definition at line 40 of file adaptive_tolerance_goal_checker.hpp.

Member Function Documentation

◆ getTolerances()

bool nav2_controller::AdaptiveToleranceGoalChecker::getTolerances	(	geometry_msgs::msg::Pose &	pose_tolerance,
		geometry_msgs::msg::Twist &	vel_tolerance,
		double &	path_length_tolerance
	)

overridevirtual

Get the position and velocity tolerances.

Parameters

pose_tolerance	Output parameter for pose tolerance
vel_tolerance	Output parameter for velocity tolerance
path_length_tolerance	Output parameter for path length tolerance

Returns: true if tolerances are available, false otherwise

Implements nav2_core::GoalChecker.

Definition at line 294 of file adaptive_tolerance_goal_checker.cpp.

◆ initialize()

void nav2_controller::AdaptiveToleranceGoalChecker::initialize	(	const nav2::LifecycleNode::WeakPtr &	parent,
		const std::string &	plugin_name,
		const std::shared_ptr< nav2_costmap_2d::Costmap2DROS >	costmap_ros
	)

overridevirtual

Initialize the goal checker.

Parameters

parent	Weak pointer to the lifecycle node
plugin_name	Name of the plugin
costmap_ros	Shared pointer to the costmap

Implements nav2_core::GoalChecker.

Definition at line 73 of file adaptive_tolerance_goal_checker.cpp.

References updateParametersCallback(), and validateParameterUpdatesCallback().

Here is the call graph for this function:

◆ isGoalReached()

bool nav2_controller::AdaptiveToleranceGoalChecker::isGoalReached	(	const geometry_msgs::msg::Pose &	query_pose,
		const geometry_msgs::msg::Pose &	goal_pose,
		const geometry_msgs::msg::Twist &	velocity,
		const nav_msgs::msg::Path &	transformed_global_plan
	)

overridevirtual

Check if the goal is reached.

Parameters

query_pose	Current pose of the robot
goal_pose	Target goal pose
velocity	Current velocity of the robot
transformed_global_plan	The transformed global plan

Returns: true if goal is reached, false otherwise

Implements nav2_core::GoalChecker.

Definition at line 141 of file adaptive_tolerance_goal_checker.cpp.

References isGoalXYReached(), and toString().

Here is the call graph for this function:

◆ isGoalXYReached()

bool nav2_controller::AdaptiveToleranceGoalChecker::isGoalXYReached	(	const geometry_msgs::msg::Pose &	query_pose,
		const geometry_msgs::msg::Pose &	goal_pose,
		const geometry_msgs::msg::Twist &	velocity,
		const nav_msgs::msg::Path &	transformed_global_plan
	)

overridevirtual

Check if XY goal position has been reached (without considering yaw)

Parameters

query_pose	The pose to check
goal_pose	The pose to check against
velocity	The robot's current velocity
transformed_global_plan	The global plan after being processed by the path handler

Returns: True if XY goal is reached (position within tolerance, yaw ignored)

Implements nav2_core::GoalChecker.

Definition at line 182 of file adaptive_tolerance_goal_checker.cpp.

Referenced by isGoalReached().

Here is the caller graph for this function:

◆ toString()

std::string nav2_controller::AdaptiveToleranceGoalChecker::toString ( XyAcceptanceReason reason )

staticprotected

Convert XY acceptance reason to string.

Parameters

reason The XY acceptance reason

Returns: String representation of the XY acceptance reason

Definition at line 322 of file adaptive_tolerance_goal_checker.cpp.

Referenced by isGoalReached().

Here is the caller graph for this function:

◆ updateParametersCallback()

void nav2_controller::AdaptiveToleranceGoalChecker::updateParametersCallback ( const std::vector< rclcpp::Parameter > & parameters )

protected

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.

Parameters

parameters List of parameters that have been updated.

Definition at line 375 of file adaptive_tolerance_goal_checker.cpp.

Referenced by initialize().

Here is the caller graph for this function:

◆ validateParameterUpdatesCallback()

rcl_interfaces::msg::SetParametersResult nav2_controller::AdaptiveToleranceGoalChecker::validateParameterUpdatesCallback ( const std::vector< rclcpp::Parameter > & parameters )

protected

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.

Parameters

parameters List of parameters that are being updated.

Returns: rcl_interfaces::msg::SetParametersResult Result indicating whether the update is accepted.

Definition at line 340 of file adaptive_tolerance_goal_checker.cpp.

Referenced by initialize().

Here is the caller graph for this function:

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

nav2_controller/include/nav2_controller/plugins/adaptive_tolerance_goal_checker.hpp
nav2_controller/plugins/adaptive_tolerance_goal_checker.cpp

Public Member Functions

Protected Types

Protected Member Functions

Static Protected Member Functions

Protected Attributes

Additional Inherited Members

Detailed Description

Member Function Documentation

◆ getTolerances()

◆ initialize()

◆ isGoalReached()

◆ isGoalXYReached()

◆ toString()

◆ updateParametersCallback()

◆ validateParameterUpdatesCallback()