Merge pull request #460 from tkmtnt7000/add_result_cb

add displaying error msg method to controller-action-client in robot-interface.l

.github/workflows/config.yml

@@ -96,7 +96,7 @@ jobs:
              git config --global --add safe.directory $GITHUB_WORKSPACE
           fi
 
-      - name: Chcekout
+      - name: Checkout
         uses: actions/checkout@v3.0.2
 
       - name: Run jsk_travis

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,7 @@
 (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")
 
 (defun shortest-angle (d0 d1)
   (atan2 (sin (- d0 d1)) (cos (- d0 d1))))
@@ -44,6 +45,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)

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/ri-jta.l

@@ -0,0 +1,68 @@
+#!/usr/bin/env
+
+(load "package://pr2eus/pr2-interface.l")
+(load "unittest.l")
+
+(init-unit-test)
+
+(defclass dummy-robot
+  :super robot-model
+  :slots (link-1 link-2 joint-1))
+(defmethod dummy-robot
+  (:init ()
+   (let ()
+     (send-super :init)
+     (setq link-1 (instance bodyset-link :init (make-cascoords)
+                            :bodies (let ((b (make-cube 50 50 100)))
+                                      (send b :set-color :red) (list b))))
+     (setq link-2 (instance bodyset-link :init (make-cascoords :pos #f(0 0 50))
+                            :bodies (let ((b (make-cube 40 40 100 :pos #f(0 0 100))))
+                                      (send b :set-color :green) (list b))))
+     (setq joint-1 (instance rotational-joint :init :name "joint_1"
+                             :parent-link link-1 :child-link link-2
+                             :axis :y :min *-inf* :max *inf*))
+     (setq links (list link-1 link-2))
+     (setq joint-list (list joint-1))
+     (send self :assoc link-1)
+     (send link-1 :assoc link-2)
+     (send self :init-ending)))
+  (:joint_1 (&rest args) (forward-message-to joint-1 args))
+  )
+
+(defun dummy-robot () (instance dummy-robot :init))
+
+(defclass dummy-robot-interface
+  :super robot-interface
+  :slots ())
+(defmethod dummy-robot-interface
+  (:init (&rest args) (send-super* :init :robot (dummy-robot) args))
+  (:default-controller
+   ()
+   (list
+    (list
+     (cons :controller-action "dummy_controller/follow_joint_trajectory")
+     (cons :controller-state "dummy_controller/state")
+     (cons :action-type control_msgs::FollowJointTrajectoryAction)
+     (cons :joint-names '("joint_1")))))
+  )
+
+(setq *robot* (dummy-robot))
+(setq *ri* (instance dummy-robot-interface :init))
+
+;; Tests
+;; Use :wait-interpolation after :angle-vector to call :action-result-cb
+
+(deftest test-abort
+  (send *ri* :angle-vector #f(-100))
+  (send *ri* :wait-interpolation))
+
+(deftest test-preempted
+  (send *ri* :angle-vector #f(200))
+  (send *ri* :wait-interpolation))
+
+(deftest test-suceeded
+  (send *ri* :angle-vector #f(0))
+  (send *ri* :wait-interpolation))
+
+(run-all-tests)
+(exit)

pr2eus/test/ri-jta.test

@@ -0,0 +1,13 @@
+<launch>
+  <arg name="robot" default="pr2" />
+
+  <node pkg="pr2eus" type="dummy_jta_server.py" name="dummy_jta_server" >
+    <rosparam subst_value="true">
+      robot: $(arg robot)
+    </rosparam>
+  </node>
+
+  <!-- start test -->
+  <test test-name="$(arg robot)_ri_test_arm" pkg="roseus" type="roseus" retry="1"
+        args="$(find pr2eus)/test/ri-jta.l" />
+</launch>