An action server implements the behavior tree's ComputePathToPose interface and hosts various plugins of different algorithms to compute plans. More...

#include <nav2_planner/include/nav2_planner/planner_server.hpp>

Inheritance diagram for nav2_planner::PlannerServer:

[legend]

Collaboration diagram for nav2_planner::PlannerServer:

[legend]

Public Types
using	PlannerMap = std::unordered_map< std::string, nav2_core::GlobalPlanner::Ptr >

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

	~PlannerServer ()
	A destructor for nav2_planner::PlannerServer.

nav_msgs::msg::Path	getPlan (const geometry_msgs::msg::PoseStamped &start, const geometry_msgs::msg::PoseStamped &goal, const std::vector< geometry_msgs::msg::PoseStamped > &viapoints, const std::string &planner_id, std::function< bool()> cancel_checker)
	Method to get plan from the desired plugin. More...

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 DurationRepT , typename DurationT , typename CallbackT >
rclcpp::GenericTimer< CallbackT >::SharedPtr	create_timer (std::chrono::duration< DurationRepT, DurationT > period, CallbackT callback, rclcpp::CallbackGroup::SharedPtr group=nullptr)
	Create a sim-time-aware timer for Nav2 lifecycle nodes. 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 >::GoalReceivedCallback goal_received_callback=nullptr, 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 Types
using	ActionToPose = nav2_msgs::action::ComputePathToPose

using	ActionToPoseResult = ActionToPose::Result

using	ActionThroughPoses = nav2_msgs::action::ComputePathThroughPoses

using	ActionThroughPosesResult = ActionThroughPoses::Result

using	ActionServerToPose = nav2::SimpleActionServer< ActionToPose >

using	ActionServerThroughPoses = nav2::SimpleActionServer< ActionThroughPoses >

Protected Member Functions
nav2::CallbackReturn	on_configure (const rclcpp_lifecycle::State &state) override
	Configure member variables and initializes planner. More...

nav2::CallbackReturn	on_activate (const rclcpp_lifecycle::State &state) override
	Activate member variables. More...

nav2::CallbackReturn	on_deactivate (const rclcpp_lifecycle::State &state) override
	Deactivate member variables. More...

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

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

template<typename T >
bool	goalReceived (std::shared_ptr< const typename T::Goal > goal)
	Goal received callback to validate a new goal before acceptance. More...

template<typename T >
bool	isServerInactive (typename nav2::SimpleActionServer< T >::SharedPtr &action_server)
	Check if an action server is valid / active. More...

template<typename T >
bool	isCancelRequested (typename nav2::SimpleActionServer< T >::SharedPtr &action_server)
	Check if an action server has a cancellation request pending. More...

double	waitForCostmap ()
	Wait for costmap to be valid with updated sensor data or repopulate after a clearing recovery. Blocks until true without timeout. More...

template<typename T >
void	getPreemptedGoalIfRequested (typename nav2::SimpleActionServer< T >::SharedPtr &action_server, typename std::shared_ptr< const typename T::Goal > goal)
	Check if an action server has a preemption request and replaces the goal with the new preemption goal. More...

template<typename T >
bool	getStartPose (typename std::shared_ptr< const typename T::Goal > goal, geometry_msgs::msg::PoseStamped &start)
	Get the starting pose from costmap or message, if valid. More...

bool	transformPosesToGlobalFrame (geometry_msgs::msg::PoseStamped &curr_start, geometry_msgs::msg::PoseStamped &curr_goal)
	Transform start and goal poses into the costmap global frame for path planning plugins to utilize. More...

template<typename T >
bool	validatePath (const geometry_msgs::msg::PoseStamped &curr_goal, const nav_msgs::msg::Path &path, const std::string &planner_id)
	Validate that the path contains a meaningful path. More...

void	computePlan ()
	The action server callback which calls planner to get the path ComputePathToPose.

void	computePlanThroughPoses ()
	The action server callback which calls planner to get the path ComputePathThroughPoses.

void	publishPlan (const nav_msgs::msg::Path &path)
	Publish a path for visualization purposes. More...

void	exceptionWarning (const geometry_msgs::msg::PoseStamped &start, const geometry_msgs::msg::PoseStamped &goal, const std::string &planner_id, const std::exception &ex, std::string &msg)

Protected Member Functions inherited from nav2::LifecycleNode
void	printLifecycleNodeNotification ()
	Print notifications for lifecycle node.

void	register_rcl_preshutdown_callback ()

void	runCleanups ()

Protected Attributes
ActionServerToPose::SharedPtr	action_server_pose_

ActionServerThroughPoses::SharedPtr	action_server_poses_

std::unique_ptr< nav2_planner::ParameterHandler >	param_handler_

Parameters *	params_

PlannerMap	planners_

pluginlib::ClassLoader< nav2_core::GlobalPlanner >	gp_loader_

std::string	planner_ids_concat_

nav2::TransformBuffer::SharedPtr	tf_

std::shared_ptr< nav2_costmap_2d::Costmap2DROS >	costmap_ros_

std::unique_ptr< nav2::NodeThread >	costmap_thread_

nav2_costmap_2d::Costmap2D *	costmap_

nav2::Publisher< nav_msgs::msg::Path >::SharedPtr	plan_publisher_

std::unique_ptr< IsPathValidService >	is_path_valid_service_

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 implements the behavior tree's ComputePathToPose interface and hosts various plugins of different algorithms to compute plans.

Definition at line 52 of file planner_server.hpp.

Constructor & Destructor Documentation

◆ PlannerServer()

nav2_planner::PlannerServer::PlannerServer ( const rclcpp::NodeOptions & options = rclcpp::NodeOptions() )

explicit

A constructor for nav2_planner::PlannerServer.

Parameters

options Additional options to control creation of the node.

Definition at line 46 of file planner_server.cpp.

Member Function Documentation

◆ getPlan()

nav_msgs::msg::Path nav2_planner::PlannerServer::getPlan	(	const geometry_msgs::msg::PoseStamped &	start,
		const geometry_msgs::msg::PoseStamped &	goal,
		const std::vector< geometry_msgs::msg::PoseStamped > &	viapoints,
		const std::string &	planner_id,
		std::function< bool()>	cancel_checker
	)

Method to get plan from the desired plugin.

Parameters

start	starting pose
goal	goal request
planner_id	The planner to plan with
cancel_checker	A function to check if the action has been canceled

Returns: Path

Definition at line 641 of file planner_server.cpp.

Referenced by computePlan(), and computePlanThroughPoses().

Here is the caller graph for this function:

◆ getPreemptedGoalIfRequested()

template<typename T >

void nav2_planner::PlannerServer::getPreemptedGoalIfRequested	(	typename nav2::SimpleActionServer< T >::SharedPtr &	action_server,
		typename std::shared_ptr< const typename T::Goal >	goal
	)

protected

Check if an action server has a preemption request and replaces the goal with the new preemption goal.

Parameters

action_server	Action server to get updated goal if required
goal	Goal to overwrite

Definition at line 304 of file planner_server.cpp.

References nav2::SimpleActionServer< ActionT >::accept_pending_goal(), and nav2::SimpleActionServer< ActionT >::is_preempt_requested().

Here is the call graph for this function:

◆ getStartPose()

template<typename T >

bool nav2_planner::PlannerServer::getStartPose	(	typename std::shared_ptr< const typename T::Goal >	goal,
		geometry_msgs::msg::PoseStamped &	start
	)

protected

Get the starting pose from costmap or message, if valid.

Parameters

action_server	Action server to terminate if required
goal	Goal to find start from
start	The starting pose to use

Returns: bool If successful in finding a valid starting pose

Definition at line 314 of file planner_server.cpp.

◆ goalReceived()

template<typename T >

bool nav2_planner::PlannerServer::goalReceived ( std::shared_ptr< const typename T::Goal > goal )

protected

Goal received callback to validate a new goal before acceptance.

Parameters

goal	The incoming goal to validate

Returns: true if goal should be accepted, false to reject

Definition at line 239 of file planner_server.cpp.

◆ isCancelRequested()

template<typename T >

bool nav2_planner::PlannerServer::isCancelRequested ( typename nav2::SimpleActionServer< T >::SharedPtr & action_server )

protected

Check if an action server has a cancellation request pending.

Parameters

action_server Action server to test

Returns: SUCCESS or FAILURE

Definition at line 291 of file planner_server.cpp.

References nav2::SimpleActionServer< ActionT >::is_cancel_requested(), and nav2::SimpleActionServer< ActionT >::terminate_all().

Here is the call graph for this function:

◆ isServerInactive()

template<typename T >

bool nav2_planner::PlannerServer::isServerInactive ( typename nav2::SimpleActionServer< T >::SharedPtr & action_server )

protected

Check if an action server is valid / active.

Parameters

action_server Action server to test

Returns: SUCCESS or FAILURE

Definition at line 262 of file planner_server.cpp.

References nav2::SimpleActionServer< ActionT >::is_server_active().

Here is the call graph for this function:

◆ on_activate()

nav2::CallbackReturn nav2_planner::PlannerServer::on_activate ( const rclcpp_lifecycle::State & state )

overrideprotected

Activate member variables.

Parameters

state Reference to LifeCycle node state

Returns: SUCCESS or FAILURE

Definition at line 149 of file planner_server.cpp.

References nav2::LifecycleNode::createBond().

Here is the call graph for this function:

◆ on_cleanup()

nav2::CallbackReturn nav2_planner::PlannerServer::on_cleanup ( const rclcpp_lifecycle::State & state )

overrideprotected

Reset member variables.

Parameters

state Reference to LifeCycle node state

Returns: SUCCESS or FAILURE

Definition at line 208 of file planner_server.cpp.

Referenced by on_configure().

Here is the caller graph for this function:

◆ on_configure()

nav2::CallbackReturn nav2_planner::PlannerServer::on_configure ( const rclcpp_lifecycle::State & state )

overrideprotected

Configure member variables and initializes planner.

Parameters

state Reference to LifeCycle node state

Returns: SUCCESS or FAILURE

Definition at line 69 of file planner_server.cpp.

References computePlan(), computePlanThroughPoses(), nav2_costmap_2d::Costmap2D::getSizeInCellsX(), nav2_costmap_2d::Costmap2D::getSizeInCellsY(), on_cleanup(), and nav2::LifecycleNode::shared_from_this().

Here is the call graph for this function:

◆ on_deactivate()

nav2::CallbackReturn nav2_planner::PlannerServer::on_deactivate ( const rclcpp_lifecycle::State & state )

overrideprotected

Deactivate member variables.

Parameters

state Reference to LifeCycle node state

Returns: SUCCESS or FAILURE

Definition at line 176 of file planner_server.cpp.

References nav2::LifecycleNode::destroyBond().

Here is the call graph for this function:

◆ on_shutdown()

nav2::CallbackReturn nav2_planner::PlannerServer::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 232 of file planner_server.cpp.

◆ publishPlan()

void nav2_planner::PlannerServer::publishPlan ( const nav_msgs::msg::Path & path )

protected

Publish a path for visualization purposes.

Parameters

path	Reference to Global Path

Definition at line 676 of file planner_server.cpp.

Referenced by computePlan(), and computePlanThroughPoses().

Here is the caller graph for this function:

◆ transformPosesToGlobalFrame()

bool nav2_planner::PlannerServer::transformPosesToGlobalFrame	(	geometry_msgs::msg::PoseStamped &	curr_start,
		geometry_msgs::msg::PoseStamped &	curr_goal
	)

protected

Transform start and goal poses into the costmap global frame for path planning plugins to utilize.

Parameters

start	The starting pose to transform
goal	Goal pose to transform

Returns: bool If successful in transforming poses

Definition at line 327 of file planner_server.cpp.

Referenced by computePlan(), and computePlanThroughPoses().

Here is the caller graph for this function:

◆ validatePath()

template<typename T >

bool nav2_planner::PlannerServer::validatePath	(	const geometry_msgs::msg::PoseStamped &	curr_goal,
		const nav_msgs::msg::Path &	path,
		const std::string &	planner_id
	)

protected

Validate that the path contains a meaningful path.

Parameters

action_server	Action server to terminate if required
goal	Goal Current goal
path	Current path
planner_id	The planner ID used to generate the path

Returns: bool If path is valid

Definition at line 341 of file planner_server.cpp.

◆ waitForCostmap()

double nav2_planner::PlannerServer::waitForCostmap ( )

protected

Wait for costmap to be valid with updated sensor data or repopulate after a clearing recovery. Blocks until true without timeout.

Returns: Duration in seconds spent waiting for the costmap (0.0 if already current)

Definition at line 273 of file planner_server.cpp.

Referenced by computePlan(), and computePlanThroughPoses().

Here is the caller graph for this function:

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

nav2_planner/include/nav2_planner/planner_server.hpp
nav2_planner/src/planner_server.cpp

Public Types

Public Member Functions

Protected Types

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ PlannerServer()

Member Function Documentation

◆ getPlan()

◆ getPreemptedGoalIfRequested()

◆ getStartPose()

◆ goalReceived()

◆ isCancelRequested()

◆ isServerInactive()

◆ on_activate()

◆ on_cleanup()

◆ on_configure()

◆ on_deactivate()

◆ on_shutdown()

◆ publishPlan()

◆ transformPosesToGlobalFrame()

◆ validatePath()

◆ waitForCostmap()