Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

add eusgazebo package #41

Closed
wants to merge 43 commits into from
Closed
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
43 commits
Select commit Hold shift + click to select a range
90965cc
added eusgazebo
mmurooka Mar 28, 2014
7cc813a
added eusgazebo/euslisp/eusgazebo-core.l
mmurooka Mar 28, 2014
c11a5cc
implemtented core part of eusgazebo. you can execute test program by …
mmurooka Mar 29, 2014
5580890
added jsk_model_tools to depand packages
mmurooka Mar 29, 2014
074fbb9
added scripts/eus2urdf_for_gazebo.py
mmurooka Mar 30, 2014
9feb9ab
add author, remove comments
mmurooka Mar 30, 2014
3a890ff
added manifest.xml
mmurooka Mar 30, 2014
64b069c
added Makefile, catkin.cake, remove comment lines, add install and test
mmurooka Mar 30, 2014
1571312
added sample and test files
mmurooka Mar 30, 2014
e30aba4
fixed test
mmurooka Mar 30, 2014
cd48d46
added rosdep for gazebo_ros to manifest.xml
mmurooka Mar 30, 2014
126a365
fixed test/test-fall-arrow-object-simulation.test
mmurooka Mar 30, 2014
df0b064
enable test in Makefile
mmurooka Mar 30, 2014
660a017
added build dependencies in package.xml
mmurooka Mar 30, 2014
a306367
deleted incorrect line in package.xml
mmurooka Apr 6, 2014
1295285
added mass-property-util.l
mmurooka Apr 7, 2014
7257eaa
added domino-cube-object.l
mmurooka Apr 7, 2014
5a1d223
overwrite mass property with proper value and deal with the eus model…
mmurooka Apr 7, 2014
4c83e39
add samples/play-domino-simulation.l
mmurooka Apr 7, 2014
ec95b9f
set valid CoG, generate static model.
mmurooka Apr 7, 2014
7dd068c
added test program for play domino
mmurooka Apr 7, 2014
6b9dd1f
added pinball sample
mmurooka Apr 16, 2014
57f4ca2
change repository of eusgazebo
mmurooka Apr 17, 2014
799ea3a
Merge remote-tracking branch 'export_project/eusgazebo-devel' into eu…
mmurooka Apr 17, 2014
34ea50a
added README.md for instruction
mmurooka Apr 17, 2014
1476d60
added comment to README.md
mmurooka Apr 17, 2014
3c67d82
Merge branch 'master', remote-tracking branch 'origin' into eusgazebo…
mmurooka Apr 18, 2014
a74c540
fix bug in overwrite-mass-property-if-too-small
mmurooka Apr 29, 2014
a81f8c0
for travis restarting
mmurooka Apr 29, 2014
1a69f99
add play-top-samples
May 5, 2014
2c40349
Merge pull request #2 from YuOhara/eusgazebo-devel
mmurooka May 7, 2014
fd7b495
enable to set velocity for gazebo object
mmurooka May 7, 2014
3296cc8
modify samples for last commit
mmurooka May 7, 2014
aacd8f1
Merge remote-tracking branch 'origin/master' into eusgazebo-devel
mmurooka May 8, 2014
d969721
added dependency to gazebo_msgs
mmurooka May 8, 2014
1d4f641
use collada_to_urdf of collada_urdf_jsk_patch
mmurooka May 8, 2014
210733a
add macro for generating message
mmurooka May 9, 2014
4770a6e
move simple-ball-object to euslisp/eus/models and load that.
mmurooka May 9, 2014
98416d3
added rostest line. this is for temporary debug.
mmurooka May 9, 2014
4805edd
fixed last commit
mmurooka May 10, 2014
90b4ff4
do ls for gazebo_msgs message
mmurooka May 11, 2014
8705d66
find ~/.ros/roseus/hydro for debug
mmurooka May 11, 2014
333509d
merge origin/master
mmurooka Jul 3, 2014
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
109 changes: 81 additions & 28 deletions .travis.yml
Original file line number Diff line number Diff line change
@@ -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
Expand All @@ -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
10 changes: 10 additions & 0 deletions eusgazebo/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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)
2 changes: 2 additions & 0 deletions eusgazebo/Makefile
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
EXTRA_CMAKE_FLAGS = -DUSE_ROSBUILD:BOOL=1
include $(shell rospack find mk)/cmake.mk
46 changes: 46 additions & 0 deletions eusgazebo/README.md
Original file line number Diff line number Diff line change
@@ -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
```

14 changes: 14 additions & 0 deletions eusgazebo/catkin.cmake
Original file line number Diff line number Diff line change
@@ -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)
103 changes: 103 additions & 0 deletions eusgazebo/euslisp/eusgazebo-util.l
Original file line number Diff line number Diff line change
@@ -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))
Loading