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::SimpleActionServer< FollowObject >

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 >

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 typename nav2::SimpleActionServer< ActionT >::SharedPtr &action_server)
	Gets a preempted goal if immediately requested. More...

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

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

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

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

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

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

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

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

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
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr	dyn_params_handler_

std::mutex	dynamic_params_lock_

double	controller_frequency_

double	rotate_to_object_timeout_

double	static_object_timeout_

rclcpp::Time	static_object_start_time_

bool	static_timer_initialized_

double	transform_tolerance_

double	linear_tolerance_

double	angular_tolerance_

int	max_retries_

int	num_retries_

std::string	base_frame_

std::string	fixed_frame_

double	desired_distance_

bool	skip_orientation_

bool	search_by_rotating_

double	search_angle_

rclcpp::Time	iteration_start_time_

rclcpp::Time	action_start_time_

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

nav2::Publisher< geometry_msgs::msg::PoseStamped >::SharedPtr	filtered_dynamic_pose_pub_

geometry_msgs::msg::PoseStamped	detected_dynamic_pose_

std::unique_ptr< opennav_docking::PoseFilter >	filter_

double	detection_timeout_

std::unique_ptr< nav2_util::TwistPublisher >	vel_publisher_

std::unique_ptr< nav2_util::OdomSmoother >	odom_sub_

FollowingActionServer::SharedPtr	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::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_

Detailed Description

An action server which implements a dynamic following behavior.

Definition at line 45 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 379 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	(	typename nav2::SimpleActionServer< ActionT >::SharedPtr &	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 198 of file following_server.cpp.

References nav2::SimpleActionServer< ActionT >::is_cancel_requested().

Here is the call graph for this function:

◆ checkAndWarnIfPreempted()

template<typename ActionT >

bool opennav_following::FollowingServer::checkAndWarnIfPreempted	(	typename nav2::SimpleActionServer< ActionT >::SharedPtr &	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 210 of file following_server.cpp.

References nav2::SimpleActionServer< ActionT >::is_preempt_requested().

Here is the call graph for this function:

◆ dynamicParametersCallback()

rcl_interfaces::msg::SetParametersResult opennav_following::FollowingServer::dynamicParametersCallback ( std::vector< rclcpp::Parameter > parameters )

protected

Callback executed when a parameter change is detected.

Parameters

event ParameterEvent message

Definition at line 710 of file following_server.cpp.

Referenced by on_activate().

Here is the caller graph for this function:

◆ 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 623 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 670 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 typename nav2::SimpleActionServer< ActionT >::SharedPtr &	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 188 of file following_server.cpp.

References nav2::SimpleActionServer< ActionT >::accept_pending_goal(), and nav2::SimpleActionServer< ActionT >::is_preempt_requested().

Here is the call graph for this function:

◆ 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 554 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 652 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 691 of file following_server.cpp.

Referenced by approachObject().

Here is the caller graph for this function:

◆ on_activate()

nav2::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 127 of file following_server.cpp.

References nav2::LifecycleNode::createBond(), dynamicParametersCallback(), and nav2::LifecycleNode::shared_from_this().

Here is the call graph for this function:

◆ on_cleanup()

nav2::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 168 of file following_server.cpp.

◆ on_configure()

nav2::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 56 of file following_server.cpp.

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

Here is the call graph for this function:

◆ on_deactivate()

nav2::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 149 of file following_server.cpp.

References nav2::LifecycleNode::destroyBond().

Here is the call graph for this function:

◆ on_shutdown()

nav2::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 181 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 545 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 446 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()

◆ dynamicParametersCallback()

◆ getFramePose()

◆ getPoseAtDistance()

◆ getPreemptedGoalIfRequested()

◆ getRefinedPose()

◆ getTrackingPose()

◆ isGoalReached()

◆ on_activate()

◆ on_cleanup()

◆ on_configure()

◆ on_deactivate()

◆ on_shutdown()

◆ publishFollowingFeedback()

◆ rotateToObject()