Merge pull request #426 from k-okada/wide_stereo_camera_info

Fix wide_stereo camera model parameters

pr2eus/make-pr2-model-file.l

@@ -1,11 +1,19 @@
-(defun make-pr2-model-file (&key (output-directory (ros::rospack-find "pr2eus")))
-  (let ((robot "pr2") s
-        fname_urdf fname_collada fname_yaml fname_lisp
-	valid-cameras camera-paths)
-    (unless (setq s (ros::get-param "/robot_description"))
-      (ros::ros-error "could not load model file from /robot_description~%")
-      (return-from make-pr2-model-file))
+;; re-define until https://github.com/jsk-ros-pkg/jsk_roseus/pull/629 is merged
+(defun ros::list-param ()
+  "Get the list of all the parameters in the server"
+  (let ((s (piped-fork "rosparam list")) tmp ret)
+    (while (setq tmp (read s nil))
+      (push (format nil "~A" tmp) ret))
+    ret))
 
+(defun get-pr2-names ()
+  ;; get list of rosparam, if /robot_description found, returns (list "")
+  (reverse (mapcan #'(lambda (x) (if (substringp "robot_description" x)
+                                     (if (> (length x) (+ (length "robot_description") 1)) (list (subseq x 1 (- (length x) (length "/robot_description")))) (list ""))))
+                   (ros::list-param))))
+
+(defun make-pr2-robot-model (robot robot-description)
+  (let (fname_urdf fname_collada fname_yaml fname_lisp)
     ;; variable setup
     (setq fname_urdf (format nil "/tmp/~a_~d.urdf" robot (unix::getpid)))
     (setq fname_collada (format nil "/tmp/~a_~d.dae" robot (unix::getpid)))
@@ -16,15 +24,20 @@
     (unless (probe-file fname_lisp)
       (with-open-file
        (f fname_urdf :direction :output)
-       (format f s))
+       (format f robot-description))
 
       (unix::system (format nil "rosrun collada_urdf urdf_to_collada ~A ~A" fname_urdf fname_collada))
       (unix::system (format nil "rosrun euscollada collada2eus ~A ~A ~A" fname_collada fname_yaml fname_lisp))
 
       (warning-message 2 "load model file from parameter server /robot_description to ~A~%" fname_lisp)
       )
+    ))
 
-    ;; camera setup
+(defun make-pr2-sensor-model (robot robot-camera-map)
+  (let (fname_list robot-names all-valid-cameras)
+    (setq robot-names (mapcar #'car robot-camera-map))
+    (setq all-valid-cameras (remove-duplicates (mapcan #'cdr (copy-object robot-camera-map)) :test #'string= :key #'(lambda (x) (cdr (assoc :camera-name x)))))
+    (setq fname_lisp (format nil "/tmp/~a_~d.l" robot (unix::getpid)))
     (with-open-file
      (f fname_lisp :direction :output :if-exists :append)
      (format f ";;~%")
@@ -35,85 +48,120 @@
      (format f ";; make-camera-from-ros-camera-info-aux is defined in roseus since 1.0.1, but to use this code in jskeus, we re-define here~%")
      (format f "~A~%" (nconc (list 'defun 'make-camera-from-ros-camera-info-aux) (cddddr #'make-camera-from-ros-camera-info-aux)))
      (format f ";;~%")
-     (format f "(defun pr2 () (setq *pr2* (instance pr2-sensor-robot :init)))~%")
+     (format f "(defun pr2 (&optional (name :~A)) (setq *pr2* (instance pr2-sensor-robot :init name)))~%" (if (string= (car robot-names) "") "pr2" (car robot-names)))
      (format f "~%")
-     (setq camera-paths
-           (list "narrow_stereo/left"
-                 "narrow_stereo/right"
-                 "wide_stereo/left"
-                 "wide_stereo/right"
-                 "l_forearm_cam"
-                 "r_forearm_cam"
-                 "prosilica"
-		 "kinect_head/rgb"
-		 "kinect_head/depth"
-		 ))
-     (dolist (camera-path camera-paths)
-       (let* ((camera-info (format nil "~A/camera_info" camera-path))
-	      (camera-name (substitute #\- #\/ (string-downcase camera-path)))
-	      (var (intern (string-upcase camera-name)))
-	      (frame-id)
-	      (i 0))
-         (ros::subscribe camera-info sensor_msgs::CameraInfo
-                         #'(lambda (msg)
-                             (set var msg)
-                             var))
-
-         (ros::rate 10)
-         (while (and (ros::ok) (not (boundp var)) (< (incf i) 50))
-           (ros::spin-once)
-           (ros::sleep))
-         (ros::unsubscribe camera-info)
-	 (if (boundp var)
-	     (progn
-	       (setq var (eval var))
-	       (setq frame-id (send var :header :frame_id))
-	       (if (eq (elt frame-id 0) #\/) (setq frame-id (subseq frame-id 1)))
-	       (warning-message 2 "received ~A ~A ~A~%" camera-info var frame-id)
-	       (push (list (cons :camera-path camera-path) (cons :camera-name camera-name) (cons :camera-info camera-info) (cons :frame-id frame-id)) valid-cameras))
-	   (ros::ros-error "could not receive ~A ~A" camera-info var))
-	   ) ;; let
-        ) ;; dolist
+     ;;
      (format f "(defclass ~A-sensor-robot~%" robot)
      (format f "  :super pr2-robot~%")
-     (format f "  :slots (")
-     (dolist (valid-camera valid-cameras)
+     (format f "  :slots (name ")
+     (dolist (valid-camera all-valid-cameras)
        (let* ((camera-name (cdr (assoc :camera-name valid-camera))))
-	 (format f "~A " camera-name)))
+         (format f "~A " camera-name)))
      (format f "))~%")
      (format f "~%")
      (format f "(defmethod ~A-sensor-robot~%" robot)
-     (format f "  (:init (&rest args)~%")
+     (format f "  (:init (n &rest args)~%")
      (format f "   (send-super* :init args)~%")
+     (format f "   (setq name n)~%")
      ;; pr2 specific kinect_head frames
      (format f "   ;; kinect_head frame definition, this data is taken from jsk_pr2_startup kinect_head_launch ~%")
      (format f "#|~%")
      (format f " get frame coordinates data from pr1012:/etc/ros/distro/urdf/robot.xml~%")
      (format f "|#~%")
-     (format f "   ;; define cameras~%")
-     (dolist (valid-camera valid-cameras)
-       (let* ((camera-path (cdr (assoc :camera-path valid-camera)))
-	      (camera-name (cdr (assoc :camera-name valid-camera)))
-	      (frame-id (cdr (assoc :frame-id valid-camera)))
-	      (var (eval (intern (string-upcase camera-name)))))
-	 (format f "   ;; ~A ~A~%" var (send var :P))
-	 (format f "   (setq ~A (make-camera-from-ros-camera-info-aux ~A ~A ~A ~A_lk :name :~A))~%~%" camera-name (send var :width) (send var :height) (send var :p) frame-id camera-path)))
-     ;; :cameras
-     (format f "   (setq cameras (list ")
-     (dolist (valid-camera valid-cameras)
-       (let ((camera-name (cdr (assoc :camera-name valid-camera))))
-	 (format f " (send self :~A)" camera-name)))
-     (format f "))~%")
+     (format f "   (case name~%")
+     (dolist (robot-name robot-names)
+       (setq valid-cameras (cdr (assoc robot-name robot-camera-map :test #'string=)))
+       (format f "     ;; define cameras for ~A~%" robot-name)
+       (format f "     (:~A~%" (if (string= robot-name "") "pr2" robot-name))
+       (dolist (valid-camera valid-cameras)
+         (let* ((camera-path (cdr (assoc :camera-path valid-camera)))
+                (camera-name (cdr (assoc :camera-name valid-camera)))
+                (frame-id (cdr (assoc :frame-id valid-camera)))
+                (var (eval (intern (string-upcase (format nil "~A_~A" robot-name camera-name))))))
+           (format f "      ;; ~A ~A~%" (send (class var) :name) (send var :P))
+           (format f "      (setq ~A (make-camera-from-ros-camera-info-aux ~A ~A ~A ~A_lk :name :~A))~%~%" camera-name (send var :width) (send var :height) (send var :p) frame-id camera-path)))
+       ;; :cameras
+       (format f "      (setq cameras (list ")
+       (dolist (valid-camera valid-cameras)
+         (let ((camera-name (cdr (assoc :camera-name valid-camera))))
+           (format f " (send self :~A)" camera-name)))
+       (format f "))~%")
+       (format f "      ) ;; case ~A~%" robot-name)) ;; ease
+     (format f "   )~%")
      (format f "   self)~%")
      (format f "~%")
      ;; accessor to camera
-     (dolist (valid-camera valid-cameras)
+     (dolist (valid-camera all-valid-cameras)
        (let* ((camera-name (cdr (assoc :camera-name valid-camera))))
 	 (format f "  (:~A (&rest args) (forward-message-to ~A args))~%" camera-name camera-name)))
      (format f "  ) ;; defmethod ~A-robot~%~%~%" robot)
      ) ;; with-open-file
-    (warning-message 1 "copy model file from ~A to ~A/~A.l~%" fname_lisp output-directory robot)
+    ))
+
+(defun make-pr2-model-file (&key (output-directory (ros::rospack-find "pr2eus")))
+  (let ((robot "pr2") robot-description robot-names
+        robot-camera-map camera-paths)
+
+    ;; get robot naems from robot_description
+    (setq robot-names (get-pr2-names))
+    (ros::ros-info "found ~A robots, ~A" (length robot-names) robot-names)
+
+    (unless (setq robot-description (ros::get-param (format nil "/~A/robot_description" (car robot-names))))
+      (ros::ros-error "could not load model file from /robot_description~%")
+      (return-from make-pr2-model-file))
+
+    (make-pr2-robot-model robot robot-description)
+
+    (setq camera-paths
+          (list "narrow_stereo/left"
+                "narrow_stereo/right"
+                "wide_stereo/left"
+                "wide_stereo/right"
+                "l_forearm_cam"
+                "r_forearm_cam"
+                "prosilica"
+                "kinect_head/rgb"
+                "kinect_head/depth"
+                ))
+    (dolist (robot-name robot-names)
+      (let (valid-cameras)
+        (dolist (camera-path camera-paths)
+          (let* ((camera-info (format nil "/~A/~A/camera_info" robot-name camera-path))
+                 (camera-name (substitute #\- #\/ (string-downcase camera-path)))
+                 (var (intern (string-upcase (format nil "~A_~A" robot-name camera-name))))
+                 (frame-id)
+                 (i 0))
+            (ros::subscribe camera-info sensor_msgs::CameraInfo
+                            #'(lambda (msg)
+                                (set var msg)
+                                var))
+            (ros::rate 10)
+            (while (and (ros::ok) (not (boundp var)) (< (incf i) 50))
+              (ros::spin-once)
+              (ros::sleep))
+            (ros::unsubscribe camera-info)
+            (if (boundp var)
+                (progn
+                  (setq var (eval var))
+                  (setq frame-id (send var :header :frame_id))
+                  (if (eq (elt frame-id 0) #\/) (setq frame-id (subseq frame-id 1)))
+                  (warning-message 2 "received ~A ~A ~A~%" camera-info (send var :P) frame-id)
+                  (push (list (cons :camera-path camera-path) (cons :camera-name camera-name) (cons :camera-info camera-info) (cons :frame-id frame-id)) valid-cameras))
+              (ros::ros-error "could not receive ~A ~A" camera-info var))
+            )) ;; dolist (let *
+        (push (cons robot-name valid-cameras) robot-camera-map)
+        ))
+    (setq robot-camera-map (reverse robot-camera-map))
+    (make-pr2-sensor-model robot robot-camera-map)
+
+
+    (warning-message 2 ";;~%")
+    (warning-message 2 ";; copy model file from ~A to ~A/~A.l~%" fname_lisp output-directory robot)
+    (warning-message 2 ";;~%")
     (unix::system (format nil "mv ~A ~A/~A.l" fname_lisp output-directory robot))
+    (warning-message 2 ";;~%")
+    (warning-message 2 ";; successfully generated ~A/~A robot file, now exiting program...~%" output-directory robot)
+    (warning-message 2 ";;~%")
     ))
 
 ;;(ros::roseus "make-pr2-modle-file")

pr2eus/make-pr2-model-file.launch

@@ -0,0 +1,11 @@
+<launch>
+  <!-- launch scriept to generate pr2.l from urdf and sensor message -->
+  <rosparam command="load" file="$(find pr2eus)/test/pr1012_urdf.yaml" param="/pr1012/robot_description"/>
+  <rosparam command="load" file="$(find pr2eus)/test/pr1040_urdf.yaml" param="/pr1040/robot_description"/>
+  <node name="pr1012_camera_info" pkg="rosbag" type="play"
+	    args="--prefix=/pr1012 -l $(find pr2eus)/test/pr1012_camera_info.bag" />
+  <node name="pr1040_camera_info" pkg="rosbag" type="play"
+	    args="--prefix=/pr1040 -l $(find pr2eus)/test/pr1040_camera_info.bag" />
+  <node name="make_pr2_model_file" pkg="roseus" type="roseus" required="true" output="screen"
+        args="$(find pr2eus)/make-pr2-model-file.l &quot;(progn (ros::roseus \&quot;make-pr2-modle-file\&quot;) (make-pr2-model-file) (sys::exit 0))&quot;" />
+</launch>