From c5197c09d18aa79140e743aee72a220608917446 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 25 Nov 2024 22:46:54 +0000 Subject: [PATCH 1/6] Add clang job --- .github/workflows/build-binary.yml | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/.github/workflows/build-binary.yml b/.github/workflows/build-binary.yml index c0a587a2..871e5bac 100644 --- a/.github/workflows/build-binary.yml +++ b/.github/workflows/build-binary.yml @@ -25,3 +25,14 @@ jobs: ros_repo: ${{ matrix.ROS_REPO }} upstream_workspace: control_toolbox-not-released.${{ matrix.ROS_DISTRO }}.repos ref_for_scheduled_build: ros2-master + binary_clang: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@clang + with: + ros_distro: rolling + ros_repo: testing + upstream_workspace: control_toolbox-not-released.rolling.repos + ref_for_scheduled_build: ros2-master + additional_debs: clang + c_compiler: clang + cxx_compiler: clang++ + not_test_build: true From 7fec5509a77b4936470bdd06a857332778f01e0b Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 26 Nov 2024 06:35:56 +0000 Subject: [PATCH 2/6] Fix implicit conversion --- src/pid_ros.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/pid_ros.cpp b/src/pid_ros.cpp index 58d0f609..0cdef37d 100644 --- a/src/pid_ros.cpp +++ b/src/pid_ros.cpp @@ -221,7 +221,7 @@ std::shared_ptr> PidROS::getPidSt double PidROS::computeCommand(double error, rclcpp::Duration dt) { - double cmd_ = pid_.computeCommand(error, dt.nanoseconds()); + double cmd_ = pid_.computeCommand(error, static_cast(dt.nanoseconds())); publishPIDState(cmd_, error, dt); return cmd_; @@ -229,7 +229,7 @@ double PidROS::computeCommand(double error, rclcpp::Duration dt) double PidROS::computeCommand(double error, double error_dot, rclcpp::Duration dt) { - double cmd_ = pid_.computeCommand(error, error_dot, dt.nanoseconds()); + double cmd_ = pid_.computeCommand(error, error_dot, static_cast(dt.nanoseconds())); publishPIDState(cmd_, error, dt); return cmd_; From 439c27ab25eed1e60320b2f5821428f99f49ef09 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 26 Nov 2024 19:17:44 +0000 Subject: [PATCH 3/6] Fix Eigen::Index --- include/control_toolbox/low_pass_filter.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/control_toolbox/low_pass_filter.hpp b/include/control_toolbox/low_pass_filter.hpp index 07351bdd..9ab7be8e 100644 --- a/include/control_toolbox/low_pass_filter.hpp +++ b/include/control_toolbox/low_pass_filter.hpp @@ -147,7 +147,7 @@ bool LowPassFilter::configure() // Initialize storage Vectors filtered_value = filtered_old_value = old_value = 0; // TODO(destogl): make the size parameterizable and more intelligent is using complex types - for (size_t i = 0; i < 6; ++i) + for (Eigen::Index i = 0; i < 6; ++i) { msg_filtered[i] = msg_filtered_old[i] = msg_old[i] = 0; } From 915c861993bc177be09ae0b1d0bcd993de2b5084 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 26 Nov 2024 19:18:00 +0000 Subject: [PATCH 4/6] Fix dt --- src/pid_ros.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/pid_ros.cpp b/src/pid_ros.cpp index 0cdef37d..36e18d28 100644 --- a/src/pid_ros.cpp +++ b/src/pid_ros.cpp @@ -221,7 +221,7 @@ std::shared_ptr> PidROS::getPidSt double PidROS::computeCommand(double error, rclcpp::Duration dt) { - double cmd_ = pid_.computeCommand(error, static_cast(dt.nanoseconds())); + double cmd_ = pid_.computeCommand(error, static_cast(dt.nanoseconds())); publishPIDState(cmd_, error, dt); return cmd_; @@ -229,7 +229,7 @@ double PidROS::computeCommand(double error, rclcpp::Duration dt) double PidROS::computeCommand(double error, double error_dot, rclcpp::Duration dt) { - double cmd_ = pid_.computeCommand(error, error_dot, static_cast(dt.nanoseconds())); + double cmd_ = pid_.computeCommand(error, error_dot, static_cast(dt.nanoseconds())); publishPIDState(cmd_, error, dt); return cmd_; From 814ade3f4bd47c3aebe86673ead45112d070e02d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 27 Nov 2024 22:41:24 +0100 Subject: [PATCH 5/6] Update .github/workflows/build-binary.yml --- .github/workflows/build-binary.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build-binary.yml b/.github/workflows/build-binary.yml index 871e5bac..6c85872e 100644 --- a/.github/workflows/build-binary.yml +++ b/.github/workflows/build-binary.yml @@ -26,7 +26,7 @@ jobs: upstream_workspace: control_toolbox-not-released.${{ matrix.ROS_DISTRO }}.repos ref_for_scheduled_build: ros2-master binary_clang: - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@clang + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master with: ros_distro: rolling ros_repo: testing From 5f37cb28f508e45e02007490aeb5d0b523411726 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 7 Dec 2024 08:29:23 +0000 Subject: [PATCH 6/6] Explicitely cast dt --- test/pid_tests.cpp | 60 +++++++++++++++++++++++----------------------- 1 file changed, 30 insertions(+), 30 deletions(-) diff --git a/test/pid_tests.cpp b/test/pid_tests.cpp index 22e3e3af..31498fda 100644 --- a/test/pid_tests.cpp +++ b/test/pid_tests.cpp @@ -80,11 +80,11 @@ TEST(ParameterTest, integrationClampTest) double cmd = 0.0; // Test lower limit - cmd = pid.computeCommand(-10.03, 1.0 * 1e9); + cmd = pid.computeCommand(-10.03, static_cast(1.0 * 1e9)); EXPECT_EQ(-1.0, cmd); // Test upper limit - cmd = pid.computeCommand(30.0, 1.0 * 1e9); + cmd = pid.computeCommand(30.0, static_cast(1.0 * 1e9)); EXPECT_EQ(1.0, cmd); } @@ -104,13 +104,13 @@ TEST(ParameterTest, integrationClampZeroGainTest) double cmd = 0.0; double pe, ie, de; - cmd = pid.computeCommand(-1.0, 1.0 * 1e9); + cmd = pid.computeCommand(-1.0, static_cast(1.0 * 1e9)); pid.getCurrentPIDErrors(pe, ie, de); EXPECT_LE(i_min, cmd); EXPECT_LE(cmd, i_max); EXPECT_EQ(0.0, cmd); - cmd = pid.computeCommand(-1.0, 1.0 * 1e9); + cmd = pid.computeCommand(-1.0, static_cast(1.0 * 1e9)); EXPECT_LE(i_min, cmd); EXPECT_LE(cmd, i_max); EXPECT_EQ(0.0, cmd); @@ -129,16 +129,16 @@ TEST(ParameterTest, integrationAntiwindupTest) double cmd = 0.0; - cmd = pid.computeCommand(-1.0, 1.0 * 1e9); + cmd = pid.computeCommand(-1.0, static_cast(1.0 * 1e9)); EXPECT_EQ(-1.0, cmd); - cmd = pid.computeCommand(-1.0, 1.0 * 1e9); + cmd = pid.computeCommand(-1.0, static_cast(1.0 * 1e9)); EXPECT_EQ(-1.0, cmd); - cmd = pid.computeCommand(0.5, 1.0 * 1e9); + cmd = pid.computeCommand(0.5, static_cast(1.0 * 1e9)); EXPECT_EQ(0.0, cmd); - cmd = pid.computeCommand(-1.0, 1.0 * 1e9); + cmd = pid.computeCommand(-1.0, static_cast(1.0 * 1e9)); EXPECT_EQ(-1.0, cmd); } @@ -155,16 +155,16 @@ TEST(ParameterTest, negativeIntegrationAntiwindupTest) double cmd = 0.0; - cmd = pid.computeCommand(0.1, 1.0 * 1e9); + cmd = pid.computeCommand(0.1, static_cast(1.0 * 1e9)); EXPECT_EQ(-0.2, cmd); - cmd = pid.computeCommand(0.1, 1.0 * 1e9); + cmd = pid.computeCommand(0.1, static_cast(1.0 * 1e9)); EXPECT_EQ(-0.2, cmd); - cmd = pid.computeCommand(-0.05, 1.0 * 1e9); + cmd = pid.computeCommand(-0.05, static_cast(1.0 * 1e9)); EXPECT_EQ(-0.075, cmd); - cmd = pid.computeCommand(0.1, 1.0 * 1e9); + cmd = pid.computeCommand(0.1, static_cast(1.0 * 1e9)); EXPECT_EQ(-0.2, cmd); } @@ -223,7 +223,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) // Send update command to populate errors ------------------------------------------------- pid1.setCurrentCmd(10); - (void) pid1.computeCommand(20, 1.0 * 1e9); + (void) pid1.computeCommand(20, static_cast(1.0 * 1e9)); // Test copy constructor ------------------------------------------------- Pid pid2(pid1); @@ -291,22 +291,22 @@ TEST(CommandTest, proportionalOnlyTest) double cmd = 0.0; // If initial error = 0, p-gain = 1, dt = 1 - cmd = pid.computeCommand(-0.5, 1.0 * 1e9); + cmd = pid.computeCommand(-0.5, static_cast(1.0 * 1e9)); // Then expect command = error EXPECT_EQ(-0.5, cmd); // If call again - cmd = pid.computeCommand(-0.5, 1.0 * 1e9); + cmd = pid.computeCommand(-0.5, static_cast(1.0 * 1e9)); // Then expect the same as before EXPECT_EQ(-0.5, cmd); // If call again doubling the error - cmd = pid.computeCommand(-1.0, 1.0 * 1e9); + cmd = pid.computeCommand(-1.0, static_cast(1.0 * 1e9)); // Then expect the command doubled EXPECT_EQ(-1.0, cmd); // If call with positive error - cmd = pid.computeCommand(0.5, 1.0 * 1e9); + cmd = pid.computeCommand(0.5, static_cast(1.0 * 1e9)); // Then expect always command = error EXPECT_EQ(0.5, cmd); } @@ -323,26 +323,26 @@ TEST(CommandTest, integralOnlyTest) double cmd = 0.0; // If initial error = 0, i-gain = 1, dt = 1 - cmd = pid.computeCommand(-0.5, 1.0 * 1e9); + cmd = pid.computeCommand(-0.5, static_cast(1.0 * 1e9)); // Then expect command = error EXPECT_EQ(-0.5, cmd); // If call again with same arguments - cmd = pid.computeCommand(-0.5, 1.0 * 1e9); + cmd = pid.computeCommand(-0.5, static_cast(1.0 * 1e9)); // Then expect the integral part to double the command EXPECT_EQ(-1.0, cmd); // Call again with no error - cmd = pid.computeCommand(0.0, 1.0 * 1e9); + cmd = pid.computeCommand(0.0, static_cast(1.0 * 1e9)); // Expect the integral part to keep the previous command because it ensures error = 0 EXPECT_EQ(-1.0, cmd); // Double check that the integral contribution keep the previous command - cmd = pid.computeCommand(0.0, 1.0 * 1e9); + cmd = pid.computeCommand(0.0, static_cast(1.0 * 1e9)); EXPECT_EQ(-1.0, cmd); // Finally call again with positive error to see if the command changes in the opposite direction - cmd = pid.computeCommand(1.0, 1.0 * 1e9); + cmd = pid.computeCommand(1.0, static_cast(1.0 * 1e9)); // Expect that the command is cleared since error = -1 * previous command, i-gain = 1, dt = 1 EXPECT_EQ(0.0, cmd); } @@ -359,27 +359,27 @@ TEST(CommandTest, derivativeOnlyTest) double cmd = 0.0; // If initial error = 0, d-gain = 1, dt = 1 - cmd = pid.computeCommand(-0.5, 1.0 * 1e9); + cmd = pid.computeCommand(-0.5, static_cast(1.0 * 1e9)); // Then expect command = error EXPECT_EQ(-0.5, cmd); // If call again with same error - cmd = pid.computeCommand(-0.5, 1.0 * 1e9); + cmd = pid.computeCommand(-0.5, static_cast(1.0 * 1e9)); // Then expect command = 0 due to no variation on error EXPECT_EQ(0.0, cmd); // If call again with same error and smaller control period - cmd = pid.computeCommand(-0.5, 0.1 * 1e9); + cmd = pid.computeCommand(-0.5, static_cast(0.1 * 1e9)); // Then expect command = 0 again EXPECT_EQ(0.0, cmd); // If the error increases, with dt = 1 - cmd = pid.computeCommand(-1.0, 1.0 * 1e9); + cmd = pid.computeCommand(-1.0, static_cast(1.0 * 1e9)); // Then expect the command = change in dt EXPECT_EQ(-0.5, cmd); // If error decreases, with dt = 1 - cmd = pid.computeCommand(-0.5, 1.0 * 1e9); + cmd = pid.computeCommand(-0.5, static_cast(1.0 * 1e9)); // Then expect always the command = change in dt (note the sign flip) EXPECT_EQ(0.5, cmd); } @@ -396,17 +396,17 @@ TEST(CommandTest, completePIDTest) // All contributions are tested, here few tests check that they sum up correctly // If initial error = 0, all gains = 1, dt = 1 - cmd = pid.computeCommand(-0.5, 1.0 * 1e9); + cmd = pid.computeCommand(-0.5, static_cast(1.0 * 1e9)); // Then expect command = 3x error EXPECT_EQ(-1.5, cmd); // If call again with same arguments, no error change, but integration do its part - cmd = pid.computeCommand(-0.5, 1.0 * 1e9); + cmd = pid.computeCommand(-0.5, static_cast(1.0 * 1e9)); // Then expect command = 3x error again EXPECT_EQ(-1.5, cmd); // If call again increasing the error - cmd = pid.computeCommand(-1.0, 1.0 * 1e9); + cmd = pid.computeCommand(-1.0, static_cast(1.0 * 1e9)); // Then expect command equals to p = -1, i = -2.0 (i.e. - 0.5 - 0.5 - 1.0), d = -0.5 EXPECT_EQ(-3.5, cmd); }