Program Listing for File controller_manager.hpp
↰ Return to documentation for file (include/controller_manager/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 <tuple>
#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/visibility_control.h"
#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/handle.hpp"
#include "hardware_interface/resource_manager.hpp"
#include "pluginlib/class_loader.hpp"
#include "rclcpp/executor.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/node_interfaces/node_logging_interface.hpp"
#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
#include "rclcpp/parameter.hpp"
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
namespace controller_manager
{
using ControllersListIterator = std::vector<controller_manager::ControllerSpec>::const_iterator;
CONTROLLER_MANAGER_PUBLIC rclcpp::NodeOptions get_cm_node_options();
class ControllerManager : public rclcpp::Node
{
public:
static constexpr bool kWaitForAllResources = false;
static constexpr auto kInfiniteTimeout = 0;
CONTROLLER_MANAGER_PUBLIC
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 & namespace_ = "",
const rclcpp::NodeOptions & options = get_cm_node_options());
CONTROLLER_MANAGER_PUBLIC
ControllerManager(
std::shared_ptr<rclcpp::Executor> executor,
const std::string & manager_node_name = "controller_manager",
const std::string & namespace_ = "",
const rclcpp::NodeOptions & options = get_cm_node_options());
CONTROLLER_MANAGER_PUBLIC
virtual ~ControllerManager() = default;
CONTROLLER_MANAGER_PUBLIC
void robot_description_callback(const std_msgs::msg::String & msg);
CONTROLLER_MANAGER_PUBLIC
void init_resource_manager(const std::string & robot_description);
CONTROLLER_MANAGER_PUBLIC
controller_interface::ControllerInterfaceBaseSharedPtr load_controller(
const std::string & controller_name, const std::string & controller_type);
CONTROLLER_MANAGER_PUBLIC
controller_interface::ControllerInterfaceBaseSharedPtr load_controller(
const std::string & controller_name);
CONTROLLER_MANAGER_PUBLIC
controller_interface::return_type unload_controller(const std::string & controller_name);
CONTROLLER_MANAGER_PUBLIC
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.next_update_cycle_time = std::make_shared<rclcpp::Time>(0);
return add_controller_impl(controller_spec);
}
CONTROLLER_MANAGER_PUBLIC
controller_interface::return_type configure_controller(const std::string & controller_name);
CONTROLLER_MANAGER_PUBLIC
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_MANAGER_PUBLIC
void read(const rclcpp::Time & time, const rclcpp::Duration & period);
CONTROLLER_MANAGER_PUBLIC
controller_interface::return_type update(
const rclcpp::Time & time, const rclcpp::Duration & period);
CONTROLLER_MANAGER_PUBLIC
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_;
// Per controller update rate support
CONTROLLER_MANAGER_PUBLIC
unsigned int get_update_rate() const;
protected:
CONTROLLER_MANAGER_PUBLIC
void init_services();
CONTROLLER_MANAGER_PUBLIC
controller_interface::ControllerInterfaceBaseSharedPtr add_controller_impl(
const ControllerSpec & controller);
CONTROLLER_MANAGER_PUBLIC
void manage_switch();
CONTROLLER_MANAGER_PUBLIC
void deactivate_controllers(
const std::vector<ControllerSpec> & rt_controller_list,
const std::vector<std::string> controllers_to_deactivate);
CONTROLLER_MANAGER_PUBLIC
void switch_chained_mode(
const std::vector<std::string> & chained_mode_switch_list, bool to_chained_mode);
CONTROLLER_MANAGER_PUBLIC
void activate_controllers(
const std::vector<ControllerSpec> & rt_controller_list,
const std::vector<std::string> controllers_to_activate);
CONTROLLER_MANAGER_PUBLIC
void activate_controllers_asap(
const std::vector<ControllerSpec> & rt_controller_list,
const std::vector<std::string> controllers_to_activate);
CONTROLLER_MANAGER_PUBLIC
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);
CONTROLLER_MANAGER_PUBLIC
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);
CONTROLLER_MANAGER_PUBLIC
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);
CONTROLLER_MANAGER_PUBLIC
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);
CONTROLLER_MANAGER_PUBLIC
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);
CONTROLLER_MANAGER_PUBLIC
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);
CONTROLLER_MANAGER_PUBLIC
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);
CONTROLLER_MANAGER_PUBLIC
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);
CONTROLLER_MANAGER_PUBLIC
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);
CONTROLLER_MANAGER_PUBLIC
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_ = 100;
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 subscribe_to_robot_description_topic();
void clear_requests();
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);
controller_interface::return_type check_preceeding_controllers_for_deactivate(
const std::vector<ControllerSpec> & controllers, int strictness,
const ControllersListIterator controller_it);
bool controller_sorting(
const ControllerSpec & ctrl_a, const ControllerSpec & ctrl_b,
const std::vector<controller_manager::ControllerSpec> & controllers);
void controller_activity_diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat);
rclcpp::NodeOptions determine_controller_node_options(const ControllerSpec & controller) const;
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);
// 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;
};
RTControllerListWrapper rt_controllers_wrapper_;
std::mutex services_lock_;
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::vector<std::string> activate_request_, deactivate_request_;
std::vector<std::string> to_chained_mode_request_, from_chained_mode_request_;
std::vector<std::string> activate_command_interface_request_,
deactivate_command_interface_request_;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr robot_description_subscription_;
struct SwitchParams
{
bool do_switch = {false};
bool started = {false};
rclcpp::Time init_time = {rclcpp::Time::max()};
// Switch options
int strictness = {0};
bool activate_asap = {false};
rclcpp::Duration timeout = rclcpp::Duration{0, 0};
};
SwitchParams switch_params_;
class ControllerThreadWrapper
{
public:
ControllerThreadWrapper(
std::shared_ptr<controller_interface::ControllerInterfaceBase> & controller,
int cm_update_rate)
: terminated_(false), controller_(controller), thread_{}, cm_update_rate_(cm_update_rate)
{
}
ControllerThreadWrapper(const ControllerThreadWrapper & t) = delete;
ControllerThreadWrapper(ControllerThreadWrapper && t) = default;
~ControllerThreadWrapper()
{
terminated_.store(true, std::memory_order_seq_cst);
if (thread_.joinable())
{
thread_.join();
}
}
void activate()
{
thread_ = std::thread(&ControllerThreadWrapper::call_controller_update, this);
}
void call_controller_update()
{
using TimePoint = std::chrono::system_clock::time_point;
unsigned int used_update_rate =
controller_->get_update_rate() == 0
? cm_update_rate_
: controller_
->get_update_rate(); // determines if the controller's or CM's update rate is used
while (!terminated_.load(std::memory_order_relaxed))
{
auto const period = std::chrono::nanoseconds(1'000'000'000 / used_update_rate);
TimePoint next_iteration_time =
TimePoint(std::chrono::nanoseconds(controller_->get_node()->now().nanoseconds()));
if (controller_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
{
// critical section, not implemented yet
}
next_iteration_time += period;
std::this_thread::sleep_until(next_iteration_time);
}
}
private:
std::atomic<bool> terminated_;
std::shared_ptr<controller_interface::ControllerInterfaceBase> controller_;
std::thread thread_;
unsigned int cm_update_rate_;
};
std::unordered_map<std::string, std::unique_ptr<ControllerThreadWrapper>>
async_controller_threads_;
};
} // namespace controller_manager
#endif // CONTROLLER_MANAGER__CONTROLLER_MANAGER_HPP_