Merge branch 'master' into go-pos-unsafe-return

.github/workflows/config.yml

@@ -50,14 +50,16 @@ jobs:
             CONTAINER: ubuntu:18.04
             USE_DEB : true
             TEST_PKGS: "pr2eus_moveit pr2eus_tutorials"
-          # - ROS_DISTRO: noetic
-          #   CONTAINER: ubuntu:20.04
-          #   USE_DEB : true
-          #   TEST_PKGS: "pr2eus"
-          # - ROS_DISTRO: noetic
-          #   CONTAINER: ubuntu:20.04
-          #   USE_DEB : true
-          #   TEST_PKGS: "pr2eus_moveit pr2eus_tutorials"
+          - ROS_DISTRO: noetic
+            CONTAINER: ubuntu:20.04
+            USE_DEB : true
+            BUILD_PKGS: "pr2eus"
+            TEST_PKGS: "pr2eus"
+          - ROS_DISTRO: noetic
+            CONTAINER: ubuntu:20.04
+            USE_DEB : true
+            BUILD_PKGS: "pr2eus_moveit pr2eus_tutorials"
+            TEST_PKGS: "pr2eus_moveit pr2eus_tutorials"
           - ROS_DISTRO: noetic
             CONTAINER: ubuntu:20.04
             USE_DEB : false
@@ -94,8 +96,8 @@ jobs:
              git config --global --add safe.directory $GITHUB_WORKSPACE
           fi
 
-      - name: Chcekout
-        uses: actions/checkout@v2
+      - name: Checkout
+        uses: actions/checkout@v3.0.2
 
       - name: Run jsk_travis
         uses: jsk-ros-pkg/jsk_travis@master

pr2eus/CMakeLists.txt

@@ -55,6 +55,9 @@ if(CATKIN_ENABLE_TESTING)
     endif()
     add_rostest(test/pr2-ri-test-simple.launch)
     add_rostest(test/robot-init-test.test) # this uses pr2
+    add_rostest(test/ri-jta.test ARGS robot:=pr2)
+    add_rostest(test/ri-jta.test ARGS robot:=fetch)
+    add_rostest(test/ri-jta.test ARGS robot:=kinova)
   endif()
   add_rostest(test/robot-no-clock.test)
   add_rostest(test/default-ri-test.test)

pr2eus/robot-interface.l

@@ -9,6 +9,13 @@
 (ros::roseus-add-msgs "move_base_msgs")
 (ros::roseus-add-msgs "nav_msgs")
 (require :speak "package://pr2eus/speak.l")
+(require :actionlib-commstate "package://roseus/euslisp/actionlib-commstate.l")
+
+;; ROSEUS_RESUME
+(defvar *enable-roseus-resume*
+  #+:condition (ros::rospack-find "roseus_resume"))
+;; make dummy package to avoid symbol search error
+(unless (find-package "ROSEUS_RESUME") (make-package "ROSEUS_RESUME"))
 
 (defun shortest-angle (d0 d1)
   (atan2 (sin (- d0 d1)) (cos (- d0 d1))))
@@ -44,6 +51,50 @@
   (:time-to-finish ()
     (ros::ros-debug "[~A] time-to-finish ~A" ros::name-space time-to-finish)
     time-to-finish)
+  (:action-result-cb (msg)
+    "
+    display error messages from [depending on robots]/follow_joint_trajectory/result
+    "
+    ;; The following error messages will not appear for the first (send *ri* :angle-vector)
+    ;; To display it, use :wait-interpolation or (ros::spin-once groupname)
+    (let ((status-num (send (send msg :status) :status))
+          (status-text (send (send msg :status) :text))
+          (error-code nil)
+          (error-word '("SUCCESSFUL"
+                        "INVALID_GOAL"
+                        "INVALID_JOINTS"
+                        "OLD_HEADER_TIMESTAMP"
+                        "PATH_TOLERANCE_VIOLATED"
+                        "GOAL_TOLERANCE_VIOLATED")))
+      ;; status
+      (let ((log-func nil))
+        (cond ((= status-num actionlib_msgs::GoalStatus::*succeeded*)
+               (send-super :action-result-cb msg)
+               (return-from :action-result-cb nil))
+              ((= status-num actionlib_msgs::GoalStatus::*recalled*)
+               (setq log-func #'ros::ros-warn))
+              (t
+               (setq log-func #'ros::ros-error)))
+        (when log-func
+          (funcall log-func "joint trajectory status: ~A~%"
+                   (cons status-num (goal-status-to-string status-num)))
+          (unless (string= "" status-text)
+            (funcall log-func "~A~%" status-text))))
+      ;; error_code and error_string
+      (cond ((derivedp msg control_msgs::FollowJointTrajectoryActionResult)
+             ;; e.g. pr2_controllers_msgs/JointTrajectoryActionResult does not have :error_code
+             (setq error-code (send (send msg :result) :error_code))
+             (when (< error-code 0)
+               (ros::ros-error "error_code: ~A~%"
+                               (cons error-code (elt error-word (* -1 error-code))))
+               (unless (string= "" (send (send msg :result) :error_string))
+                 (ros::ros-error "~A~%" (send (send msg :result) :error_string)))))
+            (t
+             (ros::ros-warn "~A ~A ~A"
+                            "Result message type is not"
+                            "control_msgs/FollowJointTrajectoryActionResult."
+                            "So error_code is not displaying."))))
+    (send-super :action-result-cb msg))
   (:action-feedback-cb (msg)
    (let ((finish-time) (current-time))
      (ros::ros-debug "[~A] feedback-cb ~A" ros::name-space msg)
@@ -219,6 +270,9 @@
        (pushnew `(lambda () (send ,self :robot-interface-simulation-callback)) *timer-job*)
        (warning-message 3 "current *timer-job* is ~A~%" *timer-job*)
        ))
+   ;;
+   (when *enable-roseus-resume*
+     (roseus_resume::install-interruption-handler self))
    self)
   ;;
   (:add-controller (ctype &key (joint-enable-check) (create-actions))
@@ -2254,4 +2308,11 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i
       (funcall (eval (read-from-string (format nil "#'~A-init" robot-name))))
       ))))
 
+;; roseus_resume must be loaded after `robot-interface' definition
+(if *enable-roseus-resume*
+    (progn
+      (require :roseus_resume "package://roseus_resume/euslisp/interruption-handler.l")
+      (warning-message 2 ";; roseus_resume is enabled.~%"))
+    (warning-message 3 ";; roseus_resume is disabled.~%"))
+
 (provide :robot-interface "robot-interface.l")

pr2eus/test/dummy_jta_server.py

@@ -0,0 +1,79 @@
+#!/usr/bin/env python
+
+
+import math
+import rospy
+import actionlib
+from control_msgs.msg import *
+
+
+class DummyJTA(object):
+    # create messages that are used to publish feedback/result
+    _feedback = FollowJointTrajectoryFeedback()
+    _result = FollowJointTrajectoryResult()
+
+    def __init__(self, robot='pr2'):
+        self._as = actionlib.SimpleActionServer('dummy_controller/follow_joint_trajectory', FollowJointTrajectoryAction, execute_cb=self.execute_cb, auto_start = False)
+        self._as.start()
+        rospy.logwarn("Started Dummy Joint Trajectory Action Server")
+        rospy.logwarn("If joint < 0, set aborted")
+        rospy.logwarn("If joint >= 100, set preempted")
+
+    # Return result based on real robot jta server.
+    # https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/460#issuecomment-975418370
+    def execute_cb(self, goal):
+        if len(goal.trajectory.points) > 0 and len(goal.trajectory.points[0].positions) > 0:
+            position = goal.trajectory.points[0].positions[0] * 180 / math.pi
+            rospy.loginfo("Received {}".format(position))
+            self._as.publish_feedback(self._feedback)
+            if robot == 'pr2':
+                if position < 0:
+                    rospy.logwarn("Set aborted")
+                    self._result.error_code = FollowJointTrajectoryResult.GOAL_TOLERANCE_VIOLATED
+                    self._as.set_aborted(result=self._result)
+                elif position >= 100:
+                    rospy.logwarn("Set preempted")
+                    self._result.error_code = FollowJointTrajectoryResult.SUCCESSFUL
+                    self._as.set_preempted(result=self._result)
+                else:
+                    self._result.error_code = FollowJointTrajectoryResult.SUCCESSFUL
+                    self._as.set_succeeded(result=self._result)
+            if robot == 'fetch':
+                if position < 0:
+                    rospy.logwarn("Set aborted")
+                    self._result.error_code = FollowJointTrajectoryResult.SUCCESSFUL
+                    self._as.set_aborted(
+                        result=self._result,
+                        text='Controller manager forced preemption.')
+                elif position >= 100:
+                    rospy.logwarn("Set preempted")
+                    self._result.error_code = FollowJointTrajectoryResult.SUCCESSFUL
+                    self._as.set_preempted(
+                        result=self._result,
+                        text='Trajectory preempted')
+                else:
+                    self._result.error_code = FollowJointTrajectoryResult.SUCCESSFUL
+                    self._as.set_succeeded(
+                        result=self._result,
+                        text='Trajectory succeeded.')
+            if robot == 'kinova':
+                if position < 0:
+                    rospy.logwarn("Set aborted")
+                    self._result.error_code = FollowJointTrajectoryResult.PATH_TOLERANCE_VIOLATED
+                    self._result.error_string = 'After validation, trajectory execution failed in the arm with sub error code SUB_ERROR_NONE'
+                    self._as.set_aborted(result=self._result)
+                elif position >= 100:
+                    rospy.logwarn("Set preempted")
+                    self._result.error_code = FollowJointTrajectoryResult.PATH_TOLERANCE_VIOLATED
+                    self._result.error_string = 'Trajectory execution failed in the arm with sub error code 55\nThe speed while executing\\n\ the trajectory was too damn high and caused the robot to stop.\n'
+                    # NOTE: this line is not typo but kinova driver's behavior
+                    self._as.set_aborted(result=self._result)
+                else:
+                    self._result.error_code = FollowJointTrajectoryResult.SUCCESSFUL
+                    self._as.set_succeeded(result=self._result)
+
+if __name__ == '__main__':
+    rospy.init_node('dummy_jta')
+    robot = rospy.get_param('~robot', 'pr2')
+    server = DummyJTA(robot)
+    rospy.spin()

pr2eus/test/pr2-ri-test-base.launch

@@ -1,9 +1,12 @@
 <launch>
   <arg name="gui" default="false"/>
+  <env name="DISPLAY" value=":0.0" if="$(arg gui)"/>
+  <env name="DISPLAY" value="" unless="$(arg gui)"/>
+
   <include file="$(find pr2eus)/test/pr2-ri-test-bringup.launch"
            pass_all_args="true" />
 
   <!-- start test -->
-  <test test-name="pr2_ri_test_base" pkg="roseus" type="roseus" retry="1"
+  <test test-name="pr2_ri_test_base" pkg="roseus" type="roseus" retry="3"
         args="$(find pr2eus)/test/pr2-ri-test-base.l" time-limit="800" />
 </launch>

pr2eus/test/pr2-ri-test-bringup.launch

@@ -63,7 +63,7 @@
     <param name="sensor_timeout" value="1.0"/>
     <param name="publish_tf" value="true"/>
     <param name="odom_used" value="true"/>
-    <param name="imu_used" value="true"/>
+    <param name="imu_used" value="false"/>
     <param name="vo_used" value="false"/>
     <remap from="odom" to="base_odometry/odom" />
     <remap from="imu_data" to="torso_lift_imu/data" />

pr2eus/test/pr2-ri-test-simple.l → pr2eus/test/pr2-ri-test-simple-angle-vector.l

@@ -49,31 +49,6 @@
     (assert (eps= tm 1.0))
     ))
 
-;; https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/165#discussion_r37421484
-(deftest test-go-pos
-  (let ()
-    (setq *ri* (instance pr2-interface :init))
-    (assert (send *ri* :go-pos 1 0 0) "(send *ri* :go-pos 1 0 0)") ;; go-pos is relative to current position
-    (assert (send *ri* :go-pos 0 1 90) "(send *ri* :go-pos 0 1 90)")
-    (assert (send *ri* :go-pos-no-wait -1 1 -90) "(send *ri* :go-pos-no-wait -1 1 -90)")
-    (ros::sleep 1) ;; wait for 1 sec to activate goal status...
-    (assert (send *ri* :go-waitp) "(send *ri* :go-waitp)")
-    (assert (send *ri* :go-wait) "(send *ri* :go-wait)")
-    (assert (eps-v= (send (send *ri* :worldcoords ) :worldpos) #f(0 0 0)))
-    (assert (send *ri* :go-pos-unsafe-no-wait -1 1 -90) "(send *ri* :go-pos-unsafe-no-wait -1 1 -90)")
-    ))
-
-(deftest test-move-to
-  (let ()
-    (setq *ri* (instance pr2-interface :init))
-    (assert (send *ri* :move-to (make-coords :pos #f(1000 0 0))) "(send *ri* :move-to (make-coords :pos #f(1000 0 0)))") ;; default is world and wait
-    (send *ri* :move-to (make-coords :pos #f(1000 1000 0) :rpy (float-vector pi/2 0 0)))
-    (assert (send *ri* :move-to (make-coords) :no-wait t) "(send *ri* :move-to (make-coords) :no-wait t)") ;; no-wait t means not wait so need to call wait
-    (assert (send *ri* :move-to-wait) "(send *ri* :move-to-wait)") ;; wait move-to
-    (assert (eps-v= (send (send *ri* :worldcoords ) :worldpos) #f(0 0 0)))
-    ))
-
-
 ;; https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/143
 (defclass pr2-interface-wrong
   :super pr2-interface)
@@ -158,55 +133,6 @@
       )
     ))
 
-;; pr2 / pr2-sensor-robot test
-(deftest instantiate-pr2-sensor-robot-test
-  (let (rgb-camera-pr1012 rgb-camera-pr1040)
-    ;; call from function
-    (pr2)
-    (assert *pr2*) ;; generate *pr2*
-    (assert (derivedp *pr2* pr2-sensor-robot)) ;; pr2 derived from pr2-sensor-robot
-    (assert (equal (send *pr2* :name) "pr2")) ;; robot name is pr2
-    (assert (equal (*pr2* . name) :pr1012)) ;; but we can know serial number
-    (setq rgb-camera-1012 (send (send *pr2* :camera :kinect_head/rgb) :viewing :projection))
-
-    (pr2 :pr1040)
-    (assert *pr2*) ;; generated *pr2*
-    (assert (derivedp *pr2* pr2-sensor-robot)) ;; pr2 derived from pr2-sensor-robot
-    (assert (equal (send *pr2* :name) "pr2")) ;; robot name is pr2
-    (assert (equal (*pr2* . name) :pr1040)) ;; but we can know serial number
-    (setq rgb-camera-1040 (send (send *pr2* :camera :kinect_head/rgb) :viewing :projection))
-    (defun m= (m1 m2) (v= (array-entity m1) (array-entity m2)))
-    (when (m= rgb-camera-1012 rgb-camera-1040)
-      (warning-message 1 "each robot should have different camera param~%~A~%~A~%"
-                       rgb-camera-1040 rgb-camera-1040))
-
-    ;; use instance
-    (setq *pr2* (instance pr2-robot :init))
-    (assert *pr2*) ;; generate *pr2*
-    (assert (derivedp *pr2* pr2-robot)) ;; pr2 derived from pr2-sensor-robot
-    (assert (equal (send *pr2* :name) "pr2")) ;; robot name is pr2
-    (assert (null (assoc 'name (send *pr2* :slots)))) ;; but it does not have slot 'name
-
-    (setq *pr2* (instance pr2-sensor-robot :init))
-    (assert *pr2*) ;; generate *pr2*
-    (assert (derivedp *pr2* pr2-robot)) ;; pr2 derived from pr2-sensor-robot
-    (assert (equal (send *pr2* :name) "pr2")) ;; robot name is pr2
-    (assert (equal (*pr2* . name) :pr1012)) ;; but we can know serial number
-
-    (setq *pr2* (instance pr2-sensor-robot :init :pr1012))
-    (assert *pr2*) ;; generated *pr2*
-    (assert (derivedp *pr2* pr2-sensor-robot)) ;; pr2 derived from pr2-sensor-robot
-    (assert (equal (send *pr2* :name) "pr2")) ;; robot name is pr2
-    (assert (equal (*pr2* . name) :pr1012)) ;; but we can know serial number
-
-    (setq *pr2* (instance pr2-sensor-robot :init :pr1040))
-    (assert *pr2*) ;; generated *pr2*
-    (assert (derivedp *pr2* pr2-sensor-robot)) ;; pr2 derived from pr2-sensor-robot
-    (assert (equal (send *pr2* :name) "pr2")) ;; robot name is pr2
-    (assert (equal (*pr2* . name) :pr1040)) ;; but we can know serial number
-    ))
 
 (run-all-tests)
 (exit)
-
-

pr2eus/test/pr2-ri-test-simple-go-pos.l

@@ -0,0 +1,33 @@
+(require :unittest "lib/llib/unittest.l")
+(load "package://pr2eus/pr2-interface.l")
+
+(init-unit-test)
+
+(pr2-init)
+
+;; https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/165#discussion_r37421484
+(deftest test-go-pos
+  (let ()
+    (setq *ri* (instance pr2-interface :init))
+    (assert (send *ri* :go-pos 1 0 0) "(send *ri* :go-pos 1 0 0)") ;; go-pos is relative to current position
+    (assert (send *ri* :go-pos 0 1 90) "(send *ri* :go-pos 0 1 90)")
+    (assert (send *ri* :go-pos-no-wait -1 1 -90) "(send *ri* :go-pos-no-wait -1 1 -90)")
+    (ros::sleep 1) ;; wait for 1 sec to activate goal status...
+    (assert (send *ri* :go-waitp) "(send *ri* :go-waitp)")
+    (assert (send *ri* :go-wait) "(send *ri* :go-wait)")
+    (assert (eps-v= (send (send *ri* :worldcoords ) :worldpos) #f(0 0 0)))
+    (assert (send *ri* :go-pos-unsafe-no-wait -1 1 -90) "(send *ri* :go-pos-unsafe-no-wait -1 1 -90)")
+    ))
+
+(deftest test-move-to
+  (let ()
+    (setq *ri* (instance pr2-interface :init))
+    (assert (send *ri* :move-to (make-coords :pos #f(1000 0 0))) "(send *ri* :move-to (make-coords :pos #f(1000 0 0)))") ;; default is world and wait
+    (send *ri* :move-to (make-coords :pos #f(1000 1000 0) :rpy (float-vector pi/2 0 0)))
+    (assert (send *ri* :move-to (make-coords) :no-wait t) "(send *ri* :move-to (make-coords) :no-wait t)") ;; no-wait t means not wait so need to call wait
+    (assert (send *ri* :move-to-wait) "(send *ri* :move-to-wait)") ;; wait move-to
+    (assert (eps-v= (send (send *ri* :worldcoords ) :worldpos) #f(0 0 0)))
+    ))
+
+(run-all-tests)
+(exit)