Class ControllerServer

Inheritance Relationships

Base Type

  • public nav2_util::LifecycleNode

Class Documentation

class ControllerServer : public nav2_util::LifecycleNode

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

Public Types

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

Public Functions

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

Constructor for nav2_controller::ControllerServer.

Parameters:

options – Additional options to control creation of the node.

~ControllerServer()

Destructor for nav2_controller::ControllerServer.

Protected Types

using Action = nav2_msgs::action::FollowPath
using ActionServer = nav2_util::SimpleActionServer<Action>

Protected Functions

nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State &state) override

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

Throws:

pluginlib::PluginlibException – When failed to initialize controller plugin

Returns:

Success or Failure

nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State &state) override

Activates member variables.

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

Parameters:

state – LifeCycle Node’s state

Returns:

Success or Failure

nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &state) override

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

nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &state) override

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

nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State &state) override

Called when in Shutdown state.

Parameters:

state – LifeCycle Node’s state

Returns:

Success or Failure

void computeControl()

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.

Throws:

nav2_core::PlannerException –

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

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

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

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

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

Assigns path to controller.

Parameters:

path – Path received from action server

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.

Parameters:

velocity – Twist velocity to be published

void publishZeroVelocity()

Calls velocity publisher to publish zero velocity.

bool isGoalReached()

Checks if goal is reached.

Returns:

true or false

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

Obtain current pose of the robot.

Parameters:

pose – To store current pose of the robot

Returns:

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

inline double getThresholdedVelocity(double velocity, double threshold)

get the thresholded velocity

Parameters:

  • velocity – The current velocity from odometry

  • threshold – The minimum velocity to return non-zero

Returns:

double velocity value

inline nav_2d_msgs::msg::Twist2D getThresholdedTwist(const nav_2d_msgs::msg::Twist2D &twist)

get the thresholded Twist

Parameters:

Twist – The current Twist from odometry

Returns:

Twist Twist after thresholds applied

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

Callback executed when a parameter change is detected.

Parameters:

event – ParameterEvent message

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_
rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr vel_publisher_
rclcpp::Subscription<nav2_msgs::msg::SpeedLimit>::SharedPtr speed_limit_sub_
pluginlib::ClassLoader<nav2_core::ProgressChecker> progress_checker_loader_
nav2_core::ProgressChecker::Ptr progress_checker_
std::string default_progress_checker_id_
std::string default_progress_checker_type_
std::string progress_checker_id_
std::string progress_checker_type_
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_
geometry_msgs::msg::PoseStamped end_pose_
rclcpp::Time last_valid_cmd_time_
nav_msgs::msg::Path current_path_