Merge pull request #202 from o3de/kraken_nav

Merge ROS2 package development from the separate repository.

README.md

@@ -156,7 +156,7 @@ build/linux/bin/profile/Editor
 
 ## Building the Navigation package
 
-To build the ROS 2 navigation stack configured for this Project, please follow this [repository](https://github.com/RobotecAI/o3de_kraken_nav). Do not run it yet if you wish to follow the demo scenario.
+To build the ROS 2 navigation stack configured for this Project, please follow this [detailed document](kraken_nav/README.md). Do not run it yet if you wish to follow the demo scenario.
 
 # Running the demo scenario
 
@@ -166,7 +166,7 @@ You can try out the demo scenario as presented during ROSCon 2022. Take the foll
 2. Run the simulation with `Ctrl-G` or by pressing the Play button in the Editor. 
 3. When it loads, spawn your first Apple Kraken using the following command: `ros2 service call /spawn_entity gazebo_msgs/srv/SpawnEntity '{name: 'apple_kraken_rusty', xml: 'line1'}'`.
    1. You can learn more about spawning in [this section](#spawning-krakens)
-3. Once the simulation is running, start the [navigation stack](https://github.com/RobotecAI/o3de_kraken_nav). If you followed all the instructions for setting it up, do the following:
+3. Once the simulation is running, start the [navigation stack](kraken_nav/README.md). If you followed all the instructions for setting it up, do the following:
    1. launch the stack for the first robot with `ros2 launch o3de_kraken_nav navigation_multi.launch.py namespace:=apple_kraken_rusty_1 rviz:=True`. 
    1. You should see a new Rviz2 window.
    1. Note that the number index `_1` has been added to the namespace when it was automatically generated by the Spawner.
@@ -180,7 +180,7 @@ You can try out the demo scenario as presented during ROSCon 2022. Take the foll
    1. `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'}'`
-   1. You can also navigate with them using remaining `2D Goal Pose` buttons and trigger gathering events. Follow the instructions in [this section](https://github.com/RobotecAI/o3de_kraken_nav#running-nav-stack-for-multiple-vehicles) to launch navigation stack for each Kraken.
+   1. You can also navigate with them using remaining `2D Goal Pose` buttons and trigger gathering events. Follow the instructions in [this section](kraken_nav/README.md#running-nav-stack-for-multiple-vehicles) to launch navigation stack for each Kraken.
 
 💡 ***Note:*** If you would like to start the scenario over, **remember to close all the navigation stacks as well**. You can do this by pressing Ctrl-C in each console where you ran the `ros2 launch o3de_kraken_nav (..)` command.
 
@@ -188,7 +188,7 @@ ros2 service call /spawn_entity gazebo_msgs/srv/SpawnEntity '{name: 'apple_krake
 
 ### Navigation
 
-Please refer to [Kraken navigation project](https://github.com/RobotecAI/o3de_kraken_nav) for instructions.
+Please refer to [Kraken navigation](kraken_nav/README.md) for instructions.
 
 ### Triggering Apple Gathering
 

docker/Dockerfile

@@ -142,7 +142,7 @@ WORKDIR $WORKDIR
 
 RUN mkdir -p o3de_kraken_ws/src \
     && cd o3de_kraken_ws/src \
-    && git clone https://github.com/RobotecAI/o3de_kraken_nav.git
+    && ln -s $WORKDIR/ROSConDemo/kraken_nav kraken_nav
 
 RUN cd o3de_kraken_ws \
     && colcon build --symlink-install

kraken_nav/.gitignore

@@ -0,0 +1,2 @@
+.vscode
+__pycache__

kraken_nav/README.md

@@ -0,0 +1,208 @@
+# Apple kraken stack #
+
+This repository contains two ROS2 packages:
+ - o3de_kraken_nav
+ - o3de_kraken_orchestration
+
+## Packages Description
+
+### O3DE Kraken Navigation
+
+Provides navigation capabilities for the Apple Kraken vehicle.
+
+### O3DE Kraken Orchestration
+
+Allows the Apple Kraken vehicle to operate autonomously.
+
+## ROS2 prerequisites
+
+### Middleware
+
+- CycloneDDS
+
+```
+sudo apt install ros-${ROS_DISTRO}-cyclonedds ros-${ROS_DISTRO}-rmw-cyclonedds-cpp
+```
+
+It is required that `CycloneDDS` implementation is chosen in every working terminal:
+
+```
+export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
+```
+
+### Packages
+
+You will need to install the appropriate ROS2 package. Refer to setup requirements for the [ROS2 Gem](https://github.com/RobotecAI/o3de-ros2-gem/blob/development/README.md)
+
+In addition to the required packages for the ROS2 gem, you will also need some additional ROS2 packages.
+
+1.  Make sure to source the proper distribution's setup script
+
+    For Ubuntu 20.04 + ROS2 Galactic:
+    ```
+    source /opt/ros/galactic/setup.bash
+    ```
+
+    For Ubuntu 22.04 + ROS2 Humble:
+    ```
+    source /opt/ros/humble/setup.bash
+    ```
+
+2.  Run the following command to install the remaining required packages
+    ```
+    sudo apt install ros-${ROS_DISTRO}-slam-toolbox ros-${ROS_DISTRO}-navigation2 ros-${ROS_DISTRO}-nav2-bringup ros-${ROS_DISTRO}-pointcloud-to-laserscan ros-${ROS_DISTRO}-teleop-twist-keyboard ros-${ROS_DISTRO}-ackermann-msgs ros-${ROS_DISTRO}-topic-tools
+    ```
+
+3.  You will also need [colcon](https://colcon.readthedocs.io/en/released/user/installation.html) installed in order to build the workspace. Run the following command to install.
+
+    ```
+    sudo sh -c 'echo "deb [arch=amd64,arm64] http://repo.ros2.org/ubuntu/main `lsb_release -cs` main" > /etc/apt/sources.list.d/ros2-latest.list'
+
+    curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -
+
+    sudo apt update
+    sudo apt install python3-colcon-common-extensions
+    ```
+
+### Libraries
+
+The o3de_kraken_orchestration package uses the [python-statemachine](https://pypi.org/project/python-statemachine/) library.\
+You can install it by running the following command
+```bash
+pip install python-statemachine
+```
+
+**Note:** Make sure you have the pip Python package manager installed.
+
+## Installation ##
+
+- Use the [development](https://github.com/o3de/o3de/tree/development) branch of the `O3DE`.
+- Use the the [development](https://github.com/RobotecAI/o3de-ros2-gem/tree/development) branch of the `o3de-ros-gem`.
+- Use the [main](https://github.com/aws-lumberyard/ROSConDemo) branch of the `ROSConDemo`.
+
+1. Source ROS2 (assumed `humble`)
+
+```bash
+source /opt/ros/humble/setup.bash
+```
+
+2. Build the workspace
+
+```bash
+cd ./kraken_nav
+colcon build --symlink-install
+```
+
+## Running simulation
+
+1. [Build](https://github.com/o3de/ROSConDemo#download-and-install) and run the `ROSConDemo`
+2. Load level `Main`
+3. Start a simulation by hitting `CTRL+G`
+
+## Usage scenario
+
+## Base step (for every scenario)
+Please make sure that the enviroment is correct.
+Note that you need to set it up for every console.
+You can consider adding it to your `~/.bashrc`.
+
+1. Make sure that you have ROS2 workspace sourced
+
+```bash
+source ./kraken_nav/install/setup.bash
+```
+
+2. Make sure that you have `CycleDDS` as middleware 
+```bash
+export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
+```
+
+### Single robot teleoperation with joystick
+
+1. Start teleoperation launch, make sure that you have your joystick plugged in
+```bash
+ros2 launch o3de_kraken_nav teleop.launch.py namespace:=apple_kraken_rusty_1
+```
+
+2. Spawn a single robot
+```
+ros2 service call /spawn_entity gazebo_msgs/srv/SpawnEntity '{name: 'apple_kraken_rusty', xml: 'line1'}'
+```
+
+3. You should be able to drive the robot
+
+### Single robot navigation
+
+> 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. Run the navigation stack
+
+```bash
+ros2 launch o3de_kraken_nav navigation_multi.launch.py namespace:=apple_kraken_rusty_1 rviz:=True
+```
+
+2. Spawn the robot
+```bash
+ros2 service call /spawn_entity gazebo_msgs/srv/SpawnEntity '{name: 'apple_kraken_rusty', xml: 'line1'}'
+```
+
+3. You should be able to send goal to the robot. 
+Make sure that that you use only the first `2D Goal Pose`
+
+
+### Multiple robots navigation
+
+> 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. Run the first navigation stack with `Rviz` param set to `True`:
+
+```bash
+ros2 launch o3de_kraken_nav navigation_multi.launch.py namespace:=apple_kraken_rusty_1 rviz:=True
+```
+
+2. Run the navigation stack for rest of the robots (in separate terminals):
+
+```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
+```
+
+3. Spawn multiple 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'}'
+```
+
+4. You should be able to send goals to multiple robots.
+
+### Multiple robots with Kraken stack 
+
+In this scenario on top of four navigation stacks, the orchestrator nodes are executed.
+Those are providing navigation stacks with goals and trigger apple gathering.
+
+1. Run the first navigation stack with `Rviz` param set to `True`:
+
+```bash
+ros2 launch o3de_kraken_nav navigation_multi.launch.py namespace:=apple_kraken_rusty_1 rviz:=True
+```
+
+2. Run the navigation stack for the rest of the robots (in separate terminals):
+
+```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
+```
+
+3. Run the orchestration nodes for all the robots (in separate terminals):
+
+```bash
+ros2 run o3de_kraken_orchestration kraken_orchestration_node --ros-args -p robot_name:=apple_kraken_rusty_1 -p spawn_line:=line1
+ros2 run o3de_kraken_orchestration kraken_orchestration_node --ros-args -p robot_name:=apple_kraken_shiny_2 -p spawn_line:=line2
+ros2 run o3de_kraken_orchestration kraken_orchestration_node --ros-args -p robot_name:=apple_kraken_rusty_3 -p spawn_line:=line3
+ros2 run o3de_kraken_orchestration kraken_orchestration_node --ros-args -p robot_name:=apple_kraken_shiny_4 -p spawn_line:=line4
+```

kraken_nav/o3de_kraken_nav/launch/config/bt.xml

@@ -0,0 +1,8 @@
+<root main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <PipelineSequence name="NavigateWithReplanning">
+      <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
+      <FollowPath path="{path}"  controller_id="FollowPath"/>
+    </PipelineSequence>
+  </BehaviorTree>
+</root>

kraken_nav/o3de_kraken_nav/launch/config/config_multi.rviz

@@ -0,0 +1,749 @@
+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
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.10000000149011612
+      Class: rviz_default_plugins/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: Pose
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Arrow
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /apple_kraken_rusty_1/goal_pose
+      Value: true
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.10000000149011612
+      Class: rviz_default_plugins/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: Pose
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Arrow
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /apple_kraken_shiny_2/goal_pose
+      Value: true
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.10000000149011612
+      Class: rviz_default_plugins/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: Pose
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Arrow
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /apple_kraken_rusty_3/goal_pose
+      Value: true
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.10000000149011612
+      Class: rviz_default_plugins/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: Pose
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Arrow
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /apple_kraken_shiny_4/goal_pose
+      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/o3de_kraken_nav/launch/config/navigation_multi_params.yaml

@@ -0,0 +1,301 @@
+#
+# Copyright (c) Contributors to the Open 3D Engine Project.
+# For complete copyright and license terms please see the LICENSE at the root of this distribution.
+#
+# SPDX-License-Identifier: Apache-2.0 OR MIT
+#
+#
+
+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: 60.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 60.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: 60.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 60.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/o3de_kraken_nav/launch/config/navigation_params.yaml

@@ -0,0 +1,301 @@
+#
+# Copyright (c) Contributors to the Open 3D Engine Project.
+# For complete copyright and license terms please see the LICENSE at the root of this distribution.
+#
+# SPDX-License-Identifier: Apache-2.0 OR MIT
+#
+#
+
+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: True
+    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: 60.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 60.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: 60.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 60.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/o3de_kraken_nav/launch/config/slam_multi_params.yaml

@@ -0,0 +1,75 @@
+#
+# Copyright (c) Contributors to the Open 3D Engine Project.
+# For complete copyright and license terms please see the LICENSE at the root of this distribution.
+#
+# SPDX-License-Identifier: Apache-2.0 OR MIT
+#
+#
+
+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/o3de_kraken_nav/launch/config/slam_params.yaml

@@ -0,0 +1,81 @@
+#
+# Copyright (c) Contributors to the Open 3D Engine Project.
+# For complete copyright and license terms please see the LICENSE at the root of this distribution.
+#
+# SPDX-License-Identifier: Apache-2.0 OR MIT
+#
+#
+
+slam_toolbox:
+  ros__parameters:
+
+    # 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
+
+    # if you'd like to immediately start continuing a map at a given pose
+    # or at the dock, but they are mutually exclusive, if pose is given
+    # will use pose
+    #map_file_name: test_steve
+    # map_start_pose: [0.0, 0.0, 0.0]
+    #map_start_at_dock: true
+
+    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/o3de_kraken_nav/launch/navigation.launch.py

@@ -0,0 +1,101 @@
+#
+# Copyright (c) Contributors to the Open 3D Engine Project.
+# For complete copyright and license terms please see the LICENSE at the root of this distribution.
+#
+# SPDX-License-Identifier: Apache-2.0 OR MIT
+#
+#
+
+import pathlib
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from nav2_common.launch import RewrittenYaml
+
+
+def generate_launch_description():
+    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_params.yaml'))
+    bt_xml_file = str(pathlib.Path(package_dir).joinpath('launch', 'config', 'bt.xml'))
+
+    param_substitutions = {
+        'default_nav_to_pose_bt_xml': bt_xml_file}
+    configured_nav2_params = RewrittenYaml(
+        source_file=nav2_params_file,
+        root_key='',
+        param_rewrites=param_substitutions,
+        convert_types=True)
+
+    online_async_file = pathlib.Path(slam_toolbox_dir).joinpath('launch',
+                                                                'online_async_launch.py')
+    slam_params_file = pathlib.Path(package_dir).joinpath('launch',
+                                                          'config',
+                                                          'slam_params.yaml')
+    slam = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([str(online_async_file)]),
+        launch_arguments={
+            'slam_params_file': str(slam_params_file),
+        }.items()
+    )
+
+    navigation_launch = pathlib.Path(nav2_dir).joinpath('launch', 'navigation_launch.py')
+    navigation = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([str(navigation_launch)]),
+        launch_arguments={
+            'params_file': configured_nav2_params,
+        }.items()
+    )
+
+    pointcloud_to_laserscan = Node(
+        package='pointcloud_to_laserscan',
+        executable='pointcloud_to_laserscan_node',
+        name='pc_to_laserscan',
+        parameters=[{
+            'min_height': 0.1,
+            'max_height': 5.0,
+            'range_min': 0.2,
+            'range_max': 60.0
+        }],
+        remappings=[
+            ('/cloud_in', '/pc'),
+        ]
+    )
+
+    twist_to_ackermann = Node(
+        package='o3de_kraken_nav',
+        executable='twist_to_ackermann',
+        name='twist_to_ackermann',
+        parameters=[{
+            'wheelbase': 2.2,
+            'timeout_control_interval': 0.1,
+            'control_timeout': 0.2,
+            'publish_zeros_on_timeout': True
+        }]
+    )
+
+    rviz = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='slam',
+        arguments=[
+            '-d', str(pathlib.Path(package_dir).joinpath('launch',
+                                                         'config',
+                                                         'config.rviz')),
+        ]
+    )
+
+    ld = LaunchDescription()
+    ld.add_action(pointcloud_to_laserscan)
+    ld.add_action(twist_to_ackermann)
+    ld.add_action(slam)
+    ld.add_action(navigation)
+    ld.add_action(rviz)
+
+    return ld

kraken_nav/o3de_kraken_nav/launch/navigation_multi.launch.py

@@ -0,0 +1,279 @@
+#
+# Copyright (c) Contributors to the Open 3D Engine Project.
+# For complete copyright and license terms please see the LICENSE at the root of this distribution.
+#
+# SPDX-License-Identifier: Apache-2.0 OR MIT
+#
+#
+
+
+import pathlib
+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)
+
+    online_async_launch = pathlib.Path(slam_toolbox_dir).joinpath('launch',
+                                                                  'online_async_launch.py')
+    slam = GroupAction(
+        condition=IfCondition(use_slam),
+        actions=[
+            PushRosNamespace(namespace),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource([str(online_async_launch)]),
+                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',
+    )
+
+    navigation_launch = pathlib.Path(nav2_dir).joinpath('launch', 'navigation_launch.py')
+    nav_nodes = GroupAction(
+        actions=[
+            PushRosNamespace(namespace),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource([str(navigation_launch)]),
+                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': 60.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/launch/teleop.launch.py

@@ -0,0 +1,35 @@
+#
+# Copyright (c) Contributors to the Open 3D Engine Project.
+# For complete copyright and license terms please see the LICENSE at the root of this distribution.
+#
+# SPDX-License-Identifier: Apache-2.0 OR MIT
+#
+#
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration
+from launch.actions import DeclareLaunchArgument
+
+
+def generate_launch_description():
+    namespace = LaunchConfiguration('namespace')
+    declare_namespace_cmd = DeclareLaunchArgument(
+        'namespace',
+        default_value='',
+    )
+    return LaunchDescription([
+        declare_namespace_cmd,
+        Node(
+            package='o3de_kraken_nav',
+            executable='joy_to_ackermann',
+            name='joy_to_ackermann',
+            namespace=namespace
+        ),
+        Node(
+            package='joy',
+            namespace='',
+            executable='joy_node',
+            name='joy_node'
+        ),
+    ])

kraken_nav/o3de_kraken_nav/o3de_kraken_nav/__init__.py

@@ -0,0 +1,6 @@
+# Copyright (c) Contributors to the Open 3D Engine Project.
+# For complete copyright and license terms please see the LICENSE at the root of this distribution.
+#
+# SPDX-License-Identifier: Apache-2.0 OR MIT
+#
+#

kraken_nav/o3de_kraken_nav/o3de_kraken_nav/joy_to_ackermann.py

@@ -0,0 +1,47 @@
+#!/usr/bin/env python3
+
+#
+# Copyright (c) Contributors to the Open 3D Engine Project.
+# For complete copyright and license terms please see the LICENSE at the root of this distribution.
+#
+# SPDX-License-Identifier: Apache-2.0 OR MIT
+#
+#
+
+
+import rclpy
+from rclpy.node import Node
+
+from sensor_msgs.msg import Joy
+from ackermann_msgs.msg import AckermannDrive
+
+
+class JoyToAckermann(Node):
+    """ROS2 Node to convert joystick message to Ackermann Drive message."""
+
+    def __init__(self):
+        super().__init__('joy_to_ackermann')
+        self.publisher = self.create_publisher(AckermannDrive, 'ackermann_vel', 10)
+        self.subscription = self.create_subscription(Joy, "/joy", self.joy_callback, 10)
+
+    def joy_callback(self, msg):
+        """Create Ackermann message from the joystick message."""
+        drive = AckermannDrive()
+        drive.speed = 1.5 * msg.axes[1]
+        drive.steering_angle = 1.1 * msg.axes[0]
+        self.publisher.publish(drive)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    joyToAckermann = JoyToAckermann()
+
+    rclpy.spin(joyToAckermann)
+
+    joyToAckermann.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

kraken_nav/o3de_kraken_nav/o3de_kraken_nav/twist_to_ackermann.py

@@ -0,0 +1,87 @@
+#!/usr/bin/env python3
+
+#
+# Copyright (c) Contributors to the Open 3D Engine Project.
+# For complete copyright and license terms please see the LICENSE at the root of this distribution.
+#
+# SPDX-License-Identifier: Apache-2.0 OR MIT
+#
+#
+
+
+import math
+from rclpy.node import Node
+import rclpy
+
+from geometry_msgs.msg import Twist
+from ackermann_msgs.msg import AckermannDrive
+
+
+class TwistToAckermann(Node):
+    """ROS2 node to convert velocity commands to Ackermann commands."""
+
+    def __init__(self):
+        super().__init__('twist_to_ackermann')
+
+        self.declare_parameter('wheelbase', 2.2)
+
+        self.declare_parameter('publish_zeros_on_timeout', True)
+        self.declare_parameter('timeout_control_interval', 0.1)
+        self.declare_parameter('control_timeout', 0.2)
+
+        self.wheelbase = self.get_parameter('wheelbase').get_parameter_value().double_value
+        self.timeout_control_interval = self.get_parameter(
+            'timeout_control_interval').get_parameter_value().double_value
+
+        par_timeout = self.get_parameter('control_timeout')
+        par_publish_zero_tout = self.get_parameter('publish_zeros_on_timeout')
+
+        self.control_timeout = par_timeout.get_parameter_value().double_value
+        self.publish_zeros_on_timeout = par_publish_zero_tout.get_parameter_value().bool_value
+
+        self.last_message_time = self.get_clock().now()
+        self.sub_ = self.create_subscription(Twist, "cmd_vel", self.cmd_vel_cb, 10)
+        self.pub_ = self.create_publisher(AckermannDrive, 'ackermann_vel', 10)
+        if self.publish_zeros_on_timeout:
+            self.timer = self.create_timer(self.timeout_control_interval, self.control)
+
+    def control(self):
+        time_diff = (self.get_clock().now() - self.last_message_time)
+        if time_diff.nanoseconds * 1e-9 > self.control_timeout:
+            msg = AckermannDrive()
+            msg.steering_angle = 0.0
+            msg.speed = 0.0
+            self.pub_.publish(msg)
+
+    def twist_to_ackermann(self, vel, omega):
+        if omega == 0 or vel == 0:
+            return 0
+
+        radius = vel / omega
+        steering = math.atan(self.wheelbase / radius)
+        return float(steering)
+
+    def cmd_vel_cb(self, data):
+        self.last_message_time = self.get_clock().now()
+
+        vel = data.linear.x
+        steering = self.twist_to_ackermann(vel, data.angular.z)
+
+        msg = AckermannDrive()
+        msg.steering_angle = float(steering)
+        msg.speed = vel
+
+        self.pub_.publish(msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    tta = TwistToAckermann()
+    rclpy.spin(tta)
+    tta.timer.cancel()
+    tta.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

kraken_nav/o3de_kraken_nav/package.xml

@@ -0,0 +1,20 @@
+<?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>o3de_kraken_nav</name>
+  <version>1.0.0</version>
+  <description>Navigation package for Roscon apple kraken demo</description>
+  <maintainer email="[email protected]">Piotr Jaroszek</maintainer>
+  <license>Apache 2.0</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <exec_depend>ros2launch</exec_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

kraken_nav/o3de_kraken_nav/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/o3de_kraken_nav
+[install]
+install_scripts=$base/lib/o3de_kraken_nav

kraken_nav/o3de_kraken_nav/setup.py

@@ -0,0 +1,42 @@
+#
+# Copyright (c) Contributors to the Open 3D Engine Project.
+# For complete copyright and license terms please see the LICENSE at the root of this distribution.
+#
+# SPDX-License-Identifier: Apache-2.0 OR MIT
+#
+#
+
+
+from setuptools import setup
+from glob import glob
+
+package_name = 'o3de_kraken_nav'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        ('share/' + package_name + "/launch/config", glob('launch/config/*.yaml')),
+        ('share/' + package_name + "/launch/config", glob('launch/config/*.xml')),
+        ('share/' + package_name + "/launch/config", glob('launch/config/*.rviz')),
+        ('share/' + package_name + "/launch", glob('launch/*.launch.py')),
+        ('share/' + package_name + "/launch", glob('launch/*.xml')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='Piotr Jaroszek',
+    maintainer_email='[email protected]',
+    description='Navigation package for Roscon apple kraken demo',
+    license='Apache 2.0',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'twist_to_ackermann = o3de_kraken_nav.twist_to_ackermann:main',
+            'joy_to_ackermann = o3de_kraken_nav.joy_to_ackermann:main',
+        ],
+    },
+)

kraken_nav/o3de_kraken_nav/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# 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.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
[email protected]
[email protected]
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

kraken_nav/o3de_kraken_nav/test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# 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.
+
+from ament_pep257.main import main
+import pytest
+
+
[email protected]
[email protected]
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'

kraken_nav/o3de_kraken_orchestration/o3de_kraken_orchestration/__init__.py

@@ -0,0 +1,7 @@
+#
+# Copyright (c) Contributors to the Open 3D Engine Project.
+# For complete copyright and license terms please see the LICENSE at the root of this distribution.
+#
+# SPDX-License-Identifier: Apache-2.0 OR MIT
+#
+#

kraken_nav/o3de_kraken_orchestration/o3de_kraken_orchestration/global_kraken_orchestration.py

@@ -0,0 +1,144 @@
+#!/usr/bin/env python3
+
+#
+# Copyright (c) Contributors to the Open 3D Engine Project.
+# For complete copyright and license terms please see the LICENSE at the root of this distribution.
+#
+# SPDX-License-Identifier: Apache-2.0 OR MIT
+#
+#
+
+import rclpy
+import time
+from threading import Thread
+
+from geometry_msgs.msg import PoseStamped
+
+from .state_machine import GlobalOrchestrationSM
+from .orchestration_node import KrakenOrchestrationNode
+
+
+class GlobalOrchestration(GlobalOrchestrationSM, KrakenOrchestrationNode):
+    def __init__(self):
+        rclpy.init()
+
+        GlobalOrchestrationSM.__init__(self)
+        KrakenOrchestrationNode.__init__(self)
+        self.logger = KrakenOrchestrationNode.get_logger(self)
+
+        spin_thread = Thread(target=rclpy.spin, args=(self,))
+        spin_thread.start()
+
+        self.poses = []
+        self.current_index = 0
+
+    def get_next_pose(self):
+        """Retrieve next goal from the Plan."""
+        if self.current_index >= len(self.poses):
+            self.logger.error('Invalid index!')
+            return PoseStamped()
+
+        pose = self.poses[self.current_index]
+        self.logger.info("Current goal id is %d " % self.current_index)
+        self.current_index += 1
+        return pose
+
+    def __del__(self):
+        self.destroy_node()
+        rclpy.shutdown()
+
+    def on_enter_waiting(self):
+        """Is called on entry to waiting state."""
+        self.logger.info('Has the simulation started?')
+        self.send_state("Wait for connection")
+        # Perform a check whether the simulation has started or not.
+        self.started()
+
+    def on_enter_spawning(self):
+        """Is called on entry to spawning state and spawns robot."""
+        self.logger.info('Spawning the robot.')
+        self.send_state("Spawning the robot")
+        if self.spawn_kraken():
+            self.spawned()
+        else:
+            self.logger.info('Robot spawning failed.')
+
+    def on_spawned(self):
+        """Is called on spawn, retrieves plan and position."""
+        self.logger.info('Robot spawned successfully. Fetching info.')
+        pose = self.get_spawn_point_pose()
+        self.poses = self.get_plan_poses(pose)
+        # Give nav stack a few seconds before we start first goal point
+        self.logger.info('Waiting for robot sensors to latch...')
+        time.sleep(3)
+
+    def on_enter_gathering(self):
+        """Is called on entry to gathering state."""
+        self.logger.info('Is the robot in a gathering position?')
+        # Perform a check whether the robot is in a gathering position or not.
+        if self.can_gather:
+            self.gather()
+        else:
+            self.navigate()
+
+    def on_enter_finishing(self):
+        """Is called on entry to finishing apple gathering state."""
+        self.logger.info('Has the robot completed its job?')
+        # Perform a check whether the robot has finished its job or not.
+        if self.current_index >= len(self.poses):
+            self.finished()
+        else:
+            self.finish()
+
+    def on_navigate(self):
+        """Is called on nav. state, sends goal position to nav2, monitors."""
+        self.send_state("Navigating")
+        self.logger.info('Navigating the robot to a closest gathering position?')
+        self.can_gather = False
+        # Instruct the robot to navigate to the closest gathering position.
+        self.navigate_to_pose(self.get_next_pose())
+
+        while not self.goal_pose_reached:
+            time.sleep(1)
+
+        self.goal_pose_reached = False
+        self.can_gather = True
+
+    def on_gather(self):
+        """Is called on gathering state and monitor progress."""
+        self.logger.info("Requesting gathering...")
+        self.send_state("Gather %.1f " % self.apple_progress)
+        trigger_res = self.trigger_apple_gathering()
+        self.logger.info(trigger_res.message)
+
+        if trigger_res.success:
+            while not self.gathering_done:
+                self.send_state("Gather %.0f %%" % self.apple_progress)
+                time.sleep(0.2)
+
+            self.logger.info("Done gathering apples.")
+        else:
+            self.logger.info("Freeing up the AppleKraken...")
+            cancel_res = self.cancel_apple_gathering()
+            self.logger.info(cancel_res.message)
+            time.sleep(2)
+
+    def on_finish(self):
+        """Is called on finish state, navigates to last goal."""
+        # Same action is performed.
+        self.on_navigate()
+
+    def on_finished(self):
+        """Is called after finished."""
+        self.send_state("Completed")
+        self.logger.info('The robot has completed its job.')
+        # The robot has finished its job.
+
+
+def main():
+    global_orch = GlobalOrchestration()
+    global_orch.run()
+
+
+if __name__ == '__main__':
+    main()

kraken_nav/o3de_kraken_orchestration/o3de_kraken_orchestration/orchestration_node.py

@@ -0,0 +1,194 @@
+#!/usr/bin/env python3
+
+#
+# Copyright (c) Contributors to the Open 3D Engine Project.
+# For complete copyright and license terms please see the LICENSE at the root of this distribution.
+#
+# SPDX-License-Identifier: Apache-2.0 OR MIT
+#
+#
+
+from rclpy.node import Node
+from gazebo_msgs.srv import SpawnEntity
+from gazebo_msgs.srv import GetModelState
+from nav_msgs.srv import GetPlan
+from std_srvs.srv import Trigger, Empty
+from rcl_interfaces.msg import Log
+from geometry_msgs.msg import PoseStamped
+from std_msgs.msg import Header
+from rclpy.qos import QoSProfile, QoSHistoryPolicy, QoSReliabilityPolicy, QoSDurabilityPolicy
+from std_msgs.msg import String
+from std_msgs.msg import Float32
+
+
+class KrakenOrchestrationNode(Node):
+    """Class containing implementation of state machine."""
+
+    def init_spawn_entity_client(self):
+        """Initialize ROS2 service client to spawn robot."""
+        self.spawn_entity_client = self.create_client(SpawnEntity, 'spawn_entity')
+        while not self.spawn_entity_client.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('Spawn entity service not available waiting again...')
+
+    def init_get_spawn_point_info_client(self):
+        """Initialize ROS2 service to retrieve spawn points from simulation."""
+        self.get_spawn_point_info_client = self.create_client(
+            GetModelState,
+            'get_spawn_point_info')
+        while not self.get_spawn_point_info_client.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('Get spawn point info service not available waiting again...')
+
+    def init_get_plan_client(self):
+        """Initialize ROS2 service to retrieve gathering plan from simulation."""
+        self.get_plan_client = self.create_client(GetPlan, 'get_gathering_plan')
+        while not self.get_plan_client.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('Get plan service not available waiting again...')
+
+    def apple_gathering_done_callback(self, request, response):
+        """Is called on finished apple gathering by the Simulation."""
+        self.get_logger().info("Received done client msg.")
+        self.gathering_done = True
+        return Empty.Response()
+
+    def apple_progress_callback(self, msg):
+        """Is called on progress change in gathering by the Simulation."""
+        self.apple_progress = 100.0 * float(msg.data)
+
+    def init_apple_gathering_srvs(self):
+        """Initialize servers for services that are served by this node."""
+        self.done_apple_gathering_service = self.create_service(
+            Empty, f'{self.kraken_name}/done_apple_gathering',
+            self.apple_gathering_done_callback
+        )
+
+        self.trigger_apple_gathering_client = self.create_client(
+            Trigger,
+            f'{self.kraken_name}/trigger_apple_gathering'
+        )
+
+        self.cancel_apple_gathering_client = self.create_client(
+            Trigger,
+            f'{self.kraken_name}/cancel_apple_gathering'
+        )
+
+        while (not self.trigger_apple_gathering_client.wait_for_service(timeout_sec=1.0)) \
+                and (not self.cancel_apple_gathering_client.wait_for_service(timeout_sec=1.0)):
+            self.get_logger().info('Apple gathering services not available waiting again...')
+
+    def init_apple_subscriptions(self):
+        """Initialize apple gathering progress topic subscription."""
+        qos = QoSProfile(
+            depth=5,
+            history=QoSHistoryPolicy.KEEP_LAST,
+            reliability=QoSReliabilityPolicy.RELIABLE,
+            durability=QoSDurabilityPolicy.VOLATILE
+        )
+        self.progress_apple_gathering_sub = self.create_subscription(
+            Float32,
+            f'{self.kraken_name}/progress_apple_gathering',
+            self.apple_progress_callback, qos)
+
+    def init_goal_pose_publisher(self):
+        """Initialize goal topic subscription, listened by this node."""
+        qos = QoSProfile(
+            depth=5,
+            history=QoSHistoryPolicy.KEEP_LAST,
+            reliability=QoSReliabilityPolicy.RELIABLE,
+            durability=QoSDurabilityPolicy.VOLATILE
+        )
+
+        self.goal_pose_publisher = self.create_publisher(
+            PoseStamped,
+            f'{self.kraken_name}/goal_pose', qos
+        )
+
+    def init_status_publisher(self):
+        """Initialize topic publishers, sent by this node."""
+        qos = QoSProfile(
+            depth=5,
+            history=QoSHistoryPolicy.KEEP_LAST,
+            reliability=QoSReliabilityPolicy.RELIABLE,
+            durability=QoSDurabilityPolicy.VOLATILE
+        )
+        self.status_publisher = self.create_publisher(
+            String,
+            f'{self.kraken_name}/orchestration_status', qos
+        )
+
+    def send_state(self, status_str):
+        """Publish status message which is presented by the Simulator's GUI."""
+        msg = String()
+        msg.data = status_str
+        self.status_publisher.publish(msg)
+
+    def __init__(self):
+        super().__init__('kraken_orchestration_node')
+
+        self.declare_parameter('robot_name', 'apple_kraken_rusty_1')
+        self.kraken_name = self.get_parameter('robot_name').get_parameter_value().string_value
+
+        self.declare_parameter('spawn_line', 'line1')
+        self.spawn_point = self.get_parameter('spawn_line').get_parameter_value().string_value
+
+        self.log_sub = self.create_subscription(Log, "/rosout", self.log_cb, 10)
+        self.goal_pose_reached = False
+        self.gathering_done = False
+        self.apple_progress = 0.0
+
+        self.init_spawn_entity_client()
+        self.init_get_spawn_point_info_client()
+        self.init_get_plan_client()
+        self.init_goal_pose_publisher()
+        self.init_status_publisher()
+        self.init_apple_subscriptions()
+
+    def log_cb(self, msg):
+        """Is called for messages sent from nav2, used to find if navigation has succeed."""
+        if msg.msg == "Goal succeeded" and self.kraken_name in str(msg.name):
+            self.get_logger().info("Navigation point reached.")
+            self.goal_pose_reached = True
+
+    def spawn_kraken(self):
+        """Spawn robot by calling service."""
+        req = SpawnEntity.Request()
+        req.name = self.kraken_name[:len(self.kraken_name) - 2]
+        req.xml = self.spawn_point
+
+        res = self.spawn_entity_client.call(req)  # TODO - Add exception handling
+        if res.success:
+            self.init_apple_gathering_srvs()
+
+        return res.success
+
+    def get_spawn_point_pose(self):
+        """Retrieve spawn points from the Simulation."""
+        req = GetModelState.Request()
+        req.model_name = self.spawn_point
+
+        res = self.get_spawn_point_info_client.call(req)
+
+        return res.pose  # TODO - Add exception handling
+
+    def get_plan_poses(self, pose):
+        """Retrieve plan from the Simulation."""
+        req = GetPlan.Request()
+        req.start.pose = pose
+
+        res = self.get_plan_client.call(req)
+        return res.plan.poses
+
+    def trigger_apple_gathering(self):
+        """Call service the Simulation to start apple gathering."""
+        self.gathering_done = False
+        req = Trigger.Request()
+        return self.trigger_apple_gathering_client.call(req)
+
+    def cancel_apple_gathering(self):
+        """Call service the Simulation to abort apple gathering."""
+        req = Trigger.Request()
+        return self.cancel_apple_gathering_client.call(req)
+
+    def navigate_to_pose(self, pose):
+        """Set navigation goal to nav2."""
+        pose.header = Header(stamp=self.get_clock().now().to_msg(), frame_id='map')
+        self.goal_pose_publisher.publish(pose)

kraken_nav/o3de_kraken_orchestration/o3de_kraken_orchestration/state_machine.py

@@ -0,0 +1,38 @@
+#!/usr/bin/env python3
+
+#
+# Copyright (c) Contributors to the Open 3D Engine Project.
+# For complete copyright and license terms please see the LICENSE at the root of this distribution.
+#
+# SPDX-License-Identifier: Apache-2.0 OR MIT
+#
+#
+
+from statemachine import StateMachine, State
+
+
+class GlobalOrchestrationSM(StateMachine):
+    """State machine implementation, contains list of states and valid transitions."""
+
+    can_gather = False
+    has_finished = True
+
+    start = State('Start', initial=True)
+    waiting = State('Waiting')  # Waiting for the simulation start
+    spawning = State('Spawning')  # Spawning the robot
+    gathering = State('Gathering')
+    finishing = State('Finishing')
+    # This state may be removed if the simulation is restarted upon completion.
+    terminating = State('Terminating')
+
+    run = start.to(waiting)
+    await_start = waiting.to(waiting)
+    started = waiting.to(spawning)
+    spawned = spawning.to(gathering)
+    gather = gathering.to(finishing)
+    navigate = gathering.to(gathering)
+    finish = finishing.to(gathering)
+    finished = finishing.to(terminating)
+
+    def __init__(self) -> None:
+        super().__init__()

kraken_nav/o3de_kraken_orchestration/package.xml

@@ -0,0 +1,20 @@
+<?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>o3de_kraken_orchestration</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">alekkam</maintainer>
+  <license>Apache 2.0</license>
+
+  <test_depend>statemachine</test_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

kraken_nav/o3de_kraken_orchestration/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/o3de_kraken_orchestration
+[install]
+install_scripts=$base/lib/o3de_kraken_orchestration

kraken_nav/o3de_kraken_orchestration/setup.py

@@ -0,0 +1,35 @@
+#
+# Copyright (c) Contributors to the Open 3D Engine Project.
+# For complete copyright and license terms please see the LICENSE at the root of this distribution.
+#
+# SPDX-License-Identifier: Apache-2.0 OR MIT
+#
+#
+
+from setuptools import setup
+
+package_name = 'o3de_kraken_orchestration'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='alekkam',
+    maintainer_email='[email protected]',
+    description='Orchestration code for AppleKraken robot',
+    license='Apache 2.0',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'kraken_orchestration_node = '
+            'o3de_kraken_orchestration.global_kraken_orchestration:main'
+        ],
+    },
+)

kraken_nav/o3de_kraken_orchestration/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# 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.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
[email protected]
[email protected]
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

kraken_nav/o3de_kraken_orchestration/test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# 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.
+
+from ament_pep257.main import main
+import pytest
+
+
[email protected]
[email protected]
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'