Skip to content

Commit

Permalink
[PidROS] Enable interpreting prefix as param prefix. (#129)
Browse files Browse the repository at this point in the history 
* Enable setting prefix as param prefix.

* Fixup formatting.

* Addtests for prefix-if-for-params flag and optimize tests a bit.

* Restructure accoring to discussion with Bence.

* Apply suggestions from code review

* doc fixes

Co-authored-by: Christoph Fröhlich <[email protected]>

---------

Co-authored-by: Bence Magyar <[email protected]>
Co-authored-by: Christoph Fröhlich <[email protected]>
  • Loading branch information
@destogl @bmagyar @christophfroehlich
3 people authored Apr 4, 2023
1 parent edeff2f commit f08760c
Show file tree
Hide file tree
Showing 3 changed files with 182 additions and 70 deletions.
44 changes: 28 additions & 16 deletions include/control_toolbox/pid_ros.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
}

Expand All @@ -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
Expand Down Expand Up @@ -197,6 +201,10 @@ class CONTROL_TOOLBOX_PUBLIC PidROS
return parameter_callback_;
}

protected:
std::string topic_prefix_;
std::string param_prefix_;

private:
void setParameterEventCallback();

Expand All @@ -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_;
Expand All @@ -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
Expand Down
54 changes: 49 additions & 5 deletions src/pid_ros.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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)
Expand Down
154 changes: 105 additions & 49 deletions test/pid_parameters_tests.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand All @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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());

Expand Down Expand Up @@ -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;
Expand All @@ -255,5 +310,6 @@ int main(int argc, char ** argv)
{
testing::InitGoogleTest(&argc, argv);
rclcpp::init(0, nullptr);

return RUN_ALL_TESTS();
}

0 comments on commit f08760c

Please sign in to comment.