diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index efcf82afdae..122e25944a9 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 diff --git a/.github/workflows/reviewer_lottery.yml b/.github/workflows/reviewer_lottery.yml index 2edbc9b59e8..ed28964e013 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 diff --git a/controller_manager/controller_manager/launch_utils.py b/controller_manager/controller_manager/launch_utils.py index 93941cc11fa..5a23c02ceca 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/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index a05f6a3afc4..46c46fa0280 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/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 1561c9c2430..90ac48b5550 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1007,7 +1007,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; diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index e7f88f2c206..2747e79a1b5 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 diff --git a/hardware_interface/include/hardware_interface/component_parser.hpp b/hardware_interface/include/hardware_interface/component_parser.hpp index 5112f7927ef..d5d999cca83 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 00000000000..e0333756f4e --- /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 8e9b6bf16b0..4f67d3e8b68 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 f4aee6c8a6d..22d8aa573ca 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); } } } diff --git a/hardware_interface/test/test_components/test_actuator.cpp b/hardware_interface/test/test_components/test_actuator.cpp index 7cf55b50d3a..2f606b74cb2 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 235fec668fc..f029d3dd372 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 ae31c734366..48112441384 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 3326cb893b8..bc7a75df6f9 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 8246cc207d3..84519c20fa1 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 05ffad00d62..a2f6195c673 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) + diff --git a/ros2controlcli/doc/userdoc.rst b/ros2controlcli/doc/userdoc.rst index 5eada2acb6b..6dadd312269 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 diff --git a/transmission_interface/test/differential_transmission_loader_test.cpp b/transmission_interface/test/differential_transmission_loader_test.cpp index d2ed5336525..70d0adf2cdd 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 7e27194bb64..14ffe968bc8 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 d5960ab47c9..53b584b7b57 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 a44feb178c4..f74e4def6a9 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 1518bacc3bb..4623d8c409b 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 d7601cdd205..33a14f92d19 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