From 87e1fe5f8c79fd9daec2fa042898800c56878256 Mon Sep 17 00:00:00 2001
From: Kentaro Wada <www.kentaro.wada@gmail.com>
Date: Mon, 18 Apr 2016 08:24:51 +0900
Subject: [PATCH] Test predefined pose methods in generated pr2 model

Modified:
  - euscollada/CMakeLists.txt
  - euscollada/package.xml
Added:
  - euscollada/.gitignore
  - euscollada/test/test_pr2.sh
---
 euscollada/.gitignore       |  3 +++
 euscollada/CMakeLists.txt   | 44 ++++++++++++++++++++++++++++++++++++-
 euscollada/package.xml      |  5 +++--
 euscollada/test/test_pr2.sh | 12 ++++++++++
 4 files changed, 61 insertions(+), 3 deletions(-)
 create mode 100644 euscollada/.gitignore
 create mode 100755 euscollada/test/test_pr2.sh

diff --git a/euscollada/.gitignore b/euscollada/.gitignore
new file mode 100644
index 000000000..2fdfbf392
--- /dev/null
+++ b/euscollada/.gitignore
@@ -0,0 +1,3 @@
+pr2.dae
+pr2.l
+pr2.urdf
diff --git a/euscollada/CMakeLists.txt b/euscollada/CMakeLists.txt
index c614dc159..021aa37bf 100644
--- a/euscollada/CMakeLists.txt
+++ b/euscollada/CMakeLists.txt
@@ -8,7 +8,7 @@ execute_process(
   RESULT_VARIABLE rosversion_ge_indigo)
 
 IF(${rosversion_ge_indigo})
-find_package(catkin REQUIRED COMPONENTS collada_urdf rospack collada_parser resource_retriever)
+find_package(catkin REQUIRED COMPONENTS collada_urdf rospack collada_parser resource_retriever pr2_description)
 ELSE()
 find_package(catkin REQUIRED COMPONENTS collada_urdf rospack collada_parser urdfdom resource_retriever)
 ENDIF()
@@ -55,3 +55,45 @@ install(DIRECTORY src scripts
 file(GLOB _install_files RELATIVE ${PROJECT_SOURCE_DIR} *.yaml *.sh)
 install(FILES ${_install_files}
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+
+# --------------
+# Generate pr2.l
+# --------------
+if(EXISTS ${pr2_description_PREFIX}/share/pr2_description/robots/pr2.urdf.xacro)
+  set(pr2_xacro ${pr2_description_PREFIX}/share/pr2_description/robots/pr2.urdf.xacro)
+elseif(EXISTS ${pr2_mechanism_model_SOURCE_PREFIX}/robots/pr2.urdf)
+  set(pr2_xacro ${pr2_description_SOURCE_PREFIX}/robots/pr2.urdf.xacro)
+else()
+  message(WARNING "Could not found pr2.urdf.xacro in ${pr2_description_PREFIX}/share/pr2_description/robots/pr2.urdf.xacro and ${pr2_description_SOURCE_PREFIX}/robots/pr2.urdf.xacro")
+endif()
+if (EXISTS ${pr2_xacro})
+  message(STATUS "Found pr2.urdf.xacro at ${pr2_xacro}")
+  add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/pr2.urdf
+    COMMAND rosrun xacro xacro ${pr2_xacro} > pr2.urdf
+    WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}
+    DEPENDS ${pr2_xacro})
+  add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/pr2.dae
+    COMMAND rosrun collada_urdf urdf_to_collada pr2.urdf pr2.dae
+    WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}
+    DEPENDS pr2.urdf)
+  add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/pr2.l
+    COMMAND rosrun euscollada collada2eus pr2.dae pr2.yaml pr2.l
+    WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}
+    DEPENDS pr2.dae pr2.yaml)
+  # get collada-dom version
+  find_package(PkgConfig)
+  pkg_check_modules(COLLADA collada-dom>=2.4.4 )
+  if ( ${COLLADA_FOUND} )
+    add_custom_target(generate_pr2_lisp ALL DEPENDS ${PROJECT_SOURCE_DIR}/pr2.l)
+  else()
+    pkg_check_modules(COLLADA collada-dom)
+    message(WARNING "urdf_to_collada requries collada-dom >= 2.4.4, installed version is ${COLLADA_VERSION}")
+  endif()
+endif()
+
+
+if(CATKIN_ENABLE_TESTING)
+  find_package(jsk_tools)
+  jsk_tools_add_shell_test(COMMAND "rosrun ${PROJECT_NAME} test_pr2.sh" DEPENDENCIES generate_pr2_lisp)
+endif()
diff --git a/euscollada/package.xml b/euscollada/package.xml
index 5eeb61743..2eb406872 100644
--- a/euscollada/package.xml
+++ b/euscollada/package.xml
@@ -33,6 +33,7 @@
   <build_depend>tf</build_depend>
   <build_depend>mk</build_depend>
   <build_depend>collada_urdf</build_depend>
+  <build_depend>xacro</build_depend>
 
   <run_depend>roscpp</run_depend>
   <run_depend>rospack</run_depend>
@@ -48,10 +49,10 @@
   <run_depend>tf</run_depend>
   <run_depend>collada_urdf</run_depend>
 
-  <!-- <test_depend>collada_urdf</test_depend> -->
+  <test_depend>jsk_tools</test_depend>
+  <test_depend>roseus</test_depend>
   <!-- <test_depend>euslisp</test_depend> -->
   <!-- <test_depend>roseus</test_depend> -->
-  <!-- <test_depend>jsk_tools</test_depend> -->
 
   <export>
     <rosdoc config="rosdoc.yaml"/>
diff --git a/euscollada/test/test_pr2.sh b/euscollada/test/test_pr2.sh
new file mode 100755
index 000000000..260e7ec0e
--- /dev/null
+++ b/euscollada/test/test_pr2.sh
@@ -0,0 +1,12 @@
+#!/bin/sh
+
+rosrun roseus roseus lib/llib/unittest.l "(init-unit-test)" "\
+(progn
+  (load \"package://euscollada/pr2.l\")
+  (pr2)
+  (send *pr2* :reset-pose)
+  (send *pr2* :reset-pose :larm)
+  (send *pr2* :reset-pose :rarm)
+  (exit)
+  )
+"