Create simple object detector node as unique passthrough filter

frames.gv

@@ -0,0 +1,25 @@
+digraph G {
+"base_link" -> "base_inertia"[label="Broadcaster: /control/smb_robot_state_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 2862.910 sec old)\nBuffer length: 0.000 sec\n"];
+"odom" -> "base_link"[label="Broadcaster: /ekf_se\nAverage rate: 30.612 Hz\nMost recent transform: 2862.872 ( 0.038 sec old)\nBuffer length: 1.960 sec\n"];
+"imu_link" -> "imu"[label="Broadcaster: /control/smb_robot_state_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 2862.910 sec old)\nBuffer length: 0.000 sec\n"];
+"base_link" -> "imu_link"[label="Broadcaster: /control/smb_robot_state_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 2862.910 sec old)\nBuffer length: 0.000 sec\n"];
+"base_link" -> "lidar_mount_link"[label="Broadcaster: /control/smb_robot_state_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 2862.910 sec old)\nBuffer length: 0.000 sec\n"];
+"base_link" -> "rgb_camera_link"[label="Broadcaster: /control/smb_robot_state_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 2862.910 sec old)\nBuffer length: 0.000 sec\n"];
+"rgb_camera_link" -> "rgb_camera_optical_link"[label="Broadcaster: /control/smb_robot_state_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 2862.910 sec old)\nBuffer length: 0.000 sec\n"];
+"lidar_mount_link" -> "rslidar_base_link"[label="Broadcaster: /control/smb_robot_state_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 2862.910 sec old)\nBuffer length: 0.000 sec\n"];
+"rslidar_base_link" -> "rslidar"[label="Broadcaster: /control/smb_robot_state_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 2862.910 sec old)\nBuffer length: 0.000 sec\n"];
+"base_link" -> "top"[label="Broadcaster: /control/smb_robot_state_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 2862.910 sec old)\nBuffer length: 0.000 sec\n"];
+"odom_graph_msf" -> "world_graph_msf"[label="Broadcaster: /smb_estimator_node\nAverage rate: 386.500 Hz\nMost recent transform: 2862.902 ( 0.008 sec old)\nBuffer length: 2.000 sec\n"];
+"base_link" -> "odom_graph_msf"[label="Broadcaster: /smb_estimator_node\nAverage rate: 386.114 Hz\nMost recent transform: 2862.905 ( 0.005 sec old)\nBuffer length: 2.002 sec\n"];
+"base_link" -> "LF_WHEEL"[label="Broadcaster: /control/smb_robot_state_publisher\nAverage rate: 50.505 Hz\nMost recent transform: 2862.891 ( 0.019 sec old)\nBuffer length: 1.980 sec\n"];
+"base_link" -> "LH_WHEEL"[label="Broadcaster: /control/smb_robot_state_publisher\nAverage rate: 50.505 Hz\nMost recent transform: 2862.891 ( 0.019 sec old)\nBuffer length: 1.980 sec\n"];
+"base_link" -> "RF_WHEEL"[label="Broadcaster: /control/smb_robot_state_publisher\nAverage rate: 50.505 Hz\nMost recent transform: 2862.891 ( 0.019 sec old)\nBuffer length: 1.980 sec\n"];
+"base_link" -> "RH_WHEEL"[label="Broadcaster: /control/smb_robot_state_publisher\nAverage rate: 50.505 Hz\nMost recent transform: 2862.891 ( 0.019 sec old)\nBuffer length: 1.980 sec\n"];
+"world_graph_msf" -> "map_o3d_graph_msf_aligned"[label="Broadcaster: /smb_estimator_node\nAverage rate: 13.193 Hz\nMost recent transform: 2862.750 ( 0.160 sec old)\nBuffer length: 1.895 sec\n"];
+"map_o3d" -> "raw_rs_o3d"[label="Broadcaster: /mapping\nAverage rate: 10.582 Hz\nMost recent transform: 2862.855 ( 0.055 sec old)\nBuffer length: 1.890 sec\n"];
+"rslidar" -> "map_o3d"[label="Broadcaster: /mapping\nAverage rate: 10.582 Hz\nMost recent transform: 2862.855 ( 0.055 sec old)\nBuffer length: 1.890 sec\n"];
+edge [style=invis];
+ subgraph cluster_legend { style=bold; color=black; label ="view_frames Result";
+"Recorded at time: 2862.910"[ shape=plaintext ] ;
+ }->"odom";
+}

record_sensors.sh

@@ -25,4 +25,6 @@ rosbag record --output-name=${outpath}/${now}"_smb" \
 /imu \
 /rslidar/points  \
 /tf \
-/tf_static
+/tf_static \
+/clock \
+/object_detector/detection_info

smb_msf_graph/launch/smb_msf_graph.launch

@@ -25,7 +25,7 @@
         <include file="$(find open3d_slam_ros)/launch/development.launch" if="$(arg launch_o3d_slam)">
             <arg name="odometry_topic" default="/graph_msf/est_odometry_odom_imu"/>
             <arg name="assumed_external_odometry_tracked_frame" default="imu"/>
-            <arg name="parameter_folder_path" default="$(find open3d_slam_ros)/param/"/>
+            <arg name="parameter_folder_path" default="$(find team6_stack)/param/msf_graph"/>
             <arg name="parameter_filename" default="$(arg parameter_filename)"/>
             <arg name="map_saving_folder" default="$(arg map_saving_folder)"/>
             <arg name="use_sim_time" value="$(arg use_sim_time)" />

smb_opc/launch/opc_rviz.launch

@@ -0,0 +1,5 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <node name="$(anon rviz)" pkg="rviz" type="rviz" args="-d $(find smb_opc)/rviz/smb_vis.rviz"
+    output="screen"/>
+</launch>

smb_opc/rviz/smb_vis.rviz

@@ -15,8 +15,10 @@ Panels:
         - /FarPlanner1/GoalPoint1/Namespaces1
         - /FarPlanner1/GlobalGraph1
         - /TarePlanner1
+        - /PointCloud21
+        - /PointCloud22
       Splitter Ratio: 0.5111420750617981
-    Tree Height: 770
+    Tree Height: 777
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -85,8 +87,8 @@ Visualization Manager:
             - Alpha: 1
               Autocompute Intensity Bounds: true
               Autocompute Value Bounds:
-                Max Value: 1.5905910730361938
-                Min Value: -1.1293858289718628
+                Max Value: 2.158384323120117
+                Min Value: -1.0142515897750854
                 Value: true
               Axis: Z
               Channel Name: intensity
@@ -145,12 +147,10 @@ Visualization Manager:
                   Value: true
                 map_o3d_graph_msf_aligned:
                   Value: true
-                map_o3d_viz:
+                odom:
                   Value: true
                 odom_graph_msf:
                   Value: true
-                odom_o3d:
-                  Value: true
                 raw_rs_o3d:
                   Value: true
                 rgb_camera_link:
@@ -178,46 +178,43 @@ Visualization Manager:
               Show Axes: true
               Show Names: true
               Tree:
-                base_link:
-                  LF_WHEEL:
-                    {}
-                  LH_WHEEL:
-                    {}
-                  RF_WHEEL:
-                    {}
-                  RH_WHEEL:
-                    {}
-                  base_inertia:
-                    {}
-                  imu_link:
-                    imu:
+                odom:
+                  base_link:
+                    LF_WHEEL:
                       {}
-                  lidar_mount_link:
-                    rslidar_base_link:
-                      rslidar:
-                        map_o3d:
-                          map_o3d_viz:
-                            {}
-                          raw_rs_o3d:
-                            {}
-                        odom_o3d:
-                          {}
-                        sensor:
-                          camera:
-                            {}
-                          vehicle:
-                            {}
-                  odom_graph_msf:
-                    world_graph_msf:
-                      map_o3d_graph_msf_aligned:
+                    LH_WHEEL:
+                      {}
+                    RF_WHEEL:
+                      {}
+                    RH_WHEEL:
+                      {}
+                    base_inertia:
+                      {}
+                    imu_link:
+                      imu:
                         {}
-                      sensor_at_scan:
+                    lidar_mount_link:
+                      rslidar_base_link:
+                        rslidar:
+                          map_o3d:
+                            raw_rs_o3d:
+                              {}
+                          sensor:
+                            camera:
+                              {}
+                            vehicle:
+                              {}
+                    odom_graph_msf:
+                      world_graph_msf:
+                        map_o3d_graph_msf_aligned:
+                          {}
+                        sensor_at_scan:
+                          {}
+                    rgb_camera_link:
+                      rgb_camera_optical_link:
                         {}
-                  rgb_camera_link:
-                    rgb_camera_optical_link:
+                    top:
                       {}
-                  top:
-                    {}
               Update Interval: 0
               Value: true
             - Alpha: 1
@@ -926,15 +923,15 @@ Visualization Manager:
           Marker Topic: /sensor_coverage_planner/tare_visualizer/local_planning_horizon
           Name: LocalPlanningHorizon
           Namespaces:
-            "": true
+            {}
           Queue Size: 100
           Value: true
         - Class: rviz/Marker
           Enabled: true
           Marker Topic: /sensor_coverage_planner/tare_visualizer/exploring_subspaces
           Name: ExplorationSubspaces
           Namespaces:
-            "": true
+            {}
           Queue Size: 100
           Value: true
         - Alpha: 1
@@ -1017,13 +1014,69 @@ Visualization Manager:
           Use Fixed Frame: true
           Use rainbow: true
           Value: true
-      Enabled: true
+      Enabled: false
       Name: TarePlanner
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 0; 255; 0
+      Min Color: 255; 255; 255
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.30000001192092896
+      Style: Boxes
+      Topic: /sensor_coverage_planner/viewpoint_vis_cloud
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: false
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: RGB8
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.20000000298023224
+      Style: Flat Squares
+      Topic: /sensor_coverage_planner/planner_cloud
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
     Default Light: true
-    Fixed Frame: base_link
+    Fixed Frame: world_graph_msf
     Frame Rate: 30
   Name: root
   Tools:
@@ -1051,25 +1104,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 46.70221710205078
+      Distance: 101.05590057373047
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Field of View: 0.7853981852531433
       Focal Point:
-        X: 1.7170408964157104
-        Y: -3.2570488452911377
-        Z: 8.846016883850098
+        X: -0.06670406460762024
+        Y: 12.508928298950195
+        Z: 2.5416529178619385
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.7397965788841248
+      Pitch: 0.9747965335845947
       Target Frame: <Fixed Frame>
-      Yaw: 3.385552406311035
+      Yaw: 3.390552043914795
     Saved: ~
 Window Geometry:
   ControlPanel:
@@ -1081,7 +1134,7 @@ Window Geometry:
   Hide Right Dock: true
   Image bounding Boxes:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000002d000000542fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000003d000003a2000000fb0100001cfa000000010100000002fb000000180043006f006e00740072006f006c00500061006e0065006c0100000000ffffffff000000d200fffffffb000000100044006900730070006c0061007900730100000000000001ea0000015600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001a0053004d00420050006f007700650072004d006f0074006f007201000003e5000000b70000007300fffffffb0000001e0053004d00420050006f007700650072005000610079006c006f0061006401000004a2000000dd0000007300ffffff00000001000001c3000003b9fc0200000005fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003b9000000a400fffffffb0000001200520061007700200049006d00610067006500000003a20000019a0000001600fffffffb000000280049006d00610067006500200062006f0075006e00640069006e006700200042006f007800650073000000032b000002110000001600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000070f0000005efc0100000002fb0000000800540069006d006501000000000000070f000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004390000054200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000002d000000546fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000003b000003a5000000ed0100001afa000000010100000002fb000000180043006f006e00740072006f006c00500061006e0065006c0100000000ffffffff000000bf00fffffffb000000100044006900730070006c0061007900730100000000000001ea0000015600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001a0053004d00420050006f007700650072004d006f0074006f007201000003e6000000b70000006d00fffffffb0000001e0053004d00420050006f007700650072005000610079006c006f0061006401000004a3000000de0000006d00ffffff00000001000001c3000003b9fc0200000005fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003b9000000a000fffffffb0000001200520061007700200049006d00610067006500000003a20000019a0000001600fffffffb000000280049006d00610067006500200062006f0075006e00640069006e006700200042006f007800650073000000032b000002110000001600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000070f0000005efc0100000002fb0000000800540069006d006501000000000000070f0000030700fffffffb0000000800540069006d00650100000000000004500000000000000000000004390000054600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Raw Image:
     collapsed: false
   SMBPowerMotor:
@@ -1097,5 +1150,5 @@ Window Geometry:
   Views:
     collapsed: true
   Width: 1807
-  X: 0
-  Y: 27
+  X: -1
+  Y: 3