Merge branch 'master' into use-follow-base-traj

jsk_pr2eus/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package jsk_pr2eus
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.3.4 (2016-06-22)
+------------------
+
 0.3.3 (2016-05-28)
 ------------------
 

jsk_pr2eus/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>jsk_pr2eus</name>
-  <version>0.3.3</version>
+  <version>0.3.4</version>
   <description>
    <p>Metapackage that contains robot eus client package for jsk-ros-pkg</p>
   </description>

pr2eus/CHANGELOG.rst

@@ -2,6 +2,15 @@
 Changelog for package pr2eus
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.3.4 (2016-06-22)
+------------------
+* Merge pull request `#235 <https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/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)
 ------------------
 

pr2eus/package.xml

@@ -6,7 +6,7 @@
   <author email="[email protected]">Kei Okada</author>
   <maintainer email="[email protected]">Kei Okada</maintainer>
   <license>BSD</license>
-  <version>0.3.3</version>
+  <version>0.3.4</version>
   <url>http://ros.org/wiki/pr2eus</url>
   <buildtool_depend>catkin</buildtool_depend>
 

pr2eus/robot-interface.l

@@ -27,23 +27,30 @@
 
 (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))
+     ;; 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)))
@@ -515,6 +522,21 @@ 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)
+                         (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))
       (send self :ros-wait 0.005)))
   (:interpolating-smoothp (time-to-finish &optional (ctype)) ;; controller-type
@@ -705,8 +727,9 @@ 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)
        )))
   (:update-robot-state

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))
   )

pr2eus_moveit/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package pr2eus_moveit
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.3.4 (2016-06-22)
+------------------
+
 0.3.3 (2016-05-28)
 ------------------
 * CMakeLists.txt : forget to install euslisp directory ( `#230 <https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/230>`_ )

pr2eus_moveit/package.xml

@@ -1,6 +1,6 @@
 <package>
   <name>pr2eus_moveit</name>
-  <version>0.3.3</version>
+  <version>0.3.4</version>
   <description>pr2eus_moveit</description>
   <maintainer email="[email protected]">YoheiKakiuchi</maintainer>
 

pr2eus_tutorials/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package pr2eus_tutorials
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.3.4 (2016-06-22)
+------------------
+
 0.3.3 (2016-05-28)
 ------------------
 

pr2eus_tutorials/package.xml

@@ -1,6 +1,6 @@
 <package>
   <name>pr2eus_tutorials</name>
-  <version>0.3.3</version>
+  <version>0.3.4</version>
   <description>pr2eus_tutorials</description>
 
   <author email="[email protected]">Kei Okada</author>