From 059bedac65163a4363eb8e0b47a7bee685ac4069 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 20 Jun 2016 19:18:03 +0900 Subject: [PATCH 1/8] 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))) From 5240dec7a1878c1c2e48fbd68e6d1e80e72f7d1a Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 20 Jun 2016 19:21:06 +0900 Subject: [PATCH 2/8] robot-interface.l : wait for feedback message is updated --- pr2eus/robot-interface.l | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/pr2eus/robot-interface.l b/pr2eus/robot-interface.l index e8c73d63e..e556c7392 100644 --- a/pr2eus/robot-interface.l +++ b/pr2eus/robot-interface.l @@ -29,20 +29,23 @@ (defclass controller-action-client :super ros::simple-action-client - :slots (time-to-finish + :slots (time-to-finish last-feedback-msg-stamp ri angle-vector-sequence timer-sequence current-time current-angle-vector previous-angle-vector scale-angle-vector;; slot for angle-vector-sequence )) (defmethod controller-action-client (:init (r &rest args) (setq ri r) ;; robot-interface (setq time-to-finish 0) + (setq last-feedback-msg-stamp (ros::time-now)) ;; this is for real robot (send-super* :init args)) + (:last-feedback-msg-stamp () last-feedback-msg-stamp) (:time-to-finish () (ros::ros-debug "[~A] time-to-fnish ~A" ros::name-space time-to-finish) time-to-finish) (:action-feedback-cb (msg) (let ((finish-time) (current-time)) (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)) (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)) @@ -517,6 +520,14 @@ return t if interpolating" (send *ri* :angle-vector av) (send *ri* :wait-interpolation-smooth 300)) Return value is a list of interpolatingp for all controllers, so (null (some #'identity (send *ri* :wait-interpolation))) -> t if all interpolation has stopped" + (when (not (send self :simulation-modep)) + (let ((tm (ros::time-now)) + (cacts (cond + (ctype (gethash ctype controller-table)) + (t controller-actions)))) + (while (some #'(lambda (x) (<= (send (ros::time- (send x :last-feedback-msg-stamp) tm) :to-sec) 0)) cacts) + (send self :spin-once) ;; need to wait for feedback + (send self :ros-wait 0.005)))) (while (null (send self :interpolating-smoothp time-to-finish ctype)) (send self :ros-wait 0.005))) (:interpolating-smoothp (time-to-finish &optional (ctype)) ;; controller-type From a8923e549954f62147ceb1ca51ea38009d841e0c Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 21 Jun 2016 01:49:41 +0900 Subject: [PATCH 3/8] add code when last-feedback-msgs-stamp is not updated --- pr2eus/robot-interface.l | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/pr2eus/robot-interface.l b/pr2eus/robot-interface.l index e556c7392..8b9c345c2 100644 --- a/pr2eus/robot-interface.l +++ b/pr2eus/robot-interface.l @@ -525,7 +525,14 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i (cacts (cond (ctype (gethash ctype controller-table)) (t controller-actions)))) - (while (some #'(lambda (x) (<= (send (ros::time- (send x :last-feedback-msg-stamp) tm) :to-sec) 0)) cacts) + (while (some #'(lambda (x) + (and + (<= (send (ros::time- (send x :last-feedback-msg-stamp) tm) :to-sec) 0) + (let ((goal (send (x . ros::comm-state) :action-goal))) + (>= (send (ros::time- (ros::time+ (send goal :header :stamp) + (send (car (last (send goal :goal :trajectory :points))) :time_from_start)) + (ros::time-now)) :to-sec) 0)))) + cacts) (send self :spin-once) ;; need to wait for feedback (send self :ros-wait 0.005)))) (while (null (send self :interpolating-smoothp time-to-finish ctype)) From 3a52882709b0bfed7c339fd98bac00a3bac5df8d Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 22 Jun 2016 17:40:43 +0900 Subject: [PATCH 4/8] update CHANGELOG.rst --- jsk_pr2eus/CHANGELOG.rst | 3 +++ pr2eus/CHANGELOG.rst | 9 +++++++++ pr2eus_moveit/CHANGELOG.rst | 3 +++ pr2eus_tutorials/CHANGELOG.rst | 3 +++ 4 files changed, 18 insertions(+) diff --git a/jsk_pr2eus/CHANGELOG.rst b/jsk_pr2eus/CHANGELOG.rst index 0dc359241..44f256942 100644 --- a/jsk_pr2eus/CHANGELOG.rst +++ b/jsk_pr2eus/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package jsk_pr2eus ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.3.3 (2016-05-28) ------------------ diff --git a/pr2eus/CHANGELOG.rst b/pr2eus/CHANGELOG.rst index 5c1790d0f..e7ca5b657 100644 --- a/pr2eus/CHANGELOG.rst +++ b/pr2eus/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package pr2eus ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge pull request `#235 `_ from k-okada/fix_smooth + fix :wait-interpolation-smooth for pr2_controllers_msgs/JointTrajectoryActionFeedback +* add code when last-feedback-msgs-stamp is not updated +* robot-interface.l : wait for feedback message is updated +* fix :wait-interpolation-smooth for pr2_controllers_msgs/JointTrajectoryActionFeedback +* Contributors: Kei Okada + 0.3.3 (2016-05-28) ------------------ diff --git a/pr2eus_moveit/CHANGELOG.rst b/pr2eus_moveit/CHANGELOG.rst index efb2c9991..a8f25645e 100644 --- a/pr2eus_moveit/CHANGELOG.rst +++ b/pr2eus_moveit/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package pr2eus_moveit ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.3.3 (2016-05-28) ------------------ * CMakeLists.txt : forget to install euslisp directory ( `#230 `_ ) diff --git a/pr2eus_tutorials/CHANGELOG.rst b/pr2eus_tutorials/CHANGELOG.rst index b7455570a..797536cb8 100644 --- a/pr2eus_tutorials/CHANGELOG.rst +++ b/pr2eus_tutorials/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package pr2eus_tutorials ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.3.3 (2016-05-28) ------------------ From c2f45923f9ae40ae25b4b407b9c23fe906966d00 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 22 Jun 2016 17:40:51 +0900 Subject: [PATCH 5/8] 0.3.4 --- jsk_pr2eus/CHANGELOG.rst | 4 ++-- jsk_pr2eus/package.xml | 2 +- pr2eus/CHANGELOG.rst | 4 ++-- pr2eus/package.xml | 2 +- pr2eus_moveit/CHANGELOG.rst | 4 ++-- pr2eus_moveit/package.xml | 2 +- pr2eus_tutorials/CHANGELOG.rst | 4 ++-- pr2eus_tutorials/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/jsk_pr2eus/CHANGELOG.rst b/jsk_pr2eus/CHANGELOG.rst index 44f256942..b65e422b0 100644 --- a/jsk_pr2eus/CHANGELOG.rst +++ b/jsk_pr2eus/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package jsk_pr2eus ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.3.4 (2016-06-22) +------------------ 0.3.3 (2016-05-28) ------------------ diff --git a/jsk_pr2eus/package.xml b/jsk_pr2eus/package.xml index 2b4db2dfe..1aa071416 100644 --- a/jsk_pr2eus/package.xml +++ b/jsk_pr2eus/package.xml @@ -1,7 +1,7 @@ jsk_pr2eus - 0.3.3 + 0.3.4

Metapackage that contains robot eus client package for jsk-ros-pkg

 diff --git a/pr2eus/CHANGELOG.rst b/pr2eus/CHANGELOG.rst index e7ca5b657..aa8c6376a 100644 --- a/pr2eus/CHANGELOG.rst +++ b/pr2eus/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pr2eus ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.3.4 (2016-06-22) +------------------ * Merge pull request `#235 `_ from k-okada/fix_smooth fix :wait-interpolation-smooth for pr2_controllers_msgs/JointTrajectoryActionFeedback * add code when last-feedback-msgs-stamp is not updated diff --git a/pr2eus/package.xml b/pr2eus/package.xml index 77c433947..e7ad666c6 100644 --- a/pr2eus/package.xml +++ b/pr2eus/package.xml @@ -6,7 +6,7 @@ Kei Okada Kei Okada BSD - 0.3.3 + 0.3.4 http://ros.org/wiki/pr2eus catkin diff --git a/pr2eus_moveit/CHANGELOG.rst b/pr2eus_moveit/CHANGELOG.rst index a8f25645e..fc7068bc4 100644 --- a/pr2eus_moveit/CHANGELOG.rst +++ b/pr2eus_moveit/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pr2eus_moveit ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.3.4 (2016-06-22) +------------------ 0.3.3 (2016-05-28) ------------------ diff --git a/pr2eus_moveit/package.xml b/pr2eus_moveit/package.xml index 94fc37338..952e00a79 100644 --- a/pr2eus_moveit/package.xml +++ b/pr2eus_moveit/package.xml @@ -1,6 +1,6 @@ pr2eus_moveit - 0.3.3 + 0.3.4 pr2eus_moveit YoheiKakiuchi diff --git a/pr2eus_tutorials/CHANGELOG.rst b/pr2eus_tutorials/CHANGELOG.rst index 797536cb8..2a8365dac 100644 --- a/pr2eus_tutorials/CHANGELOG.rst +++ b/pr2eus_tutorials/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pr2eus_tutorials ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.3.4 (2016-06-22) +------------------ 0.3.3 (2016-05-28) ------------------ diff --git a/pr2eus_tutorials/package.xml b/pr2eus_tutorials/package.xml index f64346a3c..907852512 100644 --- a/pr2eus_tutorials/package.xml +++ b/pr2eus_tutorials/package.xml @@ -1,6 +1,6 @@ pr2eus_tutorials - 0.3.3 + 0.3.4 pr2eus_tutorials Kei Okada From 1c172b113f7cc188b80c040bbb51163c4771acc8 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Sat, 25 Jun 2016 16:40:39 +0900 Subject: [PATCH 6/8] add test-state-wait-until-update --- pr2eus/test/default-ri-test.l | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/pr2eus/test/default-ri-test.l b/pr2eus/test/default-ri-test.l index b3ea3d997..fba3af082 100644 --- a/pr2eus/test/default-ri-test.l +++ b/pr2eus/test/default-ri-test.l @@ -46,6 +46,13 @@ (assert (< (norm (v- (send *robot* :angle-vector) (send *ri* :potentio-vector))) 10.0)) ) +(deftest test-state-:wait-until-update + (assert (send *robot* :reset-pose)) + (assert (send *ri* :angle-vector (send *robot* :angle-vector) 2000)) + (assert (send *ri* :state :potentio-vector)) + (assert (send *ri* :state :potentio-vector :wait-until-update t)) + ) + (deftest test-worldpos (assert (send *ri* :worldcoords)) ) From 24f71b73cd1435e20630c44db818934fb8bb477b Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Sat, 25 Jun 2016 17:24:31 +0900 Subject: [PATCH 7/8] robot-interface.l: :wait-until-update-all-joints need to call :robot-interface-simulation-callback explicitly --- pr2eus/robot-interface.l | 1 + pr2eus/test/default-ri-test.l | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/pr2eus/robot-interface.l b/pr2eus/robot-interface.l index 2ae6cd9b6..4d5878325 100644 --- a/pr2eus/robot-interface.l +++ b/pr2eus/robot-interface.l @@ -731,6 +731,7 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i (while t (when (null (position-if #'null (mapcar #'(lambda (ts) (> (send ts :to-nsec) initial-time)) (cdr (assoc :stamp-list robot-state))))) (return-from nil nil)) + (if (send self :simulation-modep) (send self :robot-interface-simulation-callback)) ;; to update robot-state (send self :spin-once) ))) (:update-robot-state diff --git a/pr2eus/test/default-ri-test.l b/pr2eus/test/default-ri-test.l index fba3af082..c0a0640fa 100644 --- a/pr2eus/test/default-ri-test.l +++ b/pr2eus/test/default-ri-test.l @@ -46,7 +46,7 @@ (assert (< (norm (v- (send *robot* :angle-vector) (send *ri* :potentio-vector))) 10.0)) ) -(deftest test-state-:wait-until-update +(deftest test-state-wait-until-update (assert (send *robot* :reset-pose)) (assert (send *ri* :angle-vector (send *robot* :angle-vector) 2000)) (assert (send *ri* :state :potentio-vector)) From 0b6f8769d75fb25fa2781834b94999996cf1ed9e Mon Sep 17 00:00:00 2001 From: MasakiMurooka Date: Sun, 26 Jun 2016 17:23:02 +0900 Subject: [PATCH 8/8] [pr2eus/robot-interface.l] fix the implementation of condition to break loop in :wait-until-update-all-joints. --- pr2eus/robot-interface.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pr2eus/robot-interface.l b/pr2eus/robot-interface.l index 4d5878325..af4921797 100644 --- a/pr2eus/robot-interface.l +++ b/pr2eus/robot-interface.l @@ -729,7 +729,7 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i (setq initial-time (send tgt-tm :to-nsec)) (setq initial-time (send (send (ros::time) :now) :to-nsec))) (while t - (when (null (position-if #'null (mapcar #'(lambda (ts) (> (send ts :to-nsec) initial-time)) (cdr (assoc :stamp-list robot-state))))) + (when (every #'identity (mapcar #'(lambda (ts) (> (send ts :to-nsec) initial-time)) (cdr (assoc :stamp-list robot-state)))) (return-from nil nil)) (if (send self :simulation-modep) (send self :robot-interface-simulation-callback)) ;; to update robot-state (send self :spin-once)