/******************************************************************************
ROS 2 computed_torque_controller Package
Computed Torque Controller
- Copyright (C) 2013..2022 Walter Fetter Lages <w.fetter@ieee.org>
+ Copyright (C) 2013..2025 Walter Fetter Lages <w.fetter@ieee.org>
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
#include <kdl_parser/kdl_parser.hpp>
#include <kdl/chainidsolver_recursive_newton_euler.hpp>
-#include <realtime_tools/realtime_buffer.h>
+#include <realtime_tools/realtime_buffer.hpp>
#include <computed_torque_controller/visibility_control.h>
/******************************************************************************
ROS 2 computed_torque_controller Package
Computed Torque Controller
- Copyright (C) 2013..2024 Walter Fetter Lages <w.fetter@ieee.org>
+ Copyright (C) 2013..2025 Walter Fetter Lages <w.fetter@ieee.org>
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
{
for(unsigned int i=0;i < nJoints_;i++)
{
- q_(i)=state_interfaces_[2*i].get_value();
- dq_(i)=state_interfaces_[2*i+1].get_value();
+ q_(i)=state_interfaces_[2*i].get_optional().value_or(q_(i));
+ dq_(i)=state_interfaces_[2*i+1].get_optional().value_or(dq_(i));
}
qr_=q_;
dqr_=dq_;
controller_interface::CallbackReturn ComputedTorqueController::on_deactivate(const rclcpp_lifecycle::State &/*previous_state*/)
{
for(unsigned int i=0;i < nJoints_;i++)
- q_(i)=state_interfaces_[2*i].get_value();
+ q_(i)=state_interfaces_[2*i].get_optional().value_or(q_(i));
SetToZero(dqr_);
SetToZero(ddqr_);
for(unsigned int i=0;i < nJoints_;i++)
{
- q_(i)=state_interfaces_[2*i].get_value();
- dq_(i)=state_interfaces_[2*i+1].get_value();
+ q_(i)=state_interfaces_[2*i].get_optional().value_or(q_(i));
+ dq_(i)=state_interfaces_[2*i+1].get_optional().value_or(dq_(i));
}
for(auto &f : fext_) f.Zero();