diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 203fc4b6fc..9d862f8038 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -46,7 +46,7 @@ jobs: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v3.1.3 + - uses: actions/upload-artifact@v4.0.0 with: name: colcon-logs-ubuntu-22.04-coverage-rolling path: ros_ws/log diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 879946fced..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 @@ -49,7 +50,7 @@ jobs: https://raw.githubusercontent.com/ros2/ros2/${{ inputs.ros2_repo_branch }}/ros2.repos https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_control.${{ inputs.ros_distro }}.repos?token=${{ secrets.GITHUB_TOKEN }} colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: actions/upload-artifact@v3.1.3 + - uses: actions/upload-artifact@v4.0.0 with: name: colcon-logs-ubuntu-22.04 path: ros_ws/log 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 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/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/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; 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 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 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