Add flatland simulation

.gitmodules

@@ -0,0 +1,4 @@
+[submodule "flatland"]
+  path = flatland
+  url = [email protected]:avidbots/flatland.git
+  branch = ros2-humble

beluga_example/CMakeLists.txt

@@ -7,7 +7,9 @@ find_package(beluga_amcl REQUIRED)
 install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
 install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
 install(DIRECTORY maps DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY models DESTINATION share/${PROJECT_NAME})
 install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY worlds DESTINATION share/${PROJECT_NAME})
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)

beluga_example/launch/example_launch.py

@@ -74,6 +74,23 @@ def generate_launch_description():
                     {"node_names": ["map_server", "amcl"]},
                 ],
             ),
+            Node(
+                package="flatland_server",
+                executable="flatland_server",
+                output="screen",
+                parameters=[
+                    {
+                        "world_path": os.path.join(
+                            example_dir, "worlds", "turtlebot3_world.yaml"
+                        )
+                    },
+                    {"update_rate": 200.0},
+                    {"step_size": 0.005},
+                    {"show_viz": False},
+                    {"viz_pub_rate": 30.0},
+                    {"use_sim_time": True},
+                ],
+            ),
         ]
     )
 

beluga_example/models/turtlebot.model.yaml

@@ -0,0 +1,50 @@
+bodies:
+  - name: base
+    pose: [0, 0, 0]
+    type: dynamic
+    color: [1, 1, 1, 0.75]
+    footprints:
+      - type: circle
+        radius: 0.5
+        center: [-1, 0.0]
+        density: 1.0
+
+      - type: polygon
+        sensor: true
+        points: [[-.45, -.05], [-.45, 0.05], [-.35, 0.05], [-.35, -0.05]]
+        layers: []
+        density: 1.0
+
+      - type: polygon
+        sensor: true
+        points: [[-.125, -.4], [-.125, -.3], [.125, -.3], [.125, -.4]]
+        density: 1.0
+
+      - type: polygon
+        sensor: true
+        points: [[-.125, .4], [-.125, .3], [.125, .3], [.125, .4]]
+        density: 100000
+
+plugins:
+  - type: DiffDrive
+    name: turtlebot_drive
+    body: base
+    twist_sub: cmd_vel
+    odom_frame_id: map
+    pub_rate: 10
+
+  - type: ModelTfPublisher
+    name: tf_publisher
+    publish_tf_world: true
+
+  - type: Laser
+    name: laser_front
+    frame: laser_front
+    topic: scan
+    body: base
+    broadcast_tf: true
+    origin: [0.28, 0, 0]
+    range: 20
+    angle: {min: -2.356194490192345, max: 2.356194490192345, increment: 0.004363323129985824}
+    noise_std_dev: 0.02
+    update_rate: 40

beluga_example/package.xml

@@ -13,8 +13,9 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>launch_ros</build_depend>
 
-  <exec_depend>launch_ros</exec_depend>
   <exec_depend>beluga_amcl</exec_depend>
+  <exec_depend>flatland_server</exec_depend>
+  <exec_depend>launch_ros</exec_depend>
   <exec_depend>nav2_lifecycle_manager</exec_depend>
   <exec_depend>nav2_map_server</exec_depend>
   <exec_depend>nav2_rviz_plugins</exec_depend>

beluga_example/rviz/rviz.rviz

@@ -9,8 +9,9 @@ Panels:
         - /ParticleCloud1
         - /Map1
         - /TF1
+        - /LaserScan1
       Splitter Ratio: 0.5
-    Tree Height: 1555
+    Tree Height: 1166
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -28,7 +29,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: ""
+    SyncSource: LaserScan
 Visualization Manager:
   Class: ""
   Displays:
@@ -54,8 +55,8 @@ Visualization Manager:
       Class: nav2_rviz_plugins/ParticleCloud
       Color: 255; 25; 0
       Enabled: true
-      Max Arrow Length: 0.1
-      Min Arrow Length: 0.05
+      Max Arrow Length: 0.10000000149011612
+      Min Arrow Length: 0.05000000074505806
       Name: ParticleCloud
       Shape: Arrow (3D)
       Topic:
@@ -92,15 +93,58 @@ Visualization Manager:
       Frame Timeout: 15
       Frames:
         All Enabled: true
+        base:
+          Value: true
+        laser_front:
+          Value: true
+        map:
+          Value: true
       Marker Scale: 1
       Name: TF
       Show Arrows: true
       Show Axes: true
       Show Names: true
       Tree:
-        {}
+        map:
+          base:
+            laser_front:
+              {}
       Update Interval: 0
       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/LaserScan
+      Color: 0; 255; 0
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: LaserScan
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.019999999552965164
+      Style: Spheres
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /scan
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -147,7 +191,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 16.030778884887695
+      Distance: 7.173450469970703
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -162,18 +206,18 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.9253982305526733
+      Pitch: 1.100398302078247
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 2.375394582748413
+      Yaw: 3.5103964805603027
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 2032
+  Height: 1536
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001dc00000701fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000ab00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000069000007010000017800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000014f00000701fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000069000007010000012300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000e7000000051fc0100000002fb0000000800540069006d0065010000000000000e700000045300fffffffb0000000800540069006d0065010000000000000450000000000000000000000b2d0000070100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001dc00000553fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000553000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000014f00000553fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000553000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b800000051fc0100000002fb0000000800540069006d00650100000000000009b80000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000006810000055300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -182,6 +226,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 3696
-  X: 144
-  Y: 54
+  Width: 2488
+  X: 72
+  Y: 27

beluga_example/worlds/turtlebot3_world.yaml

@@ -0,0 +1,9 @@
+properties: {}
+
+layers:
+  - name: "map"
+    map: "../maps/turtlebot3_world.yaml"
+models:
+  - name: turtlebot
+    model: "../models/turtlebot.model.yaml"
+    pose: [0, 2, 0]