Nav2 Navigation Stack - humble  humble
ROS 2 Navigation Stack
Public Member Functions | Protected Member Functions | Static Protected Member Functions | Protected Attributes | List of all members
nav2_controller::SimpleProgressChecker Class Reference

This plugin is used to check the position of the robot to make sure that it is actually progressing towards a goal. More...

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

Inheritance diagram for nav2_controller::SimpleProgressChecker:
Inheritance graph
[legend]
Collaboration diagram for nav2_controller::SimpleProgressChecker:
Collaboration graph
[legend]

Public Member Functions

void initialize (const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent, const std::string &plugin_name) override
 Initialize parameters for ProgressChecker. More...
 
bool check (geometry_msgs::msg::PoseStamped &current_pose) override
 Checks if the robot has moved compare to previous pose. More...
 
void reset () override
 Reset class state upon calling.
 

Protected Member Functions

bool isRobotMovedEnough (const geometry_msgs::msg::Pose2D &pose)
 Calculates robots movement from baseline pose. More...
 
void resetBaselinePose (const geometry_msgs::msg::Pose2D &pose)
 Resets baseline pose with the current pose of the robot. More...
 
rcl_interfaces::msg::SetParametersResult dynamicParametersCallback (std::vector< rclcpp::Parameter > parameters)
 Callback executed when a paramter change is detected. More...
 

Static Protected Member Functions

static double pose_distance (const geometry_msgs::msg::Pose2D &, const geometry_msgs::msg::Pose2D &)
 

Protected Attributes

rclcpp::Clock::SharedPtr clock_
 
double radius_
 
rclcpp::Duration time_allowance_ {0, 0}
 
geometry_msgs::msg::Pose2D baseline_pose_
 
rclcpp::Time baseline_time_
 
bool baseline_pose_set_ {false}
 
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_
 
std::string plugin_name_
 

Additional Inherited Members

- Public Types inherited from nav2_core::ProgressChecker
typedef std::shared_ptr< nav2_core::ProgressCheckerPtr
 

Detailed Description

This plugin is used to check the position of the robot to make sure that it is actually progressing towards a goal.

Definition at line 34 of file simple_progress_checker.hpp.

Member Function Documentation

◆ check()

bool nav2_controller::SimpleProgressChecker::check ( geometry_msgs::msg::PoseStamped &  current_pose)
overridevirtual

Checks if the robot has moved compare to previous pose.

Parameters
current_poseCurrent pose of the robot
Returns
True if progress is made

Implements nav2_core::ProgressChecker.

Definition at line 56 of file simple_progress_checker.cpp.

References isRobotMovedEnough(), and resetBaselinePose().

Here is the call graph for this function:

◆ dynamicParametersCallback()

rcl_interfaces::msg::SetParametersResult nav2_controller::SimpleProgressChecker::dynamicParametersCallback ( std::vector< rclcpp::Parameter >  parameters)
protected

Callback executed when a paramter change is detected.

Parameters
parameterslist of changed parameters

Definition at line 98 of file simple_progress_checker.cpp.

Referenced by initialize().

Here is the caller graph for this function:

◆ initialize()

void nav2_controller::SimpleProgressChecker::initialize ( const rclcpp_lifecycle::LifecycleNode::WeakPtr &  parent,
const std::string &  plugin_name 
)
overridevirtual

Initialize parameters for ProgressChecker.

Parameters
parentNode pointer

Implements nav2_core::ProgressChecker.

Definition at line 32 of file simple_progress_checker.cpp.

References dynamicParametersCallback().

Referenced by nav2_controller::PoseProgressChecker::initialize().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ isRobotMovedEnough()

bool nav2_controller::SimpleProgressChecker::isRobotMovedEnough ( const geometry_msgs::msg::Pose2D &  pose)
protected

Calculates robots movement from baseline pose.

Parameters
poseCurrent pose of the robot
Returns
true, if movement is greater than radius_, or false

Definition at line 82 of file simple_progress_checker.cpp.

Referenced by check().

Here is the caller graph for this function:

◆ resetBaselinePose()

void nav2_controller::SimpleProgressChecker::resetBaselinePose ( const geometry_msgs::msg::Pose2D &  pose)
protected

Resets baseline pose with the current pose of the robot.

Parameters
poseCurrent pose of the robot

Definition at line 75 of file simple_progress_checker.cpp.

Referenced by nav2_controller::PoseProgressChecker::check(), and check().

Here is the caller graph for this function:

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