add 3d map

clubcapra · Jun 9, 2024 · 02cbba1 · 02cbba1
1 parent 404d289
commit 02cbba1
Show file tree

Hide file tree

Showing 6 changed files with 95 additions and 29 deletions.
diff --git a/src/rove_bringup/launch/autonomy.launch.py b/src/rove_bringup/launch/autonomy.launch.py
@@ -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=IfCondition(use_slam3d)
     )
 
     slam3d = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             os.path.join(pkg_rove_slam, "launch", "slam3d.launch.py"),
         ),
         launch_arguments={
-            "use_sim_time": "true",
+            "use_sim_time": use_sim_time,
             "deskewing": "true",
         }.items(),
         condition=IfCondition(use_slam3d)
@@ -43,12 +46,14 @@ 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,
+            #slam,
             slam3d,
             nav,
             ])
diff --git a/src/rove_bringup/launch/common.launch.py b/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(),
     )
 

diff --git a/src/rove_description/config/basic.rviz b/src/rove_description/config/basic.rviz
@@ -3,7 +3,8 @@ Panels:
     Help Height: 78
     Name: Displays
     Property Tree Widget:
-      Expanded: ~
+      Expanded:
+        - /Global Options1
       Splitter Ratio: 0.5
     Tree Height: 517
   - Class: rviz_common/Selection
@@ -152,9 +153,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 +179,18 @@ Visualization Manager:
         All Enabled: false
         base_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,20 +215,31 @@ 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:
+        map:
+          odom:
+            base_link:
+              flipper_fl:
+                {}
+              flipper_fr:
+                {}
+              flipper_rl:
+                {}
+              flipper_rr:
+                {}
+              sensor_link:
+                {}
+              track_l:
+                track_fl:
+                  {}
+                track_rl:
+                  {}
+              track_r:
+                track_fr:
+                  {}
+                track_rr:
+                  {}
+              velodyne_link:
+                {}
             velodyne_laser:
               {}
       Update Interval: 0
@@ -336,10 +360,44 @@ 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: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 0
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: 3d map
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /assembled_cloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
-    Fixed Frame: base_link
+    Fixed Frame: map
     Frame Rate: 30
   Name: root
   Tools:
@@ -382,7 +440,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 7.311348915100098
+      Distance: 19.558191299438477
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -397,10 +455,10 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 1.0553982257843018
+      Pitch: 0.7503987550735474
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 4.913582801818848
+      Yaw: 4.633583068847656
     Saved: ~
 Window Geometry:
   Displays:

diff --git a/src/rove_description/urdf/gazebo.urdf.xacro b/src/rove_description/urdf/gazebo.urdf.xacro
@@ -29,6 +29,8 @@
 
             <topic>cmd_vel</topic>
             <odom_topic>odom</odom_topic>
+            <frame_id>odom</frame_id>    
+            <child_frame_id>base_link</child_frame_id>
         </plugin>
 
         <plugin 

diff --git a/src/rove_slam/config/ekf.yaml b/src/rove_slam/config/ekf.yaml
@@ -7,10 +7,10 @@ ekf_filter_node_local:
         map_frame: map              
         odom_frame: odom            
         base_link_frame: base_link
-        world_frame: odom           
+        world_frame: odom
 
         odom0: /odom
-        odom0_config: [true,  true,  false,
+        odom0_config: [false,  false,  false,
                        false, false, false,
                        true, true, false,
                        false, false, false,

diff --git a/src/rove_slam/launch/slam3d.launch.py b/src/rove_slam/launch/slam3d.launch.py
@@ -5,6 +5,8 @@
 
 def generate_launch_description():
 
+    camera_model = 'zed2i'
+
     lidar_frame_id = LaunchConfiguration('lidar_frame_id')
 
     use_sim_time = LaunchConfiguration('use_sim_time')
@@ -62,7 +64,6 @@ def generate_launch_description():
                 package='rtabmap_util', executable='point_cloud_assembler', output='screen',
                 parameters=[{
                     'max_clouds': 10,
-                    'fixed_frame_id': '',
                     'use_sim_time': use_sim_time,
                 }],
                 remappings=[