From 37ce51d55861ca504c59684de0247a7c6c48cd37 Mon Sep 17 00:00:00 2001
From: Cannon Lewis <cannon10100@gmail.com>
Date: Mon, 24 Apr 2017 00:25:03 -0500
Subject: [PATCH] Modified package and method definitions to expose moveit
 scene file parsing

---
 lisp/amino-rx.asd                      |  2 +-
 lisp/rx/robray-package.lisp            |  3 +++
 lisp/rx/scenefile/moveit-to-tmkit.lisp | 20 ++++++++++++++++++++
 lisp/rx/scenefile/moveit.lisp          |  2 +-
 4 files changed, 25 insertions(+), 2 deletions(-)
 create mode 100644 lisp/rx/scenefile/moveit-to-tmkit.lisp

diff --git a/lisp/amino-rx.asd b/lisp/amino-rx.asd
index d181eb90..56b64e61 100644
--- a/lisp/amino-rx.asd
+++ b/lisp/amino-rx.asd
@@ -78,7 +78,7 @@
                (:file "rx/lexer" :depends-on ("rx/util"))
                (:file "rx/inex" :depends-on ("rx/util"))
                (:file "rx/scenefile/curly" :depends-on ("rx/scenegraph" "rx/mesh" "rx/lexer" "rx/inex"))
-               ;(:file "rx/scenefile/moveit" :depends-on ("rx/scenegraph" "rx/mesh"))
+               (:file "rx/scenefile/moveit" :depends-on ("rx/scenegraph" "rx/mesh"))
                (:file "rx/scenefile/scenefile"
                       :depends-on ("rx/scenefile/urdf" "rx/scenefile/curly"))
                (:file "rx/animate" :depends-on ("rx/scenegraph" "rx/povray"))
diff --git a/lisp/rx/robray-package.lisp b/lisp/rx/robray-package.lisp
index fec6cff3..91bd6465 100644
--- a/lisp/rx/robray-package.lisp
+++ b/lisp/rx/robray-package.lisp
@@ -102,4 +102,7 @@
    :motion-plan-endpoint-map
    :motion-plan-endpoint-array
 
+   ;; scenegraph parsing
+   :load-moveit-scene
+
    ))
diff --git a/lisp/rx/scenefile/moveit-to-tmkit.lisp b/lisp/rx/scenefile/moveit-to-tmkit.lisp
new file mode 100644
index 00000000..273397aa
--- /dev/null
+++ b/lisp/rx/scenefile/moveit-to-tmkit.lisp
@@ -0,0 +1,20 @@
+(load "/home/cannon/tmkit/load.lisp")
+
+(in-package robray)
+
+(require :amino-rx)
+
+(defparameter *robray-tmp-directory* "/tmp/amino-cannon")
+
+(robray::load-moveit-scene "~/moveit_scene.scene")
+
+
+(with-open-file (stream "~/moveit_scene.robray"
+                     :direction :output
+                     :if-exists :supersede
+                     :if-does-not-exist :create)
+  (format stream (sycamore:rope-string (sycamore:rope (robray::load-moveit-scene "~/moveit_scene.scene")))))
+
+
+(TMSMT:TMP-DRIVER :START-SCENE '("package://ur_description/urdf/ur5_robotiq_robot_limited.urdf" "/home/cannon/baxter-blocks/moveit_scene.robray" "/home/cannon/baxter-blocks/ur5_robotiq85_allowed_collision.robray") :GOAL-SCENE '("/home/cannon/baxter-blocks/moveit_scene_goal.robray") :PDDL '("/home/cannon/baxter-blocks/tm-blocks.pddl") :GUI "1" :SCRIPTS '("/home/cannon/baxter-blocks/tm-blocks.py") :VERBOSE NIL :MAX-STEPS 10 :OUTPUT "ur5-robotiq-sussman.tmp" :WRITE-FACTS NIL :MOTION-TIMEOUT NIL :START-PLAN NIL :START NIL :PREFIX-CACHE T :CONSTRAINTS :STATE)
+
diff --git a/lisp/rx/scenefile/moveit.lisp b/lisp/rx/scenefile/moveit.lisp
index 6279c6be..28819913 100644
--- a/lisp/rx/scenefile/moveit.lisp
+++ b/lisp/rx/scenefile/moveit.lisp
@@ -140,7 +140,7 @@
                                                   (vec3 translation))
                                          :geometry (scene-geometry-box (draw-options-default :color (subseq rgba 0 3)
                                                                                              :alpha (elt rgba 3))
-                                                                       (scene-box size))))))
+                                                                        size)))))
                  (parse-mesh (name)
                    (destructuring-bind (vertex-count face-count)
                        (int-line)