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_util::SimpleActionServer< DockRobot >

using	UndockingActionServer = nav2_util::SimpleActionServer< UndockRobot >

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 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	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_util::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< nav_2d_utils::OdomSubscriber >	odom_sub_

std::unique_ptr< DockingActionServer >	docking_action_server_

std::unique_ptr< UndockingActionServer >	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_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 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 484 of file docking_server.cpp.

References checkAndWarnIfCancelled(), checkAndWarnIfPreempted(), and publishDockingFeedback().

Here is the call graph for this function:

◆ checkAndWarnIfCancelled()

template<typename ActionT >

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

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

Here is the caller graph for this function:

◆ checkAndWarnIfPreempted()

template<typename ActionT >

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

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

Here is the caller 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 426 of file docking_server.cpp.

References checkAndWarnIfCancelled(), checkAndWarnIfPreempted(), and 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 827 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 416 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 618 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 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 185 of file docking_server.cpp.

Referenced by dockRobot(), and undockRobot().

Here is the caller 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 801 of file docking_server.cpp.

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

Here is the caller graph for this function:

◆ on_activate()

nav2_util::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 115 of file docking_server.cpp.

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

Here is the call graph for this function:

◆ on_cleanup()

nav2_util::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 161 of file docking_server.cpp.

Referenced by on_configure().

Here is the caller graph for this function:

◆ on_configure()

nav2_util::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 49 of file docking_server.cpp.

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

Here is the call graph for this function:

◆ on_deactivate()

nav2_util::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 140 of file docking_server.cpp.

References nav2_util::LifecycleNode::destroyBond().

Here is the call graph for this function:

◆ on_shutdown()

nav2_util::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 178 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 817 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 582 of file docking_server.cpp.

References checkAndWarnIfCancelled(), checkAndWarnIfPreempted(), 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 447 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 550 of file docking_server.cpp.

References checkAndWarnIfCancelled(), checkAndWarnIfPreempted(), and 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()