add dummy_jta_server.py and pr2-ri-jta for example code of jsk-ros-pk…

…g#460

pr2eus/CMakeLists.txt

@@ -54,6 +54,7 @@ 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/pr2-ri-jta.test)
   endif()
   add_rostest(test/robot-no-clock.test)
   add_rostest(test/default-ri-test.test)

pr2eus/test/dummy_jta_server.py

@@ -0,0 +1,41 @@
+#!/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 = FollowJointTrajectoryActionFeedback()
+    _result = FollowJointTrajectoryResult()
+
+    def __init__(self):
+        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")
+
+    def execute_cb(self, goal):
+        success = True
+        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))
+            if position < 0:
+                rospy.logwarn("Set aborted")
+                self._as.set_aborted()
+            if position >= 100:
+                rospy.logwarn("Set preempted")
+                self._as.set_preempted()
+                success = False
+        self._result.error_code = FollowJointTrajectoryResult.SUCCESSFUL
+        if success:
+            self._as.set_succeeded(self._result)
+
+if __name__ == '__main__':
+    rospy.init_node('dummy_jta')
+    server = DummyJTA()
+    rospy.spin()

pr2eus/test/pr2-ri-jta.l

@@ -0,0 +1,64 @@
+#!/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))
+
+;;
+
+(deftest test-abort
+  (send *ri* :angle-vector #f(-100)))
+
+(deftest test-preempted
+  (send *ri* :angle-vector #f(200)))
+
+(deftest test-suceeded
+  (send *ri* :angle-vector #f(0)))
+
+(run-all-tests)
+(exit)

pr2eus/test/pr2-ri-jta.test

@@ -0,0 +1,7 @@
+<launch>
+  <node pkg="pr2eus" type="dummy_jta_server.py" name="dummy_jta_server" />
+
+  <!-- start test -->
+  <test test-name="pr2_ri_test_arm" pkg="roseus" type="roseus" retry="1"
+        args="$(find pr2eus)/test/pr2-ri-jta.l" />
+</launch>