Merge pull request #599 from k-okada/check_pr2eus

.travis.yml: run jsk_pr2eus tests
Add test to find #567 regression

.travis.yml

@@ -20,16 +20,19 @@ env:
     - ROS_DISTRO=indigo USE_DEB=true   ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu
     - ROS_DISTRO=indigo USE_DEB=false  NOT_TEST_INSTALL=true
     - ROS_DISTRO=indigo USE_DEB=source NOT_TEST_INSTALL=true
+    - ROS_DISTRO=indigo USE_DEB=true   NOT_TEST_INSTALL=true INSTALL_SRC="http://github.com/jsk-ros-pkg/jsk_pr2eus" TEST_PKGS="pr2eus pr2eus_moveit"
     - ROS_DISTRO=kinetic USE_DEB=true
     - ROS_DISTRO=kinetic USE_DEB=true   NOT_TEST_INSTALL=true INSTALL_SRC="http://github.com/jsk-ros-pkg/jsk_planning" TEST_PKGS="task_compiler"
     - ROS_DISTRO=kinetic USE_DEB=true   ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu
     - ROS_DISTRO=kinetic USE_DEB=false  NOT_TEST_INSTALL=true
     - ROS_DISTRO=kinetic USE_DEB=source NOT_TEST_INSTALL=true
+    - ROS_DISTRO=kinetic USE_DEB=true   NOT_TEST_INSTALL=true INSTALL_SRC="http://github.com/jsk-ros-pkg/jsk_pr2eus" TEST_PKGS="pr2eus pr2eus_moveit"    
     - ROS_DISTRO=melodic USE_DEB=true
     - ROS_DISTRO=melodic USE_DEB=true   NOT_TEST_INSTALL=true INSTALL_SRC="http://github.com/jsk-ros-pkg/jsk_planning" TEST_PKGS="task_compiler"
     - ROS_DISTRO=melodic USE_DEB=true   ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu
     - ROS_DISTRO=melodic USE_DEB=false  NOT_TEST_INSTALL=true
     - ROS_DISTRO=melodic USE_DEB=source NOT_TEST_INSTALL=true
+    - ROS_DISTRO=melodic USE_DEB=true   NOT_TEST_INSTALL=true INSTALL_SRC="http://github.com/jsk-ros-pkg/jsk_pr2eus" TEST_PKGS="pr2eus pr2eus_moveit"
 matrix:
   fast_finish: true
   # allow_failures:

roseus/CMakeLists.txt

@@ -190,7 +190,6 @@ if(CATKIN_ENABLE_TESTING)
   add_rostest(test/test-mark-lock.test)
   add_rostest(test/test-tf.test)
   add_rostest(test/test-disconnect.test)
-  add_rostest(test/test-subscribe-dispose.test)
   add_rostest(test/test-multi-queue.test)
   add_rostest(test/test-namespace.test)
   add_rostest(test/test-nodename.test)

roseus/euslisp/actionlib.l

@@ -15,7 +15,7 @@
 (unless (find-package "ROSACTIONLIB") (make-package "ROSACTIONLIB"))
 (defclass ros::simple-action-client
   :super ros::object
-  :slots (name-space action-spec simple-state comm-state status-stamp last-status-msg
+  :slots (name-space action-spec simple-state comm-state status-stamp
                      action-goal-class action-result-class action-feedback-class
                      action-done-cb action-active-cb action-feedback-cb
                      goal_id groupname seq-count))
@@ -24,7 +24,6 @@
   (:goal-status-cb
    (msg)
    ;;(ros::ros-debug "[~A] status-cb (seq:~A ~A)" name-space (send msg :header :seq) (mapcar #'(lambda (s) (format nil "id:~A, status:~A" (send s :goal_id :id) (goal-status-to-string (send s :status)))) (send msg :status_list)))
-   (setq last-status-msg msg)
    (let ((m (send comm-state :find-status-by-goal-id msg)))
      (when m
        (let ((com-state (send comm-state :update-status msg))
@@ -59,7 +58,6 @@
    (msg)
    (let (dummy-msg)
      (ros::ros-debug "[~A] reuslt-cb ~A ~A" name-space (send msg :status :goal_id :id) (goal-status-to-string (send msg :status :status)))
-     (if (null goal_id) (return-from :action-result-cb nil))
      (unless (send comm-state :update-result msg)
        (ros::ros-warn "[~A] action-result-cb may recieved old client's goal" name-space)
        (ros::ros-warn "     expected goal id ~A" (and (send comm-state :action-goal) (send (send comm-state :action-goal) :goal_id :id)))
@@ -234,7 +232,6 @@
          (setq state actionlib_msgs::GoalStatus::*active*)))
       state))
   (:get-goal-status-text () (send (send comm-state :latest-goal-status) :text))
-  (:last-status-msg () last-status-msg)
   (:cancel-all-goals
    ()
    (let (cancel)

roseus/roseus.cpp

@@ -123,12 +123,6 @@ using namespace std;
 #define isInstalledCheck \
   if( ! ros::ok() ) { error(E_USER,"You must call (ros::roseus \"name\") before creating the first NodeHandle"); }
 
-class EusSubscriber : public ros::Subscriber {
-public:
-  EusSubscriber(Subscriber e) : ros::Subscriber(e) {
-  }
-  pointer _funcp;
-};
 class RoseusStaticData
 {
 public:
@@ -138,7 +132,7 @@ class RoseusStaticData
   boost::shared_ptr<ros::NodeHandle> node;
   boost::shared_ptr<ros::Rate> rate;
   map<string, boost::shared_ptr<Publisher> > mapAdvertised; ///< advertised topics
-  map<string, boost::shared_ptr<EusSubscriber> > mapSubscribed; ///< subscribed topics
+  map<string, boost::shared_ptr<Subscriber> > mapSubscribed; ///< subscribed topics
   map<string, boost::shared_ptr<ServiceServer> > mapServiced; ///< subscribed topics
   map<string, Timer > mapTimered; ///< subscribed timers
 
@@ -356,7 +350,7 @@ template<> struct Serializer<EuslispMessage> {
  ************************************************************/
 class EuslispSubscriptionCallbackHelper : public ros::SubscriptionCallbackHelper {
 public:
-  pointer _scb,_args,_funcp;
+  pointer _scb,_args;
   EuslispMessage _msg;
 
   EuslispSubscriptionCallbackHelper(pointer scb, pointer args,pointer tmpl) :  _args(args), _msg(tmpl) {
@@ -376,8 +370,7 @@ class EuslispSubscriptionCallbackHelper : public ros::SubscriptionCallbackHelper
     }
     // avoid gc
     pointer p=gensym(ctx);
-    _funcp=intern(ctx,(char*)(p->c.sym.pname->c.str.chars),strlen((char*)(p->c.sym.pname->c.str.chars)),lisppkg);
-    set_special(ctx,_funcp,cons(ctx,scb,args));
+    setval(ctx,intern(ctx,(char*)(p->c.sym.pname->c.str.chars),strlen((char*)(p->c.sym.pname->c.str.chars)),lisppkg),cons(ctx,scb,args));
   }
   ~EuslispSubscriptionCallbackHelper() {
       ROS_ERROR("subscription gc");
@@ -885,18 +878,8 @@ pointer ROSEUS_SUBSCRIBE(register context *ctx,int n,pointer *argv)
   SubscribeOptions so(topicname, queuesize, msg.__getMD5Sum(), msg.__getDataType());
   so.helper = *callback;
   Subscriber subscriber = lnode->subscribe(so);
-  boost::shared_ptr<EusSubscriber> sub = boost::shared_ptr<EusSubscriber>(new EusSubscriber(static_cast<EusSubscriber>(subscriber)));
-  sub->_funcp = boost::dynamic_pointer_cast<EuslispSubscriptionCallbackHelper>(*callback)->_funcp;
+  boost::shared_ptr<Subscriber> sub = boost::shared_ptr<Subscriber>(new Subscriber(subscriber));
   if ( !!sub ) {
-    if( s_mapSubscribed.find(topicname) != s_mapSubscribed.end() ) {
-      pointer dest=(pointer)mkstream(ctx,K_OUT,makebuffer(64));
-      prinx(ctx,speval(boost::dynamic_pointer_cast<EuslispSubscriptionCallbackHelper>(*callback)->_funcp),dest);
-      pointer str = makestring((char *)dest->c.stream.buffer->c.str.chars,
-                               intval(dest->c.stream.count));
-      ROS_DEBUG("topic %s already subscribed, release previous call back functions %s", topicname.c_str(), get_string(str));
-      // clean up pointer to avoid gc
-      set_special(ctx,s_mapSubscribed[topicname]->_funcp,NIL);
-    }
     s_mapSubscribed[topicname] = sub;
   } else {
     ROS_ERROR("s_mapSubscribed");
@@ -928,7 +911,7 @@ pointer ROSEUS_GETNUMPUBLISHERS(register context *ctx,int n,pointer *argv)
   else error(E_NOSTRING);
 
   bool bSuccess = false;
-  map<string, boost::shared_ptr<EusSubscriber> >::iterator it = s_mapSubscribed.find(topicname);
+  map<string, boost::shared_ptr<Subscriber> >::iterator it = s_mapSubscribed.find(topicname);
   if( it != s_mapSubscribed.end() ) {
     boost::shared_ptr<Subscriber> subscriber = (it->second);
     ret = subscriber->getNumPublishers();
@@ -948,7 +931,7 @@ pointer ROSEUS_GETTOPICSUBSCRIBER(register context *ctx,int n,pointer *argv)
   else error(E_NOSTRING);
 
   bool bSuccess = false;
-  map<string, boost::shared_ptr<EusSubscriber> >::iterator it = s_mapSubscribed.find(topicname);
+  map<string, boost::shared_ptr<Subscriber> >::iterator it = s_mapSubscribed.find(topicname);
   if( it != s_mapSubscribed.end() ) {
     boost::shared_ptr<Subscriber> subscriber = (it->second);
     ret = subscriber->getTopic();

roseus/test/simple-client-cancel-test.l

@@ -155,6 +155,42 @@
       (assert nil "Goal 2 didn't finish~%"))
     ))
 
+(deftest test-client-send-one-goal-with-two-instance ()
+  (let (c1 c2 g1 g2)
+    ;;
+    ;; send goal twice from two instance
+    ;;
+    (setq c1 (instance ros::simple-action-client :init
+                      "reference_action" actionlib::TestAction))
+    (warning-message 1 "wait-for-server 1~%")
+    (send c1 :wait-for-server)
+
+    (setq g1 (instance actionlib::TestActionGoal :init))
+    (warning-message 1 "send-goal 1~%")
+    (send g1 :goal :goal 4) ;; 10 sec to goal return set aborted
+    (send c1 :send-goal g1)
+    (warning-message 1 "sent goal 1 (~A)~%" (send g1 :goal_id :id))
+    (unix:sleep 1)
+    ;;
+    (setq c2 (instance ros::simple-action-client :init
+                      "reference_action" actionlib::TestAction))
+    (warning-message 1 "wait-for-server 2~%")
+    (send c2 :wait-for-server)
+    ;; goal send by c1 is taken by c2, thus call (send c2 :wait-for-result) never retuns, so we need actinlib do not return from :wait-for-result, if that goal is overridden from different instance in same roseus process  hack (https://github.com/jsk-ros-pkg/jsk_roseus/pull/519#issuecomment-303000720)
+
+    ;;
+    (warning-message 1 "wait-for-result 1 (~A)~%" (send g1 :goal_id :id))
+    (unless (send c1 :wait-for-result :timeout 20) ;; c1 is retuen after 10 sec
+      (warning-message 1 "wail-for-result 1 failed...~%")
+      ;; if the goal is aborted that's what we expected
+      (cond ((equal actionlib_msgs::GoalStatus::*ABORTED*
+                    (send c1 :get-state))
+             (warning-message 1 "goal is aborted, as expected...~%")
+             )
+            (t
+             (assert nil "Goal 1 didn't finish~%"))))
+    ))
+
 (setq *dispose* nil)
 (defclass ros::simple-action-client-dispose
   :super ros::simple-action-client)

roseus/test/simple-client-test.l

@@ -15,7 +15,7 @@
 (setq sys::*gc-hook* #'(lambda (a b) (format t ";; gc ~A ~A~%" a b)))
 
 (deftest test-simple-client ()
-  (let (c goal status)
+  (let (c goal)
     (setq c (instance ros::simple-action-client :init
                       "reference_simple_action" actionlib::TestAction))
     (warning-message 1 "wait-for-server~%")
@@ -59,11 +59,6 @@
     (assert (equal (send c :get-state) actionlib_msgs::GoalStatus::*succeeded*))
     (assert (equal (send (elt saved-feedback 0) :feedback :feedback) 9))
 
-    ;; check last-status-msg
-    (setq status (send c :last-status-msg))
-    (assert (derivedp status actionlib_msgs::GoalStatusArray))
-    (assert (> (length (send status :status_list)) 0))
-
     (send c :cancel-all-goals)
     ))