WIP: added sherpa

src/sherpa/CMakeLists.txt

@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 3.8)
+project(sherpa)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
+
+# add launch file
+
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/)
+install(DIRECTORY config DESTINATION share/${PROJECT_NAME}/)
+install(DIRECTORY urdf   DESTINATION share/${PROJECT_NAME}/)
+install(DIRECTORY worlds DESTINATION share/${PROJECT_NAME}/)

src/sherpa/config/fastlio2_vlp16.yaml

@@ -0,0 +1,49 @@
+/**:
+    ros__parameters:
+        feature_extract_enable: false
+        point_filter_num: 1
+        max_iteration: 25
+        filter_size_surf: 0.5
+        filter_size_map: 0.5
+        cube_side_length: 1000.0
+        runtime_pos_log_enable: false
+        map_file_path: "/home/timo/Downloads/scan.pcd"
+
+        common:
+            lid_topic:  "/velodyne_points"
+            imu_topic:  "/imu"
+            time_sync_en: false         # ONLY turn on when external time synchronization is really not possible
+            time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README).
+                                        # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0
+
+        preprocess:
+            lidar_type: 2                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 
+            scan_line: 16
+            scan_rate: 10                # only need to be set for velodyne, unit: Hz,
+            timestamp_unit: 3            # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
+            blind: 1.05
+
+        mapping:
+            acc_cov: 0.01
+            gyr_cov: 0.05
+            b_acc_cov: 0.0005
+            b_gyr_cov: 0.00025
+            fov_degree:    360.0
+            det_range:     100.0
+            extrinsic_est_en:  true      # true: enable the online estimation of IMU-LiDAR extrinsic,
+            extrinsic_T: [ 0., 0., -0.05]
+            extrinsic_R: [ 1., 0., 0., 
+                        0., 1., 0., 
+                        0., 0., -1.]
+
+        publish:
+            path_en:  true
+            map_en: true
+            scan_publish_en:  true       # false: close all the point cloud output
+            dense_publish_en: true       # false: low down the points number in a global-frame point clouds scan.
+            scan_bodyframe_pub_en: true  # true: output the point cloud scans in IMU-body-frame
+
+        pcd_save:
+            pcd_save_en: true
+            interval: -1                 # how many LiDAR frames saved in each pcd file; 
+                                        # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

src/sherpa/config/sherpa.rviz

@@ -0,0 +1,339 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Scan1
+      Splitter Ratio: 0.5
+    Tree Height: 549
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: Scan
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        body:
+          Value: true
+        camera_init:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: false
+      Tree:
+        camera_init:
+          body:
+            {}
+      Update Interval: 0
+      Value: true
+    - Angle Tolerance: 0.10000000149011612
+      Class: rviz_default_plugins/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.30000001192092896
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: false
+      Keep: 100
+      Name: Odometry
+      Position Tolerance: 0.10000000149011612
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.10000000149011612
+        Color: 255; 25; 0
+        Head Length: 0.30000001192092896
+        Head Radius: 0.10000000149011612
+        Shaft Length: 1
+        Shaft Radius: 0.05000000074505806
+        Value: Arrow
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /Odometry
+      Value: false
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz_default_plugins/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
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /path
+      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_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 131
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: Scan
+      Position Transformer: XYZ
+      Selectable: false
+      Size (Pixels): 2.5
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /cloud_registered
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 0.05000000074505806
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 7.478440284729004
+        Min Value: -0.853997528553009
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: AxisColor
+      Decay Time: 4096
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 142
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: CloudRegistered
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 1
+      Size (m): 0.05000000074505806
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /cloud_registered
+      Use Fixed Frame: true
+      Use rainbow: true
+      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_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 184
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: CloudEffected
+      Position Transformer: XYZ
+      Selectable: true
+      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: /cloud_effected
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: -9999
+        Min Value: 9999
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: AxisColor
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 255
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: CloudMap
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.05000000074505806
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /Laser_map
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+  Enabled: true
+  Global Options:
+    Background Color: 0; 0; 0
+    Fixed Frame: camera_init
+    Frame Rate: 60
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 78.56363677978516
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: -3.594757080078125
+        Y: 1.8168765306472778
+        Z: 8.380480766296387
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 1.3997966051101685
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz_default_plugins)
+      Yaw: 0.8503900766372681
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ad0000003efc0100000002fb0000000800540069006d00650100000000000005ad000002fb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000451000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1453
+  X: 247
+  Y: 261

src/sherpa/config/sim.rviz

@@ -0,0 +1,339 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Scan1
+      Splitter Ratio: 0.5
+    Tree Height: 549
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: Scan
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        body:
+          Value: true
+        camera_init:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: false
+      Tree:
+        camera_init:
+          body:
+            {}
+      Update Interval: 0
+      Value: true
+    - Angle Tolerance: 0.10000000149011612
+      Class: rviz_default_plugins/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.30000001192092896
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: false
+      Keep: 100
+      Name: Odometry
+      Position Tolerance: 0.10000000149011612
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.10000000149011612
+        Color: 255; 25; 0
+        Head Length: 0.30000001192092896
+        Head Radius: 0.10000000149011612
+        Shaft Length: 1
+        Shaft Radius: 0.05000000074505806
+        Value: Arrow
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /Odometry
+      Value: false
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz_default_plugins/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
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /path
+      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_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 131
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: Scan
+      Position Transformer: XYZ
+      Selectable: false
+      Size (Pixels): 2.5
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /cloud_registered
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 0.05000000074505806
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 7.478440284729004
+        Min Value: -0.853997528553009
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: AxisColor
+      Decay Time: 4096
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 142
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: CloudRegistered
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 1
+      Size (m): 0.05000000074505806
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /cloud_registered
+      Use Fixed Frame: true
+      Use rainbow: true
+      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_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 184
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: CloudEffected
+      Position Transformer: XYZ
+      Selectable: true
+      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: /cloud_effected
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: -9999
+        Min Value: 9999
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: AxisColor
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 255
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: CloudMap
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.05000000074505806
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /Laser_map
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+  Enabled: true
+  Global Options:
+    Background Color: 0; 0; 0
+    Fixed Frame: camera_init
+    Frame Rate: 60
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 78.56363677978516
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: -3.594757080078125
+        Y: 1.8168765306472778
+        Z: 8.380480766296387
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 1.3997966051101685
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz_default_plugins)
+      Yaw: 0.8503900766372681
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ad0000003efc0100000002fb0000000800540069006d00650100000000000005ad000002fb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000451000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1453
+  X: 247
+  Y: 261

src/sherpa/config/vlp-16-pointcloud.yaml

@@ -0,0 +1,52 @@
+lasers:
+  - {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+      focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 0, rot_correction: 0.0,
+          vert_correction: -0.2617993877991494, vert_offset_correction: 0.0}
+  - {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+      focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 1, rot_correction: 0.0,
+          vert_correction: 0.017453292519943295, vert_offset_correction: 0.0}
+  - {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+      focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 2, rot_correction: 0.0,
+          vert_correction: -0.22689280275926285, vert_offset_correction: 0.0}
+  - {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+      focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 3, rot_correction: 0.0,
+          vert_correction: 0.05235987755982989, vert_offset_correction: 0.0}
+  - {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+      focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 4, rot_correction: 0.0,
+          vert_correction: -0.19198621771937624, vert_offset_correction: 0.0}
+  - {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+      focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 5, rot_correction: 0.0,
+          vert_correction: 0.08726646259971647, vert_offset_correction: 0.0}
+  - {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+      focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 6, rot_correction: 0.0,
+          vert_correction: -0.15707963267948966, vert_offset_correction: 0.0}
+  - {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+      focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 7, rot_correction: 0.0,
+          vert_correction: 0.12217304763960307, vert_offset_correction: 0.0}
+  - {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+      focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 8, rot_correction: 0.0,
+          vert_correction: -0.12217304763960307, vert_offset_correction: 0.0}
+  - {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+      focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 9, rot_correction: 0.0,
+          vert_correction: 0.15707963267948966, vert_offset_correction: 0.0}
+  - {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+      focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 10, rot_correction: 0.0,
+          vert_correction: -0.08726646259971647, vert_offset_correction: 0.0}
+  - {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+      focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 11, rot_correction: 0.0,
+          vert_correction: 0.19198621771937624, vert_offset_correction: 0.0}
+  - {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+      focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 12, rot_correction: 0.0,
+          vert_correction: -0.05235987755982989, vert_offset_correction: 0.0}
+  - {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+      focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 13, rot_correction: 0.0,
+          vert_correction: 0.22689280275926285, vert_offset_correction: 0.0}
+  - {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+      focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 14, rot_correction: 0.0,
+          vert_correction: -0.017453292519943295, vert_offset_correction: 0.0}
+  - {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+      focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 15, rot_correction: 0.0,
+          vert_correction: 0.2617993877991494, vert_offset_correction: 0.0}
+num_lasers: 16
+distance_resolution: 0.002
+

src/sherpa/config/wt931.yml

@@ -0,0 +1,64 @@
+witmotion:
+  ros__parameters:
+    port: ttyUSB1
+    baud_rate: 921600 # baud
+    polling_interval: 2 # ms
+    timeout_ms: 150 # ms
+    restart_service_name: /restart_imu
+    imu_publisher:
+      topic_name: /imu
+      frame_id: imu
+      use_native_orientation: false
+      measurements:
+        acceleration:
+          enabled: true
+          covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
+        angular_velocity:
+          enabled: true
+          covariance: [0.05, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.05]
+        orientation:
+          enabled: true
+          covariance: [0.05, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.05]
+    temperature_publisher:
+      enabled: false
+      topic_name: /temperature
+      frame_id: base_link
+      from_message: magnetometer # acceleration, angular_vel, orientation, magnetometer
+      variance: 0.02683
+      coefficient: 1.0 # Linear calibration parameters: coefficient
+      addition: 0.0 # and addendum
+    magnetometer_publisher:
+      enabled: false
+      topic_name: /magnetometer
+      frame_id: imu
+      coefficient: 0.00000001 # Linear calibration parameters: coefficient
+      addition: 0.0 # and addendum
+      covariance:
+        [0.000000187123, 0.0, 0.0, 0.0, 0.000000105373, 0.0, 0.0, 0.0, 0.000000165816]
+    barometer_publisher:
+      enabled: false
+      topic_name: /barometer
+      frame_id: base_link
+      variance: 0.001
+      coefficient: 1.0 # Linear calibration parameters: coefficient
+      addition: 0.0 # and addendum
+    altimeter_publisher:
+      enabled: false
+      topic_name: /altitude
+      coefficient: 1.0 # Linear calibration parameters: coefficient
+      addition: 0.0 # and addendum
+    orientation_publisher:
+      enabled: false
+      topic_name: /orientation
+    gps_publisher:
+      enabled: false
+      navsat_fix_frame_id: world
+      navsat_fix_topic_name: /gps
+      navsat_altitude_topic_name: /gps_altitude
+      navsat_satellites_topic_name: /gps_satellites
+      navsat_variance_topic_name: /gps_variance
+      ground_speed_topic_name: /gps_ground_speed
+    rtc_publisher:
+      enabled: false
+      topic_name: /witmotion_clock
+      presync: true

src/sherpa/launch/odometry.launch.py

@@ -0,0 +1,82 @@
+import os.path
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+from launch.conditions import IfCondition
+
+def generate_launch_description():
+
+    ld = LaunchDescription()   
+
+    velodyne_raw = Node(
+        package='velodyne_driver',
+        executable='velodyne_driver_node',
+        arguments=["--ros-args", "-p", "model:=VLP16", "-p", "rpm:=600.0"]
+    )
+
+    velodyne_pointcloud = Node(
+        package='velodyne_pointcloud',
+        executable='velodyne_transform_node',
+        arguments=["--ros-args", "-p", f"calibration:={os.path.join(get_package_share_directory('sherpa'), 'config', 'vlp-16-pointcloud.yaml')}", "-p", "model:=VLP16"]
+    )
+
+    velodyne_laserscan = Node(
+        package='velodyne_laserscan',
+        executable='velodyne_laserscan_node',
+        arguments=[]
+    )
+
+    imu = Node(
+        package = 'witmotion_ros',
+        executable = 'witmotion_ros_node',
+        parameters = [os.path.join(get_package_share_directory('sherpa'), 'config', 'wt931.yml')]
+    )
+
+    fast_lio = Node(
+        package='fast_lio',
+        executable='fastlio_mapping',
+        output='screen',
+        parameters=[PathJoinSubstitution([os.path.join(get_package_share_directory('sherpa'), 'config'), 'fastlio2_vlp16.yaml'])]
+    )
+
+    tf_laserscan = Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        arguments=['0.0', '0', '0.0', '0', '0', '0', '1', 'body', 'velodyne']
+    )
+
+    tf_map = Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        arguments=['0.0', '0', '0.0', '0', '0', '0', '1', 'camera_init', 'map']
+    )
+
+    #slam_toolbox = Node(
+    #    package='slam_toolbox',
+    #    executable='async_slam_toolbox_node',
+    #    arguments=['--ros-args', '-p', 'map_frame:=map', '-p', 'odom_frame:=camera_init', '-p', 'base_frame:=body', '-p', 'map_update_interval:=0.1', '-p', 'scan_queue_size:=256', '-p', 'position_covariance_scale:=0.1', '-p', 'use_scan_matching:=false']
+    #)
+
+    #rviz2 = Node(
+        #package='rviz2',
+        #executable='rviz2',
+        #arguments=['-d', f"{os.path.join(get_package_share_directory('fast_lio'), 'rviz', 'fastlio.rviz')}"]
+    #)
+
+
+    ld.add_action(velodyne_raw)
+    ld.add_action(velodyne_pointcloud)
+    ld.add_action(velodyne_laserscan)
+    ld.add_action(imu)
+    ld.add_action(fast_lio)
+    ld.add_action(tf_laserscan)
+    ld.add_action(tf_map)
+    #ld.add_action(slam_toolbox)
+
+    #ld.add_action(rviz2)
+
+    return ld

src/sherpa/launch/record.launch.py

@@ -0,0 +1,37 @@
+import os.path
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+from launch.conditions import IfCondition
+
+def generate_launch_description():
+
+    ld = LaunchDescription()   
+
+    velodyne_raw = Node(
+        package='velodyne_driver',
+        executable='velodyne_driver_node',
+        arguments=["--ros-args", "-p", "model:=VLP16", "-p", "rpm:=600.0"]
+    )
+
+    velodyne_pointcloud = Node(
+        package='velodyne_pointcloud',
+        executable='velodyne_transform_node',
+        arguments=["--ros-args", "-p", f"calibration:={os.path.join(get_package_share_directory('sherpa'), 'config', 'vlp-16-pointcloud.yaml')}", "-p", "model:=VLP16"]
+    )
+
+    imu = Node(
+        package = 'witmotion_ros',
+        executable = 'witmotion_ros_node',
+        parameters = [os.path.join(get_package_share_directory('sherpa'), 'config', 'wt931.yml')]
+    )
+
+    ld.add_action(velodyne_raw)
+    ld.add_action(velodyne_pointcloud)
+    ld.add_action(imu)
+
+    return ld

src/sherpa/launch/simulator.launch.py

@@ -0,0 +1,108 @@
+import os.path
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from ament_index_python.packages import get_package_share_directory
+from launch.substitutions import Command
+
+def generate_launch_description():
+
+    ld = LaunchDescription()  
+
+    pkg_name = 'sherpa'
+    pkg_share = get_package_share_directory(pkg_name)
+    pkg_prefix = os.path.dirname(pkg_share) 
+
+    world_path = os.path.join(pkg_share, 'worlds', 'empty.world')
+    xacro_file = os.path.join(pkg_share, 'urdf', 'sherpa.xacro')
+
+    # create nodes
+
+    gz_env = SetEnvironmentVariable(
+        name='GZ_SIM_RESOURCE_PATH',
+        value=[pkg_prefix]
+    )
+
+    gz = ExecuteProcess(
+        cmd=[
+            'ros2', 'launch', 'ros_gz_sim', 'gz_sim.launch.py',
+            f'gz_args:=-r {world_path}'
+        ],
+        output='screen'
+    )
+
+    state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        output='screen',
+        parameters=[{
+            'robot_description': Command(['xacro ', xacro_file])
+        }]
+    )
+
+    joint_pub = Node(
+        package='joint_state_publisher',
+        executable='joint_state_publisher',
+        name='joint_state_publisher',
+        output='screen'
+    )
+
+    static_tf = Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        name='static_tf_base_to_lidar',
+        arguments=[
+            '0', '0', '0',
+            '0', '0', '0',
+            'lidar_link',
+            'camera_init',
+        ]          
+    )
+
+    robot = ExecuteProcess(
+        cmd=[
+            'ros2', 'run', 'ros_gz_sim', 'create',
+            '-name', 'sherpa',
+            '-topic', 'robot_description',
+            '-x', '0', '-y', '0', '-z', '0.5'
+        ],
+        output='screen'
+    )
+
+    gz_bridge = Node(
+        package='ros_gz_bridge',
+        executable='parameter_bridge',
+        arguments=[
+            '/velodyne_points/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked',
+            '/model/sherpa/joint_states@gz.msgs.Model_JointState@ros2/sensor_msgs/msg/JointState',
+        ],
+        remappings=[
+            ('/velodyne_points/points', '/velodyne_points'),
+            ('/model/sherpa/joint_states', '/joint_states'),
+        ],
+        output='screen'
+    )
+
+
+    rviz2 = Node(
+        package='rviz2',
+        executable='rviz2',
+        arguments=['-d', f"{os.path.join(get_package_share_directory('sherpa'), 'config', 'sherpa.rviz')}"]
+    )
+
+    ld.add_action(gz_env)
+    ld.add_action(gz)
+    ld.add_action(state_publisher)
+    ld.add_action(joint_pub)
+    ld.add_action(static_tf)
+    ld.add_action(robot)
+    ld.add_action(gz_bridge)
+    ld.add_action(rviz2)
+
+    return ld

src/sherpa/launch/visualizer.launch.py

@@ -0,0 +1,23 @@
+import os.path
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+from launch.conditions import IfCondition
+
+def generate_launch_description():
+
+    ld = LaunchDescription()   
+
+    rviz2 = Node(
+        package='rviz2',
+        executable='rviz2',
+        arguments=['-d', f"{os.path.join(get_package_share_directory('sherpa'), 'config', 'sherpa.rviz')}"]
+    )
+
+    ld.add_action(rviz2)
+
+    return ld

src/sherpa/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>sherpa</name>
+  <version>0.0.1</version>
+  <description>Studienprojekt</description>
+  <maintainer email="timoschneider04@gmail.com">timo</maintainer>
+  <license>Apache 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/sherpa/scripts/fix_fastlio2.sh

@@ -0,0 +1,2 @@
+#!/bin/bash
+sed -i 's/\(14\)/17/g' $(dirname "$0")/../../FAST_LIO/CMakeLists.txt

src/sherpa/urdf/components/mecanum_wheel.xacro

@@ -0,0 +1,74 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" xmlns:gz="http://gazebosim.org">
+
+    <material name="mecanum_wheel_material">
+        <ambient>0.2 0.2 0.2 1</ambient>
+        <diffuse>0.2 0.2 0.2 1</diffuse>
+        <specular>0.2 0.2 0.2 1</specular>
+        <emissive>0 0 0 0</emissive>
+        <color rgba="0.2 0.2 0.2 1"/> 
+    </material>
+
+    <xacro:macro name="mecanum_wheel" params="name parent xyz rpy direction radius mass">
+
+        <!-- get left or right wheel -->
+        <xacro:if value="${direction == 'l'}">
+            <xacro:property name="mesh" value="https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_left.STL"/>
+            <xacro:property name="friction_dir" value="-1"/>
+        </xacro:if>
+        <xacro:if value="${direction == 'r'}">
+            <xacro:property name="mesh" value="https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_right.STL"/>
+            <xacro:property name="friction_dir" value="1"/>
+        </xacro:if>
+
+        <xacro:property name="mesh_scale" value="${0.006 * (radius / 0.3)}"/>
+
+        <link name='${name}_link'>
+            <inertial>
+                <mass value="${mass}"/>
+                <inertia 
+                    ixx="${0.5 * mass * radius * radius}"
+                    ixy="0.0"
+                    ixz="0.0"
+                    iyy="${0.5 * mass * radius * radius}"
+                    iyz="0.0"
+                    izz="${0.25 * mass * radius * radius}"
+                />
+            </inertial>
+            <visual name='visual'>
+                <pose>0 0 0 0 0 0</pose>
+                <geometry>
+                    <mesh
+                        filename="${mesh}"
+                        scale="${mesh_scale} ${mesh_scale} ${mesh_scale}"
+                    />
+                </geometry>
+                <material name="mecanum_wheel_material"/>
+            </visual>
+            <collision name='collision'>
+            <geometry>
+                <sphere radius="${radius}"/>
+            </geometry>
+            <surface>
+                <friction>
+                <ode>
+                    <mu>1.2</mu>
+                    <mu2>0</mu2>
+                    <fdir1 gz:expressed_in="${parent}">1 ${friction_dir} 0</fdir1>
+                </ode>
+                </friction>
+            </surface>
+            </collision>
+        </link>
+        <joint name='${name}_joint' type='continuous'>
+            <parent link="${parent}_link"/>
+            <child link="${name}_link"/>
+            <axis xyz="0 1 0"/>
+            <limit 
+                velocity="5"
+                effort="100000"
+            />
+            <origin xyz="${xyz}" rpy="${rpy}"/>
+        </joint>
+    </xacro:macro>
+</robot>

src/sherpa/urdf/components/ouster_lidar.xacro

@@ -0,0 +1,175 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <xacro:macro name="ouster_lidar" params="name parent xyz rpy model frequency topic">
+
+        <!-- different lidar models -->
+        <xacro:if value="${model == 'vlp16'}">
+            <xacro:property name="min_range"      value="0.01"/>
+            <xacro:property name="max_range"      value="100.0"/>
+            <xacro:property name="precision"      value="0.03"/>
+            <xacro:property name="max_frequency"  value="20.0"/>
+            <xacro:property name="vertical_fov"   value="30.0"/>
+            <xacro:property name="horizontal_res" value="900"/>
+            <xacro:property name="nr_of_channels" value="16"/>
+        </xacro:if>
+        <xacro:if value="${model == 'vlp16hi'}">
+            <xacro:property name="min_range"      value="0.5"/>
+            <xacro:property name="max_range"      value="100.0"/>
+            <xacro:property name="precision"      value="0.03"/>
+            <xacro:property name="max_frequency"  value="20.0"/>
+            <xacro:property name="vertical_fov"   value="40.0"/>
+            <xacro:property name="horizontal_res" value="900"/>
+            <xacro:property name="nr_of_channels" value="16"/>
+        </xacro:if>
+        <xacro:if value="${model == 'vlp32'}">
+            <xacro:property name="min_range"      value="0.5"/>
+            <xacro:property name="max_range"      value="200.0"/>
+            <xacro:property name="precision"      value="0.03"/>
+            <xacro:property name="max_frequency"  value="20.0"/>
+            <xacro:property name="vertical_fov"   value="40.0"/>
+            <xacro:property name="horizontal_res" value="900"/>
+            <xacro:property name="nr_of_channels" value="32"/>
+        </xacro:if>
+        <xacro:if value="${model == 'vls128'}">
+            <xacro:property name="min_range"      value="0.5"/>
+            <xacro:property name="max_range"      value="300.0"/>
+            <xacro:property name="precision"      value="0.03"/>
+            <xacro:property name="max_frequency"  value="20.0"/>
+            <xacro:property name="vertical_fov"   value="40.0"/>
+            <xacro:property name="horizontal_res" value="900"/>
+            <xacro:property name="nr_of_channels" value="128"/>
+        </xacro:if>
+        <xacro:if value="${model == 'OS0'}">
+            <xacro:property name="min_range"      value="0.3"/>
+            <xacro:property name="max_range"      value="100.0"/>
+            <xacro:property name="precision"      value="0.03"/>
+            <xacro:property name="max_frequency"  value="20.0"/>
+            <xacro:property name="vertical_fov"   value="90.0"/>
+            <xacro:property name="horizontal_res" value="1024"/>
+            <xacro:property name="nr_of_channels" value="128"/>
+        </xacro:if>
+        <xacro:if value="${model == 'OS1'}">
+            <xacro:property name="min_range"      value="0.3"/>
+            <xacro:property name="max_range"      value="200.0"/>
+            <xacro:property name="precision"      value="0.03"/>
+            <xacro:property name="max_frequency"  value="20.0"/>
+            <xacro:property name="vertical_fov"   value="45.0"/>
+            <xacro:property name="horizontal_res" value="1024"/>
+            <xacro:property name="nr_of_channels" value="128"/>
+        </xacro:if>
+        <xacro:if value="${model == 'OS2'}">
+            <xacro:property name="min_range"      value="0.3"/>
+            <xacro:property name="max_range"      value="400.0"/>
+            <xacro:property name="precision"      value="0.02"/>
+            <xacro:property name="max_frequency"  value="20.0"/>
+            <xacro:property name="vertical_fov"   value="22.5"/>
+            <xacro:property name="horizontal_res" value="1024"/>
+            <xacro:property name="nr_of_channels" value="128"/>
+        </xacro:if>
+
+
+        <link name="${name}_link">
+            <visual name="base">
+                <pose>0 0 -0.035 0 0 0</pose>
+                <geometry>
+                    <cylinder radius="0.06" length="0.0015"/>
+                </geometry>
+            </visual>
+            <collision name="base_collision">
+                <pose>0 0 -0.035 0 0 0</pose>
+                <geometry>
+                    <cylinder radius="0.06" length="0.01"/>
+                </geometry>
+            </collision>
+            <visual name="suport_1">
+                <pose>-0.035 0.035 -0.08 0 0 0</pose>
+                <geometry>
+                    <cylinder radius="0.005" length="0.09"/>
+                </geometry>
+            </visual>
+            <visual name="suport_2">
+                <pose>-0.035 -0.035 -0.08 0 0 0</pose>
+                <geometry>
+                    <cylinder radius="0.005" length="0.09"/>
+                </geometry>
+            </visual>
+            <visual name="suport_3">
+                <pose>0.0495 0 -0.08 0 0 0</pose>
+                <geometry>
+                    <cylinder radius="0.005" length="0.09"/>
+                </geometry>
+            </visual>
+            <!-- Bottom cylinder of Ouster/Velodyne 3D Lidar -->
+            <visual name="bot_cylinder">
+                <pose>0 0 -0.035 0 0 0</pose>
+                <geometry>
+                    <mesh filename="package://sherpa/urdf/meshes/VLP16_base_1.dae" scale="1 1 1"/>
+                </geometry>
+            </visual>
+            <!-- Top cylinder of Ouster/Velodyne 3D Lidar -->
+            <visual name="top_cylinder">
+                <pose>0 0 -0.035 0 0 0</pose>
+                <geometry>
+                    <mesh filename="package://sherpa/urdf/meshes/VLP16_base_2.dae" scale="1 1 1"/>
+                </geometry>
+            </visual>
+            <!-- Main cylinder of Ouster/Velodyne 3D Lidar -->
+            <visual name="main_cylinder">
+                <pose>0 0 -0.035 0 0 0</pose>
+                <geometry>
+                    <mesh filename="package://sherpa/urdf/meshes/VLP16_scan.dae" scale="1 1 1"/>
+                </geometry>
+            </visual>
+            <inertial>
+                <mass value="0.83"/>
+                <inertia 
+                    ixx="0.001828"
+                    ixy="0.0"
+                    ixz="0.0"
+                    iyy="0.001828"
+                    iyz="0.0"
+                    izz="0.003528"
+                />
+            </inertial>
+        </link>
+        <joint name="${name}_joint" type="fixed">
+            <parent link="${parent}_link"/>
+            <child link="${name}_link"/>
+            <origin xyz="${xyz}" rpy="${rpy}"/>
+        </joint>
+        <gazebo reference="${name}_link">
+            <sensor name="${name}" type="gpu_lidar">
+                <plugin
+                    filename="gz-sim-sensors-system"
+                    name="gz::sim::systems::Sensors">
+                    <render_engine>ogre2</render_engine>
+                </plugin>
+                <update_rate>${frequency}</update_rate>
+                <topic>/velodyne_points</topic>
+                <gz_frame_id>${name}_link</gz_frame_id>
+                <lidar>
+                    <scan>
+                        <horizontal>
+                            <samples>${horizontal_res * (max_frequency / frequency)}</samples>
+                            <resolution>1</resolution>
+                            <min_angle>-3.141592654</min_angle>
+                            <max_angle>3.141592654</max_angle>
+                        </horizontal>
+                        <vertical>
+                            <samples>${nr_of_channels}</samples>
+                            <resolution>1</resolution>
+                            <min_angle>${vertical_fov * -0.0087266463}</min_angle>
+                            <max_angle>${vertical_fov * 0.0087266463}</max_angle>
+                        </vertical>
+                    </scan>
+                    <range>
+                        <min>${min_range}</min>
+                        <max>${max_range}</max>
+                        <resolution>${precision}</resolution>
+                    </range>
+                </lidar>
+            </sensor>
+        </gazebo>
+    </xacro:macro>
+</robot>

src/sherpa/urdf/mecanum.sdf

@@ -0,0 +1,367 @@
+<?xml version="1.0" ?>
+<!--
+  Gazebo Mecanum drive plugin demo
+
+  Try sending commands:
+
+    gz topic -t "/model/vehicle_blue/cmd_vel" -m gz.msgs.Twist -p "linear: {x: 0.5, y: 0.5}"
+
+    gz topic -t "/model/vehicle_blue/cmd_vel" -m gz.msgs.Twist -p "linear: {y: 0.5}, angular: {z: 0.2}"
+
+-->
+<sdf version="1.6">
+  <world name="mecanum_drive">
+
+    <physics name="1ms" type="ignored">
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1.0</real_time_factor>
+    </physics>
+    <plugin
+      filename="gz-sim-physics-system"
+      name="gz::sim::systems::Physics">
+    </plugin>
+    <plugin
+      filename="gz-sim-user-commands-system"
+      name="gz::sim::systems::UserCommands">
+    </plugin>
+    <plugin
+      filename="gz-sim-scene-broadcaster-system"
+      name="gz::sim::systems::SceneBroadcaster">
+    </plugin>
+
+    <light type="directional" name="sun">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>1 1 1 1</diffuse>
+      <specular>0.5 0.5 0.5 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+
+    <model name="ground_plane">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>50</mu>
+              </ode>
+            </friction>
+          </surface>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <model name='vehicle_blue' xmlns:gz="http://gazebosim.org/schema">
+      <pose>0 2 0.325 0 -0 0</pose>
+
+      <link name='chassis'>
+        <pose>-0.151427 -0 0.175 0 -0 0</pose>
+        <inertial>
+          <mass>1.14395</mass>
+          <inertia>
+            <ixx>0.126164</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.416519</iyy>
+            <iyz>0</iyz>
+            <izz>0.481014</izz>
+          </inertia>
+        </inertial>
+        <visual name='visual'>
+          <geometry>
+            <box>
+              <size>2.01142 1 0.568726</size>
+            </box>
+          </geometry>
+          <material>
+            <ambient>0.5 0.5 1.0 1</ambient>
+            <diffuse>0.5 0.5 1.0 1</diffuse>
+            <specular>0.0 0.0 1.0 1</specular>
+          </material>
+        </visual>
+        <collision name='collision'>
+          <geometry>
+            <box>
+              <size>2.01142 1 0.568726</size>
+            </box>
+          </geometry>
+        </collision>
+      </link>
+
+      <link name='front_left_wheel'>
+        <pose>0.554283 0.625029 -0.025 -1.5707 0 0</pose>
+        <inertial>
+          <mass>2</mass>
+          <inertia>
+            <ixx>0.145833</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.145833</iyy>
+            <iyz>0</iyz>
+            <izz>0.125</izz>
+          </inertia>
+        </inertial>
+        <visual name='visual'>
+          <pose>0 0 0 1.5707 0 0</pose>
+          <geometry>
+            <!-- scale mesh to radius == 0.3 -->
+            <mesh>
+              <uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_left.STL</uri>
+              <scale>0.006 0.006 0.006</scale>
+            </mesh>
+          </geometry>
+          <material>
+            <ambient>0.2 0.2 0.2 1</ambient>
+            <diffuse>0.2 0.2 0.2 1</diffuse>
+            <specular>0.2 0.2 0.2 1</specular>
+          </material>
+        </visual>
+        <collision name='collision'>
+          <geometry>
+            <sphere>
+              <radius>0.3</radius>
+            </sphere>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>1.0</mu>
+                <mu2>0.0</mu2>
+                <fdir1 gz:expressed_in="chassis">1 -1 0</fdir1>
+              </ode>
+            </friction>
+          </surface>
+        </collision>
+      </link>
+
+      <link name='rear_left_wheel'>
+        <pose>-0.957138 0.625029 -0.025 -1.5707 0 0</pose>
+        <inertial>
+          <mass>2</mass>
+          <inertia>
+            <ixx>0.145833</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.145833</iyy>
+            <iyz>0</iyz>
+            <izz>0.125</izz>
+          </inertia>
+        </inertial>
+        <visual name='visual'>
+          <pose>0 0 0 1.5707 0 0</pose>
+          <geometry>
+            <!-- scale mesh to radius == 0.3 -->
+            <mesh>
+              <uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_right.STL</uri>
+              <scale>0.006 0.006 0.006</scale>
+            </mesh>
+          </geometry>
+          <material>
+            <ambient>0.2 0.2 0.2 1</ambient>
+            <diffuse>0.2 0.2 0.2 1</diffuse>
+            <specular>0.2 0.2 0.2 1</specular>
+          </material>
+        </visual>
+        <collision name='collision'>
+          <geometry>
+            <sphere>
+              <radius>0.3</radius>
+            </sphere>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>1.0</mu>
+                <mu2>0.0</mu2>
+                <fdir1 gz:expressed_in="chassis">1 1 0</fdir1>
+              </ode>
+            </friction>
+          </surface>
+        </collision>
+      </link>
+
+      <link name='front_right_wheel'>
+        <pose>0.554282 -0.625029 -0.025 -1.5707 0 0</pose>
+        <inertial>
+          <mass>2</mass>
+          <inertia>
+            <ixx>0.145833</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.145833</iyy>
+            <iyz>0</iyz>
+            <izz>0.125</izz>
+          </inertia>
+        </inertial>
+        <visual name='visual'>
+          <pose>0 0 0 1.5707 0 0</pose>
+          <geometry>
+            <!-- scale mesh to radius == 0.3 -->
+            <mesh>
+              <uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_right.STL</uri>
+              <scale>0.006 0.006 0.006</scale>
+            </mesh>
+          </geometry>
+          <material>
+            <ambient>0.2 0.2 0.2 1</ambient>
+            <diffuse>0.2 0.2 0.2 1</diffuse>
+            <specular>0.2 0.2 0.2 1</specular>
+          </material>
+        </visual>
+        <collision name='collision'>
+          <geometry>
+            <sphere>
+              <radius>0.3</radius>
+            </sphere>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>1.0</mu>
+                <mu2>0.0</mu2>
+                <fdir1 gz:expressed_in="chassis">1 1 0</fdir1>
+              </ode>
+            </friction>
+          </surface>
+        </collision>
+      </link>
+
+      <link name='rear_right_wheel'>
+        <pose>-0.957138 -0.625029 -0.025 -1.5707 0 0</pose>
+        <inertial>
+          <mass>2</mass>
+          <inertia>
+            <ixx>0.145833</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.145833</iyy>
+            <iyz>0</iyz>
+            <izz>0.125</izz>
+          </inertia>
+        </inertial>
+        <visual name='visual'>
+          <pose>0 0 0 1.5707 0 0</pose>
+          <geometry>
+            <!-- scale mesh to radius == 0.3 -->
+            <mesh>
+              <uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_left.STL</uri>
+              <scale>0.006 0.006 0.006</scale>
+            </mesh>
+          </geometry>
+          <material>
+            <ambient>0.2 0.2 0.2 1</ambient>
+            <diffuse>0.2 0.2 0.2 1</diffuse>
+            <specular>0.2 0.2 0.2 1</specular>
+          </material>
+        </visual>
+        <collision name='collision'>
+          <geometry>
+            <sphere>
+              <radius>0.3</radius>
+            </sphere>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>1.0</mu>
+                <mu2>0.0</mu2>
+                <fdir1 gz:expressed_in="chassis">1 -1 0</fdir1>
+              </ode>
+            </friction>
+          </surface>
+        </collision>
+      </link>
+
+
+      <joint name='front_left_wheel_joint' type='revolute'>
+        <parent>chassis</parent>
+        <child>front_left_wheel</child>
+        <axis>
+          <xyz>0 0 1</xyz>
+          <limit>
+            <lower>-1.79769e+308</lower>
+            <upper>1.79769e+308</upper>
+          </limit>
+        </axis>
+      </joint>
+
+      <joint name='front_right_wheel_joint' type='revolute'>
+        <parent>chassis</parent>
+        <child>front_right_wheel</child>
+        <axis>
+          <xyz>0 0 1</xyz>
+          <limit>
+            <lower>-1.79769e+308</lower>
+            <upper>1.79769e+308</upper>
+          </limit>
+        </axis>
+      </joint>
+
+      <joint name='rear_left_wheel_joint' type='revolute'>
+        <parent>chassis</parent>
+        <child>rear_left_wheel</child>
+        <axis>
+          <xyz>0 0 1</xyz>
+          <limit>
+            <lower>-1.79769e+308</lower>
+            <upper>1.79769e+308</upper>
+          </limit>
+        </axis>
+      </joint>
+
+      <joint name='rear_right_wheel_joint' type='revolute'>
+        <parent>chassis</parent>
+        <child>rear_right_wheel</child>
+        <axis>
+          <xyz>0 0 1</xyz>
+          <limit>
+            <lower>-1.79769e+308</lower>
+            <upper>1.79769e+308</upper>
+          </limit>
+        </axis>
+      </joint>
+
+      <plugin
+        filename="gz-sim-mecanum-drive-system"
+        name="gz::sim::systems::MecanumDrive">
+        <front_left_joint>front_left_wheel_joint</front_left_joint>
+        <front_right_joint>front_right_wheel_joint</front_right_joint>
+        <back_left_joint>rear_left_wheel_joint</back_left_joint>
+        <back_right_joint>rear_right_wheel_joint</back_right_joint>
+        <wheel_separation>1.25</wheel_separation>
+        <wheelbase>1.511</wheelbase>
+        <wheel_radius>0.3</wheel_radius>
+        <min_acceleration>-5</min_acceleration>
+        <max_acceleration>5</max_acceleration>
+      </plugin>
+
+    </model>
+
+  </world>
+</sdf>

src/sherpa/urdf/sherpa.urdf

@@ -0,0 +1,262 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from /ros2_ws/src/sherpa/urdf/sherpa.xacro | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="sherpa" xmlns:gz="http://gazebosim.org">
+  <link name="base_link">
+    <inertial>
+      <mass value="15"/>
+      <inertia ixx="0.001828" ixy="0.0" ixz="0.0" iyy="0.001828" iyz="0.0" izz="0.003528"/>
+    </inertial>
+  </link>
+  <material name="mecanum_wheel_material">
+    <ambient>0.2 0.2 0.2 1</ambient>
+    <diffuse>0.2 0.2 0.2 1</diffuse>
+    <specular>0.2 0.2 0.2 1</specular>
+    <emissive>0 0 0 0</emissive>
+    <color rgba="0.2 0.2 0.2 1"/>
+  </material>
+  <link name="lidar_link">
+    <visual name="base">
+      <pose>0 0 -0.035 0 0 0</pose>
+      <geometry>
+        <cylinder length="0.0015" radius="0.06"/>
+      </geometry>
+    </visual>
+    <collision name="base_collision">
+      <pose>0 0 -0.035 0 0 0</pose>
+      <geometry>
+        <cylinder length="0.01" radius="0.06"/>
+      </geometry>
+    </collision>
+    <visual name="suport_1">
+      <pose>-0.035 0.035 -0.08 0 0 0</pose>
+      <geometry>
+        <cylinder length="0.09" radius="0.005"/>
+      </geometry>
+    </visual>
+    <visual name="suport_2">
+      <pose>-0.035 -0.035 -0.08 0 0 0</pose>
+      <geometry>
+        <cylinder length="0.09" radius="0.005"/>
+      </geometry>
+    </visual>
+    <visual name="suport_3">
+      <pose>0.0495 0 -0.08 0 0 0</pose>
+      <geometry>
+        <cylinder length="0.09" radius="0.005"/>
+      </geometry>
+    </visual>
+    <!-- Bottom cylinder of Ouster/Velodyne 3D Lidar -->
+    <visual name="bot_cylinder">
+      <pose>0 0 -0.035 0 0 0</pose>
+      <geometry>
+        <mesh filename="package://sherpa/urdf/meshes/VLP16_base_1.dae" scale="1 1 1"/>
+      </geometry>
+    </visual>
+    <!-- Top cylinder of Ouster/Velodyne 3D Lidar -->
+    <visual name="top_cylinder">
+      <pose>0 0 -0.035 0 0 0</pose>
+      <geometry>
+        <mesh filename="package://sherpa/urdf/meshes/VLP16_base_2.dae" scale="1 1 1"/>
+      </geometry>
+    </visual>
+    <!-- Main cylinder of Ouster/Velodyne 3D Lidar -->
+    <visual name="main_cylinder">
+      <pose>0 0 -0.035 0 0 0</pose>
+      <geometry>
+        <mesh filename="package://sherpa/urdf/meshes/VLP16_scan.dae" scale="1 1 1"/>
+      </geometry>
+    </visual>
+    <inertial>
+      <mass value="0.83"/>
+      <inertia ixx="0.001828" ixy="0.0" ixz="0.0" iyy="0.001828" iyz="0.0" izz="0.003528"/>
+    </inertial>
+  </link>
+  <joint name="lidar_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="lidar_link"/>
+    <origin rpy="0 0 0" xyz="0 0 0.15"/>
+  </joint>
+  <gazebo reference="lidar_link">
+    <sensor name="lidar" type="gpu_lidar">
+      <plugin filename="gz-sim-sensors-system" name="gz::sim::systems::Sensors">
+        <render_engine>ogre2</render_engine>
+      </plugin>
+      <update_rate>10</update_rate>
+      <topic>/velodyne_points</topic>
+      <gz_frame_id>lidar_link</gz_frame_id>
+      <lidar>
+        <scan>
+          <horizontal>
+            <samples>1800.0</samples>
+            <resolution>1</resolution>
+            <min_angle>-3.141592654</min_angle>
+            <max_angle>3.141592654</max_angle>
+          </horizontal>
+          <vertical>
+            <samples>16</samples>
+            <resolution>1</resolution>
+            <min_angle>-0.261799389</min_angle>
+            <max_angle>0.261799389</max_angle>
+          </vertical>
+        </scan>
+        <range>
+          <min>0.01</min>
+          <max>100.0</max>
+          <resolution>0.03</resolution>
+        </range>
+      </lidar>
+    </sensor>
+  </gazebo>
+  <link name="wheel_fr_link">
+    <inertial>
+      <mass value="0.25"/>
+      <inertia ixx="0.0002" ixy="0.0" ixz="0.0" iyy="0.0002" iyz="0.0" izz="0.0001"/>
+    </inertial>
+    <visual name="visual">
+      <pose>0 0 0 0 0 0</pose>
+      <geometry>
+        <mesh filename="https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_right.STL" scale="0.0008 0.0008 0.0008"/>
+      </geometry>
+      <material name="mecanum_wheel_material"/>
+    </visual>
+    <collision name="collision">
+      <geometry>
+        <sphere radius="0.04"/>
+      </geometry>
+      <surface>
+        <friction>
+          <ode>
+            <mu>1.2</mu>
+            <mu2>0</mu2>
+            <fdir1 gz:expressed_in="base">1 1 0</fdir1>
+          </ode>
+        </friction>
+      </surface>
+    </collision>
+  </link>
+  <joint name="wheel_fr_joint" type="continuous">
+    <parent link="base_link"/>
+    <child link="wheel_fr_link"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="100000" velocity="5"/>
+    <origin rpy="0 0 0" xyz=" 0.11 -0.11 0"/>
+  </joint>
+  <link name="wheel_fl_link">
+    <inertial>
+      <mass value="0.25"/>
+      <inertia ixx="0.0002" ixy="0.0" ixz="0.0" iyy="0.0002" iyz="0.0" izz="0.0001"/>
+    </inertial>
+    <visual name="visual">
+      <pose>0 0 0 0 0 0</pose>
+      <geometry>
+        <mesh filename="https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_left.STL" scale="0.0008 0.0008 0.0008"/>
+      </geometry>
+      <material name="mecanum_wheel_material"/>
+    </visual>
+    <collision name="collision">
+      <geometry>
+        <sphere radius="0.04"/>
+      </geometry>
+      <surface>
+        <friction>
+          <ode>
+            <mu>1.2</mu>
+            <mu2>0</mu2>
+            <fdir1 gz:expressed_in="base">1 -1 0</fdir1>
+          </ode>
+        </friction>
+      </surface>
+    </collision>
+  </link>
+  <joint name="wheel_fl_joint" type="continuous">
+    <parent link="base_link"/>
+    <child link="wheel_fl_link"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="100000" velocity="5"/>
+    <origin rpy="0 0 0" xyz=" 0.11  0.11 0"/>
+  </joint>
+  <link name="wheel_br_link">
+    <inertial>
+      <mass value="0.25"/>
+      <inertia ixx="0.0002" ixy="0.0" ixz="0.0" iyy="0.0002" iyz="0.0" izz="0.0001"/>
+    </inertial>
+    <visual name="visual">
+      <pose>0 0 0 0 0 0</pose>
+      <geometry>
+        <mesh filename="https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_left.STL" scale="0.0008 0.0008 0.0008"/>
+      </geometry>
+      <material name="mecanum_wheel_material"/>
+    </visual>
+    <collision name="collision">
+      <geometry>
+        <sphere radius="0.04"/>
+      </geometry>
+      <surface>
+        <friction>
+          <ode>
+            <mu>1.2</mu>
+            <mu2>0</mu2>
+            <fdir1 gz:expressed_in="base">1 -1 0</fdir1>
+          </ode>
+        </friction>
+      </surface>
+    </collision>
+  </link>
+  <joint name="wheel_br_joint" type="continuous">
+    <parent link="base_link"/>
+    <child link="wheel_br_link"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="100000" velocity="5"/>
+    <origin rpy="0 0 0" xyz="-0.11 -0.11 0"/>
+  </joint>
+  <link name="wheel_bl_link">
+    <inertial>
+      <mass value="0.25"/>
+      <inertia ixx="0.0002" ixy="0.0" ixz="0.0" iyy="0.0002" iyz="0.0" izz="0.0001"/>
+    </inertial>
+    <visual name="visual">
+      <pose>0 0 0 0 0 0</pose>
+      <geometry>
+        <mesh filename="https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_right.STL" scale="0.0008 0.0008 0.0008"/>
+      </geometry>
+      <material name="mecanum_wheel_material"/>
+    </visual>
+    <collision name="collision">
+      <geometry>
+        <sphere radius="0.04"/>
+      </geometry>
+      <surface>
+        <friction>
+          <ode>
+            <mu>1.2</mu>
+            <mu2>0</mu2>
+            <fdir1 gz:expressed_in="base">1 1 0</fdir1>
+          </ode>
+        </friction>
+      </surface>
+    </collision>
+  </link>
+  <joint name="wheel_bl_joint" type="continuous">
+    <parent link="base_link"/>
+    <child link="wheel_bl_link"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="100000" velocity="5"/>
+    <origin rpy="0 0 0" xyz="-0.11  0.11 0"/>
+  </joint>
+  <gazebo>
+    <plugin filename="gz-sim-mecanum-drive-system" name="gz::sim::systems::MecanumDrive">
+      <front_left_joint>wheel_fl_joint</front_left_joint>
+      <front_right_joint>wheel_fr_joint</front_right_joint>
+      <back_left_joint>wheel_bl_joint</back_left_joint>
+      <back_right_joint>wheel_br_joint</back_right_joint>
+      <wheel_separation>0.22</wheel_separation>
+      <wheelbase>0.22</wheelbase>
+      <wheel_radius>0.04</wheel_radius>
+      <min_acceleration>-5</min_acceleration>
+      <max_acceleration>5</max_acceleration>
+    </plugin>
+  </gazebo>
+</robot>

src/sherpa/urdf/sherpa.xacro

@@ -0,0 +1,40 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="sherpa">   
+    <link name="base_link">
+        <inertial>
+            <mass value="15"/>
+            <inertia 
+                ixx="0.001828"
+                ixy="0.0"
+                ixz="0.0"
+                iyy="0.001828"
+                iyz="0.0"
+                izz="0.003528"
+            />
+        </inertial>
+    </link>
+
+    <xacro:include filename="components/mecanum_wheel.xacro"/>
+    <xacro:include filename="components/ouster_lidar.xacro"/>
+
+    <xacro:ouster_lidar name="lidar" parent="base" xyz="0 0 0.15" rpy="0 0 0" model="vlp16" frequency="10" topic="/points"/>
+    
+    <xacro:mecanum_wheel name="wheel_fr" parent="base" xyz=" 0.11 -0.11 0" rpy="0 0 0" direction="r" radius="0.04" mass="0.25"/>
+    <xacro:mecanum_wheel name="wheel_fl" parent="base" xyz=" 0.11  0.11 0" rpy="0 0 0" direction="l" radius="0.04" mass="0.25"/>
+    <xacro:mecanum_wheel name="wheel_br" parent="base" xyz="-0.11 -0.11 0" rpy="0 0 0" direction="l" radius="0.04" mass="0.25"/>
+    <xacro:mecanum_wheel name="wheel_bl" parent="base" xyz="-0.11  0.11 0" rpy="0 0 0" direction="r" radius="0.04" mass="0.25"/>
+
+    <gazebo>
+        <plugin filename="gz-sim-mecanum-drive-system" name="gz::sim::systems::MecanumDrive">
+            <front_left_joint>wheel_fl_joint</front_left_joint>
+            <front_right_joint>wheel_fr_joint</front_right_joint>
+            <back_left_joint>wheel_bl_joint</back_left_joint>
+            <back_right_joint>wheel_br_joint</back_right_joint>
+            <wheel_separation>0.22</wheel_separation>
+            <wheelbase>0.22</wheelbase>
+            <wheel_radius>0.04</wheel_radius>
+            <min_acceleration>-5</min_acceleration>
+            <max_acceleration>5</max_acceleration>
+        </plugin>
+    </gazebo>
+</robot>

src/sherpa/worlds/empty.world

@@ -0,0 +1,54 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+  <world name="empty">
+    <light name="sun" type="directional">
+      <pose>0 0 20 0 0 0</pose>
+      <diffuse>0.8 0.8 0.8 1.0</diffuse>
+      <specular>0.8 0.8 0.8 1.0</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.05</linear>
+        <quadratic>0.01</quadratic>
+      </attenuation>
+      <cast_shadows>true</cast_shadows>
+    </light>
+
+    <model name="ground_plane">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>1000 1000</size>
+            </plane>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>0.8</mu>
+                <mu2>0.6</mu2>
+              </ode>
+            </friction>
+          </surface>
+        </collision>
+        <visual name="visual">
+          <cast_shadows>false</cast_shadows>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>1000 1000</size>
+            </plane>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+      </link>
+    </model>
+  </world>
+</sdf>