From 0a3705f932507c8b5e0a88d6fc6994d6ac9a2e3d Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Wed, 20 Dec 2023 19:36:39 +0100 Subject: [PATCH 01/16] Try using SCHED_FIFO on any kernel (#1142) --- controller_manager/doc/userdoc.rst | 15 ++++++++++++--- controller_manager/src/ros2_control_node.cpp | 16 +++++++--------- 2 files changed, 19 insertions(+), 12 deletions(-) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index a05f6a3afc..46c46fa028 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -11,10 +11,9 @@ Determinism ----------- For best performance when controlling hardware you want the controller manager to have as little jitter as possible in the main control loop. -The normal linux kernel is optimized for computational throughput and therefore is not well suited for hardware control. -The two easiest kernel options are the `Real-time Ubuntu 22.04 LTS Beta `_ or `linux-image-rt-amd64 `_ on Debian Bullseye. -If you have a realtime kernel installed, the main thread of Controller Manager attempts to configure ``SCHED_FIFO`` with a priority of ``50``. +Independent of the kernel installed, the main thread of Controller Manager attempts to +configure ``SCHED_FIFO`` with a priority of ``50``. By default, the user does not have permission to set such a high priority. To give the user such permissions, add a group named realtime and add the user controlling your robot to this group: @@ -36,6 +35,16 @@ Afterwards, add the following limits to the realtime group in ``/etc/security/li The limits will be applied after you log out and in again. +The normal linux kernel is optimized for computational throughput and therefore is not well suited for hardware control. +Alternatives to the standard kernel include + +- `Real-time Ubuntu 22.04 LTS Beta `_ on Ubuntu 22.04 +- `linux-image-rt-amd64 `_ on Debian Bullseye +- lowlatency kernel (``sudo apt install linux-lowlatency``) on any ubuntu + +Though installing a realtime-kernel will definitely get the best results when it comes to low +jitter, using a lowlatency kernel can improve things a lot with being really easy to install. + Parameters ----------- diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index e7f88f2c20..2747e79a1b 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -48,16 +48,14 @@ int main(int argc, char ** argv) std::thread cm_thread( [cm]() { - if (realtime_tools::has_realtime_kernel()) + if (!realtime_tools::configure_sched_fifo(kSchedPriority)) { - if (!realtime_tools::configure_sched_fifo(kSchedPriority)) - { - RCLCPP_WARN(cm->get_logger(), "Could not enable FIFO RT scheduling policy"); - } - } - else - { - RCLCPP_INFO(cm->get_logger(), "RT kernel is recommended for better performance"); + RCLCPP_WARN( + cm->get_logger(), + "Could not enable FIFO RT scheduling policy. Consider setting up your user to do FIFO RT " + "scheduling. See " + "[https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] " + "for details."); } // for calculating sleep time From e92b6c2c84946c8dab44146a5b4a666390543d61 Mon Sep 17 00:00:00 2001 From: Maximilian Schik Date: Wed, 20 Dec 2023 20:11:40 +0100 Subject: [PATCH 02/16] Improve transmission tests (#1238) * [transmission_interface] relaxes float tolerance in tests * [transmission_interface] replaces EXPECT_EQ for floats with EXPECT_THAT(..., DoubleNear(...)) --- .../differential_transmission_loader_test.cpp | 64 ++++++++++--------- .../test/differential_transmission_test.cpp | 14 ++-- ...r_bar_linkage_transmission_loader_test.cpp | 64 ++++++++++--------- .../four_bar_linkage_transmission_test.cpp | 14 ++-- .../test/simple_transmission_loader_test.cpp | 22 ++++--- .../test/simple_transmission_test.cpp | 20 +++--- 6 files changed, 106 insertions(+), 92 deletions(-) diff --git a/transmission_interface/test/differential_transmission_loader_test.cpp b/transmission_interface/test/differential_transmission_loader_test.cpp index d2ed533652..70d0adf2cd 100644 --- a/transmission_interface/test/differential_transmission_loader_test.cpp +++ b/transmission_interface/test/differential_transmission_loader_test.cpp @@ -26,8 +26,12 @@ #include "transmission_interface/differential_transmission_loader.hpp" #include "transmission_interface/transmission_loader.hpp" +using testing::DoubleNear; using testing::SizeIs; +// Floating-point value comparison threshold +const double EPS = 1e-5; + class TransmissionPluginLoader { public: @@ -113,16 +117,16 @@ TEST(DifferentialTransmissionLoaderTest, FullSpec) const std::vector & actuator_reduction = differential_transmission->get_actuator_reduction(); - EXPECT_EQ(50.0, actuator_reduction[0]); - EXPECT_EQ(-50.0, actuator_reduction[1]); + EXPECT_THAT(50.0, DoubleNear(actuator_reduction[0], EPS)); + EXPECT_THAT(-50.0, DoubleNear(actuator_reduction[1], EPS)); const std::vector & joint_reduction = differential_transmission->get_joint_reduction(); - EXPECT_EQ(2.0, joint_reduction[0]); - EXPECT_EQ(-2.0, joint_reduction[1]); + EXPECT_THAT(2.0, DoubleNear(joint_reduction[0], EPS)); + EXPECT_THAT(-2.0, DoubleNear(joint_reduction[1], EPS)); const std::vector & joint_offset = differential_transmission->get_joint_offset(); - EXPECT_EQ(0.5, joint_offset[0]); - EXPECT_EQ(-0.5, joint_offset[1]); + EXPECT_THAT(0.5, DoubleNear(joint_offset[0], EPS)); + EXPECT_THAT(-0.5, DoubleNear(joint_offset[1], EPS)); } TEST(DifferentialTransmissionLoaderTest, only_mech_red_specified) @@ -184,16 +188,16 @@ TEST(DifferentialTransmissionLoaderTest, only_mech_red_specified) const std::vector & actuator_reduction = differential_transmission->get_actuator_reduction(); - EXPECT_EQ(50.0, actuator_reduction[0]); - EXPECT_EQ(-50.0, actuator_reduction[1]); + EXPECT_THAT(50.0, DoubleNear(actuator_reduction[0], EPS)); + EXPECT_THAT(-50.0, DoubleNear(actuator_reduction[1], EPS)); const std::vector & joint_reduction = differential_transmission->get_joint_reduction(); - EXPECT_EQ(1.0, joint_reduction[0]); - EXPECT_EQ(1.0, joint_reduction[1]); + EXPECT_THAT(1.0, DoubleNear(joint_reduction[0], EPS)); + EXPECT_THAT(1.0, DoubleNear(joint_reduction[1], EPS)); const std::vector & joint_offset = differential_transmission->get_joint_offset(); - EXPECT_EQ(0.0, joint_offset[0]); - EXPECT_EQ(0.0, joint_offset[1]); + EXPECT_THAT(0.0, DoubleNear(joint_offset[0], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint_offset[1], EPS)); } TEST(SimpleTransmissionLoaderTest, offset_and_mech_red_not_specified) @@ -247,16 +251,16 @@ TEST(SimpleTransmissionLoaderTest, offset_and_mech_red_not_specified) const std::vector & actuator_reduction = differential_transmission->get_actuator_reduction(); - EXPECT_EQ(1.0, actuator_reduction[0]); - EXPECT_EQ(1.0, actuator_reduction[1]); + EXPECT_THAT(1.0, DoubleNear(actuator_reduction[0], EPS)); + EXPECT_THAT(1.0, DoubleNear(actuator_reduction[1], EPS)); const std::vector & joint_reduction = differential_transmission->get_joint_reduction(); - EXPECT_EQ(1.0, joint_reduction[0]); - EXPECT_EQ(1.0, joint_reduction[1]); + EXPECT_THAT(1.0, DoubleNear(joint_reduction[0], EPS)); + EXPECT_THAT(1.0, DoubleNear(joint_reduction[1], EPS)); const std::vector & joint_offset = differential_transmission->get_joint_offset(); - EXPECT_EQ(0.0, joint_offset[0]); - EXPECT_EQ(0.0, joint_offset[1]); + EXPECT_THAT(0.0, DoubleNear(joint_offset[0], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint_offset[1], EPS)); } TEST(DifferentialTransmissionLoaderTest, mechanical_reduction_not_a_number) @@ -319,16 +323,16 @@ TEST(DifferentialTransmissionLoaderTest, mechanical_reduction_not_a_number) // default kicks in for ill-defined values const std::vector & actuator_reduction = differential_transmission->get_actuator_reduction(); - EXPECT_EQ(1.0, actuator_reduction[0]); - EXPECT_EQ(1.0, actuator_reduction[1]); + EXPECT_THAT(1.0, DoubleNear(actuator_reduction[0], EPS)); + EXPECT_THAT(1.0, DoubleNear(actuator_reduction[1], EPS)); const std::vector & joint_reduction = differential_transmission->get_joint_reduction(); - EXPECT_EQ(1.0, joint_reduction[0]); - EXPECT_EQ(1.0, joint_reduction[1]); + EXPECT_THAT(1.0, DoubleNear(joint_reduction[0], EPS)); + EXPECT_THAT(1.0, DoubleNear(joint_reduction[1], EPS)); const std::vector & joint_offset = differential_transmission->get_joint_offset(); - EXPECT_EQ(0.0, joint_offset[0]); - EXPECT_EQ(0.0, joint_offset[1]); + EXPECT_THAT(0.0, DoubleNear(joint_offset[0], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint_offset[1], EPS)); } TEST(DifferentialTransmissionLoaderTest, offset_ill_defined) @@ -393,17 +397,17 @@ TEST(DifferentialTransmissionLoaderTest, offset_ill_defined) // default kicks in for ill-defined values const std::vector & actuator_reduction = differential_transmission->get_actuator_reduction(); - EXPECT_EQ(50.0, actuator_reduction[0]); - EXPECT_EQ(-50.0, actuator_reduction[1]); + EXPECT_THAT(50.0, DoubleNear(actuator_reduction[0], EPS)); + EXPECT_THAT(-50.0, DoubleNear(actuator_reduction[1], EPS)); const std::vector & joint_reduction = differential_transmission->get_joint_reduction(); - EXPECT_EQ(2.0, joint_reduction[0]); - EXPECT_EQ(-2.0, joint_reduction[1]); + EXPECT_THAT(2.0, DoubleNear(joint_reduction[0], EPS)); + EXPECT_THAT(-2.0, DoubleNear(joint_reduction[1], EPS)); // default kicks in for ill-defined values const std::vector & joint_offset = differential_transmission->get_joint_offset(); - EXPECT_EQ(0.0, joint_offset[0]); - EXPECT_EQ(0.0, joint_offset[1]); + EXPECT_THAT(0.0, DoubleNear(joint_offset[0], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint_offset[1], EPS)); } TEST(DifferentialTransmissionLoaderTest, mech_red_invalid_value) diff --git a/transmission_interface/test/differential_transmission_test.cpp b/transmission_interface/test/differential_transmission_test.cpp index 7e27194bb6..14ffe968bc 100644 --- a/transmission_interface/test/differential_transmission_test.cpp +++ b/transmission_interface/test/differential_transmission_test.cpp @@ -29,7 +29,7 @@ using transmission_interface::DifferentialTransmission; using transmission_interface::Exception; using transmission_interface::JointHandle; // Floating-point value comparison threshold -const double EPS = 1e-6; +const double EPS = 1e-5; TEST(PreconditionsTest, ExceptionThrowing) { @@ -79,12 +79,12 @@ TEST(PreconditionsTest, AccessorValidation) EXPECT_EQ(2u, trans.num_actuators()); EXPECT_EQ(2u, trans.num_joints()); - EXPECT_EQ(2.0, trans.get_actuator_reduction()[0]); - EXPECT_EQ(-2.0, trans.get_actuator_reduction()[1]); - EXPECT_EQ(4.0, trans.get_joint_reduction()[0]); - EXPECT_EQ(-4.0, trans.get_joint_reduction()[1]); - EXPECT_EQ(1.0, trans.get_joint_offset()[0]); - EXPECT_EQ(-1.0, trans.get_joint_offset()[1]); + EXPECT_THAT(2.0, DoubleNear(trans.get_actuator_reduction()[0], EPS)); + EXPECT_THAT(-2.0, DoubleNear(trans.get_actuator_reduction()[1], EPS)); + EXPECT_THAT(4.0, DoubleNear(trans.get_joint_reduction()[0], EPS)); + EXPECT_THAT(-4.0, DoubleNear(trans.get_joint_reduction()[1], EPS)); + EXPECT_THAT(1.0, DoubleNear(trans.get_joint_offset()[0], EPS)); + EXPECT_THAT(-1.0, DoubleNear(trans.get_joint_offset()[1], EPS)); } void testConfigureWithBadHandles(std::string interface_name) diff --git a/transmission_interface/test/four_bar_linkage_transmission_loader_test.cpp b/transmission_interface/test/four_bar_linkage_transmission_loader_test.cpp index d5960ab47c..53b584b7b5 100644 --- a/transmission_interface/test/four_bar_linkage_transmission_loader_test.cpp +++ b/transmission_interface/test/four_bar_linkage_transmission_loader_test.cpp @@ -26,8 +26,12 @@ #include "transmission_interface/four_bar_linkage_transmission_loader.hpp" #include "transmission_interface/transmission_loader.hpp" +using testing::DoubleNear; using testing::SizeIs; +// Floating-point value comparison threshold +const double EPS = 1e-5; + class TransmissionPluginLoader { public: @@ -113,17 +117,17 @@ TEST(FourBarLinkageTransmissionLoaderTest, FullSpec) const std::vector & actuator_reduction = four_bar_linkage_transmission->get_actuator_reduction(); - EXPECT_EQ(50.0, actuator_reduction[0]); - EXPECT_EQ(-50.0, actuator_reduction[1]); + EXPECT_THAT(50.0, DoubleNear(actuator_reduction[0], EPS)); + EXPECT_THAT(-50.0, DoubleNear(actuator_reduction[1], EPS)); const std::vector & joint_reduction = four_bar_linkage_transmission->get_joint_reduction(); - EXPECT_EQ(2.0, joint_reduction[0]); - EXPECT_EQ(-2.0, joint_reduction[1]); + EXPECT_THAT(2.0, DoubleNear(joint_reduction[0], EPS)); + EXPECT_THAT(-2.0, DoubleNear(joint_reduction[1], EPS)); const std::vector & joint_offset = four_bar_linkage_transmission->get_joint_offset(); - EXPECT_EQ(0.5, joint_offset[0]); - EXPECT_EQ(-0.5, joint_offset[1]); + EXPECT_THAT(0.5, DoubleNear(joint_offset[0], EPS)); + EXPECT_THAT(-0.5, DoubleNear(joint_offset[1], EPS)); } TEST(FourBarLinkageTransmissionLoaderTest, only_mech_red_specified) @@ -185,17 +189,17 @@ TEST(FourBarLinkageTransmissionLoaderTest, only_mech_red_specified) const std::vector & actuator_reduction = four_bar_linkage_transmission->get_actuator_reduction(); - EXPECT_EQ(50.0, actuator_reduction[0]); - EXPECT_EQ(-50.0, actuator_reduction[1]); + EXPECT_THAT(50.0, DoubleNear(actuator_reduction[0], EPS)); + EXPECT_THAT(-50.0, DoubleNear(actuator_reduction[1], EPS)); const std::vector & joint_reduction = four_bar_linkage_transmission->get_joint_reduction(); - EXPECT_EQ(1.0, joint_reduction[0]); - EXPECT_EQ(1.0, joint_reduction[1]); + EXPECT_THAT(1.0, DoubleNear(joint_reduction[0], EPS)); + EXPECT_THAT(1.0, DoubleNear(joint_reduction[1], EPS)); const std::vector & joint_offset = four_bar_linkage_transmission->get_joint_offset(); - EXPECT_EQ(0.0, joint_offset[0]); - EXPECT_EQ(0.0, joint_offset[1]); + EXPECT_THAT(0.0, DoubleNear(joint_offset[0], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint_offset[1], EPS)); } TEST(DifferentialTransmissionLoaderTest, offset_and_mech_red_not_specified) @@ -249,17 +253,17 @@ TEST(DifferentialTransmissionLoaderTest, offset_and_mech_red_not_specified) const std::vector & actuator_reduction = four_bar_linkage_transmission->get_actuator_reduction(); - EXPECT_EQ(1.0, actuator_reduction[0]); - EXPECT_EQ(1.0, actuator_reduction[1]); + EXPECT_THAT(1.0, DoubleNear(actuator_reduction[0], EPS)); + EXPECT_THAT(1.0, DoubleNear(actuator_reduction[1], EPS)); const std::vector & joint_reduction = four_bar_linkage_transmission->get_joint_reduction(); - EXPECT_EQ(1.0, joint_reduction[0]); - EXPECT_EQ(1.0, joint_reduction[1]); + EXPECT_THAT(1.0, DoubleNear(joint_reduction[0], EPS)); + EXPECT_THAT(1.0, DoubleNear(joint_reduction[1], EPS)); const std::vector & joint_offset = four_bar_linkage_transmission->get_joint_offset(); - EXPECT_EQ(0.0, joint_offset[0]); - EXPECT_EQ(0.0, joint_offset[1]); + EXPECT_THAT(0.0, DoubleNear(joint_offset[0], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint_offset[1], EPS)); } TEST(FourBarLinkageTransmissionLoaderTest, mechanical_reduction_not_a_number) @@ -322,17 +326,17 @@ TEST(FourBarLinkageTransmissionLoaderTest, mechanical_reduction_not_a_number) // default kicks in for ill-defined values const std::vector & actuator_reduction = four_bar_linkage_transmission->get_actuator_reduction(); - EXPECT_EQ(1.0, actuator_reduction[0]); - EXPECT_EQ(1.0, actuator_reduction[1]); + EXPECT_THAT(1.0, DoubleNear(actuator_reduction[0], EPS)); + EXPECT_THAT(1.0, DoubleNear(actuator_reduction[1], EPS)); const std::vector & joint_reduction = four_bar_linkage_transmission->get_joint_reduction(); - EXPECT_EQ(1.0, joint_reduction[0]); - EXPECT_EQ(1.0, joint_reduction[1]); + EXPECT_THAT(1.0, DoubleNear(joint_reduction[0], EPS)); + EXPECT_THAT(1.0, DoubleNear(joint_reduction[1], EPS)); const std::vector & joint_offset = four_bar_linkage_transmission->get_joint_offset(); - EXPECT_EQ(0.0, joint_offset[0]); - EXPECT_EQ(0.0, joint_offset[1]); + EXPECT_THAT(0.0, DoubleNear(joint_offset[0], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint_offset[1], EPS)); } TEST(FourBarLinkageTransmissionLoaderTest, offset_ill_defined) @@ -397,18 +401,18 @@ TEST(FourBarLinkageTransmissionLoaderTest, offset_ill_defined) // default kicks in for ill-defined values const std::vector & actuator_reduction = four_bar_linkage_transmission->get_actuator_reduction(); - EXPECT_EQ(50.0, actuator_reduction[0]); - EXPECT_EQ(-50.0, actuator_reduction[1]); + EXPECT_THAT(50.0, DoubleNear(actuator_reduction[0], EPS)); + EXPECT_THAT(-50.0, DoubleNear(actuator_reduction[1], EPS)); const std::vector & joint_reduction = four_bar_linkage_transmission->get_joint_reduction(); - EXPECT_EQ(2.0, joint_reduction[0]); - EXPECT_EQ(-2.0, joint_reduction[1]); + EXPECT_THAT(2.0, DoubleNear(joint_reduction[0], EPS)); + EXPECT_THAT(-2.0, DoubleNear(joint_reduction[1], EPS)); // default kicks in for ill-defined values const std::vector & joint_offset = four_bar_linkage_transmission->get_joint_offset(); - EXPECT_EQ(0.0, joint_offset[0]); - EXPECT_EQ(0.0, joint_offset[1]); + EXPECT_THAT(0.0, DoubleNear(joint_offset[0], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint_offset[1], EPS)); } TEST(FourBarLinkageTransmissionLoaderTest, mech_red_invalid_value) diff --git a/transmission_interface/test/four_bar_linkage_transmission_test.cpp b/transmission_interface/test/four_bar_linkage_transmission_test.cpp index a44feb178c..f74e4def6a 100644 --- a/transmission_interface/test/four_bar_linkage_transmission_test.cpp +++ b/transmission_interface/test/four_bar_linkage_transmission_test.cpp @@ -33,7 +33,7 @@ using transmission_interface::FourBarLinkageTransmission; using transmission_interface::JointHandle; // Floating-point value comparison threshold -const double EPS = 1e-6; +const double EPS = 1e-5; TEST(PreconditionsTest, ExceptionThrowing) { @@ -83,12 +83,12 @@ TEST(PreconditionsTest, AccessorValidation) EXPECT_EQ(2u, trans.num_actuators()); EXPECT_EQ(2u, trans.num_joints()); - EXPECT_EQ(2.0, trans.get_actuator_reduction()[0]); - EXPECT_EQ(-2.0, trans.get_actuator_reduction()[1]); - EXPECT_EQ(4.0, trans.get_joint_reduction()[0]); - EXPECT_EQ(-4.0, trans.get_joint_reduction()[1]); - EXPECT_EQ(1.0, trans.get_joint_offset()[0]); - EXPECT_EQ(-1.0, trans.get_joint_offset()[1]); + EXPECT_THAT(2.0, DoubleNear(trans.get_actuator_reduction()[0], EPS)); + EXPECT_THAT(-2.0, DoubleNear(trans.get_actuator_reduction()[1], EPS)); + EXPECT_THAT(4.0, DoubleNear(trans.get_joint_reduction()[0], EPS)); + EXPECT_THAT(-4.0, DoubleNear(trans.get_joint_reduction()[1], EPS)); + EXPECT_THAT(1.0, DoubleNear(trans.get_joint_offset()[0], EPS)); + EXPECT_THAT(-1.0, DoubleNear(trans.get_joint_offset()[1], EPS)); } void testConfigureWithBadHandles(std::string interface_name) diff --git a/transmission_interface/test/simple_transmission_loader_test.cpp b/transmission_interface/test/simple_transmission_loader_test.cpp index 1518bacc3b..4623d8c409 100644 --- a/transmission_interface/test/simple_transmission_loader_test.cpp +++ b/transmission_interface/test/simple_transmission_loader_test.cpp @@ -26,8 +26,12 @@ #include "transmission_interface/simple_transmission_loader.hpp" #include "transmission_interface/transmission_loader.hpp" +using testing::DoubleNear; using testing::SizeIs; +// Floating-point value comparison threshold +const double EPS = 1e-5; + class TransmissionPluginLoader { public: @@ -229,8 +233,8 @@ TEST(SimpleTransmissionLoaderTest, FullSpec) transmission_interface::SimpleTransmission * simple_transmission = dynamic_cast(transmission.get()); ASSERT_TRUE(nullptr != simple_transmission); - EXPECT_EQ(325.949, simple_transmission->get_actuator_reduction()); - EXPECT_EQ(0.0, simple_transmission->get_joint_offset()); + EXPECT_THAT(325.949, DoubleNear(simple_transmission->get_actuator_reduction(), EPS)); + EXPECT_THAT(0.0, DoubleNear(simple_transmission->get_joint_offset(), EPS)); } TEST(SimpleTransmissionLoaderTest, only_mech_red_specified) @@ -275,8 +279,8 @@ TEST(SimpleTransmissionLoaderTest, only_mech_red_specified) transmission_interface::SimpleTransmission * simple_transmission = dynamic_cast(transmission.get()); ASSERT_TRUE(nullptr != simple_transmission); - EXPECT_EQ(50.0, simple_transmission->get_actuator_reduction()); - EXPECT_EQ(0.0, simple_transmission->get_joint_offset()); + EXPECT_THAT(50.0, DoubleNear(simple_transmission->get_actuator_reduction(), EPS)); + EXPECT_THAT(0.0, DoubleNear(simple_transmission->get_joint_offset(), EPS)); } TEST(SimpleTransmissionLoaderTest, offset_and_mech_red_not_specified) @@ -317,8 +321,8 @@ TEST(SimpleTransmissionLoaderTest, offset_and_mech_red_not_specified) transmission_interface::SimpleTransmission * simple_transmission = dynamic_cast(transmission.get()); ASSERT_TRUE(nullptr != simple_transmission); - EXPECT_EQ(1.0, simple_transmission->get_actuator_reduction()); - EXPECT_EQ(0.0, simple_transmission->get_joint_offset()); + EXPECT_THAT(1.0, DoubleNear(simple_transmission->get_actuator_reduction(), EPS)); + EXPECT_THAT(0.0, DoubleNear(simple_transmission->get_joint_offset(), EPS)); } TEST(SimpleTransmissionLoaderTest, mechanical_reduction_not_a_number) @@ -360,7 +364,7 @@ TEST(SimpleTransmissionLoaderTest, mechanical_reduction_not_a_number) dynamic_cast(transmission.get()); ASSERT_TRUE(nullptr != simple_transmission); // default kicks in for ill-defined values - EXPECT_EQ(1.0, simple_transmission->get_actuator_reduction()); + EXPECT_THAT(1.0, DoubleNear(simple_transmission->get_actuator_reduction(), EPS)); } TEST(SimpleTransmissionLoaderTest, offset_ill_defined) @@ -403,8 +407,8 @@ TEST(SimpleTransmissionLoaderTest, offset_ill_defined) dynamic_cast(transmission.get()); ASSERT_TRUE(nullptr != simple_transmission); // default kicks in for ill-defined values - EXPECT_EQ(0.0, simple_transmission->get_joint_offset()); - EXPECT_EQ(50.0, simple_transmission->get_actuator_reduction()); + EXPECT_THAT(0.0, DoubleNear(simple_transmission->get_joint_offset(), EPS)); + EXPECT_THAT(50.0, DoubleNear(simple_transmission->get_actuator_reduction(), EPS)); } TEST(SimpleTransmissionLoaderTest, mech_red_invalid_value) diff --git a/transmission_interface/test/simple_transmission_test.cpp b/transmission_interface/test/simple_transmission_test.cpp index d7601cdd20..33a14f92d1 100644 --- a/transmission_interface/test/simple_transmission_test.cpp +++ b/transmission_interface/test/simple_transmission_test.cpp @@ -27,8 +27,10 @@ using transmission_interface::Exception; using transmission_interface::JointHandle; using transmission_interface::SimpleTransmission; +using testing::DoubleNear; + // Floating-point value comparison threshold -const double EPS = 1e-6; +const double EPS = 1e-5; TEST(PreconditionsTest, ExceptionThrownWithInvalidParameters) { @@ -53,8 +55,8 @@ TEST(PreconditionsTest, AccessorValidation) EXPECT_EQ(1u, trans.num_actuators()); EXPECT_EQ(1u, trans.num_joints()); - EXPECT_EQ(2.0, trans.get_actuator_reduction()); - EXPECT_EQ(-1.0, trans.get_joint_offset()); + EXPECT_THAT(2.0, DoubleNear(trans.get_actuator_reduction(), EPS)); + EXPECT_THAT(-1.0, DoubleNear(trans.get_joint_offset(), EPS)); } TEST(PreconditionsTest, ConfigureFailsWithInvalidHandles) @@ -127,7 +129,7 @@ class BlackBoxTest : public TransmissionSetup, public ::testing::TestWithParam Date: Wed, 27 Dec 2023 18:15:31 +0900 Subject: [PATCH 03/16] controller_manager: Add space to string literal concatenation (#1245) --- controller_manager/src/controller_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 2e703f4219..e1cf41f7e4 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -948,7 +948,7 @@ controller_interface::return_type ControllerManager::switch_controller( { RCLCPP_WARN( get_logger(), - "Controller with name '%s' is not inactive so its following" + "Controller with name '%s' is not inactive so its following " "controllers do not have to be checked, because it cannot be activated.", controller_it->info.name.c_str()); status = controller_interface::return_type::ERROR; From 6d3e3a628bd8ad2af9b40da01dfd10caaf20623a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 27 Dec 2023 11:00:32 +0100 Subject: [PATCH 04/16] Do not run reviewer lottery on bot PRs (#1250) --- .github/workflows/reviewer_lottery.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/reviewer_lottery.yml b/.github/workflows/reviewer_lottery.yml index 2edbc9b59e..ed28964e01 100644 --- a/.github/workflows/reviewer_lottery.yml +++ b/.github/workflows/reviewer_lottery.yml @@ -6,6 +6,7 @@ on: jobs: test: runs-on: ubuntu-latest + if: github.actor != 'dependabot[bot]' && github.actor != 'mergify[bot]' steps: - uses: actions/checkout@v4 - uses: uesteibar/reviewer-lottery@v3 From 54ced2a7be75750ca2cd00a7e8d572b65095bc4c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 29 Dec 2023 12:10:49 +0100 Subject: [PATCH 05/16] Set ref explicitely for action-ros-ci (#1254) --- .github/workflows/reusable-ros-tooling-source-build.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index efcf82afda..122e25944a 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -36,6 +36,7 @@ jobs: with: target-ros2-distro: ${{ inputs.ros_distro }} # build all packages listed in the meta package + ref: ${{ inputs.ref }} # otherwise the default branch is used for scheduled workflows package-name: controller_interface controller_manager From e149646d3f2595f269cfa4e1cd0681abde89ee69 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 3 Jan 2024 15:51:39 +0100 Subject: [PATCH 06/16] [docs] Remove joint_state_controller (#1263) --- controller_manager/controller_manager/launch_utils.py | 6 +++--- ros2controlcli/doc/userdoc.rst | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/controller_manager/controller_manager/launch_utils.py b/controller_manager/controller_manager/launch_utils.py index 93941cc11f..5a23c02cec 100644 --- a/controller_manager/controller_manager/launch_utils.py +++ b/controller_manager/controller_manager/launch_utils.py @@ -32,12 +32,12 @@ def generate_load_controller_launch_description( Examples # noqa: D416 -------- # Assuming the controller type and controller parameters are known to the controller_manager - generate_load_controller_launch_description('joint_state_controller') + generate_load_controller_launch_description('joint_state_broadcaster') # Passing controller type and controller parameter file to load generate_load_controller_launch_description( - 'joint_state_controller', - controller_type='joint_state_controller/JointStateController', + 'joint_state_broadcaster', + controller_type='joint_state_broadcaster/JointStateBroadcaster', controller_params_file=os.path.join(get_package_share_directory('my_pkg'), 'config', 'controller_params.yaml') ) diff --git a/ros2controlcli/doc/userdoc.rst b/ros2controlcli/doc/userdoc.rst index 5eada2acb6..6dadd31226 100644 --- a/ros2controlcli/doc/userdoc.rst +++ b/ros2controlcli/doc/userdoc.rst @@ -73,7 +73,7 @@ Example output: $ ros2 control list_controller_types diff_drive_controller/DiffDriveController controller_interface::ControllerInterface - joint_state_controller/JointStateController controller_interface::ControllerInterface + joint_state_broadcaster/JointStateBroadcaster controller_interface::ControllerInterface joint_trajectory_controller/JointTrajectoryController controller_interface::ControllerInterface From 0dfbd3dfe160d8ea0e8403955a794f94766e4443 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 4 Jan 2024 16:55:25 +0100 Subject: [PATCH 07/16] Use portable version for string-to-double conversion (#1257) --- .../hardware_interface/component_parser.hpp | 3 -- .../hardware_interface/lexical_casts.hpp | 52 +++++++++++++++++++ hardware_interface/src/component_parser.cpp | 31 +++++------ .../src/mock_components/generic_system.cpp | 18 ++----- 4 files changed, 68 insertions(+), 36 deletions(-) create mode 100644 hardware_interface/include/hardware_interface/lexical_casts.hpp diff --git a/hardware_interface/include/hardware_interface/component_parser.hpp b/hardware_interface/include/hardware_interface/component_parser.hpp index 5112f7927e..d5d999cca8 100644 --- a/hardware_interface/include/hardware_interface/component_parser.hpp +++ b/hardware_interface/include/hardware_interface/component_parser.hpp @@ -33,8 +33,5 @@ namespace hardware_interface HARDWARE_INTERFACE_PUBLIC std::vector parse_control_resources_from_urdf(const std::string & urdf); -HARDWARE_INTERFACE_PUBLIC -bool parse_bool(const std::string & bool_string); - } // namespace hardware_interface #endif // HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_ diff --git a/hardware_interface/include/hardware_interface/lexical_casts.hpp b/hardware_interface/include/hardware_interface/lexical_casts.hpp new file mode 100644 index 0000000000..e0333756f4 --- /dev/null +++ b/hardware_interface/include/hardware_interface/lexical_casts.hpp @@ -0,0 +1,52 @@ +// Copyright 2023 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. + +#ifndef HARDWARE_INTERFACE__LEXICAL_CASTS_HPP_ +#define HARDWARE_INTERFACE__LEXICAL_CASTS_HPP_ + +#include +#include +#include +#include + +namespace hardware_interface +{ + +/** \brief Helper function to convert a std::string to double in a locale-independent way. + \throws std::invalid_argument if not a valid number + * from + https://github.com/ros-planning/srdfdom/blob/ad17b8d25812f752c397a6011cec64aeff090c46/src/model.cpp#L53 +*/ +double stod(const std::string & s) +{ + // convert from string using no locale + std::istringstream stream(s); + stream.imbue(std::locale::classic()); + double result; + stream >> result; + if (stream.fail() || !stream.eof()) + { + throw std::invalid_argument("Failed converting string to real number"); + } + return result; +} + +bool parse_bool(const std::string & bool_string) +{ + return bool_string == "true" || bool_string == "True"; +} + +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__LEXICAL_CASTS_HPP_ diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 8e9b6bf16b..4f67d3e8b6 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -23,6 +23,7 @@ #include "hardware_interface/component_parser.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/lexical_casts.hpp" namespace { @@ -128,26 +129,23 @@ double get_parameter_value_or( { while (params_it) { - // Fill the map with parameters - const auto tag_name = params_it->Name(); - if (strcmp(tag_name, parameter_name) == 0) + try { - const auto tag_text = params_it->GetText(); - if (tag_text) + // Fill the map with parameters + const auto tag_name = params_it->Name(); + if (strcmp(tag_name, parameter_name) == 0) { - // Parse and return double value if there is no parsing error - double result_value; - const auto parse_result = - std::from_chars(tag_text, tag_text + std::strlen(tag_text), result_value); - if (parse_result.ec == std::errc()) + const auto tag_text = params_it->GetText(); + if (tag_text) { - return result_value; + return hardware_interface::stod(tag_text); } - - // Parsing failed - exit loop and return default value - break; } } + catch (const std::exception & e) + { + return default_value; + } params_it = params_it->NextSiblingElement(); } @@ -616,9 +614,4 @@ std::vector parse_control_resources_from_urdf(const std::string & return hardware_info; } -bool parse_bool(const std::string & bool_string) -{ - return bool_string == "true" || bool_string == "True"; -} - } // namespace hardware_interface diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index f4aee6c8a6..22d8aa573c 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -26,22 +26,12 @@ #include #include "hardware_interface/component_parser.hpp" +#include "hardware_interface/lexical_casts.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rcutils/logging_macros.h" namespace mock_components { -double parse_double(const std::string & text) -{ - double result_value; - const auto parse_result = std::from_chars(text.data(), text.data() + text.size(), result_value); - if (parse_result.ec == std::errc()) - { - return result_value; - } - - return 0.0; -} CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & info) { @@ -123,7 +113,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i it = info_.hardware_parameters.find("position_state_following_offset"); if (it != info_.hardware_parameters.end()) { - position_state_following_offset_ = parse_double(it->second); + position_state_following_offset_ = hardware_interface::stod(it->second); it = info_.hardware_parameters.find("custom_interface_with_following_offset"); if (it != info_.hardware_parameters.end()) { @@ -169,7 +159,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i auto param_it = joint.parameters.find("multiplier"); if (param_it != joint.parameters.end()) { - mimic_joint.multiplier = parse_double(joint.parameters.at("multiplier")); + mimic_joint.multiplier = hardware_interface::stod(joint.parameters.at("multiplier")); } mimic_joints_.push_back(mimic_joint); } @@ -696,7 +686,7 @@ void GenericSystem::initialize_storage_vectors( // Check the initial_value param is used if (!interface.initial_value.empty()) { - states[index][i] = parse_double(interface.initial_value); + states[index][i] = hardware_interface::stod(interface.initial_value); } } } From 64c9e2ab9636c98e0383e6dba4fb431f8e0948d5 Mon Sep 17 00:00:00 2001 From: Maximilian Schik Date: Thu, 4 Jan 2024 16:57:08 +0100 Subject: [PATCH 08/16] [ResourceManager] adds test for uninitialized hardware (#1243) --- .../test/test_components/test_actuator.cpp | 10 ++++ .../test/test_components/test_components.xml | 18 +++++++ .../test/test_components/test_sensor.cpp | 10 ++++ .../test/test_components/test_system.cpp | 10 ++++ .../test/test_resource_manager.cpp | 40 +++++++++++++++ .../ros2_control_test_assets/descriptions.hpp | 51 +++++++++++++++++++ 6 files changed, 139 insertions(+) diff --git a/hardware_interface/test/test_components/test_actuator.cpp b/hardware_interface/test/test_components/test_actuator.cpp index 7cf55b50d3..2f606b74cb 100644 --- a/hardware_interface/test/test_components/test_actuator.cpp +++ b/hardware_interface/test/test_components/test_actuator.cpp @@ -120,5 +120,15 @@ class TestActuator : public ActuatorInterface double max_velocity_command_ = 0.0; }; +class TestUnitilizableActuator : public TestActuator +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + ActuatorInterface::on_init(info); + return CallbackReturn::ERROR; + } +}; + #include "pluginlib/class_list_macros.hpp" // NOLINT PLUGINLIB_EXPORT_CLASS(TestActuator, hardware_interface::ActuatorInterface) +PLUGINLIB_EXPORT_CLASS(TestUnitilizableActuator, hardware_interface::ActuatorInterface) diff --git a/hardware_interface/test/test_components/test_components.xml b/hardware_interface/test/test_components/test_components.xml index 235fec668f..f029d3dd37 100644 --- a/hardware_interface/test/test_components/test_components.xml +++ b/hardware_interface/test/test_components/test_components.xml @@ -17,4 +17,22 @@ Test System + + + + Test Unitilizable Actuator + + + + + + Test Unitilizable Sensor + + + + + + Test Unitilizable System + + diff --git a/hardware_interface/test/test_components/test_sensor.cpp b/hardware_interface/test/test_components/test_sensor.cpp index ae31c73436..4811244138 100644 --- a/hardware_interface/test/test_components/test_sensor.cpp +++ b/hardware_interface/test/test_components/test_sensor.cpp @@ -55,5 +55,15 @@ class TestSensor : public SensorInterface double velocity_state_ = 0.0; }; +class TestUnitilizableSensor : public TestSensor +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + SensorInterface::on_init(info); + return CallbackReturn::ERROR; + } +}; + #include "pluginlib/class_list_macros.hpp" // NOLINT PLUGINLIB_EXPORT_CLASS(TestSensor, hardware_interface::SensorInterface) +PLUGINLIB_EXPORT_CLASS(TestUnitilizableSensor, hardware_interface::SensorInterface) diff --git a/hardware_interface/test/test_components/test_system.cpp b/hardware_interface/test/test_components/test_system.cpp index 3326cb893b..bc7a75df6f 100644 --- a/hardware_interface/test/test_components/test_system.cpp +++ b/hardware_interface/test/test_components/test_system.cpp @@ -123,5 +123,15 @@ class TestSystem : public SystemInterface double configuration_command_ = 0.0; }; +class TestUnitilizableSystem : public TestSystem +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + SystemInterface::on_init(info); + return CallbackReturn::ERROR; + } +}; + #include "pluginlib/class_list_macros.hpp" // NOLINT PLUGINLIB_EXPORT_CLASS(TestSystem, hardware_interface::SystemInterface) +PLUGINLIB_EXPORT_CLASS(TestUnitilizableSystem, hardware_interface::SystemInterface) diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index 8246cc207d..84519c20fa 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -69,6 +69,7 @@ class TestableResourceManager : public hardware_interface::ResourceManager FRIEND_TEST(ResourceManagerTest, post_initialization_add_components); FRIEND_TEST(ResourceManagerTest, managing_controllers_reference_interfaces); FRIEND_TEST(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle); + FRIEND_TEST(ResourceManagerTest, test_unitilizable_hardware_no_validation); TestableResourceManager() : hardware_interface::ResourceManager() {} @@ -154,6 +155,45 @@ TEST_F(ResourceManagerTest, post_initialization_with_urdf) ASSERT_NO_THROW(rm.load_urdf(ros2_control_test_assets::minimal_robot_urdf)); } +TEST_F(ResourceManagerTest, test_unitilizable_hardware_validation) +{ + // If the the hardware can not be initialized and load_urdf tried to validate the interfaces a + // runtime exception is thrown + TestableResourceManager rm; + ASSERT_THROW( + rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, true), + std::runtime_error); +} + +TEST_F(ResourceManagerTest, test_unitilizable_hardware_no_validation) +{ + // If the the hardware can not be initialized and load_urdf didn't try to validate the interfaces, + // the interface should not show up + TestableResourceManager rm; + EXPECT_NO_THROW(rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, false)); + + // test actuator + EXPECT_FALSE(rm.state_interface_exists("joint1/position")); + EXPECT_FALSE(rm.state_interface_exists("joint1/velocity")); + EXPECT_FALSE(rm.command_interface_exists("joint1/position")); + EXPECT_FALSE(rm.command_interface_exists("joint1/max_velocity")); + + // test sensor + EXPECT_FALSE(rm.state_interface_exists("sensor1/velocity")); + + // test system + EXPECT_FALSE(rm.state_interface_exists("joint2/position")); + EXPECT_FALSE(rm.state_interface_exists("joint2/velocity")); + EXPECT_FALSE(rm.state_interface_exists("joint2/acceleration")); + EXPECT_FALSE(rm.command_interface_exists("joint2/velocity")); + EXPECT_FALSE(rm.command_interface_exists("joint2/max_acceleration")); + EXPECT_FALSE(rm.state_interface_exists("joint3/position")); + EXPECT_FALSE(rm.state_interface_exists("joint3/velocity")); + EXPECT_FALSE(rm.state_interface_exists("joint3/acceleration")); + EXPECT_FALSE(rm.command_interface_exists("joint3/velocity")); + EXPECT_FALSE(rm.command_interface_exists("joint3/max_acceleration")); +} + TEST_F(ResourceManagerTest, initialization_with_urdf_manual_validation) { // we validate the results manually diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index 05ffad00d6..a2f6195c67 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -176,6 +176,55 @@ const auto hardware_resources = )"; +const auto unitilizable_hardware_resources = + R"( + + + test_unitilizable_actuator + + + + + + + + + + + test_unitilizable_sensor + 2 + 2 + + + + + + + + test_unitilizable_system + 2 + 2 + + + + + + + + + + + + + + + + + + + +)"; + const auto hardware_resources_missing_state_keys = R"( @@ -406,6 +455,8 @@ const auto diffbot_urdf = const auto minimal_robot_urdf = std::string(urdf_head) + std::string(hardware_resources) + std::string(urdf_tail); +const auto minimal_unitilizable_robot_urdf = + std::string(urdf_head) + std::string(unitilizable_hardware_resources) + std::string(urdf_tail); const auto minimal_robot_missing_state_keys_urdf = std::string(urdf_head) + std::string(hardware_resources_missing_state_keys) + From 2d82de84ba0ce746ecc6d3db58b0e12b2533bd2c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 8 Jan 2024 11:09:54 +0100 Subject: [PATCH 09/16] Fix rqt controller manager crash on ros2_control restart (#1273) * add service_timeout to the other service methods * catch RuntimeErrors when the service is no longer available --- .../controller_manager_services.py | 54 ++++++++++++++----- .../controller_manager.py | 26 +++++---- 2 files changed, 56 insertions(+), 24 deletions(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 5fe78c69b8..62b350f7d3 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -47,57 +47,75 @@ def service_caller(node, service_name, service_type, request, service_timeout=10 raise RuntimeError(f"Exception while calling service: {future.exception()}") -def configure_controller(node, controller_manager_name, controller_name): +def configure_controller(node, controller_manager_name, controller_name, service_timeout=10.0): request = ConfigureController.Request() request.name = controller_name return service_caller( - node, f"{controller_manager_name}/configure_controller", ConfigureController, request + node, + f"{controller_manager_name}/configure_controller", + ConfigureController, + request, + service_timeout, ) -def list_controllers(node, controller_manager_name): +def list_controllers(node, controller_manager_name, service_timeout=10.0): request = ListControllers.Request() return service_caller( - node, f"{controller_manager_name}/list_controllers", ListControllers, request + node, + f"{controller_manager_name}/list_controllers", + ListControllers, + request, + service_timeout, ) -def list_controller_types(node, controller_manager_name): +def list_controller_types(node, controller_manager_name, service_timeout=10.0): request = ListControllerTypes.Request() return service_caller( - node, f"{controller_manager_name}/list_controller_types", ListControllerTypes, request + node, + f"{controller_manager_name}/list_controller_types", + ListControllerTypes, + request, + service_timeout, ) -def list_hardware_components(node, controller_manager_name): +def list_hardware_components(node, controller_manager_name, service_timeout=10.0): request = ListHardwareComponents.Request() return service_caller( node, f"{controller_manager_name}/list_hardware_components", ListHardwareComponents, request, + service_timeout, ) -def list_hardware_interfaces(node, controller_manager_name): +def list_hardware_interfaces(node, controller_manager_name, service_timeout=10.0): request = ListHardwareInterfaces.Request() return service_caller( node, f"{controller_manager_name}/list_hardware_interfaces", ListHardwareInterfaces, request, + service_timeout, ) -def load_controller(node, controller_manager_name, controller_name): +def load_controller(node, controller_manager_name, controller_name, service_timeout=10.0): request = LoadController.Request() request.name = controller_name return service_caller( - node, f"{controller_manager_name}/load_controller", LoadController, request + node, + f"{controller_manager_name}/load_controller", + LoadController, + request, + service_timeout, ) -def reload_controller_libraries(node, controller_manager_name, force_kill): +def reload_controller_libraries(node, controller_manager_name, force_kill, service_timeout=10.0): request = ReloadControllerLibraries.Request() request.force_kill = force_kill return service_caller( @@ -105,10 +123,13 @@ def reload_controller_libraries(node, controller_manager_name, force_kill): f"{controller_manager_name}/reload_controller_libraries", ReloadControllerLibraries, request, + service_timeout, ) -def set_hardware_component_state(node, controller_manager_name, component_name, lifecyle_state): +def set_hardware_component_state( + node, controller_manager_name, component_name, lifecyle_state, service_timeout=10.0 +): request = SetHardwareComponentState.Request() request.name = component_name request.target_state = lifecyle_state @@ -117,6 +138,7 @@ def set_hardware_component_state(node, controller_manager_name, component_name, f"{controller_manager_name}/set_hardware_component_state", SetHardwareComponentState, request, + service_timeout, ) @@ -143,9 +165,13 @@ def switch_controllers( ) -def unload_controller(node, controller_manager_name, controller_name): +def unload_controller(node, controller_manager_name, controller_name, service_timeout=10.0): request = UnloadController.Request() request.name = controller_name return service_caller( - node, f"{controller_manager_name}/unload_controller", UnloadController, request + node, + f"{controller_manager_name}/unload_controller", + UnloadController, + request, + service_timeout, ) diff --git a/rqt_controller_manager/rqt_controller_manager/controller_manager.py b/rqt_controller_manager/rqt_controller_manager/controller_manager.py index 4d3c247e5b..7ca1f6d8c3 100644 --- a/rqt_controller_manager/rqt_controller_manager/controller_manager.py +++ b/rqt_controller_manager/rqt_controller_manager/controller_manager.py @@ -172,16 +172,22 @@ def _list_controllers(self): @rtype [str] """ # Add loaded controllers first - controllers = list_controllers(self._node, self._cm_name).controller - - # Append potential controller configs found in the node's parameters - for name in _get_parameter_controller_names(self._node, self._cm_name): - add_ctrl = all(name != ctrl.name for ctrl in controllers) - if add_ctrl: - type_str = _get_controller_type(self._node, self._cm_name, name) - uninit_ctrl = ControllerState(name=name, type=type_str) - controllers.append(uninit_ctrl) - return controllers + try: + controllers = list_controllers( + self._node, self._cm_name, 2.0 / self._cm_update_freq + ).controller + + # Append potential controller configs found in the node's parameters + for name in _get_parameter_controller_names(self._node, self._cm_name): + add_ctrl = all(name != ctrl.name for ctrl in controllers) + if add_ctrl: + type_str = _get_controller_type(self._node, self._cm_name, name) + uninit_ctrl = ControllerState(name=name, type=type_str) + controllers.append(uninit_ctrl) + return controllers + except RuntimeError as e: + print(e) + return [] def _show_controllers(self): table_view = self._widget.table_view From 672480dfd674c31436878dd52c5fac086dd99ee9 Mon Sep 17 00:00:00 2001 From: bailaC Date: Mon, 8 Jan 2024 16:44:49 +0530 Subject: [PATCH 10/16] Issue 695: Changing 'namespace_' variables to 'node_namespace' to make it more explicit (#1239) --- .../controller_interface/controller_interface_base.hpp | 2 +- controller_interface/src/controller_interface_base.cpp | 5 +++-- .../test/test_controller_with_options.hpp | 4 ++-- .../include/controller_manager/controller_manager.hpp | 4 ++-- controller_manager/src/controller_manager.cpp | 8 ++++---- .../test_controller_failed_init.cpp | 2 +- .../test_controller_failed_init.hpp | 2 +- .../test_controller_manager_hardware_error_handling.cpp | 4 ++-- .../test/test_controller_manager_urdf_passing.cpp | 4 ++-- .../test_controllers_chaining_with_controller_manager.cpp | 4 ++-- 10 files changed, 20 insertions(+), 19 deletions(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index bbc45c8583..2e953ee932 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -115,7 +115,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy CONTROLLER_INTERFACE_PUBLIC virtual return_type init( const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate, - const std::string & namespace_ = "", + const std::string & node_namespace = "", const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions().enable_logger_service(true)); /// Custom configure method to read additional parameters for controller-nodes diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 84cd0d5e2b..560feff5f9 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -26,10 +26,11 @@ namespace controller_interface { return_type ControllerInterfaceBase::init( const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate, - const std::string & namespace_, const rclcpp::NodeOptions & node_options) + const std::string & node_namespace, const rclcpp::NodeOptions & node_options) { node_ = std::make_shared( - controller_name, namespace_, node_options, false); // disable LifecycleNode service interfaces + controller_name, node_namespace, node_options, + false); // disable LifecycleNode service interfaces urdf_ = urdf; try diff --git a/controller_interface/test/test_controller_with_options.hpp b/controller_interface/test/test_controller_with_options.hpp index 1221536784..1416a7d2df 100644 --- a/controller_interface/test/test_controller_with_options.hpp +++ b/controller_interface/test/test_controller_with_options.hpp @@ -41,13 +41,13 @@ class ControllerWithOptions : public controller_interface::ControllerInterface controller_interface::return_type init( const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate, - const std::string & namespace_ = "", + const std::string & node_namespace = "", const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions() .allow_undeclared_parameters(true) .automatically_declare_parameters_from_overrides(true)) override { - ControllerInterface::init(controller_name, urdf, cm_update_rate, namespace_, node_options); + ControllerInterface::init(controller_name, urdf, cm_update_rate, node_namespace, node_options); switch (on_init()) { diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 79a9861463..44b3c3215a 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -71,14 +71,14 @@ class ControllerManager : public rclcpp::Node std::unique_ptr resource_manager, std::shared_ptr executor, const std::string & manager_node_name = "controller_manager", - const std::string & namespace_ = "", + const std::string & node_namespace = "", const rclcpp::NodeOptions & options = get_cm_node_options()); CONTROLLER_MANAGER_PUBLIC ControllerManager( std::shared_ptr executor, const std::string & manager_node_name = "controller_manager", - const std::string & namespace_ = "", + const std::string & node_namespace = "", const rclcpp::NodeOptions & options = get_cm_node_options()); CONTROLLER_MANAGER_PUBLIC diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index e1cf41f7e4..270af53fb3 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -255,8 +255,8 @@ rclcpp::NodeOptions get_cm_node_options() ControllerManager::ControllerManager( std::shared_ptr executor, const std::string & manager_node_name, - const std::string & namespace_, const rclcpp::NodeOptions & options) -: rclcpp::Node(manager_node_name, namespace_, options), + const std::string & node_namespace, const rclcpp::NodeOptions & options) +: rclcpp::Node(manager_node_name, node_namespace, options), resource_manager_(std::make_unique( update_rate_, this->get_node_clock_interface())), diagnostics_updater_(this), @@ -297,8 +297,8 @@ ControllerManager::ControllerManager( ControllerManager::ControllerManager( std::unique_ptr resource_manager, std::shared_ptr executor, const std::string & manager_node_name, - const std::string & namespace_, const rclcpp::NodeOptions & options) -: rclcpp::Node(manager_node_name, namespace_, options), + const std::string & node_namespace, const rclcpp::NodeOptions & options) +: rclcpp::Node(manager_node_name, node_namespace, options), resource_manager_(std::move(resource_manager)), diagnostics_updater_(this), executor_(executor), 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 index 6a6f954e87..a106d5cbdf 100644 --- 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 @@ -33,7 +33,7 @@ TestControllerFailedInit::on_init() controller_interface::return_type TestControllerFailedInit::init( const std::string & /* controller_name */, const std::string & /* urdf */, - unsigned int /*cm_update_rate*/, const std::string & /*namespace_*/, + unsigned int /*cm_update_rate*/, const std::string & /*node_namespace*/, const rclcpp::NodeOptions & /*node_options*/) { return controller_interface::return_type::ERROR; 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 index 28cb48ab6c..315b0745b0 100644 --- 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 @@ -41,7 +41,7 @@ class TestControllerFailedInit : public controller_interface::ControllerInterfac CONTROLLER_INTERFACE_PUBLIC controller_interface::return_type init( const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate, - const std::string & namespace_ = "", + const std::string & node_namespace = "", const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions() .allow_undeclared_parameters(true) diff --git a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp index f12ab9e96b..4c800d41c2 100644 --- a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp +++ b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp @@ -54,9 +54,9 @@ class TestableControllerManager : public controller_manager::ControllerManager std::unique_ptr resource_manager, std::shared_ptr executor, const std::string & manager_node_name = "controller_manager", - const std::string & namespace_ = "") + const std::string & node_namespace = "") : controller_manager::ControllerManager( - std::move(resource_manager), executor, manager_node_name, namespace_) + std::move(resource_manager), executor, manager_node_name, node_namespace) { } }; diff --git a/controller_manager/test/test_controller_manager_urdf_passing.cpp b/controller_manager/test/test_controller_manager_urdf_passing.cpp index 102cbdfbd4..5c2ebd14f6 100644 --- a/controller_manager/test/test_controller_manager_urdf_passing.cpp +++ b/controller_manager/test/test_controller_manager_urdf_passing.cpp @@ -40,9 +40,9 @@ class TestableControllerManager : public controller_manager::ControllerManager std::unique_ptr resource_manager, std::shared_ptr executor, const std::string & manager_node_name = "controller_manager", - const std::string & namespace_ = "") + const std::string & node_namespace = "") : controller_manager::ControllerManager( - std::move(resource_manager), executor, manager_node_name, namespace_) + std::move(resource_manager), executor, manager_node_name, node_namespace) { } }; diff --git a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp index 0ebddd85e0..eadca39756 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -94,9 +94,9 @@ class TestableControllerManager : public controller_manager::ControllerManager std::unique_ptr resource_manager, std::shared_ptr executor, const std::string & manager_node_name = "controller_manager", - const std::string & namespace_ = "") + const std::string & node_namespace = "") : controller_manager::ControllerManager( - std::move(resource_manager), executor, manager_node_name, namespace_) + std::move(resource_manager), executor, manager_node_name, node_namespace) { } }; From bdad0091946097542cb0397fbdf8130bfac42b3a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 8 Jan 2024 12:18:16 +0100 Subject: [PATCH 11/16] Initialize the controller manager services after initializing resource manager (#1271) --- controller_manager/src/controller_manager.cpp | 8 ++++++-- .../test/test_hardware_management_srvs.cpp | 12 +++++++++--- 2 files changed, 15 insertions(+), 5 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 270af53fb3..e7a2293672 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -286,12 +286,12 @@ ControllerManager::ControllerManager( "[Deprecated] Passing the robot description parameter directly to the control_manager node " "is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead."); init_resource_manager(robot_description_); + init_services(); } diagnostics_updater_.setHardwareID("ros2_control"); diagnostics_updater_.add( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); - init_services(); } ControllerManager::ControllerManager( @@ -313,12 +313,15 @@ ControllerManager::ControllerManager( RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value."); } + if (resource_manager_->is_urdf_already_loaded()) + { + init_services(); + } subscribe_to_robot_description_topic(); diagnostics_updater_.setHardwareID("ros2_control"); diagnostics_updater_.add( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); - init_services(); } void ControllerManager::subscribe_to_robot_description_topic() @@ -352,6 +355,7 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & return; } init_resource_manager(robot_description_); + init_services(); } catch (std::runtime_error & e) { diff --git a/controller_manager/test/test_hardware_management_srvs.cpp b/controller_manager/test/test_hardware_management_srvs.cpp index 0fc7a2f27e..0b4b107ee1 100644 --- a/controller_manager/test/test_hardware_management_srvs.cpp +++ b/controller_manager/test/test_hardware_management_srvs.cpp @@ -84,7 +84,9 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs "Unable to initialize resource manager, no robot description found."); } - cm_->init_resource_manager(robot_description); + auto msg = std_msgs::msg::String(); + msg.data = robot_description_; + cm_->robot_description_callback(msg); SetUpSrvsCMExecutor(); } @@ -383,7 +385,9 @@ class TestControllerManagerHWManagementSrvsWithoutParams "Unable to initialize resource manager, no robot description found."); } - cm_->init_resource_manager(robot_description); + auto msg = std_msgs::msg::String(); + msg.data = robot_description_; + cm_->robot_description_callback(msg); SetUpSrvsCMExecutor(); } @@ -440,7 +444,9 @@ class TestControllerManagerHWManagementSrvsOldParameters "Unable to initialize resource manager, no robot description found."); } - cm_->init_resource_manager(robot_description); + auto msg = std_msgs::msg::String(); + msg.data = robot_description_; + cm_->robot_description_callback(msg); SetUpSrvsCMExecutor(); } From 71506d5f12e1e8529be4951ccf45f7144969f841 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 9 Jan 2024 10:41:06 +0100 Subject: [PATCH 12/16] fix the multiple definitions of lexical casts methods (#1281) --- hardware_interface/CMakeLists.txt | 1 + .../hardware_interface/lexical_casts.hpp | 19 +--------- hardware_interface/src/lexical_casts.cpp | 37 +++++++++++++++++++ 3 files changed, 40 insertions(+), 17 deletions(-) create mode 100644 hardware_interface/src/lexical_casts.cpp diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index bcd03a0a16..510d970c84 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -27,6 +27,7 @@ add_library(hardware_interface SHARED src/resource_manager.cpp src/sensor.cpp src/system.cpp + src/lexical_casts.cpp ) target_compile_features(hardware_interface PUBLIC cxx_std_17) target_include_directories(hardware_interface PUBLIC diff --git a/hardware_interface/include/hardware_interface/lexical_casts.hpp b/hardware_interface/include/hardware_interface/lexical_casts.hpp index e0333756f4..1b9bad7018 100644 --- a/hardware_interface/include/hardware_interface/lexical_casts.hpp +++ b/hardware_interface/include/hardware_interface/lexical_casts.hpp @@ -28,24 +28,9 @@ namespace hardware_interface * from https://github.com/ros-planning/srdfdom/blob/ad17b8d25812f752c397a6011cec64aeff090c46/src/model.cpp#L53 */ -double stod(const std::string & s) -{ - // convert from string using no locale - std::istringstream stream(s); - stream.imbue(std::locale::classic()); - double result; - stream >> result; - if (stream.fail() || !stream.eof()) - { - throw std::invalid_argument("Failed converting string to real number"); - } - return result; -} +double stod(const std::string & s); -bool parse_bool(const std::string & bool_string) -{ - return bool_string == "true" || bool_string == "True"; -} +bool parse_bool(const std::string & bool_string); } // namespace hardware_interface diff --git a/hardware_interface/src/lexical_casts.cpp b/hardware_interface/src/lexical_casts.cpp new file mode 100644 index 0000000000..c9adcccf83 --- /dev/null +++ b/hardware_interface/src/lexical_casts.cpp @@ -0,0 +1,37 @@ +// Copyright 2024 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 "hardware_interface/lexical_casts.hpp" + +namespace hardware_interface +{ +double stod(const std::string & s) +{ + // convert from string using no locale + std::istringstream stream(s); + stream.imbue(std::locale::classic()); + double result; + stream >> result; + if (stream.fail() || !stream.eof()) + { + throw std::invalid_argument("Failed converting string to real number"); + } + return result; +} + +bool parse_bool(const std::string & bool_string) +{ + return bool_string == "true" || bool_string == "True"; +} +} // namespace hardware_interface From acbeeea057aaa07fd9eec4399cc27f912653537a Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Fri, 12 Jan 2024 15:34:53 +0100 Subject: [PATCH 13/16] Fix return of ERROR and calls of cleanup when system is unconfigured of finalized (#1279) --------- Co-authored-by: Andrea Scipione Co-authored-by: Bence Magyar Co-authored-by: Dr. Denis --- hardware_interface/src/actuator.cpp | 14 ++++++-- hardware_interface/src/sensor.cpp | 7 +++- hardware_interface/src/system.cpp | 14 ++++++-- .../test/test_component_interfaces.cpp | 32 +++++++++++++------ 4 files changed, 52 insertions(+), 15 deletions(-) diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 694e92355e..6b58e365dc 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -218,7 +218,12 @@ const rclcpp_lifecycle::State & Actuator::get_state() const { return impl_->get_ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & period) { - // TODO(destogl): discuss what should be default return value, e.g., "NOT_EXECUTED" + if ( + impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || + impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + { + return return_type::OK; + } return_type result = return_type::ERROR; if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || @@ -235,7 +240,12 @@ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & p return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & period) { - // TODO(destogl): discuss what should be default return value, e.g., "NOT_EXECUTED" + if ( + impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || + impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + { + return return_type::OK; + } return_type result = return_type::ERROR; if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index ade9b8781a..2e53e447b9 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -195,7 +195,12 @@ const rclcpp_lifecycle::State & Sensor::get_state() const { return impl_->get_st return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & period) { - // TODO(destogl): discuss what should be default return value, e.g., "NOT_EXECUTED" + if ( + impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || + impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + { + return return_type::OK; + } return_type result = return_type::ERROR; if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index f8703a47bc..ee942d6581 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -214,7 +214,12 @@ const rclcpp_lifecycle::State & System::get_state() const { return impl_->get_st return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & period) { - // TODO(destogl): discuss what should be default return value, e.g., "NOT_EXECUTED" + if ( + impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || + impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + { + return return_type::OK; + } return_type result = return_type::ERROR; if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || @@ -231,7 +236,12 @@ return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & per return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & period) { - // TODO(destogl): discuss what should be default return value, e.g., "NOT_EXECUTED" + if ( + impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || + impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + { + return return_type::OK; + } return_type result = return_type::ERROR; if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 128771058b..d90756c324 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -446,17 +446,17 @@ TEST(TestComponentInterfaces, dummy_actuator) double velocity_value = 1.0; command_interfaces[0].set_value(velocity_value); // velocity - ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); // Noting should change because it is UNCONFIGURED for (auto step = 0u; step < 10; ++step) { - ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); ASSERT_TRUE(std::isnan(state_interfaces[0].get_value())); // position value ASSERT_TRUE(std::isnan(state_interfaces[1].get_value())); // velocity - ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } state = actuator_hw.configure(); @@ -496,12 +496,12 @@ TEST(TestComponentInterfaces, dummy_actuator) // Noting should change because it is FINALIZED for (auto step = 0u; step < 10; ++step) { - ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); EXPECT_EQ(20 * velocity_value, state_interfaces[0].get_value()); // position value EXPECT_EQ(0, state_interfaces[1].get_value()); // velocity - ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } EXPECT_EQ( @@ -587,12 +587,12 @@ TEST(TestComponentInterfaces, dummy_system) command_interfaces[0].set_value(velocity_value); // velocity command_interfaces[1].set_value(velocity_value); // velocity command_interfaces[2].set_value(velocity_value); // velocity - ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); // Noting should change because it is UNCONFIGURED for (auto step = 0u; step < 10; ++step) { - ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); ASSERT_TRUE(std::isnan(state_interfaces[0].get_value())); // position value ASSERT_TRUE(std::isnan(state_interfaces[1].get_value())); // velocity @@ -601,7 +601,7 @@ TEST(TestComponentInterfaces, dummy_system) ASSERT_TRUE(std::isnan(state_interfaces[4].get_value())); // position value ASSERT_TRUE(std::isnan(state_interfaces[5].get_value())); // velocity - ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } state = system_hw.configure(); @@ -649,7 +649,7 @@ TEST(TestComponentInterfaces, dummy_system) // Noting should change because it is FINALIZED for (auto step = 0u; step < 10; ++step) { - ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); EXPECT_EQ(20 * velocity_value, state_interfaces[0].get_value()); // position value EXPECT_EQ(0.0, state_interfaces[1].get_value()); // velocity @@ -658,7 +658,7 @@ TEST(TestComponentInterfaces, dummy_system) EXPECT_EQ(20 * velocity_value, state_interfaces[4].get_value()); // position value EXPECT_EQ(0.0, state_interfaces[5].get_value()); // velocity - ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } EXPECT_EQ(hardware_interface::return_type::OK, system_hw.prepare_command_mode_switch({}, {})); @@ -841,6 +841,12 @@ TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + // Noting should change because it is UNCONFIGURED + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); + } + // activate again and expect reset values state = sensor_hw.configure(); EXPECT_EQ(state_interfaces[0].get_value(), 0.0); @@ -860,6 +866,12 @@ TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + // Noting should change because it is FINALIZED + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); + } + // can not change state anymore state = sensor_hw.configure(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); From f0c36421298798938270c9c1ba2a4b565fad52d1 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Fri, 12 Jan 2024 16:30:59 +0100 Subject: [PATCH 14/16] [Doc] Make interface comments clearer in the doc strings. (#1288) --- .../types/hardware_interface_type_values.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp index 98ca67226f..27b5207a0f 100644 --- a/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp @@ -17,13 +17,13 @@ namespace hardware_interface { -/// Constant defining position interface +/// Constant defining position interface name constexpr char HW_IF_POSITION[] = "position"; -/// Constant defining velocity interface +/// Constant defining velocity interface name constexpr char HW_IF_VELOCITY[] = "velocity"; -/// Constant defining acceleration interface +/// Constant defining acceleration interface name constexpr char HW_IF_ACCELERATION[] = "acceleration"; -/// Constant defining effort interface +/// Constant defining effort interface name constexpr char HW_IF_EFFORT[] = "effort"; } // namespace hardware_interface From 477c85da524ba3eb9f587a30e4747b5fdeb725ef Mon Sep 17 00:00:00 2001 From: bailaC Date: Fri, 12 Jan 2024 22:39:37 +0530 Subject: [PATCH 15/16] Adding backward compatibility for string-to-double conversion (#1284) --------- Co-authored-by: Sai Kishor Kothakota Co-authored-by: Dr. Denis --- .../hardware_interface/lexical_casts.hpp | 1 + hardware_interface/src/lexical_casts.cpp | 28 +++++++++++++++++-- 2 files changed, 26 insertions(+), 3 deletions(-) diff --git a/hardware_interface/include/hardware_interface/lexical_casts.hpp b/hardware_interface/include/hardware_interface/lexical_casts.hpp index 1b9bad7018..846d9f757c 100644 --- a/hardware_interface/include/hardware_interface/lexical_casts.hpp +++ b/hardware_interface/include/hardware_interface/lexical_casts.hpp @@ -16,6 +16,7 @@ #define HARDWARE_INTERFACE__LEXICAL_CASTS_HPP_ #include +#include #include #include #include diff --git a/hardware_interface/src/lexical_casts.cpp b/hardware_interface/src/lexical_casts.cpp index c9adcccf83..940beb6d0f 100644 --- a/hardware_interface/src/lexical_casts.cpp +++ b/hardware_interface/src/lexical_casts.cpp @@ -16,20 +16,42 @@ namespace hardware_interface { -double stod(const std::string & s) +namespace impl +{ +std::optional stod(const std::string & s) { +#if __cplusplus < 202002L // convert from string using no locale + // Impl with std::istringstream std::istringstream stream(s); stream.imbue(std::locale::classic()); double result; stream >> result; if (stream.fail() || !stream.eof()) { - throw std::invalid_argument("Failed converting string to real number"); + return std::nullopt; } return result; +#else + // Impl with std::from_chars + double result_value; + const auto parse_result = std::from_chars(s.data(), s.data() + s.size(), result_value); + if (parse_result.ec == std::errc()) + { + return result_value; + } + return std::nullopt; +#endif +} +} // namespace impl +double stod(const std::string & s) +{ + if (const auto result = impl::stod(s)) + { + return *result; + } + throw std::invalid_argument("Failed converting string to real number"); } - bool parse_bool(const std::string & bool_string) { return bool_string == "true" || bool_string == "True"; From 8c34ab6b383ea53c53c05fe460f9c7c743a96fa1 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Fri, 12 Jan 2024 18:40:56 +0100 Subject: [PATCH 16/16] Add additional checks for non existing and not available interfaces. (#1218) --- hardware_interface/CMakeLists.txt | 4 + .../hardware_interface/resource_manager.hpp | 6 +- hardware_interface/src/resource_manager.cpp | 135 ++++-- .../mock_components/test_generic_system.cpp | 81 ++-- .../test/test_components/test_actuator.cpp | 16 + .../test_system_with_command_modes.cpp | 3 + .../test/test_resource_manager.cpp | 121 +----- .../test/test_resource_manager.hpp | 80 ++++ ...esource_manager_prepare_perform_switch.cpp | 389 ++++++++++++++++++ 9 files changed, 646 insertions(+), 189 deletions(-) create mode 100644 hardware_interface/test/test_resource_manager.hpp create mode 100644 hardware_interface/test/test_resource_manager_prepare_perform_switch.cpp diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 510d970c84..35823b3ce9 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -114,6 +114,10 @@ if(BUILD_TESTING) target_link_libraries(test_resource_manager hardware_interface) ament_target_dependencies(test_resource_manager ros2_control_test_assets) + ament_add_gmock(test_resource_manager_prepare_perform_switch test/test_resource_manager_prepare_perform_switch.cpp) + target_link_libraries(test_resource_manager_prepare_perform_switch hardware_interface) + ament_target_dependencies(test_resource_manager_prepare_perform_switch ros2_control_test_assets) + ament_add_gmock(test_generic_system test/mock_components/test_generic_system.cpp) target_include_directories(test_generic_system PRIVATE include) target_link_libraries(test_generic_system hardware_interface) diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 92bde14817..146b903f46 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -331,7 +331,9 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * by default * \param[in] start_interfaces vector of string identifiers for the command interfaces starting. * \param[in] stop_interfaces vector of string identifiers for the command interfaces stopping. - * \return true if switch can be prepared, false if a component rejects switch request. + * \return true if switch can be prepared; false if a component rejects switch request, and if + * at least one of the input interfaces are not existing or not available (i.e., component is not + * in ACTIVE or INACTIVE state). */ bool prepare_command_mode_switch( const std::vector & start_interfaces, @@ -344,6 +346,8 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * \note this is intended for mode-switching when a hardware interface needs to change * control mode depending on which command interface is claimed. * \note this is for realtime switching of the command interface. + * \note it is assumed that `prepare_command_mode_switch` is called just before this method + * with the same input arguments. * \param[in] start_interfaces vector of string identifiers for the command interfaces starting. * \param[in] stop_interfaces vector of string identifiers for the command interfacs stopping. * \return true if switch is performed, false if a component rejects switching. diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 5c5d5af3e6..0cbb620c76 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -287,6 +287,7 @@ class ResourceStorage if (result) { + remove_all_hardware_interfaces_from_available_list(hardware.get_name()); async_component_threads_.erase(hardware.get_name()); // TODO(destogl): change this - deimport all things if there is there are interfaces there // deimport_non_movement_command_interfaces(hardware); @@ -1072,6 +1073,12 @@ bool ResourceManager::prepare_command_mode_switch( const std::vector & start_interfaces, const std::vector & stop_interfaces) { + // When only broadcaster is activated then this lists are empty + if (start_interfaces.empty() && stop_interfaces.empty()) + { + return true; + } + auto interfaces_to_string = [&]() { std::stringstream ss; @@ -1090,27 +1097,85 @@ bool ResourceManager::prepare_command_mode_switch( return ss.str(); }; - for (auto & component : resource_storage_->actuators_) + // Check if interface exists + std::stringstream ss_not_existing; + ss_not_existing << "Not existing: " << std::endl << "[" << std::endl; + auto check_exist = [&](const std::vector & list_to_check) { - if (return_type::OK != component.prepare_command_mode_switch(start_interfaces, stop_interfaces)) + bool all_exist = true; + for (const auto & interface : list_to_check) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", "Component '%s' did not accept new command resource combination: \n %s", - component.get_name().c_str(), interfaces_to_string().c_str()); - return false; + if (!command_interface_exists(interface)) + { + all_exist = false; + ss_not_existing << " " << interface << std::endl; + } } + return all_exist; + }; + if (!(check_exist(start_interfaces) && check_exist(stop_interfaces))) + { + ss_not_existing << "]" << std::endl; + RCUTILS_LOG_ERROR_NAMED( + "resource_manager", "Not acceptable command interfaces combination: \n%s%s", + interfaces_to_string().c_str(), ss_not_existing.str().c_str()); + return false; } - for (auto & component : resource_storage_->systems_) + + // Check if interfaces are available + std::stringstream ss_not_available; + ss_not_available << "Not available: " << std::endl << "[" << std::endl; + auto check_available = [&](const std::vector & list_to_check) { - if (return_type::OK != component.prepare_command_mode_switch(start_interfaces, stop_interfaces)) + bool all_available = true; + for (const auto & interface : list_to_check) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", "Component '%s' did not accept new command resource combination: \n %s", - component.get_name().c_str(), interfaces_to_string().c_str()); - return false; + if (!command_interface_is_available(interface)) + { + all_available = false; + ss_not_available << " " << interface << std::endl; + } } + return all_available; + }; + if (!(check_available(start_interfaces) && check_available(stop_interfaces))) + { + ss_not_available << "]" << std::endl; + RCUTILS_LOG_ERROR_NAMED( + "resource_manager", "Not acceptable command interfaces combination: \n%s%s", + interfaces_to_string().c_str(), ss_not_available.str().c_str()); + return false; } - return true; + + auto call_prepare_mode_switch = + [&start_interfaces, &stop_interfaces, &interfaces_to_string](auto & components) + { + bool ret = true; + for (auto & component : components) + { + if ( + component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + if ( + return_type::OK != + component.prepare_command_mode_switch(start_interfaces, stop_interfaces)) + { + RCUTILS_LOG_ERROR_NAMED( + "resource_manager", + "Component '%s' did not accept command interfaces combination: \n%s", + component.get_name().c_str(), interfaces_to_string().c_str()); + ret = false; + } + } + } + return ret; + }; + + const bool actuators_result = call_prepare_mode_switch(resource_storage_->actuators_); + const bool systems_result = call_prepare_mode_switch(resource_storage_->systems_); + + return actuators_result && systems_result; } // CM API: Called in "update"-thread @@ -1118,27 +1183,39 @@ bool ResourceManager::perform_command_mode_switch( const std::vector & start_interfaces, const std::vector & stop_interfaces) { - for (auto & component : resource_storage_->actuators_) + // When only broadcaster is activated then this lists are empty + if (start_interfaces.empty() && stop_interfaces.empty()) { - if (return_type::OK != component.perform_command_mode_switch(start_interfaces, stop_interfaces)) - { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", "Component '%s' could not perform switch", - component.get_name().c_str()); - return false; - } + return true; } - for (auto & component : resource_storage_->systems_) + + auto call_perform_mode_switch = [&start_interfaces, &stop_interfaces](auto & components) { - if (return_type::OK != component.perform_command_mode_switch(start_interfaces, stop_interfaces)) + bool ret = true; + for (auto & component : components) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", "Component '%s' could not perform switch", - component.get_name().c_str()); - return false; + if ( + component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + if ( + return_type::OK != + component.perform_command_mode_switch(start_interfaces, stop_interfaces)) + { + RCUTILS_LOG_ERROR_NAMED( + "resource_manager", "Component '%s' could not perform switch", + component.get_name().c_str()); + ret = false; + } + } } - } - return true; + return ret; + }; + + const bool actuators_result = call_perform_mode_switch(resource_storage_->actuators_); + const bool systems_result = call_perform_mode_switch(resource_storage_->systems_); + + return actuators_result && systems_result; } // CM API: Called in "callback/slow"-thread diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index 57b5796d0d..0641cfda57 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -51,7 +51,7 @@ class TestGenericSystem : public ::testing::Test { hardware_system_2dof_ = R"( - + mock_components/GenericSystem @@ -72,7 +72,7 @@ class TestGenericSystem : public ::testing::Test hardware_system_2dof_asymetric_ = R"( - + mock_components/GenericSystem @@ -93,7 +93,7 @@ class TestGenericSystem : public ::testing::Test hardware_system_2dof_standard_interfaces_ = R"( - + mock_components/GenericSystem @@ -118,7 +118,7 @@ class TestGenericSystem : public ::testing::Test hardware_system_2dof_with_other_interface_ = R"( - + mock_components/GenericSystem @@ -153,7 +153,7 @@ class TestGenericSystem : public ::testing::Test hardware_system_2dof_with_sensor_ = R"( - + mock_components/GenericSystem @@ -181,7 +181,7 @@ class TestGenericSystem : public ::testing::Test hardware_system_2dof_with_sensor_mock_command_ = R"( - + mock_components/GenericSystem true @@ -210,7 +210,7 @@ class TestGenericSystem : public ::testing::Test hardware_system_2dof_with_sensor_mock_command_True_ = R"( - + mock_components/GenericSystem True @@ -239,7 +239,7 @@ class TestGenericSystem : public ::testing::Test hardware_system_2dof_with_mimic_joint_ = R"( - + mock_components/GenericSystem @@ -264,7 +264,7 @@ class TestGenericSystem : public ::testing::Test hardware_system_2dof_standard_interfaces_with_offset_ = R"( - + mock_components/GenericSystem -3 @@ -290,7 +290,7 @@ class TestGenericSystem : public ::testing::Test hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_missing_ = R"( - + mock_components/GenericSystem -3 @@ -321,7 +321,7 @@ class TestGenericSystem : public ::testing::Test hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_ = R"( - + mock_components/GenericSystem -3 @@ -352,7 +352,7 @@ class TestGenericSystem : public ::testing::Test valid_urdf_ros2_control_system_robot_with_gpio_ = R"( - + mock_components/GenericSystem 2 @@ -389,7 +389,7 @@ class TestGenericSystem : public ::testing::Test valid_urdf_ros2_control_system_robot_with_gpio_mock_command_ = R"( - + mock_components/GenericSystem true @@ -425,7 +425,7 @@ class TestGenericSystem : public ::testing::Test valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_ = R"( - + mock_components/GenericSystem True @@ -461,7 +461,7 @@ class TestGenericSystem : public ::testing::Test sensor_with_initial_value_ = R"( - + mock_components/GenericSystem @@ -481,7 +481,7 @@ class TestGenericSystem : public ::testing::Test gpio_with_initial_value_ = R"( - + mock_components/GenericSystem @@ -495,7 +495,7 @@ class TestGenericSystem : public ::testing::Test hardware_system_2dof_standard_interfaces_with_different_control_modes_ = R"( - + mock_components/GenericSystem true @@ -528,7 +528,7 @@ class TestGenericSystem : public ::testing::Test valid_hardware_system_2dof_standard_interfaces_with_different_control_modes_ = R"( - + mock_components/GenericSystem true @@ -568,7 +568,7 @@ class TestGenericSystem : public ::testing::Test disabled_commands_ = R"( - + mock_components/GenericSystem True @@ -689,7 +689,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_symetric_interfaces) ros2_control_test_assets::urdf_tail; TestableResourceManager rm(urdf); // Activate components to get all interfaces available - activate_components(rm, {"HardwareSystem2dof"}); + activate_components(rm, {"MockHardwareSystem"}); // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -720,7 +720,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_asymetric_interfaces) ros2_control_test_assets::urdf_tail; TestableResourceManager rm(urdf); // Activate components to get all interfaces available - activate_components(rm, {"HardwareSystem2dofAsymetric"}); + activate_components(rm, {"MockHardwareSystem"}); // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -866,7 +866,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality) auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_standard_interfaces_ + ros2_control_test_assets::urdf_tail; - generic_system_functional_test(urdf, {"HardwareSystem2dofStandardInterfaces"}); + generic_system_functional_test(urdf, {"MockHardwareSystem"}); } TEST_F(TestGenericSystem, generic_system_2dof_other_interfaces) @@ -875,7 +875,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_other_interfaces) ros2_control_test_assets::urdf_tail; TestableResourceManager rm(urdf); // Activate components to get all interfaces available - activate_components(rm, {"HardwareSystem2dofWithOtherInterface"}); + activate_components(rm, {"MockHardwareSystem"}); // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -958,7 +958,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor) ros2_control_test_assets::urdf_tail; TestableResourceManager rm(urdf); // Activate components to get all interfaces available - activate_components(rm, {"HardwareSystem2dofWithSensor"}); + activate_components(rm, {"MockHardwareSystem"}); // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -1180,7 +1180,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor_mock_command) auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_sensor_mock_command_ + ros2_control_test_assets::urdf_tail; - test_generic_system_with_mock_sensor_commands(urdf, "HardwareSystem2dofWithSensorMockCommand"); + test_generic_system_with_mock_sensor_commands(urdf, "MockHardwareSystem"); } TEST_F(TestGenericSystem, generic_system_2dof_sensor_mock_command_True) @@ -1189,8 +1189,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor_mock_command_True) hardware_system_2dof_with_sensor_mock_command_True_ + ros2_control_test_assets::urdf_tail; - test_generic_system_with_mock_sensor_commands( - urdf, "HardwareSystem2dofWithSensorMockCommandTrue"); + test_generic_system_with_mock_sensor_commands(urdf, "MockHardwareSystem"); } void TestGenericSystem::test_generic_system_with_mimic_joint( @@ -1265,7 +1264,7 @@ TEST_F(TestGenericSystem, hardware_system_2dof_with_mimic_joint) auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_mimic_joint_ + ros2_control_test_assets::urdf_tail; - test_generic_system_with_mimic_joint(urdf, "HardwareSystem2dofWithMimicJoint"); + test_generic_system_with_mimic_joint(urdf, "MockHardwareSystem"); } TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset) @@ -1274,7 +1273,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset) hardware_system_2dof_standard_interfaces_with_offset_ + ros2_control_test_assets::urdf_tail; - generic_system_functional_test(urdf, "HardwareSystem2dofStandardInterfacesWithOffset", -3); + generic_system_functional_test(urdf, "MockHardwareSystem", -3); } TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_interface_missing) @@ -1284,8 +1283,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_i ros2_control_test_assets::urdf_tail; // custom interface is missing so offset will not be applied - generic_system_functional_test( - urdf, "HardwareSystem2dofStandardInterfacesWithCustomInterfaceForOffsetMissing", 0.0); + generic_system_functional_test(urdf, "MockHardwareSystem", 0.0); } TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_interface) @@ -1298,8 +1296,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_i TestableResourceManager rm(urdf); - const std::string hardware_name = - "HardwareSystem2dofStandardInterfacesWithCustomInterfaceForOffset"; + const std::string hardware_name = "MockHardwareSystem"; // check is hardware is configured auto status_map = rm.get_components_status(); @@ -1412,7 +1409,7 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio) valid_urdf_ros2_control_system_robot_with_gpio_ + ros2_control_test_assets::urdf_tail; TestableResourceManager rm(urdf); - const std::string hardware_name = "ValidURDFros2controlSystemRobotWithGPIO"; + const std::string hardware_name = "MockHardwareSystem"; // check is hardware is started auto status_map = rm.get_components_status(); @@ -1617,8 +1614,7 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_co valid_urdf_ros2_control_system_robot_with_gpio_mock_command_ + ros2_control_test_assets::urdf_tail; - test_generic_system_with_mock_gpio_commands( - urdf, "ValidURDFros2controlSystemRobotWithGPIOMockCommand"); + test_generic_system_with_mock_gpio_commands(urdf, "MockHardwareSystem"); } TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True) @@ -1627,8 +1623,7 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_co valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_ + ros2_control_test_assets::urdf_tail; - test_generic_system_with_mock_gpio_commands( - urdf, "ValidURDFros2controlSystemRobotWithGPIOMockCommandTrue"); + test_generic_system_with_mock_gpio_commands(urdf, "MockHardwareSystem"); } TEST_F(TestGenericSystem, sensor_with_initial_value) @@ -1637,7 +1632,7 @@ TEST_F(TestGenericSystem, sensor_with_initial_value) ros2_control_test_assets::urdf_tail; TestableResourceManager rm(urdf); // Activate components to get all interfaces available - activate_components(rm, {"SensorWithInitialValue"}); + activate_components(rm, {"MockHardwareSystem"}); // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -1665,7 +1660,7 @@ TEST_F(TestGenericSystem, gpio_with_initial_value) ros2_control_test_assets::urdf_tail; TestableResourceManager rm(urdf); // Activate components to get all interfaces available - activate_components(rm, {"GPIOWithInitialValue"}); + activate_components(rm, {"MockHardwareSystem"}); // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -1686,7 +1681,7 @@ TEST_F(TestGenericSystem, simple_dynamics_pos_vel_acc_control_modes_interfaces) TestableResourceManager rm(urdf); // Activate components to get all interfaces available - activate_components(rm, {"HardwareSystem2dofStandardInterfacesWithDifferentControlModes"}); + activate_components(rm, {"MockHardwareSystem"}); // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -1880,7 +1875,7 @@ TEST_F(TestGenericSystem, disabled_commands_flag_is_active) ros2_control_test_assets::urdf_head + disabled_commands_ + ros2_control_test_assets::urdf_tail; TestableResourceManager rm(urdf); // Activate components to get all interfaces available - activate_components(rm, {"DisabledCommands"}); + activate_components(rm, {"MockHardwareSystem"}); // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -1927,6 +1922,8 @@ TEST_F(TestGenericSystem, prepare_command_mode_switch_works_with_all_example_tag { TestableResourceManager rm( ros2_control_test_assets::urdf_head + urdf + ros2_control_test_assets::urdf_tail); + rclcpp_lifecycle::State state(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active"); + rm.set_component_state("MockHardwareSystem", state); auto start_interfaces = rm.command_interface_keys(); std::vector stop_interfaces; return rm.prepare_command_mode_switch(start_interfaces, stop_interfaces); diff --git a/hardware_interface/test/test_components/test_actuator.cpp b/hardware_interface/test/test_components/test_actuator.cpp index 2f606b74cb..41f27e201b 100644 --- a/hardware_interface/test/test_components/test_actuator.cpp +++ b/hardware_interface/test/test_components/test_actuator.cpp @@ -74,6 +74,22 @@ class TestActuator : public ActuatorInterface return command_interfaces; } + hardware_interface::return_type prepare_command_mode_switch( + const std::vector & /*start_interfaces*/, + const std::vector & /*stop_interfaces*/) override + { + position_state_ += 1.0; + return hardware_interface::return_type::OK; + } + + hardware_interface::return_type perform_command_mode_switch( + const std::vector & /*start_interfaces*/, + const std::vector & /*stop_interfaces*/) override + { + position_state_ += 100.0; + return hardware_interface::return_type::OK; + } + return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { // simulate error on read diff --git a/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp b/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp index 846508b1e2..a594d3b70a 100644 --- a/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp +++ b/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp @@ -120,6 +120,8 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface const std::vector & start_interfaces, const std::vector & stop_interfaces) override { + acceleration_state_[0] += 1.0; + // Starting interfaces start_modes_.clear(); stop_modes_.clear(); @@ -166,6 +168,7 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface const std::vector & start_interfaces, const std::vector & /*stop_interfaces*/) override { + acceleration_state_[0] += 100.0; // Test of failure in perform command mode switch // Fail if given an empty list. // This should never occur in a real system as the same start_interfaces list is sent to both diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index 84519c20fa..4bb9e0c5fe 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -14,7 +14,7 @@ // Authors: Karsten Knese, Denis Stogl -#include +#include "test_resource_manager.hpp" #include #include @@ -23,7 +23,6 @@ #include #include "hardware_interface/actuator_interface.hpp" -#include "hardware_interface/resource_manager.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -46,59 +45,6 @@ using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_PLUGIN_NAME; using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_STATE_INTERFACES; using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_TYPE; -class ResourceManagerTest : public ::testing::Test -{ -public: - static void SetUpTestCase() {} - - void SetUp() {} -}; - -// Forward declaration -namespace hardware_interface -{ -class ResourceStorage; -} - -class TestableResourceManager : public hardware_interface::ResourceManager -{ -public: - friend ResourceManagerTest; - - FRIEND_TEST(ResourceManagerTest, initialization_with_urdf_manual_validation); - FRIEND_TEST(ResourceManagerTest, post_initialization_add_components); - FRIEND_TEST(ResourceManagerTest, managing_controllers_reference_interfaces); - FRIEND_TEST(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle); - FRIEND_TEST(ResourceManagerTest, test_unitilizable_hardware_no_validation); - - TestableResourceManager() : hardware_interface::ResourceManager() {} - - TestableResourceManager( - const std::string & urdf, bool validate_interfaces = true, bool activate_all = false) - : hardware_interface::ResourceManager(urdf, validate_interfaces, activate_all) - { - } -}; - -std::vector set_components_state( - TestableResourceManager & rm, const std::vector & components, const uint8_t state_id, - const std::string & state_name) -{ - auto int_components = components; - if (int_components.empty()) - { - int_components = {"TestActuatorHardware", "TestSensorHardware", "TestSystemHardware"}; - } - std::vector results; - for (const auto & component : int_components) - { - rclcpp_lifecycle::State state(state_id, state_name); - const auto result = rm.set_component_state(component, state); - results.push_back(result); - } - return results; -} - auto configure_components = [](TestableResourceManager & rm, const std::vector & components = {}) { @@ -445,68 +391,9 @@ TEST_F(ResourceManagerTest, default_prepare_perform_switch) // Activate components to get all interfaces available activate_components(rm); - EXPECT_TRUE(rm.prepare_command_mode_switch({""}, {""})); - EXPECT_TRUE(rm.perform_command_mode_switch({""}, {""})); -} - -const auto hardware_resources_command_modes = - R"( - - - test_hardware_components/TestSystemCommandModes - - - - - - - - - - - - - - -)"; -const auto command_mode_urdf = std::string(ros2_control_test_assets::urdf_head) + - std::string(hardware_resources_command_modes) + - std::string(ros2_control_test_assets::urdf_tail); - -TEST_F(ResourceManagerTest, custom_prepare_perform_switch) -{ - TestableResourceManager rm(command_mode_urdf); - // Scenarios defined by example criteria - std::vector empty_keys = {}; - std::vector irrelevant_keys = {"elbow_joint/position", "should_joint/position"}; - std::vector illegal_single_key = {"joint1/position"}; - std::vector legal_keys_position = {"joint1/position", "joint2/position"}; - std::vector legal_keys_velocity = {"joint1/velocity", "joint2/velocity"}; - // Default behavior for empty key lists - EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, empty_keys)); - - // Default behavior when given irrelevant keys - EXPECT_TRUE(rm.prepare_command_mode_switch(irrelevant_keys, irrelevant_keys)); - EXPECT_TRUE(rm.prepare_command_mode_switch(irrelevant_keys, empty_keys)); - EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, irrelevant_keys)); - - // The test hardware interface has a criteria that both joints must change mode - EXPECT_FALSE(rm.prepare_command_mode_switch(illegal_single_key, illegal_single_key)); - EXPECT_FALSE(rm.prepare_command_mode_switch(illegal_single_key, empty_keys)); - EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, illegal_single_key)); - - // Test legal start keys - EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_position, legal_keys_position)); - EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_velocity, legal_keys_velocity)); - EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_position, empty_keys)); - EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, legal_keys_position)); - EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_velocity, empty_keys)); - EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, legal_keys_velocity)); - - // Test rejection from perform_command_mode_switch, test hardware rejects empty start sets - EXPECT_TRUE(rm.perform_command_mode_switch(legal_keys_position, legal_keys_position)); - EXPECT_FALSE(rm.perform_command_mode_switch(empty_keys, empty_keys)); - EXPECT_FALSE(rm.perform_command_mode_switch(empty_keys, legal_keys_position)); + // Default behavior for empty key lists, e.g., when a Broadcaster is activated + EXPECT_TRUE(rm.prepare_command_mode_switch({}, {})); + EXPECT_TRUE(rm.perform_command_mode_switch({}, {})); } TEST_F(ResourceManagerTest, resource_status) diff --git a/hardware_interface/test/test_resource_manager.hpp b/hardware_interface/test/test_resource_manager.hpp new file mode 100644 index 0000000000..46a487f2ef --- /dev/null +++ b/hardware_interface/test/test_resource_manager.hpp @@ -0,0 +1,80 @@ +// Copyright 2023 ros2_control Developers +// +// 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. + +// Authors: Dr. Denis + +#ifndef TEST_RESOURCE_MANAGER_HPP_ +#define TEST_RESOURCE_MANAGER_HPP_ + +#include + +#include +#include + +#include "hardware_interface/resource_manager.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" + +class ResourceManagerTest : public ::testing::Test +{ +public: + static void SetUpTestCase() {} + + void SetUp() {} +}; + +// Forward declaration +namespace hardware_interface +{ +class ResourceStorage; +} + +class TestableResourceManager : public hardware_interface::ResourceManager +{ +public: + friend ResourceManagerTest; + + FRIEND_TEST(ResourceManagerTest, initialization_with_urdf_manual_validation); + FRIEND_TEST(ResourceManagerTest, post_initialization_add_components); + FRIEND_TEST(ResourceManagerTest, managing_controllers_reference_interfaces); + FRIEND_TEST(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle); + FRIEND_TEST(ResourceManagerTest, test_unitilizable_hardware_no_validation); + + TestableResourceManager() : hardware_interface::ResourceManager() {} + + TestableResourceManager( + const std::string & urdf, bool validate_interfaces = true, bool activate_all = false) + : hardware_interface::ResourceManager(urdf, validate_interfaces, activate_all) + { + } +}; + +std::vector set_components_state( + TestableResourceManager & rm, const std::vector & components, const uint8_t state_id, + const std::string & state_name) +{ + auto int_components = components; + if (int_components.empty()) + { + int_components = {"TestActuatorHardware", "TestSensorHardware", "TestSystemHardware"}; + } + std::vector results; + for (const auto & component : int_components) + { + rclcpp_lifecycle::State state(state_id, state_name); + const auto result = rm.set_component_state(component, state); + results.push_back(result); + } + return results; +} +#endif // TEST_RESOURCE_MANAGER_HPP_ diff --git a/hardware_interface/test/test_resource_manager_prepare_perform_switch.cpp b/hardware_interface/test/test_resource_manager_prepare_perform_switch.cpp new file mode 100644 index 0000000000..8f6ba8f99a --- /dev/null +++ b/hardware_interface/test/test_resource_manager_prepare_perform_switch.cpp @@ -0,0 +1,389 @@ +// Copyright 2023 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. + +// Authors: Dr. Denis + +#include "test_resource_manager.hpp" + +#include +#include + +#include "hardware_interface/loaned_state_interface.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +const auto hardware_resources_command_modes = + R"( + + + test_hardware_components/TestSystemCommandModes + + + + + + + + + + + + + + + + + test_actuator + + + + + + + + + )"; +const auto command_mode_urdf = std::string(ros2_control_test_assets::urdf_head) + + std::string(hardware_resources_command_modes) + + std::string(ros2_control_test_assets::urdf_tail); + +class ResourceManagerPreparePerformTest : public ResourceManagerTest +{ +public: + void SetUp() + { + ResourceManagerTest::SetUp(); + + rm_ = std::make_unique(command_mode_urdf); + ASSERT_EQ(1u, rm_->actuator_components_size()); + ASSERT_EQ(1u, rm_->system_components_size()); + + // empty call can be done at any time and it doesn't propagates to HW components - always true + ASSERT_TRUE(rm_->perform_command_mode_switch(empty_keys, empty_keys)); + // empty call can be done at any time and it doesn't propagates to HW components - always true + ASSERT_TRUE(rm_->perform_command_mode_switch(empty_keys, empty_keys)); + + // Set both HW to ACTIVE to claim interfaces. There should stay persistent because we are not + // cleaning them for now, so this is a good way to keep the access and "f* the system" + set_components_state( + *rm_, {"TestSystemCommandModes"}, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active"); + set_components_state( + *rm_, {"TestActuatorHardware"}, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active"); + // State to get feedback how many times "prepare_for_switch" is called + claimed_system_acceleration_state_ = std::make_unique( + rm_->claim_state_interface("joint1/acceleration")); + claimed_actuator_position_state_ = std::make_unique( + rm_->claim_state_interface("joint3/position")); + } + + void preconfigure_components( + const uint8_t system_state_id, const std::string syste_state_name, + const uint8_t actuator_state_id, const std::string actuator_state_name) + { + set_components_state(*rm_, {"TestSystemCommandModes"}, system_state_id, syste_state_name); + set_components_state(*rm_, {"TestActuatorHardware"}, actuator_state_id, actuator_state_name); + + auto status_map = rm_->get_components_status(); + EXPECT_EQ(status_map["TestSystemCommandModes"].state.id(), system_state_id); + EXPECT_EQ(status_map["TestActuatorHardware"].state.id(), actuator_state_id); + } + + std::unique_ptr rm_; + + std::unique_ptr claimed_system_acceleration_state_; + std::unique_ptr claimed_actuator_position_state_; + + // Scenarios defined by example criteria + std::vector empty_keys = {}; + std::vector non_existing_keys = {"elbow_joint/position", "should_joint/position"}; + std::vector legal_keys_system = {"joint1/position", "joint2/position"}; + std::vector legal_keys_actuator = {"joint3/position"}; +}; + +// System : ACTIVE +// Actuator: UNCONFIGURED +TEST_F( + ResourceManagerPreparePerformTest, + when_system_active_and_actuator_unconfigured_expect_system_passing) +{ + preconfigure_components( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active", + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, "unconfigured"); + + // Default behavior for empty key lists, e.g., when a Broadcaster is activated + EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, empty_keys)); + // empty call can be done at any time and it doesn't propagates to HW components - always true + EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, empty_keys)); + + // When TestSystemCommandModes is ACTIVE expect OK + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 1.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 101.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 102.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 202.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + + EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 203.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 303.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + + // When TestActuatorHardware is UNCONFIGURED expect OK + EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 303.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 403.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + + EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 403.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 503.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + + EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 503.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 603.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); +}; + +// System : ACTIVE +// Actuator: INACTIVE +TEST_F( + ResourceManagerPreparePerformTest, when_system_active_and_actuator_inactive_expect_both_passing) +{ + preconfigure_components( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active", + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, "inactive"); + + // When TestSystemCommandModes is ACTIVE expect OK + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 1.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 1.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 101.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 101.0); + + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 102.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 102.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 202.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 202.0); + + EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 203.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 203.0); + EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 303.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 303.0); + + // When TestActuatorHardware is INACTIVE expect OK + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 304.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 304.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 404.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 404.0); + + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 405.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 405.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 505.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 505.0); + + EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 506.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 506.0); + EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 606.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 606.0); +}; + +// System : INACTIVE +// Actuator: ACTIVE +TEST_F( + ResourceManagerPreparePerformTest, when_system_inactive_and_actuator_active_expect_both_passing) +{ + preconfigure_components( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, "inactive", + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active"); + + // When TestSystemCommandModes is INACTIVE expect OK + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 1.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 1.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 101.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 101.0); + + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 102.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 102.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 202.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 202.0); + + EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 203.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 203.0); + EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 303.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 303.0); + + // When TestActuatorHardware is ACTIVE expect OK + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 304.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 304.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 404.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 404.0); + + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 405.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 405.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 505.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 505.0); + + EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 506.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 506.0); + EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 606.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 606.0); +}; + +// System : UNCONFIGURED +// Actuator: ACTIVE +TEST_F( + ResourceManagerPreparePerformTest, + when_system_unconfigured_and_actuator_active_expect_actuator_passing) +{ + preconfigure_components( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, "unconfigured", + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active"); + + // When TestSystemCommandModes is UNCONFIGURED expect error + EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 100.0); + + EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 100.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 200.0); + + EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 200.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 300.0); + + // When TestActuatorHardware is INACTIVE expect OK + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 301.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 401.0); + + EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 402.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 502.0); + + EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 503.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 603.0); +}; + +// System : UNCONFIGURED +// Actuator: FINALIZED +TEST_F( + ResourceManagerPreparePerformTest, + when_system_unconfigured_and_actuator_finalized_expect_none_passing) +{ + preconfigure_components( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, "unconfigured", + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, "finalized"); + + // When TestSystemCommandModes is UNCONFIGURED expect error + EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + + EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + + EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + + // When TestActuatorHardware is INACTIVE expect OK + EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + + EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + + EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); +};