An action server which implements charger docking node for AMRs. More...

#include <nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp>

Inheritance diagram for opennav_docking::DockingServer:

Collaboration diagram for opennav_docking::DockingServer:

[legend]

Public Types
using	DockingActionServer = nav2::SimpleActionServer< DockRobot >

using	UndockingActionServer = nav2::SimpleActionServer< UndockRobot >

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
	DockingServer (const rclcpp::NodeOptions &options=rclcpp::NodeOptions())
	A constructor for opennav_docking::DockingServer. More...

	~DockingServer ()=default
	A destructor for opennav_docking::DockingServer.

void	stashDockData (bool use_dock_id, Dock *dock, bool successful)
	Called at the conclusion of docking actions. Saves relevant docking data for later undocking action.

void	publishDockingFeedback (uint16_t state)
	Publish feedback from a docking action. More...

Dock *	generateGoalDock (std::shared_ptr< const DockRobot::Goal > goal)
	Generate a dock from action goal. More...

void	doInitialPerception (Dock *dock, geometry_msgs::msg::PoseStamped &dock_pose)
	Do initial perception, up to a timeout. More...

bool	approachDock (Dock *dock, geometry_msgs::msg::PoseStamped &dock_pose, bool backward)
	Use control law and dock perception to approach the charge dock. More...

void	rotateToDock (const geometry_msgs::msg::PoseStamped &dock_pose)
	Perform a pure rotation to dock orientation. More...

bool	waitForCharge (Dock *dock)
	Wait for charging to begin. More...

bool	resetApproach (const geometry_msgs::msg::PoseStamped &staging_pose, bool backward)
	Reset the robot for another approach by controlling back to staging pose. More...

bool	getCommandToPose (geometry_msgs::msg::Twist &cmd, const geometry_msgs::msg::PoseStamped &pose, double linear_tolerance, double angular_tolerance, bool is_docking, bool backward)
	Run a single iteration of the control loop to approach a pose. More...

virtual geometry_msgs::msg::PoseStamped	getRobotPoseInFrame (const std::string &frame)
	Get the robot pose (aka base_frame pose) in another frame. 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 ParameterT >
ParameterT	declare_or_get_parameter (const std::string &parameter_name, const ParameterDescriptor &parameter_descriptor=ParameterDescriptor())
	Declares or gets a parameter with specified type (not value). If the parameter is already declared, returns its value; otherwise declares it with the specified type. 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	dockRobot ()
	Main action callback method to complete docking request.

void	undockRobot ()
	Main action callback method to complete undocking request.

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::shared_ptr< std::mutex >	mutex_

double	controller_frequency_

double	initial_perception_timeout_

double	wait_charge_timeout_

double	dock_approach_timeout_

double	rotate_to_dock_timeout_

double	undock_linear_tolerance_

double	undock_angular_tolerance_

int	max_retries_

int	num_retries_

std::string	base_frame_

std::string	fixed_frame_

std::optional< bool >	dock_backwards_

double	dock_prestaging_tolerance_

double	rotation_angular_tolerance_

rclcpp::Time	action_start_time_

std::unique_ptr< nav2_util::TwistPublisher >	vel_publisher_

std::unique_ptr< nav2_util::OdomSmoother >	odom_sub_

DockingActionServer::SharedPtr	docking_action_server_

UndockingActionServer::SharedPtr	undocking_action_server_

std::unique_ptr< DockDatabase >	dock_db_

std::unique_ptr< Navigator >	navigator_

std::unique_ptr< Controller >	controller_

std::string	curr_dock_type_

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 charger docking node for AMRs.

Definition at line 45 of file docking_server.hpp.

Constructor & Destructor Documentation

◆ DockingServer()

opennav_docking::DockingServer::DockingServer ( const rclcpp::NodeOptions & options = rclcpp::NodeOptions() )

explicit

A constructor for opennav_docking::DockingServer.

Parameters

options Additional options to control creation of the node.

Definition at line 27 of file docking_server.cpp.

Member Function Documentation

◆ approachDock()

bool opennav_docking::DockingServer::approachDock	(	Dock *	dock,
		geometry_msgs::msg::PoseStamped &	dock_pose,
		bool	backward
	)

Use control law and dock perception to approach the charge dock.

Parameters

dock	Dock instance, gets queried for refined pose and docked state.
dock_pose	Initial dock pose, will be refined by perception.
backward	If true, the robot will drive backwards.

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

Definition at line 498 of file docking_server.cpp.

References publishDockingFeedback().

Here is the call graph for this function:

◆ checkAndWarnIfCancelled()

template<typename ActionT >

bool opennav_docking::DockingServer::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 docking_server.cpp.

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

Here is the call graph for this function:

◆ checkAndWarnIfPreempted()

template<typename ActionT >

bool opennav_docking::DockingServer::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 docking_server.cpp.

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

Here is the call graph for this function:

◆ doInitialPerception()

void opennav_docking::DockingServer::doInitialPerception	(	Dock *	dock,
		geometry_msgs::msg::PoseStamped &	dock_pose
	)

Do initial perception, up to a timeout.

Parameters

dock	Dock instance, gets queried for refined pose.
dock_pose	Initial dock pose, will be refined by perception.

Definition at line 434 of file docking_server.cpp.

References publishDockingFeedback().

Referenced by dockRobot().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ dynamicParametersCallback()

rcl_interfaces::msg::SetParametersResult opennav_docking::DockingServer::dynamicParametersCallback ( std::vector< rclcpp::Parameter > parameters )

protected

Callback executed when a parameter change is detected.

Parameters

event ParameterEvent message

Definition at line 841 of file docking_server.cpp.

Referenced by on_activate().

Here is the caller graph for this function:

◆ generateGoalDock()

Dock * opennav_docking::DockingServer::generateGoalDock ( std::shared_ptr< const DockRobot::Goal > goal )

Generate a dock from action goal.

Parameters

goal	Action goal

Returns: Raw dock pointer to manage;

Definition at line 424 of file docking_server.cpp.

Referenced by dockRobot().

Here is the caller graph for this function:

◆ getCommandToPose()

bool opennav_docking::DockingServer::getCommandToPose	(	geometry_msgs::msg::Twist &	cmd,
		const geometry_msgs::msg::PoseStamped &	pose,
		double	linear_tolerance,
		double	angular_tolerance,
		bool	is_docking,
		bool	backward
	)

Run a single iteration of the control loop to approach a pose.

Parameters

cmd	The return command.
pose	The pose to command towards.
linear_tolerance	Pose is reached when linear distance is within this tolerance.
angular_tolerance	Pose is reached when angular distance is within this tolerance.
is_docking	If true, the robot is docking. If false, the robot is undocking.
backward	If true, the robot will drive backwards.

Returns: True if pose is reached.

Definition at line 632 of file docking_server.cpp.

References getRobotPoseInFrame().

Referenced by resetApproach().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getPreemptedGoalIfRequested()

template<typename ActionT >

void opennav_docking::DockingServer::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 docking_server.cpp.

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

Here is the call graph for this function:

◆ getRobotPoseInFrame()

geometry_msgs::msg::PoseStamped opennav_docking::DockingServer::getRobotPoseInFrame ( const std::string & frame )

virtual

Get the robot pose (aka base_frame pose) in another frame.

Parameters

frame The frame_id to get the robot pose in.

Returns: Computed robot pose, throws TF2 error if failure.

Definition at line 815 of file docking_server.cpp.

Referenced by dockRobot(), getCommandToPose(), and rotateToDock().

Here is the caller graph for this function:

◆ on_activate()

nav2::CallbackReturn opennav_docking::DockingServer::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 118 of file docking_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_docking::DockingServer::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 164 of file docking_server.cpp.

Referenced by on_configure().

Here is the caller graph for this function:

◆ on_configure()

nav2::CallbackReturn opennav_docking::DockingServer::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 50 of file docking_server.cpp.

References dockRobot(), on_cleanup(), nav2::LifecycleNode::shared_from_this(), and undockRobot().

Here is the call graph for this function:

◆ on_deactivate()

nav2::CallbackReturn opennav_docking::DockingServer::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 143 of file docking_server.cpp.

References nav2::LifecycleNode::destroyBond().

Here is the call graph for this function:

◆ on_shutdown()

nav2::CallbackReturn opennav_docking::DockingServer::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 docking_server.cpp.

◆ publishDockingFeedback()

void opennav_docking::DockingServer::publishDockingFeedback ( uint16_t state )

Publish feedback from a docking action.

Parameters

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

Definition at line 831 of file docking_server.cpp.

Referenced by approachDock(), dockRobot(), doInitialPerception(), resetApproach(), and waitForCharge().

Here is the caller graph for this function:

◆ resetApproach()

bool opennav_docking::DockingServer::resetApproach	(	const geometry_msgs::msg::PoseStamped &	staging_pose,
		bool	backward
	)

Reset the robot for another approach by controlling back to staging pose.

Parameters

staging_pose	The target pose that will reset for another approach.
backward	If true, the robot will drive backwards.

Returns: True if reset is successful.

Definition at line 596 of file docking_server.cpp.

References getCommandToPose(), and publishDockingFeedback().

Here is the call graph for this function:

◆ rotateToDock()

void opennav_docking::DockingServer::rotateToDock ( const geometry_msgs::msg::PoseStamped & dock_pose )

Perform a pure rotation to dock orientation.

Parameters

dock_pose The target pose that will be used to rotate.

Definition at line 461 of file docking_server.cpp.

References getRobotPoseInFrame().

Here is the call graph for this function:

◆ waitForCharge()

bool opennav_docking::DockingServer::waitForCharge ( Dock * dock )

Wait for charging to begin.

Parameters

dock	Dock instance, used to query isCharging().

Returns: True if charging successfully started within allotted time.

Definition at line 564 of file docking_server.cpp.

References publishDockingFeedback().

Here is the call graph for this function:

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

nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp
nav2_docking/opennav_docking/src/docking_server.cpp

Public Types

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ DockingServer()

Member Function Documentation

◆ approachDock()

◆ checkAndWarnIfCancelled()

◆ checkAndWarnIfPreempted()

◆ doInitialPerception()

◆ dynamicParametersCallback()

◆ generateGoalDock()

◆ getCommandToPose()

◆ getPreemptedGoalIfRequested()

◆ getRobotPoseInFrame()

◆ on_activate()

◆ on_cleanup()

◆ on_configure()

◆ on_deactivate()

◆ on_shutdown()

◆ publishDockingFeedback()

◆ resetApproach()

◆ rotateToDock()

◆ waitForCharge()