[PidROS] Enable interpreting prefix as param prefix.

include/control_toolbox/pid_ros.hpp

@@ -61,14 +61,25 @@ 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.
+   *        If the parameter is `true` then "/" in the middle of the string will not be replaced
+   *        with "." for parameters prefix. "/" or "~/" at the beginning will be removed.
+   *
    */
-  template <class NodeT>
-  explicit PidROS(std::shared_ptr<NodeT> node_ptr, std::string topic_prefix = std::string(""))
+  template<class NodeT>
+  explicit PidROS(
+    std::shared_ptr<NodeT> 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)
+      node_ptr->get_node_base_interface(),
+      node_ptr->get_node_logging_interface(),
+      node_ptr->get_node_parameters_interface(),
+      node_ptr->get_node_topics_interface(),
+           prefix, prefix_is_for_params)
   {
   }
 
@@ -77,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 topic_prefix = std::string(""))
-  : node_base_(node_base),
-    node_logging_(node_logging),
-    node_params_(node_params),
-    topics_interface_(topics_interface)
-  {
-    initialize(topic_prefix);
-  }
+    std::string prefix = std::string(""), bool prefix_is_for_params = false);
 
   /*!
    * \brief Initialize the PID controller and set the parameters
@@ -197,6 +201,10 @@ class CONTROL_TOOLBOX_PUBLIC PidROS
     return parameter_callback_;
   }
 
+protected:
+  std::string topic_prefix_;
+  std::string param_prefix_;
+
 private:
   void setParameterEventCallback();
 
@@ -208,6 +216,12 @@ class CONTROL_TOOLBOX_PUBLIC PidROS
 
   bool getBooleanParam(const std::string & param_name, bool & value);
 
+  /*!
+   * \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
+   */
   void initialize(std::string topic_prefix);
 
   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameter_callback_;
@@ -221,8 +235,6 @@ class CONTROL_TOOLBOX_PUBLIC PidROS
   std::shared_ptr<rclcpp::Publisher<control_msgs::msg::PidState>> state_pub_;
 
   Pid pid_;
-  std::string topic_prefix_;
-  std::string param_prefix_;
 };
 
 }  // namespace control_toolbox

src/pid_ros.cpp

@@ -43,6 +43,55 @@
 namespace control_toolbox
 {
 
+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)
+{
+  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<control_msgs::msg::PidState>(
+    topics_interface_, topic_prefix_ + "pid_state", rclcpp::SensorDataQoS());
+  rt_state_pub_.reset(
+    new realtime_tools::RealtimePublisher<control_msgs::msg::PidState>(state_pub_));
+}
+
 void PidROS::initialize(std::string topic_prefix)
 {
   param_prefix_ = topic_prefix;
@@ -68,11 +117,6 @@ void PidROS::initialize(std::string topic_prefix)
   if (!topic_prefix_.empty() && topic_prefix_.back() != '/') {
     topic_prefix_.append("/");
   }
-
-  state_pub_ = rclcpp::create_publisher<control_msgs::msg::PidState>(
-    topics_interface_, topic_prefix_ + "pid_state", rclcpp::SensorDataQoS());
-  rt_state_pub_.reset(
-    new realtime_tools::RealtimePublisher<control_msgs::msg::PidState>(state_pub_));
 }
 
 bool PidROS::getBooleanParam(const std::string & param_name, bool & value)

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<class NodeT>
+  TestablePidROS(std::shared_ptr<NodeT> 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<rclcpp::Node>("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<double>(), P);
 
-  ASSERT_TRUE(node->get_parameter("i", param));
+  ASSERT_TRUE(node->get_parameter(prefix + "i", param));
   ASSERT_EQ(param.get_value<double>(), I);
 
-  ASSERT_TRUE(node->get_parameter("d", param));
+  ASSERT_TRUE(node->get_parameter(prefix + "d", param));
   ASSERT_EQ(param.get_value<double>(), 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<double>(), 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<double>(), I_MIN);
 
-  ASSERT_TRUE(node->get_parameter("antiwindup", param));
-  ASSERT_FALSE(param.get_value<bool>());
+  ASSERT_TRUE(node->get_parameter(prefix + "antiwindup", param));
+  ASSERT_EQ(param.get_value<bool>(), 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<rclcpp::Node>("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<double>(), P);
+  rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("pid_parameters_test");
 
-  ASSERT_TRUE(node->get_parameter("i", param));
-  ASSERT_EQ(param.get_value<double>(), I);
+  TestablePidROS pid(node, INPUT_PREFIX);  // default is false
 
-  ASSERT_TRUE(node->get_parameter("d", param));
-  ASSERT_EQ(param.get_value<double>(), 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<double>(), I_MAX);
+  check_set_parameters(node, pid, RESULTING_PARAM_PREFIX);
+}
 
-  ASSERT_TRUE(node->get_parameter("i_clamp_min", param));
-  ASSERT_EQ(param.get_value<double>(), 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<bool>(), ANTIWINDUP);
+  rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("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<rclcpp::Node>("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<rclcpp::Node>("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<rclcpp::Node>("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<rclcpp::Node>("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<rclcpp::Node>("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<rclcpp::Node>("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();
 }