add eusgazebo package

.travis.yml

@@ -1,3 +1,7 @@
+# Travis Continuous Integration Configuration File, based on ..
+#   Generic MoveIt Travis Continuous Integration Configuration File
+#   Works with all MoveIt! repositories/branches
+#   Author: Dave Coleman, Jonathan Bohren
 language:
   - cpp
   - python
@@ -6,34 +10,83 @@ python:
 compiler:
   - gcc
 env:
-  #- ROS_DISTRO=groovy ROSWS=rosws  BUILDER=rosbuild  USE_DEB=true
-  #- ROS_DISTRO=groovy ROSWS=rosws  BUILDER=rosbuild  USE_DEB=false
-  #- ROS_DISTRO=groovy ROSWS=wstool BUILDER=catkin    USE_DEB=true
-  #- ROS_DISTRO=groovy ROSWS=wstool BUILDER=catkin    USE_DEB=false
-  - ROS_DISTRO=hydro  ROSWS=wstool BUILDER=catkin    USE_DEB=true
+  # - ROS_DISTRO=groovy ROSWS=rosws  BUILDER=rosbuild  USE_DEB=true
+  # - ROS_DISTRO=groovy ROSWS=rosws  BUILDER=rosbuild  USE_DEB=false
+  # - ROS_DISTRO=groovy ROSWS=wstool BUILDER=catkin    USE_DEB=true
+  # - ROS_DISTRO=groovy ROSWS=wstool BUILDER=catkin    USE_DEB=false
+  # - ROS_DISTRO=hydro  ROSWS=wstool BUILDER=catkin    USE_DEB=true
   - ROS_DISTRO=hydro  ROSWS=wstool BUILDER=catkin    USE_DEB=false
-  #- ROS_DISTRO=indigo  ROSWS=wstool BUILDER=catkin   USE_DEB=true
-  #- ROS_DISTRO=indigo  ROSWS=wstool BUILDER=catkin   USE_DEB=false
-matrix:
-  allow_failures:
-    - env: ROS_DISTRO=indigo  ROSWS=wstool BUILDER=catkin   USE_DEB=true
-    - env: ROS_DISTRO=indigo  ROSWS=wstool BUILDER=catkin   USE_DEB=false
-before_install:
-  # add osrf
-  - sudo sh -c 'echo "deb http://packages.ros.org/ros-shadow-fixed/ubuntu `lsb_release -cs` main" > /etc/apt/sources.list.d/ros-latest.list'
-  - sudo sh -c 'echo "deb http://packages.osrfoundation.org/drc/ubuntu precise main" > /etc/apt/sources.list.d/drc-latest.list'
-  - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
-  - wget http://packages.osrfoundation.org/drc.key -O - | sudo apt-key add -
-  - if [ $USE_DEB == true ] ; then mkdir -p ~/ros/ws_rtmros_gazebo/src; fi
-  - if [ $USE_DEB == true ] ; then git clone https://github.com/start-jsk/rtmros_tutorials ~/ros/ws_rtmros_gazebo/src/rtmros_tutorials; fi
-install:
-  - sudo apt-get update -qq
-  - sudo apt-get install -qq drcsim-hydro
-  - sudo apt-get install -qq ros-hydro-openrtm-aist
-  - export ROS_PARALLEL_JOBS="-j4 -l4"
-script: source .travis/travis.sh
 notifications:
   email:
-    on_success: always
-    on_failure: always
-
+    recipients:
+      - [email protected]
+      - [email protected]
+      - [email protected]
+      - [email protected]
+    on_success: always #[always|never|change] # default: change
+    on_failure: always #[always|never|change] # default: always
+before_install: # Use this to prepare the system to install prerequisites or dependencies
+  # Define some config vars
+  - export CI_SOURCE_PATH=$(pwd)
+  - export REPOSITORY_NAME=${PWD##*/}
+  - echo "Testing branch $TRAVIS_BRANCH of $REPOSITORY_NAME"
+  - sudo sh -c 'echo "deb http://packages.ros.org/ros-shadow-fixed/ubuntu precise main" > /etc/apt/sources.list.d/ros-latest.list'
+  - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
+  - sudo sh -c 'echo "deb http://packages.osrfoundation.org/drc/ubuntu precise main" > /etc/apt/sources.list.d/drc-latest.list'
+  - wget http://packages.osrfoundation.org/drc.key -O - | sudo apt-key add -
+  - sudo apt-get update -qq > /dev/null
+  - sudo apt-get install -qq -y python-catkin-pkg python-rosdep python-wstool ros-$ROS_DISTRO-catkin > /dev/null
+  # MongoDB hack - I don't fully understand this but its for moveit_warehouse
+  - sudo apt-get remove -y mongodb mongodb-10gen > /dev/null
+  - sudo apt-get install -y mongodb-clients mongodb-server -o Dpkg::Options::="--force-confdef" > /dev/null # default actions
+  ##### quick hack for missing python-tk on hrpsys/waitInput.py
+  - sudo apt-get install -qq -y python-tk > /dev/null
+  - if [ $ROSWS == rosws ]; then sudo apt-get install -qq -y python-rosinstall > /dev/null; fi
+  - if [ $BUILDER == rosbuild ]; then sudo apt-get install -qq -y ros-$ROS_DISTRO-rosmake > /dev/null; fi
+  # Setup rosdep
+  - sudo rosdep init
+  - rosdep update; while [ $? != 0 ]; do sleep 1; rosdep update; done
+install: # Use this to install any prerequisites or dependencies necessary to run your build
+  # Create workspace
+  - mkdir -p ~/ros/ws_$REPOSITORY_NAME/src
+  - cd ~/ros/ws_$REPOSITORY_NAME/src
+  - if [ $USE_DEB == false -o $BUILDER == rosbuild ]; then $ROSWS init .   ; fi
+  - if [ $USE_DEB == false ]; then $ROSWS merge file://$CI_SOURCE_PATH/.rosinstall ; fi
+  - if [ $USE_DEB == false -o $BUILDER == rosbuild ]; then if [ $ROSWS == rosws ]; then $ROSWS merge /opt/ros/$ROS_DISTRO/.rosinstall; fi  ; fi
+  - if [ $USE_DEB == false ]; then sed -i "s@^\(.*github.com/$TRAVIS_REPO_SLUG.*\)@#\1@" .rosinstall               ; fi # comment out current repo
+  - if [ $USE_DEB == false ]; then $ROSWS update > /dev/null  ; fi
+  - if [ $USE_DEB == false -o $BUILDER == rosbuild ]; then $ROSWS set $REPOSITORY_NAME http://github.com/$TRAVIS_REPO_SLUG --git -y        ; fi
+  - ln -s $CI_SOURCE_PATH . # Link the repo we are testing to the new workspace
+  - cd ../
+  # Install dependencies for source repos
+  - find -L src -name package.xml -exec dirname {} \; | xargs -n 1 -i find {} -name manifest.xml | xargs -n 1 -i mv {} {}.deprecated # rename manifest.xml for rosdep install
+  - rosdep install -r -n --from-paths src --ignore-src --rosdistro $ROS_DISTRO -y > /dev/null
+  - find -L src -name manifest.xml.deprecated | xargs -n 1 -i dirname {} | xargs -n 1 -i ln -sf `pwd`/{}/manifest.xml.deprecated `pwd`/{}/manifest.xml # rename manifest.xml for rosdep install
+  - if [ $ROS_DISTRO == groovy ]; then sudo apt-get install -qq -y ros-$ROS_DISTRO-pr2-controllers > /dev/null; fi
+  - sudo apt-get install -qq -y drcsim-$ROS_DISTRO > /dev/null # install drcsim
+  # - find . \! -path "*/.*" -type f | xargs egrep -i "(hoge|fuga)" ; if [ $? == 0 ]; then exit 1; fi
+before_script: # Use this to prepare your build for testing e.g. copy database configurations, environment variables, etc.
+  - source /opt/ros/$ROS_DISTRO/setup.bash
+  - if [ $BUILDER == rosbuild ]; then source src/setup.bash        ; fi
+  - if [ $BUILDER == rosbuild ]; then rospack profile              ; fi
+script: # All commands must exit with code 0 on success. Anything else is considered failure.
+  # for catkin
+  - if [ $BUILDER == catkin ]; then catkin_make -j8 -l8            ; fi
+  - if [ $BUILDER == catkin ]; then export TARGET_PKG=`find build/$REPOSITORY_NAME -name Makefile -print |  sed s@.*/\\\\\([^\/]*\\\\\)/Makefile@\\\1@g` ; fi
+  - if [ $BUILDER == catkin ]; then source devel/setup.bash        ; fi
+  - if [ $BUILDER == catkin ]; then  find /home/travis/.ros/roseus/hydro            ; fi
+  - if [ $BUILDER == catkin ]; then rostest -t eusgazebo test-fall-arrow-object-simulation.test            ; fi
+  - if [ $BUILDER == catkin -a "$TARGET_PKG" != "" ]; then catkin_make test --pkg $TARGET_PKG -j8 -l8  ; fi
+  - if [ $BUILDER == catkin ]; then find build -name LastTest.log -exec echo "==== {} ====" \; -exec cat {} \;  ; fi
+  - if [ $BUILDER == catkin ]; then catkin_make -j8 -l8 install > /dev/null ; fi
+  - if [ $BUILDER == catkin ]; then rm -fr devel src build                  ; fi
+  - if [ $BUILDER == catkin ]; then source install/setup.bash               ; fi
+  - if [ $BUILDER == catkin -a "$TARGET_PKG" != "" ]; then export EXIT_STATUS=0; for pkg in $TARGET_PKG; do [ "`find install/share/$pkg -iname '*.test'`" == "" ] && echo "[$pkg] No tests ware found!!!"  || find install/share/$pkg -iname "*.test" -print0 | xargs -0 -n1 rostest || export EXIT_STATUS=$?; done; [ $EXIT_STATUS == 0 ] ; fi
+  # for rosbuild
+  - if [ $BUILDER == rosbuild ]; then rosmake -a --profile --pjobs=8       ; fi
+  - if [ $BUILDER == rosbuild ]; then export TARGET_PKG=`find -L src | grep $REPOSITORY_NAME | grep /build/Makefile$ | sed s@.*/\\\\\([^\/]*\\\\\)/build/Makefile@\\\1@g` ; fi
+  - if [ $BUILDER == rosbuild ]; then rosmake --test-only $TARGET_PKG --pjobs=8 ; fi
+after_failure:
+  - if [ $BUILDER == rosbuild ]; then find ${HOME}/.ros/rosmake/ -type f -exec echo "=== {} ===" \; -exec cat {} \; ; fi
+  - find ${HOME}/.ros/test_results -type f -exec echo "=== {} ===" \; -exec cat {} \;
+  - for file in ${HOME}/.ros/log/rostest-*; do echo "=== $file ==="; cat $file; done

eusgazebo/CMakeLists.txt

@@ -0,0 +1,10 @@
+if(NOT USE_ROSBUILD)
+  include(catkin.cmake)
+  return()
+endif()
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+rosbuild_init()
+
+rosbuild_add_rostest(test/test-fall-arrow-object-simulation.test)

eusgazebo/Makefile

@@ -0,0 +1,2 @@
+EXTRA_CMAKE_FLAGS = -DUSE_ROSBUILD:BOOL=1
+include $(shell rospack find mk)/cmake.mk

eusgazebo/README.md

@@ -0,0 +1,46 @@
+# EusGazebo
+
+Integration of Euslisp and Gazebo simulation.
+You can use Gazebo simulation more interactively from Euslisp interpreter.
+
+### Usage
+```lisp
+
+  ;; initialize ;;;;;;;;;;;;;;;;;;;;;;;;;;
+  ;;;; init eusgazebo
+  (setq *eusgazebo-server* (instance eusgazebo :init))
+
+  ;;;; generate eus model
+  (setq *obj* (generate-eus-model-function))
+
+  ;;;; add eus mdoel to eusgazebo
+  (send *eusgazebo-server* :add-model *obj*)
+
+
+  ;; simulate ;;;;;;;;;;;;;;;;;;;;;;;;;;
+  ;;;; set simulation setting
+  (send *obj* :newcoords (make-coords :pos #f(0 0 1000)))
+  (send *eusgazebo-server* :pause-sim)
+  (send *eusgazebo-server* :eus2gzb)
+
+  ;;;; start simulation
+  (send *eusgazebo-server* :unpause-sim)
+  (send *eusgazebo-server* :gzb2eus-loop)
+
+```
+
+### Sample
+```bash
+roscore &
+roscd eusgazebo/samples
+roseus fall-arrow-object-simulation.l "(fall-arrow-object-simulation)"
+roseus play-pinball-simulation.l "(progn (init-pinball-simulation) (play-pinball-simulation))"
+roseus play-domino-simulation.l "(progn (init-domino-simulation) (play-domino-simulation))"
+```
+
+### ROS Test
+```bash
+rostest eusgazebo test-fall-arrow-object-simulation.test
+rostest eusgazebo test-play-domino-simulation.test
+```
+

eusgazebo/catkin.cmake

@@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(eusgazebo)
+
+find_package(catkin REQUIRED COMPONENTS message_generation gazebo_msgs rostest)
+
+catkin_package(CATKIN_DEPENDS message_runtime gazebo_msgs)
+
+## Install ##
+install(DIRECTORY euslisp test scripts samples
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  USE_SOURCE_PERMISSIONS)
+
+## Testing ##
+add_rostest(test/test-fall-arrow-object-simulation.test)

eusgazebo/euslisp/eusgazebo-util.l

@@ -0,0 +1,103 @@
+;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+;; urdf & gazebo util
+
+;; convert model
+(defun irteus2urdf-for-gazebo
+  (model
+   &key
+   (name (send model :name))
+   (collada-dir "/tmp")
+   (urdf-dir nil)
+   (static nil))
+
+  (cond
+   ((not (subclassp (class model) cascaded-link))
+    (format t "invalid class, not childclass of cascaded-link~%"))
+   ((null (send model :links)) (format t "please setq links~%"))
+   (t
+    ;; convert to collada
+    (collada::eus2collada model collada-dir)
+    ;; convert to urdf
+    (let* ((collada-path (format nil "~a/~a.dae" collada-dir (send model :name))))
+      (cond
+       ((probe-file collada-path)
+	(format t "generate ~A.dae~%convert to urdf~%" name)
+	;; convert to urdf model
+	(unix:system
+	 (format nil
+		 "rosrun eusgazebo eus2urdf_for_gazebo.py ~a ~a ~a"
+		 name collada-path (if urdf-dir urdf-dir "")))
+	;; make static model
+	(unix:system
+	 (format nil
+		 "rosrun eusgazebo make_urdf_static_model_for_gazebo.py ~a ~a"
+		 name (if urdf-dir urdf-dir "")))
+	)
+       (t (format nil "dae error~%"))))
+    (format nil "~a/~a~a/~a" urdf-dir name (if static "_static" "") "model.urdf")
+    )))
+
+
+;; spawn model
+(defun spawn-model-to-gazebo
+  (urdf-model-path
+   &key
+   (model-name nil)
+   (model-coords (make-coords)))
+
+  (let* ((model-pos (scale 0.001 (send model-coords :pos)))
+	 (model-rpy (car (send model-coords :rpy-angle)))
+	 (x (elt model-pos 0))
+	 (y (elt model-pos 1))
+	 (z (elt model-pos 2))
+	 (roll (elt model-rpy 2))
+	 (pitch (elt model-rpy 1))
+	 (yaw (elt model-rpy 0)))
+    (unix:system (format nil "gzfactory spawn -f ~a -x ~a -y ~a -z ~a -R ~a -P ~a -Y ~a ~a"
+			 urdf-model-path x y z roll pitch yaw (if model-name (format nil "-m ~a" model-name) ""))))
+  )
+
+;; delete model
+(defun delete-model-to-gazebo
+  (model-name)
+
+  (unix:system (format nil "gzfactory delete -m ~a" model-name))
+  )
+
+
+;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+;; ros data conversion util
+
+(defun pose->coords
+  (msg)
+
+  (let* ((pos (scale 1000 (ros-xyz->eus-vec (send msg :position))))
+	 (quat (ros-xyzw->eus-vec (send msg :orientation)))
+	 (rot (quaternion2matrix quat)))
+    (make-coords :pos pos :rot rot)))
+
+(defun ros-xyz->eus-vec
+  (msg)
+
+  (float-vector (send msg :x) (send msg :y) (send msg :z)))
+
+(defun ros-xyzw->eus-vec
+  (msg)
+
+  (float-vector (send msg :w) (send msg :x) (send msg :y) (send msg :z)))
+
+(defun coords->pose
+  (coords)
+
+  (let* ((pose (instance geometry_msgs::Pose :init))
+	 (pos (scale 0.001 (send coords :worldpos)))
+	 (rot (send coords :worldrot))
+	 (quat (matrix2quaternion rot)))
+    (send pose :position :x (elt pos 0))
+    (send pose :position :y (elt pos 1))
+    (send pose :position :z (elt pos 2))
+    (send pose :orientation :x (elt quat 1))
+    (send pose :orientation :y (elt quat 2))
+    (send pose :orientation :z (elt quat 3))
+    (send pose :orientation :w (elt quat 0))
+    pose))