Skip to content

Commit

Permalink
Added template constructor and NodeXInterface constructor
Browse files Browse the repository at this point in the history
Signed-off-by: ahcorde <ahcorde@gmail.com>
  • Loading branch information
ahcorde authored and bmagyar committed Jul 1, 2020
1 parent 7b60195 commit d7dc461
Show file tree
Hide file tree
Showing 5 changed files with 78 additions and 74 deletions.
1 change: 1 addition & 0 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@ find_package(realtime_tools REQUIRED)
add_library(${PROJECT_NAME} SHARED
src/dither.cpp
src/limited_proxy.cpp
src/pid_ros.cpp
src/pid.cpp
src/sine_sweep.cpp
src/sinusoid.cpp)
Expand Down
38 changes: 30 additions & 8 deletions include/control_toolbox/pid_ros.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,6 @@
namespace control_toolbox
{

template<typename NodeT>
class PidROS
{
public:
Expand All @@ -64,7 +63,30 @@ class PidROS
* \param node ROS node
* \param topic_prefix prefix to add to the pid parameters.
*/
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 topic_prefix = std::string(""))
: 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)
{
}

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 topic_prefix = std::string(""))
: node_base_(node_base),
node_logging_(node_logging),
node_params_(node_params),
topics_interface_(topics_interface)
{
initialize(topic_prefix);
}

/*!
* \brief Destructor of PidROS class.
Expand Down Expand Up @@ -171,9 +193,14 @@ class PidROS

bool getBooleanParam(const std::string & param_name, bool & value);

void initialize(std::string topic_prefix);

rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameter_callback_;

std::shared_ptr<NodeT> node_;
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::shared_ptr<realtime_tools::RealtimePublisher<control_msgs::msg::PidState>> rt_state_pub_;
std::shared_ptr<rclcpp::Publisher<control_msgs::msg::PidState>> state_pub_;
Expand All @@ -184,9 +211,4 @@ class PidROS

} // namespace control_toolbox

#ifndef CONTROL_TOOLBOX__PID_ROS_IMPL_HPP_
// Template implementations
#include "pid_ros_impl.hpp"
#endif

#endif // CONTROL_TOOLBOX__PID_ROS_HPP_
97 changes: 39 additions & 58 deletions include/control_toolbox/pid_ros_impl.hpp → src/pid_ros.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -31,9 +31,6 @@
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef CONTROL_TOOLBOX__PID_ROS_IMPL_HPP_
#define CONTROL_TOOLBOX__PID_ROS_IMPL_HPP_

#include <algorithm>
#include <cmath>
#include <memory>
Expand All @@ -46,32 +43,31 @@
namespace control_toolbox
{

template<typename NodeT>
PidROS<NodeT>::PidROS(std::shared_ptr<NodeT> node_ptr, std::string topic_prefix)
: node_(node_ptr)
void PidROS::initialize(std::string topic_prefix)
{
if (topic_prefix.back() != '.' && !topic_prefix.empty()) {
topic_prefix_ = topic_prefix + ".";
} else {
topic_prefix_ = topic_prefix;
}

state_pub_ = node_->template create_publisher<control_msgs::msg::PidState>(
topic_prefix + "/pid_state", rclcpp::SensorDataQoS());
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_));
}

template<typename NodeT>
bool
PidROS<NodeT>::getBooleanParam(const std::string & param_name, bool & value)
PidROS::getBooleanParam(const std::string & param_name, bool & value)
{
rclcpp::Parameter param;
if (node_->has_parameter(param_name)) {
node_->get_parameter(param_name, param);
if (node_params_->has_parameter(param_name)) {
node_params_->get_parameter(param_name, param);
if (rclcpp::PARAMETER_BOOL != param.get_type()) {
RCLCPP_ERROR(
node_->get_logger(), "Wrong parameter type '%s', not boolean",
node_logging_->get_logger(), "Wrong parameter type '%s', not boolean",
param_name.c_str());
return false;
}
Expand All @@ -82,35 +78,35 @@ PidROS<NodeT>::getBooleanParam(const std::string & param_name, bool & value)
}
}

template<typename NodeT>
bool
PidROS<NodeT>::getDoubleParam(const std::string & param_name, double & value)
PidROS::getDoubleParam(const std::string & param_name, double & value)
{
rclcpp::Parameter param;
if (node_->has_parameter(param_name)) {
node_->get_parameter(param_name, param);
if (node_params_->has_parameter(param_name)) {
node_params_->get_parameter(param_name, param);
if (rclcpp::PARAMETER_DOUBLE != param.get_type()) {
RCLCPP_ERROR(
node_->get_logger(), "Wrong parameter type '%s', not double",
node_logging_->get_logger(), "Wrong parameter type '%s', not double",
param_name.c_str());
return false;
}
value = param.as_double();
RCLCPP_INFO_STREAM(
node_->get_logger(), "parameter '" << param_name << "' in node '" << node_->get_name() <<
node_logging_->get_logger(),
"parameter '" << param_name << "' in node '" << node_base_->get_name() <<
"' value is " << value << std::endl);
return true;
} else {
RCLCPP_INFO_STREAM(
node_->get_logger(), "parameter '" << param_name << "' in node '" << node_->get_name() <<
node_logging_->get_logger(),
"parameter '" << param_name << "' in node '" << node_base_->get_name() <<
"' does not exists" << std::endl);
return false;
}
}

template<typename NodeT>
bool
PidROS<NodeT>::initPid()
PidROS::initPid()
{
double p, i, d, i_min, i_max;
bool antiwindup = false;
Expand All @@ -132,18 +128,16 @@ PidROS<NodeT>::initPid()
return all_params_available;
}

template<typename NodeT>
void
PidROS<NodeT>::declareParam(const std::string & param_name, rclcpp::ParameterValue param_value)
PidROS::declareParam(const std::string & param_name, rclcpp::ParameterValue param_value)
{
if (!node_->has_parameter(param_name)) {
node_->declare_parameter(param_name, param_value);
if (!node_params_->has_parameter(param_name)) {
node_params_->declare_parameter(param_name, param_value);
}
}

template<typename NodeT>
void
PidROS<NodeT>::initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup)
PidROS::initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup)
{
pid_.initPid(p, i, d, i_max, i_min, antiwindup);

Expand All @@ -157,42 +151,38 @@ PidROS<NodeT>::initPid(double p, double i, double d, double i_max, double i_min,
setParameterEventCallback();
}

template<typename NodeT>
void
PidROS<NodeT>::reset()
PidROS::reset()
{
pid_.reset();
}

template<typename NodeT>

std::shared_ptr<rclcpp::Publisher<control_msgs::msg::PidState>>
PidROS<NodeT>::getPidStatePublisher()
PidROS::getPidStatePublisher()
{
return state_pub_;
}

template<typename NodeT>
double
PidROS<NodeT>::computeCommand(double error, rclcpp::Duration dt)
PidROS::computeCommand(double error, rclcpp::Duration dt)
{
double cmd_ = pid_.computeCommand(error, dt.nanoseconds());
publishPIDState(cmd_, error, dt);

return cmd_;
}

template<typename NodeT>
Pid::Gains
PidROS<NodeT>::getGains()
PidROS::getGains()
{
return pid_.getGains();
}

template<typename NodeT>
void
PidROS<NodeT>::setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup)
PidROS::setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup)
{
node_->set_parameters(
node_params_->set_parameters(
{
rclcpp::Parameter(topic_prefix_ + "p", p),
rclcpp::Parameter(topic_prefix_ + "i", i),
Expand All @@ -206,9 +196,8 @@ PidROS<NodeT>::setGains(double p, double i, double d, double i_max, double i_min
pid_.setGains(p, i, d, i_max, i_min, antiwindup);
}

template<typename NodeT>
void
PidROS<NodeT>::publishPIDState(double cmd, double error, rclcpp::Duration dt)
PidROS::publishPIDState(double cmd, double error, rclcpp::Duration dt)
{
Pid::Gains gains = pid_.getGains();

Expand Down Expand Up @@ -236,23 +225,20 @@ PidROS<NodeT>::publishPIDState(double cmd, double error, rclcpp::Duration dt)
}
}

template<typename NodeT>
void
PidROS<NodeT>::setCurrentCmd(double cmd)
PidROS::setCurrentCmd(double cmd)
{
pid_.setCurrentCmd(cmd);
}

template<typename NodeT>
double
PidROS<NodeT>::getCurrentCmd()
PidROS::getCurrentCmd()
{
return pid_.getCurrentCmd();
}

template<typename NodeT>
void
PidROS<NodeT>::getCurrentPIDErrors(double & pe, double & ie, double & de)
PidROS::getCurrentPIDErrors(double & pe, double & ie, double & de)
{
double _pe, _ie, _de;
pid_.getCurrentPIDErrors(_pe, _ie, _de);
Expand All @@ -261,17 +247,16 @@ PidROS<NodeT>::getCurrentPIDErrors(double & pe, double & ie, double & de)
de = _de;
}

template<typename NodeT>
void
PidROS<NodeT>::printValues()
PidROS::printValues()
{
Pid::Gains gains = pid_.getGains();

double p_error_, i_error_, d_error_;
getCurrentPIDErrors(p_error_, i_error_, d_error_);

RCLCPP_INFO_STREAM(
node_->get_logger(),
node_logging_->get_logger(),
"Current Values of PID template:\n" <<
" P Gain: " << gains.p_gain_ << "\n" <<
" I Gain: " << gains.i_gain_ << "\n" <<
Expand All @@ -286,16 +271,14 @@ PidROS<NodeT>::printValues()
);
}

template<typename NodeT>
void
PidROS<NodeT>::setGains(const Pid::Gains & gains)
PidROS::setGains(const Pid::Gains & gains)
{
pid_.setGains(gains);
}

template<typename NodeT>
void
PidROS<NodeT>::setParameterEventCallback()
PidROS::setParameterEventCallback()
{
auto on_parameter_event_callback = [this](const std::vector<rclcpp::Parameter> & parameters) {
rcl_interfaces::msg::SetParametersResult result;
Expand Down Expand Up @@ -325,7 +308,7 @@ PidROS<NodeT>::setParameterEventCallback()
}
} catch (const rclcpp::exceptions::InvalidParameterTypeException & e) {
RCLCPP_INFO_STREAM(
node_->get_logger(), "Please use the right type: " << e.what());
node_logging_->get_logger(), "Please use the right type: " << e.what());
}
}

Expand All @@ -338,10 +321,8 @@ PidROS<NodeT>::setParameterEventCallback()
};

parameter_callback_ =
node_->get_node_parameters_interface()->add_on_set_parameters_callback(
node_params_->add_on_set_parameters_callback(
on_parameter_event_callback);
}

} // namespace control_toolbox

#endif // CONTROL_TOOLBOX__PID_ROS_IMPL_HPP_
10 changes: 5 additions & 5 deletions test/pid_parameters_tests.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@ TEST(PidParametersTest, InitPidTest)
{
rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("pid_parameters_test");

control_toolbox::PidROS<rclcpp::Node> pid(node);
control_toolbox::PidROS pid(node);

const double P = 1.0;
const double I = 2.0;
Expand Down Expand Up @@ -73,7 +73,7 @@ TEST(PidParametersTest, InitPidWithAntiwindupTest)
{
rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("pid_parameters_test");

control_toolbox::PidROS<rclcpp::Node> pid(node);
control_toolbox::PidROS pid(node);

const double P = 1.0;
const double I = 2.0;
Expand Down Expand Up @@ -118,7 +118,7 @@ TEST(PidParametersTest, SetParametersTest)
{
rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("pid_parameters_test");

control_toolbox::PidROS<rclcpp::Node> pid(node);
control_toolbox::PidROS pid(node);

const double P = 1.0;
const double I = 2.0;
Expand Down Expand Up @@ -166,7 +166,7 @@ TEST(PidParametersTest, GetParametersTest)
{
rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("pid_parameters_test");

control_toolbox::PidROS<rclcpp::Node> pid(node);
control_toolbox::PidROS pid(node);

const double P = 1.0;
const double I = 2.0;
Expand Down Expand Up @@ -203,7 +203,7 @@ TEST(PidParametersTest, GetParametersFromParams)
{
rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("pid_parameters_test");

control_toolbox::PidROS<rclcpp::Node> pid(node);
control_toolbox::PidROS pid(node);

const double P = 1.0;
const double I = 2.0;
Expand Down
Loading

0 comments on commit d7dc461

Please sign in to comment.