Merge branch 'master' into add/interface/mapping

saikishor · Jul 22, 2024 · bffd15e · bffd15e
2 parents c78489c + 89292e3
commit bffd15e
Show file tree

Hide file tree

Showing 87 changed files with 207 additions and 683 deletions.
diff --git a/controller_interface/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp
@@ -16,7 +16,6 @@
 #define CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_HPP_
 
 #include <memory>
-#include <string>
 #include <vector>
 
 #include "controller_interface/controller_interface_base.hpp"

diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp
@@ -26,7 +26,6 @@
 #include "hardware_interface/loaned_command_interface.hpp"
 #include "hardware_interface/loaned_state_interface.hpp"
 
-#include "rclcpp/rclcpp.hpp"
 #include "rclcpp/version.h"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 
@@ -277,14 +276,14 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
 protected:
   std::vector<hardware_interface::LoanedCommandInterface> command_interfaces_;
   std::vector<hardware_interface::LoanedStateInterface> state_interfaces_;
-  unsigned int update_rate_ = 0;
-  bool is_async_ = false;
-  std::string urdf_ = "";
 
 private:
   std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
   std::map<std::string, std::string> state_interfaces_remap_;
   std::map<std::string, std::string> command_interfaces_remap_;
+  unsigned int update_rate_ = 0;
+  bool is_async_ = false;
+  std::string urdf_ = "";
 };
 
 using ControllerInterfaceBaseSharedPtr = std::shared_ptr<ControllerInterfaceBase>;

diff --git a/controller_interface/include/semantic_components/range_sensor.hpp b/controller_interface/include/semantic_components/range_sensor.hpp
@@ -15,7 +15,6 @@
 #ifndef SEMANTIC_COMPONENTS__RANGE_SENSOR_HPP_
 #define SEMANTIC_COMPONENTS__RANGE_SENSOR_HPP_
 
-#include <limits>
 #include <string>
 #include <vector>
 

diff --git a/controller_interface/src/controller_interface.cpp b/controller_interface/src/controller_interface.cpp
@@ -14,14 +14,8 @@
 
 #include "controller_interface/controller_interface.hpp"
 
-#include <memory>
-#include <string>
-#include <utility>
 #include <vector>
 
-#include "hardware_interface/types/lifecycle_state_names.hpp"
-#include "lifecycle_msgs/msg/state.hpp"
-
 namespace controller_interface
 {
 ControllerInterface::ControllerInterface() : ControllerInterfaceBase() {}

diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp
@@ -19,7 +19,6 @@
 #include <utility>
 #include <vector>
 
-#include "hardware_interface/types/lifecycle_state_names.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
 
 namespace controller_interface

diff --git a/controller_interface/test/test_chainable_controller_interface.hpp b/controller_interface/test/test_chainable_controller_interface.hpp
@@ -15,14 +15,13 @@
 #ifndef TEST_CHAINABLE_CONTROLLER_INTERFACE_HPP_
 #define TEST_CHAINABLE_CONTROLLER_INTERFACE_HPP_
 
+#include <gmock/gmock.h>
+
 #include <string>
 #include <vector>
 
-#include "gmock/gmock.h"
-
 #include "controller_interface/chainable_controller_interface.hpp"
 #include "hardware_interface/handle.hpp"
-#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 
 constexpr char TEST_CONTROLLER_NAME[] = "testable_chainable_controller";
 constexpr double INTERFACE_VALUE = 1989.0;

diff --git a/controller_interface/test/test_controller_interface.hpp b/controller_interface/test/test_controller_interface.hpp
@@ -16,7 +16,6 @@
 #define TEST_CONTROLLER_INTERFACE_HPP_
 
 #include "controller_interface/controller_interface.hpp"
-#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 
 constexpr char TEST_CONTROLLER_NAME[] = "testable_controller_interface";
 

diff --git a/controller_interface/test/test_controller_with_options.cpp b/controller_interface/test/test_controller_with_options.cpp
@@ -17,7 +17,6 @@
 #include <gtest/gtest.h>
 #include <string>
 #include "ament_index_cpp/get_package_prefix.hpp"
-#include "rclcpp/rclcpp.hpp"
 
 class FriendControllerWithOptions : public controller_with_options::ControllerWithOptions
 {

diff --git a/controller_interface/test/test_controller_with_options.hpp b/controller_interface/test/test_controller_with_options.hpp
@@ -16,11 +16,9 @@
 #define TEST_CONTROLLER_WITH_OPTIONS_HPP_
 
 #include <map>
-#include <memory>
 #include <string>
 
 #include "controller_interface/controller_interface.hpp"
-#include "hardware_interface/types/lifecycle_state_names.hpp"
 
 namespace controller_with_options
 {

diff --git a/controller_interface/test/test_force_torque_sensor.hpp b/controller_interface/test/test_force_torque_sensor.hpp
@@ -19,12 +19,12 @@
 #ifndef TEST_FORCE_TORQUE_SENSOR_HPP_
 #define TEST_FORCE_TORQUE_SENSOR_HPP_
 
+#include <gmock/gmock.h>
+
 #include <memory>
 #include <string>
 #include <vector>
 
-#include "gmock/gmock.h"
-
 #include "semantic_components/force_torque_sensor.hpp"
 
 // implementing and friending so we can access member variables

diff --git a/controller_interface/test/test_imu_sensor.hpp b/controller_interface/test/test_imu_sensor.hpp
@@ -19,12 +19,12 @@
 #ifndef TEST_IMU_SENSOR_HPP_
 #define TEST_IMU_SENSOR_HPP_
 
+#include <gmock/gmock.h>
+
 #include <memory>
 #include <string>
 #include <vector>
 
-#include "gmock/gmock.h"
-
 #include "semantic_components/imu_sensor.hpp"
 
 // implementing and friending so we can access member variables

diff --git a/controller_interface/test/test_semantic_component_interface.hpp b/controller_interface/test/test_semantic_component_interface.hpp
@@ -19,11 +19,12 @@
 #ifndef TEST_SEMANTIC_COMPONENT_INTERFACE_HPP_
 #define TEST_SEMANTIC_COMPONENT_INTERFACE_HPP_
 
+#include <gmock/gmock.h>
+
 #include <memory>
 #include <string>
 
 #include "geometry_msgs/msg/wrench.hpp"
-#include "gmock/gmock.h"
 #include "semantic_components/semantic_component_interface.hpp"
 
 // implementing and friending so we can access member variables

diff --git a/controller_manager/controller_manager/launch_utils.py b/controller_manager/controller_manager/launch_utils.py
@@ -30,7 +30,7 @@ def generate_load_controller_launch_description(
     'unload_on_kill' LaunchArguments and a Node action that runs the controller_manager
     spawner node to load and activate a controller
 
-    Examples # noqa: D416
+    Examples
     --------
       # Assuming the controller type and controller parameters are known to the controller_manager
       generate_load_controller_launch_description('joint_state_broadcaster')

diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp
@@ -18,7 +18,6 @@
 #include <map>
 #include <memory>
 #include <string>
-#include <tuple>
 #include <unordered_map>
 #include <utility>
 #include <vector>
@@ -42,17 +41,12 @@
 #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

diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp
@@ -19,11 +19,10 @@
 #ifndef CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_
 #define CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_
 
-#include <map>
 #include <memory>
 #include <string>
 #include <vector>
-#include "controller_interface/controller_interface.hpp"
+#include "controller_interface/controller_interface_base.hpp"
 #include "hardware_interface/controller_info.hpp"
 
 namespace controller_manager

diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
@@ -14,7 +14,6 @@
 
 #include "controller_manager/controller_manager.hpp"
 
-#include <list>
 #include <memory>
 #include <string>
 #include <utility>
@@ -24,7 +23,6 @@
 #include "controller_manager_msgs/msg/hardware_component_state.hpp"
 #include "hardware_interface/types/lifecycle_state_names.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
-#include "rclcpp/rclcpp.hpp"
 #include "rclcpp/version.h"
 #include "rclcpp_lifecycle/state.hpp"
 
@@ -220,20 +218,7 @@ ControllerManager::ControllerManager(
     std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(
       kControllerInterfaceNamespace, kChainableControllerInterfaceClassName))
 {
-  if (!get_parameter("update_rate", update_rate_))
-  {
-    RCLCPP_WARN(
-      get_logger(), "'update_rate' parameter not set, using default value of %d Hz.", update_rate_);
-  }
-
-  if (is_resource_manager_initialized())
-  {
-    init_services();
-  }
-
-  diagnostics_updater_.setHardwareID("ros2_control");
-  diagnostics_updater_.add(
-    "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback);
+  init_controller_manager();
 }
 
 ControllerManager::ControllerManager(
@@ -262,17 +247,20 @@ void ControllerManager::init_controller_manager()
       get_logger(), "'update_rate' parameter not set, using default value of %d Hz.", update_rate_);
   }
 
-  robot_description_notification_timer_ = create_wall_timer(
-    std::chrono::seconds(1),
-    [&]()
-    {
-      RCLCPP_WARN(
-        get_logger(), "Waiting for data on 'robot_description' topic to finish initialization");
-    });
   if (is_resource_manager_initialized())
   {
     init_services();
   }
+  else
+  {
+    robot_description_notification_timer_ = create_wall_timer(
+      std::chrono::seconds(1),
+      [&]()
+      {
+        RCLCPP_WARN(
+          get_logger(), "Waiting for data on 'robot_description' topic to finish initialization");
+      });
+  }
 
   // set QoS to transient local to get messages that have already been published
   // (if robot state publisher starts before controller manager)
@@ -305,6 +293,10 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String &
   init_resource_manager(robot_description_);
   if (is_resource_manager_initialized())
   {
+    RCLCPP_INFO(
+      get_logger(),
+      "Resource Manager has been successfully initialized. Starting Controller Manager "
+      "services...");
     init_services();
   }
 }
@@ -374,9 +366,6 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
       State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE);
     resource_manager_->set_component_state(component, active_state);
   }
-
-  // Init CM services first after the URDF is loaded an components are set
-  init_services();
   robot_description_notification_timer_->cancel();
 }
 

diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp
@@ -12,14 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <algorithm>
 #include <chrono>
 #include <memory>
 #include <string>
 #include <thread>
 
 #include "controller_manager/controller_manager.hpp"
-#include "rclcpp/rclcpp.hpp"
 #include "realtime_tools/thread_priority.hpp"
 
 using namespace std::chrono_literals;

diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp
@@ -22,22 +22,18 @@
 #include <memory>
 #include <string>
 #include <thread>
-#include <utility>
 #include <vector>
 
 #include "controller_interface/controller_interface.hpp"
 
 #include "controller_manager/controller_manager.hpp"
-#include "controller_manager_msgs/srv/list_hardware_interfaces.hpp"
 #include "controller_manager_msgs/srv/switch_controller.hpp"
 
-#include "rclcpp/rclcpp.hpp"
 #include "rclcpp/utilities.hpp"
 
 #include "std_msgs/msg/string.hpp"
 
 #include "ros2_control_test_assets/descriptions.hpp"
-#include "test_controller_failed_init/test_controller_failed_init.hpp"
 
 namespace
 {

diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp
@@ -15,7 +15,6 @@
 #include "test_controller.hpp"
 
 #include <limits>
-#include <memory>
 #include <string>
 
 #include "lifecycle_msgs/msg/state.hpp"

diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp
@@ -15,7 +15,6 @@
 #ifndef TEST_CONTROLLER__TEST_CONTROLLER_HPP_
 #define TEST_CONTROLLER__TEST_CONTROLLER_HPP_
 
-#include <memory>
 #include <string>
 #include <vector>
 

diff --git a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp
@@ -14,11 +14,6 @@
 
 #include "test_controller_failed_init.hpp"
 
-#include <memory>
-#include <string>
-
-#include "lifecycle_msgs/msg/transition.hpp"
-
 namespace test_controller_failed_init
 {
 TestControllerFailedInit::TestControllerFailedInit() : controller_interface::ControllerInterface()

diff --git a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp
@@ -15,10 +15,7 @@
 #ifndef TEST_CONTROLLER_FAILED_INIT__TEST_CONTROLLER_FAILED_INIT_HPP_
 #define TEST_CONTROLLER_FAILED_INIT__TEST_CONTROLLER_FAILED_INIT_HPP_
 
-#include <memory>
-#include <string>
-
-#include "controller_manager/controller_manager.hpp"
+#include "controller_interface/controller_interface.hpp"
 #include "controller_manager/visibility_control.h"
 
 namespace test_controller_failed_init

diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp
@@ -15,11 +15,9 @@
 #include <gmock/gmock.h>
 #include <memory>
 #include <string>
-#include <utility>
 #include <vector>
 
 #include "controller_manager/controller_manager.hpp"
-#include "controller_manager_msgs/srv/list_controllers.hpp"
 #include "controller_manager_test_common.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
 #include "test_controller/test_controller.hpp"

diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp
@@ -22,7 +22,6 @@
 #include "controller_manager_test_common.hpp"
 
 #include "controller_interface/controller_interface.hpp"
-#include "controller_manager/controller_manager.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_interfaces.hpp"