[euscollda] add test code to check generate eus code is ok

euscollada/CMakeLists.txt

@@ -50,3 +50,21 @@ install(DIRECTORY src scripts
 file(GLOB _install_files RELATIVE ${PROJECT_SOURCE_DIR} *.yaml *.sh)
 install(FILES ${_install_files}
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+if(CATKIN_ENABLE_TESTING)
+  find_package(catkin REQUIRED COMPONENTS rostest)
+  add_custom_command(OUTPUT sample1.dae
+    COMMAND apt-get download ros-$ENV{ROS_DISTRO}-openhrp3
+    COMMAND cmake -E make_directory ./opt/ros/$ENV{ROS_DISTRO}/share/OpenHRP-3.1/sample/model/
+    COMMAND dpkg -x ros-$ENV{ROS_DISTRO}-openhrp3*.deb .
+    COMMAND cp ./opt/ros/$ENV{ROS_DISTRO}/share/OpenHRP-3.1/sample/model/sample1.dae sample1.dae
+    DEPENDS collada2eus)
+  add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/test/sample1-noyaml.l
+    COMMAND rosrun euscollada collada2eus sample1.dae ${PROJECT_SOURCE_DIR}/test/sample1-noyaml.l
+    DEPENDS sample1.dae ${PROJECT_SOURCE_DIR}/test/sample1.yaml)
+  add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/test/sample1-yaml.l
+    COMMAND rosrun euscollada collada2eus sample1.dae ${PROJECT_SOURCE_DIR}/test/sample1.yaml ${PROJECT_SOURCE_DIR}/test/sample1-yaml.l
+    DEPENDS sample1.dae ${PROJECT_SOURCE_DIR}/test/sample1.yaml)
+  add_custom_target(generate_test_files ALL DEPENDS ${PROJECT_SOURCE_DIR}/test/sample1-noyaml.l ${PROJECT_SOURCE_DIR}/test/sample1-yaml.l)
+  add_rostest(test/test-sample-robot.test)
+endif()

euscollada/package.xml

@@ -48,6 +48,7 @@
   <run_depend>tf</run_depend>
   <run_depend>collada_urdf</run_depend>
 
+  <test_depend>openhrp3</test_depend>
   <!-- <test_depend>collada_urdf</test_depend> -->
   <!-- <test_depend>euslisp</test_depend> -->
   <!-- <test_depend>roseus</test_depend> -->

euscollada/test/sample1.yaml

@@ -0,0 +1,81 @@
+##
+## - collada_joint_name : euslisp_joint_name (start with :)
+##
+
+rleg:
+  - RLEG_HIP_R      : rleg-crotch-r
+  - RLEG_HIP_P      : rleg-crotch-p
+  - RLEG_HIP_Y      : rleg-crotch-y
+  - RLEG_KNEE       : rleg-knee-p
+  - RLEG_ANKLE_P    : rleg-ankle-p
+  - RLEG_ANKLE_R    : rleg-ankle-r
+rarm:
+  - RARM_SHOULDER_P : rarm-shoulder-p
+  - RARM_SHOULDER_R : rarm-shoulder-r
+  - RARM_SHOULDER_Y : rarm-shoulder-y
+  - RARM_ELBOW      : rarm-elbow-p
+  - RARM_WRIST_Y    : rarm-wrist-y
+  - RARM_WRIST_P    : rarm-wrist-p
+  - RARM_WRIST_R    : rarm-wrist-r
+lleg:
+  - LLEG_HIP_R      : lleg-crotch-r
+  - LLEG_HIP_P      : lleg-crotch-p
+  - LLEG_HIP_Y      : lleg-crotch-y
+  - LLEG_KNEE       : lleg-knee-p
+  - LLEG_ANKLE_P    : lleg-ankle-p
+  - LLEG_ANKLE_R    : lleg-ankle-r
+larm:
+  - LARM_SHOULDER_P : larm-shoulder-p
+  - LARM_SHOULDER_R : larm-shoulder-r
+  - LARM_SHOULDER_Y : larm-shoulder-y
+  - LARM_ELBOW      : larm-elbow-p
+  - LARM_WRIST_Y    : larm-wrist-y
+  - LARM_WRIST_P    : larm-wrist-p
+  - LARM_WRIST_R    : larm-wrist-r
+torso:
+  - WAIST_P      : torso-waist-p
+  - WAIST_R      : torso-waist-r
+  - CHEST        : torso-waist-y
+
+##
+## end-coords
+##
+larm-end-coords:
+  translate : [0, 0, -0.12]
+  rotate    : [0, 1,  0, 90]
+  parent    : LARM_LINK6
+rarm-end-coords:
+  translate : [0, 0, -0.12]
+  rotate    : [0, 1,  0, 90]
+  parent    : RARM_LINK6
+lleg-end-coords:
+  translate : [0.00, 0, -0.07]
+rleg-end-coords:
+  translate : [0.00, 0, -0.07]
+head-end-coords:
+  translate : [0.08, 0, 0.13]
+  rotate    : [0, 1, 0, 90]
+
+##
+## reset-pose
+##
+angle-vector:
+# old reset-pose
+#  reset-pose : [0.0, -20.0, 0.0, 47.0, -27.0, 0.0, # rleg
+#                30.0, 0.0, 0.0, -60.0,  9.0, -6.5,  36.5, # rarm
+#                0.0, -20.0, 0.0, 47.0, -27.0, 0.0, # lleg
+#                30.0, 0.0, 0.0, -60.0, -9.0, -6.5, -36.5, # larm
+#                0.0, 0.0, 0.0] # torso
+  # openhrp reset pose from the initial line of OpenHRP-3.1/sample/controller/SampleController/etc/Sample.pos
+  reset-pose : [-0.004457, -21.6929, -0.01202, 47.6723, -25.93, 0.014025, # rleg
+                17.8356, -9.13759, -6.61188, -36.456, 0.0, 0.0, 0.0, # rarm
+                -0.004457, -21.6929, -0.01202, 47.6723, -25.93, 0.014025, # lleg
+                17.8356, 9.13759, 6.61188, -36.456, 0.0, 0.0, 0.0, # larm
+                0.0, 0.0, 0.0] # torso
+  reset-manip-pose : [0.0, -20.0, 0.0, 47.0, -27.0, 0.0, # rleg
+                     30.0, 0.0, 0.0, -100.0,  9.0, -6.5,  36.5, # rarm
+                     0.0, -20.0, 0.0, 47.0, -27.0, 0.0, # lleg
+                     30.0, 0.0, 0.0, -100.0, -9.0, -6.5, -36.5, # larm
+                     0.0, 0.0, 0.0] # torso
+
+

euscollada/test/test-sample-robot.l

@@ -0,0 +1,80 @@
+#!/usr/bin/env roseus
+
+(require :unittest "lib/llib/unittest.l")
+
+(defun error (code msg1 form &optional (msg2))
+   (format *error-output* "~C[1;3~Cm~A error: ~A"
+                #x1b (+ 1 48)   *program-name* msg1)
+   (if msg2 (format *error-output* " ~A" msg2))
+   (if form (format *error-output* " in ~s" form))
+   (format *error-output* ", exitting...~C[0m~%"  #x1b)
+   (exit 1))
+
+(defun write-info (fmt &rest args)
+  (format *error-output* "~C[1;3~Cm" #x1b (+ 1 49))
+  (apply #'format *error-output* fmt args)
+  (format *error-output* "~C[0m" #x1b)
+  (finish-output *error-output*))
+
+(init-unit-test)
+
+(deftest test-no-yaml
+  (setq lisp::*error-handler* 'error)
+  (write-info "Loading robot model~%")
+  (load "sample1-noyaml.l")
+  (write-info "Create instances~%")
+  (setq *robot* (samplerobot))
+  (write-info "Check Instances name ~A~%" (send *robot* :name))
+  (assert (equal (send *robot* :name) "SampleRobot"))
+  (write-info "Done~%"))
+
+(deftest test-yaml
+  (setq lisp::*error-handler* 'error)
+  (write-info "Loading robot model~%")
+  (load "sample1-yaml.l")
+  (write-info "Create instances~%")
+  (setq *robot* (samplerobot))
+  (write-info "Check Instances name ~A~%" (send *robot* :name))
+  (assert (equal (send *robot* :name) "SampleRobot"))
+  (write-info "Check joint size -> ~d~%" (length (send *robot* :joint-list)))
+  (assert (= (length (send *robot* :joint-list)) 29))
+  (mapcar #'(lambda (a b)
+              (write-info "~20A ~20A~%" (send a :name) (send b :name))
+              (assert (equal a b)))
+          (send *robot* :joint-list)
+          (list
+           (send *robot* :RLEG_HIP_R)
+           (send *robot* :RLEG_HIP_P)
+           (send *robot* :RLEG_HIP_Y)
+           (send *robot* :RLEG_KNEE)
+           (send *robot* :RLEG_ANKLE_P)
+           (send *robot* :RLEG_ANKLE_R)
+           (send *robot* :RARM_SHOULDER_P)
+           (send *robot* :RARM_SHOULDER_R)
+           (send *robot* :RARM_SHOULDER_Y)
+           (send *robot* :RARM_ELBOW)
+           (send *robot* :RARM_WRIST_Y)
+           (send *robot* :RARM_WRIST_P)
+           (send *robot* :RARM_WRIST_R)
+           (send *robot* :LLEG_HIP_R)
+           (send *robot* :LLEG_HIP_P)
+           (send *robot* :LLEG_HIP_Y)
+           (send *robot* :LLEG_KNEE)
+           (send *robot* :LLEG_ANKLE_P)
+           (send *robot* :LLEG_ANKLE_R)
+           (send *robot* :LARM_SHOULDER_P)
+           (send *robot* :LARM_SHOULDER_R)
+           (send *robot* :LARM_SHOULDER_Y)
+           (send *robot* :LARM_ELBOW)
+           (send *robot* :LARM_WRIST_Y)
+           (send *robot* :LARM_WRIST_P)
+           (send *robot* :LARM_WRIST_R)
+           (send *robot* :WAIST_P)
+           (send *robot* :WAIST_R)
+           (send *robot* :CHEST)
+           ))
+  (write-info "Done~%"))
+
+(run-all-tests)
+
+(exit)

euscollada/test/test-sample-robot.test

@@ -0,0 +1,3 @@
+<launch>
+  <test test-name="test_sample_robot" pkg="euscollada" type="test-sample-robot.l" />
+</launch>