Ros2 control node

controller_interface/src/controller_interface.cpp

@@ -26,7 +26,10 @@ ControllerInterface::init(
   const std::string & controller_name)
 {
   robot_hardware_ = robot_hardware;
-  lifecycle_node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(controller_name);
+  lifecycle_node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(
+    controller_name,
+    rclcpp::NodeOptions().allow_undeclared_parameters(true).
+    automatically_declare_parameters_from_overrides(true));
 
   lifecycle_node_->register_on_configure(
     std::bind(&ControllerInterface::on_configure, this, std::placeholders::_1));

controller_manager/CMakeLists.txt

@@ -18,6 +18,8 @@ find_package(controller_manager_msgs REQUIRED)
 find_package(hardware_interface REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
+#TODO(anyone): remove this when using ResourceManager
+find_package(test_robot_hardware REQUIRED)
 
 add_library(controller_manager SHARED
   src/controller_manager.cpp
@@ -37,11 +39,27 @@ target_compile_definitions(controller_manager PRIVATE "CONTROLLER_MANAGER_BUILDI
 # prevent pluginlib from using boost
 target_compile_definitions(controller_manager PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
 
+add_executable(ros2_control_node src/ros2_control_node.cpp)
+target_include_directories(ros2_control_node PRIVATE include)
+target_link_libraries(ros2_control_node controller_manager)
+ament_target_dependencies(ros2_control_node
+  controller_interface
+  hardware_interface
+  rclcpp
+  #TODO(anyone): remove this when using ResourceManager
+  test_robot_hardware
+)
+
 install(TARGETS controller_manager
   RUNTIME DESTINATION bin
   LIBRARY DESTINATION lib
   ARCHIVE DESTINATION lib
 )
+
+install(TARGETS ros2_control_node
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
 install(DIRECTORY include/
   DESTINATION include
 )
@@ -116,6 +134,8 @@ ament_export_include_directories(
 ament_export_dependencies(
   controller_interface
   controller_manager_msgs
+  hardware_interface
   pluginlib
+  rclcpp
 )
 ament_package()

controller_manager/include/controller_manager/controller_manager.hpp

@@ -44,14 +44,14 @@ namespace controller_manager
 class ControllerManager : public rclcpp::Node
 {
 public:
-  static constexpr bool WAIT_FOR_ALL_RESOURCES = false;
-  static constexpr double INFINITE_TIMEOUT = 0.0;
+  static constexpr bool kWaitForAllResources = false;
+  static constexpr double kInfiniteTimeout = 0.0;
 
   CONTROLLER_MANAGER_PUBLIC
   ControllerManager(
     std::shared_ptr<hardware_interface::RobotHardware> hw,
     std::shared_ptr<rclcpp::Executor> executor,
-    const std::string & name = "controller_manager");
+    const std::string & manager_node_name = "controller_manager");
 
   CONTROLLER_MANAGER_PUBLIC
   virtual
@@ -104,13 +104,20 @@ class ControllerManager : public rclcpp::Node
     const std::vector<std::string> & start_controllers,
     const std::vector<std::string> & stop_controllers,
     int strictness,
-    bool start_asap = WAIT_FOR_ALL_RESOURCES,
-    const rclcpp::Duration & timeout = rclcpp::Duration(INFINITE_TIMEOUT));
+    bool start_asap = kWaitForAllResources,
+    const rclcpp::Duration & timeout = rclcpp::Duration(kInfiniteTimeout));
 
   CONTROLLER_MANAGER_PUBLIC
   controller_interface::return_type
   update();
 
+  /// Deterministic (real-time safe) callback group, e.g., update function.
+  /**
+   * Deterministic (real-time safe) callback group for the update function. Default behavior
+   * is read hardware, update controller and finally write new values to the hardware.
+   */
+  rclcpp::CallbackGroup::SharedPtr deterministic_callback_group_;
+
 protected:
   CONTROLLER_MANAGER_PUBLIC
   controller_interface::ControllerInterfaceSharedPtr
@@ -165,6 +172,13 @@ class ControllerManager : public rclcpp::Node
   std::shared_ptr<rclcpp::Executor> executor_;
   std::shared_ptr<pluginlib::ClassLoader<controller_interface::ControllerInterface>> loader_;
 
+  /// Best effort (non real-time safe) callback group, e.g., service callbacks.
+  /**
+   * Best effort (non real-time safe) callback group for callbacks that can possibly break
+   * real-time requirements, for example, service callbacks.
+   */
+  rclcpp::CallbackGroup::SharedPtr best_effort_callback_group_;
+
   /**
    * @brief The RTControllerListWrapper class wraps a double-buffered list of controllers
    * to avoid needing to lock the real-time thread when switching controllers in

controller_manager/src/controller_manager.cpp

@@ -49,6 +49,7 @@ rclcpp::NodeOptions get_cm_node_options()
   rclcpp::NodeOptions node_options;
   // Required for getting types of controllers to be loaded via service call
   node_options.allow_undeclared_parameters(true);
+  node_options.automatically_declare_parameters_from_overrides(true);
   return node_options;
 }
 
@@ -62,33 +63,52 @@ ControllerManager::ControllerManager(
   loader_(std::make_shared<pluginlib::ClassLoader<controller_interface::ControllerInterface>>(
       kControllerInterfaceName, kControllerInterface))
 {
+  deterministic_callback_group_ = create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+  best_effort_callback_group_ = create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
   using namespace std::placeholders;
   list_controllers_service_ = create_service<controller_manager_msgs::srv::ListControllers>(
-    "~/list_controllers", std::bind(
-      &ControllerManager::list_controllers_srv_cb, this, _1,
-      _2));
+    "~/list_controllers",
+    std::bind(&ControllerManager::list_controllers_srv_cb, this, _1, _2),
+    rmw_qos_profile_services_default,
+    best_effort_callback_group_);
   list_controller_types_service_ =
     create_service<controller_manager_msgs::srv::ListControllerTypes>(
-    "~/list_controller_types", std::bind(
-      &ControllerManager::list_controller_types_srv_cb, this, _1,
-      _2));
+    "~/list_controller_types",
+    std::bind(&ControllerManager::list_controller_types_srv_cb, this, _1, _2),
+    rmw_qos_profile_services_default,
+    best_effort_callback_group_);
   load_controller_service_ = create_service<controller_manager_msgs::srv::LoadController>(
-    "~/load_controller", std::bind(
-      &ControllerManager::load_controller_service_cb, this, _1,
-      _2));
+    "~/load_controller",
+    std::bind(&ControllerManager::load_controller_service_cb, this, _1, _2),
+    rmw_qos_profile_services_default,
+    best_effort_callback_group_);
   reload_controller_libraries_service_ =
     create_service<controller_manager_msgs::srv::ReloadControllerLibraries>(
-    "~/reload_controller_libraries", std::bind(
-      &ControllerManager::reload_controller_libraries_service_cb, this, _1,
-      _2));
+    "~/reload_controller_libraries",
+    std::bind(&ControllerManager::reload_controller_libraries_service_cb, this, _1, _2),
+    rmw_qos_profile_services_default,
+    best_effort_callback_group_);
   switch_controller_service_ = create_service<controller_manager_msgs::srv::SwitchController>(
-    "~/switch_controller", std::bind(
-      &ControllerManager::switch_controller_service_cb, this, _1,
-      _2));
+    "~/switch_controller",
+    std::bind(&ControllerManager::switch_controller_service_cb, this, _1, _2),
+    rmw_qos_profile_services_default,
+    best_effort_callback_group_);
   unload_controller_service_ = create_service<controller_manager_msgs::srv::UnloadController>(
-    "~/unload_controller", std::bind(
-      &ControllerManager::unload_controller_service_cb, this, _1,
-      _2));
+    "~/unload_controller",
+    std::bind(&ControllerManager::unload_controller_service_cb, this, _1, _2),
+    rmw_qos_profile_services_default,
+    best_effort_callback_group_);
+
+  // TODO(all): Should we declare paramters? #168 - for now yes because of the tests
+  declare_parameter("robot_description", "");
+  // load robot_description parameter
+  std::string robot_description;
+  if (!get_parameter("robot_description", robot_description)) {
+    throw std::runtime_error("No robot_description parameter found");
+  }
 }
 
 controller_interface::ControllerInterfaceSharedPtr ControllerManager::load_controller(
@@ -811,8 +831,7 @@ std::vector<std::string> ControllerManager::get_controller_names()
   return names;
 }
 
-controller_interface::return_type
-ControllerManager::update()
+controller_interface::return_type ControllerManager::update()
 {
   std::vector<ControllerSpec> & rt_controller_list =
     rt_controllers_wrapper_.update_and_get_used_by_rt_list();
@@ -843,8 +862,7 @@ ControllerManager::RTControllerListWrapper::update_and_get_used_by_rt_list()
   return controllers_lists_[used_by_realtime_controllers_index_];
 }
 
-std::vector<ControllerSpec> &
-ControllerManager::RTControllerListWrapper::get_unused_list(
+std::vector<ControllerSpec> & ControllerManager::RTControllerListWrapper::get_unused_list(
   const std::lock_guard<std::recursive_mutex> &)
 {
   assert(controllers_lock_.try_lock());
@@ -881,9 +899,7 @@ int ControllerManager::RTControllerListWrapper::get_other_list(int index) const
 }
 
 void ControllerManager::RTControllerListWrapper::wait_until_rt_not_using(
-  int index,
-  std::chrono::microseconds sleep_period)
-const
+  int index, std::chrono::microseconds sleep_period) const
 {
   while (used_by_realtime_controllers_index_ == index) {
     if (!rclcpp::ok()) {

controller_manager/src/ros2_control_node.cpp

@@ -0,0 +1,61 @@
+// Copyright 2020 ROS2-Control Development Team
+//
+// 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.
+
+#include <memory>
+#include <string>
+
+#include "controller_manager/controller_manager.hpp"
+#include "hardware_interface/robot_hardware.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "test_robot_hardware/test_robot_hardware.hpp"
+
+using namespace std::chrono_literals;
+
+constexpr const auto kLoggerName = "ros2_control_node";
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+
+  std::shared_ptr<rclcpp::Executor> executor =
+    std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
+  std::string manager_node_name = "controller_manager";
+  rclcpp::TimerBase::SharedPtr timer;
+
+  auto cm = std::make_shared<controller_manager::ControllerManager>(
+    // TODO(anyone): remove robot_hw when ResourceManager is added
+    // since RobotHW is not a plugin we had to take some type of robot
+    std::make_shared<test_robot_hardware::TestRobotHardware>(),
+    executor,
+    manager_node_name);
+
+  // Declare default controller manager rate of 100Hz
+  cm->declare_parameter("update_rate", 100);
+  // load controller_manager update time parameter
+  int update_rate;
+  if (!cm->get_parameter("update_rate", update_rate)) {
+    throw std::runtime_error("update_rate parameter not existing or empty");
+  }
+  RCLCPP_INFO(rclcpp::get_logger(kLoggerName), "update rate is %d Hz", update_rate);
+
+  timer = cm->create_wall_timer(
+    std::chrono::milliseconds(1000 / update_rate),
+    std::bind(&controller_manager::ControllerManager::update, cm.get()),
+    cm->deterministic_callback_group_);
+
+  executor->add_node(cm);
+  executor->spin();
+  rclcpp::shutdown();
+  return 0;
+}

controller_manager/test/test_controller_manager.cpp

@@ -29,7 +29,6 @@ TEST_F(TestControllerManager, controller_lifecycle) {
     robot_, executor_,
     "test_controller_manager");
 
-
   auto test_controller = std::make_shared<test_controller::TestController>();
   cm->add_controller(
     test_controller, test_controller::TEST_CONTROLLER_NAME,