modification for real hardware

clubcapra · Jun 19, 2024 · 40abdfb · 40abdfb
1 parent d3c0aa6
commit 40abdfb
Show file tree

Hide file tree

Showing 8 changed files with 113 additions and 115 deletions.
diff --git a/src/rove_bringup/launch/autonomy.launch.py b/src/rove_bringup/launch/autonomy.launch.py
@@ -24,7 +24,7 @@ def generate_launch_description():
             os.path.join(slam_pkg_path, "launch", "online_async_launch.py"),
         ),
         launch_arguments={
-            "use_sim_time": "true",
+            "use_sim_time": use_sim_time,
             "slam_params_file": os.path.join(
                 pkg_rove_slam, "config", "slam_config.yaml"
             )
@@ -34,7 +34,7 @@ def generate_launch_description():
 
     slam3d = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
-            os.path.join(pkg_rove_slam, "launch", "slam3d_full.launch.py"),
+            os.path.join(pkg_rove_slam, "launch", "velodyne_3d.launch.py"),
         ),
         launch_arguments={
             "use_sim_time": use_sim_time,

diff --git a/src/rove_bringup/launch/real.launch.py b/src/rove_bringup/launch/real.launch.py
@@ -124,7 +124,7 @@ def create_controller_node(node_name:str):
     zed = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             os.path.join(pkg_rove_zed, 'launch', 'zed_mapping.launch.py'),
-        )
+        ),
     )
 
     return LaunchDescription([

diff --git a/src/rove_description/config/basic.rviz b/src/rove_description/config/basic.rviz
@@ -7,7 +7,7 @@ Panels:
         - /Global Options1
         - /DepthCloud1/Auto Size1
       Splitter Ratio: 0.5
-    Tree Height: 517
+    Tree Height: 325
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -52,11 +52,6 @@ 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
@@ -118,6 +113,11 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
+        zed_camera_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
       Mass Properties:
         Inertia: false
         Mass: false
@@ -159,9 +159,9 @@ Visualization Manager:
       Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 0
+      Max Intensity: 208
       Min Color: 0; 0; 0
-      Min Intensity: 0
+      Min Intensity: 1
       Name: LaserScan
       Position Transformer: XYZ
       Selectable: true
@@ -179,79 +179,19 @@ Visualization Manager:
       Use rainbow: true
       Value: true
     - Class: rviz_default_plugins/TF
-      Enabled: true
+      Enabled: false
       Frame Timeout: 15
       Frames:
         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:
-          Value: true
-        track_fr:
-          Value: true
-        track_l:
-          Value: true
-        track_r:
-          Value: true
-        track_rl:
-          Value: true
-        track_rr:
-          Value: true
-        velodyne_laser:
-          Value: true
-        velodyne_link:
-          Value: true
       Marker Scale: 1
       Name: TF
       Show Arrows: true
       Show Axes: true
       Show Names: false
       Tree:
-        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
+      Value: false
     - Alpha: 0.699999988079071
       Class: rviz_default_plugins/Map
       Color Scheme: map
@@ -357,7 +297,7 @@ Visualization Manager:
         y_scale: 1
         z_scale: 1
       Class: rviz_imu_plugin/Imu
-      Enabled: true
+      Enabled: false
       Name: Imu
       Topic:
         Depth: 5
@@ -366,7 +306,7 @@ Visualization Manager:
         History Policy: Keep Last
         Reliability Policy: Reliable
         Value: /imu
-      Value: true
+      Value: false
       fixed_frame_orientation: true
     - Alpha: 1
       Autocompute Intensity Bounds: true
@@ -400,7 +340,7 @@ Visualization Manager:
       Node filtering radius (m): 0
       Position Transformer: XYZ
       Size (Pixels): 3
-      Size (m): 0.10000000149011612
+      Size (m): 0.05000000074505806
       Style: Flat Squares
       Topic:
         Depth: 5
@@ -451,6 +391,32 @@ Visualization Manager:
       Use Fixed Frame: true
       Use rainbow: true
       Value: false
+    - Class: rviz_default_plugins/Camera
+      Enabled: true
+      Far Plane Distance: 100
+      Image Rendering: background
+      Name: Camera
+      Overlay Alpha: 0.5
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /zed/zed_node/left/image_rect_color
+      Value: true
+      Visibility:
+        DepthCloud: true
+        Grid: true
+        Imu: true
+        LaserScan: true
+        Map: true
+        MapCloud: true
+        Path: true
+        RobotModel: true
+        TF: true
+        Value: true
+        filtered position: true
+      Zoom Factor: 1
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -497,33 +463,35 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 11.600849151611328
+      Distance: 6.484375
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -1.209733486175537
-        Y: -1.5692439079284668
-        Z: 2.5361857414245605
+        X: -0.19487914443016052
+        Y: -0.5717933773994446
+        Z: 2.147127151489258
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.9553995728492737
+      Pitch: 0.6753999590873718
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 3.6536216735839844
+      Yaw: 0.813623309135437
     Saved: ~
 Window Geometry:
+  Camera:
+    collapsed: false
   Displays:
     collapsed: false
   Height: 814
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000015600000290fc020000000afb000000100044006900730070006c006100790073010000003d00000290000000c900fffffffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002770000011b0000000000000000fb0000000a0049006d006100670065000000003b0000035e0000000000000000000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000035e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000044f0000003efc0100000002fb0000000800540069006d006501000000000000044f000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002f30000029000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000015600000290fc020000000bfb000000100044006900730070006c006100790073010000003d000001d0000000c900fffffffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002770000011b0000000000000000fb0000000a0049006d006100670065000000003b0000035e0000000000000000fb0000000c00430061006d0065007200610100000213000000ba0000002800ffffff000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000035e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000044f0000003efc0100000002fb0000000800540069006d006501000000000000044f000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002f30000029000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:

diff --git a/src/rove_description/urdf/sensor.urdf.xacro b/src/rove_description/urdf/sensor.urdf.xacro
@@ -8,6 +8,12 @@
         <origin xyz="0.1 0 0.4" rpy="0 0 0" />
     </joint>
 
+    <joint name="zed_camera_joint" type="fixed">
+        <parent link="sensor_link" />
+        <child link="zed_camera_link" />
+        <origin xyz="0.1 0 0" rpy="0 0 0" />
+    </joint>
+
     <joint name="camera_joint" type="fixed">
         <parent link="sensor_link" />
         <child link="camera_link" />
@@ -32,6 +38,24 @@
         </collision>
     </link>
 
+    <link name="zed_camera_link">
+        <inertial>
+            <mass value="0.1" />
+            <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001" />
+        </inertial>
+        <visual>
+            <geometry>
+                <cylinder length="0.1" radius="0.1" />
+            </geometry>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <cylinder length="0.1" radius="0.1" />
+            </geometry>
+        </collision>
+    </link>
+
 
 
     <link name="sensor_link">

diff --git a/src/rove_slam/config/ekf.yaml b/src/rove_slam/config/ekf.yaml
@@ -2,7 +2,7 @@
 ekf_filter_node_local:
     ros__parameters:
         frequency: 30.0
-        two_d_mode: false
+        two_d_mode: true
         publish_tf: true
         map_frame: map              
         odom_frame: odom            
@@ -18,9 +18,9 @@ ekf_filter_node_local:
 
         imu0: /imu
         imu0_config: [false,  false,  false, # x y z
-                      false,  false,  true, # roll pitch yaw
+                      true,  true,  true, # roll pitch yaw
                       false,  false,  false, # vx vy vz
-                      false,  false,  true, # vroll vpitch vyaw
+                      true,  true,  true, # vroll vpitch vyaw
                       false, false, false] # ax ay az
         imu0_queue_size: 20
 

diff --git a/src/rove_slam/launch/slam3d_full.launch.py b/src/rove_slam/launch/slam3d_full.launch.py
@@ -23,7 +23,7 @@ def generate_launch_description():
         'subscribe_depth':True,
         'subscribe_odom_info':False,
         'subscribe_scan_cloud': True,
-        'subscribe_rgbd':True,
+        'subscribe_rgb':True,
         'approx_sync': True,
         'publish_tf':True,
         'Odom/Strategy':'0',
@@ -81,7 +81,7 @@ def generate_launch_description():
         output='screen',
         parameters=[{
             'max_clouds': 10,
-            'fixed_frame_id':'',
+            'fixed_frame_id':'sensor_link',
             'use_sim_time': use_sim_time,
         }],
         remappings=[
@@ -121,5 +121,5 @@ def generate_launch_description():
         camera_sync,
         point_cloud_assembler_node,
         rtabmap_node,
-        #rtabmap_viz_node
+        rtabmap_viz_node
     ])
diff --git a/src/rove_zed/config/zed_mapping.yaml b/src/rove_zed/config/zed_mapping.yaml
@@ -1,26 +1,27 @@
-# Parameters to override zed_wrapper parameters 
+# Parameters to override zed_wrapper parameters
 /**:
-    ros__parameters:
-        general:
-            grab_resolution: 'HD720' # The native camera grab resolution. 'HD2K', 'HD1080', 'HD720', 'VGA', 'AUTO'
-            pub_frame_rate: 30.0 # frequency of publishing of visual images and depth images
-            grab_frame_rate: 30 # ZED SDK internal grabbing rate
-        mapping:
-            mapping_enabled: false # True to enable mapping and fused point cloud pubblication
-            resolution: 0.2 # maps resolution in meters [min: 0.01f - max: 0.2f]
-            max_mapping_range: 5.0 # maximum depth range while mapping in meters (-1 for automatic calculation) [2.0, 20.0]
-            fused_pointcloud_freq: 1.0 # frequency of the publishing of the fused colored point cloud
-            clicked_point_topic: "/clicked_point" # Topic published by Rviz when a point of the cloud is clicked. Used for plane detection
-            pd_max_distance_threshold: 0.15 # Plane detection: controls the spread of plane by checking the position difference.
-            pd_normal_similarity_threshold: 15.0 # Plane detection: controls the spread of plane by checking the angle difference
-        depth:
-            depth_mode: "PERFORMANCE" # Matches the ZED SDK setting: 'NONE', 'PERFORMANCE', 'QUALITY', 'ULTRA', 'NEURAL', 'NEURAL_PLUS' - Note: if 'NONE' all the modules that requires depth extraction are disabled by default (Pos. Tracking, Obj. Detection, Mapping, ...)
-            depth_stabilization: 1 # Forces positional tracking to start if major than 0 - Range: [0,100]
-            openni_depth_mode: false # 'false': 32bit float [meters], 'true': 16bit unsigned int [millimeters]
-            point_cloud_freq: 30.0 # [DYNAMIC] - frequency of the pointcloud publishing (equal or less to `grab_frame_rate` value)
-            depth_confidence: 100 # [DYNAMIC]
-            depth_texture_conf: 100 # [DYNAMIC]
-            remove_saturated_areas: true # [DYNAMIC]
+  ros__parameters:
+    general:
+      grab_resolution: "VGA" # The native camera grab resolution. 'HD2K', 'HD1080', 'HD720', 'VGA', 'AUTO'
+      pub_frame_rate: 30.0 # frequency of publishing of visual images and depth images
+      grab_frame_rate: 30 # ZED SDK internal grabbing rate
+      camera_name: "zed"
+    mapping:
+      mapping_enabled: false # True to enable mapping and fused point cloud pubblication
+      resolution: 0.2 # maps resolution in meters [min: 0.01f - max: 0.2f]
+      max_mapping_range: 5.0 # maximum depth range while mapping in meters (-1 for automatic calculation) [2.0, 20.0]
+      fused_pointcloud_freq: 1.0 # frequency of the publishing of the fused colored point cloud
+      clicked_point_topic: "/clicked_point" # Topic published by Rviz when a point of the cloud is clicked. Used for plane detection
+      pd_max_distance_threshold: 0.15 # Plane detection: controls the spread of plane by checking the position difference.
+      pd_normal_similarity_threshold: 15.0 # Plane detection: controls the spread of plane by checking the angle difference
+    depth:
+      depth_mode: "PERFORMANCE" # Matches the ZED SDK setting: 'NONE', 'PERFORMANCE', 'QUALITY', 'ULTRA', 'NEURAL', 'NEURAL_PLUS' - Note: if 'NONE' all the modules that requires depth extraction are disabled by default (Pos. Tracking, Obj. Detection, Mapping, ...)
+      depth_stabilization: 1 # Forces positional tracking to start if major than 0 - Range: [0,100]
+      openni_depth_mode: false # 'false': 32bit float [meters], 'true': 16bit unsigned int [millimeters]
+      point_cloud_freq: 30.0 # [DYNAMIC] - frequency of the pointcloud publishing (equal or less to `grab_frame_rate` value)
+      depth_confidence: 100 # [DYNAMIC]
+      depth_texture_conf: 100 # [DYNAMIC]
+      remove_saturated_areas: true # [DYNAMIC]
 
-            min_depth: 0.2 # Min: 0.2, Max: 3.0
-            max_depth: 10.0 # Max: 40.0
+      min_depth: 0.2 # Min: 0.2, Max: 3.0
+      max_depth: 10.0 # Max: 40.0