controller_manager.hpp

// Copyright 2020 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
#ifndef CONTROLLER_MANAGER__CONTROLLER_MANAGER_HPP_
#define CONTROLLER_MANAGER__CONTROLLER_MANAGER_HPP_
 
#include <map>
#include <memory>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>
 
#include "controller_interface/chainable_controller_interface.hpp"
#include "controller_interface/controller_interface.hpp"
#include "controller_interface/controller_interface_base.hpp"
 
#include "controller_manager/controller_spec.hpp"
#include "controller_manager_msgs/msg/controller_manager_activity.hpp"
#include "controller_manager_msgs/srv/configure_controller.hpp"
#include "controller_manager_msgs/srv/list_controller_types.hpp"
#include "controller_manager_msgs/srv/list_controllers.hpp"
#include "controller_manager_msgs/srv/list_hardware_components.hpp"
#include "controller_manager_msgs/srv/list_hardware_interfaces.hpp"
#include "controller_manager_msgs/srv/load_controller.hpp"
#include "controller_manager_msgs/srv/reload_controller_libraries.hpp"
#include "controller_manager_msgs/srv/set_hardware_component_state.hpp"
#include "controller_manager_msgs/srv/switch_controller.hpp"
#include "controller_manager_msgs/srv/unload_controller.hpp"
 
#include "diagnostic_updater/diagnostic_updater.hpp"
#include "hardware_interface/helpers.hpp"
#include "hardware_interface/resource_manager.hpp"
 
#include "pluginlib/class_loader.hpp"
 
#include "rclcpp/executor.hpp"
#include "rclcpp/node.hpp"
#include "std_msgs/msg/string.hpp"
 
namespace controller_manager
{
class ParamListener;
struct Params;
using ControllersListIterator = std::vector<controller_manager::ControllerSpec>::const_iterator;
 
rclcpp::NodeOptions get_cm_node_options();
 
class ControllerManager : public rclcpp::Node
{
public:
  static constexpr bool kWaitForAllResources = false;
  static constexpr auto kInfiniteTimeout = 0;
 
  ControllerManager(
    std::unique_ptr<hardware_interface::ResourceManager> resource_manager,
    std::shared_ptr<rclcpp::Executor> executor,
    const std::string & manager_node_name = "controller_manager",
    const std::string & node_namespace = "",
    const rclcpp::NodeOptions & options = get_cm_node_options());
 
  ControllerManager(
    std::shared_ptr<rclcpp::Executor> executor,
    const std::string & manager_node_name = "controller_manager",
    const std::string & node_namespace = "",
    const rclcpp::NodeOptions & options = get_cm_node_options());
 
  ControllerManager(
    std::shared_ptr<rclcpp::Executor> executor, const std::string & urdf,
    bool activate_all_hw_components, const std::string & manager_node_name = "controller_manager",
    const std::string & node_namespace = "",
    const rclcpp::NodeOptions & options = get_cm_node_options());
 
  virtual ~ControllerManager();
 
 
  bool shutdown_controllers();
 
  void robot_description_callback(const std_msgs::msg::String & msg);
 
  void init_resource_manager(const std::string & robot_description);
 
  controller_interface::ControllerInterfaceBaseSharedPtr load_controller(
    const std::string & controller_name, const std::string & controller_type);
 
 
  controller_interface::ControllerInterfaceBaseSharedPtr load_controller(
    const std::string & controller_name);
 
  controller_interface::return_type unload_controller(const std::string & controller_name);
 
  std::vector<ControllerSpec> get_loaded_controllers() const;
 
  template <
    typename T, typename std::enable_if<
                  std::is_convertible<T *, controller_interface::ControllerInterfaceBase *>::value,
                  T>::type * = nullptr>
  controller_interface::ControllerInterfaceBaseSharedPtr add_controller(
    std::shared_ptr<T> controller, const std::string & controller_name,
    const std::string & controller_type)
  {
    ControllerSpec controller_spec;
    controller_spec.c = controller;
    controller_spec.info.name = controller_name;
    controller_spec.info.type = controller_type;
    controller_spec.last_update_cycle_time = std::make_shared<rclcpp::Time>(0);
    return add_controller_impl(controller_spec);
  }
 
  controller_interface::ControllerInterfaceBaseSharedPtr add_controller(
    const ControllerSpec & controller_spec)
  {
    return add_controller_impl(controller_spec);
  }
 
 
  controller_interface::return_type configure_controller(const std::string & controller_name);
 
 
  controller_interface::return_type switch_controller(
    const std::vector<std::string> & activate_controllers,
    const std::vector<std::string> & deactivate_controllers, int strictness,
    bool activate_asap = kWaitForAllResources,
    const rclcpp::Duration & timeout = rclcpp::Duration::from_nanoseconds(kInfiniteTimeout));
 
 
  controller_interface::return_type switch_controller_cb(
    const std::vector<std::string> & activate_controllers,
    const std::vector<std::string> & deactivate_controllers, int strictness, bool activate_asap,
    const rclcpp::Duration & timeout, std::string & message);
 
 
  void read(const rclcpp::Time & time, const rclcpp::Duration & period);
 
 
  controller_interface::return_type update(
    const rclcpp::Time & time, const rclcpp::Duration & period);
 
 
  void write(const rclcpp::Time & time, const rclcpp::Duration & period);
 
 
  // TODO(anyone): Due to issues with the MutliThreadedExecutor, this control loop does not rely on
  // the executor (see issue #260).
  // rclcpp::CallbackGroup::SharedPtr deterministic_callback_group_;
 
 
  bool is_resource_manager_initialized() const
  {
    return resource_manager_ && resource_manager_->are_components_initialized();
  }
 
 
  unsigned int get_update_rate() const;
 
 
  rclcpp::Clock::SharedPtr get_trigger_clock() const;
 
protected:
  void init_services();
 
  controller_interface::ControllerInterfaceBaseSharedPtr add_controller_impl(
    const ControllerSpec & controller);
 
  void manage_switch();
 
 
  void deactivate_controllers(
    const std::vector<ControllerSpec> & rt_controller_list,
    const std::vector<std::string> & controllers_to_deactivate);
 
  void switch_chained_mode(
    const std::vector<std::string> & chained_mode_switch_list, bool to_chained_mode);
 
 
  void activate_controllers(
    const std::vector<ControllerSpec> & rt_controller_list,
    const std::vector<std::string> & controllers_to_activate, int strictness);
 
  void list_controllers_srv_cb(
    const std::shared_ptr<controller_manager_msgs::srv::ListControllers::Request> request,
    std::shared_ptr<controller_manager_msgs::srv::ListControllers::Response> response);
 
  void list_hardware_interfaces_srv_cb(
    const std::shared_ptr<controller_manager_msgs::srv::ListHardwareInterfaces::Request> request,
    std::shared_ptr<controller_manager_msgs::srv::ListHardwareInterfaces::Response> response);
 
  void load_controller_service_cb(
    const std::shared_ptr<controller_manager_msgs::srv::LoadController::Request> request,
    std::shared_ptr<controller_manager_msgs::srv::LoadController::Response> response);
 
  void configure_controller_service_cb(
    const std::shared_ptr<controller_manager_msgs::srv::ConfigureController::Request> request,
    std::shared_ptr<controller_manager_msgs::srv::ConfigureController::Response> response);
 
  void reload_controller_libraries_service_cb(
    const std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Request> request,
    std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Response> response);
 
  void switch_controller_service_cb(
    const std::shared_ptr<controller_manager_msgs::srv::SwitchController::Request> request,
    std::shared_ptr<controller_manager_msgs::srv::SwitchController::Response> response);
 
  void unload_controller_service_cb(
    const std::shared_ptr<controller_manager_msgs::srv::UnloadController::Request> request,
    std::shared_ptr<controller_manager_msgs::srv::UnloadController::Response> response);
 
  void list_controller_types_srv_cb(
    const std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Request> request,
    std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Response> response);
 
  void list_hardware_components_srv_cb(
    const std::shared_ptr<controller_manager_msgs::srv::ListHardwareComponents::Request> request,
    std::shared_ptr<controller_manager_msgs::srv::ListHardwareComponents::Response> response);
 
  void set_hardware_component_state_srv_cb(
    const std::shared_ptr<controller_manager_msgs::srv::SetHardwareComponentState::Request> request,
    std::shared_ptr<controller_manager_msgs::srv::SetHardwareComponentState::Response> response);
 
  // Per controller update rate support
  unsigned int update_loop_counter_ = 0;
  unsigned int update_rate_;
  std::vector<std::vector<std::string>> chained_controllers_configuration_;
 
  std::unique_ptr<hardware_interface::ResourceManager> resource_manager_;
 
private:
  std::vector<std::string> get_controller_names();
  std::pair<std::string, std::string> split_command_interface(
    const std::string & command_interface);
  void init_controller_manager();
 
  void initialize_parameters();
 
  controller_interface::return_type cleanup_controller(
    const controller_manager::ControllerSpec & controller);
 
  void shutdown_controller(const controller_manager::ControllerSpec & controller) const;
 
  void clear_requests();
 
  void perform_hardware_command_mode_change(
    const std::vector<ControllerSpec> & rt_controller_list,
    const std::vector<std::string> & activate_controllers_list,
    const std::vector<std::string> & deactivate_controllers_list,
    const std::string & rt_cycle_name);
 
  void propagate_deactivation_of_chained_mode(const std::vector<ControllerSpec> & controllers);
 
 
  controller_interface::return_type check_following_controllers_for_activate(
    const std::vector<ControllerSpec> & controllers, int strictness,
    const ControllersListIterator controller_it, std::string & message);
 
 
  controller_interface::return_type check_preceding_controllers_for_deactivate(
    const std::vector<ControllerSpec> & controllers, int strictness,
    const ControllersListIterator controller_it, std::string & message);
 
 
  controller_interface::return_type check_fallback_controllers_state_pre_activation(
    const std::vector<ControllerSpec> & controllers, const ControllersListIterator controller_it,
    std::string & message);
 
  controller_interface::return_type check_for_interfaces_availability_to_activate(
    const std::vector<ControllerSpec> & controllers, const std::vector<std::string> activation_list,
    const std::vector<std::string> deactivation_list, std::string & message);
 
  void update_list_with_controller_chain(
    const std::string & ctrl_name, std::vector<std::string>::iterator controller_iterator,
    bool append_to_controller);
 
  void build_controllers_topology_info(const std::vector<ControllerSpec> & controllers);
 
  void publish_activity();
 
  void controller_activity_diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat);
 
  void hardware_components_diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat);
 
  void controller_manager_diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat);
 
  rclcpp::NodeOptions determine_controller_node_options(const ControllerSpec & controller) const;
 
  void cleanup_controller_exported_interfaces(const ControllerSpec & controller);
 
  std::shared_ptr<controller_manager::ParamListener> cm_param_listener_;
  std::shared_ptr<controller_manager::Params> params_;
  diagnostic_updater::Updater diagnostics_updater_;
 
  std::shared_ptr<rclcpp::Executor> executor_;
 
  std::shared_ptr<pluginlib::ClassLoader<controller_interface::ControllerInterface>> loader_;
  std::shared_ptr<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>
    chainable_loader_;
 
 
  rclcpp::CallbackGroup::SharedPtr best_effort_callback_group_;
 
  class RTControllerListWrapper
  {
    // *INDENT-OFF*
  public:
    // *INDENT-ON*
 
    std::vector<ControllerSpec> & update_and_get_used_by_rt_list();
 
    std::vector<ControllerSpec> & get_unused_list(
      const std::lock_guard<std::recursive_mutex> & guard);
 
 
    const std::vector<ControllerSpec> & get_updated_list(
      const std::lock_guard<std::recursive_mutex> & guard) const;
 
    void switch_updated_list(const std::lock_guard<std::recursive_mutex> & guard);
 
 
    void set_on_switch_callback(std::function<void()> callback);
 
    // Mutex protecting the controllers list
    // must be acquired before using any list other than the "used by rt"
    mutable std::recursive_mutex controllers_lock_;
 
    // *INDENT-OFF*
  private:
    // *INDENT-ON*
 
    int get_other_list(int index) const;
 
    void wait_until_rt_not_using(
      int index, std::chrono::microseconds sleep_delay = std::chrono::microseconds(200)) const;
 
    std::vector<ControllerSpec> controllers_lists_[2];
    int updated_controllers_index_ = 0;
    int used_by_realtime_controllers_index_ = -1;
    std::function<void()> on_switch_callback_ = nullptr;
  };
 
  bool use_sim_time_;
  rclcpp::Clock::SharedPtr trigger_clock_ = nullptr;
  std::unique_ptr<rclcpp::PreShutdownCallbackHandle> preshutdown_cb_handle_{nullptr};
  RTControllerListWrapper rt_controllers_wrapper_;
  std::unordered_map<std::string, ControllerChainSpec> controller_chain_spec_;
  std::vector<std::string> ordered_controllers_names_;
  std::mutex services_lock_;
  rclcpp::Publisher<controller_manager_msgs::msg::ControllerManagerActivity>::SharedPtr
    controller_manager_activity_publisher_;
  rclcpp::Service<controller_manager_msgs::srv::ListControllers>::SharedPtr
    list_controllers_service_;
  rclcpp::Service<controller_manager_msgs::srv::ListControllerTypes>::SharedPtr
    list_controller_types_service_;
  rclcpp::Service<controller_manager_msgs::srv::LoadController>::SharedPtr load_controller_service_;
  rclcpp::Service<controller_manager_msgs::srv::ConfigureController>::SharedPtr
    configure_controller_service_;
  rclcpp::Service<controller_manager_msgs::srv::ReloadControllerLibraries>::SharedPtr
    reload_controller_libraries_service_;
  rclcpp::Service<controller_manager_msgs::srv::SwitchController>::SharedPtr
    switch_controller_service_;
  rclcpp::Service<controller_manager_msgs::srv::UnloadController>::SharedPtr
    unload_controller_service_;
 
  rclcpp::Service<controller_manager_msgs::srv::ListHardwareComponents>::SharedPtr
    list_hardware_components_service_;
  rclcpp::Service<controller_manager_msgs::srv::ListHardwareInterfaces>::SharedPtr
    list_hardware_interfaces_service_;
  rclcpp::Service<controller_manager_msgs::srv::SetHardwareComponentState>::SharedPtr
    set_hardware_component_state_service_;
 
  std::map<std::string, std::vector<std::string>> controller_chained_reference_interfaces_cache_;
  std::map<std::string, std::vector<std::string>> controller_chained_state_interfaces_cache_;
 
  rclcpp::NodeOptions cm_node_options_;
  std::string robot_description_;
  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr robot_description_subscription_;
  rclcpp::TimerBase::SharedPtr robot_description_notification_timer_;
 
  struct ControllerManagerExecutionTime
  {
    double read_time = 0.0;
    double update_time = 0.0;
    double write_time = 0.0;
    double switch_time = 0.0;
    double total_time = 0.0;
    double switch_chained_mode_time = 0.0;
    double switch_perform_mode_time = 0.0;
    double deactivation_time = 0.0;
    double activation_time = 0.0;
  };
 
  ControllerManagerExecutionTime execution_time_;
 
  controller_manager::MovingAverageStatistics periodicity_stats_;
 
  struct SwitchParams
  {
    void reset()
    {
      do_switch = false;
      started = false;
      strictness = 0;
      activate_asap = false;
    }
 
    std::atomic_bool do_switch;
    bool started;
 
    // Switch options
    int strictness;
    std::atomic_bool activate_asap;
    std::chrono::nanoseconds timeout;
 
    // conditional variable and mutex to wait for the switch to complete
    std::condition_variable cv;
    std::mutex mutex;
 
    bool skip_cycle(const controller_manager::ControllerSpec & spec) const
    {
      const std::string controller_name = spec.info.name;
      return ros2_control::has_item(activate_request, controller_name) ||
             ros2_control::has_item(deactivate_request, controller_name) ||
             ros2_control::has_item(to_chained_mode_request, controller_name) ||
             ros2_control::has_item(from_chained_mode_request, controller_name);
    }
 
    // The controllers list to activate and deactivate
    std::vector<std::string> activate_request;
    std::vector<std::string> deactivate_request;
    std::vector<std::string> to_chained_mode_request;
    std::vector<std::string> from_chained_mode_request;
    std::vector<std::string> activate_command_interface_request;
    std::vector<std::string> deactivate_command_interface_request;
  };
 
  SwitchParams switch_params_;
 
  struct RTBufferVariables
  {
    RTBufferVariables()
    {
      deactivate_controllers_list.reserve(1000);
      activate_controllers_using_interfaces_list.reserve(1000);
      fallback_controllers_list.reserve(1000);
      interfaces_to_start.reserve(1000);
      interfaces_to_stop.reserve(1000);
      concatenated_string.reserve(5000);
    }
 
    const std::string & get_concatenated_string(
      const std::vector<std::string> & strings, bool clear_string = true)
    {
      if (clear_string)
      {
        concatenated_string.clear();
      }
      for (const auto & str : strings)
      {
        concatenated_string.append(str);
        concatenated_string.append(" ");
      }
      return concatenated_string;
    }
 
    std::vector<std::string> deactivate_controllers_list;
    std::vector<std::string> activate_controllers_using_interfaces_list;
    std::vector<std::string> fallback_controllers_list;
    std::vector<std::string> interfaces_to_start;
    std::vector<std::string> interfaces_to_stop;
    std::string concatenated_string;
  };
  RTBufferVariables rt_buffer_;
};
 
}  // namespace controller_manager
 
#endif  // CONTROLLER_MANAGER__CONTROLLER_MANAGER_HPP_