Boxy melodic update

cram_3d_world/cl_bullet_vis/cl-bullet-vis.asd

@@ -41,7 +41,8 @@
                  cl-glx
                  trivial-garbage
                  alexandria
-                 cram-physics-utils)
+                 cram-physics-utils
+                 cram-math)
     :components
     ((:module "src"
               :components

cram_3d_world/cl_bullet_vis/src/bullet-world-window.lisp

@@ -69,6 +69,104 @@
   (when (frame-rate w)
     (glut:enable-tick w (truncate (* (/ (frame-rate w)) 1000)))))
 
+(defvar *draw-ui* nil)
+(defvar *draw-textured-number* nil)
+(defvar *text-to-draw* "")
+
+(defun draw-ui (window)
+  (when *draw-ui*
+    (gl:disable :lighting)
+    (gl:with-pushed-matrix
+      (let ((text-point (cl-transforms:transform-point
+                        (camera-transform window)
+                        (cl-transforms:make-3d-vector 3.0 0 1.3))))
+        (gl:translate (cl-transforms:x text-point)
+                      (cl-transforms:y text-point)
+                      (cl-transforms:z text-point)))
+      (gl:color 1 0 0 1.0)
+      (%gl:raster-pos-3f 0.0 0.0 0.0)
+      (glut:bitmap-string glut:+bitmap-times-roman-24+ *text-to-draw*)
+      (glut:bitmap-string glut:+bitmap-times-roman-24+ "Red")
+      (when *draw-textured-number*
+        ;; (print (gl:get* :current-raster-position))
+        ;; (print (gl:get* :current-raster-position-valid))
+        (let ((text-pose (cl-transforms:transform-pose
+                          (camera-transform window)
+                          (cl-transforms:make-pose
+                           (cl-transforms:make-3d-vector 3.0 0 1.3)
+                           (cl-transforms:make-identity-rotation)))))
+          ;; (gl:translate (cl-transforms:x (cl-transforms:origin text-pose))
+          ;;               (cl-transforms:y (cl-transforms:origin text-pose))
+          ;;               (cl-transforms:z (cl-transforms:origin text-pose)))
+          (gl:translate 0.5 0 0)
+          (multiple-value-bind (axis angle)
+              (cl-transforms:quaternion->axis-angle (cl-transforms:orientation text-pose))
+            (gl:rotate (cma:radians->degrees angle)
+                       (cl-transforms:x axis) (cl-transforms:y axis) (cl-transforms:z axis))
+            (gl:rotate 90 0 1 0)
+            (gl:rotate -90 0 0 1)))
+        (gl:enable :lighting)
+        (gl:color 1 0 0 1.0)
+        (gl:scale 0.1 0.1 0.1)
+        ;; (glut:solid-cube 2.0)
+        (gl:with-pushed-attrib (:texture-bit :enable-bit)
+          (multiple-value-bind (texture-handle new?)
+              (cl-bullet-vis:get-texture-handle window (gensym))
+            (gl:bind-texture :texture-2d texture-handle)
+            ;; (print new?)
+            (when new?
+              (gl:tex-env :texture-env :texture-env-mode :modulate)
+              (gl:tex-parameter :texture-2d :texture-min-filter :linear-mipmap-linear)
+              (gl:tex-parameter :texture-2d :texture-mag-filter :linear)
+              (gl:tex-parameter :texture-2d :texture-wrap-s :repeat)
+              (gl:tex-parameter :texture-2d :texture-wrap-t :repeat)
+              (glu:build-2d-mipmaps
+               :texture-2d 4 16 16 :rgba :float
+               (texture-str->bitmap
+                (concatenate
+                 'string
+                 "xxxxxxxxxxxxxxxx"
+                 "x              x"
+                 "x      xx      x"
+                 "x     x x      x"
+                 "x    x  x      x"
+                 "x   x   x      x"
+                 "x       x      x"
+                 "x       x      x"
+                 "x       x      x"
+                 "x       x      x"
+                 "x       x      x"
+                 "x       x      x"
+                 "x    xxxxxxx   x"
+                 "x              x"
+                 "x              x"
+                 "xxxxxxxxxxxxxxxx")
+                #\o #\Space #\x '(1 1 1 1)))))
+          (gl:enable :texture-2d)
+          (gl:disable :cull-face)
+          (gl:disable :lighting)
+          (gl:with-primitive :quads
+            (gl:normal 0 0 1)
+            (gl:tex-coord 0.0 0.0)
+            (gl:vertex -0.5 -0.5)
+            (gl:tex-coord 1.0 0.0)
+            (gl:vertex 0.5 -0.5)
+            (gl:tex-coord 1.0 1.0)
+            (gl:vertex 0.5 0.5)
+            (gl:tex-coord 0.0 1.0)
+            (gl:vertex -0.5 0.5)))))
+    (gl:with-pushed-matrix
+      (let ((text-point (cl-transforms:transform-point
+                        (camera-transform window)
+                        (cl-transforms:make-3d-vector 3.0 -0.2 1.3))))
+        (gl:translate (cl-transforms:x text-point)
+                      (cl-transforms:y text-point)
+                      (cl-transforms:z text-point)))
+      (gl:color 0 1 0 1.0)
+      (%gl:raster-pos-3f 0.0 0.0 0.0)
+      (glut:bitmap-string glut:+bitmap-times-roman-24+ "Green"))
+    (gl:enable :lighting)))
+
 (defmethod glut:display ((window bullet-world-window))
   (with-rendering-lock
     (gl:matrix-mode :projection)
@@ -98,6 +196,8 @@
         (dolist (obj transparent-objects)
           (draw window obj))
         (%gl:depth-mask t)))
+    ;; Draw UI texts
+    (draw-ui window)
     ;; When we are moving around, draw a little yellow disk similar to
     ;; that one RVIZ draws.
     (when (or (eq (motion-mode window) :rotate)

cram_3d_world/cl_bullet_vis/src/math-function-visualization.lisp

@@ -1,20 +1,20 @@
 ;;;
 ;;; Copyright (c) 2010, Lorenz Moesenlechner <[email protected]>
 ;;; All rights reserved.
-;;; 
+;;;
 ;;; Redistribution and use in source and binary forms, with or without
 ;;; modification, are permitted provided that the following conditions are met:
-;;; 
+;;;
 ;;;     * Redistributions of source code must retain the above copyright
 ;;;       notice, this list of conditions and the following disclaimer.
 ;;;     * 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 the Intelligent Autonomous Systems Group/
-;;;       Technische Universitaet Muenchen nor the names of its contributors 
-;;;       may be used to endorse or promote products derived from this software 
+;;;       Technische Universitaet Muenchen 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
 ;;; IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
@@ -38,33 +38,43 @@
                                                 (cl-transforms:make-3d-vector 0 0 0)
                                                 (cl-transforms:make-quaternion 0 0 0 1)))
    (fun :initarg :function :reader fun)
+   (height-fun :initarg :height-function :reader height-fun :initform (constantly 0.0))
    (alpha :initarg :alpha :reader alpha :initform 1.0)
    (color-fun :initarg :color-fun :reader color-fun :initform (constantly (list 0.0 0.0 0.6)))))
 
 (defmethod gl-object-transparent ((obj math-function-object))
   (< (alpha obj) 1.0))
 
 (defmethod draw ((context gl-context) (obj math-function-object))
-  (let ((points (make-hash-table :test 'equal))
+  (let ((value-points (make-hash-table :test 'equal))
+        (heights (make-hash-table :test 'equal))
         (colors (make-hash-table :test 'equal)))
-    (flet ((get-point (x y)
+    (flet ((get-value (x y)
              (with-slots (fun) obj
                (multiple-value-bind (old-value found?)
-                   (gethash (list x y) points)
+                   (gethash (list x y) value-points)
                  (if found?
                      old-value
                      (let ((new-value (funcall fun x y)))
-                       (setf (gethash (list x y) points)
+                       (setf (gethash (list x y) value-points)
+                             (when (numberp new-value)
+                               new-value)))))))
+           (get-height (x y)
+             (with-slots (height-fun) obj
+               (multiple-value-bind (old-value found?)
+                   (gethash (list x y) heights)
+                 (if found?
+                     old-value
+                     (let ((new-value (funcall height-fun x y)))
+                       (setf (gethash (list x y) heights)
                              (when (numberp new-value)
-                               (cl-transforms:make-3d-vector x y new-value))))))))
-           (get-color (vec)
-             (let ((x (cl-transforms:x vec))
-                   (y (cl-transforms:y vec)))
-               (with-slots (color-fun) obj
-                 (or (gethash (list x y) colors)
-                     (setf (gethash (list x y) colors)
-                           (append (subseq (funcall color-fun vec) 0 3)
-                                   (list (alpha obj))))))))
+                               new-value)))))))
+           (get-color (x y value)
+             (with-slots (color-fun) obj
+               (or (gethash (list x y) colors)
+                   (setf (gethash (list x y) colors)
+                         (append (subseq (funcall color-fun value) 0 3)
+                                 (list (alpha obj)))))))
            (vertex (vec &optional normal)
              (let ((normal (cl-transforms:v*
                             normal
@@ -86,34 +96,39 @@
             (gl:mult-matrix (pose->gl-matrix pose))
             (loop for x from (- (/ width 2)) to (- (/ width 2) step-size) by step-size do
               (loop for y from (- (/ height 2)) to (- (/ height 2) step-size) by step-size do
-                (let ((middle-pt (get-point (+ x (/ step-size 2))
-                                            (+ y (/ step-size 2)))))
-                  (when middle-pt
-                    (let* ((value (cl-transforms:z middle-pt))
-                           (pt-1 (cl-transforms:make-3d-vector
-                                  x y value))
-                           (pt-2 (cl-transforms:make-3d-vector
-                                  (+ x step-size) y value))
-                           (pt-3 (cl-transforms:make-3d-vector
-                                  x (+ y step-size) value))
-                           (pt-4 (cl-transforms:make-3d-vector
-                                  (+ x step-size) (+ y step-size) value))
-                           (n-1 (cl-transforms:cross-product
-                                 (cl-transforms:v- pt-2 pt-1)
-                                 (cl-transforms:v- pt-4 pt-1)))
-                           (n-2 (cl-transforms:cross-product
-                                 (cl-transforms:v- pt-4 pt-1)
-                                 (cl-transforms:v- pt-3 pt-1))))
+                (let* ((pt-middle-x (+ x (/ step-size 2)))
+                       (pt-middle-y (+ y (/ step-size 2)))
+                       (value (get-value pt-middle-x pt-middle-y)))
+                  (when value
+                    (let* ((color
+                             (get-color pt-middle-x pt-middle-y value))
+                           (height
+                             (get-height pt-middle-x pt-middle-y))
+                           (pt-1
+                             (cl-transforms:make-3d-vector
+                              x y height))
+                           (pt-2
+                             (cl-transforms:make-3d-vector
+                              (+ x step-size) y height))
+                           (pt-3
+                             (cl-transforms:make-3d-vector
+                              x (+ y step-size) height))
+                           (pt-4
+                             (cl-transforms:make-3d-vector
+                              (+ x step-size) (+ y step-size) height))
+                           (n-1
+                             (cl-transforms:cross-product
+                              (cl-transforms:v- pt-2 pt-1)
+                              (cl-transforms:v- pt-4 pt-1)))
+                           (n-2
+                             (cl-transforms:cross-product
+                              (cl-transforms:v- pt-4 pt-1)
+                              (cl-transforms:v- pt-3 pt-1))))
                       (gl:with-primitive :triangles
-                        (apply #'gl:color (get-color pt-1))
+                        (apply #'gl:color color)
                         (vertex pt-1 n-1)
-                        (apply #'gl:color (get-color pt-2))
                         (vertex pt-2 n-1)
-                        (apply #'gl:color (get-color pt-4))
                         (vertex pt-4 n-1)
-                        (apply #'gl:color (get-color pt-1))
                         (vertex pt-1 n-2)
-                        (apply #'gl:color (get-color pt-4))
                         (vertex pt-4 n-2)
-                        (apply #'gl:color (get-color pt-3))
                         (vertex pt-3 n-2)))))))))))))

cram_3d_world/cram_btr_spatial_relations_costmap/cram-btr-spatial-relations-costmap-tests.asd

@@ -0,0 +1,68 @@
+;;; Copyright (c) 2020, Amar Fayaz <[email protected]>
+;;; All rights reserved.
+;;;
+;;; Redistribution and use in source and binary forms, with or without
+;;; modification, are permitted provided that the following conditions are met:
+;;;
+;;;     * Redistributions of source code must retain the above copyright
+;;;       notice, this list of conditions and the following disclaimer.
+;;;     * 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 the Intelligent Autonomous Systems Group/
+;;;       Technische Universitaet Muenchen 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
+;;; IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+;;; ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+;;; LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+;;; CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+;;; SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+;;; INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+;;; CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+;;; ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+;;; POSSIBILITY OF SUCH DAMAGE.
+
+(defsystem cram-btr-spatial-relations-costmap-tests
+  :author "Amar Fayaz"
+  :license "BSD"
+  :depends-on (roslisp
+               lisp-unit
+
+               cram-language
+               cram-executive
+               cram-designators
+               cram-prolog
+               cram-projection
+               cram-occasions-events
+
+               cram-common-failures
+               cram-mobile-pick-place-plans
+               cram-object-knowledge
+
+               cram-physics-utils     ; for reading "package://" paths
+               cl-bullet ; for handling BOUNDING-BOX datastructures
+               cram-bullet-reasoning
+               cram-bullet-reasoning-belief-state
+               cram-bullet-reasoning-utilities
+
+               cram-location-costmap
+               cram-btr-visibility-costmap
+               cram-btr-spatial-relations-costmap
+               cram-robot-pose-gaussian-costmap
+               cram-occupancy-grid-costmap
+
+               cram-urdf-projection      ; for with-simulated-robot
+               cram-urdf-projection-reasoning ; to set projection reasoning to T
+               cram-fetch-deliver-plans
+               cram-urdf-environment-manipulation
+
+               cram-pr2-description)
+  :components
+  ((:module "tests"
+    :components
+    ((:file "package")
+     (:file "orientation-tests" :depends-on ("package"))))))