From 059bedac65163a4363eb8e0b47a7bee685ac4069 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 20 Jun 2016 19:18:03 +0900 Subject: [PATCH] fix :wait-interpolation-smooth for pr2_controllers_msgs/JointTrajectoryActionFeedback --- pr2eus/robot-interface.l | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/pr2eus/robot-interface.l b/pr2eus/robot-interface.l index e8c73d63e..2dc7e1c8d 100644 --- a/pr2eus/robot-interface.l +++ b/pr2eus/robot-interface.l @@ -44,8 +44,12 @@ (let ((finish-time) (current-time)) (ros::ros-debug "[~A] feedback-cb ~A" ros::name-space msg) (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)) - (setq current-time (send msg :feedback :error :time_from_start) - finish-time (send (car (last (send (send ros::comm-state :action-goal) :goal :trajectory :points))) :time_from_start)) + ;; Type: pr2_controllers_msgs/JointTrajectoryActionFeedback does not have :error + (cond ((derivedp msg pr2_controllers_msgs::jointtrajectoryactionfeedback) + (setq current-time (ros::time- (ros::time-now) (send msg :status :goal_id :stamp)))) + (t + (setq current-time (send msg :feedback :error :time_from_start)))) + (setq finish-time (send (car (last (send (send ros::comm-state :action-goal) :goal :trajectory :points))) :time_from_start)) (when (string= (send (send ros::comm-state :action-goal) :goal_id :id) (send msg :status :goal_id :id)) (setq time-to-finish (send (ros::time- finish-time current-time) :to-sec)))