From 82d81345822bb8129a1262084bcc3e3b96260065 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 5 Jul 2022 01:05:45 +0900 Subject: [PATCH 1/4] jsk_maps: add publish_scene.l, node to publish building model as a Marker --- jsk_maps/launch/map_eng2.rviz | 234 +++++++++++++++++++++++++++++++++ jsk_maps/tools/publish_scene.l | 57 ++++++++ 2 files changed, 291 insertions(+) create mode 100644 jsk_maps/launch/map_eng2.rviz create mode 100755 jsk_maps/tools/publish_scene.l diff --git a/jsk_maps/launch/map_eng2.rviz b/jsk_maps/launch/map_eng2.rviz new file mode 100644 index 0000000000..9b2644df49 --- /dev/null +++ b/jsk_maps/launch/map_eng2.rviz @@ -0,0 +1,234 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 555 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + eng2: + Value: true + eng2/1f: + Value: true + eng2/2f: + Value: true + eng2/3f: + Value: true + eng2/7f: + Value: true + eng2/7f/73B2: + Value: true + eng2/8f: + Value: true + map: + Value: true + world: + Value: true + Marker Scale: 10 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + world: + eng2: + eng2/1f: + {} + eng2/2f: + {} + eng2/3f: + {} + eng2/7f: + eng2/7f/73B2: + {} + map: + {} + eng2/8f: + {} + Update Interval: 0 + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map_1F + Topic: /eng2/1f + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map_2F + Topic: /eng2/2f + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map_3F + Topic: /eng2/3f + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map_7F + Topic: /eng2/7f + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map_8F + Topic: /eng2/8f + Unreliable: false + Use Timestamp: false + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /spots_marker_array + Name: Spot + Namespaces: + pin_body: true + pin_head: true + pin_label: true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /scene_marker + Name: Scene + Namespaces: + eng2: true + Queue Size: 100 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 17.5847225189209 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 3.0254404544830322 + Y: 12.41762924194336 + Z: -2.188706636428833 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.6754317283630371 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.052735805511475 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000024400fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1200 + X: 28 + Y: 28 diff --git a/jsk_maps/tools/publish_scene.l b/jsk_maps/tools/publish_scene.l new file mode 100755 index 0000000000..e6ae277d9b --- /dev/null +++ b/jsk_maps/tools/publish_scene.l @@ -0,0 +1,57 @@ +#!/usr/bin/env roseus + +(ros::roseus-add-msgs "jsk_rviz_plugins") + +(defvar *scene* nil) +(defvar *current-map-id* nil) +(defvar *pub-scene-marker-topic* "/scene_marker") +(defvar *tf-listener* nil) +(defvar *scene-name* nil) + +;; TODO object->marker-msg should ignore cascadedcoords in :bodies...(defun object->marker-msg (obj header &key +;; See https://github.com/jsk-ros-pkg/jsk_roseus/pull/713 +(defclass dummy-scene + :super cascaded-coords + :slots (list bodies)) +(defmethod dummy-scene + (:init (b) (send-super :init) (setq bodies b) self) + (:bodies () bodies)) +(defun load-scene () + (let (s) + (load (format nil "package://jsk_maps/src/~A-scene.l" *scene-name*)) + (setq s (funcall (intern (string-upcase (format nil "make-~A-scene" *scene-name*))))) + (setq *scene* (instance dummy-scene :init + (mapcan #'(lambda (b) + (cond ((find-method b :faces) (list b)) + (t nil))) + (send s :bodies)))))) + + +(defun publish-scene (&key (alpha 1.0) (lifetime 0)) + (let* ((stamp (ros::time-now)) + (header (instance std_msgs::header :init :stamp stamp :frame_id "/world")) + current-map-coords scene ) + + (unless (ros::get-topic-publisher *pub-scene-marker-topic*) + (unix:sleep 1)) + ;; publish scene + (setq msg (object->marker-msg *scene* header :ns *scene-name* :id 1 :alpha alpha :lifetime lifetime)) + (ros::publish *pub-scene-marker-topic* msg) + )) + + +(defun main () + (ros::roseus "publish_scene") + + ;; load params + (setq *scene-name* (ros::get-param "~scene" "eng2")) + + (ros::ros-info "Advertise ~A" *pub-scene-marker-topic* ) + (ros::advertise *pub-scene-marker-topic* visualization_msgs::Marker 5 t);; latched true + + (load-scene) + + (publish-scene) + (ros::spin)) + +(main) From f04f20f0c37e0ba027b10490cf07f808b77497fb Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 5 Jul 2022 17:12:40 +0900 Subject: [PATCH 2/4] use marker-array-msg to publish transparent object --- jsk_maps/tools/publish_scene.l | 46 +++++++++++++++++++++------------- 1 file changed, 29 insertions(+), 17 deletions(-) diff --git a/jsk_maps/tools/publish_scene.l b/jsk_maps/tools/publish_scene.l index e6ae277d9b..5e6bc9aa29 100755 --- a/jsk_maps/tools/publish_scene.l +++ b/jsk_maps/tools/publish_scene.l @@ -4,38 +4,50 @@ (defvar *scene* nil) (defvar *current-map-id* nil) -(defvar *pub-scene-marker-topic* "/scene_marker") +(defvar *pub-scene-marker-topic* "/scene_marker_array") (defvar *tf-listener* nil) (defvar *scene-name* nil) ;; TODO object->marker-msg should ignore cascadedcoords in :bodies...(defun object->marker-msg (obj header &key ;; See https://github.com/jsk-ros-pkg/jsk_roseus/pull/713 -(defclass dummy-scene - :super cascaded-coords - :slots (list bodies)) -(defmethod dummy-scene - (:init (b) (send-super :init) (setq bodies b) self) - (:bodies () bodies)) +(defun vector->rgba (cv &optional (alpha 1.0)) + (if (= 4 (length cv)) (setq alpha (elt cv 3))) + (if (vectorp cv) + (instance std_msgs::ColorRGBA :init + :r (elt cv 0) + :g (elt cv 1) + :b (elt cv 2) + :a alpha) + (instance std_msgs::ColorRGBA :init + :r 0 :g 0 :b 0 :a alpha))) +(defun object->marker-array-msg (obj header &rest args) + (let* ((bodies (mapcan #'(lambda (b) + (cond ((find-method b :faces) (list b)) + (t nil))) + (send obj :bodies))) + (b1 (mapcan #'(lambda (b) (let ((c (get b :face-color))) (if (not (and (vectorp c) (= (length c) 4) (< (elt c 3) 1.0))) (list b)))) bodies)) ;; flat color + (b2 (mapcan #'(lambda (b) (let ((c (get b :face-color))) (if (and (vectorp c) (= (length c) 4) (< (elt c 3) 1.0)) (list b)))) bodies))) ;; transparent color + (instance visualization_msgs::MarkerArray :init + :markers + (list (apply #'object->marker-msg (instance bodyset-link :init obj :bodies b1) header :id 1 args) + (apply #'object->marker-msg (instance bodyset-link :init obj :bodies b2) header :id 2 args))))) + (defun load-scene () - (let (s) + (let () (load (format nil "package://jsk_maps/src/~A-scene.l" *scene-name*)) - (setq s (funcall (intern (string-upcase (format nil "make-~A-scene" *scene-name*))))) - (setq *scene* (instance dummy-scene :init - (mapcan #'(lambda (b) - (cond ((find-method b :faces) (list b)) - (t nil))) - (send s :bodies)))))) + (setq *scene* (funcall (intern (string-upcase (format nil "make-~A-scene" *scene-name*))))))) (defun publish-scene (&key (alpha 1.0) (lifetime 0)) (let* ((stamp (ros::time-now)) (header (instance std_msgs::header :init :stamp stamp :frame_id "/world")) - current-map-coords scene ) + current-map-coords scene msg) (unless (ros::get-topic-publisher *pub-scene-marker-topic*) (unix:sleep 1)) ;; publish scene - (setq msg (object->marker-msg *scene* header :ns *scene-name* :id 1 :alpha alpha :lifetime lifetime)) + (dolist (b (send *scene* :bodies)) (if (substringp "/floor-plane" (send b :name)) (send b :set-color #f(0 1 0 0.05)))) ;; force change transparent color for floor + (setq msg (object->marker-array-msg *scene* header :ns *scene-name* :alpha alpha :lifetime lifetime)) (ros::publish *pub-scene-marker-topic* msg) )) @@ -47,7 +59,7 @@ (setq *scene-name* (ros::get-param "~scene" "eng2")) (ros::ros-info "Advertise ~A" *pub-scene-marker-topic* ) - (ros::advertise *pub-scene-marker-topic* visualization_msgs::Marker 5 t);; latched true + (ros::advertise *pub-scene-marker-topic* visualization_msgs::MarkerArray 5 t) ;; latched true (load-scene) From 2f83e4f6b8295b6854c1a121c156c29aa9826850 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 5 Jul 2022 17:13:46 +0900 Subject: [PATCH 3/4] udpate map_eng2.rviz to receive rviz/MarkerArray --- jsk_maps/launch/map_eng2.rviz | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/jsk_maps/launch/map_eng2.rviz b/jsk_maps/launch/map_eng2.rviz index 9b2644df49..1c73976add 100644 --- a/jsk_maps/launch/map_eng2.rviz +++ b/jsk_maps/launch/map_eng2.rviz @@ -158,9 +158,9 @@ Visualization Manager: pin_label: true Queue Size: 100 Value: true - - Class: rviz/Marker + - Class: rviz/MarkerArray Enabled: true - Marker Topic: /scene_marker + Marker Topic: /scene_marker_array Name: Scene Namespaces: eng2: true From 6ba80334de537279133af61773ce3d92f1015286 Mon Sep 17 00:00:00 2001 From: Naoto Tsukamoto Date: Tue, 5 Jul 2022 21:51:03 +0900 Subject: [PATCH 4/4] [jsk_maps] Add publish_scene.l to start_map_eng2.launch to visualize eus model --- jsk_maps/launch/start_map_eng2.launch | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/jsk_maps/launch/start_map_eng2.launch b/jsk_maps/launch/start_map_eng2.launch index d1b1b7d6dc..89af03389f 100644 --- a/jsk_maps/launch/start_map_eng2.launch +++ b/jsk_maps/launch/start_map_eng2.launch @@ -4,6 +4,7 @@ + @@ -31,4 +32,11 @@ + + + +