Class FollowingServer

Inheritance Relationships

Base Type

  • public nav2_util::LifecycleNode

Class Documentation

class FollowingServer : public nav2_util::LifecycleNode

An action server which implements a dynamic following behavior.

Public Types

using FollowObject = opennav_following_msgs::action::FollowObject
using FollowingActionServer = nav2_util::SimpleActionServer<FollowObject>

Public Functions

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

A constructor for opennav_following::FollowingServer.

Parameters:

options – Additional options to control creation of the node.

~FollowingServer() = default

A destructor for opennav_following::FollowingServer.

void publishFollowingFeedback(uint16_t state)

Publish feedback from a following action.

Parameters:

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

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.

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.

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

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.

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 followObject()

Main action callback method to complete following request.

virtual bool getRefinedPose(geometry_msgs::msg::PoseStamped &pose)

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

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

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

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

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.

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.

Parameters:

  • pose – Input pose

  • distance – Distance to move (in meters)

Returns:

Pose distance meters in front of the input pose

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

Check if the goal has been reached.

Parameters:

goal_pose – The goal pose to check

Returns:

true If the goal has been reached

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::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_
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_
double detection_timeout_
rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr vel_publisher_
std::unique_ptr<nav_2d_utils::OdomSubscriber> 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_