Apply suggestions from code review and run precommit

Author: VitezGabriela

controller_manager/CMakeLists.txt

@@ -243,7 +243,7 @@ if(BUILD_TESTING)
     ${controller_manager_msgs_TARGETS}
   )
 
-  ament_add_gmock(test_controller_manager_with_resource_manager 
+  ament_add_gmock(test_controller_manager_with_resource_manager
   test/test_controller_manager_with_resource_manager.cpp
   )
   target_link_libraries(test_controller_manager_with_resource_manager

controller_manager/include/controller_manager/controller_manager.hpp

@@ -243,16 +243,15 @@ class ControllerManager : public rclcpp::Node
   rclcpp::Clock::SharedPtr get_trigger_clock() const;
 
   /**
- * @brief Return the robot description timer.
- * 
- * It can be used to determine whether the Controller Manager 
- * is currently waiting for a robot description to be published.
- * 
- * @return rclcpp::TimerBase::SharedPtr if the timer exists, nullptr otherwise
- */
-  rclcpp::TimerBase::SharedPtr get_robot_description_timer() const {
-      return robot_description_notification_timer_;
+   * @brief Returns true if we have a valid robot description, currently based on whether the timer
+   * for waiting on description is still on.
+   */
+  bool has_valid_robot_description() const
+  {
+    return robot_description_notification_timer_ &&
+           !robot_description_notification_timer_->is_canceled();
   }
+
 protected:
   void init_services();
 

controller_manager/src/controller_manager.cpp

@@ -548,20 +548,26 @@ void ControllerManager::init_controller_manager()
 
   if (!is_resource_manager_initialized())
   {
-    // fallback state
-    resource_manager_ = std::make_unique<hardware_interface::ResourceManager>(trigger_clock_, get_logger());
+    // The RM failed to initialize after receiving the robot description, or no description was
+    // received at all. This is a critical error. Don't finalize controller manager, instead keep
+    // waiting for robot description - fallback state
+    resource_manager_ =
+      std::make_unique<hardware_interface::ResourceManager>(trigger_clock_, get_logger());
     if (!robot_description_notification_timer_)
     {
       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");
+          RCLCPP_WARN(
+            get_logger(), "Waiting for data on 'robot_description' topic to finish initialization");
         });
     }
-  }else
+  }
+  else
   {
-    RCLCPP_INFO(get_logger(),
+    RCLCPP_INFO(
+      get_logger(),
       "Resource Manager successfully initialized. Starting Controller Manager services...");
     init_services();
   }
@@ -677,15 +683,15 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String &
       get_logger(),
       "ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description.");
     return;
-  }  
+  }
 
   init_resource_manager(robot_description_);
   if (!is_resource_manager_initialized())
   {
-    // The RM failed to init AFTER we received the description - a critical error.
-    // don't finalize controller manager, instead keep waiting for robot description - fallback state
-    resource_manager_ = std::make_unique<hardware_interface::ResourceManager>(trigger_clock_, get_logger());
-  } else 
+    resource_manager_ =
+      std::make_unique<hardware_interface::ResourceManager>(trigger_clock_, get_logger());
+  }
+  else
   {
     RCLCPP_INFO(
       get_logger(),
@@ -707,7 +713,7 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
   resource_manager_ = std::make_unique<hardware_interface::ResourceManager>(params, false);
 
   RCLCPP_INFO_EXPRESSION(
-  get_logger(), params_->enforce_command_limits, "Enforcing command limits is enabled...");
+    get_logger(), params_->enforce_command_limits, "Enforcing command limits is enabled...");
   if (params_->enforce_command_limits)
   {
     try
@@ -732,16 +738,16 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
       return;
     }
   }
-  catch (const std::exception &e)
+  catch (const std::exception & e)
   {
     // Other possible errors when loading components
     RCLCPP_ERROR(
-      get_logger(),
-      "Exception caught while loading and initializing components: %s", e.what());
+      get_logger(), "Exception caught while loading and initializing components: %s", e.what());
     return;
   }
-  resource_manager_->set_on_component_state_switch_callback(std::bind(&ControllerManager::publish_activity, this));
-        
+  resource_manager_->set_on_component_state_switch_callback(
+    std::bind(&ControllerManager::publish_activity, this));
+
   // Get all components and if they are not defined in parameters activate them automatically
   auto components_to_activate = resource_manager_->get_components_status();
 

controller_manager/test/test_controller_manager_with_resource_manager.cpp

@@ -1,15 +1,32 @@
+// Copyright 2022 Stogl Robotics Consulting UG (haftungsbeschränkt)
+//
+// 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 "test_controller_manager_with_resource_manager.hpp"
 
 std::shared_ptr<rclcpp::Node> ControllerManagerTest::node_ = nullptr;
-std::unique_ptr<hardware_interface::ResourceManager> ControllerManagerTest::test_resource_manager_ = nullptr;
-std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> ControllerManagerTest::executor_ = nullptr;
+std::unique_ptr<hardware_interface::ResourceManager> ControllerManagerTest::test_resource_manager_ =
+  nullptr;
+std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> ControllerManagerTest::executor_ =
+  nullptr;
 
 using LifecycleCallbackReturn =
   rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
 
 void ControllerManagerTest::SetUp()
 {
-  if (!rclcpp::ok()) {
+  if (!rclcpp::ok())
+  {
     rclcpp::init(0, nullptr);
   }
 
@@ -26,7 +43,8 @@ void ControllerManagerTest::TearDown()
   node_.reset();
   test_resource_manager_.reset();
   executor_.reset();
-  if (rclcpp::ok()) {
+  if (rclcpp::ok())
+  {
     rclcpp::shutdown();
   }
 }
@@ -37,13 +55,12 @@ TEST_F(ControllerManagerTest, robot_description_callback_handles_urdf_without_ha
 
   std_msgs::msg::String invalid_urdf_msg;
   invalid_urdf_msg.data = ros2_control_test_assets::invalid_urdf_without_hardware_plugin;
-  
+
   cm.robot_description_callback(invalid_urdf_msg);
-  
-  EXPECT_FALSE(cm.is_resource_manager_initialized());
 
-  EXPECT_FALSE(cm.get_robot_description_timer()->is_canceled());  
+  EXPECT_FALSE(cm.is_resource_manager_initialized());
 
+  EXPECT_TRUE(cm.has_valid_robot_description());
 }
 
 TEST_F(ControllerManagerTest, robot_description_callback_handles_invalid_urdf)
@@ -57,8 +74,7 @@ TEST_F(ControllerManagerTest, robot_description_callback_handles_invalid_urdf)
 
   EXPECT_FALSE(cm.is_resource_manager_initialized());
 
-  EXPECT_FALSE(cm.get_robot_description_timer()->is_canceled());  
-
+  EXPECT_TRUE(cm.has_valid_robot_description());
 }
 
 TEST_F(ControllerManagerTest, robot_description_callback_handles_empty_urdf)
@@ -72,7 +88,7 @@ TEST_F(ControllerManagerTest, robot_description_callback_handles_empty_urdf)
 
   EXPECT_FALSE(cm.is_resource_manager_initialized());
 
-  EXPECT_FALSE(cm.get_robot_description_timer()->is_canceled());  
+  EXPECT_TRUE(cm.has_valid_robot_description());
 }
 
 TEST_F(ControllerManagerTest, robot_description_callback_handles_wrong_plugins)
@@ -81,12 +97,12 @@ TEST_F(ControllerManagerTest, robot_description_callback_handles_wrong_plugins)
 
   std_msgs::msg::String invalid_urdf_msg;
   invalid_urdf_msg.data = ros2_control_test_assets::invalid_urdf_with_wrong_plugin;
-  
+
   cm.robot_description_callback(invalid_urdf_msg);
-  
+
   EXPECT_FALSE(cm.is_resource_manager_initialized());
 
-  EXPECT_FALSE(cm.get_robot_description_timer()->is_canceled());  
+  EXPECT_TRUE(cm.has_valid_robot_description());
 }
 
 TEST_F(ControllerManagerTest, robot_description_callback_handles_no_geometry)
@@ -95,28 +111,29 @@ TEST_F(ControllerManagerTest, robot_description_callback_handles_no_geometry)
 
   std_msgs::msg::String invalid_urdf_msg;
   invalid_urdf_msg.data = ros2_control_test_assets::invalid_urdf_no_geometry;
-  
+
   cm.robot_description_callback(invalid_urdf_msg);
-  
+
   EXPECT_FALSE(cm.is_resource_manager_initialized());
 
-  EXPECT_FALSE(cm.get_robot_description_timer()->is_canceled());  
+  EXPECT_TRUE(cm.has_valid_robot_description());
 }
 
 TEST_F(ControllerManagerTest, init_controller_manager_with_invalid_urdf)
 {
   const std::string invalid_urdf = ros2_control_test_assets::invalid_urdf_with_wrong_plugin;
 
-  TestControllerManager cm(executor_, invalid_urdf, false, "test_controller_manager", "", rclcpp::NodeOptions{});
+  TestControllerManager cm(
+    executor_, invalid_urdf, false, "test_controller_manager", "", rclcpp::NodeOptions{});
 
   EXPECT_FALSE(cm.is_resource_manager_initialized());
 
-  EXPECT_FALSE(cm.get_robot_description_timer()->is_canceled());  
+  EXPECT_TRUE(cm.has_valid_robot_description());
 }
 
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
   int result = RUN_ALL_TESTS();
   return result;
-}
+}

controller_manager/test/test_controller_manager_with_resource_manager.hpp

@@ -1,17 +1,32 @@
-#ifndef CONTROLLER_MANAGER_TEST_HPP_
-#define CONTROLLER_MANAGER_TEST_HPP_
+// Copyright 2022 Stogl Robotics Consulting UG (haftungsbeschränkt)
+//
+// 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 TEST_CONTROLLER_MANAGER_WITH_RESOURCE_MANAGER_HPP_
+#define TEST_CONTROLLER_MANAGER_WITH_RESOURCE_MANAGER_HPP_
 
 #include <memory>
 #include <string>
 
-#include "gtest/gtest.h"
-#include "rclcpp/node.hpp"
-#include "std_msgs/msg/string.hpp"
-#include "lifecycle_msgs/msg/state.hpp"
+#include <utility>
 #include "controller_manager/controller_manager.hpp"
+#include "gtest/gtest.h"
 #include "hardware_interface/resource_manager.hpp"
+#include "lifecycle_msgs/msg/state.hpp"
+#include "rclcpp/node.hpp"
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 #include "ros2_control_test_assets/descriptions.hpp"
+#include "std_msgs/msg/string.hpp"
 
 class TestControllerManager : public controller_manager::ControllerManager
 {
@@ -22,10 +37,10 @@ class TestControllerManager : public controller_manager::ControllerManager
   using ControllerManager::robot_description_callback;
 
   using ControllerManager::is_resource_manager_initialized;
-  
+
   using ControllerManager::resource_manager_;
 
-  using ControllerManager::get_robot_description_timer;  
+  using ControllerManager::has_valid_robot_description;
 };
 
 class ControllerManagerTest : public ::testing::Test
@@ -38,4 +53,4 @@ class ControllerManagerTest : public ::testing::Test
   virtual void TearDown();
 };
 
-#endif  
+#endif  // TEST_CONTROLLER_MANAGER_WITH_RESOURCE_MANAGER_HPP_

hardware_interface/src/resource_manager.cpp

@@ -1420,16 +1420,16 @@ ResourceManager::ResourceManager(
     params.allow_controller_activation_with_inactive_hardware;
   return_failed_hardware_names_on_return_deactivate_write_cycle_ =
     params.return_failed_hardware_names_on_return_deactivate_write_cycle_;
-  
- try
+
+  try
   {
     if (load && !load_and_initialize_components(params))
     {
       RCLCPP_WARN(
         get_logger(),
         "Could not load and initialize hardware. Please check previous output for more details. "
         "After you have corrected your URDF, try to publish robot description again.");
-      return;
+      throw std::runtime_error("Failed to load and initialize components");
     }
     if (params.activate_all)
     {
@@ -1441,14 +1441,12 @@ ResourceManager::ResourceManager(
       }
     }
   }
-  catch (const std::exception &e)
+  catch (const std::exception & e)
   {
     // Other possible errors when loading components
     RCLCPP_ERROR(
-      get_logger(),
-      "Exception caught while loading and initializing components: %s", e.what());
-    return;
-  }  
+      get_logger(), "Exception caught while loading and initializing components: %s", e.what());
+  }
 }
 
 bool ResourceManager::shutdown_components()

ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp

@@ -2171,7 +2171,7 @@ const auto diff_drive_robot_sdf =
 </sdf>
 )";
 
-const auto invalid_urdf_without_hardware_plugin = 
+const auto invalid_urdf_without_hardware_plugin =
   R"(
   <robot name="minimal_robot">
     <link name="base_link"/>
@@ -2188,8 +2188,8 @@ const auto invalid_urdf_without_hardware_plugin =
   </robot>
 )";
 
-const auto invalid_urdf_with_wrong_plugin = 
- R"(
+const auto invalid_urdf_with_wrong_plugin =
+  R"(
   <robot name="minimal_robot">
     <link name="base_link"/>
     <joint name="joint1" type="revolute">
@@ -2202,18 +2202,18 @@ const auto invalid_urdf_with_wrong_plugin =
     <link name="link1"/>
     <ros2_control name="default" type="system">
       <hardware>
-        <plugin>mock_components/NonExistentSystem</plugin>        
+        <plugin>mock_components/NonExistentSystem</plugin>
       </hardware>
     </ros2_control>
   </robot>
 )";
 
-const auto invalid_urdf_no_geometry = 
-R"(
+const auto invalid_urdf_no_geometry =
+  R"(
   <robot name="minimal_robot">
     <ros2_control name="default" type="system">
       <hardware>
-        <plugin>mock_components/NonExistentSystem</plugin>        
+        <plugin>mock_components/NonExistentSystem</plugin>
       </hardware>
     </ros2_control>
   </robot>