Publish PID integrator state for joint velocity and position controllers

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

robot_mechanism_controllers/include/robot_mechanism_controllers/joint_trajectory_action_controller.h

@@ -71,6 +71,7 @@ class RTServerGoalHandle
   //typedef actionlib::ActionServer<Action>::GoalHandle GoalHandle;
   typedef actionlib::ServerGoalHandle<Action> GoalHandle;
   typedef boost::shared_ptr<Result> ResultPtr;
+  typedef boost::shared_ptr<Feedback> FeedbackPtr;
 
   uint8_t state_;
 
@@ -81,12 +82,15 @@ class RTServerGoalHandle
 public:
   GoalHandle gh_;
   ResultPtr preallocated_result_;  // Preallocated so it can be used in realtime
+  FeedbackPtr preallocated_feedback_;
 
-  RTServerGoalHandle(GoalHandle &gh, const ResultPtr &preallocated_result = ResultPtr((Result*)NULL))
-    : req_abort_(false), req_succeed_(false), gh_(gh), preallocated_result_(preallocated_result)
+ RTServerGoalHandle(GoalHandle &gh, const ResultPtr &preallocated_result = ResultPtr((Result*)NULL))
+   : req_abort_(false), req_succeed_(false), gh_(gh), preallocated_result_(preallocated_result)
   {
     if (!preallocated_result_)
       preallocated_result_.reset(new Result);
+    if (!preallocated_feedback_)
+      preallocated_feedback_.reset(new Feedback);
   }
 
   void setAborted(ResultConstPtr result = ResultConstPtr((Result*)NULL))

robot_mechanism_controllers/src/joint_position_controller.cpp

@@ -169,11 +169,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;
 

robot_mechanism_controllers/src/joint_trajectory_action_controller.cpp

@@ -492,6 +492,36 @@ void JointTrajectoryActionController::update()
       }
       controller_state_publisher_->unlockAndPublish();
     }
+    if (current_active_goal_follow)
+    {
+      current_active_goal_follow->preallocated_feedback_->header.stamp = time;
+      current_active_goal_follow->preallocated_feedback_->joint_names.resize(joints_.size());
+      current_active_goal_follow->preallocated_feedback_->desired.positions.resize(joints_.size());
+      current_active_goal_follow->preallocated_feedback_->desired.velocities.resize(joints_.size());
+      current_active_goal_follow->preallocated_feedback_->desired.accelerations.resize(joints_.size());
+      current_active_goal_follow->preallocated_feedback_->actual.positions.resize(joints_.size());
+      current_active_goal_follow->preallocated_feedback_->actual.velocities.resize(joints_.size());
+      current_active_goal_follow->preallocated_feedback_->error.positions.resize(joints_.size());
+      current_active_goal_follow->preallocated_feedback_->error.velocities.resize(joints_.size());
+      for (size_t j = 0; j < joints_.size(); ++j)
+      {
+        current_active_goal_follow->preallocated_feedback_->joint_names[j] = joints_[j]->joint_->name;
+        current_active_goal_follow->preallocated_feedback_->desired.positions[j] = q[j];
+        current_active_goal_follow->preallocated_feedback_->desired.velocities[j] = qd[j];
+        current_active_goal_follow->preallocated_feedback_->desired.accelerations[j] = qdd[j];
+        current_active_goal_follow->preallocated_feedback_->actual.positions[j] = joints_[j]->position_;
+        current_active_goal_follow->preallocated_feedback_->actual.velocities[j] = joints_[j]->velocity_;
+        current_active_goal_follow->preallocated_feedback_->error.positions[j] = error[j];
+        current_active_goal_follow->preallocated_feedback_->error.velocities[j] = joints_[j]->velocity_ - qd[j];
+      }
+      const actionlib_msgs::GoalID goalID = current_active_goal_follow->gh_.getGoalID();
+      ros::Duration time_from_start = time - goalID.stamp;
+      current_active_goal_follow->preallocated_feedback_->desired.time_from_start = time_from_start;
+      current_active_goal_follow->preallocated_feedback_->actual.time_from_start = time_from_start;
+      current_active_goal_follow->preallocated_feedback_->error.time_from_start = time_from_start;
+      current_active_goal_follow->gh_.publishFeedback(*current_active_goal_follow->preallocated_feedback_);
+
+    }
   }
 
   ++loop_count_;

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;