jsk_perception/euslisp/eusmodel_template_gen_utils.l: add :gen-image-…

…feature :gen-object-marker to add imagesift and kalman-filtered-objectdetection-marker.l to autogenerated launch files
jsk-ros-pkg · Jul 19, 2023 · 47948b0 · 47948b0
1 parent 19184d5
commit 47948b0
Show file tree

Hide file tree

Showing 2 changed files with 37 additions and 7 deletions.
diff --git a/jsk_perception/euslisp/eusmodel_template_gen_utils.l b/jsk_perception/euslisp/eusmodel_template_gen_utils.l
@@ -44,7 +44,7 @@
 	(list template-include width height trans)
 	))))
 
-(defun gen-detection-nodes (obj)
+(defun gen-detection-nodes (obj &key remap-args)
   (let (nodes)
     (setq nodes
 	  (mapcar
@@ -59,6 +59,7 @@
 		   (setq quat (matrix2quaternion (send trans :rot)))
 		   (instance rosnode :init "jsk_perception" "point_pose_extractor"
 			     :name (format nil "point_pose_extractor_~a" (send obj :name))
+                             :remap_args remap-args
 			     :params
       (list
        (instance rosparam :init :name "window_name" :value (send obj :name))
@@ -71,7 +72,17 @@
 	   (send obj :faces)))
     (remove nil nodes)))
 
-(defun gen-all-launch (objects launch-file-name &key (ns nil))
+(defun gen-all-launch (objects launch-file-name &key (ns nil)
+                                                  (image-remap-args nil)
+                                                  (gen-image-feature nil)
+                                                  (image-topic "image")
+                                                  (image-topic-transport "raw")
+                                                  (image-feature-pkg "imagesift") (image-feature-type "imagesift")
+                                                  (gen-object-marker nil)
+                                                  (objectdetection-remap-args nil)
+                                                  (object-builder "(make-cube 60 60 60)")
+                                                  (object-frame-id "/map")
+                                                  )
   ;; generate launchfile
   (with-open-file
    (launch-f launch-file-name :direction :output)
@@ -84,7 +95,25 @@
 		     (send obj :name)))
        (setq obj-include
              (format nil "$(find ~A)/template/_~x_~A.xml" *pkgname* (system::address obj) (send obj :name)))
-       (setq nodes (gen-detection-nodes obj))
+       (setq nodes (gen-detection-nodes obj :remap-args image-remap-args))
+       (if gen-image-feature
+           (push (instance rosnode :init image-feature-pkg image-feature-type
+                           :name (format nil "~A_~A" image-feature-type (send obj :name))
+                           :remap_args image-remap-args
+                           :params (list (instance rosparam :init :name "image_transport" :value image-topic-transport))
+                           ) nodes))
+       (when gen-object-marker
+         (let* ((aface (car (mapcan #'(lambda (aface) (if (send aface :get :gl-textureimage) (list aface))) (send obj :faces))))
+                (tmpl-info (get-template-info obj aface)))
+           (push (instance rosnode :init "jsk_perception" "kalman-filtered-objectdetection-marker.l"
+                           :name (format nil "objectdetection_marker_~A" (send obj :name))
+                           :remap_args objectdetection-remap-args
+                           :params (list (instance rosparam :init :name "object_builder" :value object-builder)
+                                         (instance rosparam :init :name "frame_id" :value object-frame-id)
+                                         (instance rosparam :init :name "object_width" :value (* (elt tmpl-info 1) 0.001))
+                                         (instance rosparam :init :name "object_height" :value (* (elt tmpl-info 2) 0.001))
+                                         )
+                           ) nodes)))
        (send-all nodes :namespace ns)
        (setq xmllist (append (list "launch" nil)
 			     (send-all nodes :list)))

diff --git a/jsk_perception/euslisp/kalman-filtered-objectdetection-marker.l b/jsk_perception/euslisp/kalman-filtered-objectdetection-marker.l
@@ -6,7 +6,7 @@
 (ros::roseus "marker-publish")
 (ros::roseus-add-msgs "posedetection_msgs")
 
-(setq *map-id* "/map")
+(setq *map-id* (ros::get-param "~frame_id" "/map"))
 (setq *solve-tf* t)
 (setq *object-width* (ros::get-param "~object_width" 0))
 (setq *object-height* (ros::get-param "~object_height" 0))
@@ -17,7 +17,8 @@
 (setq *calc-kalman* t)
 (setq *detect-flag* t)
 (setq *tf-force* nil)
-(setq *target-obj* (make-cube 60 60 60))
+(setq *target-obj-code* (read-from-string (ros::get-param "~object_builder" "(make-cube 60 60 60)")))
+(setq *target-obj* (eval *target-obj-code*))
 (send *target-obj* :reset-coords)
 (setf (get *target-obj* :type) (ros::get-param "~target_type" nil))
 (setq *map-frame-objectdetection* (instance posedetection_msgs::ObjectDetection :init))
@@ -122,7 +123,7 @@
                                   (object-name *map-frame-objectdetection*)
                                   (text-name nil) (text-color nil) (obj-color nil))
   (let ((mf-obj-lst (send object-name :objects))
-        (tmp-tgtobj (make-cube 60 60 60))
+        (tmp-tgtobj (eval *target-obj-code*))
         (tp (get target-obj :type)))
     (when obj-color
       (send tmp-tgtobj :set-color obj-color))
@@ -164,7 +165,7 @@
 
 (defun catch-outlier (target-obj)
   (let ((mf-obj-lst (send *map-frame-objectdetection* :objects))
-        (tmp-tgtobj (make-cube 60 60 60))
+        (tmp-tgtobj (eval *target-obj-code*))
         (marker-life 700)
         (outlierflag nil)
         (tp (get target-obj :type)))