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, rclcpp::PublisherMatchedCallbackType matched_callback=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	transformedPlanAndGoal (const geometry_msgs::msg::PoseStamped &current_robot_pose)
	Refreshes transformed_global_plan_ and transformed_end_pose_ for the current cycle. More...

void	computeAndPublishVelocity (const geometry_msgs::msg::PoseStamped &current_robot_pose)
	Calculates velocity and publishes to "cmd_vel" topic. More...

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 (const geometry_msgs::msg::PoseStamped &current_robot_pose)
	Checks if goal is reached. More...

geometry_msgs::msg::PoseStamped	getCurrentRobotPose ()
	Obtain the current pose of the robot in the costmap 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 39 of file controller_server.cpp.

Member Function Documentation

◆ computeAndPublishVelocity()

void nav2_controller::ControllerServer::computeAndPublishVelocity ( const geometry_msgs::msg::PoseStamped & current_robot_pose )

protected

Calculates velocity and publishes to "cmd_vel" topic.

Parameters

current_robot_pose Pose of the robot to be used as reference

Definition at line 742 of file controller_server.cpp.

References getThresholdedTwist(), and publishVelocity().

Referenced by computeControl().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ 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 496 of file controller_server.cpp.

References computeAndPublishVelocity(), findControllerId(), findGoalCheckerId(), findPathHandlerId(), findProgressCheckerId(), getCurrentRobotPose(), isGoalReached(), onGoalExit(), setPlannerPath(), transformedPlanAndGoal(), 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 334 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 360 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 428 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 386 of file controller_server.cpp.

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

Here is the caller graph for this function:

◆ getCurrentRobotPose()

geometry_msgs::msg::PoseStamped nav2_controller::ControllerServer::getCurrentRobotPose ( )

protected

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

Returns: Current robot pose

Exceptions

nav2_core::ControllerTFError if the pose cannot be obtained or its transform is stale

Definition at line 970 of file controller_server.cpp.

Referenced by computeControl().

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 246 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 236 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 454 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 ( const geometry_msgs::msg::PoseStamped & current_robot_pose )

protected

Checks if goal is reached.

Parameters

current_robot_pose The robot pose to be used as reference

Returns: true or false

Definition at line 961 of file controller_server.cpp.

References 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 226 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 297 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 65 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 265 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 328 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 923 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 697 of file controller_server.cpp.

Referenced by computeControl(), and updateGlobalPath().

Here is the caller graph for this function:

◆ transformedPlanAndGoal()

void nav2_controller::ControllerServer::transformedPlanAndGoal ( const geometry_msgs::msg::PoseStamped & current_robot_pose )

protected

Refreshes transformed_global_plan_ and transformed_end_pose_ for the current cycle.

Parameters

current_robot_pose Pose of the robot to be used as reference

Exceptions

nav2_core::ControllerTFError if the robot pose or end pose cannot be obtained

Definition at line 720 of file controller_server.cpp.

Referenced by computeControl().

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 680 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

◆ computeAndPublishVelocity()

◆ computeControl()

◆ findControllerId()

◆ findGoalCheckerId()

◆ findPathHandlerId()

◆ findProgressCheckerId()

◆ getCurrentRobotPose()

◆ getThresholdedTwist()

◆ getThresholdedVelocity()

◆ goalReceived()

◆ isGoalReached()

◆ on_activate()

◆ on_cleanup()

◆ on_configure()

◆ on_deactivate()

◆ on_shutdown()

◆ publishVelocity()

◆ setPlannerPath()

◆ transformedPlanAndGoal()

◆ waitForCostmap()