add test code for (pr2) and (instance pr2-robot :init) (instance r2-sensor-robot :init)

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

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 

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)