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/13] 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/13] 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/13] 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/13] 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/13] 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/13] [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/13] 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/13] [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/13] 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/13] 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/13] 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/13] 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/13] 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());