Support PR2 and Fetch: don't relay on time_from_start of feedback

jsk-ros-pkg · Jul 13, 2018 · edd43fc · edd43fc
1 parent 3e343fc
commit edd43fc
Showing 1 changed file with 15 additions and 6 deletions.
diff --git a/pr2eus/robot-interface.l b/pr2eus/robot-interface.l
@@ -46,12 +46,21 @@
      (ros::ros-debug "[~A] feedback-cb ~A" ros::name-space msg)
      (setq last-feedback-msg-stamp (send msg :header :stamp))
      (unless (and (send ros::comm-state :action-goal) (not (equal (send (class ros::action-spec) :name) 'control_msgs::SingleJointPositionAction))) (return-from :action-feedback-cb nil))
-     (cond ((derivedp msg control_msgs::followjointtrajectoryactionfeedback)
-            (setq current-time (send msg :feedback :error :time_from_start)))
-           (t
-            (ros::ros-warn "feedback message type is not control_msgs/FollowJointTrajectoryActionFeedback.")
-            ;; e.g. Type: pr2_controllers_msgs/JointTrajectoryActionFeedback does not have :error
-            (setq current-time (ros::time- (ros::time-now) (send msg :status :goal_id :stamp)))))
+     ;; TODO: Use time_from_start of feedback.
+     ;; FIXME: Currently, time_from_start varies by robot.
+     ;; https://github.com/start-jsk/rtmros_common/pull/1052#issuecomment-404753185
+     ;; (cond ((derivedp msg control_msgs::followjointtrajectoryactionfeedback)
+     ;;        (setq current-time (send msg :feedback :error :time_from_start)))
+     ;;       (t
+     ;;        (ros::ros-warn "feedback message type is not control_msgs/FollowJointTrajectoryActionFeedback.")
+     ;;        ;; e.g. Type: pr2_controllers_msgs/JointTrajectoryActionFeedback does not have :error
+     ;;        (setq current-time (ros::time- (ros::time-now) (send msg :status :goal_id :stamp)))))
+
+     ;; NOTE: Use stamp of feedback msg instead of (ros::time-now) to depend on feedback.
+     ;; NOTE: As stamp of goal_id is when goal is accepted, it may be different from trajectory start time.
+     ;; NOTE: So trajectory start time is got from stamp of goal trajectory instead of goal_id stamp.
+     (setq current-time (ros::time- (send msg :header :stamp)
+                                    (send (send ros::comm-state :action-goal) :goal :trajectory :header :stamp)))
      (setq finish-time (send (car (last (send (send ros::comm-state :action-goal) :goal :trajectory :points))) :time_from_start))
      (if (string= (send (send ros::comm-state :action-goal) :goal_id :id)
                   (send msg :status :goal_id :id))