Chane update() to use the period received as argument, instead of computing it from...

diff --git a/include/pid_plus_gravity_controller/pid_plus_gravity_controller.hpp b/include/pid_plus_gravity_controller/pid_plus_gravity_controller.hpp
index b51de9f..18a4ef9 100644 (file)
--- a/include/pid_plus_gravity_controller/pid_plus_gravity_controller.hpp
+++ b/include/pid_plus_gravity_controller/pid_plus_gravity_controller.hpp
@@ -57,8 +57,6 @@ namespace effort_controllers
                
                int priority_;
                
-               rclcpp::Time lastTime_;
-               
                realtime_tools::RealtimeBuffer<std::shared_ptr<trajectory_msgs::msg::JointTrajectoryPoint>> referencePoint_;
 
                void commandCB(const trajectory_msgs::msg::JointTrajectoryPoint::SharedPtr referencePoint);

diff --git a/src/pid_plus_gravity_controller.cpp b/src/pid_plus_gravity_controller.cpp
index d1fadc4..fd6d597 100644 (file)
--- a/src/pid_plus_gravity_controller.cpp
+++ b/src/pid_plus_gravity_controller.cpp
@@ -175,8 +175,6 @@ namespace effort_controllers
                else if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1)
                        RCLCPP_WARN(get_node()->get_logger(),"Failed to lock memory.");
 
-               lastTime_=get_node()->get_clock()->now();
-
                return controller_interface::CallbackReturn::SUCCESS;
        }
 
@@ -190,13 +188,12 @@ namespace effort_controllers
 
                auto time=get_node()->get_clock()->now();
                for(unsigned int i=0;i < nJoints_;i++)
-                       command_interfaces_[i].set_value(gravity_(i)+pid_[i]->computeCommand(0,time-lastTime_));
-               lastTime_=time;
+                       command_interfaces_[i].set_value(gravity_(i));
 
                return controller_interface::CallbackReturn::SUCCESS;
        }
 
-       controller_interface::return_type PidPlusGravityController::update(const rclcpp::Time &/*time*/,const rclcpp::Duration &/*period*/)
+       controller_interface::return_type PidPlusGravityController::update(const rclcpp::Time &/*time*/,const rclcpp::Duration & period)
        {
                auto referencePoint=referencePoint_.readFromRT();
                if(referencePoint && *referencePoint)
@@ -209,10 +206,8 @@ namespace effort_controllers
                if(chainParam_->JntToGravity(q_,gravity_) < 0)
                        RCLCPP_ERROR(get_node()->get_logger(),"KDL dynamics solver failed.");
 
-               auto time=get_node()->get_clock()->now();
                for(unsigned int i=0;i < nJoints_;i++)
-                       command_interfaces_[i].set_value(gravity_(i)+pid_[i]->computeCommand(qr_(i)-q_(i),time-lastTime_));
-               lastTime_=time;
+                       command_interfaces_[i].set_value(gravity_(i)+pid_[i]->computeCommand(qr_(i)-q_(i),period));
 
                return controller_interface::return_type::OK;
        }