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

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

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	isPathValid (const std::shared_ptr< rmw_request_id_t > request_header, const std::shared_ptr< nav2_msgs::srv::IsPathValid::Request > request, std::shared_ptr< nav2_msgs::srv::IsPathValid::Response > response)
	The service callback to determine if the path is still valid. More...

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)

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
ActionServerToPose::SharedPtr	action_server_pose_

ActionServerThroughPoses::SharedPtr	action_server_poses_

rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr	dyn_params_handler_

std::mutex	dynamic_params_lock_

PlannerMap	planners_

pluginlib::ClassLoader< nav2_core::GlobalPlanner >	gp_loader_

std::vector< std::string >	default_ids_

std::vector< std::string >	default_types_

std::vector< std::string >	planner_ids_

std::vector< std::string >	planner_types_

double	max_planner_duration_

rclcpp::Duration	costmap_update_timeout_

std::string	planner_ids_concat_

std::shared_ptr< tf2_ros::Buffer >	tf_

std::shared_ptr< nav2_costmap_2d::Costmap2DROS >	costmap_ros_

std::unique_ptr< nav2::NodeThread >	costmap_thread_

nav2_costmap_2d::Costmap2D *	costmap_

std::unique_ptr< nav2_costmap_2d::FootprintCollisionChecker< nav2_costmap_2d::Costmap2D * > >	collision_checker_

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

nav2::ServiceServer< nav2_msgs::srv::IsPathValid >::SharedPtr	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 42 of file planner_server.cpp.

Member Function Documentation

◆ dynamicParametersCallback()

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

protected

Callback executed when a parameter change is detected.

Parameters

event ParameterEvent message

Definition at line 722 of file planner_server.cpp.

Referenced by on_activate().

Here is the caller graph for this function:

◆ getPlan()

nav_msgs::msg::Path nav2_planner::PlannerServer::getPlan	(	const geometry_msgs::msg::PoseStamped &	start,
		const geometry_msgs::msg::PoseStamped &	goal,
		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 602 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 306 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 316 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 293 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:

◆ isPathValid()

void nav2_planner::PlannerServer::isPathValid	(	const std::shared_ptr< rmw_request_id_t >	request_header,
		const std::shared_ptr< nav2_msgs::srv::IsPathValid::Request >	request,
		std::shared_ptr< nav2_msgs::srv::IsPathValid::Response >	response
	)

protected

The service callback to determine if the path is still valid.

Parameters

request	to the service
response	from the service

The lethal check starts at the closest point to avoid points that have already been passed and may have become occupied. The method for collision detection is based on the shape of the footprint.

Definition at line 643 of file planner_server.cpp.

References nav2_costmap_2d::Costmap2D::getCost(), and nav2_costmap_2d::Costmap2D::worldToMap().

Referenced by on_activate().

Here is the call graph for this function:

Here is the caller 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 268 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 173 of file planner_server.cpp.

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

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 237 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 81 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 206 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 261 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 635 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 329 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 343 of file planner_server.cpp.

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

◆ dynamicParametersCallback()

◆ getPlan()

◆ getPreemptedGoalIfRequested()

◆ getStartPose()

◆ isCancelRequested()

◆ isPathValid()

◆ isServerInactive()

◆ on_activate()

◆ on_cleanup()

◆ on_configure()

◆ on_deactivate()

◆ on_shutdown()

◆ publishPlan()

◆ transformPosesToGlobalFrame()

◆ validatePath()