diff --git a/README.md b/README.md new file mode 100644 index 0000000..c467bfd --- /dev/null +++ b/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 diff --git a/cram_gazebo/CMakeLists.txt b/cram_gazebo/CMakeLists.txt index 095433d..b02b909 100644 --- a/cram_gazebo/CMakeLists.txt +++ b/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() diff --git a/cram_gazebo/package.xml b/cram_gazebo/package.xml index 6019b62..dc559cd 100644 --- a/cram_gazebo/package.xml +++ b/cram_gazebo/package.xml @@ -2,20 +2,22 @@ cram_gazebo 1.0.0 - Metapackage holding the Gazebo interface for CRAM. - - + Jan Winkler Jan Winkler - + BSD - + http://ros.org/wiki/cram_gazebo catkin cram_gazebo_utilities gazebo_perception_process_module + + + + diff --git a/cram_gazebo_utilities/CMakeLists.txt b/cram_gazebo_utilities/CMakeLists.txt index f8f1c9c..8928352 100644 --- a/cram_gazebo_utilities/CMakeLists.txt +++ b/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() diff --git a/cram_gazebo_utilities/cram-gazebo-utilities.asd b/cram_gazebo_utilities/cram-gazebo-utilities.asd index 8ac17e1..fcb43c0 100644 --- a/cram_gazebo_utilities/cram-gazebo-utilities.asd +++ b/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 diff --git a/cram_gazebo_utilities/src/package.lisp b/cram_gazebo_utilities/src/package.lisp index 6fc8e7a..2dfef07 100644 --- a/cram_gazebo_utilities/src/package.lisp +++ b/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)) diff --git a/cram_gazebo_utilities/src/ros.lisp b/cram_gazebo_utilities/src/ros.lisp index f5feb53..7daeeae 100644 --- a/cram_gazebo_utilities/src/ros.lisp +++ b/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) diff --git a/cram_gazebo_utilities/src/utils.lisp b/cram_gazebo_utilities/src/utils.lisp index 889b6df..ed3588d 100644 --- a/cram_gazebo_utilities/src/utils.lisp +++ b/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) diff --git a/gazebo_perception_process_module/gazebo-perception-process-module.asd b/gazebo_perception_process_module/gazebo-perception-process-module.asd index d7a2636..f909a84 100644 --- a/gazebo_perception_process_module/gazebo-perception-process-module.asd +++ b/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")))))) diff --git a/gazebo_perception_process_module/package.xml b/gazebo_perception_process_module/package.xml index 2cbc93f..363d600 100644 --- a/gazebo_perception_process_module/package.xml +++ b/gazebo_perception_process_module/package.xml @@ -1,4 +1,4 @@ - + gazebo_perception_process_module 1.0.0 @@ -15,36 +15,17 @@ catkin - - - cram_roslisp_common - cram_process_modules - cram_reasoning - cram_plan_knowledge - cram_designators - designators_ros - cram_utilities - cram_manipulation_knowledge - cram_plan_failures - gazebo_msgs - cram_projection - cram_physics_utils - cl_urdf - bullet_reasoning - - cram_roslisp_common - cram_process_modules - cram_reasoning - cram_plan_knowledge - cram_designators - designators_ros - cram_utilities - cram_manipulation_knowledge - cram_plan_failures - gazebo_msgs - cram_projection - cram_physics_utils - cl_urdf - bullet_reasoning + roslisp_utilities + cram_language + cram_prolog + cram_process_modules + cram_utilities + cram_designators + cram_plan_occasions_events + cram_projection + cram_physics_utils + cl_urdf + cram_gazebo_utilities + gazebo_visibility_ros diff --git a/gazebo_perception_process_module/src/designator.lisp b/gazebo_perception_process_module/src/designator.lisp index 6e5c926..894f2fb 100644 --- a/gazebo_perception_process_module/src/designator.lisp +++ b/gazebo_perception_process_module/src/designator.lisp @@ -29,15 +29,15 @@ (in-package :gazebo-perception-pm) (defun designator-model-pose (name) - (let ((found-objects (find-object :object-name name))) + (let ((found-objects (find-objects :object-name name))) (when found-objects (object-pose (first found-objects))))) (def-fact-group process-module (matching-process-module available-process-module) (<- (matching-process-module ?designator gazebo-perception-process-module) - (desig-prop ?designator (to perceive)) - (desig-prop ?designator (obj ?object)) + (desig-prop ?designator (:to :perceive)) + (desig-prop ?designator (:obj ?object)) (obj-desig? ?object)) (<- (available-process-module gazebo-perception-process-module) @@ -46,14 +46,14 @@ (def-fact-group perception-action-designator (action-desig) (<- (action-desig ?desig ?object) - (desig-prop ?desig (to perceive)) - (desig-prop ?desig (obj ?object)))) + (desig-prop ?desig (:to :perceive)) + (desig-prop ?desig (:obj ?object)))) (def-fact-group gazebo-object-locations (desig-solution) (<- (desig-solution ?designator ?solution) - (desig-prop ?designator (of ?object)) + (desig-prop ?designator (:of ?object)) (obj-desig? ?object) - (desig-prop ?object (name ?name)) + (desig-prop ?object (:name ?name)) (lisp-fun designator-model-pose ?name ?solution) (lisp-pred identity ?solution))) diff --git a/gazebo_perception_process_module/src/gazebo-designator-data.lisp b/gazebo_perception_process_module/src/gazebo-designator-data.lisp index 9b31dd0..c0f1c76 100644 --- a/gazebo_perception_process_module/src/gazebo-designator-data.lisp +++ b/gazebo_perception_process_module/src/gazebo-designator-data.lisp @@ -35,11 +35,11 @@ ((type :initarg :type :reader object-type))) (defclass gazebo-designator-mesh-data - (gazebo-designator-data cram-manipulation-knowledge:object-mesh-data-mixin) + (gazebo-designator-data cram-physics-utils:object-mesh-data-mixin) ()) (defclass gazebo-designator-shape-data - (gazebo-designator-data cram-manipulation-knowledge:object-shape-data-mixin) + (gazebo-designator-data cram-physics-utils:object-shape-data-mixin) ()) (defun get-object-geometry (urdf &key (mesh-identifier :visual)) @@ -89,6 +89,19 @@ element (:VISUAL) or the collision element (:COLLISION)." (physics-utils:face-points face))) faces)))) +(defun urdf-mesh->3d-model (mesh) + (let* ((filename (cl-urdf:filename mesh)) + (scale (cl-urdf:scale mesh)) + (size (cl-urdf:size mesh)) + (3d-model (physics-utils:load-3d-model filename)) + (3d-model (if size + (physics-utils:resize-3d-model 3d-model size) + 3d-model)) + (3d-model (if (and scale (not (equal scale 1)) (not (equal scale 1.0)) (not (equal scale (cl-transforms:make-3d-vector 1.0 1.0 1.0)))) + (physics-utils:scale-3d-model 3d-model scale) + 3d-model))) + 3d-model)) + (defgeneric geometry->designator-data (name pose type geometry &optional geometry-pose) (:documentation "Returns an instance of a subclass of @@ -97,7 +110,8 @@ element (:VISUAL) or the collision element (:COLLISION)." (:method (name pose type (mesh cl-urdf:mesh) &optional (geometry-pose (cl-transforms:make-identity-pose))) (let ((mesh (physics-utils:transform-3d-model - (physics-utils:3d-model mesh) geometry-pose))) +;; TODO: apply scale and/or size to the loaded model + (urdf-mesh->3d-model mesh) geometry-pose))) (make-instance 'gazebo-designator-mesh-data :object-identifier name :type type diff --git a/gazebo_perception_process_module/src/package.lisp b/gazebo_perception_process_module/src/package.lisp index b639392..efbbf79 100644 --- a/gazebo_perception_process_module/src/package.lisp +++ b/gazebo_perception_process_module/src/package.lisp @@ -30,15 +30,14 @@ (:nicknames :gazebo-perception-pm) (:use #:common-lisp - #:cram-roslisp-common - #:cram-reasoning + #:cram-prolog #:cram-process-modules #:cram-designators #:cut - #:designators-ros #:roslisp - #:cram-plan-failures) - (:export #:gazebo-perception-process-module) + #:cram-common-failures) + (:export #:gazebo-perception-process-module + ignore-object) (:shadowing-import-from #:cpl fail) (:import-from #:cram-process-modules matching-process-module available-process-module) diff --git a/gazebo_perception_process_module/src/process-module.lisp b/gazebo_perception_process_module/src/process-module.lisp index e54fd60..556ee97 100644 --- a/gazebo_perception_process_module/src/process-module.lisp +++ b/gazebo_perception_process_module/src/process-module.lisp @@ -27,6 +27,37 @@ (in-package :gazebo-perception-process-module) +; Generate logs - copied from the robosherlock perception module... +(cut:define-hook cram-language::on-prepare-perception-request (designator-request)) +(cut:define-hook cram-language::on-finish-perception-request (log-id designators-result)) + +; The following stuff is copied from cram-task-knowledge since it is not yet available in cram2... +(define-hook objects-perceived (object-template object-designators)) + +(defparameter *tf-listener* nil) +(defun ensure-tf-listener () + (unless *tf-listener* + (progn + (setf *tf-listener* (make-instance 'cl-tf:transform-listener)) + (roslisp:wait-duration 1.0))) + *tf-listener*) + +(defun destroy-tf-listener () + (setf *tf-listener* nil)) + +(roslisp-utilities:register-ros-cleanup-function destroy-tf-listener) + + +(defgeneric filter-perceived-objects (object-template perceived-objects) + (:documentation "Filters all perceived objects according to all registered filters. This method is mainly used by perception process modules that want to validate and filter their results. Also, this function triggers the `object-perceived-event' plan event, updating the belief state.") + (:method (object-template perceived-objects) + (let* ((filtered-objects + (loop for filter-result in (objects-perceived + object-template perceived-objects) + append filter-result))) + filtered-objects))) + +; Start of the original module (defmethod designator-pose ((designator object-designator)) (object-pose (reference designator))) @@ -41,15 +72,15 @@ properties of `perceived-object'.") (:method ((old-desig object-designator) (perceived-object object-designator-data)) (let ((obj-loc-desig (make-designator - 'location - `((pose ,(object-pose perceived-object))))) - (object-name (or (when (desig-prop-value old-desig 'desig-props:name) - (desig-prop-value old-desig 'desig-props:name)) + :location + `((:pose ,(object-pose perceived-object))))) + (object-name (or (when (desig-prop-value old-desig :name) + (desig-prop-value old-desig :name)) (object-identifier perceived-object)))) - `((desig-props:at ,obj-loc-desig) - (desig-props:name ,object-name) + `((:at ,obj-loc-desig) + (:name ,object-name) ,@(remove-if (lambda (element) - (member element '(at type name))) + (member element '(:at type :name))) (description old-desig) :key #'car))))) (defun make-handle-designator-sequence (handles) @@ -61,89 +92,183 @@ purposes." (mapcar (lambda (handle-desc) (destructuring-bind (pose radius) handle-desc `(handle - ,(make-designator 'object - `((at ,(make-designator - 'location `((pose ,pose)))) - (radius ,radius) - (type handle)))))) + ,(make-designator :object + `((:at ,(make-designator + :location `((:pose ,pose)))) + (:radius ,radius) + (:type handle)))))) handles)) -(defun find-object (&key object-name object-type) +(defun filter-models-by-ignored-objects (model-names) + (cpl:mapcar-clean (lambda (model-name) + (unless (find model-name *ignored-objects* :test #'string=) + model-name)) + model-names)) + +(defun filter-models-by-name (model-names &key template-name) + "If defined, `template-name' is the only valid model name returned (if present in `model-names'). Otherwise, `model-names' is returned." + (cond (template-name + (cpl:mapcar-clean (lambda (model-name) + (when (string= template-name model-name) + model-name)) + model-names)) + (t model-names))) + +(defun filter-models-by-field-of-view (model-names) + (let* ((camera-pose (cl-transforms-stamped:lookup-transform + (ensure-tf-listener) "odom_combined" "head_tilt_link" + :timeout 2.0)) + (camera-fwd (cl-transforms-stamped:make-3d-vector 1 0 0)) + (camera-fwd (cl-transforms-stamped:rotate (cl-transforms-stamped:rotation camera-pose) camera-fwd)) + (camera-up (cl-transforms-stamped:make-3d-vector 0 0 1)) + (camera-up (cl-transforms-stamped:rotate (cl-transforms-stamped:rotation camera-pose) camera-up)) + (camera-pose (cl-transforms-stamped:translation camera-pose)) + (camera-pose (roslisp:make-message "geometry_msgs/Point" + :x (cl-transforms-stamped:x camera-pose) + :y (cl-transforms-stamped:y camera-pose) + :z (cl-transforms-stamped:z camera-pose))) + (camera-fwd (roslisp:make-message "geometry_msgs/Point" + :x (cl-transforms-stamped:x camera-fwd) + :y (cl-transforms-stamped:y camera-fwd) + :z (cl-transforms-stamped:z camera-fwd))) + (camera-up (roslisp:make-message "geometry_msgs/Point" + :x (cl-transforms-stamped:x camera-up) + :y (cl-transforms-stamped:y camera-up) + :z (cl-transforms-stamped:z camera-up))) + (focal-distance 1) + (width 1) + (height 1) + (max-distance 12) + (threshold 0.2)) + (cpl:mapcar-clean (lambda (model-name) + (roslisp:with-fields (visible) + (roslisp:call-service "/gazebo_visibility_ros/QueryGazeboVisibility" + "gazebo_visibility_ros/QueryGazeboVisibility" + :name model-name + :camera_pose camera-pose + :camera_fwd camera-fwd + :camera_up camera-up + :focal_distance focal-distance + :width width + :height height + :max_distance max-distance + :threshold threshold) + (unless (eql visible 0) + model-name))) + model-names))) + +(defun find-objects (&key object-name) "Finds objects based on either their name `object-name' or their type `object-type', depending what is given. An invalid combination of both parameters will result in an empty list. When no parameters are given, all known objects from the knowledge base are returned." - (cond (object-name - (let* ((obj-symbol object-name) - (model-pose (cram-gazebo-utilities:get-model-pose - object-name))) - (when model-pose - (list (make-instance 'gazebo-designator-shape-data - :object-identifier obj-symbol - :pose model-pose))))) - (object-type - (loop for model-data in (cram-gazebo-utilities:get-models) - as name = (car model-data) - when (and (>= (length name) (length object-type)) - (string= object-type (subseq name 0 (length object-type)))) - collect - (make-instance 'gazebo-designator-shape-data - :object-identifier name - :pose (cdr model-data)))) - (t - (mapcar (lambda (model-data) - (destructuring-bind (model-name . model-pose) - model-data - (make-instance 'gazebo-designator-shape-data - :object-identifier model-name - :pose model-pose))) - (cram-gazebo-utilities:get-models))))) - -(defun perceived-object->designator (designator perceived-object) - (make-effective-designator - designator - :new-properties (make-new-desig-description - designator perceived-object) - :data-object perceived-object)) + (let* ((model-names (mapcar #'car (cram-gazebo-utilities:get-models))) + (filtered-model-names (filter-models-by-ignored-objects + model-names)) + (filtered-model-names (filter-models-by-name + filtered-model-names + :template-name object-name)) + ;; TODO: Fix this external component; it returns all spawned + ;; objects instead of the currently visible ones. This is + ;; intended behavior and is related to problems in Gazebo + ;; 2.2.3 w.r.t. raytracing code. + ;(filtered-model-names (filter-models-by-field-of-view + ; filtered-model-names)) + ) + (mapcar (lambda (model-name) + (let ((pose (cram-gazebo-utilities:get-model-pose model-name))) + (make-instance 'gazebo-designator-shape-data + :object-identifier model-name + :pose pose))) + filtered-model-names))) (defun find-with-designator (designator) - (with-desig-props (desig-props::name desig-props::type) designator - (let* ((at (desig-prop-value designator 'desig-props::at)) - (pose-in-at (desig-prop-value at 'desig-props::pose)) - (filter-function - (cond ((and at (not pose-in-at)) - (lambda (object-check) - (let* ((sample (reference at)) - ;; This is a 2d comparison; put the z - ;; coordinate from the sample into the - ;; pose before validating. Otherwise, - ;; gravity will mess up everything. - (pose (desig-prop-value - (desig-prop-value - object-check - 'desig-props::at) - 'desig-props::pose)) - (pose-elevated - (tf:copy-pose - pose - :origin (tf:make-3d-vector (tf:x (tf:origin pose)) - (tf:y (tf:origin pose)) - (tf:z (tf:origin sample)))))) - (not (validate-location-designator-solution at pose-elevated))))) - (t #'not)))) - (remove-if - filter-function - (mapcar (lambda (perceived-object) - (perceived-object->designator - designator perceived-object)) - (find-object :object-name desig-props::name - :object-type desig-props::type)))))) + (let* ((template-name (desig-prop-value designator :name)) + (template-type (desig-prop-value designator :type)) + (models (find-objects :object-name template-name))) + (cpl:mapcar-clean (lambda (model) + (with-slots ((model-name desig::object-identifier) (pose desig::pose)) model + (let* ((pose (cram-gazebo-utilities:get-model-pose model-name)) + (location (make-designator :location `((:pose ,pose)))) + (description (cram-gazebo-utilities:spawned-object-description model-name)) + (description-type (cadr (find :type description :test (lambda (x y) + (eql x (car y)))))) + (model-data (make-instance 'gazebo-designator-shape-data + :object-identifier model-name + :pose pose))) + (when (or (and template-type (equal template-type description-type)) + (not template-type)) + (make-effective-designator + designator + :new-properties (append `((:name ,model-name) + (:at ,location)) + description) + :data-object model-data))))) + models))) + +(defun get-bullet-objects () + (cpl:mapcar-clean + #'identity + (cut:force-ll + (cut:lazy-mapcar + (lambda (bdgs) + (cut:with-vars-bound (?o) bdgs + (when (stringp ?o) ?o))) + (cram-prolog:prolog + `(and (btr:bullet-world ?w) + (btr:object ?w ?o) + (not (btr::robot ?o)))))))) + +(defun update-bullet-object (name pose) + (cram-prolog:prolog + `(and (btr:bullet-world ?w) + (btr:assert (btr:object ?w :box ,name ,pose))))) + +(defun add-bullet-object (name pose dimensions) + (cram-prolog:prolog + `(and (btr:bullet-world ?w) + ;;(btr:retract (btr:object ?w ,name)) + (btr:assert (btr:object ?w :box ,name ,pose + :mass 0.1 + :size ,dimensions))))) + +(defun update-belief-state (objects) + (let* ((bullet-objects (get-bullet-objects)) + (new-objects + (cpl:mapcar-clean (lambda (object) + (let* ((name (desig-prop-value object :name))) + (unless (find name bullet-objects :test #'string=) + object))) + objects)) + (present-objects + (cpl:mapcar-clean (lambda (object) + (let ((name (desig-prop-value object :name))) + (when (find name bullet-objects :test #'string=) + object))) + objects))) + (loop for object in present-objects do + (let* ((at (desig-prop-value object :at)) + (pose (desig-prop-value at :pose)) + (name (desig-prop-value object :name))) + (update-bullet-object name pose))) + (loop for object in new-objects do + (let* ((at (desig-prop-value object :at)) + (pose (desig-prop-value at :pose)) + (dimensions (desig-prop-value object :dimensions)) + (name (desig-prop-value object :name)) + (dimensions-list `(,(tf:x dimensions) + ,(tf:y dimensions) + ,(tf:z dimensions)))) + (add-bullet-object name pose dimensions-list))))) (def-process-module gazebo-perception-process-module (input) (assert (typep input 'action-designator)) - (let ((object-designator (desig-prop-value input 'desig-props::obj))) - (ros-info (gazebo perception-process-module) - "Searching for object ~a" object-designator) - (cram-task-knowledge:filter-perceived-objects - object-designator - (find-with-designator object-designator)))) + (let* ((object-designator (desig-prop-value input :obj)) + (log-id (first (cram-language::on-prepare-perception-request object-designator)))) + (ros-info (gazebo perception-process-module) "Searching for object ~a" object-designator) + (let ((results (find-with-designator object-designator))) + (update-belief-state results) + (cram-language::on-finish-perception-request log-id results) + (if (not results) + (cpl:fail 'cram-common-failures:perception-object-not-found :object-desig object-designator) + results)))) diff --git a/gazebo_perception_process_module/src/utilities.lisp b/gazebo_perception_process_module/src/utilities.lisp index db77f73..88f6a61 100644 --- a/gazebo_perception_process_module/src/utilities.lisp +++ b/gazebo_perception_process_module/src/utilities.lisp @@ -28,6 +28,8 @@ (in-package :gazebo-perception-pm) +(defvar *ignored-objects* nil) + (defun object-names-equal (name-1 name-2) "In designators, we want to support symbols as names, too. It first converts all names that are symbols to strings and then compares @@ -39,3 +41,12 @@ (symbol (symbol-name name-2)) (string name-2)))) (string-equal name-1 name-2))) + +(defun ignore-object (name &optional (ignore t)) + (if ignore + (unless (is-object-ignored name) + (push name *ignored-objects*)) + (setf *ignored-objects* (remove name *ignored-objects* :test #'string=)))) + +(defun is-object-ignored (name) + (not (not (find name *ignored-objects* :test #'string=)))) diff --git a/gazebo_visibility_ros/CMakeLists.txt b/gazebo_visibility_ros/CMakeLists.txt new file mode 100644 index 0000000..4fcb790 --- /dev/null +++ b/gazebo_visibility_ros/CMakeLists.txt @@ -0,0 +1,68 @@ +cmake_minimum_required(VERSION 2.8.3) +project(gazebo_visibility_ros) + +FIND_PACKAGE( Boost 1.40 COMPONENTS system REQUIRED ) +set (CMAKE_CXX_FLAGS "-g -Wall -std=c++11 ${CMAKE_CXX_FLAGS}") + +find_package(PkgConfig) +include (FindPkgConfig) +if (PKG_CONFIG_FOUND) + pkg_check_modules(GAZEBO gazebo) + pkg_check_modules(SDF sdformat) +endif() + +set(MSG_DEPS + std_msgs + geometry_msgs +) + +set(MSG_FILES + Dummy.msg +) + +set(SRV_FILES + QueryGazeboVisibility.srv +) + +set(ACT_FILES +) + +# Load catkin and all dependencies required for this package +find_package(catkin REQUIRED COMPONENTS + roscpp + gazebo_ros + ${MSG_DEPS} + message_generation +) + +# Depend on system install of Gazebo +find_package(gazebo REQUIRED) + +#add_action_files(DIRECTORY action FILES ${ACT_FILES}) +add_message_files(DIRECTORY msg FILES ${MSG_FILES}) +add_service_files(DIRECTORY srv FILES ${SRV_FILES}) + +generate_messages(DEPENDENCIES ${MSG_DEPS}) + +catkin_package( + INCLUDE_DIRS include + DEPENDS + ${MSG_DEPS} + message_runtime + roscpp + gazebo_ros +) +include_directories( + ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} + ${GAZEBO_INCLUDE_DIRS} + ${SDF_INCLUDE_DIRS} + ) +link_directories(${GAZEBO_LIBRARY_DIRS}) +include_directories(${GAZEBO_INCLUDE_DIRS}) + +add_library(${PROJECT_NAME} src/gazebo_visibility_ros.cpp) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} ${SDF_LIBRARIES}) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) + + + diff --git a/gazebo_visibility_ros/include/gazebo_visibility_ros/gazebo_visibility_ros.h b/gazebo_visibility_ros/include/gazebo_visibility_ros/gazebo_visibility_ros.h new file mode 100644 index 0000000..60134d3 --- /dev/null +++ b/gazebo_visibility_ros/include/gazebo_visibility_ros/gazebo_visibility_ros.h @@ -0,0 +1,5 @@ +#ifndef __GAZEBO_VISIBILITY_ROS__ +#define __GAZEBO_VISIBILITY_ROS__ + +#endif + diff --git a/gazebo_visibility_ros/msg/Dummy.msg b/gazebo_visibility_ros/msg/Dummy.msg new file mode 100644 index 0000000..e69de29 diff --git a/gazebo_visibility_ros/package.xml b/gazebo_visibility_ros/package.xml new file mode 100644 index 0000000..5571988 --- /dev/null +++ b/gazebo_visibility_ros/package.xml @@ -0,0 +1,38 @@ + + gazebo_visibility_ros + 0.1.0 + + + A little service to (approximately) test whether an object is visible in a Gazebo + world from a given camera pose + + + Mihai Pomarlan + Mihai Pomarlan + BSD + + http://open-ease.org + https://github.com/mpomarlan/meshproc/issues + https://github.com/mpomarlan/meshproc + + catkin + + gazebo_ros + roscpp + message_generation + geometry_msgs + std_msgs + + gazebo_ros + roscpp + message_runtime + geometry_msgs + std_msgs + + + + + + + + diff --git a/gazebo_visibility_ros/src/gazebo_visibility_ros.cpp b/gazebo_visibility_ros/src/gazebo_visibility_ros.cpp new file mode 100644 index 0000000..152b70a --- /dev/null +++ b/gazebo_visibility_ros/src/gazebo_visibility_ros.cpp @@ -0,0 +1,252 @@ +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; + +namespace gazebo +{ + +void getBBoxGrid(math::Box const& bbox, int size, std::vector &points) +{ + ROS_INFO("Model bounding box %f %f %f %f %f %f", bbox.GetCenter().x, bbox.GetCenter().y, bbox.GetCenter().z, + bbox.GetXLength(), bbox.GetYLength(), bbox.GetZLength()); + points.clear(); + points.resize(size*size*size); + math::Vector3 d = bbox.GetSize(); + math::Vector3 lrd = bbox.GetCenter() - d*0.5; + math::Vector3 s = d*(1.0/(size + 1)); + int l = 0; + for(int k = 0; k < size; k++) + for(int j = 0; j < size; j++) + for(int i = 0; i < size; i++) + { + math::Vector3 disp; + disp.x = s.x*(k + 1); + disp.y = s.y*(j + 1); + disp.z = s.z*(i + 1); + points[l] = lrd + disp; + l++; + }; +} + +math::Vector3 getEndPoint(math::Vector3 const& start, math::Vector3 const& end, double maxDist) +{ + math::Vector3 dir = end - start; + dir = dir.Normalize(); + dir = dir*maxDist; + + return start + dir; +} + +bool inImage(math::Vector3 const& start, math::Vector3 const& end, math::Vector3 const& cx, math::Vector3 const& cy, math::Vector3 const& cz, double upAngle, double sideAngle) +{ + math::Vector3 dir = end - start; + dir = dir.Normalize(); + + math::Vector3 hProj, vProj; + double sA, uA; + + hProj = dir - (cz*dir)*cz; + hProj = hProj.Normalize(); + vProj = dir - (cy*dir)*cy; + vProj = vProj.Normalize(); + + sA = hProj.Dot(cx); + uA = vProj.Dot(cx); + return ((sideAngle <= sA) && (upAngle <= uA)); +} + +std::string trimSubEnts(std::string const& name) +{ + return name.substr(0, name.find(':')); +} + +/*Need to implement this because gazebo can't compute the bounding box properly for a model that contains more +than a single link*/ +void getBaseBox(physics::Base const* base, math::Vector3 &maxs, math::Vector3 &mins, bool &inited) +{ + if(base->HasType(physics::Base::LINK)) + { + physics::Link const* link = dynamic_cast(base); + math::Box box = link->GetBoundingBox(); + if(!inited) + { + inited = true; + maxs = box.max; + mins = box.min; + } + else + { + math::Vector3 maxc = box.max; + math::Vector3 minc = box.min; + maxs.x = (maxc.x < maxs.x) ? maxs.x : minc.x; + maxs.y = (maxc.y < maxs.y) ? maxs.y : minc.y; + maxs.z = (maxc.z < maxs.z) ? maxs.z : minc.z; + mins.x = (minc.x > mins.x) ? mins.x : minc.x; + mins.y = (minc.y > mins.y) ? mins.y : minc.y; + mins.z = (minc.z > mins.z) ? mins.z : minc.z; + } + } + else + { + int maxK = base->GetChildCount(); + for(int k = 0; k < maxK; k++) + getBaseBox(base->GetChild(k).get(), maxs, mins, inited); + } +} + +math::Box getModelBox(physics::ModelPtr const& model) +{ + math::Vector3 mins, maxs; + bool inited = false; + if(model->HasType(physics::Base::LINK)) + return model->GetBoundingBox(); + int maxK = model->GetChildCount(); + for(int k = 0; k < maxK; k++) + getBaseBox(model->GetChild(k).get(), maxs, mins, inited); + math::Box box; + box.max = maxs; + box.min = mins; + return box; +} + +bool doQueryGazeboVisibility(physics::WorldPtr world, sdf::ElementPtr sdf, gazebo_visibility_ros::QueryGazeboVisibility::Request &request, gazebo_visibility_ros::QueryGazeboVisibility::Response &response) +{ + ROS_INFO("Got QueryGazeboVisibility service request."); + physics::ModelPtr model = world->GetModel(request.name); + if((!model.get())) + { + ROS_INFO("Model name not loaded."); + response.visible = false; + return true; + } + ROS_INFO("Model name seems to make sense, and points to a model of type %d with %d children.", model->GetType(), model->GetChildCount()); + + math::Box bbox = getModelBox(model); + + math::Vector3 start, cameraFwd, cameraSide, cameraUp; + start.x = request.camera_pose.x; + start.y = request.camera_pose.y; + start.z = request.camera_pose.z; + cameraFwd.x = request.camera_fwd.x; + cameraFwd.y = request.camera_fwd.y; + cameraFwd.z = request.camera_fwd.z; + cameraFwd = cameraFwd.Normalize(); + cameraUp.x = request.camera_up.x; + cameraUp.y = request.camera_up.y; + cameraUp.z = request.camera_up.z; + cameraUp = cameraUp.Normalize(); + cameraUp = cameraUp - (cameraUp*cameraFwd)*cameraFwd; + cameraUp = cameraUp.Normalize(); + cameraSide = cameraUp.Cross(cameraFwd); + + double h2 = request.height*0.5; + double w2 = request.width*0.5; + + math::Vector3 aux = request.focal_distance*cameraFwd + h2*cameraUp; + aux = aux.Normalize(); + double upAngle = aux.Dot(cameraFwd); + aux = request.focal_distance*cameraFwd + w2*cameraSide; + aux = aux.Normalize(); + double sideAngle = aux.Dot(cameraFwd); + + std::vector points; + + getBBoxGrid(bbox, 8, points); + ROS_INFO("Got a grid of points to test."); + + int maxK = points.size(); + int okPoints = 0; + + if(5 <= GAZEBO_MAJOR_VERSION) + { + gazebo::physics::PhysicsEnginePtr engine = world->GetPhysicsEngine(); + engine->InitForThread(); + + gazebo::physics::ShapePtr ray = engine->CreateShape("ray", gazebo::physics::CollisionPtr()); + + for(int k = 0; k < maxK; k++) + { + math::Vector3 end = getEndPoint(start, points[k], request.max_distance); + if(inImage(start, end, cameraFwd, cameraSide, cameraUp, upAngle, sideAngle)) + { + boost::dynamic_pointer_cast(ray)->SetPoints(start, end); + double dist; + std::string entityName; + boost::dynamic_pointer_cast(ray)->GetIntersection(dist, entityName); + if(request.name == trimSubEnts(entityName)) + okPoints++; + } + } + } + else + { + for(int k = 0; k < maxK; k++) + { + /*A bit of a hack to get something running on older gazebo versions, even if a little.*/ + math::Vector3 end = getEndPoint(start, points[k], request.max_distance); + if(inImage(start, end, cameraFwd, cameraSide, cameraUp, upAngle, sideAngle)) + okPoints++; + } + } + + ROS_INFO("Done with raytrace."); + + response.visible = false; + if(request.threshold <= (okPoints*1.0)/(maxK*1.0)) + response.visible = true; + + return true; +} + +class GazeboVisibilityROS : public WorldPlugin +{ +public: + GazeboVisibilityROS() : WorldPlugin(), n("~") + { + } + + void Load(physics::WorldPtr world, sdf::ElementPtr sdf) + { + // Make sure the ROS node for Gazebo has already been initialized + if (!ros::isInitialized()) + { + ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } + + _world = world; + _sdf = sdf; + + ROS_INFO("Starting up the QueryGazeboVisibility service."); + + queryGazeboVisibility_service = n.advertiseService("/gazebo_visibility_ros/QueryGazeboVisibility", + boost::bind(doQueryGazeboVisibility, _world, _sdf, _1, _2)); + + } + +private: + physics::WorldPtr _world; + sdf::ElementPtr _sdf; + ros::NodeHandle n; + ros::ServiceServer queryGazeboVisibility_service; + +}; +GZ_REGISTER_WORLD_PLUGIN(GazeboVisibilityROS) +} diff --git a/gazebo_visibility_ros/srv/QueryGazeboVisibility.srv b/gazebo_visibility_ros/srv/QueryGazeboVisibility.srv new file mode 100644 index 0000000..70783f8 --- /dev/null +++ b/gazebo_visibility_ros/srv/QueryGazeboVisibility.srv @@ -0,0 +1,12 @@ +string name +geometry_msgs/Point camera_pose +geometry_msgs/Point camera_fwd +geometry_msgs/Point camera_up +float64 focal_distance +float64 width +float64 height +float64 max_distance +float64 threshold +--- +int32 visible +