From 7e6a567b90e2a0b429890a2c264bb906ce92f10e Mon Sep 17 00:00:00 2001
From: Mickey Li <mickeyhli@outlook.com>
Date: Mon, 30 May 2022 14:33:40 +0100
Subject: [PATCH] Added docker-compose and fixed typo initialised vehicle
 location map

---
 README.md                                     |  2 +-
 docker-compose.yml                            | 58 +++++++++++++++++++
 kubernetes.yaml                               |  4 ++
 .../src/trajectory.cpp                        |  3 +
 4 files changed, 66 insertions(+), 1 deletion(-)
 create mode 100644 docker-compose.yml

diff --git a/README.md b/README.md
index 120a332..ce7b065 100644
--- a/README.md
+++ b/README.md
@@ -1,4 +1,4 @@
-# Syncrhonous Position Trajectory Controller
+# Synchronous Position Trajectory Controller
 
 This project provides a synchonous simple position trajectory controller for ROS2, MAVROS and (currently) PX4 for multi-vehicle flight
 
diff --git a/docker-compose.yml b/docker-compose.yml
new file mode 100644
index 0000000..bd29122
--- /dev/null
+++ b/docker-compose.yml
@@ -0,0 +1,58 @@
+
+version: '3'
+
+services:
+  simhost: # Must be called simhost
+    image: uobflightlabstarling/starling-sim-iris-px4-flightarena:latest
+    command: [ "ros2", "launch", "launch/system.launch.xml" , sim_only:=true]
+    ports:
+      - "8080:8080"
+
+  # ----------
+    # Spawn Vehicle 1
+  gazebo_spawn_1:
+    image: uobflightlabstarling/starling-sim-iris:latest
+    command: [ "./spawn_iris.sh" ]
+    environment:
+      - "PX4_SIM_HOST=localhost"
+      - "PX4_INSTANCE=0"
+      - "IGNORE_FAILURE=true"
+    depends_on:
+      - simhost
+    pid: "host" # Share Process ID Namespace
+
+  sitl:
+    image: uobflightlabstarling/starling-sim-px4-sitl:latest
+    environment:
+      - "PX4_SIM_HOST=simhost"
+      - "PX4_OFFBOARD_HOST=mavros"
+      - "PX4_INSTANCE=0"
+    depends_on:
+      - gazebo_spawn_1
+    ports:
+      - "18570:18570/udp"
+
+  mavros:
+    image: uobflightlabstarling/starling-mavros:latest
+    command: ros2 launch launch/mavros_bridge.launch.xml
+    environment:
+      - "MAVROS_TGT_SYSTEM=1"
+      - "MAVROS_FCU_IP=0.0.0.0"
+      - "MAVROS_GCS_URL=tcp-l://0.0.0.0:5760"
+    depends_on:
+      - sitl
+    ports:
+      - "5760:5760"
+
+  rosbridge-suite:
+    image: uobflightlabstarling/rosbridge-suite:latest
+    ports:
+      - "9090:9090"
+
+  allocator:
+    image: uobflightlabstarling/starling-allocator:latest
+
+  starling-ui-dashly:
+    image: mickeyli789/starling-ui-dashly:latest
+    ports:
+      - "3001:3000"
\ No newline at end of file
diff --git a/kubernetes.yaml b/kubernetes.yaml
index e3a8d0d..31439a7 100644
--- a/kubernetes.yaml
+++ b/kubernetes.yaml
@@ -82,4 +82,8 @@ spec:
         imagePullPolicy: Always
         ports:
         - containerPort: 3000
+      - name: rosbridge-suite
+        image: uobflightlabstarling/rosbridge-suite:latest
+        imagePullPolicy: Always
+        ports:
         - containerPort: 9090
diff --git a/position_trajectory_controller/src/trajectory.cpp b/position_trajectory_controller/src/trajectory.cpp
index 4eb87a0..3efcf01 100644
--- a/position_trajectory_controller/src/trajectory.cpp
+++ b/position_trajectory_controller/src/trajectory.cpp
@@ -188,6 +188,9 @@ void TrajectoryHandler::reset(){
     this->next_delay = std::chrono::duration<double>(0.0);
     this->sync_cumulative_delay = std::chrono::duration<double>(0.0);
     this->vehicle_delays.clear();
+    Duration delay = Duration::from_seconds(0.0);
+    auto const result = this->vehicle_delays.insert(std::make_pair(this->vehicle_id, delay));
+    if (not result.second) { result.first->second = delay; }
     this->current_task_idx = 0;
     this->sync_wait_until = nullptr;