Class DockingServer

Inheritance Relationships

Base Type

  • public nav2_util::LifecycleNode

Class Documentation

class DockingServer : public nav2_util::LifecycleNode

An action server which implements charger docking node for AMRs.

Public Types

using DockingActionServer = nav2_util::SimpleActionServer<DockRobot>
using UndockingActionServer = nav2_util::SimpleActionServer<UndockRobot>

Public Functions

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

A constructor for opennav_docking::DockingServer.

Parameters:

options – Additional options to control creation of the node.

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

Parameters:

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

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

Generate a dock from action goal.

Parameters:

goal – Action goal

Returns:

Raw dock pointer to manage;

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

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

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.

Returns:

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

bool waitForCharge(Dock *dock)

Wait for charging to begin.

Parameters:

dock – Dock instance, used to query isCharging().

Returns:

True if charging successfully started within alloted time.

bool resetApproach(const geometry_msgs::msg::PoseStamped &staging_pose)

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

Parameters:

staging_pose – The target pose that will reset for another approach.

Returns:

True if reset is successful.

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.

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.

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

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.

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.

Parameters:

  • Goal – goal to check or replace if required with preemption

  • action_server – Action server to check for preemptions on

Returns:

SUCCESS or FAILURE

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.

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

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.

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

nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State &state) override

Configure member variables.

Parameters:

state – Reference to LifeCycle node state

Returns:

SUCCESS or FAILURE

nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State &state) override

Activate member variables.

Parameters:

state – Reference to LifeCycle node state

Returns:

SUCCESS or FAILURE

nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &state) override

Deactivate member variables.

Parameters:

state – Reference to LifeCycle node state

Returns:

SUCCESS or FAILURE

nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &state) override

Reset member variables.

Parameters:

state – Reference to LifeCycle node state

Returns:

SUCCESS or FAILURE

nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State &state) override

Called when in shutdown state.

Parameters:

state – Reference to LifeCycle node state

Returns:

SUCCESS or FAILURE

void publishZeroVelocity()

Publish zero velocity at terminal condition.

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

Parameters:

event – ParameterEvent message

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 undock_linear_tolerance_
double undock_angular_tolerance_
int max_retries_
int num_retries_
std::string base_frame_
std::string fixed_frame_
bool dock_backwards_
double dock_prestaging_tolerance_
rclcpp::Time action_start_time_
rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr vel_publisher_
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_