Merge branch 'master' into update/ros2controlcli

.pre-commit-config.yaml

@@ -50,13 +50,13 @@ repos:
       args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"]
 
   - repo: https://github.com/psf/black
-    rev: 24.4.2
+    rev: 24.8.0
     hooks:
       - id: black
         args: ["--line-length=99"]
 
   - repo: https://github.com/pycqa/flake8
-    rev: 7.1.0
+    rev: 7.1.1
     hooks:
     - id: flake8
       args: ["--extend-ignore=E501"]
@@ -133,7 +133,7 @@ repos:
         exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$
 
   - repo: https://github.com/python-jsonschema/check-jsonschema
-    rev: 0.29.1
+    rev: 0.29.2
     hooks:
       - id: check-github-workflows
         args: ["--verbose"]

controller_interface/include/controller_interface/async_controller.hpp

@@ -82,7 +82,8 @@ class AsyncControllerThread
       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)
+      if (
+        controller_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
       {
         auto const current_time = controller_->get_node()->now();
         auto const measured_period = current_time - previous_time;

controller_interface/include/controller_interface/controller_interface_base.hpp

@@ -147,7 +147,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
   std::shared_ptr<const rclcpp_lifecycle::LifecycleNode> get_node() const;
 
   CONTROLLER_INTERFACE_PUBLIC
-  const rclcpp_lifecycle::State & get_state() const;
+  const rclcpp_lifecycle::State & get_lifecycle_state() const;
 
   CONTROLLER_INTERFACE_PUBLIC
   unsigned int get_update_rate() const;

controller_interface/src/chainable_controller_interface.cpp

@@ -97,7 +97,7 @@ bool ChainableControllerInterface::set_chained_mode(bool chained_mode)
 {
   bool result = false;
 
-  if (get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+  if (get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
   {
     result = on_set_chained_mode(chained_mode);
 
@@ -112,7 +112,8 @@ bool ChainableControllerInterface::set_chained_mode(bool chained_mode)
       get_node()->get_logger(),
       "Can not change controller's chained mode because it is no in '%s' state. "
       "Current state is '%s'.",
-      hardware_interface::lifecycle_state_names::UNCONFIGURED, get_state().label().c_str());
+      hardware_interface::lifecycle_state_names::UNCONFIGURED,
+      get_lifecycle_state().label().c_str());
   }
 
   return result;

controller_interface/src/controller_interface_base.cpp

@@ -83,7 +83,7 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure()
   // Then we don't need to do state-machine related checks.
   //
   // Other solution is to add check into the LifecycleNode if a transition is valid to trigger
-  if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
+  if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
   {
     update_rate_ = static_cast<unsigned int>(get_node()->get_parameter("update_rate").as_int());
     is_async_ = get_node()->get_parameter("is_async").as_bool();
@@ -106,7 +106,7 @@ void ControllerInterfaceBase::release_interfaces()
   state_interfaces_.clear();
 }
 
-const rclcpp_lifecycle::State & ControllerInterfaceBase::get_state() const
+const rclcpp_lifecycle::State & ControllerInterfaceBase::get_lifecycle_state() const
 {
   return node_->get_current_state();
 }

controller_manager/controller_manager/spawner.py

@@ -234,7 +234,7 @@ def main(args=None):
 
             node.get_logger().info(
                 bcolors.OKGREEN
-                + "Configured and activated all the parsed controllers list!"
+                + f"Configured and activated all the parsed controllers list : {controller_names}!"
                 + bcolors.ENDC
             )
 
@@ -258,16 +258,25 @@ def main(args=None):
                     )
                     return 1
 
-                node.get_logger().info("Deactivated controller")
-
-            ret = unload_controller(node, controller_manager_name, controller_name)
-            if not ret.ok:
-                node.get_logger().error(
-                    bcolors.FAIL + "Failed to unload controller" + bcolors.ENDC
+                node.get_logger().info(
+                    f"Successfully deactivated controllers : {controller_names}"
                 )
-                return 1
 
-            node.get_logger().info("Unloaded controller")
+            unload_status = True
+            for controller_name in controller_names:
+                ret = unload_controller(node, controller_manager_name, controller_name)
+                if not ret.ok:
+                    unload_status = False
+                    node.get_logger().error(
+                        bcolors.FAIL
+                        + f"Failed to unload controller : {controller_name}"
+                        + bcolors.ENDC
+                    )
+
+            if unload_status:
+                node.get_logger().info(f"Successfully unloaded controllers : {controller_names}")
+            else:
+                return 1
         return 0
     except KeyboardInterrupt:
         pass

controller_manager/src/controller_manager.cpp

@@ -58,7 +58,8 @@ static const rmw_qos_profile_t qos_services = {
 
 inline bool is_controller_inactive(const controller_interface::ControllerInterfaceBase & controller)
 {
-  return controller.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE;
+  return controller.get_lifecycle_state().id() ==
+         lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE;
 }
 
 inline bool is_controller_inactive(
@@ -69,7 +70,7 @@ inline bool is_controller_inactive(
 
 inline bool is_controller_active(const controller_interface::ControllerInterfaceBase & controller)
 {
-  return controller.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE;
+  return controller.get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE;
 }
 
 inline bool is_controller_active(
@@ -466,13 +467,21 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c
       controller = chainable_loader_->createSharedInstance(controller_type);
     }
   }
-  catch (const pluginlib::CreateClassException & e)
+  catch (const std::exception & e)
   {
     RCLCPP_ERROR(
-      get_logger(), "Error happened during creation of controller '%s' with type '%s':\n%s",
+      get_logger(), "Caught exception while loading the controller '%s' of plugin type '%s':\n%s",
       controller_name.c_str(), controller_type.c_str(), e.what());
     return nullptr;
   }
+  catch (...)
+  {
+    RCLCPP_ERROR(
+      get_logger(),
+      "Caught unknown exception while loading the controller '%s' of plugin type '%s'",
+      controller_name.c_str(), controller_type.c_str());
+    throw;
+  }
 
   ControllerSpec controller_spec;
   controller_spec.c = controller;
@@ -659,7 +668,7 @@ controller_interface::return_type ControllerManager::configure_controller(
   }
   auto controller = found_it->c;
 
-  auto state = controller->get_state();
+  auto state = controller->get_lifecycle_state();
   if (
     state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE ||
     state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
@@ -670,7 +679,7 @@ controller_interface::return_type ControllerManager::configure_controller(
     return controller_interface::return_type::ERROR;
   }
 
-  auto new_state = controller->get_state();
+  auto new_state = controller->get_lifecycle_state();
   if (state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
   {
     RCLCPP_DEBUG(
@@ -1744,7 +1753,7 @@ void ControllerManager::list_controllers_srv_cb(
     controller_state.name = controllers[i].info.name;
     controller_state.type = controllers[i].info.type;
     controller_state.claimed_interfaces = controllers[i].info.claimed_interfaces;
-    controller_state.state = controllers[i].c->get_state().label();
+    controller_state.state = controllers[i].c->get_lifecycle_state().label();
     controller_state.is_chainable = controllers[i].c->is_chainable();
     controller_state.is_chained = controllers[i].c->is_in_chained_mode();
 
@@ -2728,7 +2737,7 @@ void ControllerManager::controller_activity_diagnostic_callback(
     {
       all_active = false;
     }
-    stat.add(controllers[i].info.name, controllers[i].c->get_state().label());
+    stat.add(controllers[i].info.name, controllers[i].c->get_lifecycle_state().label());
   }
 
   if (all_active)

controller_manager/src/ros2_control_node.cpp

@@ -50,7 +50,6 @@ int main(int argc, char ** argv)
       // A simple way to reject non ros args
       continue;
     }
-    RCLCPP_INFO(rclcpp::get_logger("CM args"), "Adding argument: %s", argv[i]);
     node_arguments.push_back(argv[i]);
   }
   cm_node_options.arguments(node_arguments);

controller_manager/test/test_chainable_controller/test_chainable_controller.cpp

@@ -33,8 +33,8 @@ controller_interface::InterfaceConfiguration
 TestChainableController::command_interface_configuration() const
 {
   if (
-    get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
-    get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+    get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
+    get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
   {
     return cmd_iface_cfg_;
   }
@@ -49,8 +49,8 @@ controller_interface::InterfaceConfiguration
 TestChainableController::state_interface_configuration() const
 {
   if (
-    get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
-    get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+    get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
+    get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
   {
     auto state_iface_cfg = state_iface_cfg_;
     if (imu_sensor_)

controller_manager/test/test_controller/test_controller.cpp

@@ -31,8 +31,8 @@ TestController::TestController()
 controller_interface::InterfaceConfiguration TestController::command_interface_configuration() const
 {
   if (
-    get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
-    get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+    get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
+    get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
   {
     return cmd_iface_cfg_;
   }
@@ -46,8 +46,8 @@ controller_interface::InterfaceConfiguration TestController::command_interface_c
 controller_interface::InterfaceConfiguration TestController::state_interface_configuration() const
 {
   if (
-    get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
-    get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+    get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
+    get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
   {
     return state_iface_cfg_;
   }

controller_manager/test/test_controller_manager.cpp

@@ -127,7 +127,8 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)
     << "Update should not reach an unconfigured controller";
 
   EXPECT_EQ(
-    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id());
+    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+    test_controller->get_lifecycle_state().id());
 
   // configure controller
   {
@@ -141,7 +142,9 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)
   EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started";
   EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is not started";
 
-  EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id());
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+    test_controller->get_lifecycle_state().id());
 
   // Start controller, will take effect at the end of the update function
   std::vector<std::string> start_controllers = {"fake_controller", TEST_CONTROLLER2_NAME};
@@ -182,7 +185,8 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)
     ControllerManagerRunner cm_runner(this);
     EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
   }
-  EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id());
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id());
 
   EXPECT_EQ(
     controller_interface::return_type::OK,
@@ -210,7 +214,9 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)
     EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
   }
 
-  EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id());
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+    test_controller->get_lifecycle_state().id());
   auto unload_future = std::async(
     std::launch::async, &controller_manager::ControllerManager::unload_controller, cm_,
     test_controller::TEST_CONTROLLER_NAME);
@@ -221,7 +227,8 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)
   EXPECT_EQ(controller_interface::return_type::OK, unload_future.get());
 
   EXPECT_EQ(
-    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id());
+    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+    test_controller->get_lifecycle_state().id());
   EXPECT_EQ(1, test_controller.use_count());
 }
 
@@ -242,7 +249,8 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate)
     << "Update should not reach an unconfigured controller";
 
   EXPECT_EQ(
-    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id());
+    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+    test_controller->get_lifecycle_state().id());
 
   test_controller->get_node()->set_parameter({"update_rate", 4});
   // configure controller
@@ -255,7 +263,9 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate)
     cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
   EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started";
 
-  EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id());
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+    test_controller->get_lifecycle_state().id());
 
   // Start controller, will take effect at the end of the update function
   std::vector<std::string> start_controllers = {test_controller::TEST_CONTROLLER_NAME};
@@ -276,7 +286,8 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate)
     EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
   }
 
-  EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id());
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id());
 
   // As the controller frequency is 4Hz, it needs to pass 25 iterations for 1 update cycle
   for (size_t i = 0; i < 25; i++)
@@ -323,7 +334,8 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd
     << "Update should not reach an unconfigured controller";
 
   EXPECT_EQ(
-    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id());
+    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+    test_controller->get_lifecycle_state().id());
 
   rclcpp::Parameter update_rate_parameter("update_rate", static_cast<int>(ctrl_update_rate));
   test_controller->get_node()->set_parameter(update_rate_parameter);
@@ -337,7 +349,9 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd
     cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
   EXPECT_EQ(last_internal_counter, test_controller->internal_counter)
     << "Controller is not started";
-  EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id());
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+    test_controller->get_lifecycle_state().id());
 
   // Start controller, will take effect at the end of the update function
   std::vector<std::string> start_controllers = {test_controller::TEST_CONTROLLER_NAME};
@@ -360,7 +374,8 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd
     EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
   }
 
-  EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id());
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id());
 
   const auto pre_internal_counter = test_controller->internal_counter;
   rclcpp::Rate loop_rate(cm_->get_update_rate());
@@ -430,7 +445,8 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
   EXPECT_EQ(2, test_controller.use_count());
 
   EXPECT_EQ(
-    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id());
+    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+    test_controller->get_lifecycle_state().id());
 
   test_controller->get_node()->set_parameter({"update_rate", static_cast<int>(ctrl_update_rate)});
   // configure controller
@@ -443,7 +459,9 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
     cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
   EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started";
 
-  EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id());
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+    test_controller->get_lifecycle_state().id());
 
   // Start controller, will take effect at the end of the update function
   time_ = test_controller->get_node()->now();  // set to something nonzero
@@ -467,7 +485,8 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
     EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
   }
 
-  EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id());
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id());
 
   EXPECT_EQ(test_controller->get_update_rate(), ctrl_update_rate);
   const auto cm_update_rate = cm_->get_update_rate();