added rviz config file to global fusion node

global_fusion/launch/display.rviz

@@ -0,0 +1,207 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Marker1
+        - /Marker1/Status1
+        - /Path1
+        - /Camera1
+      Splitter Ratio: 0.5
+    Tree Height: 322
+  - 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: Camera
+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: <Fixed Frame>
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        cam0:
+          Value: true
+        cam0_gps:
+          Value: true
+        cam1:
+          Value: true
+        global:
+          Value: true
+        gps_global:
+          Value: true
+        imu:
+          Value: true
+        imu_gps:
+          Value: true
+      Marker Alpha: 1
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        gps_global:
+          imu_gps:
+            cam0_gps:
+              {}
+      Update Interval: 0
+      Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /ov_msckf/vis_markers
+      Name: Marker
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 25; 255; 0
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Lines
+      Line Width: 0.029999999329447746
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Queue Size: 10
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic: /global_fusion_node/global_path
+      Unreliable: false
+      Value: true
+    - Class: rviz/Camera
+      Enabled: true
+      Image Rendering: background and overlay
+      Image Topic: /global_fusion_node/sensor_left
+      Name: Camera
+      Overlay Alpha: 0.5
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: true
+      Visibility:
+        Grid: true
+        Marker: true
+        Path: true
+        TF: true
+        Value: true
+      Zoom Factor: 1
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: gps_global
+    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: 1631.015869140625
+      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: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 1.0853980779647827
+      Target Frame: <Fixed Frame>
+      Yaw: 5.583582878112793
+    Saved: ~
+Window Geometry:
+  Camera:
+    collapsed: false
+  Displays:
+    collapsed: false
+  Height: 1016
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000003ae00000358fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e000001ce000000ca00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000212000001840000001600ffffff000000010000010f00000358fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003e00000358000000a600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000027e00fffffffb0000000800540069006d00650100000000000004500000000000000000000003cc0000035800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1920
+  X: 0
+  Y: 27

ov_msckf/launch/display.rviz

@@ -6,8 +6,9 @@ Panels:
       Expanded:
         - /Camera1
         - /Camera1/Status1
+        - /Marker1
       Splitter Ratio: 0.6011396050453186
-    Tree Height: 345
+    Tree Height: 452
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -26,7 +27,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: ""
+    SyncSource: SLAM Points
 Preferences:
   PromptSaveOnExit: true
 Toolbars:
@@ -52,21 +53,31 @@ Visualization Manager:
       Plane Cell Count: 80
       Reference Frame: <Fixed Frame>
       Value: true
-    - Class: rviz_camera_stream/CameraPub
+    - Background Color: 0; 0; 0
+      Camera Info Topic: /ov_msckf/camera0_info
+      Class: rviz_camera_stream/CameraPub
+      Display namespace: ""
       Enabled: true
       Frame Rate: 10
+      Image Encoding: rgb8
       Image Topic: /ov_msckf/rendered
-      Camera Info Topic: /ov_msckf/camera0_info
       Name: CameraPub1
+      Near Clip Distance: 0.009999999776482582
       Queue Size: 2
       Value: true
       Visibility:
-        Axes: true
+        ACTIVE Features: true
-        CameraPub2: true
+        ARUCO Points: true
+        Camera: true
+        Current Depths: true
         Grid: true
-        Image1: true
+        Image Tracks: true
-        Image2: true
+        MSCKF Points: true
-        TF: true
+        Marker: true
+        Path GT: true
+        Path VIO: true
+        SIM Points: true
+        SLAM Points: true
         Value: true
     - Class: rviz/Image
       Enabled: true
@@ -81,7 +92,7 @@ Visualization Manager:
       Unreliable: false
       Value: true
     - Class: rviz/Image
-      Enabled: true
+      Enabled: false
       Image Topic: /ov_msckf/loop_depth_colored
       Max Value: 1
       Median window: 5
@@ -91,7 +102,7 @@ Visualization Manager:
       Queue Size: 2
       Transport Hint: compressed
       Unreliable: false
-      Value: true
+      Value: false
     - Alpha: 1
       Buffer Length: 1
       Class: rviz/Path
@@ -281,7 +292,6 @@ Visualization Manager:
       Use rainbow: true
       Value: true
     - Class: rviz/Camera
-      Camera Info Topic: /ov_msckf/camera_info
       Enabled: true
       Image Rendering: background and overlay
       Image Topic: /ov_msckf/trackhist
@@ -294,20 +304,29 @@ Visualization Manager:
       Visibility:
         ACTIVE Features: true
         ARUCO Points: true
+        CameraPub1: true
         Current Depths: true
         Grid: true
         Image Tracks: true
         MSCKF Points: true
+        Marker: true
         Path GT: true
         Path VIO: true
         SIM Points: true
         SLAM Points: true
-        TF: true
         Value: true
       Zoom Factor: 1
     - Class: rviz/Marker
       Enabled: true
-      Marker Topic: /ov_msckf/vis_markers
+      Marker Topic: /ov_msckf/vio_vis_markers
+      Name: Marker
+      Namespaces:
+        gps_namespace: true
+      Queue Size: 100
+      Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: visualization_marker
       Name: Marker
       Namespaces:
         {}
@@ -341,7 +360,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 12.363367080688477
+      Distance: 5925.900390625
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -373,7 +392,7 @@ Window Geometry:
   Hide Right Dock: true
   Image Tracks:
     collapsed: false
-  QMainWindow State: 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
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Time:

ov_msckf/src/render_cubes.cpp

@@ -288,16 +288,16 @@ std::vector<std::vector<double>> cashedCubeVisPoints;
 
 ros::Publisher pub_cubes, pub_cubes_gps, pub_aligned_traj, pub_sensor_left_gps;
 ros::Subscriber sub_imu, subs_T_align, sub_stereo_left;
-visualization_msgs::Marker Marker;
+visualization_msgs::Marker Marker, Marker_gps;
 std::ofstream fT_aligned;
 nav_msgs::Path curr_path;
 
 void callback_imu(const sensor_msgs::Imu::ConstPtr &msg){
     ROS_INFO_STREAM("received imu");
-    pub_cubes_gps.publish(Marker);
+    if (pub_cubes.getNumSubscribers() != 0 || pub_cubes_gps.getNumSubscribers() != 0){
-    Marker.header.frame_id = "global";
+        pub_cubes_gps.publish(Marker_gps);
         pub_cubes.publish(Marker);
-
+    }
 }
 
 void T_align_callback(const geometry_msgs::TransformStamped &msg){
@@ -434,5 +434,33 @@ int main(int argc, char* argv[]){
         Marker.points.push_back(tempPoint);
     }
 
+    Marker_gps.header.frame_id = "global";
+    Marker_gps.header.stamp = ros::Time();
+    Marker_gps.ns = "gps_namespace";
+    Marker_gps.id = 1;
+    Marker_gps.type = visualization_msgs::Marker::CUBE_LIST;
+    Marker_gps.action = visualization_msgs::Marker::ADD;
+    Marker_gps.pose.orientation.x = 0.0;
+    Marker_gps.pose.orientation.y = 0.0;
+    Marker_gps.pose.orientation.z = 0.0;
+    Marker_gps.pose.orientation.w = 1.0;
+    Marker_gps.scale.x = 6.8f;
+    Marker_gps.scale.y = 6.8f;
+    Marker_gps.scale.z = 6.8f;
+    Marker_gps.color.a = 1.0; // Don't forget to set the alpha!
+    Marker_gps.color.r = 0.0;
+    Marker_gps.color.g = 1.0;
+    Marker_gps.color.b = 0.0;
+//only if using a MESH_RESOURCE marker type:
+    Marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
+
+    for (auto & cubeVisPoint : cubeVisPoints){
+        geometry_msgs::Point tempPoint;
+        tempPoint.x = cubeVisPoint[0];
+        tempPoint.y = cubeVisPoint[1];
+        tempPoint.z = cubeVisPoint[2];
+        Marker_gps.points.push_back(tempPoint);
+    }
+
     ros::spin();
 }