From 14fcf908b022c7f8af1477da234c6c32b22263c6 Mon Sep 17 00:00:00 2001
From: Kei Okada <k-okada@jsk.t.u-tokyo.ac.jp>
Date: Wed, 17 Nov 2021 19:36:06 +0900
Subject: [PATCH 1/2] add test code for (pr2) and (instance pr2-robot :init)
 (instance r2-sensor-robot :init)

---
 pr2eus/test/pr2-ri-test-simple.l | 47 ++++++++++++++++++++++++++++++++
 1 file changed, 47 insertions(+)

diff --git a/pr2eus/test/pr2-ri-test-simple.l b/pr2eus/test/pr2-ri-test-simple.l
index 3d574574..e4923fb7 100644
--- a/pr2eus/test/pr2-ri-test-simple.l
+++ b/pr2eus/test/pr2-ri-test-simple.l
@@ -158,6 +158,53 @@
       )
     ))
 
+;; pr2 / pr2-sensor-robot test
+(deftest instantiate-pr2-sensor-robot-test
+  (let (rgb-camera-pr1012 rgb-camera-pr1040)
+    ;; call from function
+    (pr2)
+    (assert *pr2*) ;; generate *pr2*
+    (assert (derivedp *pr2* pr2-sensor-robot)) ;; pr2 derived from pr2-sensor-robot
+    (assert (equal (send *pr2* :name) "pr2")) ;; robot name is pr2
+    (assert (equal (*pr2* . name) :pr1012)) ;; but we can know serial number
+    (setq rgb-camera-1012 (send (send *pr2* :camera :kinect_head/rgb) :viewing :projection))
+
+    (pr2 :pr1040)
+    (assert *pr2*) ;; generated *pr2*
+    (assert (derivedp *pr2* pr2-sensor-robot)) ;; pr2 derived from pr2-sensor-robot
+    (assert (equal (send *pr2* :name) "pr2")) ;; robot name is pr2
+    (assert (equal (*pr2* . name) :pr1040)) ;; but we can know serial number
+    (setq rgb-camera-1040 (send (send *pr2* :camera :kinect_head/rgb) :viewing :projection))
+    (defun m= (m1 m2) (v= (array-entity m1) (array-entity m2)))
+    (when (m= rgb-camera-1012 rgb-camera-1040)
+      (warning-message 1 "each robot should have different camera param~%~A~%~A~%"
+                       rgb-camera-1040 rgb-camera-1040))
+
+    ;; use instance
+    (setq *pr2* (instance pr2-robot :init))
+    (assert *pr2*) ;; generate *pr2*
+    (assert (derivedp *pr2* pr2-robot)) ;; pr2 derived from pr2-sensor-robot
+    (assert (equal (send *pr2* :name) "pr2")) ;; robot name is pr2
+    (assert (null (assoc 'name (send *pr2* :slots)))) ;; but it does not have slot 'name
+
+    (setq *pr2* (instance pr2-sensor-robot :init))
+    (assert *pr2*) ;; generate *pr2*
+    (assert (derivedp *pr2* pr2-robot)) ;; pr2 derived from pr2-sensor-robot
+    (assert (equal (send *pr2* :name) "pr2")) ;; robot name is pr2
+    (assert (equal (*pr2* . name) :pr1012)) ;; but we can know serial number
+
+    (setq *pr2* (instance pr2-sensor-robot :init :pr1012))
+    (assert *pr2*) ;; generated *pr2*
+    (assert (derivedp *pr2* pr2-sensor-robot)) ;; pr2 derived from pr2-sensor-robot
+    (assert (equal (send *pr2* :name) "pr2")) ;; robot name is pr2
+    (assert (equal (*pr2* . name) :pr1012)) ;; but we can know serial number
+
+    (setq *pr2* (instance pr2-sensor-robot :init :pr1040))
+    (assert *pr2*) ;; generated *pr2*
+    (assert (derivedp *pr2* pr2-sensor-robot)) ;; pr2 derived from pr2-sensor-robot
+    (assert (equal (send *pr2* :name) "pr2")) ;; robot name is pr2
+    (assert (equal (*pr2* . name) :pr1040)) ;; but we can know serial number
+    ))
 
 (run-all-tests)
 (exit)

From 3ca09d406b944df50860cea09d9da84f225a51b1 Mon Sep 17 00:00:00 2001
From: Kei Okada <k-okada@jsk.t.u-tokyo.ac.jp>
Date: Wed, 17 Nov 2021 19:37:33 +0900
Subject: [PATCH 2/2] add optional for :init of pr2-sensor-robot :init

---
 pr2eus/make-pr2-model-file.l | 2 +-
 pr2eus/pr2.l                 | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/pr2eus/make-pr2-model-file.l b/pr2eus/make-pr2-model-file.l
index 3f6e050a..8d0b2025 100644
--- a/pr2eus/make-pr2-model-file.l
+++ b/pr2eus/make-pr2-model-file.l
@@ -60,7 +60,7 @@
      (format f "))~%")
      (format f "~%")
      (format f "(defmethod ~A-sensor-robot~%" robot)
-     (format f "  (:init (n &rest args)~%")
+     (format f "  (:init (&optional (n :~A) &rest args)~%" (if (string= (car robot-names) "") "pr2" (car robot-names)))
      (format f "   (send-super* :init args)~%")
      (format f "   (setq name n)~%")
      ;; pr2 specific kinect_head frames
diff --git a/pr2eus/pr2.l b/pr2eus/pr2.l
index 16fbc4a9..179d7740 100644
--- a/pr2eus/pr2.l
+++ b/pr2eus/pr2.l
@@ -12432,7 +12432,7 @@
   :slots (name kinect_head-depth kinect_head-rgb prosilica r_forearm_cam l_forearm_cam wide_stereo-right wide_stereo-left narrow_stereo-right narrow_stereo-left ))
 
 (defmethod pr2-sensor-robot
-  (:init (n &rest args)
+  (:init (&optional (n :pr1012) &rest args)
    (send-super* :init args)
    (setq name n)
    ;; kinect_head frame definition, this data is taken from jsk_pr2_startup kinect_head_launch