Merge pull request #640 from 708yamaguchi/shape_msgs

add functions to convert shape_msgs/SolidPrimitive <-> euslisp object

roseus/euslisp/roseus-utils.l

@@ -620,6 +620,75 @@
                                  (send rgba :b)))
              cb)) points)))
     ))
+
+;;
+;; Shape
+;;
+(ros::roseus-add-msgs "shape_msgs")
+
+;; eus shape object -> shape_msgs::SolidPrimitive
+
+(defun cube->shape-msg (cb)
+  "Convert cube object to shape_msgs::SolidPrimitive"
+  (instance shape_msgs::SolidPrimitive
+            :init :type shape_msgs::SolidPrimitive::*BOX*
+            :dimensions (float-vector
+                         (/ (x-of-cube cb) 1000.0)
+                         (/ (y-of-cube cb) 1000.0)
+                         (/ (z-of-cube cb) 1000.0))))
+
+(defun sphere->shape-msg (sp)
+  "Convert sphere object to shape_msgs::SolidPrimitive"
+  (instance shape_msgs::SolidPrimitive
+            :init :type shape_msgs::SolidPrimitive::*SPHERE*
+            :dimensions (float-vector
+                         (/ (radius-of-sphere sp) 1000.0))))
+
+(defun cylinder->shape-msg (cyl)
+  "Convert cylinder object to shape_msgs::SolidPrimitive"
+  (instance shape_msgs::SolidPrimitive
+            :init :type shape_msgs::SolidPrimitive::*CYLINDER*
+            :dimensions (float-vector
+                         (/ (height-of-cylinder cyl) 1000.0)
+                         (/ (radius-of-cylinder cyl) 1000.0))))
+
+;; shape_msgs::SolidPrimitive -> eus shape object
+
+(defun shape-msg->shape (msg)
+  "Convert shape_msgs::SolidPrimitive to euslisp object"
+  (let ((type (send msg :type)))
+    (cond
+     ((eq type shape_msgs::SolidPrimitive::*BOX*)
+      (shape-msg->cube msg))
+     ((eq type shape_msgs::SolidPrimitive::*SPHERE*)
+      (shape-msg->sphere msg))
+     ((eq type shape_msgs::SolidPrimitive::*CYLINDER*)
+      (shape-msg->cylinder msg))
+     (t
+      (error "unknown type ~A" type)))))
+
+(defun shape-msg->cube (msg)
+  "Convert shape_msgs::SolidPrimitive to euslisp cube object"
+  (let* ((scale (send msg :dimensions))
+         (cb (make-cube (* (elt scale 0) 1000)
+                        (* (elt scale 1) 1000)
+                        (* (elt scale 2) 1000))))
+    cb))
+
+(defun shape-msg->sphere (msg)
+  "Convert shape_msgs::SolidPrimitive to euslisp sphere object"
+  (let* ((scale (send msg :dimensions))
+         (cp (make-sphere (* (elt scale 0) 1000))))
+    cp))
+
+(defun shape-msg->cylinder (msg)
+  "Convert shape_msgs::SolidPrimitive to euslisp cylinder object"
+  (let* ((scale (send msg :dimensions))
+         (height (* (elt scale 0) 1000))
+         (radius (* (elt scale 1) 1000))
+         (cyl (make-cylinder radius height)))
+    cyl))
+
 ;;
 ;; for pointcloud
 ;;

roseus/package.xml

@@ -36,6 +36,7 @@
   <build_depend>std_msgs</build_depend>
   <build_depend>std_srvs</build_depend>
   <build_depend>sensor_msgs</build_depend>
+  <build_depend>shape_msgs</build_depend>
   <build_depend>visualization_msgs</build_depend>
   <build_depend>message_generation</build_depend>
   <build_depend>dynamic_reconfigure</build_depend>
@@ -61,6 +62,7 @@
   <run_depend>std_msgs</run_depend>
   <run_depend>std_srvs</run_depend>
   <run_depend>sensor_msgs</run_depend>
+  <run_depend>shape_msgs</run_depend>
   <run_depend>visualization_msgs</run_depend>
   <run_depend>message_runtime</run_depend>
   <run_depend>dynamic_reconfigure</run_depend>

roseus/test/test-roseus.l

@@ -103,6 +103,9 @@
     (assert (marker-msg->shape (cylinder->marker-msg (make-cylinder 100 100) (instance std_msgs::header))))
     (assert (marker-msg->shape (cube->marker-msg (make-cube 100 100 100) (instance std_msgs::header))))
     (assert (marker-msg->shape (sphere->marker-msg (make-sphere 100) (instance std_msgs::header))))
+    (assert (shape-msg->shape (cylinder->shape-msg (make-cylinder 100 100))))
+    (assert (shape-msg->shape (cube->shape-msg (make-cube 100 100 100))))
+    (assert (shape-msg->shape (sphere->shape-msg (make-sphere 100))))
     ;;
     ))