Joint Position and Velocity controllers startup fix

robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_controller.h

@@ -97,6 +97,7 @@ class JointPositionController : public pr2_controller_interface::Controller
   virtual void starting() {
     command_ = joint_state_->position_;
     pid_controller_.reset();
+    last_time_ = robot_->getTime();
   }
 
   /*!

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/include/robot_mechanism_controllers/joint_velocity_controller.h

@@ -106,6 +106,7 @@ class JointVelocityController : public pr2_controller_interface::Controller
   virtual void starting() {
     command_ = 0.0;
     pid_controller_.reset();
+    last_time_ = robot_->getTime();
   }
   virtual void update();
 

robot_mechanism_controllers/src/joint_position_controller.cpp

@@ -58,7 +58,6 @@ bool JointPositionController::init(pr2_mechanism_model::RobotState *robot, const
 {
   assert(robot);
   robot_ = robot;
-  last_time_ = robot->getTime();
 
   joint_state_ = robot_->getJointState(joint_name);
   if (!joint_state_)
@@ -139,7 +138,7 @@ void JointPositionController::update()
   double error(0);
   ros::Time time = robot_->getTime();
   assert(joint_state_->joint_);
-  dt_= time - last_time_;
+  dt_= time - last_time_;   //will be 0.0 if starting() was called on this iteration
 
   if (!initialized_)
   {

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

@@ -56,7 +56,6 @@ bool JointVelocityController::init(pr2_mechanism_model::RobotState *robot, const
 {
   assert(robot);
   robot_ = robot;
-  last_time_ = robot->getTime();
 
   joint_state_ = robot_->getJointState(joint_name);
   if (!joint_state_)
@@ -136,7 +135,7 @@ void JointVelocityController::update()
   ros::Time time = robot_->getTime();
 
   double error = joint_state_->velocity_ - command_;
-  dt_ = time - last_time_;
+  dt_ = time - last_time_;    //will be 0.0 if starting() was called on this iteration
   double command = pid_controller_.updatePid(error, dt_);
   joint_state_->commanded_effort_ += command;