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 >

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_util::LifecycleNode
	LifecycleNode (const std::string &node_name, const std::string &ns="", const rclcpp::NodeOptions &options=rclcpp::NodeOptions())
	A lifecycle node constructor. More...

void	add_parameter (const std::string &name, const rclcpp::ParameterValue &default_value, const std::string &description="", const std::string &additional_constraints="", bool read_only=false)
	Declare a parameter that has no integer or floating point range constraints. More...

void	add_parameter (const std::string &name, const rclcpp::ParameterValue &default_value, const floating_point_range fp_range, const std::string &description="", const std::string &additional_constraints="", bool read_only=false)
	Declare a parameter that has a floating point range constraint. More...

void	add_parameter (const std::string &name, const rclcpp::ParameterValue &default_value, const integer_range int_range, const std::string &description="", const std::string &additional_constraints="", bool read_only=false)
	Declare a parameter that has an integer range constraint. More...

std::shared_ptr< nav2_util::LifecycleNode >	shared_from_this ()
	Get a shared pointer of this.

nav2_util::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_util::SimpleActionServer< Action >

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

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

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

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

nav2_util::CallbackReturn	on_shutdown (const rclcpp_lifecycle::State &state) override
	Called when in Shutdown state. 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...

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.

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

nav_2d_msgs::msg::Twist2D	getThresholdedTwist (const nav_2d_msgs::msg::Twist2D &twist)
	get the thresholded Twist 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_util::LifecycleNode
void	printLifecycleNodeNotification ()
	Print notifications for lifecycle node.

void	register_rcl_preshutdown_callback ()

void	runCleanups ()

Protected Attributes
std::unique_ptr< ActionServer >	action_server_

rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr	dyn_params_handler_

std::mutex	dynamic_params_lock_

std::shared_ptr< nav2_costmap_2d::Costmap2DROS >	costmap_ros_

std::unique_ptr< nav2_util::NodeThread >	costmap_thread_

std::unique_ptr< nav_2d_utils::OdomSubscriber >	odom_sub_

std::unique_ptr< nav2_util::TwistPublisher >	vel_publisher_

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

pluginlib::ClassLoader< nav2_core::ProgressChecker >	progress_checker_loader_

ProgressCheckerMap	progress_checkers_

std::vector< std::string >	default_progress_checker_ids_

std::vector< std::string >	default_progress_checker_types_

std::vector< std::string >	progress_checker_ids_

std::vector< std::string >	progress_checker_types_

std::string	progress_checker_ids_concat_

std::string	current_progress_checker_

pluginlib::ClassLoader< nav2_core::GoalChecker >	goal_checker_loader_

GoalCheckerMap	goal_checkers_

std::vector< std::string >	default_goal_checker_ids_

std::vector< std::string >	default_goal_checker_types_

std::vector< std::string >	goal_checker_ids_

std::vector< std::string >	goal_checker_types_

std::string	goal_checker_ids_concat_

std::string	current_goal_checker_

pluginlib::ClassLoader< nav2_core::Controller >	lp_loader_

ControllerMap	controllers_

std::vector< std::string >	default_ids_

std::vector< std::string >	default_types_

std::vector< std::string >	controller_ids_

std::vector< std::string >	controller_types_

std::string	controller_ids_concat_

std::string	current_controller_

double	controller_frequency_

double	min_x_velocity_threshold_

double	min_y_velocity_threshold_

double	min_theta_velocity_threshold_

double	failure_tolerance_

bool	use_realtime_priority_

bool	publish_zero_velocity_

rclcpp::Duration	costmap_update_timeout_

geometry_msgs::msg::PoseStamped	end_pose_

rclcpp::Time	last_valid_cmd_time_

nav_msgs::msg::Path	current_path_

Protected Attributes inherited from nav2_util::LifecycleNode
std::unique_ptr< rclcpp::PreShutdownCallbackHandle >	rcl_preshutdown_cb_handle_ {nullptr}

std::shared_ptr< bond::Bond >	bond_ {nullptr}

double	bond_heartbeat_period

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 49 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 37 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 426 of file controller_server.cpp.

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

Referenced by on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ dynamicParametersCallback()

rcl_interfaces::msg::SetParametersResult nav2_controller::ControllerServer::dynamicParametersCallback ( std::vector< rclcpp::Parameter > parameters )

protected

Callback executed when a parameter change is detected.

Parameters

event ParameterEvent message

Definition at line 831 of file controller_server.cpp.

Referenced by on_activate().

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

Referenced by computeControl(), 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 374 of file controller_server.cpp.

Referenced by computeControl(), 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 400 of file controller_server.cpp.

Referenced by computeControl(), 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 812 of file controller_server.cpp.

Referenced by computeAndPublishVelocity(), and isGoalReached().

Here is the caller graph for this function:

◆ getThresholdedTwist()

nav_2d_msgs::msg::Twist2D nav2_controller::ControllerServer::getThresholdedTwist ( const nav_2d_msgs::msg::Twist2D & twist )

inlineprotected

get the thresholded Twist

Parameters

Twist The current Twist from odometry

Returns: Twist Twist after thresholds applied

Definition at line 211 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 201 of file controller_server.hpp.

Referenced by getThresholdedTwist().

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 790 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_util::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 255 of file controller_server.cpp.

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

Here is the call graph for this function:

◆ on_cleanup()

nav2_util::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 314 of file controller_server.cpp.

Referenced by on_configure().

Here is the caller graph for this function:

◆ on_configure()

nav2_util::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 83 of file controller_server.cpp.

References computeControl(), on_cleanup(), and nav2_util::LifecycleNode::shared_from_this().

Here is the call graph for this function:

◆ on_deactivate()

nav2_util::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 282 of file controller_server.cpp.

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

Here is the call graph for this function:

◆ on_shutdown()

nav2_util::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 342 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 752 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 596 of file controller_server.cpp.

Referenced by computeControl(), and updateGlobalPath().

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()

◆ dynamicParametersCallback()

◆ findControllerId()

◆ findGoalCheckerId()

◆ findProgressCheckerId()

◆ getRobotPose()

◆ getThresholdedTwist()

◆ getThresholdedVelocity()

◆ isGoalReached()

◆ on_activate()

◆ on_cleanup()

◆ on_configure()

◆ on_deactivate()

◆ on_shutdown()

◆ publishVelocity()

◆ setPlannerPath()