From 86adf797d0487478a48c760e73f2c0cef0adf84d Mon Sep 17 00:00:00 2001
From: Naoya Yamaguchi <708yamaguchi@gmail.com>
Date: Sun, 12 Apr 2020 20:35:27 +0900
Subject: [PATCH] [pr2eus_moveit] replace code with functions to create
 shape_msgs::SolidPrimitive

---
 .../euslisp/collision-object-publisher.l       | 18 +++---------------
 1 file changed, 3 insertions(+), 15 deletions(-)

diff --git a/pr2eus_moveit/euslisp/collision-object-publisher.l b/pr2eus_moveit/euslisp/collision-object-publisher.l
index a9f9dba36..e7766eed8 100644
--- a/pr2eus_moveit/euslisp/collision-object-publisher.l
+++ b/pr2eus_moveit/euslisp/collision-object-publisher.l
@@ -62,11 +62,7 @@
          (send colobj :primitives
                (append geom
                        (list
-                        (instance shape_msgs::SolidPrimitive
-                                  :init :type shape_msgs::SolidPrimitive::*CYLINDER*
-                                  :dimensions (float-vector
-                                               (/ (height-of-cylinder obj) 1000.0)
-                                               (/ (radius-of-cylinder obj) 1000.0))))))
+                        (cylinder->shape-msg obj))))
          (send colobj :primitive_poses
                (append pose
                        (list
@@ -77,12 +73,7 @@
          (send colobj :primitives
                (append geom
                        (list
-                        (instance shape_msgs::SolidPrimitive
-                                  :init :type shape_msgs::SolidPrimitive::*BOX*
-                                  :dimensions (float-vector
-                                               (/ (elt (send obj :body-type) 1) 1000.0)
-                                               (/ (elt (send obj :body-type) 2) 1000.0)
-                                               (/ (elt (send obj :body-type) 3) 1000.0))))))
+                        (cube->shape-msg obj))))
          (send colobj :primitive_poses
                (append pose
                        (list
@@ -93,10 +84,7 @@
          (send colobj :primitives
                (append geom
                        (list
-                        (instance shape_msgs::SolidPrimitive
-                                  :init :type shape_msgs::SolidPrimitive::*SPHERE*
-                                  :dimensions (float-vector
-                                               (/ (radius-of-sphere obj) 1000.0))))))
+                        (sphere->shape-msg obj))))
          (send colobj :primitive_poses
                (append pose
                        (list