From 9f590e06159c278846cc97af3913bd87b3b628da Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Fri, 7 Aug 2020 03:39:19 +0900 Subject: [PATCH] evaluate all bodies only when it has bodies --- pr2eus_moveit/euslisp/collision-object-publisher.l | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/pr2eus_moveit/euslisp/collision-object-publisher.l b/pr2eus_moveit/euslisp/collision-object-publisher.l index a9f9dba36..52342e82d 100644 --- a/pr2eus_moveit/euslisp/collision-object-publisher.l +++ b/pr2eus_moveit/euslisp/collision-object-publisher.l @@ -105,7 +105,8 @@ (let ((org-cds (send obj :copy-worldcoords))) (send obj :reset-coords) (send obj :worldcoords) - (mapcar #'(lambda (body) (send body :worldcoords)) (send obj :bodies)) + (when (find-method obj :bodies) + (mapcar #'(lambda (body) (send body :worldcoords)) (send obj :bodies))) (let ((fs (body-to-faces obj)) (geom (instance shape_msgs::mesh :init)) idx-lst vertices (cntr 0))