An action server which implements a dynamic following behavior. More...

#include <nav2_following/opennav_following/include/opennav_following/following_server.hpp>

Inheritance diagram for opennav_following::FollowingServer:

Collaboration diagram for opennav_following::FollowingServer:

[legend]

Public Types
using	FollowObject = nav2_msgs::action::FollowObject

using	FollowingActionServer = nav2_util::SimpleActionServer< FollowObject >

Public Member Functions
	FollowingServer (const rclcpp::NodeOptions &options=rclcpp::NodeOptions())
	A constructor for opennav_following::FollowingServer. More...

	~FollowingServer ()=default
	A destructor for opennav_following::FollowingServer.

void	publishFollowingFeedback (uint16_t state)
	Publish feedback from a following action. More...

virtual bool	approachObject (geometry_msgs::msg::PoseStamped &object_pose, const std::string &target_frame=std::string(""))
	Use control law and perception to approach the object. More...

virtual bool	rotateToObject (geometry_msgs::msg::PoseStamped &object_pose, const std::string &target_frame=std::string(""))
	Rotate the robot to find the object again. More...

template<typename ActionT >
void	getPreemptedGoalIfRequested (typename std::shared_ptr< const typename ActionT::Goal > goal, const std::unique_ptr< nav2_util::SimpleActionServer< ActionT >> &action_server)
	Gets a preempted goal if immediately requested. More...

template<typename ActionT >
bool	checkAndWarnIfCancelled (std::unique_ptr< nav2_util::SimpleActionServer< ActionT >> &action_server, const std::string &name)
	Checks and logs warning if action canceled. More...

template<typename ActionT >
bool	checkAndWarnIfPreempted (std::unique_ptr< nav2_util::SimpleActionServer< ActionT >> &action_server, const std::string &name)
	Checks and logs warning if action preempted. More...

nav2_util::CallbackReturn	on_configure (const rclcpp_lifecycle::State &state) override
	Configure member variables. More...

nav2_util::CallbackReturn	on_activate (const rclcpp_lifecycle::State &state) override
	Activate member variables. More...

nav2_util::CallbackReturn	on_deactivate (const rclcpp_lifecycle::State &state) override
	Deactivate member variables. More...

nav2_util::CallbackReturn	on_cleanup (const rclcpp_lifecycle::State &state) override
	Reset member variables. More...

nav2_util::CallbackReturn	on_shutdown (const rclcpp_lifecycle::State &state) override
	Called when in shutdown state. More...

void	publishZeroVelocity ()
	Publish zero velocity at terminal condition.

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.

virtual void	on_rcl_preshutdown ()
	Perform preshutdown activities before our Context is shutdown. Note that this is related to our Context's shutdown sequence, not the lifecycle node state machine.

void	createBond ()
	Create bond connection to lifecycle manager.

void	destroyBond ()
	Destroy bond connection to lifecycle manager.

Protected Member Functions
void	followObject ()
	Main action callback method to complete following request.

virtual bool	getRefinedPose (geometry_msgs::msg::PoseStamped &pose)
	Method to obtain the refined dynamic pose. More...

virtual bool	getFramePose (geometry_msgs::msg::PoseStamped &pose, const std::string &frame_id)
	Get the pose of a specific frame in the fixed frame. More...

virtual bool	getTrackingPose (geometry_msgs::msg::PoseStamped &pose, const std::string &frame_id)
	Get the tracking pose based on the current tracking mode. More...

geometry_msgs::msg::PoseStamped	getPoseAtDistance (const geometry_msgs::msg::PoseStamped &pose, double distance)
	Get the pose at a distance in front of the input pose. More...

bool	isGoalReached (const geometry_msgs::msg::PoseStamped &goal_pose)
	Check if the goal has been reached. 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
std::unique_ptr< opennav_following::ParameterHandler >	param_handler_

Parameters *	params_

rclcpp::Time	static_object_start_time_

bool	static_timer_initialized_

int	num_retries_

rclcpp::Time	iteration_start_time_

rclcpp::Time	action_start_time_

rclcpp::Subscription< geometry_msgs::msg::PoseStamped >::SharedPtr	dynamic_pose_sub_

rclcpp_lifecycle::LifecyclePublisher< geometry_msgs::msg::PoseStamped >::SharedPtr	filtered_dynamic_pose_pub_

geometry_msgs::msg::PoseStamped	detected_dynamic_pose_

std::unique_ptr< opennav_docking::PoseFilter >	filter_

std::unique_ptr< nav2_util::TwistPublisher >	vel_publisher_

std::unique_ptr< nav2_util::OdomSmoother >	odom_sub_

std::unique_ptr< FollowingActionServer >	following_action_server_

std::unique_ptr< opennav_docking::Controller >	controller_

std::shared_ptr< tf2_ros::Buffer >	tf2_buffer_

std::unique_ptr< tf2_ros::TransformListener >	tf2_listener_

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

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

double	bond_heartbeat_period

rclcpp::TimerBase::SharedPtr	autostart_timer_

Detailed Description

An action server which implements a dynamic following behavior.

Definition at line 46 of file following_server.hpp.

Constructor & Destructor Documentation

◆ FollowingServer()

opennav_following::FollowingServer::FollowingServer ( const rclcpp::NodeOptions & options = rclcpp::NodeOptions() )

explicit

A constructor for opennav_following::FollowingServer.

Parameters

options Additional options to control creation of the node.

Definition at line 31 of file following_server.cpp.

Member Function Documentation

◆ approachObject()

bool opennav_following::FollowingServer::approachObject	(	geometry_msgs::msg::PoseStamped &	object_pose,
		const std::string &	target_frame = `std::string("")`
	)

virtual

Use control law and perception to approach the object.

Parameters

object_pose	Initial object pose, will be refined by perception.
target_frame	The frame to be tracked instead of the pose.

Returns: True if successfully approached, False if cancelled. For any internal error, will throw.

Definition at line 344 of file following_server.cpp.

References getPoseAtDistance(), getTrackingPose(), isGoalReached(), and publishFollowingFeedback().

Referenced by followObject().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ checkAndWarnIfCancelled()

template<typename ActionT >

bool opennav_following::FollowingServer::checkAndWarnIfCancelled	(	std::unique_ptr< nav2_util::SimpleActionServer< ActionT >> &	action_server,
		const std::string &	name
	)

Checks and logs warning if action canceled.

Parameters

action_server	Action server to check for cancellation on
name	Name of action to put in warning message

Returns: True if action has been cancelled

Definition at line 162 of file following_server.cpp.

◆ checkAndWarnIfPreempted()

template<typename ActionT >

bool opennav_following::FollowingServer::checkAndWarnIfPreempted	(	std::unique_ptr< nav2_util::SimpleActionServer< ActionT >> &	action_server,
		const std::string &	name
	)

Checks and logs warning if action preempted.

Parameters

action_server	Action server to check for preemption on
name	Name of action to put in warning message

Returns: True if action has been preempted

Definition at line 174 of file following_server.cpp.

◆ getFramePose()

bool opennav_following::FollowingServer::getFramePose	(	geometry_msgs::msg::PoseStamped &	pose,
		const std::string &	frame_id
	)

protectedvirtual

Get the pose of a specific frame in the fixed frame.

Parameters

pose	The output pose.
frame_id	The frame to get the pose for.

Returns: true if successful, false otherwise

Definition at line 586 of file following_server.cpp.

Referenced by getTrackingPose().

Here is the caller graph for this function:

◆ getPoseAtDistance()

geometry_msgs::msg::PoseStamped opennav_following::FollowingServer::getPoseAtDistance	(	const geometry_msgs::msg::PoseStamped &	pose,
		double	distance
	)

protected

Get the pose at a distance in front of the input pose.

Parameters

pose	Input pose
distance	Distance to move (in meters)

Returns: Pose distance meters in front of the input pose

Definition at line 635 of file following_server.cpp.

Referenced by approachObject().

Here is the caller graph for this function:

◆ getPreemptedGoalIfRequested()

template<typename ActionT >

void opennav_following::FollowingServer::getPreemptedGoalIfRequested	(	typename std::shared_ptr< const typename ActionT::Goal >	goal,
		const std::unique_ptr< nav2_util::SimpleActionServer< ActionT >> &	action_server
	)

Gets a preempted goal if immediately requested.

Parameters

Goal	goal to check or replace if required with preemption
action_server	Action server to check for preemptions on

Returns: SUCCESS or FAILURE

Definition at line 152 of file following_server.cpp.

◆ getRefinedPose()

bool opennav_following::FollowingServer::getRefinedPose ( geometry_msgs::msg::PoseStamped & pose )

protectedvirtual

Method to obtain the refined dynamic pose.

Parameters

pose	The initial estimate of the dynamic pose which will be updated with the refined pose.

Returns: true if successful, false otherwise

Definition at line 518 of file following_server.cpp.

Referenced by getTrackingPose().

Here is the caller graph for this function:

◆ getTrackingPose()

bool opennav_following::FollowingServer::getTrackingPose	(	geometry_msgs::msg::PoseStamped &	pose,
		const std::string &	frame_id
	)

protectedvirtual

Get the tracking pose based on the current tracking mode.

Parameters

pose	The output pose.
frame_id	The frame to get the pose for.

Returns: true if successful, false otherwise.

Definition at line 617 of file following_server.cpp.

References getFramePose(), and getRefinedPose().

Referenced by approachObject(), and rotateToObject().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ isGoalReached()

bool opennav_following::FollowingServer::isGoalReached ( const geometry_msgs::msg::PoseStamped & goal_pose )

protected

Check if the goal has been reached.

Parameters

goal_pose The goal pose to check

Returns: true If the goal has been reached

Definition at line 657 of file following_server.cpp.

Referenced by approachObject().

Here is the caller graph for this function:

◆ on_activate()

nav2_util::CallbackReturn opennav_following::FollowingServer::on_activate ( const rclcpp_lifecycle::State & state )

override

Activate member variables.

Parameters

state Reference to LifeCycle node state

Returns: SUCCESS or FAILURE

Definition at line 97 of file following_server.cpp.

References nav2_util::LifecycleNode::createBond().

Here is the call graph for this function:

◆ on_cleanup()

nav2_util::CallbackReturn opennav_following::FollowingServer::on_cleanup ( const rclcpp_lifecycle::State & state )

override

Reset member variables.

Parameters

state Reference to LifeCycle node state

Returns: SUCCESS or FAILURE

Definition at line 132 of file following_server.cpp.

◆ on_configure()

nav2_util::CallbackReturn opennav_following::FollowingServer::on_configure ( const rclcpp_lifecycle::State & state )

override

Configure member variables.

Parameters

state Reference to LifeCycle node state

Returns: SUCCESS or FAILURE

Definition at line 38 of file following_server.cpp.

References followObject(), and nav2_util::LifecycleNode::shared_from_this().

Here is the call graph for this function:

◆ on_deactivate()

nav2_util::CallbackReturn opennav_following::FollowingServer::on_deactivate ( const rclcpp_lifecycle::State & state )

override

Deactivate member variables.

Parameters

state Reference to LifeCycle node state

Returns: SUCCESS or FAILURE

Definition at line 114 of file following_server.cpp.

References nav2_util::LifecycleNode::destroyBond().

Here is the call graph for this function:

◆ on_shutdown()

nav2_util::CallbackReturn opennav_following::FollowingServer::on_shutdown ( const rclcpp_lifecycle::State & state )

override

Called when in shutdown state.

Parameters

state Reference to LifeCycle node state

Returns: SUCCESS or FAILURE

Definition at line 145 of file following_server.cpp.

◆ publishFollowingFeedback()

void opennav_following::FollowingServer::publishFollowingFeedback ( uint16_t state )

Publish feedback from a following action.

Parameters

state Current state - should be one of those defined in message.

Definition at line 509 of file following_server.cpp.

Referenced by approachObject(), followObject(), and rotateToObject().

Here is the caller graph for this function:

◆ rotateToObject()

bool opennav_following::FollowingServer::rotateToObject	(	geometry_msgs::msg::PoseStamped &	object_pose,
		const std::string &	target_frame = `std::string("")`
	)

virtual

Rotate the robot to find the object again.

Parameters

object_pose	The last known object pose.
target_frame	The frame to be tracked instead of the pose.

Returns: True if successful.

Definition at line 399 of file following_server.cpp.

References getTrackingPose(), and publishFollowingFeedback().

Referenced by followObject().

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_following/opennav_following/include/opennav_following/following_server.hpp
nav2_following/opennav_following/src/following_server.cpp

Public Types

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ FollowingServer()

Member Function Documentation

◆ approachObject()

◆ checkAndWarnIfCancelled()

◆ checkAndWarnIfPreempted()

◆ getFramePose()

◆ getPoseAtDistance()

◆ getPreemptedGoalIfRequested()

◆ getRefinedPose()

◆ getTrackingPose()

◆ isGoalReached()

◆ on_activate()

◆ on_cleanup()

◆ on_configure()

◆ on_deactivate()

◆ on_shutdown()

◆ publishFollowingFeedback()

◆ rotateToObject()