This class hosts variety of plugins of different algorithms to complete control tasks from the exposed FollowPath action server. More...

#include <nav2_controller/include/nav2_controller/controller_server.hpp>

Inheritance diagram for nav2_controller::ControllerServer:

[legend]

Collaboration diagram for nav2_controller::ControllerServer:

[legend]

Public Types
using	ControllerMap = std::unordered_map< std::string, nav2_core::Controller::Ptr >

using	GoalCheckerMap = std::unordered_map< std::string, nav2_core::GoalChecker::Ptr >

using	ProgressCheckerMap = std::unordered_map< std::string, nav2_core::ProgressChecker::Ptr >

using	PathHandlerMap = std::unordered_map< std::string, nav2_core::PathHandler::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
	ControllerServer (const rclcpp::NodeOptions &options=rclcpp::NodeOptions())
	Constructor for nav2_controller::ControllerServer. More...

	~ControllerServer ()
	Destructor for nav2_controller::ControllerServer.

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	Action = nav2_msgs::action::FollowPath

using	ActionServer = nav2::SimpleActionServer< Action >

Protected Member Functions
nav2::CallbackReturn	on_configure (const rclcpp_lifecycle::State &state) override
	Configures controller parameters and member variables. More...

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

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

nav2::CallbackReturn	on_cleanup (const rclcpp_lifecycle::State &state) override
	Calls clean up states and resets member variables. More...

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

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

void	computeControl ()
	FollowPath action server callback. Handles action server updates and spins server until goal is reached. More...

bool	findControllerId (const std::string &c_name, std::string &name)
	Find the valid controller ID name for the given request. More...

bool	findGoalCheckerId (const std::string &c_name, std::string &name)
	Find the valid goal checker ID name for the specified parameter. More...

bool	findProgressCheckerId (const std::string &c_name, std::string &name)
	Find the valid progress checker ID name for the specified parameter. More...

bool	findPathHandlerId (const std::string &c_name, std::string &name)
	Find the valid path handler ID name for the specified parameter. More...

void	setPlannerPath (const nav_msgs::msg::Path &path)
	Assigns path to controller. More...

void	computeAndPublishVelocity ()
	Calculates velocity and publishes to "cmd_vel" topic.

void	updateGlobalPath ()
	Calls setPlannerPath method with an updated path received from action server.

void	publishVelocity (const geometry_msgs::msg::TwistStamped &velocity)
	Calls velocity publisher to publish the velocity on "cmd_vel" topic. More...

void	publishZeroVelocity ()
	Calls velocity publisher to publish zero velocity.

void	onGoalExit (bool force_stop)
	Called on goal exit.

double	waitForCostmap ()
	Wait for costmap to become current, with timeout. More...

bool	isGoalReached ()
	Checks if goal is reached. More...

bool	getRobotPose (geometry_msgs::msg::PoseStamped &pose)
	Obtain current pose of the robot in costmap's frame. More...

double	getThresholdedVelocity (double velocity, double threshold)
	get the thresholded velocity More...

geometry_msgs::msg::Twist	getThresholdedTwist (const geometry_msgs::msg::Twist &twist)
	get the thresholded Twist More...

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

void	register_rcl_preshutdown_callback ()

void	runCleanups ()

Protected Attributes
ActionServer::SharedPtr	action_server_

std::shared_ptr< nav2_costmap_2d::Costmap2DROS >	costmap_ros_

std::unique_ptr< nav2::NodeThread >	costmap_thread_

std::unique_ptr< nav2_util::OdomSmoother >	odom_sub_

std::unique_ptr< nav2_util::TwistPublisher >	vel_publisher_

nav2::Subscription< nav2_msgs::msg::SpeedLimit >::SharedPtr	speed_limit_sub_

nav2::Publisher< nav2_msgs::msg::TrackingFeedback >::SharedPtr	tracking_feedback_pub_

pluginlib::ClassLoader< nav2_core::ProgressChecker >	progress_checker_loader_

ProgressCheckerMap	progress_checkers_

std::string	progress_checker_ids_concat_

std::string	current_progress_checker_

pluginlib::ClassLoader< nav2_core::GoalChecker >	goal_checker_loader_

GoalCheckerMap	goal_checkers_

std::string	goal_checker_ids_concat_

std::string	current_goal_checker_

pluginlib::ClassLoader< nav2_core::Controller >	lp_loader_

ControllerMap	controllers_

std::string	controller_ids_concat_

std::string	current_controller_

pluginlib::ClassLoader< nav2_core::PathHandler >	path_handler_loader_

PathHandlerMap	path_handlers_

std::string	path_handler_ids_concat_

std::string	current_path_handler_

size_t	start_index_

geometry_msgs::msg::PoseStamped	end_pose_

geometry_msgs::msg::PoseStamped	transformed_end_pose_

rclcpp::Time	last_valid_cmd_time_

nav_msgs::msg::Path	current_path_

nav_msgs::msg::Path	transformed_global_plan_

std::unique_ptr< nav2_controller::ParameterHandler >	param_handler_

Parameters *	params_

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

double	transform_tolerance_

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

This class hosts variety of plugins of different algorithms to complete control tasks from the exposed FollowPath action server.

Definition at line 52 of file controller_server.hpp.

Constructor & Destructor Documentation

◆ ControllerServer()

nav2_controller::ControllerServer::ControllerServer ( const rclcpp::NodeOptions & options = rclcpp::NodeOptions() )

explicit

Constructor for nav2_controller::ControllerServer.

Parameters

options Additional options to control creation of the node.

Definition at line 38 of file controller_server.cpp.

Member Function Documentation

◆ computeControl()

void nav2_controller::ControllerServer::computeControl ( )

protected

FollowPath action server callback. Handles action server updates and spins server until goal is reached.

Provides global path to controller received from action client. Twist velocities for the robot are calculated and published using controller at the specified rate till the goal is reached.

Exceptions

nav2_core::PlannerException

Definition at line 491 of file controller_server.cpp.

References computeAndPublishVelocity(), findControllerId(), findGoalCheckerId(), findPathHandlerId(), findProgressCheckerId(), isGoalReached(), onGoalExit(), setPlannerPath(), updateGlobalPath(), and waitForCostmap().

Referenced by on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ findControllerId()

bool nav2_controller::ControllerServer::findControllerId	(	const std::string &	c_name,
		std::string &	name
	)

protected

Find the valid controller ID name for the given request.

Parameters

c_name	The requested controller name
name	Reference to the name to use for control if any valid available

Returns: bool Whether it found a valid controller to use

Definition at line 329 of file controller_server.cpp.

Referenced by computeControl(), goalReceived(), and updateGlobalPath().

Here is the caller graph for this function:

◆ findGoalCheckerId()

bool nav2_controller::ControllerServer::findGoalCheckerId	(	const std::string &	c_name,
		std::string &	name
	)

protected

Find the valid goal checker ID name for the specified parameter.

Parameters

c_name	The goal checker name
name	Reference to the name to use for goal checking if any valid available

Returns: bool Whether it found a valid goal checker to use

Definition at line 355 of file controller_server.cpp.

Referenced by computeControl(), goalReceived(), and updateGlobalPath().

Here is the caller graph for this function:

◆ findPathHandlerId()

bool nav2_controller::ControllerServer::findPathHandlerId	(	const std::string &	c_name,
		std::string &	name
	)

protected

Find the valid path handler ID name for the specified parameter.

Parameters

c_name	The path handler name
name	Reference to the name to use for path handling if any valid available

Returns: bool Whether it found a valid path handler to use

Definition at line 423 of file controller_server.cpp.

Referenced by computeControl(), goalReceived(), and updateGlobalPath().

Here is the caller graph for this function:

◆ findProgressCheckerId()

bool nav2_controller::ControllerServer::findProgressCheckerId	(	const std::string &	c_name,
		std::string &	name
	)

protected

Find the valid progress checker ID name for the specified parameter.

Parameters

c_name	The progress checker name
name	Reference to the name to use for progress checking if any valid available

Returns: bool Whether it found a valid progress checker to use

Definition at line 381 of file controller_server.cpp.

Referenced by computeControl(), goalReceived(), and updateGlobalPath().

Here is the caller graph for this function:

◆ getRobotPose()

bool nav2_controller::ControllerServer::getRobotPose ( geometry_msgs::msg::PoseStamped & pose )

protected

Obtain current pose of the robot in costmap's frame.

Parameters

pose	To store current pose of the robot

Returns: true if able to obtain current pose of the robot, else false

Definition at line 964 of file controller_server.cpp.

Referenced by computeAndPublishVelocity(), and isGoalReached().

Here is the caller graph for this function:

◆ getThresholdedTwist()

geometry_msgs::msg::Twist nav2_controller::ControllerServer::getThresholdedTwist ( const geometry_msgs::msg::Twist & twist )

inlineprotected

get the thresholded Twist

Parameters

Twist The current Twist from odometry

Returns: Twist Twist after thresholds applied

Definition at line 237 of file controller_server.hpp.

References getThresholdedVelocity().

Referenced by computeAndPublishVelocity(), and isGoalReached().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getThresholdedVelocity()

double nav2_controller::ControllerServer::getThresholdedVelocity	(	double	velocity,
		double	threshold
	)

inlineprotected

get the thresholded velocity

Parameters

velocity	The current velocity from odometry
threshold	The minimum velocity to return non-zero

Returns: double velocity value

Definition at line 227 of file controller_server.hpp.

Referenced by getThresholdedTwist().

Here is the caller graph for this function:

◆ goalReceived()

bool nav2_controller::ControllerServer::goalReceived ( std::shared_ptr< const Action::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 449 of file controller_server.cpp.

References findControllerId(), findGoalCheckerId(), findPathHandlerId(), and findProgressCheckerId().

Referenced by on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ isGoalReached()

bool nav2_controller::ControllerServer::isGoalReached ( )

protected

Checks if goal is reached.

Returns: true or false

Definition at line 941 of file controller_server.cpp.

References getRobotPose(), and getThresholdedTwist().

Referenced by computeControl().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ on_activate()

nav2::CallbackReturn nav2_controller::ControllerServer::on_activate ( const rclcpp_lifecycle::State & state )

overrideprotected

Activates member variables.

Activates controller, costmap, velocity publisher and follow path action server

Parameters

state LifeCycle Node's state

Returns: Success or Failure

Definition at line 221 of file controller_server.cpp.

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

Here is the call graph for this function:

◆ on_cleanup()

nav2::CallbackReturn nav2_controller::ControllerServer::on_cleanup ( const rclcpp_lifecycle::State & state )

overrideprotected

Calls clean up states and resets member variables.

Controller and costmap clean up state is called, and resets rest of the variables

Parameters

state LifeCycle Node's state

Returns: Success or Failure

Definition at line 292 of file controller_server.cpp.

Referenced by on_configure().

Here is the caller graph for this function:

◆ on_configure()

nav2::CallbackReturn nav2_controller::ControllerServer::on_configure ( const rclcpp_lifecycle::State & state )

overrideprotected

Configures controller parameters and member variables.

Configures controller plugin and costmap; Initialize odom subscriber, velocity publisher and follow path action server.

Parameters

state LifeCycle Node's state

Returns: Success or Failure

Exceptions

pluginlib::PluginlibException When failed to initialize controller plugin

Definition at line 64 of file controller_server.cpp.

References computeControl(), goalReceived(), on_cleanup(), and nav2::LifecycleNode::shared_from_this().

Here is the call graph for this function:

◆ on_deactivate()

nav2::CallbackReturn nav2_controller::ControllerServer::on_deactivate ( const rclcpp_lifecycle::State & state )

overrideprotected

Deactivates member variables.

Deactivates follow path action server, controller, costmap and velocity publisher. Before calling deactivate state, velocity is being set to zero.

Parameters

state LifeCycle Node's state

Returns: Success or Failure

Definition at line 260 of file controller_server.cpp.

References nav2::LifecycleNode::destroyBond(), and publishZeroVelocity().

Here is the call graph for this function:

◆ on_shutdown()

nav2::CallbackReturn nav2_controller::ControllerServer::on_shutdown ( const rclcpp_lifecycle::State & state )

overrideprotected

Called when in Shutdown state.

Parameters

state LifeCycle Node's state

Returns: Success or Failure

Definition at line 323 of file controller_server.cpp.

◆ publishVelocity()

void nav2_controller::ControllerServer::publishVelocity ( const geometry_msgs::msg::TwistStamped & velocity )

protected

Calls velocity publisher to publish the velocity on "cmd_vel" topic.

Parameters

velocity Twist velocity to be published

Definition at line 903 of file controller_server.cpp.

Referenced by computeAndPublishVelocity(), and publishZeroVelocity().

Here is the caller graph for this function:

◆ setPlannerPath()

void nav2_controller::ControllerServer::setPlannerPath ( const nav_msgs::msg::Path & path )

protected

Assigns path to controller.

Parameters

path	Path received from action server

Definition at line 685 of file controller_server.cpp.

Referenced by computeControl(), and updateGlobalPath().

Here is the caller graph for this function:

◆ waitForCostmap()

double nav2_controller::ControllerServer::waitForCostmap ( )

protected

Wait for costmap to become current, with timeout.

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

Exceptions

nav2_core::ControllerTimedOut if costmap update times out

Definition at line 668 of file controller_server.cpp.

Referenced by computeControl().

Here is the caller graph for this function:

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

nav2_controller/include/nav2_controller/controller_server.hpp
nav2_controller/src/controller_server.cpp

Public Types

Public Member Functions

Protected Types

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ ControllerServer()

Member Function Documentation

◆ computeControl()

◆ findControllerId()

◆ findGoalCheckerId()

◆ findPathHandlerId()

◆ findProgressCheckerId()

◆ getRobotPose()

◆ getThresholdedTwist()

◆ getThresholdedVelocity()

◆ goalReceived()

◆ isGoalReached()

◆ on_activate()

◆ on_cleanup()

◆ on_configure()

◆ on_deactivate()

◆ on_shutdown()

◆ publishVelocity()

◆ setPlannerPath()

◆ waitForCostmap()