Merge remote-tracking branch 'origin/3d-map-pointcloud' into ui

.gitignore

@@ -17,3 +17,6 @@ __pycache__/
 
 # Ignore files related to your dev environment
 .vscode/
+
+# generated map database by rtabmap
+*.db

src/rove_bringup/launch/autonomy.launch.py

@@ -6,7 +6,7 @@
 
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
-from launch.conditions import IfCondition
+from launch.conditions import IfCondition, UnlessCondition
 
 
 def generate_launch_description():
@@ -16,6 +16,8 @@ def generate_launch_description():
     slam_pkg_path = get_package_share_directory("slam_toolbox")  
 
     use_slam3d = LaunchConfiguration('use_slam3d')
+    use_sim_time = LaunchConfiguration('use_sim_time')
+
 
     slam = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
@@ -27,14 +29,15 @@ def generate_launch_description():
                 pkg_rove_slam, "config", "slam_config.yaml"
             )
         }.items(),
+        condition=UnlessCondition(use_slam3d)
     )
 
     slam3d = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
-            os.path.join(pkg_rove_slam, "launch", "slam3d.launch.py"),
+            os.path.join(pkg_rove_slam, "launch", "slam3d_full.launch.py"),
         ),
         launch_arguments={
-            "use_sim_time": "true",
+            "use_sim_time": use_sim_time,
             "deskewing": "true",
         }.items(),
         condition=IfCondition(use_slam3d)
@@ -43,10 +46,12 @@ def generate_launch_description():
     nav = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             os.path.join(pkg_rove_nav, "navigation.launch.py"),
-        )
+        ),
+        launch_arguments={
+            "use_sim_time": use_sim_time,
+        }.items()
     )
 
-
     return LaunchDescription([
             slam,
             slam3d,

src/rove_bringup/launch/common.launch.py

@@ -94,7 +94,7 @@ def generate_launch_description():
         launch_arguments={
             'use_sim_time': use_sim_time,
             "deskewing": "true",
-            "use_slam3d": "false",
+            "use_slam3d": "true",
         }.items(),
     )
 

src/rove_bringup/launch/real.launch.py

@@ -14,6 +14,7 @@ def generate_launch_description():
     # Get the launch directory
     pkg_rove_bringup = get_package_share_directory('rove_bringup')
     pkg_rove_description = get_package_share_directory('rove_description')
+    pkg_rove_zed = get_package_share_directory('rove_zed')
 
     # Get the URDF file
     urdf_path = os.path.join(pkg_rove_description, 'urdf', 'rove.urdf.xacro')
@@ -120,11 +121,18 @@ def create_controller_node(node_name:str):
         ),
     )
 
+    zed = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_rove_zed, 'launch', 'zed_mapping.launch.py'),
+        )
+    )
+
     return LaunchDescription([
             control_node,
             common,
             joint_state_broadcaster_spawner,
             *delayed_controller_nodes,
             vectornav,
             velodyne,
+            zed,
             ])

src/rove_bringup/launch/test.launch.py

@@ -101,8 +101,8 @@ def generate_launch_description():
             # robot_state_publisher,
             robot_localization_node_local,
             robot_localization_node_global,
-            navsat_transform,
+            # navsat_transform,
             vectornav,
-            velodyne,
+            #velodyne,
             rviz,
             ])

src/rove_description/config/basic.rviz

@@ -3,7 +3,9 @@ Panels:
     Help Height: 78
     Name: Displays
     Property Tree Widget:
-      Expanded: ~
+      Expanded:
+        - /Global Options1
+        - /DepthCloud1/Auto Size1
       Splitter Ratio: 0.5
     Tree Height: 517
   - Class: rviz_common/Selection
@@ -50,6 +52,11 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
+        camera_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
         flipper_fl:
           Alpha: 1
           Show Axes: false
@@ -152,9 +159,9 @@ Visualization Manager:
       Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 125
+      Max Intensity: 0
       Min Color: 0; 0; 0
-      Min Intensity: 1
+      Min Intensity: 0
       Name: LaserScan
       Position Transformer: XYZ
       Selectable: true
@@ -178,6 +185,20 @@ Visualization Manager:
         All Enabled: false
         base_link:
           Value: true
+        camera_link:
+          Value: true
+        flipper_fl:
+          Value: true
+        flipper_fr:
+          Value: true
+        flipper_rl:
+          Value: true
+        flipper_rr:
+          Value: true
+        map:
+          Value: true
+        odom:
+          Value: true
         sensor_link:
           Value: true
         track_fl:
@@ -202,22 +223,33 @@ Visualization Manager:
       Show Axes: true
       Show Names: false
       Tree:
-        base_link:
-          sensor_link:
-            {}
-          track_l:
-            track_fl:
-              {}
-            track_rl:
-              {}
-          track_r:
-            track_fr:
-              {}
-            track_rr:
-              {}
-          velodyne_link:
-            velodyne_laser:
-              {}
+        map:
+          odom:
+            base_link:
+              flipper_fl:
+                {}
+              flipper_fr:
+                {}
+              flipper_rl:
+                {}
+              flipper_rr:
+                {}
+              sensor_link:
+                camera_link:
+                  {}
+              track_l:
+                track_fl:
+                  {}
+                track_rl:
+                  {}
+              track_r:
+                track_fr:
+                  {}
+                track_rr:
+                  {}
+              velodyne_link:
+                velodyne_laser:
+                  {}
       Update Interval: 0
       Value: true
     - Alpha: 0.699999988079071
@@ -306,7 +338,7 @@ Visualization Manager:
         Filter size: 10
         History Policy: Keep Last
         Reliability Policy: Reliable
-        Value: /odometry/filtered
+        Value: /odometry/local
       Value: true
     - Acceleration properties:
         Acc. vector alpha: 1
@@ -336,10 +368,93 @@ Visualization Manager:
         Value: /imu
       Value: true
       fixed_frame_orientation: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rtabmap_rviz_plugins/MapCloud
+      Cloud decimation: 4
+      Cloud from scan: false
+      Cloud max depth (m): 4
+      Cloud min depth (m): 0
+      Cloud voxel size (m): 0.10000000149011612
+      Color: 255; 255; 255
+      Color Transformer: RGB8
+      Download graph: false
+      Download map: false
+      Download namespace: rtabmap
+      Enabled: true
+      Filter ceiling (m): 10
+      Filter floor (m): 0
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: MapCloud
+      Node filtering angle (degrees): 30
+      Node filtering radius (m): 0
+      Position Transformer: XYZ
+      Size (Pixels): 3
+      Size (m): 0.10000000149011612
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /mapData
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Auto Size:
+        Auto Size Factor: 1
+        Value: true
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/DepthCloud
+      Color: 255; 255; 255
+      Color Image Topic: /zed/zed_node/rgb/image_rect_color
+      Color Transformer: RGB8
+      Color Transport Hint: raw
+      Decay Time: 0
+      Depth Map Topic: /zed/zed_node/depth/depth_registered
+      Depth Map Transport Hint: raw
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: DepthCloud
+      Occlusion Compensation:
+        Occlusion Time-Out: 30
+        Value: false
+      Position Transformer: XYZ
+      Queue Size: 5
+      Reliability Policy: Best effort
+      Selectable: true
+      Size (Pixels): 3
+      Style: Flat Squares
+      Topic Filter: true
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
-    Fixed Frame: base_link
+    Fixed Frame: map
     Frame Rate: 30
   Name: root
   Tools:
@@ -382,25 +497,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 7.311348915100098
+      Distance: 11.600849151611328
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -0.7862069010734558
-        Y: 0.829606294631958
-        Z: -1.3278541564941406
+        X: -1.209733486175537
+        Y: -1.5692439079284668
+        Z: 2.5361857414245605
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 1.0553982257843018
+      Pitch: 0.9553995728492737
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 4.913582801818848
+      Yaw: 3.6536216735839844
     Saved: ~
 Window Geometry:
   Displays:

src/rove_description/config/default_bridge.yaml

@@ -53,3 +53,24 @@
   ros_type_name: "sensor_msgs/msg/NavSatFix"
   gz_type_name: "ignition.msgs.NavSat"
   direction: GZ_TO_ROS
+
+# gz topic published by camera (rgb)
+- ros_topic_name: "/zed/zed_node/rgb/image_rect_color"
+  gz_topic_name: "/camera"
+  ros_type_name: "sensor_msgs/msg/Image"
+  gz_type_name: "gz.msgs.Image"
+  direction: GZ_TO_ROS
+
+# gz topic published by camera (depth)
+- ros_topic_name: "/zed/zed_node/depth/depth_registered"
+  gz_topic_name: "/depth_camera"
+  ros_type_name: "sensor_msgs/msg/Image"
+  gz_type_name: "gz.msgs.Image"
+  direction: GZ_TO_ROS
+
+# gz topic published by camera (info)
+- ros_topic_name: "/zed/zed_node/depth/camera_info"
+  gz_topic_name: "/camera_info"
+  ros_type_name: "sensor_msgs/msg/CameraInfo"
+  gz_type_name: "gz.msgs.CameraInfo"
+  direction: GZ_TO_ROS