Multi nav (#3)

Multi robot support

Signed-off-by: Piotr Jaroszek <[email protected]>

kraken_nav/README.md

@@ -81,26 +81,44 @@ colcon build --symlink-install
 ## Running simulation
 
 1. [Build](https://github.com/aws-lumberyard/ROSConDemo#download-and-install) and run the `ROSConDemo`
-1. Load level `Main`
+2. Load level `Main`
 
-## Running nav stack
+## Running nav stack with multiple vehicles
 
-1. Source the workspace
+> Note: Slam is turned off by default since we have ground truth information about the robot's position from the simulator. However, it is possible to enable `slam_toolbox` forcefully. You can allow slam by adding `use_slam:=True` to the navigation launch command.
+
+1. For our scenario, spawn the following robots:
+
+```bash
+ros2 service call /spawn_entity gazebo_msgs/srv/SpawnEntity '{name: 'apple_kraken_rusty', xml: 'line1'}' &&
+ros2 service call /spawn_entity gazebo_msgs/srv/SpawnEntity '{name: 'apple_kraken_shiny', xml: 'line2'}' &&
+ros2 service call /spawn_entity gazebo_msgs/srv/SpawnEntity '{name: 'apple_kraken_rusty', xml: 'line3'}' &&
+ros2 service call /spawn_entity gazebo_msgs/srv/SpawnEntity '{name: 'apple_kraken_shiny', xml: 'line4'}'
+```
+
+2. Source the workspace
 
 ```bash
 cd ~/o3de_kraken_ws
 source ./install/setup.bash
 ```
 
-2. Set up `CycloneDDS` rmw
+3. Set up `CycloneDDS` rmw:
 
 ```bash
 export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
 ```
 
-3. Run the navigation stack
+4. Run the first stack with `Rviz` param set to `True`:
 
 ```bash
-ros2 launch o3de_kraken_nav navigation.launch.py
+ros2 launch o3de_kraken_nav navigation_multi.launch.py namespace:=apple_kraken_rusty_1 rviz:=True
 ```
 
+5. Run the stack for rest of the robots (in different terminals, remember about points `1-3`):
+
+```bash
+ros2 launch o3de_kraken_nav navigation_multi.launch.py namespace:=apple_kraken_shiny_2 rviz:=False
+ros2 launch o3de_kraken_nav navigation_multi.launch.py namespace:=apple_kraken_rusty_3 rviz:=False
+ros2 launch o3de_kraken_nav navigation_multi.launch.py namespace:=apple_kraken_shiny_4 rviz:=False
+```

kraken_nav/launch/config/config_multi.rviz

@@ -0,0 +1,669 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /TF1
+        - /LaserScan1/Topic1
+      Splitter Ratio: 0.5
+    Tree Height: 854
+  - 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: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        apple_kraken_rusty_1/base_link:
+          Value: true
+        apple_kraken_rusty_1/kraken_manipulator_link_0:
+          Value: true
+        apple_kraken_rusty_1/kraken_manipulator_link_1:
+          Value: true
+        apple_kraken_rusty_1/kraken_manipulator_link_2:
+          Value: true
+        apple_kraken_rusty_1/kraken_manipulator_link_3:
+          Value: true
+        apple_kraken_rusty_1/kraken_manipulator_link_4:
+          Value: true
+        apple_kraken_rusty_1/lidar:
+          Value: true
+        apple_kraken_rusty_1/lidar_mount:
+          Value: true
+        apple_kraken_rusty_1/map:
+          Value: true
+        apple_kraken_rusty_1/odom:
+          Value: true
+        apple_kraken_rusty_1/steering_front_left_link:
+          Value: true
+        apple_kraken_rusty_1/steering_front_right_link:
+          Value: true
+        apple_kraken_rusty_1/wheel_front_left_link:
+          Value: true
+        apple_kraken_rusty_1/wheel_front_right_link:
+          Value: true
+        apple_kraken_rusty_1/wheel_rear_left_link:
+          Value: true
+        apple_kraken_rusty_1/wheel_rear_right_link:
+          Value: true
+        apple_kraken_rusty_3/base_link:
+          Value: true
+        apple_kraken_rusty_3/kraken_manipulator_link_0:
+          Value: true
+        apple_kraken_rusty_3/kraken_manipulator_link_1:
+          Value: true
+        apple_kraken_rusty_3/kraken_manipulator_link_2:
+          Value: true
+        apple_kraken_rusty_3/kraken_manipulator_link_3:
+          Value: true
+        apple_kraken_rusty_3/kraken_manipulator_link_4:
+          Value: true
+        apple_kraken_rusty_3/lidar:
+          Value: true
+        apple_kraken_rusty_3/lidar_mount:
+          Value: true
+        apple_kraken_rusty_3/map:
+          Value: true
+        apple_kraken_rusty_3/odom:
+          Value: true
+        apple_kraken_rusty_3/steering_front_left_link:
+          Value: true
+        apple_kraken_rusty_3/steering_front_right_link:
+          Value: true
+        apple_kraken_rusty_3/wheel_front_left_link:
+          Value: true
+        apple_kraken_rusty_3/wheel_front_right_link:
+          Value: true
+        apple_kraken_rusty_3/wheel_rear_left_link:
+          Value: true
+        apple_kraken_rusty_3/wheel_rear_right_link:
+          Value: true
+        apple_kraken_shiny_2/base_link:
+          Value: true
+        apple_kraken_shiny_2/kraken_manipulator_link_0:
+          Value: true
+        apple_kraken_shiny_2/kraken_manipulator_link_1:
+          Value: true
+        apple_kraken_shiny_2/kraken_manipulator_link_2:
+          Value: true
+        apple_kraken_shiny_2/kraken_manipulator_link_3:
+          Value: true
+        apple_kraken_shiny_2/kraken_manipulator_link_4:
+          Value: true
+        apple_kraken_shiny_2/lidar:
+          Value: true
+        apple_kraken_shiny_2/lidar_mount:
+          Value: true
+        apple_kraken_shiny_2/map:
+          Value: true
+        apple_kraken_shiny_2/odom:
+          Value: true
+        apple_kraken_shiny_2/steering_front_left_link:
+          Value: true
+        apple_kraken_shiny_2/steering_front_right_link:
+          Value: true
+        apple_kraken_shiny_2/wheel_front_left_link:
+          Value: true
+        apple_kraken_shiny_2/wheel_front_right_link:
+          Value: true
+        apple_kraken_shiny_2/wheel_rear_left_link:
+          Value: true
+        apple_kraken_shiny_2/wheel_rear_right_link:
+          Value: true
+        apple_kraken_shiny_4/base_link:
+          Value: true
+        apple_kraken_shiny_4/kraken_manipulator_link_0:
+          Value: true
+        apple_kraken_shiny_4/kraken_manipulator_link_1:
+          Value: true
+        apple_kraken_shiny_4/kraken_manipulator_link_2:
+          Value: true
+        apple_kraken_shiny_4/kraken_manipulator_link_3:
+          Value: true
+        apple_kraken_shiny_4/kraken_manipulator_link_4:
+          Value: true
+        apple_kraken_shiny_4/lidar:
+          Value: true
+        apple_kraken_shiny_4/lidar_mount:
+          Value: true
+        apple_kraken_shiny_4/map:
+          Value: true
+        apple_kraken_shiny_4/odom:
+          Value: true
+        apple_kraken_shiny_4/steering_front_left_link:
+          Value: true
+        apple_kraken_shiny_4/steering_front_right_link:
+          Value: true
+        apple_kraken_shiny_4/wheel_front_left_link:
+          Value: true
+        apple_kraken_shiny_4/wheel_front_right_link:
+          Value: true
+        apple_kraken_shiny_4/wheel_rear_left_link:
+          Value: true
+        apple_kraken_shiny_4/wheel_rear_right_link:
+          Value: true
+        map:
+          Value: true
+      Marker Scale: 3
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: false
+      Tree:
+        map:
+          apple_kraken_rusty_1/map:
+            apple_kraken_rusty_1/odom:
+              apple_kraken_rusty_1/base_link:
+                apple_kraken_rusty_1/kraken_manipulator_link_0:
+                  {}
+                apple_kraken_rusty_1/kraken_manipulator_link_1:
+                  apple_kraken_rusty_1/kraken_manipulator_link_2:
+                    apple_kraken_rusty_1/kraken_manipulator_link_3:
+                      apple_kraken_rusty_1/kraken_manipulator_link_4:
+                        {}
+                apple_kraken_rusty_1/lidar_mount:
+                  apple_kraken_rusty_1/lidar:
+                    {}
+                apple_kraken_rusty_1/steering_front_left_link:
+                  apple_kraken_rusty_1/wheel_front_left_link:
+                    {}
+                apple_kraken_rusty_1/steering_front_right_link:
+                  apple_kraken_rusty_1/wheel_front_right_link:
+                    {}
+                apple_kraken_rusty_1/wheel_rear_left_link:
+                  {}
+                apple_kraken_rusty_1/wheel_rear_right_link:
+                  {}
+          apple_kraken_rusty_3/map:
+            apple_kraken_rusty_3/odom:
+              apple_kraken_rusty_3/base_link:
+                apple_kraken_rusty_3/kraken_manipulator_link_0:
+                  {}
+                apple_kraken_rusty_3/kraken_manipulator_link_1:
+                  apple_kraken_rusty_3/kraken_manipulator_link_2:
+                    apple_kraken_rusty_3/kraken_manipulator_link_3:
+                      apple_kraken_rusty_3/kraken_manipulator_link_4:
+                        {}
+                apple_kraken_rusty_3/lidar_mount:
+                  apple_kraken_rusty_3/lidar:
+                    {}
+                apple_kraken_rusty_3/steering_front_left_link:
+                  apple_kraken_rusty_3/wheel_front_left_link:
+                    {}
+                apple_kraken_rusty_3/steering_front_right_link:
+                  apple_kraken_rusty_3/wheel_front_right_link:
+                    {}
+                apple_kraken_rusty_3/wheel_rear_left_link:
+                  {}
+                apple_kraken_rusty_3/wheel_rear_right_link:
+                  {}
+          apple_kraken_shiny_2/map:
+            apple_kraken_shiny_2/odom:
+              apple_kraken_shiny_2/base_link:
+                apple_kraken_shiny_2/kraken_manipulator_link_0:
+                  {}
+                apple_kraken_shiny_2/kraken_manipulator_link_1:
+                  apple_kraken_shiny_2/kraken_manipulator_link_2:
+                    apple_kraken_shiny_2/kraken_manipulator_link_3:
+                      apple_kraken_shiny_2/kraken_manipulator_link_4:
+                        {}
+                apple_kraken_shiny_2/lidar_mount:
+                  apple_kraken_shiny_2/lidar:
+                    {}
+                apple_kraken_shiny_2/steering_front_left_link:
+                  apple_kraken_shiny_2/wheel_front_left_link:
+                    {}
+                apple_kraken_shiny_2/steering_front_right_link:
+                  apple_kraken_shiny_2/wheel_front_right_link:
+                    {}
+                apple_kraken_shiny_2/wheel_rear_left_link:
+                  {}
+                apple_kraken_shiny_2/wheel_rear_right_link:
+                  {}
+          apple_kraken_shiny_4/map:
+            apple_kraken_shiny_4/odom:
+              apple_kraken_shiny_4/base_link:
+                apple_kraken_shiny_4/kraken_manipulator_link_0:
+                  {}
+                apple_kraken_shiny_4/kraken_manipulator_link_1:
+                  apple_kraken_shiny_4/kraken_manipulator_link_2:
+                    apple_kraken_shiny_4/kraken_manipulator_link_3:
+                      apple_kraken_shiny_4/kraken_manipulator_link_4:
+                        {}
+                apple_kraken_shiny_4/lidar_mount:
+                  apple_kraken_shiny_4/lidar:
+                    {}
+                apple_kraken_shiny_4/steering_front_left_link:
+                  apple_kraken_shiny_4/wheel_front_left_link:
+                    {}
+                apple_kraken_shiny_4/steering_front_right_link:
+                  apple_kraken_shiny_4/wheel_front_right_link:
+                    {}
+                apple_kraken_shiny_4/wheel_rear_left_link:
+                  {}
+                apple_kraken_shiny_4/wheel_rear_right_link:
+                  {}
+      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: 255; 0; 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.10000000149011612
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Best Effort
+        Value: /scan
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz_default_plugins/Map
+      Color Scheme: costmap
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /apple_kraken_rusty_1/local_costmap/costmap
+      Update Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /apple_kraken_rusty_1/local_costmap/costmap_updates
+      Use Timestamp: false
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz_default_plugins/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /apple_kraken_rusty_1/global_costmap/costmap
+      Update Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /apple_kraken_rusty_1/global_costmap/costmap_updates
+      Use Timestamp: false
+      Value: true
+    - 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: /apple_kraken_rusty_1/plan
+      Value: true
+    - Alpha: 1
+      Class: rviz_default_plugins/Polygon
+      Color: 25; 255; 0
+      Enabled: true
+      Name: Polygon
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /apple_kraken_rusty_1/local_costmap/published_footprint
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz_default_plugins/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /apple_kraken_shiny_2/global_costmap/costmap
+      Update Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /apple_kraken_shiny_2/global_costmap/costmap_updates
+      Use Timestamp: false
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz_default_plugins/Map
+      Color Scheme: costmap
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /apple_kraken_shiny_2/local_costmap/costmap
+      Update Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /apple_kraken_shiny_2/local_costmap/costmap_updates
+      Use Timestamp: false
+      Value: true
+    - 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: /apple_kraken_shiny_2/plan
+      Value: true
+    - Alpha: 1
+      Class: rviz_default_plugins/Polygon
+      Color: 25; 255; 0
+      Enabled: true
+      Name: Polygon
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /apple_kraken_shiny_2/local_costmap/published_footprint
+      Value: true
+    - Alpha: 1
+      Class: rviz_default_plugins/Polygon
+      Color: 25; 255; 0
+      Enabled: true
+      Name: Polygon
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /apple_kraken_rusty_3/local_costmap/published_footprint
+      Value: true
+    - Alpha: 1
+      Class: rviz_default_plugins/Polygon
+      Color: 25; 255; 0
+      Enabled: true
+      Name: Polygon
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /apple_kraken_shiny_4/global_costmap/published_footprint
+      Value: true
+    - 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: /apple_kraken_rusty_3/plan
+      Value: true
+    - 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: /apple_kraken_shiny_4/plan
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: map
+    Frame Rate: 30
+  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: /apple_kraken_rusty_1/goal_pose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /apple_kraken_shiny_2/goal_pose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /apple_kraken_rusty_3/goal_pose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /apple_kraken_shiny_4/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:
+      Angle: 0
+      Class: rviz_default_plugins/TopDownOrtho
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Scale: 28.554161071777344
+      Target Frame: <Fixed Frame>
+      Value: TopDownOrtho (rviz_default_plugins)
+      X: -59.6247444152832
+      Y: 35.75860595703125
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1151
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000003e1fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003e1000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003e1fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003e1000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007920000003efc0100000002fb0000000800540069006d0065010000000000000792000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000521000003e100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1938
+  X: 60
+  Y: 60

kraken_nav/launch/config/navigation_multi_params.yaml

@@ -0,0 +1,293 @@
+bt_navigator:
+  ros__parameters:
+    use_sim_time: True
+    global_frame: map
+    robot_base_frame: base_link
+    odom_topic: /odom
+    bt_loop_duration: 10
+    default_server_timeout: 20
+    enable_groot_monitoring: False
+    groot_zmq_publisher_port: 1666
+    groot_zmq_server_port: 1667
+    # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
+    # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
+    # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
+    # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
+    default_nav_to_pose_bt_xml: "bt.xml" 
+    plugin_lib_names:
+    - nav2_compute_path_to_pose_action_bt_node
+    - nav2_compute_path_through_poses_action_bt_node
+    - nav2_follow_path_action_bt_node
+    - nav2_back_up_action_bt_node
+    - nav2_spin_action_bt_node
+    - nav2_wait_action_bt_node
+    - nav2_clear_costmap_service_bt_node
+    - nav2_is_stuck_condition_bt_node
+    - nav2_goal_reached_condition_bt_node
+    - nav2_goal_updated_condition_bt_node
+    - nav2_initial_pose_received_condition_bt_node
+    - nav2_reinitialize_global_localization_service_bt_node
+    - nav2_rate_controller_bt_node
+    - nav2_distance_controller_bt_node
+    - nav2_speed_controller_bt_node
+    - nav2_truncate_path_action_bt_node
+    - nav2_goal_updater_node_bt_node
+    - nav2_recovery_node_bt_node
+    - nav2_pipeline_sequence_bt_node
+    - nav2_round_robin_node_bt_node
+    - nav2_transform_available_condition_bt_node
+    - nav2_time_expired_condition_bt_node
+    - nav2_distance_traveled_condition_bt_node
+    - nav2_single_trigger_bt_node
+    - nav2_is_battery_low_condition_bt_node
+    - nav2_navigate_through_poses_action_bt_node
+    - nav2_navigate_to_pose_action_bt_node
+    - nav2_remove_passed_goals_action_bt_node
+    - nav2_planner_selector_bt_node
+    - nav2_controller_selector_bt_node
+    - nav2_goal_checker_selector_bt_node
+
+bt_navigator_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+controller_server:
+  ros__parameters:
+    controller_frequency: 10.0
+    min_x_velocity_threshold: 0.2
+    min_y_velocity_threshold: 0.5
+    min_theta_velocity_threshold: 0.001
+    progress_checker_plugin: "progress_checker"
+    goal_checker_plugins: ["goal_checker"]
+    controller_plugins: ["FollowPath"]
+
+    progress_checker:
+      plugin: "nav2_controller::SimpleProgressChecker"
+      required_movement_radius: 1.0
+      movement_time_allowance: 5.0
+    goal_checker:
+      plugin: "nav2_controller::SimpleGoalChecker"
+      xy_goal_tolerance: 0.5
+      yaw_goal_tolerance: 0.2
+      stateful: True
+    FollowPath:
+      plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
+      desired_linear_vel: 1.0
+      lookahead_dist: 0.5
+      min_lookahead_dist: 0.4
+      max_lookahead_dist: 0.9
+      lookahead_time: 3.5
+      rotate_to_heading_angular_vel: 0.2
+      transform_tolerance: 0.2
+      use_velocity_scaled_lookahead_dist: true
+      min_approach_linear_velocity: 0.3
+      use_approach_linear_velocity_scaling: true
+      approach_velocity_scaling_dist: 6.5
+      max_allowed_time_to_collision_up_to_carrot: 1.0
+      use_regulated_linear_velocity_scaling: true
+      use_cost_regulated_linear_velocity_scaling: true
+      regulated_linear_scaling_min_radius: 4.2
+      regulated_linear_scaling_min_speed: 0.50
+      use_rotate_to_heading: false
+      rotate_to_heading_min_angle: 0.785
+      max_angular_accel: 1.2
+      max_robot_pose_search_dist: 10.0
+      use_interpolation: false
+      cost_scaling_dist: 0.3
+      cost_scaling_gain: 1.0
+      inflation_cost_scaling_factor: 1.0
+      
+controller_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      width: 20
+      height: 20
+      update_frequency: 5.0
+      publish_frequency: 2.0
+      global_frame: odom
+      robot_base_frame: base_link
+      use_sim_time: True
+      rolling_window: true
+      resolution: 0.1
+      plugins: ["obstacle_layer", "inflation_layer"]
+      footprint: "[ [2.5, 0.4], [2.5, -0.4], [-0.5, -0.4], [-0.5, 0.4] ]"
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 20.0
+        inflation_radius: 10.20
+      obstacle_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
+        enabled: True
+        observation_sources: scan
+        scan:
+          topic: scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+          raytrace_max_range: 20.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 20.0
+          obstacle_min_range: 0.0
+      always_send_full_costmap: True
+  local_costmap_client:
+    ros__parameters:
+      use_sim_time: True
+  local_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: True
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      origin_x: -50.0
+      origin_y: -50.0
+      width: 100
+      height: 100
+      update_frequency: 2.0
+      publish_frequency: 2.0
+      global_frame: map
+      robot_base_frame: base_link
+      use_sim_time: True
+      footprint: "[ [2.5, 0.4], [2.5, -0.4], [-0.5, -0.4], [-0.5, 0.4] ]"
+      resolution: 0.2
+      rolling_window: true
+      track_unknown_space: true
+      plugins: ["obstacle_layer", "inflation_layer"]
+      obstacle_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
+        enabled: True
+        observation_sources: scan
+        scan:
+          topic: scan
+          max_obstacle_height: 4.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+          raytrace_max_range: 40.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 40.0
+          obstacle_min_range: 0.0
+      inflation_layer:
+        enabled: True
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 0.8
+        inflation_radius: 15.0
+      always_send_full_costmap: True
+  global_costmap_client:
+    ros__parameters:
+      use_sim_time: True
+  global_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: True
+
+map_saver:
+  ros__parameters:
+    use_sim_time: True
+    save_map_timeout: 5.0
+    free_thresh_default: 0.25
+    occupied_thresh_default: 0.65
+    map_subscribe_transient_local: True
+
+planner_server:
+  ros__parameters:
+    expected_planner_frequency: 10.0
+    use_sim_time: True
+    planner_plugins: ["GridBased"]
+    GridBased:
+      plugin: "nav2_smac_planner/SmacPlannerHybrid"
+      tolerance: 1.0                      # tolerance for planning if unable to reach exact pose, in meters, for 2D node
+      downsample_costmap: false           # whether or not to downsample the map
+      downsampling_factor: 1              # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm)
+      allow_unknown: true                # allow traveling in unknown space
+      max_iterations: 1000000             # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable
+      max_on_approach_iterations: 1000    # maximum number of iterations to attempt to reach goal once in tolerance, 2D only
+      max_planning_time: 3.5              # max time in s for planner to plan, smooth, and upsample. Will scale maximum smoothing and upsampling times based on remaining time after planning.
+      motion_model_for_search: "DUBIN"    # For Hybrid Dubin, Redds-Shepp
+      # cost_travel_multiplier: 1.0         # For 2D: Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*.
+      angle_quantization_bins: 64         # For Hybrid nodes: Number of angle bins for search, must be 1 for 2D node (no angle search)
+      analytic_expansion_ratio: 1.5       # For Hybrid/Lattice nodes: The ratio to attempt analytic expansions during search for final approach.
+      analytic_expansion_max_length: 3.0    # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting (in meters). This should be scaled with minimum turning radius and be no less than 4-5x the minimum radius
+      minimum_turning_radius: 3.2        # For Hybrid/Lattice nodes: minimum turning radius in m of path / vehicle
+      # reverse_penalty: 2.1                # For Reeds-Shepp model: penalty to apply if motion is reversing, must be => 1
+      change_penalty: 3.0                 # For Hybrid nodes: penalty to apply if motion is changing directions, must be >= 0
+      non_straight_penalty: 6.5          # For Hybrid nodes: penalty to apply if motion is non-straight, must be => 1
+      cost_penalty: 0.5                   # For Hybrid nodes: penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable.
+      retrospective_penalty: 0.025        # For Hybrid/Lattice nodes: penalty to prefer later maneuvers before earlier along the path. Saves search time since earlier nodes are not expanded until it is necessary. Must be >= 0.0 and <= 1.0
+      # rotation_penalty: 5.0               # For Lattice node: Penalty to apply only to pure rotate in place commands when using minimum control sets containing rotate in place primitives. This should always be set sufficiently high to weight against this action unless strictly necessary for obstacle avoidance or there may be frequent discontinuities in the plan where it requests the robot to rotate in place to short-cut an otherwise smooth path for marginal path distance savings.
+      lookup_table_size: 20.0               # For Hybrid nodes: Size of the dubin/reeds-sheep distance window to cache, in meters.
+      cache_obstacle_heuristic: True      # For Hybrid nodes: Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static.  
+      # allow_reverse_expansion: True      # For Lattice nodes: Whether to expand state lattice graph in forward primitives or reverse as well, will double the branching factor at each step.   
+      smooth_path: True                   # For Lattice/Hybrid nodes: Whether or not to smooth the path, always true for 2D nodes.
+      smoother:
+        max_iterations: 1000
+        w_smooth: 0.3
+        w_data: 0.2
+        tolerance: 1e-10
+        do_refinement: true               # Whether to recursively run the smoother 3 times on the results from prior runs to refine the results further
+
+# planner_server:
+#   ros__parameters:
+#     planner_plugins: ['GridBased']
+#     GridBased:
+#       plugin: 'nav2_navfn_planner/NavfnPlanner'
+#       use_astar: True
+#       allow_unknown: True
+#       tolerance: 1.0
+
+planner_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+recoveries_server:
+  ros__parameters:
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    recovery_plugins: ["spin", "backup", "wait"]
+    spin:
+      plugin: "nav2_recoveries/Spin"
+    backup:
+      plugin: "nav2_recoveries/BackUp"
+    wait:
+      plugin: "nav2_recoveries/Wait"
+    global_frame: odom
+    robot_base_frame: base_link
+    transform_timeout: 0.1
+    use_sim_time: true
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+
+robot_state_publisher:
+  ros__parameters:
+    use_sim_time: True
+
+waypoint_follower:
+  ros__parameters:
+    loop_rate: 20
+    stop_on_failure: false
+    waypoint_task_executor_plugin: "wait_at_waypoint"   
+    wait_at_waypoint:
+      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
+      enabled: True
+      waypoint_pause_duration: 200
+
+velocity_smoother:
+  ros__parameters:
+    smoothing_frequency: 20.0
+    scale_velocities: false
+    feedback: "OPEN_LOOP"
+    max_velocity: [1.0, 0.0, 2.5]
+    min_velocity: [-0.5, 0.0, -2.5]
+    deadband_velocity: [0.0, 0.0, 0.0]
+    velocity_timeout: 1.0
+    max_accel: [2.5, 0.0, 3.2]
+    max_decel: [-2.5, 0.0, -3.2]
+    odom_topic: "odom"
+    odom_duration: 0.1

kraken_nav/launch/config/navigation_params.yaml

@@ -72,7 +72,7 @@ controller_server:
       stateful: True
     FollowPath:
       plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
-      desired_linear_vel: 0.8
+      desired_linear_vel: 1.0
       lookahead_dist: 0.5
       min_lookahead_dist: 0.4
       max_lookahead_dist: 0.9
@@ -114,7 +114,7 @@ local_costmap:
       rolling_window: true
       resolution: 0.1
       plugins: ["obstacle_layer", "inflation_layer"]
-      footprint: "[ [2.5, 0.8], [2.5, -0.8], [-0.5, -0.8], [-0.5, 0.8] ]"
+      footprint: "[ [2.5, 0.4], [2.5, -0.4], [-0.5, -0.4], [-0.5, 0.4] ]"
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 20.0
@@ -153,7 +153,7 @@ global_costmap:
       global_frame: map
       robot_base_frame: base_link
       use_sim_time: True
-      footprint: "[ [2.5, 0.8], [2.5, -0.8], [-0.5, -0.8], [-0.5, 0.8] ]"
+      footprint: "[ [2.5, 0.4], [2.5, -0.4], [-0.5, -0.4], [-0.5, 0.4] ]"
       resolution: 0.2
       rolling_window: true
       track_unknown_space: true

kraken_nav/launch/config/slam_multi_params.yaml

@@ -0,0 +1,67 @@
+slam_toolbox:
+  ros__parameters:
+    use_sim_time: true
+
+    # Plugin params
+    solver_plugin: solver_plugins::CeresSolver
+    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
+    ceres_preconditioner: SCHUR_JACOBI
+    ceres_trust_strategy: LEVENBERG_MARQUARDT
+    ceres_dogleg_type: TRADITIONAL_DOGLEG
+    ceres_loss_function: None
+
+    # ROS Parameters
+    odom_frame: odom
+    map_frame: map
+    base_frame: base_link
+    scan_topic: scan
+    mode: mapping #localization
+
+    debug_logging: false
+    throttle_scans: 1
+    transform_publish_period: 0.02 #if 0 never publishes odometry
+    map_update_interval: 1.0
+    resolution: 0.1
+    max_laser_range: 100.0 #for rastering images
+    minimum_time_interval: 0.5
+    transform_timeout: 0.2
+    tf_buffer_duration: 30.
+    stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
+    enable_interactive_mode: true
+
+    # General Parameters
+    use_scan_matching: true
+    use_scan_barycenter: true
+    minimum_travel_distance: 0.5
+    minimum_travel_heading: 0.5
+    scan_buffer_size: 10
+    scan_buffer_maximum_scan_distance: 10.0
+    link_match_minimum_response_fine: 0.1  
+    link_scan_maximum_distance: 1.5
+    loop_search_maximum_distance: 3.0
+    do_loop_closing: true 
+    loop_match_minimum_chain_size: 10           
+    loop_match_maximum_variance_coarse: 3.0  
+    loop_match_minimum_response_coarse: 0.35    
+    loop_match_minimum_response_fine: 0.45
+
+    # Correlation Parameters - Correlation Parameters
+    correlation_search_space_dimension: 0.5
+    correlation_search_space_resolution: 0.01
+    correlation_search_space_smear_deviation: 0.1 
+
+    # Correlation Parameters - Loop Closure Parameters
+    loop_search_space_dimension: 8.0
+    loop_search_space_resolution: 0.05
+    loop_search_space_smear_deviation: 0.03
+
+    # Scan Matcher Parameters
+    distance_variance_penalty: 0.5      
+    angle_variance_penalty: 1.0    
+
+    fine_search_angle_offset: 0.00349     
+    coarse_search_angle_offset: 0.349   
+    coarse_angle_resolution: 0.0349        
+    minimum_angle_penalty: 0.9
+    minimum_distance_penalty: 0.5
+    use_response_expansion: true

kraken_nav/launch/navigation_multi.launch.py

@@ -0,0 +1,272 @@
+# Copyright 2019-2022 Robotec.ai.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import pathlib
+from unicodedata import name
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, GroupAction, DeclareLaunchArgument
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PythonExpression, TextSubstitution
+from launch_ros.actions import Node, PushRosNamespace
+from nav2_common.launch import RewrittenYaml
+from launch.conditions import IfCondition, UnlessCondition
+
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+
+def substitute_namespace(namespace, value):
+    if not namespace:
+        return TextSubstitution(text=value)
+    else:
+        return PythonExpression(['str("', namespace, '")', "+", f"'/{value}'"])
+    
+def substitute_name(namespace, value):
+    if not namespace:
+        return TextSubstitution(text=value)
+    else:
+        return PythonExpression(['str("', namespace, '")', "+", f"'_{value}'"])
+
+def generate_launch_description():
+    namespace = LaunchConfiguration('namespace')
+    declare_namespace_cmd = DeclareLaunchArgument(
+        'namespace',
+        default_value='',
+    )
+
+    use_slam = LaunchConfiguration('use_slam')
+    declare_use_slam_cmd = DeclareLaunchArgument(
+        'use_slam',
+        default_value='False',
+    )
+    
+    rviz = LaunchConfiguration('rviz')
+    declare_rviz_cmd = DeclareLaunchArgument(
+        'rviz',
+        default_value='True',
+    )
+
+    package_dir = get_package_share_directory("o3de_kraken_nav")
+    slam_toolbox_dir = get_package_share_directory('slam_toolbox')
+    nav2_dir = get_package_share_directory("nav2_bringup")
+
+    nav2_params_file = str(pathlib.Path(package_dir).joinpath('launch', 'config', 'navigation_multi_params.yaml'))
+    bt_xml_file = str(pathlib.Path(package_dir).joinpath('launch', 'config', 'bt.xml'))
+    slam_params_file = str(pathlib.Path(package_dir).joinpath('launch', 'config', 'slam_multi_params.yaml'))
+
+    nav_param_substitutions = {
+        'default_nav_to_pose_bt_xml': bt_xml_file,
+        'robot_base_frame': substitute_namespace(namespace, "base_link"),
+        'local_costmap.local_costmap.ros__parameters.global_frame': substitute_namespace(namespace, "odom"),
+        'global_costmap.global_costmap.ros__parameters.global_frame': substitute_namespace(namespace, "map"),
+        'bt_navigator.ros__parameters.global_frame': substitute_namespace(namespace, "map"),
+        # 'topic': substitute_namespace(namespace, "scan")
+    }
+    slam_param_substitutions = {
+        'base_frame': substitute_namespace(namespace, "base_link"),
+        'odom_frame': substitute_namespace(namespace, "odom"),
+        'map_frame': substitute_namespace(namespace, "map")
+    }
+    
+    configured_nav2_params = RewrittenYaml(
+        source_file=nav2_params_file,
+        root_key='',
+        param_rewrites=nav_param_substitutions,
+        convert_types=True)
+    
+    configured_slam_params = RewrittenYaml(
+        source_file=slam_params_file,
+        root_key=namespace,
+        param_rewrites=slam_param_substitutions,
+        convert_types=True)
+    
+    slam = GroupAction(
+        condition=IfCondition(use_slam),
+        actions=[
+            PushRosNamespace(namespace),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource([str(pathlib.Path(slam_toolbox_dir).joinpath('launch', 'online_async_launch.py'))]),
+                launch_arguments = {
+                    'slam_params_file': configured_slam_params,
+                }.items()
+            ),
+        ]
+    )
+    
+    local_costmap_scan_relay = Node(
+        name="pc_relay",
+        package="topic_tools",
+        executable="relay",
+        parameters=[
+            {'input_topic': substitute_namespace(namespace, 'scan')},
+            {'output_topic': substitute_namespace(namespace, 'local_costmap/scan')}
+        ]
+    )
+
+    global_costmap_scan_relay = Node(
+        name="pc_relay",
+        package="topic_tools",
+        executable="relay",
+        parameters=[
+            {'input_topic': substitute_namespace(namespace, 'scan')},
+            {'output_topic': substitute_namespace(namespace, 'global_costmap/scan')}
+        ]
+    )
+
+    pc_relay = Node(
+        name="pc_relay",
+        package="topic_tools",
+        executable="relay",
+        parameters=[
+            {'input_topic': substitute_namespace(namespace, 'pc')},
+            {'output_topic': '/pc'}
+        ]
+    )
+
+    tf_relay = Node(
+        name="tf_relay",
+        package="topic_tools",
+        executable="relay",
+        parameters=[
+            {'input_topic': '/tf'},
+            {'output_topic': substitute_namespace(namespace, 'tf')}
+        ]
+    )
+
+    tf_static_relay = Node(
+        name="tf_static_relay",
+        package="topic_tools",
+        executable="relay",
+        parameters=[
+            {'input_topic': '/tf_static'},
+            {'output_topic': substitute_namespace(namespace, 'tf_static')}
+        ]
+    )
+
+    tf_odom_container = ComposableNodeContainer(
+        condition=UnlessCondition(use_slam),
+        name='image_container',
+        namespace=namespace,
+        package='rclcpp_components',
+        executable='component_container',
+        composable_node_descriptions=[
+            ComposableNode(
+                package='tf2_ros',
+                plugin='tf2_ros::StaticTransformBroadcasterNode',
+                parameters=[
+                    {'frame_id': substitute_namespace(namespace, 'map')},
+                    {'child_frame_id': substitute_namespace(namespace, 'odom')}
+                ]
+            )
+        ],
+        output='both',
+    )
+
+    tf_map_container = ComposableNodeContainer(
+        name='image_container',
+        namespace=namespace,
+        package='rclcpp_components',
+        executable='component_container',
+        composable_node_descriptions=[
+            ComposableNode(
+                package='tf2_ros',
+                plugin='tf2_ros::StaticTransformBroadcasterNode',
+                parameters=[
+                    {'frame_id': 'map'},
+                    {'child_frame_id': substitute_namespace(namespace, 'map')}
+                ]
+            )
+        ],
+        output='both',
+    )
+
+    nav_nodes = GroupAction(
+        actions=[
+            PushRosNamespace(namespace),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource([str(pathlib.Path(nav2_dir).joinpath('launch', 'navigation_launch.py'))]),
+                launch_arguments = {
+                    'params_file': configured_nav2_params,
+                    'namespace': namespace,
+                    'use_sim_time': 'True',
+                    'autostart': 'True'
+                }.items()
+            )
+        ]
+    )
+    
+    pointcloud_to_laserscan = Node(
+        package='pointcloud_to_laserscan',
+        executable='pointcloud_to_laserscan_node',
+        name='pc_to_laserscan',
+        namespace=namespace,
+        parameters=[{
+            'min_height': 0.1,
+            'max_height': 5.0,
+            'range_min': 0.2,
+            'range_max': 20.0
+        }],
+        remappings=[
+            ('cloud_in', 'pc'),
+        ]
+    )
+    
+    twist_to_ackermann = Node(
+        package='o3de_kraken_nav',
+        executable='twist_to_ackermann',
+        name='twist_to_ackermann',
+        namespace=namespace,
+        parameters=[{
+            'wheelbase': 2.2,
+            'timeout_control_interval': 0.1,
+            'control_timeout': 0.2,
+            'publish_zeros_on_timeout': True
+        }],
+        remappings=[
+            ('/ackermann_vel', substitute_namespace(namespace, 'ackermann_vel')),
+        ]
+    )
+    
+    rviz = GroupAction(
+        condition=IfCondition(rviz),
+        actions=[
+            Node(
+                package='rviz2',
+                executable='rviz2',
+                name='slam',
+                arguments=[
+                    '-d', str(pathlib.Path(package_dir).joinpath('launch', 'config', 'config_multi.rviz')),
+                ]
+            )
+        ]
+    )
+    
+    ld = LaunchDescription()
+    ld.add_action(declare_namespace_cmd)
+    ld.add_action(declare_use_slam_cmd)
+    ld.add_action(declare_rviz_cmd)
+    ld.add_action(tf_odom_container)
+    ld.add_action(tf_map_container)
+    ld.add_action(tf_relay)
+    ld.add_action(local_costmap_scan_relay)
+    ld.add_action(global_costmap_scan_relay)
+    ld.add_action(tf_static_relay)
+    ld.add_action(pc_relay)
+    ld.add_action(pointcloud_to_laserscan)
+    ld.add_action(twist_to_ackermann)
+    ld.add_action(slam)
+    ld.add_action(nav_nodes)
+    ld.add_action(rviz)
+    
+    return ld

kraken_nav/o3de_kraken_nav/twist_to_ackermann.py

@@ -74,7 +74,7 @@ def main(args=None):
   rclpy.init(args=args)
   tta = TwistToAckermann()
   rclpy.spin(tta)
-  
+  tta.timer.cancel()
   tta.destroy_node()
   rclpy.shutdown()