From 6c90a2064a97b2995c7023970704049c41064c2b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 8 Dec 2022 18:50:44 +0100 Subject: [PATCH 1/6] Enable setting prefix as param prefix. --- include/control_toolbox/pid_ros.hpp | 22 ++++++++++++++++------ src/pid_ros.cpp | 22 +++++++++++++++++----- 2 files changed, 33 insertions(+), 11 deletions(-) diff --git a/include/control_toolbox/pid_ros.hpp b/include/control_toolbox/pid_ros.hpp index f39cd499..3ccc5c2c 100644 --- a/include/control_toolbox/pid_ros.hpp +++ b/include/control_toolbox/pid_ros.hpp @@ -61,16 +61,19 @@ class CONTROL_TOOLBOX_PUBLIC PidROS * to add a prefix to the pid parameters * * \param node ROS node - * \param topic_prefix prefix to add to the pid parameters. + * \param prefix prefix to add to the pid parameters. + * Per default is prefix interpreted as prefix for topics. + * \param prefix_is_for_params provided prefix should be interpreted as prefix for parameters. + * See `initialize` for details. */ template - explicit PidROS(std::shared_ptr node_ptr, std::string topic_prefix = std::string("")) + explicit PidROS(std::shared_ptr node_ptr, std::string prefix = std::string(""), bool prefix_is_for_params = false) : PidROS( node_ptr->get_node_base_interface(), node_ptr->get_node_logging_interface(), node_ptr->get_node_parameters_interface(), node_ptr->get_node_topics_interface(), - topic_prefix) + prefix, prefix_is_for_params) { } @@ -79,13 +82,13 @@ class CONTROL_TOOLBOX_PUBLIC PidROS rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_params, rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics_interface, - std::string topic_prefix = std::string("")) + std::string prefix = std::string(""), bool prefix_is_for_params = false) : node_base_(node_base), node_logging_(node_logging), node_params_(node_params), topics_interface_(topics_interface) { - initialize(topic_prefix); + initialize(prefix, prefix_is_for_params); } /*! @@ -211,7 +214,14 @@ class CONTROL_TOOLBOX_PUBLIC PidROS bool getBooleanParam(const std::string & param_name, bool & value); - void initialize(std::string topic_prefix); + /*! + * \param prefix prefix to add to the pid parameters. + * Per default is prefix interpreted as prefix for topics. + * \param prefix_is_for_params provided prefix should be interpreted as prefix for parameters. + * If the parameter is `true` then "/" will not be replaced with "." for parameters prefix, + * and "/" will be added to topic prefix, if prefix is not starting with "/" or "~". + */ + void initialize(std::string prefix, bool prefix_is_for_params = false); rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameter_callback_; diff --git a/src/pid_ros.cpp b/src/pid_ros.cpp index 432e8939..9f0551ca 100644 --- a/src/pid_ros.cpp +++ b/src/pid_ros.cpp @@ -43,9 +43,9 @@ namespace control_toolbox { -void PidROS::initialize(std::string topic_prefix) +void PidROS::initialize(std::string prefix, bool prefix_is_for_params) { - param_prefix_ = topic_prefix; + param_prefix_ = prefix; // If it starts with a "~", remove it if (param_prefix_.compare(0, 1, "~") == 0) { param_prefix_.erase(0, 1); @@ -54,14 +54,26 @@ void PidROS::initialize(std::string topic_prefix) if (param_prefix_.compare(0, 1, "/") == 0) { param_prefix_.erase(0, 1); } - // Replace namespacing separator from "/" to "." in parameters - std::replace(param_prefix_.begin(), param_prefix_.end(), '/', '.'); + if (!prefix_is_for_params) + { + // Replace namespacing separator from "/" to "." in parameters + std::replace(param_prefix_.begin(), param_prefix_.end(), '/', '.'); + } // Add a trailing "." if (!param_prefix_.empty() && param_prefix_.back() != '.') { param_prefix_.append("."); } - topic_prefix_ = topic_prefix; + topic_prefix_ = prefix; + if (prefix_is_for_params) + { + // Add local namespace is global is not defined + if (param_prefix_.compare(0, 1, "~") != 0 && param_prefix_.compare(0, 1, "/") != 0) + { + topic_prefix_ = "/" + topic_prefix_; + } + } + // Replace parameter separator from "." to "/" in topics std::replace(topic_prefix_.begin(), topic_prefix_.end(), '.', '/'); // Add a trailing "/" From ee9f34bf495bd33f50b3d0ba0a01aa49e80dbebf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 14 Feb 2023 17:35:30 +0100 Subject: [PATCH 2/6] Fixup formatting. --- include/control_toolbox/pid_ros.hpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/include/control_toolbox/pid_ros.hpp b/include/control_toolbox/pid_ros.hpp index 3ccc5c2c..5db7175b 100644 --- a/include/control_toolbox/pid_ros.hpp +++ b/include/control_toolbox/pid_ros.hpp @@ -67,7 +67,11 @@ class CONTROL_TOOLBOX_PUBLIC PidROS * See `initialize` for details. */ template - explicit PidROS(std::shared_ptr node_ptr, std::string prefix = std::string(""), bool prefix_is_for_params = false) + explicit PidROS( + std::shared_ptr node_ptr, + std::string prefix = std::string(""), + bool prefix_is_for_params = false + ) : PidROS( node_ptr->get_node_base_interface(), node_ptr->get_node_logging_interface(), From 505e067327f029ff2cf0aa861493d411ce639e20 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 14 Feb 2023 20:47:41 +0100 Subject: [PATCH 3/6] Addtests for prefix-if-for-params flag and optimize tests a bit. --- include/control_toolbox/pid_ros.hpp | 13 ++- src/pid_ros.cpp | 14 +-- test/pid_parameters_tests.cpp | 154 +++++++++++++++++++--------- 3 files changed, 119 insertions(+), 62 deletions(-) diff --git a/include/control_toolbox/pid_ros.hpp b/include/control_toolbox/pid_ros.hpp index 5db7175b..d8e81648 100644 --- a/include/control_toolbox/pid_ros.hpp +++ b/include/control_toolbox/pid_ros.hpp @@ -207,6 +207,10 @@ class CONTROL_TOOLBOX_PUBLIC PidROS return parameter_callback_; } +protected: + std::string topic_prefix_; + std::string param_prefix_; + private: void setParameterEventCallback(); @@ -221,9 +225,12 @@ class CONTROL_TOOLBOX_PUBLIC PidROS /*! * \param prefix prefix to add to the pid parameters. * Per default is prefix interpreted as prefix for topics. + * If not stated explicitly using "/" or "~", prefix is interpreted as global, i.e., + * "/" will be added in front of topic prefix, if prefix is not starting with + * "/" or "~". * \param prefix_is_for_params provided prefix should be interpreted as prefix for parameters. - * If the parameter is `true` then "/" will not be replaced with "." for parameters prefix, - * and "/" will be added to topic prefix, if prefix is not starting with "/" or "~". + * If the parameter is `true` then "/" in the middle of the string will not be replaced + * with "." for parameters prefix. ""/" at the begin will be removed. */ void initialize(std::string prefix, bool prefix_is_for_params = false); @@ -238,8 +245,6 @@ class CONTROL_TOOLBOX_PUBLIC PidROS std::shared_ptr> state_pub_; Pid pid_; - std::string topic_prefix_; - std::string param_prefix_; }; } // namespace control_toolbox diff --git a/src/pid_ros.cpp b/src/pid_ros.cpp index 9f0551ca..fc51eeed 100644 --- a/src/pid_ros.cpp +++ b/src/pid_ros.cpp @@ -65,21 +65,17 @@ void PidROS::initialize(std::string prefix, bool prefix_is_for_params) } topic_prefix_ = prefix; - if (prefix_is_for_params) - { - // Add local namespace is global is not defined - if (param_prefix_.compare(0, 1, "~") != 0 && param_prefix_.compare(0, 1, "/") != 0) - { - topic_prefix_ = "/" + topic_prefix_; - } - } - // Replace parameter separator from "." to "/" in topics std::replace(topic_prefix_.begin(), topic_prefix_.end(), '.', '/'); // Add a trailing "/" if (!topic_prefix_.empty() && topic_prefix_.back() != '/') { topic_prefix_.append("/"); } + // Add global namespace if none is defined + if (topic_prefix_.compare(0, 1, "~") != 0 && topic_prefix_.compare(0, 1, "/") != 0) + { + topic_prefix_ = "/" + topic_prefix_; + } state_pub_ = rclcpp::create_publisher( topics_interface_, diff --git a/test/pid_parameters_tests.cpp b/test/pid_parameters_tests.cpp index 0e08909d..3e655138 100644 --- a/test/pid_parameters_tests.cpp +++ b/test/pid_parameters_tests.cpp @@ -24,40 +24,62 @@ using rclcpp::executors::MultiThreadedExecutor; -TEST(PidParametersTest, InitPidTest) +class TestablePidROS : public control_toolbox::PidROS +{ + FRIEND_TEST(PidParametersTest, InitPidTest); + FRIEND_TEST(PidParametersTest, InitPid_when_not_prefix_for_params_then_replace_slash_with_dot); + FRIEND_TEST(PidParametersTest, InitPid_when_prefix_for_params_then_dont_replace_slash_with_dot); + FRIEND_TEST( + PidParametersTest, + InitPid_when_not_prefix_for_params_then_replace_slash_with_dot_leading_slash); + FRIEND_TEST( + PidParametersTest, + InitPid_when_prefix_for_params_then_dont_replace_slash_with_dot_leading_slash); + +public: + template + TestablePidROS(std::shared_ptr node_ptr, + std::string prefix = std::string(""), + bool prefix_is_for_params = false) + : control_toolbox::PidROS(node_ptr, prefix, prefix_is_for_params) + {} +}; + +void check_set_parameters( + const rclcpp::Node::SharedPtr & node, + control_toolbox::PidROS & pid, + const std::string & prefix = "" +) { - rclcpp::Node::SharedPtr node = std::make_shared("pid_parameters_test"); - - control_toolbox::PidROS pid(node); - const double P = 1.0; const double I = 2.0; const double D = 3.0; const double I_MAX = 10.0; const double I_MIN = -10.0; + const bool ANTIWINDUP = true; - ASSERT_NO_THROW(pid.initPid(P, I, D, I_MAX, I_MIN, false)); + ASSERT_NO_THROW(pid.initPid(P, I, D, I_MAX, I_MIN, ANTIWINDUP)); rclcpp::Parameter param; // check parameters were set - ASSERT_TRUE(node->get_parameter("p", param)); + ASSERT_TRUE(node->get_parameter(prefix + "p", param)); ASSERT_EQ(param.get_value(), P); - ASSERT_TRUE(node->get_parameter("i", param)); + ASSERT_TRUE(node->get_parameter(prefix + "i", param)); ASSERT_EQ(param.get_value(), I); - ASSERT_TRUE(node->get_parameter("d", param)); + ASSERT_TRUE(node->get_parameter(prefix + "d", param)); ASSERT_EQ(param.get_value(), D); - ASSERT_TRUE(node->get_parameter("i_clamp_max", param)); + ASSERT_TRUE(node->get_parameter(prefix + "i_clamp_max", param)); ASSERT_EQ(param.get_value(), I_MAX); - ASSERT_TRUE(node->get_parameter("i_clamp_min", param)); + ASSERT_TRUE(node->get_parameter(prefix + "i_clamp_min", param)); ASSERT_EQ(param.get_value(), I_MIN); - ASSERT_TRUE(node->get_parameter("antiwindup", param)); - ASSERT_FALSE(param.get_value()); + ASSERT_TRUE(node->get_parameter(prefix + "antiwindup", param)); + ASSERT_EQ(param.get_value(), ANTIWINDUP); // check gains were set control_toolbox::Pid::Gains gains = pid.getGains(); @@ -66,59 +88,92 @@ TEST(PidParametersTest, InitPidTest) ASSERT_EQ(gains.d_gain_, D); ASSERT_EQ(gains.i_max_, I_MAX); ASSERT_EQ(gains.i_min_, I_MIN); - ASSERT_FALSE(gains.antiwindup_); + ASSERT_TRUE(gains.antiwindup_); } -TEST(PidParametersTest, InitPidWithAntiwindupTest) +TEST(PidParametersTest, InitPidTest) { rclcpp::Node::SharedPtr node = std::make_shared("pid_parameters_test"); - control_toolbox::PidROS pid(node); + TestablePidROS pid(node); - const double P = 1.0; - const double I = 2.0; - const double D = 3.0; - const double I_MAX = 10.0; - const double I_MIN = -10.0; - const bool ANTIWINDUP = true; + ASSERT_EQ(pid.topic_prefix_, "/"); + ASSERT_EQ(pid.param_prefix_, ""); - pid.initPid(P, I, D, I_MAX, I_MIN, ANTIWINDUP); + check_set_parameters(node, pid); +} - rclcpp::Parameter param; +TEST(PidParametersTest, InitPid_when_not_prefix_for_params_then_replace_slash_with_dot) +{ + const std::string INPUT_PREFIX = "slash/to/dots"; + const std::string RESULTING_TOPIC_PREFIX = "/" + INPUT_PREFIX + "/"; + const std::string RESULTING_PARAM_PREFIX = "slash.to.dots."; - ASSERT_TRUE(node->get_parameter("p", param)); - ASSERT_EQ(param.get_value(), P); + rclcpp::Node::SharedPtr node = std::make_shared("pid_parameters_test"); - ASSERT_TRUE(node->get_parameter("i", param)); - ASSERT_EQ(param.get_value(), I); + TestablePidROS pid(node, INPUT_PREFIX); // default is false - ASSERT_TRUE(node->get_parameter("d", param)); - ASSERT_EQ(param.get_value(), D); + ASSERT_EQ(pid.topic_prefix_, RESULTING_TOPIC_PREFIX); + ASSERT_EQ(pid.param_prefix_, RESULTING_PARAM_PREFIX); - ASSERT_TRUE(node->get_parameter("i_clamp_max", param)); - ASSERT_EQ(param.get_value(), I_MAX); + check_set_parameters(node, pid, RESULTING_PARAM_PREFIX); +} - ASSERT_TRUE(node->get_parameter("i_clamp_min", param)); - ASSERT_EQ(param.get_value(), I_MIN); +TEST(PidParametersTest, InitPid_when_prefix_for_params_then_dont_replace_slash_with_dot) +{ + const std::string INPUT_PREFIX = "slash/to/dots"; + const std::string RESULTING_TOPIC_PREFIX = "/" + INPUT_PREFIX + "/"; + const std::string RESULTING_PARAM_PREFIX = INPUT_PREFIX + "."; - ASSERT_TRUE(node->get_parameter("antiwindup", param)); - ASSERT_EQ(param.get_value(), ANTIWINDUP); + rclcpp::Node::SharedPtr node = std::make_shared("pid_parameters_test"); - // check gains were set - control_toolbox::Pid::Gains gains = pid.getGains(); - ASSERT_EQ(gains.p_gain_, P); - ASSERT_EQ(gains.i_gain_, I); - ASSERT_EQ(gains.d_gain_, D); - ASSERT_EQ(gains.i_max_, I_MAX); - ASSERT_EQ(gains.i_min_, I_MIN); - ASSERT_EQ(gains.antiwindup_, ANTIWINDUP); + TestablePidROS pid(node, INPUT_PREFIX, true); // prefix is for parameters + + ASSERT_EQ(pid.topic_prefix_, RESULTING_TOPIC_PREFIX); + ASSERT_EQ(pid.param_prefix_, RESULTING_PARAM_PREFIX); + + check_set_parameters(node, pid, RESULTING_PARAM_PREFIX); +} + +TEST( + PidParametersTest, InitPid_when_not_prefix_for_params_then_replace_slash_with_dot_leading_slash) +{ + const std::string INPUT_PREFIX = "/slash/to/dots"; + const std::string RESULTING_TOPIC_PREFIX = INPUT_PREFIX + "/"; + const std::string RESULTING_PARAM_PREFIX = "slash.to.dots."; + + rclcpp::Node::SharedPtr node = std::make_shared("pid_parameters_test"); + + TestablePidROS pid(node, INPUT_PREFIX); // default is false + + ASSERT_EQ(pid.topic_prefix_, RESULTING_TOPIC_PREFIX); + ASSERT_EQ(pid.param_prefix_, RESULTING_PARAM_PREFIX); + + check_set_parameters(node, pid, RESULTING_PARAM_PREFIX); +} + +TEST( + PidParametersTest, InitPid_when_prefix_for_params_then_dont_replace_slash_with_dot_leading_slash) +{ + const std::string INPUT_PREFIX = "/slash/to/dots"; + const std::string RESULTING_TOPIC_PREFIX = INPUT_PREFIX + "/"; + const std::string RESULTING_PARAM_PREFIX = INPUT_PREFIX.substr(1) + "."; + + rclcpp::Node::SharedPtr node = std::make_shared("pid_parameters_test"); + + TestablePidROS pid(node, INPUT_PREFIX, true); // prefix is for parameters + + ASSERT_EQ(pid.topic_prefix_, RESULTING_TOPIC_PREFIX); + ASSERT_EQ(pid.param_prefix_, RESULTING_PARAM_PREFIX); + + check_set_parameters(node, pid, RESULTING_PARAM_PREFIX); } TEST(PidParametersTest, SetParametersTest) { rclcpp::Node::SharedPtr node = std::make_shared("pid_parameters_test"); - control_toolbox::PidROS pid(node); + TestablePidROS pid(node); const double P = 1.0; const double I = 2.0; @@ -166,7 +221,7 @@ TEST(PidParametersTest, GetParametersTest) { rclcpp::Node::SharedPtr node = std::make_shared("pid_parameters_test"); - control_toolbox::PidROS pid(node); + TestablePidROS pid(node); const double P = 1.0; const double I = 2.0; @@ -203,7 +258,7 @@ TEST(PidParametersTest, GetParametersFromParams) { rclcpp::Node::SharedPtr node = std::make_shared("pid_parameters_test"); - control_toolbox::PidROS pid(node); + TestablePidROS pid(node); ASSERT_TRUE(pid.initPid()); @@ -232,8 +287,8 @@ TEST(PidParametersTest, MultiplePidInstances) { rclcpp::Node::SharedPtr node = std::make_shared("multiple_pid_instances"); - control_toolbox::PidROS pid_1(node, "PID_1"); - control_toolbox::PidROS pid_2(node, "PID_2"); + TestablePidROS pid_1(node, "PID_1"); + TestablePidROS pid_2(node, "PID_2"); const double P = 1.0; const double I = 2.0; @@ -255,5 +310,6 @@ int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); rclcpp::init(0, nullptr); + return RUN_ALL_TESTS(); } From 085bea574fe492b97d421ccde74224cd8c45cf9e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 4 Apr 2023 15:32:32 +0200 Subject: [PATCH 4/6] Restructure accoring to discussion with Bence. --- include/control_toolbox/pid_ros.hpp | 18 ++------ src/pid_ros.cpp | 72 +++++++++++++++++++++-------- test/pid_parameters_tests.cpp | 4 +- 3 files changed, 61 insertions(+), 33 deletions(-) diff --git a/include/control_toolbox/pid_ros.hpp b/include/control_toolbox/pid_ros.hpp index f4b2f21c..7f41d1d9 100644 --- a/include/control_toolbox/pid_ros.hpp +++ b/include/control_toolbox/pid_ros.hpp @@ -64,7 +64,9 @@ class CONTROL_TOOLBOX_PUBLIC PidROS * \param prefix prefix to add to the pid parameters. * Per default is prefix interpreted as prefix for topics. * \param prefix_is_for_params provided prefix should be interpreted as prefix for parameters. - * See `initialize` for details. + * If the parameter is `true` then "/" in the middle of the string will not be replaced + * with "." for parameters prefix. ""/" at the begin will be removed. + * */ template explicit PidROS( @@ -86,14 +88,7 @@ class CONTROL_TOOLBOX_PUBLIC PidROS rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_params, rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics_interface, - std::string prefix = std::string(""), bool prefix_is_for_params = false) - : node_base_(node_base), - node_logging_(node_logging), - node_params_(node_params), - topics_interface_(topics_interface) - { - initialize(prefix, prefix_is_for_params); - } + std::string prefix = std::string(""), bool prefix_is_for_params = false); /*! * \brief Initialize the PID controller and set the parameters @@ -227,11 +222,8 @@ class CONTROL_TOOLBOX_PUBLIC PidROS * If not stated explicitly using "/" or "~", prefix is interpreted as global, i.e., * "/" will be added in front of topic prefix, if prefix is not starting with * "/" or "~". - * \param prefix_is_for_params provided prefix should be interpreted as prefix for parameters. - * If the parameter is `true` then "/" in the middle of the string will not be replaced - * with "." for parameters prefix. ""/" at the begin will be removed. */ - void initialize(std::string prefix, bool prefix_is_for_params = false); + void initialize(std::string topic_prefix); rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameter_callback_; diff --git a/src/pid_ros.cpp b/src/pid_ros.cpp index 7e93361a..242c80a7 100644 --- a/src/pid_ros.cpp +++ b/src/pid_ros.cpp @@ -43,9 +43,58 @@ namespace control_toolbox { -void PidROS::initialize(std::string prefix, bool prefix_is_for_params) +PidROS::PidROS( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_params, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics_interface, + std::string prefix, bool prefix_is_for_params) +: node_base_(node_base), node_logging_(node_logging), node_params_(node_params), + topics_interface_(topics_interface) { - param_prefix_ = prefix; + if (prefix_is_for_params) + { + param_prefix_ = prefix; + // If it starts with a "~", remove it + if (param_prefix_.compare(0, 1, "~") == 0) { + param_prefix_.erase(0, 1); + } + // If it starts with a "/" or a "~/", remove those as well + if (param_prefix_.compare(0, 1, "/") == 0) { + param_prefix_.erase(0, 1); + } + // Add a trailing "." + if (!param_prefix_.empty() && param_prefix_.back() != '.') { + param_prefix_.append("."); + } + + topic_prefix_ = prefix; + // Replace parameter separator from "." to "/" in topics + std::replace(topic_prefix_.begin(), topic_prefix_.end(), '.', '/'); + // Add a trailing "/" + if (!topic_prefix_.empty() && topic_prefix_.back() != '/') { + topic_prefix_.append("/"); + } + // Add global namespace if none is defined + if (topic_prefix_.compare(0, 1, "~") != 0 && topic_prefix_.compare(0, 1, "/") != 0) + { + topic_prefix_ = "/" + topic_prefix_; + } + } + else + { + initialize(prefix); + } + + state_pub_ = rclcpp::create_publisher( + topics_interface_, topic_prefix_ + "pid_state", rclcpp::SensorDataQoS()); + rt_state_pub_.reset( + new realtime_tools::RealtimePublisher(state_pub_)); +} + +void PidROS::initialize(std::string topic_prefix) +{ + param_prefix_ = topic_prefix; // If it starts with a "~", remove it if (param_prefix_.compare(0, 1, "~") == 0) { param_prefix_.erase(0, 1); @@ -54,33 +103,20 @@ void PidROS::initialize(std::string prefix, bool prefix_is_for_params) if (param_prefix_.compare(0, 1, "/") == 0) { param_prefix_.erase(0, 1); } - if (!prefix_is_for_params) - { - // Replace namespacing separator from "/" to "." in parameters - std::replace(param_prefix_.begin(), param_prefix_.end(), '/', '.'); - } + // Replace namespacing separator from "/" to "." in parameters + std::replace(param_prefix_.begin(), param_prefix_.end(), '/', '.'); // Add a trailing "." if (!param_prefix_.empty() && param_prefix_.back() != '.') { param_prefix_.append("."); } - topic_prefix_ = prefix; + topic_prefix_ = topic_prefix; // Replace parameter separator from "." to "/" in topics std::replace(topic_prefix_.begin(), topic_prefix_.end(), '.', '/'); // Add a trailing "/" if (!topic_prefix_.empty() && topic_prefix_.back() != '/') { topic_prefix_.append("/"); } - // Add global namespace if none is defined - if (topic_prefix_.compare(0, 1, "~") != 0 && topic_prefix_.compare(0, 1, "/") != 0) - { - topic_prefix_ = "/" + topic_prefix_; - } - - state_pub_ = rclcpp::create_publisher( - topics_interface_, topic_prefix_ + "pid_state", rclcpp::SensorDataQoS()); - rt_state_pub_.reset( - new realtime_tools::RealtimePublisher(state_pub_)); } bool PidROS::getBooleanParam(const std::string & param_name, bool & value) diff --git a/test/pid_parameters_tests.cpp b/test/pid_parameters_tests.cpp index 3e655138..56ac10ce 100644 --- a/test/pid_parameters_tests.cpp +++ b/test/pid_parameters_tests.cpp @@ -97,7 +97,7 @@ TEST(PidParametersTest, InitPidTest) TestablePidROS pid(node); - ASSERT_EQ(pid.topic_prefix_, "/"); + ASSERT_EQ(pid.topic_prefix_, ""); ASSERT_EQ(pid.param_prefix_, ""); check_set_parameters(node, pid); @@ -106,7 +106,7 @@ TEST(PidParametersTest, InitPidTest) TEST(PidParametersTest, InitPid_when_not_prefix_for_params_then_replace_slash_with_dot) { const std::string INPUT_PREFIX = "slash/to/dots"; - const std::string RESULTING_TOPIC_PREFIX = "/" + INPUT_PREFIX + "/"; + const std::string RESULTING_TOPIC_PREFIX = INPUT_PREFIX + "/"; const std::string RESULTING_PARAM_PREFIX = "slash.to.dots."; rclcpp::Node::SharedPtr node = std::make_shared("pid_parameters_test"); From 794d076ee93bbac1c45b40c12e4d96d78752f418 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 4 Apr 2023 15:34:22 +0200 Subject: [PATCH 5/6] Apply suggestions from code review --- include/control_toolbox/pid_ros.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/control_toolbox/pid_ros.hpp b/include/control_toolbox/pid_ros.hpp index 7f41d1d9..b0397e81 100644 --- a/include/control_toolbox/pid_ros.hpp +++ b/include/control_toolbox/pid_ros.hpp @@ -217,7 +217,7 @@ class CONTROL_TOOLBOX_PUBLIC PidROS bool getBooleanParam(const std::string & param_name, bool & value); /*! - * \param prefix prefix to add to the pid parameters. + * \param topic_prefix prefix to add to the pid parameters. * Per default is prefix interpreted as prefix for topics. * If not stated explicitly using "/" or "~", prefix is interpreted as global, i.e., * "/" will be added in front of topic prefix, if prefix is not starting with From bcf667b4d0cdee4f3b40ad13e6f6a4a472c8d321 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 4 Apr 2023 20:38:37 +0100 Subject: [PATCH 6/6] doc fixes MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- include/control_toolbox/pid_ros.hpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/include/control_toolbox/pid_ros.hpp b/include/control_toolbox/pid_ros.hpp index b0397e81..588d5537 100644 --- a/include/control_toolbox/pid_ros.hpp +++ b/include/control_toolbox/pid_ros.hpp @@ -65,7 +65,7 @@ class CONTROL_TOOLBOX_PUBLIC PidROS * Per default is prefix interpreted as prefix for topics. * \param prefix_is_for_params provided prefix should be interpreted as prefix for parameters. * If the parameter is `true` then "/" in the middle of the string will not be replaced - * with "." for parameters prefix. ""/" at the begin will be removed. + * with "." for parameters prefix. "/" or "~/" at the beginning will be removed. * */ template @@ -220,8 +220,7 @@ class CONTROL_TOOLBOX_PUBLIC PidROS * \param topic_prefix prefix to add to the pid parameters. * Per default is prefix interpreted as prefix for topics. * If not stated explicitly using "/" or "~", prefix is interpreted as global, i.e., - * "/" will be added in front of topic prefix, if prefix is not starting with - * "/" or "~". + * "/" will be added in front of topic prefix */ void initialize(std::string topic_prefix);