[jsk_2015_05_baxter_apc] Test motion for move arm to bin

jsk_2015_05_baxter_apc/CMakeLists.txt

@@ -237,13 +237,7 @@ install(TARGETS
 #   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
 # endif()
 
-# TODO(wkentaro): add test for recognition & motion
-# if(CATKIN_ENABLE_TESTING)
-#   find_package(rostest REQUIRED)
-#   # install test data by a script
-#   add_custom_target(install_test_data COMMAND ${PROJECT_SOURCE_DIR}/scripts/install_test_data.py)
-#   if(TARGET tests)
-#     add_dependencies(tests install_test_data)
-#   endif()
-#   add_rostest(test/test_recognition_in_hand.test DEPENDENCIES install_test_data)
-# endif()
+if(CATKIN_ENABLE_TESTING)
+  find_package(jsk_tools REQUIRED)
+  jsk_tools_add_shell_test(COMMAND "roseus ${PROJECT_SOURCE_DIR}/euslisp/test/test-move-arm-to-bin.l")
+endif()

jsk_2015_05_baxter_apc/euslisp/test/test-move-arm-to-bin.l

@@ -0,0 +1,39 @@
+;; vim: set ft=lisp:
+
+(require :unittest "lib/llib/unittest.l")
+
+(require :baxter "package://jsk_2015_05_baxter_apc/euslisp/jsk_2015_05_baxter_apc/baxter.l")
+(require :util "package://jsk_2015_05_baxter_apc/euslisp/jsk_2015_05_baxter_apc/util.l")
+
+;; load *bin-cubes*
+(require :bin-cubes "package://jsk_2015_05_baxter_apc/euslisp/test/data/bin-cubes.l")
+
+
+(init-unit-test)
+
+
+(setq *baxter* (instance jsk_2015_05_baxter_apc::baxter-robot :init))
+(setq *robot* *baxter*)
+
+
+(deftest
+  test-move-arm-to-bin
+  (dolist (bin (list :a :b :c :d :e :f :g :h :i :j :k :l))
+    (case bin
+      ((:a :b :d :e :g :h :j :k) (setq arm :larm))
+      ((:c :f :i :l) (setq arm :rarm))
+      )
+    (let ((cube (gethash bin *bin-cubes*)))
+      (dolist (offset (list #f(-150 0 0) #f(-50 0 0) #f(0 0 0) #f(0 0 -80)))
+        (let ((goal (send cube :copy-worldcoords)))
+          (send goal :translate offset :world)
+          (assert (send *robot* arm :inverse-kinematics goal))
+          )
+        )
+      )
+    )
+  )
+
+
+(run-all-tests)
+(exit)

jsk_2015_05_baxter_apc/package.xml

@@ -53,7 +53,6 @@
   <run_depend>turtlebot_description</run_depend>
 
   <test_depend>jsk_tools</test_depend>
-  <test_depend>rostest</test_depend>
 
   <export></export>
 </package>