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();