Merge pull request #219 from bmagyar/joint_traj_action_feedback

Write feedback for the RealtimeServerGoalHandle to publish -- rebased
ros-controls · May 22, 2016 · 89417ce · 89417ce
2 parents 0d5cffc + 22fc93a
commit 89417ce
Showing 1 changed file with 15 additions and 0 deletions.
diff --git a/...jectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h b/...jectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h
@@ -441,6 +441,20 @@ update(const ros::Time& time, const ros::Duration& period)
   hw_iface_adapter_.updateCommand(time_data.uptime, time_data.period,
                                   desired_state_, state_error_);
 
+  // Set action feedback
+  if (rt_active_goal_)
+  {
+    rt_active_goal_->preallocated_feedback_->header.stamp          = time_data_.readFromRT()->time;
+    rt_active_goal_->preallocated_feedback_->desired.positions     = desired_state_.position;
+    rt_active_goal_->preallocated_feedback_->desired.velocities    = desired_state_.velocity;
+    rt_active_goal_->preallocated_feedback_->desired.accelerations = desired_state_.acceleration;
+    rt_active_goal_->preallocated_feedback_->actual.positions      = current_state_.position;
+    rt_active_goal_->preallocated_feedback_->actual.velocities     = current_state_.velocity;
+    rt_active_goal_->preallocated_feedback_->error.positions       = state_error_.position;
+    rt_active_goal_->preallocated_feedback_->error.velocities      = state_error_.velocity;
+    rt_active_goal_->setFeedback( rt_active_goal_->preallocated_feedback_ );
+  }
+
   // Publish state
   publishState(time_data.uptime);
 }
@@ -555,6 +569,7 @@ goalCB(GoalHandle gh)
   RealtimeGoalHandlePtr rt_goal(new RealtimeGoalHandle(gh));
   const bool update_ok = updateTrajectoryCommand(internal::share_member(gh.getGoal(), gh.getGoal()->trajectory),
                                                  rt_goal);
+  rt_goal->preallocated_feedback_->joint_names = joint_names_;
 
   if (update_ok)
   {