Working version with new CRAM

README.md

@@ -0,0 +1,4 @@
+# cram_gazebo
+CRAM packages for interfacing with gazebo, including gazebo fake perception.
+
+This version works with the newest CRAM version v0.7.0

cram_gazebo/CMakeLists.txt

@@ -1,4 +1,4 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(cram_gazebo)
-
-catkin_package()
+find_package(catkin REQUIRED)
+catkin_metapackage()

cram_gazebo/package.xml

@@ -2,20 +2,22 @@
   <name>cram_gazebo</name>
   <version>1.0.0</version>
   <description>
-
     Metapackage holding the Gazebo interface for CRAM.
-
   </description>
-  
+
   <maintainer email="winkler@cs.uni-bremen.de">Jan Winkler</maintainer>
   <author email="winkler@cs.uni-bremen.de">Jan Winkler</author>
-  
+
   <license>BSD</license>
-  
+
   <url>http://ros.org/wiki/cram_gazebo</url>
 
   <buildtool_depend>catkin</buildtool_depend>
 
   <run_depend>cram_gazebo_utilities</run_depend>
   <run_depend>gazebo_perception_process_module</run_depend>
+
+  <export>
+    <metapackage/>
+  </export>
 </package>

cram_gazebo_utilities/CMakeLists.txt

@@ -1,5 +1,13 @@
 cmake_minimum_required(VERSION 2.4.6)
-include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+
+project(cram_gazebo_utilities)
+find_package(catkin REQUIRED)
+catkin_package()
+
+
+
+#include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
 
 # Set the build type.  Options are:
 #  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
@@ -9,12 +17,12 @@ include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
 #  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
 #set(ROS_BUILD_TYPE RelWithDebInfo)
 
-rosbuild_init()
+#rosbuild_init()
 
 #set the default path for built executables to the "bin" directory
-set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
 #set the default path for built libraries to the "lib" directory
-set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+#set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
 
 #uncomment if you have defined messages
 #rosbuild_genmsg()

cram_gazebo_utilities/cram-gazebo-utilities.asd

@@ -30,27 +30,29 @@
   :license "BSD"
   :description "CRAM gazebo utilities"
 
-  :depends-on (cram-roslisp-common
+  :depends-on (roslisp-utilities
                cram-language
-               cram-reasoning
-               process-modules
+	       cl-transforms-stamped
+               cram-prolog
+               cram-process-modules
                cram-utilities
-               cram-plan-knowledge
-               designators
-               designators-ros
+               ;cram-plan-knowledge
+               cram-designators
+               ;designators-ros
                actionlib
-               semantic-map-cache
+               ;semantic-map-cache
                ;; vision_msgs-msg
                ;; vision_srvs-srv
                std_msgs-msg
                pr2_msgs-msg
 	       trajectory_msgs-msg
-               cram-plan-failures
+               cram-common-failures
                gazebo_msgs-msg
                gazebo_msgs-srv
-               cl-semantic-map-utils
+               cram-semantic-map-utils
                cram-plan-library
-	       attache_msgs-srv)
+	       attache_msgs-srv
+	       )
   :components
   ((:module "src"
     :components

cram_gazebo_utilities/src/package.lisp

@@ -27,24 +27,23 @@
 
 (in-package :cl-user)
 
-(desig-props:def-desig-package
- cram-gazebo-utilities
- (:documentation "CRAM gazebo utilities")
- (:nicknames :cram-gu)
- (:use
-  #:common-lisp
-  #:cram-roslisp-common
-  #:cram-reasoning
-  #:cram-process-modules
-  #:cram-plan-library
-  #:crs
-  #:cut
-  #:desig
-  #:designators-ros
-  #:roslisp
-  #:cram-plan-failures
-  #:semantic-map-cache)
+(desig-props:def-desig-package cram-gazebo-utilities
+  (:documentation "CRAM Gazebo Utilities")
+  (:nicknames :cram-gu)
+  (:use
+   #:common-lisp
+   #:cram-prolog
+   #:cram-process-modules
+   #:cram-plan-library
+   #:cut
+   #:desig
+   #:roslisp
+   #:cram-common-failures)
   (:export init-cram-gazebo-utilities
            get-model-pose set-model-state
            spawn-gazebo-model object-in-world?
-           get-models gazebo-present))
+           get-models gazebo-present
+           spawned-object-description
+           spawned-objects
+           delete-spawned-objects
+           model-present))

cram_gazebo_utilities/src/ros.lisp

@@ -53,9 +53,9 @@ about the current state of all models in the simulated world."
         msg
       (map 'nil (lambda (model-name model-pose-msg)
                   (setf (gethash model-name *known-models*)
-                        (tf:pose->pose-stamped
+                        (cl-transforms-stamped:pose->pose-stamped
                          "map" 0.0
-                         (tf:msg->pose model-pose-msg))))
+                         (cl-transforms-stamped:from-msg model-pose-msg))))
            names poses))))
 
 (defun get-model-pose (name)

cram_gazebo_utilities/src/utils.lisp

@@ -9,9 +9,6 @@
 ;;;     * Redistributions in binary form must reproduce the above copyright
 ;;;       notice, this list of conditions and the following disclaimer in the
 ;;;       documentation and/or other materials provided with the distribution.
-;;;     * Neither the name of Willow Garage, Inc. nor the names of its
-;;;       contributors may be used to endorse or promote products derived from
-;;;       this software without specific prior written permission.
 ;;; 
 ;;; THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 ;;; AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
@@ -27,26 +24,93 @@
 
 (in-package :cram-gazebo-utilities)
 
+
+;;;
+;;; Class definitions
+;;;
+
+(defclass spawned-object ()
+  ((id :reader id :initarg :id)
+   (description :reader description :initarg :description)))
+
+
+;;;
+;;; Global variables
+;;;
+
+(defvar *spawned-objects* (make-hash-table :test 'equal))
+
+
+;;;
+;;; Functions: Spawned object knowledge
+;;;
+
+(defun clear-spawned-object-knowledge ()
+  (setf *spawned-objects* (make-hash-table :test 'equal)))
+
+(defun register-spawned-object (object-id description)
+  (setf (gethash object-id *spawned-objects*)
+        (make-instance 'spawned-object
+                       :id object-id :description description)))
+
+(defun unregister-spawned-object (object-id)
+  (remhash object-id *spawned-objects*))
+
+(defun spawned-object-description (object-id)
+  (let ((spawned-object (gethash object-id *spawned-objects*)))
+    (when spawned-object
+      (description spawned-object))))
+
+(defun spawned-objects ()
+  (loop for name being the hash-keys of *spawned-objects*
+        collect name))
+
+(defun delete-spawned-objects ()
+  (dolist (object-id (spawned-objects))
+    (delete-gazebo-model object-id))
+  (clear-spawned-object-knowledge))
+
+
+;;;
+;;; Functions: Model state, spawning, deleting
+;;;
+
 (defun set-model-state (model-name new-pose)
   (call-service "gazebo/set_model_state"
                 'gazebo_msgs-srv:setmodelstate
-                :model_state (make-msg "gazebo_msgs/ModelState"
-                                       :model_name model-name
-                                       :pose (tf:pose->msg new-pose)
-                                       :reference_frame (tf:frame-id new-pose))))
+                :model_state
+                (make-msg "gazebo_msgs/ModelState"
+                          :model_name model-name
+                          :pose (cl-transforms-stamped:to-msg new-pose)
+                          :reference_frame (cl-transforms-stamped:frame-id new-pose))))
 
-(defun spawn-gazebo-model (name pose urdf-file)
+(defun spawn-gazebo-model (name pose urdf-file &key description)
   (call-service "gazebo/spawn_urdf_model"
                 'gazebo_msgs-srv:spawnmodel
                 :model_name name
                 :model_xml (file-string urdf-file)
-                :initial_pose (tf:pose->msg pose)
-                :reference_frame (tf:frame-id pose)))
+                :initial_pose (cl-transforms-stamped:to-msg
+                               (cl-tf:pose-stamped->pose pose))
+                :reference_frame (cl-transforms-stamped:frame-id pose))
+  (register-spawned-object name description))
 
 (defun delete-gazebo-model (name)
   (call-service "gazebo/delete_model"
                 'gazebo_msgs-srv:deletemodel
-                :model_name name))
+                :model_name name)
+  (unregister-spawned-object name))
+
+(defun model-present (name)
+  (with-fields (success)
+      (call-service "gazebo/get_model_state"
+                    'gazebo_msgs-srv:getmodelstate
+                    :model_name name
+                    :relative_entity_name "")
+    success))
+
+;;;
+;;; Functions: Utility
+;;;
 
 (defun file-string (path)
   (with-open-file (s path)
@@ -59,6 +123,11 @@
 (defun gazebo-present ()
   (roslisp:wait-for-service "gazebo/spawn_urdf_model" 0.25))
 
+
+;;;
+;;; Functions: Joint manipulation
+;;;
+
 (defun set-joint-effort (joint effort &key (duration 2.0))
   (call-service "gazebo/apply_joint_effort"
                 'gazebo_msgs-srv:applyjointeffort
@@ -76,21 +145,22 @@
                  "gazebo_msgs/ODEJointProperties"
                  :damping (vector damping))))
 
-(defun set-joint-position (model joint position &optional hold)
-  (call-service "gazebo/joint_control"
-                'attache_msgs-srv:jointcontrol
-                :model model
-                :joint joint
-                :position position
-                :hold_position hold))
-
-(defun get-joint-information (model joint)
-  (with-fields (success position min max)
-      (call-service "gazebo/joint_information"
-                    'attache_msgs-srv:jointinformation
-                    :model model
-                    :joint joint)
-    (values success position min max)))
+;;Not sure if this is needed
+;;(defun set-joint-position (model joint position &optional hold)
+;;  (call-service "gazebo/joint_control"
+;;                'attache_msgs-srv:JointControl
+;;                :model model
+;;                :joint joint
+;;                :position position
+;;                :hold_position hold))
+
+;;(defun get-joint-information (model joint)
+;;  (with-fields (success position min max)
+;;      (call-service "gazebo/joint_information"
+;;                    'attache_msgs-srv:jointinformation
+;;                    :model model
+;;                    :joint joint)
+;;    (values success position min max)))
 
 (defun open-joint (model joint &optional hold)
   (multiple-value-bind (success position min max)

gazebo_perception_process_module/gazebo-perception-process-module.asd

@@ -31,29 +31,27 @@
   :license "BSD"
   :description "Gazebo perception process module"
 
-  :depends-on (cram-roslisp-common
+  :depends-on (roslisp-utilities
                cram-language
-               cram-reasoning
-               process-modules
+               cram-prolog
+               cram-process-modules
                cram-utilities
-               cram-plan-knowledge
-               designators
-               designators-ros
-               cram-plan-failures
+               cram-designators
+               cram-common-failures
                cram-projection
-               cram-manipulation-knowledge
-               physics-utils
+               cram-physics-utils
                cl-urdf
-               bullet-reasoning
-               cram-task-knowledge
-               cram-gazebo-utilities)
+               cram-gazebo-utilities
+               gazebo_visibility_ros-srv)
   :components
   ((:module "src"
     :components
     ((:file "package")
      (:file "designator" :depends-on ("package"))
+     (:file "utilities" :depends-on ("package"))
      (:file "gazebo-designator-data" :depends-on ("package"))
      (:file "process-module"
       :depends-on ("package"
                    "designator"
-                   "gazebo-designator-data"))))))
+                   "gazebo-designator-data"
+                   "utilities"))))))