Class WaypointFollower

Inheritance Relationships

Base Type

  • public nav2_util::LifecycleNode

Class Documentation

class WaypointFollower : public nav2_util::LifecycleNode

An action server that uses behavior tree for navigating a robot to its goal position.

Public Types

using ActionT = nav2_msgs::action::FollowWaypoints
using ClientT = nav2_msgs::action::NavigateToPose
using ActionServer = nav2_util::SimpleActionServer<ActionT>
using ActionClient = rclcpp_action::Client<ClientT>
using ActionTGPS = nav2_msgs::action::FollowGPSWaypoints
using ActionServerGPS = nav2_util::SimpleActionServer<ActionTGPS>

Public Functions

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

A constructor for nav2_waypoint_follower::WaypointFollower class.

Parameters:

options – Additional options to control creation of the node.

~WaypointFollower()

A destructor for nav2_waypoint_follower::WaypointFollower class.

Protected Functions

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

Configures member variables.

Initializes action server for “follow_waypoints”

Parameters:

state – Reference to LifeCycle node state

Returns:

SUCCESS or FAILURE

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

Activates action server.

Parameters:

state – Reference to LifeCycle node state

Returns:

SUCCESS or FAILURE

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

Deactivates action server.

Parameters:

state – Reference to LifeCycle node state

Returns:

SUCCESS or FAILURE

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

Resets 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

template<typename T, typename V, typename Z>
void followWaypointsHandler(const T &action_server, const V &feedback, const Z &result)

Templated function to perform internal logic behind waypoint following, Both GPS and non GPS waypoint following callbacks makes use of this function when a client asked to do so. Callbacks fills in appropriate types for the tempelated types, see followWaypointCallback funtions for details.

Template Parameters:

  • T – action_server

  • V – feedback

  • Z – result

Parameters:

  • action_server –

  • poses –

  • feedback –

  • result –

void followWaypointsCallback()

Action server callbacks.

void followGPSWaypointsCallback()

send robot through each of GPS point , which are converted to map frame first then using a client to FollowWaypoints action.

Parameters:

waypoints, acquired – from action client

void resultCallback(const rclcpp_action::ClientGoalHandle<ClientT>::WrappedResult &result)

Action client result callback.

Parameters:

result – Result of action server updated asynchronously

void goalResponseCallback(const rclcpp_action::ClientGoalHandle<ClientT>::SharedPtr &goal)

Action client goal response callback.

Parameters:

goal – Response of action server updated asynchronously

std::vector<geometry_msgs::msg::PoseStamped> convertGPSPosesToMapPoses(const std::vector<geographic_msgs::msg::GeoPose> &gps_poses)

given some gps_poses, converts them to map frame using robot_localization’s service fromLL. Constructs a vector of stamped poses in map frame and returns them.

Parameters:

gps_poses, from – the action server

Returns:

std::vector<geometry_msgs::msg::PoseStamped>

template<typename T>
std::vector<geometry_msgs::msg::PoseStamped> getLatestGoalPoses(const T &action_server)

get the latest poses on the action server goal. If they are GPS poses, convert them to the global cartesian frame using /fromLL robot localization server

Parameters:

action_server, to – which the goal was sent

Returns:

std::vector<geometry_msgs::msg::PoseStamped>

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

Callback executed when a parameter change is detected.

Parameters:

event – ParameterEvent message

Protected Attributes

std::vector<int> failed_ids_
std::string global_frame_id_ = {"map"}
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_
std::unique_ptr<ActionServer> xyz_action_server_
ActionClient::SharedPtr nav_to_pose_client_
rclcpp::CallbackGroup::SharedPtr callback_group_
rclcpp::executors::SingleThreadedExecutor callback_group_executor_
std::shared_future<rclcpp_action::ClientGoalHandle<ClientT>::SharedPtr> future_goal_handle_
std::unique_ptr<ActionServerGPS> gps_action_server_
std::unique_ptr<nav2_util::ServiceClient<robot_localization::srv::FromLL, std::shared_ptr<nav2_util::LifecycleNode>>> from_ll_to_map_client_
bool stop_on_failure_
int loop_rate_
GoalStatus current_goal_status_
pluginlib::ClassLoader<nav2_core::WaypointTaskExecutor> waypoint_task_executor_loader_
pluginlib::UniquePtr<nav2_core::WaypointTaskExecutor> waypoint_task_executor_
std::string waypoint_task_executor_id_
std::string waypoint_task_executor_type_