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

#include <nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp>

Inheritance diagram for nav2_waypoint_follower::WaypointFollower:

Collaboration diagram for nav2_waypoint_follower::WaypointFollower:

[legend]

Public Types
using	ActionT = nav2_msgs::action::FollowWaypoints

using	ClientT = nav2_msgs::action::NavigateToPose

using	ActionServer = nav2::SimpleActionServer< ActionT >

using	ActionClient = nav2::ActionClient< ClientT >

using	ActionTGPS = nav2_msgs::action::FollowGPSWaypoints

using	ActionServerGPS = nav2::SimpleActionServer< ActionTGPS >

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

	~WaypointFollower ()
	A destructor for nav2_waypoint_follower::WaypointFollower class.

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
nav2::CallbackReturn	on_configure (const rclcpp_lifecycle::State &state) override
	Configures member variables. More...

nav2::CallbackReturn	on_activate (const rclcpp_lifecycle::State &state) override
	Activates action server. More...

nav2::CallbackReturn	on_deactivate (const rclcpp_lifecycle::State &state) override
	Deactivates action server. More...

nav2::CallbackReturn	on_cleanup (const rclcpp_lifecycle::State &state) override
	Resets member variables. More...

nav2::CallbackReturn	on_shutdown (const rclcpp_lifecycle::State &state) override
	Called when in shutdown state. More...

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 templated types, see followWaypointCallback functions for details. More...

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

void	resultCallback (const rclcpp_action::ClientGoalHandle< ClientT >::WrappedResult &result)
	Action client result callback. More...

void	goalResponseCallback (const rclcpp_action::ClientGoalHandle< ClientT >::SharedPtr &goal)
	Action client goal response callback. More...

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

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

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
std::vector< int >	failed_ids_

std::string	global_frame_id_ {"map"}

rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr	dyn_params_handler_

ActionServer::SharedPtr	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_

ActionServerGPS::SharedPtr	gps_action_server_

nav2::ServiceClient< robot_localization::srv::FromLL >::SharedPtr	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_

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 that uses behavior tree for navigating a robot to its goal position.

Definition at line 67 of file waypoint_follower.hpp.

Constructor & Destructor Documentation

◆ WaypointFollower()

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

explicit

A constructor for nav2_waypoint_follower::WaypointFollower class.

Parameters

options Additional options to control creation of the node.

Definition at line 30 of file waypoint_follower.cpp.

Member Function Documentation

◆ convertGPSPosesToMapPoses()

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

protected

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>

Definition at line 477 of file waypoint_follower.cpp.

References nav2::ServiceClient< ServiceT >::invoke(), and nav2::ServiceClient< ServiceT >::wait_for_service().

Referenced by getLatestGoalPoses().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ dynamicParametersCallback()

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

protected

Callback executed when a parameter change is detected.

Parameters

event ParameterEvent message

Definition at line 449 of file waypoint_follower.cpp.

Referenced by on_activate().

Here is the caller graph for this function:

◆ followGPSWaypointsCallback()

void nav2_waypoint_follower::WaypointFollower::followGPSWaypointsCallback ( )

protected

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

Definition at line 390 of file waypoint_follower.cpp.

References followWaypointsHandler().

Referenced by on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ followWaypointsHandler()

template<typename T , typename V , typename Z >

void nav2_waypoint_follower::WaypointFollower::followWaypointsHandler	(	const T &	action_server,
		const V &	feedback,
		const Z &	result
	)

protected

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 templated types, see followWaypointCallback functions for details.

Template Parameters

T	action_server
V	feedback
Z	result

Parameters

action_server
poses
feedback
result

Definition at line 190 of file waypoint_follower.cpp.

References goalResponseCallback(), and resultCallback().

Referenced by followGPSWaypointsCallback(), and followWaypointsCallback().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getLatestGoalPoses()

template<typename T >

std::vector< geometry_msgs::msg::PoseStamped > nav2_waypoint_follower::WaypointFollower::getLatestGoalPoses ( const T & action_server )

protected

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>

Definition at line 166 of file waypoint_follower.cpp.

References convertGPSPosesToMapPoses().

Here is the call graph for this function:

◆ goalResponseCallback()

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

protected

Action client goal response callback.

Parameters

goal	Response of action server updated asynchronously

Definition at line 436 of file waypoint_follower.cpp.

Referenced by followWaypointsHandler().

Here is the caller graph for this function:

◆ on_activate()

nav2::CallbackReturn nav2_waypoint_follower::WaypointFollower::on_activate ( const rclcpp_lifecycle::State & state )

overrideprotected

Activates action server.

Parameters

state Reference to LifeCycle node state

Returns: SUCCESS or FAILURE

Definition at line 112 of file waypoint_follower.cpp.

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

Here is the call graph for this function:

◆ on_cleanup()

nav2::CallbackReturn nav2_waypoint_follower::WaypointFollower::on_cleanup ( const rclcpp_lifecycle::State & state )

overrideprotected

Resets member variables.

Parameters

state Reference to LifeCycle node state

Returns: SUCCESS or FAILURE

Definition at line 146 of file waypoint_follower.cpp.

Referenced by on_configure().

Here is the caller graph for this function:

◆ on_configure()

nav2::CallbackReturn nav2_waypoint_follower::WaypointFollower::on_configure ( const rclcpp_lifecycle::State & state )

overrideprotected

Configures member variables.

Initializes action server for "follow_waypoints"

Parameters

state Reference to LifeCycle node state

Returns: SUCCESS or FAILURE

Definition at line 55 of file waypoint_follower.cpp.

References followGPSWaypointsCallback(), followWaypointsCallback(), on_cleanup(), and nav2::LifecycleNode::shared_from_this().

Here is the call graph for this function:

◆ on_deactivate()

nav2::CallbackReturn nav2_waypoint_follower::WaypointFollower::on_deactivate ( const rclcpp_lifecycle::State & state )

overrideprotected

Deactivates action server.

Parameters

state Reference to LifeCycle node state

Returns: SUCCESS or FAILURE

Definition at line 131 of file waypoint_follower.cpp.

References nav2::LifecycleNode::destroyBond().

Here is the call graph for this function:

◆ on_shutdown()

nav2::CallbackReturn nav2_waypoint_follower::WaypointFollower::on_shutdown ( const rclcpp_lifecycle::State & state )

overrideprotected

Called when in shutdown state.

Parameters

state Reference to LifeCycle node state

Returns: SUCCESS or FAILURE

Definition at line 159 of file waypoint_follower.cpp.

◆ resultCallback()

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

protected

Action client result callback.

Parameters

result Result of action server updated asynchronously

Definition at line 403 of file waypoint_follower.cpp.

Referenced by followWaypointsHandler().

Here is the caller graph for this function:

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

nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp
nav2_waypoint_follower/src/waypoint_follower.cpp

Public Types

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ WaypointFollower()

Member Function Documentation

◆ convertGPSPosesToMapPoses()

◆ dynamicParametersCallback()

◆ followGPSWaypointsCallback()

◆ followWaypointsHandler()

◆ getLatestGoalPoses()

◆ goalResponseCallback()

◆ on_activate()

◆ on_cleanup()

◆ on_configure()

◆ on_deactivate()

◆ on_shutdown()

◆ resultCallback()