diff --git a/CMakeLists.txt b/CMakeLists.txt index ba544bcd..ef06902c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,6 +13,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(control_msgs REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) find_package(realtime_tools REQUIRED) add_library(${PROJECT_NAME} SHARED @@ -25,6 +26,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC include) ament_target_dependencies(${PROJECT_NAME} control_msgs rclcpp + rclcpp_lifecycle realtime_tools) if(BUILD_TESTING) @@ -43,6 +45,7 @@ if(BUILD_TESTING) ament_add_gtest(pid_publisher_tests test/pid_publisher_tests.cpp) target_link_libraries(pid_publisher_tests ${PROJECT_NAME}) + ament_target_dependencies(pid_publisher_tests rclcpp_lifecycle) endif() # Install @@ -55,7 +58,9 @@ install(TARGETS ${PROJECT_NAME} RUNTIME DESTINATION bin) ament_export_dependencies( + control_msgs rclcpp + rclcpp_lifecycle realtime_tools) ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) diff --git a/include/control_toolbox/pid_ros.hpp b/include/control_toolbox/pid_ros.hpp index 4d766c0a..ae4f716b 100644 --- a/include/control_toolbox/pid_ros.hpp +++ b/include/control_toolbox/pid_ros.hpp @@ -31,8 +31,8 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#ifndef CONTROL_TOOLBOX__PIDROS_HPP_ -#define CONTROL_TOOLBOX__PIDROS_HPP_ +#ifndef CONTROL_TOOLBOX__PID_ROS_HPP_ +#define CONTROL_TOOLBOX__PID_ROS_HPP_ #include #include @@ -177,9 +177,9 @@ class PidROS } // namespace control_toolbox -#ifndef CONTROL_TOOLBOX__PIDROS_IMPL_HPP_ +#ifndef CONTROL_TOOLBOX__PID_ROS_IMPL_HPP_ // Template implementations #include "pid_ros_impl.hpp" #endif -#endif // CONTROL_TOOLBOX__PIDROS_HPP_ +#endif // CONTROL_TOOLBOX__PID_ROS_HPP_ diff --git a/include/control_toolbox/pid_ros_impl.hpp b/include/control_toolbox/pid_ros_impl.hpp index e516113b..2657fa24 100644 --- a/include/control_toolbox/pid_ros_impl.hpp +++ b/include/control_toolbox/pid_ros_impl.hpp @@ -31,23 +31,26 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#ifndef CONTROL_TOOLBOX__PIDROS_IMPL_HPP_ -#define CONTROL_TOOLBOX__PIDROS_IMPL_HPP_ +#ifndef CONTROL_TOOLBOX__PID_ROS_IMPL_HPP_ +#define CONTROL_TOOLBOX__PID_ROS_IMPL_HPP_ #include #include +#include #include #include #include #include "control_toolbox/pid_ros.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rclcpp_lifecycle/lifecycle_publisher.hpp" namespace control_toolbox { template PidROS::PidROS(std::shared_ptr node_ptr, std::string topic_prefix) -:node_(node_ptr) +: node_(node_ptr) { if (topic_prefix.back() != '.' && !topic_prefix.empty()) { topic_prefix_ = topic_prefix + "."; @@ -55,8 +58,16 @@ PidROS::PidROS(std::shared_ptr node_ptr, std::string topic_prefix) topic_prefix_ = topic_prefix; } - state_pub_ = node_->create_publisher( + state_pub_ = node_->template create_publisher( topic_prefix + "/pid_state", rclcpp::SensorDataQoS()); + rclcpp_lifecycle::LifecycleNode::SharedPtr is_lifecycle_node = + std::dynamic_pointer_cast(node_); + if (is_lifecycle_node) { + auto state_pub_lifecycle_ = + std::dynamic_pointer_cast>( + state_pub_); + state_pub_lifecycle_->on_activate(); + } rt_state_pub_.reset( new realtime_tools::RealtimePublisher(state_pub_)); } @@ -69,7 +80,9 @@ PidROS::getBooleanParam(const std::string & param_name, bool & value) if (node_->has_parameter(param_name)) { node_->get_parameter(param_name, param); if (rclcpp::PARAMETER_BOOL != param.get_type()) { - RCLCPP_ERROR(node_->get_logger(), "Wrong parameter type '%s', not boolean", param_name.c_str()); + RCLCPP_ERROR( + node_->get_logger(), "Wrong parameter type '%s', not boolean", + param_name.c_str()); return false; } value = param.as_bool(); @@ -87,7 +100,9 @@ PidROS::getDoubleParam(const std::string & param_name, double & value) if (node_->has_parameter(param_name)) { node_->get_parameter(param_name, param); if (rclcpp::PARAMETER_DOUBLE != param.get_type()) { - RCLCPP_ERROR(node_->get_logger(), "Wrong parameter type '%s', not double", param_name.c_str()); + RCLCPP_ERROR( + node_->get_logger(), "Wrong parameter type '%s', not double", + param_name.c_str()); return false; } value = param.as_double(); @@ -327,9 +342,9 @@ PidROS::setParameterEventCallback() parameter_callback_ = node_->get_node_parameters_interface()->add_on_set_parameters_callback( - on_parameter_event_callback); + on_parameter_event_callback); } } // namespace control_toolbox -#endif // CONTROL_TOOLBOX__PIDROS_IMPL_HPP_ +#endif // CONTROL_TOOLBOX__PID_ROS_IMPL_HPP_ diff --git a/package.xml b/package.xml index 42914c5f..32c63635 100644 --- a/package.xml +++ b/package.xml @@ -20,6 +20,7 @@ control_msgs rclcpp + rclcpp_lifecycle realtime_tools ament_cmake_gmock diff --git a/src/pid.cpp b/src/pid.cpp index 7825312f..4e276e3e 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -140,7 +140,7 @@ double Pid::computeCommand(double error, uint64_t dt) error_dot_ = d_error_; // Calculate the derivative error - error_dot_ = (error - p_error_last_) / (dt/1e9); + error_dot_ = (error - p_error_last_) / (dt / 1e9); p_error_last_ = error; return computeCommand(error, error_dot_, dt); @@ -166,7 +166,7 @@ double Pid::computeCommand(double error, double error_dot, uint64_t dt) p_term = gains.p_gain_ * p_error_; // Calculate the integral of the position error - i_error_ += (dt/1e9) * p_error_; + i_error_ += (dt / 1e9) * p_error_; if (gains.antiwindup_ && gains.i_gain_ != 0) { // Prevent i_error_ from climbing higher than permitted by i_max_/i_min_ diff --git a/test/pid_publisher_tests.cpp b/test/pid_publisher_tests.cpp index d0e4b788..7067bb80 100644 --- a/test/pid_publisher_tests.cpp +++ b/test/pid_publisher_tests.cpp @@ -65,6 +65,40 @@ TEST(PidPublihserTest, PublishTest) ASSERT_TRUE(callback_called); } +TEST(PidPublihserTest, PublishTestLifecycle) +{ + const size_t ATTEMPTS = 100; + const std::chrono::milliseconds DELAY(250); + + auto node = std::make_shared("pid_publisher_test"); + + control_toolbox::PidROS pid_ros(node); + + pid_ros.initPid(1.0, 1.0, 1.0, 5.0, -5.0, false); + + bool callback_called = false; + control_msgs::msg::PidState::SharedPtr last_state_msg; + auto state_callback = [&](const control_msgs::msg::PidState::SharedPtr) + { + callback_called = true; + }; + + auto state_sub = node->create_subscription( + "/pid_state", rclcpp::SensorDataQoS(), state_callback); + + double command = pid_ros.computeCommand(-0.5, rclcpp::Duration(1, 0)); + EXPECT_EQ(-1.5, command); + + // wait for callback + for (size_t i = 0; i < ATTEMPTS && !callback_called; ++i) { + pid_ros.computeCommand(-0.5, rclcpp::Duration(1, 0)); + rclcpp::spin_some(node->get_node_base_interface()); + std::this_thread::sleep_for(DELAY); + } + + ASSERT_TRUE(callback_called); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/test/pid_tests.cpp b/test/pid_tests.cpp index eee46d2c..2020a267 100644 --- a/test/pid_tests.cpp +++ b/test/pid_tests.cpp @@ -53,12 +53,12 @@ TEST(ParameterTest, ITermBadIBoundsTest) double cmd = 0.0; double pe, ie, de; - cmd = pid.computeCommand(-1.0, 1.0); + cmd = pid.computeCommand(-1.0, 1.0 * 1e9); pid.getCurrentPIDErrors(pe, ie, de); EXPECT_FALSE(std::isinf(ie)); EXPECT_FALSE(std::isnan(cmd)); - cmd = pid.computeCommand(-1.0, 1.0); + cmd = pid.computeCommand(-1.0, 1.0 * 1e9); pid.getCurrentPIDErrors(pe, ie, de); EXPECT_FALSE(std::isinf(ie)); EXPECT_FALSE(std::isnan(cmd)); @@ -76,11 +76,11 @@ TEST(ParameterTest, integrationClampTest) double cmd = 0.0; // Test lower limit - cmd = pid.computeCommand(-10.03, 1.0); + cmd = pid.computeCommand(-10.03, 1.0 * 1e9); EXPECT_EQ(-1.0, cmd); // Test upper limit - cmd = pid.computeCommand(30.0, 1.0); + cmd = pid.computeCommand(30.0, 1.0 * 1e9); EXPECT_EQ(1.0, cmd); } @@ -100,13 +100,13 @@ TEST(ParameterTest, integrationClampZeroGainTest) double cmd = 0.0; double pe, ie, de; - cmd = pid.computeCommand(-1.0, 1.0); + cmd = pid.computeCommand(-1.0, 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); + cmd = pid.computeCommand(-1.0, 1.0 * 1e9); EXPECT_LE(i_min, cmd); EXPECT_LE(cmd, i_max); EXPECT_EQ(0.0, cmd); @@ -125,16 +125,16 @@ TEST(ParameterTest, integrationAntiwindupTest) double cmd = 0.0; - cmd = pid.computeCommand(-1.0, 1.0); + cmd = pid.computeCommand(-1.0, 1.0 * 1e9); EXPECT_EQ(-1.0, cmd); - cmd = pid.computeCommand(-1.0, 1.0); + cmd = pid.computeCommand(-1.0, 1.0 * 1e9); EXPECT_EQ(-1.0, cmd); - cmd = pid.computeCommand(0.5, 1.0); + cmd = pid.computeCommand(0.5, 1.0 * 1e9); EXPECT_EQ(0.0, cmd); - cmd = pid.computeCommand(-1.0, 1.0); + cmd = pid.computeCommand(-1.0, 1.0 * 1e9); EXPECT_EQ(-1.0, cmd); } @@ -151,16 +151,16 @@ TEST(ParameterTest, negativeIntegrationAntiwindupTest) double cmd = 0.0; - cmd = pid.computeCommand(0.1, 1.0); + cmd = pid.computeCommand(0.1, 1.0 * 1e9); EXPECT_EQ(-0.2, cmd); - cmd = pid.computeCommand(0.1, 1.0); + cmd = pid.computeCommand(0.1, 1.0 * 1e9); EXPECT_EQ(-0.2, cmd); - cmd = pid.computeCommand(-0.05, 1.0); + cmd = pid.computeCommand(-0.05, 1.0 * 1e9); EXPECT_EQ(-0.075, cmd); - cmd = pid.computeCommand(0.1, 1.0); + cmd = pid.computeCommand(0.1, 1.0 * 1e9); EXPECT_EQ(-0.2, cmd); } @@ -219,7 +219,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) // Send update command to populate errors ------------------------------------------------- pid1.setCurrentCmd(10); - pid1.computeCommand(20, 1.0); + pid1.computeCommand(20, 1.0 * 1e9); // Test copy constructor ------------------------------------------------- Pid pid2(pid1); @@ -287,22 +287,22 @@ TEST(CommandTest, proportionalOnlyTest) double cmd = 0.0; // If initial error = 0, p-gain = 1, dt = 1 - cmd = pid.computeCommand(-0.5, 1.0); + cmd = pid.computeCommand(-0.5, 1.0 * 1e9); // Then expect command = error EXPECT_EQ(-0.5, cmd); // If call again - cmd = pid.computeCommand(-0.5, 1.0); + cmd = pid.computeCommand(-0.5, 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); + cmd = pid.computeCommand(-1.0, 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); + cmd = pid.computeCommand(0.5, 1.0 * 1e9); // Then expect always command = error EXPECT_EQ(0.5, cmd); } @@ -319,26 +319,26 @@ TEST(CommandTest, integralOnlyTest) double cmd = 0.0; // If initial error = 0, i-gain = 1, dt = 1 - cmd = pid.computeCommand(-0.5, 1.0); + cmd = pid.computeCommand(-0.5, 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); + cmd = pid.computeCommand(-0.5, 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); + cmd = pid.computeCommand(0.0, 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); + cmd = pid.computeCommand(0.0, 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); + cmd = pid.computeCommand(1.0, 1.0 * 1e9); // Expect that the command is cleared since error = -1 * previous command, i-gain = 1, dt = 1 EXPECT_EQ(0.0, cmd); } @@ -355,27 +355,27 @@ TEST(CommandTest, derivativeOnlyTest) double cmd = 0.0; // If initial error = 0, d-gain = 1, dt = 1 - cmd = pid.computeCommand(-0.5, 1.0); + cmd = pid.computeCommand(-0.5, 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); + cmd = pid.computeCommand(-0.5, 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); + cmd = pid.computeCommand(-0.5, 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); + cmd = pid.computeCommand(-1.0, 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); + cmd = pid.computeCommand(-0.5, 1.0 * 1e9); // Then expect always the command = change in dt (note the sign flip) EXPECT_EQ(0.5, cmd); } @@ -392,17 +392,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); + cmd = pid.computeCommand(-0.5, 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); + cmd = pid.computeCommand(-0.5, 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); + cmd = pid.computeCommand(-1.0, 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); }