From fe9086e22593960cd56ecf968ee3786751933f1b Mon Sep 17 00:00:00 2001 From: Gustavo Goretkin Date: Mon, 16 Mar 2015 21:08:41 -0700 Subject: [PATCH] publish state of PID integrator in i_error --- pr2_controllers_msgs/msg/JointControllerState.msg | 1 + robot_mechanism_controllers/src/joint_position_controller.cpp | 4 ++++ robot_mechanism_controllers/src/joint_velocity_controller.cpp | 4 ++++ 3 files changed, 9 insertions(+) diff --git a/pr2_controllers_msgs/msg/JointControllerState.msg b/pr2_controllers_msgs/msg/JointControllerState.msg index e2bceb1..5f57150 100755 --- a/pr2_controllers_msgs/msg/JointControllerState.msg +++ b/pr2_controllers_msgs/msg/JointControllerState.msg @@ -3,6 +3,7 @@ float64 set_point float64 process_value float64 process_value_dot float64 error +float64 i_error float64 time_step float64 command float64 p diff --git a/robot_mechanism_controllers/src/joint_position_controller.cpp b/robot_mechanism_controllers/src/joint_position_controller.cpp index 3aee240..0af712c 100755 --- a/robot_mechanism_controllers/src/joint_position_controller.cpp +++ b/robot_mechanism_controllers/src/joint_position_controller.cpp @@ -175,11 +175,15 @@ void JointPositionController::update() { if(controller_state_publisher_ && controller_state_publisher_->trylock()) { + double p_error, i_error, d_error; + pid_controller_.getCurrentPIDErrors(&p_error, &i_error, &d_error); + controller_state_publisher_->msg_.header.stamp = time; controller_state_publisher_->msg_.set_point = command_; controller_state_publisher_->msg_.process_value = joint_state_->position_; controller_state_publisher_->msg_.process_value_dot = joint_state_->velocity_; controller_state_publisher_->msg_.error = error; + controller_state_publisher_->msg_.i_error = i_error; controller_state_publisher_->msg_.time_step = dt_.toSec(); controller_state_publisher_->msg_.command = commanded_effort; diff --git a/robot_mechanism_controllers/src/joint_velocity_controller.cpp b/robot_mechanism_controllers/src/joint_velocity_controller.cpp index 960c66c..c82978d 100755 --- a/robot_mechanism_controllers/src/joint_velocity_controller.cpp +++ b/robot_mechanism_controllers/src/joint_velocity_controller.cpp @@ -144,10 +144,14 @@ void JointVelocityController::update() { if(controller_state_publisher_ && controller_state_publisher_->trylock()) { + double p_error, i_error, d_error; + pid_controller_.getCurrentPIDErrors(&p_error, &i_error, &d_error); + controller_state_publisher_->msg_.header.stamp = time; controller_state_publisher_->msg_.set_point = command_; controller_state_publisher_->msg_.process_value = joint_state_->velocity_; controller_state_publisher_->msg_.error = error; + controller_state_publisher_->msg_.i_error = i_error; controller_state_publisher_->msg_.time_step = dt_.toSec(); controller_state_publisher_->msg_.command = command;