From c4fb598a3aa195b5df45b566ecb6945dd07aad14 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Thu, 6 May 2021 14:05:37 +0800
Subject: [PATCH 01/66] Add address sanitizer.

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .github/workflows/asan.yaml | 31 +++++++++++++++++++++++++++++++
 1 file changed, 31 insertions(+)
 create mode 100644 .github/workflows/asan.yaml

diff --git a/.github/workflows/asan.yaml b/.github/workflows/asan.yaml
new file mode 100644
index 00000000..230213bf
--- /dev/null
+++ b/.github/workflows/asan.yaml
@@ -0,0 +1,31 @@
+name: tsan
+on: [push, pull_request]
+jobs:
+  tsan:
+    name: asan
+    runs-on: ubuntu-20.04
+    steps:
+      - name: deps
+        uses: ros-tooling/setup-ros@v0.1
+        with:
+          required-ros-distributions: foxy
+      - name: build_and_test
+        uses: ros-tooling/action-ros-ci@v0.2
+        env:
+          CC: clang
+          CXX: clang++
+        with:
+          target-ros2-distro: foxy
+          # build all packages listed in the meta package
+          package-name: |
+            rmf_traffic
+          vcs-repo-file-url: |
+            https://raw.githubusercontent.com/open-rmf/rmf/main/rmf.repos
+          colcon-defaults: |
+            {
+              "build": {
+                "mixin": ["asan"],
+                "cmake-args": ["-DCMAKE_BUILD_TYPE=Debug"]
+              }
+            }
+          colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml

From 84262e717e41370dcb2f46e43a8b34f1f82e4d0b Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Mon, 24 May 2021 09:07:03 +0800
Subject: [PATCH 02/66] back ported useful changes

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../rmf_traffic/reservations/Reservation.hpp  |  70 ++++++++
 .../reservations/ReservationRequest.hpp       |  77 +++++++++
 .../rmf_traffic/reservations/Reservation.cpp  | 159 ++++++++++++++++++
 .../reservations/ReservationRequest.cpp       | 125 ++++++++++++++
 4 files changed, 431 insertions(+)
 create mode 100644 rmf_traffic/include/rmf_traffic/reservations/Reservation.hpp
 create mode 100644 rmf_traffic/include/rmf_traffic/reservations/ReservationRequest.hpp
 create mode 100644 rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp
 create mode 100644 rmf_traffic/src/rmf_traffic/reservations/ReservationRequest.cpp

diff --git a/rmf_traffic/include/rmf_traffic/reservations/Reservation.hpp b/rmf_traffic/include/rmf_traffic/reservations/Reservation.hpp
new file mode 100644
index 00000000..ed589154
--- /dev/null
+++ b/rmf_traffic/include/rmf_traffic/reservations/Reservation.hpp
@@ -0,0 +1,70 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * 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.
+ *
+*/
+
+#ifndef RMF_TRAFFIC__RESERVATIONS_HPP
+#define RMF_TRAFFIC__RESERVATIONS_HPP
+
+#include <optional>
+#include <vector>
+#include <rmf_traffic/Time.hpp>
+#include <rmf_utils/impl_ptr.hpp>
+
+namespace rmf_traffic {
+namespace reservations {
+
+using ReservationId = uint64_t;
+//==============================================================================
+class Reservation
+{
+public:
+  const rmf_traffic::Time start_time() const;
+
+  const std::optional<rmf_traffic::Duration> duration() const;
+
+  const std::optional<rmf_traffic::Time> finish_time() const;
+
+  void set_actual_finish_time(rmf_traffic::Time dur);
+
+  Reservation propose_new_finish_time(rmf_traffic::Time dur);
+
+  Reservation propose_new_start_time(rmf_traffic::Time dur);
+
+  bool is_indefinite() const;
+
+  const std::optional<rmf_traffic::Time> actual_finish_time() const;
+
+  const std::string resource_name() const;
+
+  ReservationId reservation_id() const;
+
+  bool operator==(const Reservation& other) const;
+
+  static Reservation make_reservation(
+    rmf_traffic::Time start_time,
+    std::string resource_name,
+    schedule::ParticipantId pid,
+    std::optional<rmf_traffic::Duration> duration,
+    std::optional<rmf_traffic::Time> finish_time);
+
+  class Implementation;
+private:
+  Reservation();
+  rmf_utils::impl_ptr<Implementation> _pimpl;
+};
+} // end namespace reservations
+} // end namespace rmf_traffic
+#endif
\ No newline at end of file
diff --git a/rmf_traffic/include/rmf_traffic/reservations/ReservationRequest.hpp b/rmf_traffic/include/rmf_traffic/reservations/ReservationRequest.hpp
new file mode 100644
index 00000000..c175209b
--- /dev/null
+++ b/rmf_traffic/include/rmf_traffic/reservations/ReservationRequest.hpp
@@ -0,0 +1,77 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * 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.
+ *
+*/
+
+#ifndef RMF_TRAFFIC__RESERVATION_REQUEST_HPP
+#define RMF_TRAFFIC__RESERVATION_REQUEST_HPP
+
+#include <optional>
+#include <vector>
+#include <rmf_traffic/Time.hpp>
+#include <rmf_utils/impl_ptr.hpp>
+
+namespace rmf_traffic {
+namespace reservations {
+class ReservationRequest
+{
+
+public:
+  class TimeRange
+  {
+  public:
+    const std::optional<Time> lower_bound() const;
+    const std::optional<Time> upper_bound() const;
+
+    static TimeRange make_time_range(
+      std::optional<Time> lower_bound,
+      std::optional<Time> upper_bound
+    );
+
+    class Implementation;
+
+  private:
+    TimeRange();
+    rmf_utils::impl_ptr<Implementation> _pimpl;
+  };
+
+  const std::optional<TimeRange> start_time() const;
+
+  const std::optional<Duration> duration() const;
+
+  const std::optional<Time> finish_time() const;
+
+  const std::string resource_name() const;
+
+  bool is_indefinite() const;
+
+  static ReservationRequest make_request(
+    std::string resource_name,
+    schedule::ParticipantId participant,
+    std::optional<TimeRange> start = std::nullopt,
+    std::optional<Duration> duration = std::nullopt,
+    std::optional<Time> finish = std::nullopt
+  );
+
+  class Implementation;
+
+private:
+  ReservationRequest();
+  rmf_utils::impl_ptr<Implementation>  _pimpl;
+};
+}
+}
+
+#endif
\ No newline at end of file
diff --git a/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp b/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp
new file mode 100644
index 00000000..c8cdd8bd
--- /dev/null
+++ b/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp
@@ -0,0 +1,159 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * 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.
+ *
+*/
+
+#include <rmf_traffic/reservations/Reservation.hpp>
+#include <atomic>
+
+namespace rmf_traffic {
+namespace reservations {
+
+//=============================================================================
+class Reservation::Implementation
+{
+public:
+  ReservationId _res_id;
+  std::string _resource_name;
+  rmf_traffic::Time _start_time;
+  std::optional<rmf_traffic::Duration>  _duration;
+  std::optional<rmf_traffic::Time>  _finish_time;
+};
+
+//=============================================================================
+const rmf_traffic::Time Reservation::start_time() const
+{
+  return _pimpl->_start_time;
+}
+
+//=============================================================================
+const std::optional<rmf_traffic::Duration> Reservation::duration() const
+{
+  return _pimpl->_duration;
+}
+
+//=============================================================================
+const std::optional<rmf_traffic::Time> Reservation::finish_time() const
+{
+  return _pimpl->_finish_time;
+}
+
+//=============================================================================
+void Reservation::set_actual_finish_time(rmf_traffic::Time dur)
+{
+  if(dur < _pimpl->_start_time)
+    _pimpl->_start_time = dur;
+
+  _pimpl->_finish_time = dur;
+  _pimpl->_duration.emplace(*_pimpl->_finish_time - _pimpl->_start_time);
+}
+
+//=============================================================================
+Reservation Reservation::propose_new_finish_time(rmf_traffic::Time time)
+{
+  Reservation res(*this);
+  res.set_actual_finish_time(time);
+  return res;
+}
+
+//=============================================================================
+Reservation Reservation::propose_new_start_time(rmf_traffic::Time start_time)
+{
+  Reservation res(*this);
+  res._pimpl->_start_time = start_time;
+  return res;
+}
+
+//=============================================================================
+const std::optional<rmf_traffic::Time> Reservation::actual_finish_time() const
+{
+  if(is_indefinite()) return std::nullopt;
+
+  if(_pimpl->_finish_time.has_value() && _pimpl->_duration.has_value())
+  {
+    return {std::max(_pimpl->_start_time + *_pimpl->_duration, 
+      *_pimpl->_finish_time)};
+  }
+  else if(_pimpl->_finish_time.has_value())
+  {
+    return {std::max(_pimpl->_start_time, *_pimpl->_finish_time)};
+  }
+  else
+  {
+    return _pimpl->_start_time + *_pimpl->_duration;
+  }
+}
+
+//=============================================================================
+bool Reservation::is_indefinite() const
+{
+  return !_pimpl->_finish_time.has_value() && !_pimpl->_duration.has_value();
+}
+
+//=============================================================================
+const std::string Reservation::resource_name() const
+{
+  return _pimpl->_resource_name;
+}
+
+//=============================================================================
+ReservationId Reservation::reservation_id() const
+{
+  return _pimpl->_res_id;
+}
+
+//=============================================================================
+bool Reservation::operator==(const Reservation& other) const
+{
+  return other._pimpl->_res_id == _pimpl->_res_id
+    && other._pimpl->_start_time == _pimpl->_start_time
+    && other._pimpl->_pid == _pimpl->_pid
+    && other._pimpl->_resource_name == _pimpl->_resource_name
+    && other._pimpl->_duration == _pimpl->_duration
+    && other._pimpl->_finish_time == _pimpl->_finish_time;
+}
+
+//=============================================================================
+Reservation Reservation::make_reservation(
+  rmf_traffic::Time start_time,
+  std::string resource_name,
+  schedule::ParticipantId pid,
+  std::optional<rmf_traffic::Duration> duration,
+  std::optional<rmf_traffic::Time> finish_time)
+{
+  static std::atomic<uint64_t> counter {0};
+  Reservation res;
+
+  res._pimpl->_res_id = counter;
+  res._pimpl->_start_time = start_time;
+  res._pimpl->_pid = pid;
+  res._pimpl->_resource_name = resource_name;
+  res._pimpl->_duration = duration;
+  res._pimpl->_finish_time = finish_time;
+  
+  counter++;
+
+  return res;
+}
+
+//=============================================================================
+Reservation::Reservation(): 
+  _pimpl(rmf_utils::make_impl<Implementation>())
+{
+
+}
+
+}
+}
\ No newline at end of file
diff --git a/rmf_traffic/src/rmf_traffic/reservations/ReservationRequest.cpp b/rmf_traffic/src/rmf_traffic/reservations/ReservationRequest.cpp
new file mode 100644
index 00000000..b5981eff
--- /dev/null
+++ b/rmf_traffic/src/rmf_traffic/reservations/ReservationRequest.cpp
@@ -0,0 +1,125 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * 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.
+ *
+*/
+
+#include <rmf_traffic/reservations/ReservationRequest.hpp>
+
+namespace rmf_traffic {
+namespace reservations {
+
+class ReservationRequest::TimeRange::Implementation
+{
+public:
+  std::optional<Time> _lower_bound;
+  std::optional<Time> _upper_bound;
+};
+
+
+const std::optional<Time> 
+  ReservationRequest::TimeRange::lower_bound() const
+{
+  return _pimpl->_lower_bound;
+}
+const std::optional<Time> 
+  ReservationRequest::TimeRange::upper_bound() const
+{
+  return _pimpl->_upper_bound;
+}
+
+ReservationRequest::TimeRange
+ReservationRequest::TimeRange::make_time_range(
+  std::optional<Time> lower_bound,
+  std::optional<Time> upper_bound)
+{
+  ReservationRequest::TimeRange tr;
+  tr._pimpl->_lower_bound = lower_bound;
+  tr._pimpl->_upper_bound = upper_bound;
+  return tr;
+}
+
+ReservationRequest::TimeRange::TimeRange():
+  _pimpl(rmf_utils::make_impl<Implementation>())
+{
+
+}
+
+class ReservationRequest::Implementation
+{
+public:
+  std::string _resource_name;
+  std::optional<TimeRange> _start;
+  std::optional<Duration> _duration;
+  std::optional<Time> _finish;
+};
+
+//=============================================================================
+const std::optional<ReservationRequest::TimeRange> 
+    ReservationRequest::start_time() const
+{
+  return _pimpl->_start;
+}
+
+//=============================================================================
+const std::optional<Duration> ReservationRequest::duration() const
+{
+  return _pimpl->_duration;
+}
+
+//=============================================================================
+const std::optional<Time> ReservationRequest::finish_time() const
+{
+  return _pimpl->_finish;
+}
+
+//=============================================================================
+const std::string ReservationRequest::resource_name() const
+{
+  return _pimpl->_resource_name;
+}
+
+//=============================================================================
+bool ReservationRequest::is_indefinite() const
+{
+  return !_pimpl->_duration.has_value() && !_pimpl->_finish.has_value();
+}
+
+//=============================================================================
+ReservationRequest ReservationRequest::make_request(
+  std::string resource_name,
+  schedule::ParticipantId participant,
+  std::optional<TimeRange> start,
+  std::optional<Duration> duration,
+  std::optional<Time> finish)
+{
+  ReservationRequest req;
+  req._pimpl->_resource_name = resource_name;
+  req._pimpl->_participant = participant;
+  req._pimpl->_start = start;
+  req._pimpl->_finish = finish;
+  req._pimpl->_duration = duration;
+
+  return req;
+}
+
+//=============================================================================
+ReservationRequest::ReservationRequest():
+  _pimpl(rmf_utils::make_impl<Implementation>())
+{
+}
+
+}
+}
+

From a3a424e7d440555e42ecc48e813c7dd008d7b72e Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Mon, 24 May 2021 10:59:29 +0800
Subject: [PATCH 03/66] Added Writer API

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../rmf_traffic/reservations/Writer.hpp       | 51 +++++++++++++++++++
 .../reservations/ReservationRequest.cpp       |  8 ++-
 2 files changed, 54 insertions(+), 5 deletions(-)
 create mode 100644 rmf_traffic/include/rmf_traffic/reservations/Writer.hpp

diff --git a/rmf_traffic/include/rmf_traffic/reservations/Writer.hpp b/rmf_traffic/include/rmf_traffic/reservations/Writer.hpp
new file mode 100644
index 00000000..659148d6
--- /dev/null
+++ b/rmf_traffic/include/rmf_traffic/reservations/Writer.hpp
@@ -0,0 +1,51 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * 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.
+ *
+*/
+
+#ifndef RMF_TRAFFIC__WRITER_HPP
+#define RMF_TRAFFIC__WRITER_HPP
+
+#include <rmf_traffic/reservations/Participant.hpp>
+#include <rmf_traffic/reservations/Reservation.hpp>
+#include <rmf_traffic/reservations/ReservationRequest.hpp>
+
+namespace rmf_traffic {
+namespace reservations {
+
+class Writer
+{
+public:
+  virtual void register_participant(
+    ParticipantId id,
+    std::shared_ptr<Participant> participant) = 0;
+
+  virtual void unregister_participant(
+    ParticipantId id
+  ) = 0;
+
+  virtual void request_reservation(
+    ParticipantId id,
+    RequestId req,
+    std::vector<ReservationRequest>& request_options,
+    int priority = 0
+  ) = 0;
+
+  virtual void cancel_request(RequestId req) = 0;
+};
+}
+}
+
+#endif
\ No newline at end of file
diff --git a/rmf_traffic/src/rmf_traffic/reservations/ReservationRequest.cpp b/rmf_traffic/src/rmf_traffic/reservations/ReservationRequest.cpp
index b5981eff..0221ea2e 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/ReservationRequest.cpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/ReservationRequest.cpp
@@ -28,12 +28,12 @@ class ReservationRequest::TimeRange::Implementation
 };
 
 
-const std::optional<Time> 
+const std::optional<Time>
   ReservationRequest::TimeRange::lower_bound() const
 {
   return _pimpl->_lower_bound;
 }
-const std::optional<Time> 
+const std::optional<Time>
   ReservationRequest::TimeRange::upper_bound() const
 {
   return _pimpl->_upper_bound;
@@ -66,7 +66,7 @@ class ReservationRequest::Implementation
 };
 
 //=============================================================================
-const std::optional<ReservationRequest::TimeRange> 
+const std::optional<ReservationRequest::TimeRange>
     ReservationRequest::start_time() const
 {
   return _pimpl->_start;
@@ -99,14 +99,12 @@ bool ReservationRequest::is_indefinite() const
 //=============================================================================
 ReservationRequest ReservationRequest::make_request(
   std::string resource_name,
-  schedule::ParticipantId participant,
   std::optional<TimeRange> start,
   std::optional<Duration> duration,
   std::optional<Time> finish)
 {
   ReservationRequest req;
   req._pimpl->_resource_name = resource_name;
-  req._pimpl->_participant = participant;
   req._pimpl->_start = start;
   req._pimpl->_finish = finish;
   req._pimpl->_duration = duration;

From b737dcbac3da9e265b03358062f07c0b74ac2fc6 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Mon, 24 May 2021 11:00:04 +0800
Subject: [PATCH 04/66] Add basic participant store

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../rmf_traffic/reservations/Database.hpp     | 56 +++++++++++
 .../rmf_traffic/reservations/Participant.hpp  | 39 ++++++++
 .../rmf_traffic/reservations/Reservation.hpp  |  1 -
 .../reservations/ReservationRequest.hpp       |  4 +-
 .../src/rmf_traffic/reservations/Database.cpp | 93 +++++++++++++++++++
 .../rmf_traffic/reservations/Reservation.cpp  |  6 +-
 .../reservations/internal/Optimizer.hpp       |  0
 .../internal/ParticipantStore.hpp             | 64 +++++++++++++
 .../reservations/internal/RequestQueue.hpp    | 29 ++++++
 .../reservations/internal/State.hpp           |  0
 10 files changed, 285 insertions(+), 7 deletions(-)
 create mode 100644 rmf_traffic/include/rmf_traffic/reservations/Database.hpp
 create mode 100644 rmf_traffic/include/rmf_traffic/reservations/Participant.hpp
 create mode 100644 rmf_traffic/src/rmf_traffic/reservations/Database.cpp
 create mode 100644 rmf_traffic/src/rmf_traffic/reservations/internal/Optimizer.hpp
 create mode 100644 rmf_traffic/src/rmf_traffic/reservations/internal/ParticipantStore.hpp
 create mode 100644 rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp
 create mode 100644 rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp

diff --git a/rmf_traffic/include/rmf_traffic/reservations/Database.hpp b/rmf_traffic/include/rmf_traffic/reservations/Database.hpp
new file mode 100644
index 00000000..98671348
--- /dev/null
+++ b/rmf_traffic/include/rmf_traffic/reservations/Database.hpp
@@ -0,0 +1,56 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * 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.
+ *
+*/
+
+#ifndef RMF_TRAFFIC__DATABASE_HPP
+#define RMF_TRAFFIC__DATABASE_HPP
+
+#include <rmf_traffic/reservations/Writer.hpp>
+
+namespace rmf_traffic {
+namespace reservations {
+
+class Database: public Writer
+{
+public:
+  void register_participant(
+    ParticipantId id,
+    std::shared_ptr<Participant> participant) override;
+
+  void unregister_participant(
+    ParticipantId id
+  ) override;
+
+  void request_reservation(
+    ParticipantId id,
+    RequestId req,
+    std::vector<ReservationRequest>& request_options,
+    int priority = 0
+  ) override;
+
+  void cancel_request(RequestId req) override;
+
+  Database();
+
+  class Implementation;
+private:
+  rmf_utils::impl_ptr<Implementation>  _pimpl;
+};
+
+}
+}
+
+#endif
\ No newline at end of file
diff --git a/rmf_traffic/include/rmf_traffic/reservations/Participant.hpp b/rmf_traffic/include/rmf_traffic/reservations/Participant.hpp
new file mode 100644
index 00000000..e1f9a79c
--- /dev/null
+++ b/rmf_traffic/include/rmf_traffic/reservations/Participant.hpp
@@ -0,0 +1,39 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * 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.
+ *
+*/
+
+#ifndef RMF_TRAFFIC__PARTICIPANT_HPP
+#define RMF_TRAFFIC__PARTICIPANT_HPP
+
+#include <rmf_traffic/reservations/Reservation.hpp>
+
+namespace rmf_traffic {
+namespace reservations {
+
+using ParticipantId = uint64_t;
+
+class Participant
+{
+public:
+  virtual void request_assignment_received(
+    RequestId id,
+    Reservation& res
+  ) = 0;
+};
+
+}
+}
+#endif
\ No newline at end of file
diff --git a/rmf_traffic/include/rmf_traffic/reservations/Reservation.hpp b/rmf_traffic/include/rmf_traffic/reservations/Reservation.hpp
index ed589154..708364dd 100644
--- a/rmf_traffic/include/rmf_traffic/reservations/Reservation.hpp
+++ b/rmf_traffic/include/rmf_traffic/reservations/Reservation.hpp
@@ -56,7 +56,6 @@ class Reservation
   static Reservation make_reservation(
     rmf_traffic::Time start_time,
     std::string resource_name,
-    schedule::ParticipantId pid,
     std::optional<rmf_traffic::Duration> duration,
     std::optional<rmf_traffic::Time> finish_time);
 
diff --git a/rmf_traffic/include/rmf_traffic/reservations/ReservationRequest.hpp b/rmf_traffic/include/rmf_traffic/reservations/ReservationRequest.hpp
index c175209b..c496f1d4 100644
--- a/rmf_traffic/include/rmf_traffic/reservations/ReservationRequest.hpp
+++ b/rmf_traffic/include/rmf_traffic/reservations/ReservationRequest.hpp
@@ -25,6 +25,9 @@
 
 namespace rmf_traffic {
 namespace reservations {
+
+using RequestId = uint64_t;
+
 class ReservationRequest
 {
 
@@ -59,7 +62,6 @@ class ReservationRequest
 
   static ReservationRequest make_request(
     std::string resource_name,
-    schedule::ParticipantId participant,
     std::optional<TimeRange> start = std::nullopt,
     std::optional<Duration> duration = std::nullopt,
     std::optional<Time> finish = std::nullopt
diff --git a/rmf_traffic/src/rmf_traffic/reservations/Database.cpp b/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
new file mode 100644
index 00000000..903efcfd
--- /dev/null
+++ b/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
@@ -0,0 +1,93 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * 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.
+ *
+*/
+
+#include <rmf_traffic/reservations/Database.hpp>
+#include "internal/ParticipantStore.hpp"
+
+namespace rmf_traffic {
+namespace reservations {
+
+class Database::Implementation
+{
+public:
+  Implementation()
+  {
+
+  }
+
+  void register_participant(
+    ParticipantId id,
+    std::shared_ptr<Participant> participant)
+  {
+    _participant_store.add_participant(id, participant);
+  }
+
+  void unregister_participant(
+    ParticipantId id
+  )
+  {
+    _participant_store.remove_participant(id);
+  }
+
+  void request_reservation(
+    ParticipantId id,
+    RequestId req,
+    std::vector<ReservationRequest>& request_options,
+    int priority)
+  {
+
+  }
+
+  void cancel_request(RequestId req)
+  {
+  }
+private:
+  ParticipantStore _participant_store;
+
+};
+
+void Database::register_participant(
+  ParticipantId id,
+  std::shared_ptr<Participant> participant)
+{
+  _pimpl->register_participant(
+    id,
+    participant
+  );
+}
+
+void Database::unregister_participant(ParticipantId id)
+{
+  _pimpl->unregister_participant(id);
+}
+
+void Database::request_reservation(
+  ParticipantId id,
+  RequestId req,
+  std::vector<ReservationRequest>& request_options,
+  int priority = 0)
+{
+  _pimpl->request_reservation(id, req, request_options, priority);
+}
+
+Database::Database():
+  _pimpl(rmf_utils::make_impl<Implementation>())
+{
+}
+
+}
+}
\ No newline at end of file
diff --git a/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp b/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp
index c8cdd8bd..e2343b42 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp
@@ -119,7 +119,6 @@ bool Reservation::operator==(const Reservation& other) const
 {
   return other._pimpl->_res_id == _pimpl->_res_id
     && other._pimpl->_start_time == _pimpl->_start_time
-    && other._pimpl->_pid == _pimpl->_pid
     && other._pimpl->_resource_name == _pimpl->_resource_name
     && other._pimpl->_duration == _pimpl->_duration
     && other._pimpl->_finish_time == _pimpl->_finish_time;
@@ -129,7 +128,6 @@ bool Reservation::operator==(const Reservation& other) const
 Reservation Reservation::make_reservation(
   rmf_traffic::Time start_time,
   std::string resource_name,
-  schedule::ParticipantId pid,
   std::optional<rmf_traffic::Duration> duration,
   std::optional<rmf_traffic::Time> finish_time)
 {
@@ -138,18 +136,16 @@ Reservation Reservation::make_reservation(
 
   res._pimpl->_res_id = counter;
   res._pimpl->_start_time = start_time;
-  res._pimpl->_pid = pid;
   res._pimpl->_resource_name = resource_name;
   res._pimpl->_duration = duration;
   res._pimpl->_finish_time = finish_time;
-  
   counter++;
 
   return res;
 }
 
 //=============================================================================
-Reservation::Reservation(): 
+Reservation::Reservation():
   _pimpl(rmf_utils::make_impl<Implementation>())
 {
 
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/Optimizer.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/Optimizer.hpp
new file mode 100644
index 00000000..e69de29b
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/ParticipantStore.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/ParticipantStore.hpp
new file mode 100644
index 00000000..ba437fa0
--- /dev/null
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/ParticipantStore.hpp
@@ -0,0 +1,64 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * 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.
+ *
+*/
+
+#ifndef RMF_TRAFFIC__RESERVATIONS__INTERNAL_PARTICIPANTSTORE_HPP
+#define RMF_TRAFFIC__RESERVATIONS__INTERNAL_PARTICIPANTSTORE_HPP
+
+#include <rmf_traffic/reservations/Database.hpp>
+#include <unordered_map>
+
+namespace rmf_traffic {
+namespace reservations {
+
+class ParticipantStore
+{
+public:
+  void add_participant(
+    ParticipantId id,
+    std::shared_ptr<Participant> participant
+  )
+  {
+    _participants.insert_or_assign(
+      id,
+      participant
+    );
+  }
+
+  void remove_participant(ParticipantId id)
+  {
+    auto participant = _participants.find(id);
+    if(participant == _participants.end())
+      return;
+    _participants.erase(participant);
+  }
+
+  std::optional<std::shared_ptr<Participant>>
+    get_participant(ParticipantId id)
+  {
+    auto participant = _participants.find(id);
+    if(participant == _participants.end())
+      return std::nullopt;
+    return {participant->second};
+  }
+private:
+  std::unordered_map<ParticipantId, std::shared_ptr<Participant>>
+    _participants;
+};
+
+}
+}
+#endif
\ No newline at end of file
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp
new file mode 100644
index 00000000..9ed9e64e
--- /dev/null
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp
@@ -0,0 +1,29 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * 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.
+ *
+*/
+
+#ifndef RMF_TRAFFIC__RESERVATIONS__INTERNAL_REQUESTQUEUE_HPP
+#define RMF_TRAFFIC__RESERVATIONS__INTERNAL_REQUESTQUEUE_HPP
+
+#include <rmf_traffic/reservations/Database.hpp>
+#include <unordered_map>
+
+namespace rmf_traffic {
+namespace reservations {
+}
+}
+
+#endif
\ No newline at end of file
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
new file mode 100644
index 00000000..e69de29b

From 374abd9403bb051e9d1a85e589974685a4a32a3b Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Mon, 24 May 2021 15:48:11 +0800
Subject: [PATCH 05/66] Add state API

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../rmf_traffic/reservations/Database.hpp     |  2 +-
 .../rmf_traffic/reservations/Participant.hpp  |  2 +-
 .../rmf_traffic/reservations/Writer.hpp       |  2 +-
 .../src/rmf_traffic/reservations/Database.cpp | 11 ++-
 .../reservations/internal/RequestQueue.hpp    | 41 +++++++++-
 .../reservations/internal/State.hpp           | 80 +++++++++++++++++++
 6 files changed, 131 insertions(+), 7 deletions(-)

diff --git a/rmf_traffic/include/rmf_traffic/reservations/Database.hpp b/rmf_traffic/include/rmf_traffic/reservations/Database.hpp
index 98671348..f8c29ddb 100644
--- a/rmf_traffic/include/rmf_traffic/reservations/Database.hpp
+++ b/rmf_traffic/include/rmf_traffic/reservations/Database.hpp
@@ -41,7 +41,7 @@ class Database: public Writer
     int priority = 0
   ) override;
 
-  void cancel_request(RequestId req) override;
+  void cancel_request(ParticipantId id, RequestId req) override;
 
   Database();
 
diff --git a/rmf_traffic/include/rmf_traffic/reservations/Participant.hpp b/rmf_traffic/include/rmf_traffic/reservations/Participant.hpp
index e1f9a79c..245d6642 100644
--- a/rmf_traffic/include/rmf_traffic/reservations/Participant.hpp
+++ b/rmf_traffic/include/rmf_traffic/reservations/Participant.hpp
@@ -28,7 +28,7 @@ using ParticipantId = uint64_t;
 class Participant
 {
 public:
-  virtual void request_assignment_received(
+  virtual bool request_assignment_received(
     RequestId id,
     Reservation& res
   ) = 0;
diff --git a/rmf_traffic/include/rmf_traffic/reservations/Writer.hpp b/rmf_traffic/include/rmf_traffic/reservations/Writer.hpp
index 659148d6..510f7129 100644
--- a/rmf_traffic/include/rmf_traffic/reservations/Writer.hpp
+++ b/rmf_traffic/include/rmf_traffic/reservations/Writer.hpp
@@ -43,7 +43,7 @@ class Writer
     int priority = 0
   ) = 0;
 
-  virtual void cancel_request(RequestId req) = 0;
+  virtual void cancel_request(ParticipantId id, RequestId req) = 0;
 };
 }
 }
diff --git a/rmf_traffic/src/rmf_traffic/reservations/Database.cpp b/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
index 903efcfd..c7608a4f 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
@@ -17,7 +17,7 @@
 
 #include <rmf_traffic/reservations/Database.hpp>
 #include "internal/ParticipantStore.hpp"
-
+#include "internal/RequestQueue.hpp"
 namespace rmf_traffic {
 namespace reservations {
 
@@ -41,6 +41,7 @@ class Database::Implementation
   )
   {
     _participant_store.remove_participant(id);
+    _request_queue.erase_participant_requests(id);
   }
 
   void request_reservation(
@@ -52,12 +53,12 @@ class Database::Implementation
 
   }
 
-  void cancel_request(RequestId req)
+  void cancel_request(ParticipantId id, RequestId req)
   {
   }
 private:
   ParticipantStore _participant_store;
-
+  RequestQueue _request_queue;
 };
 
 void Database::register_participant(
@@ -84,6 +85,10 @@ void Database::request_reservation(
   _pimpl->request_reservation(id, req, request_options, priority);
 }
 
+void Database::cancel_request(ParticipantId id, RequestId req)
+{
+  _pimpl->cancel_request(id, req);
+}
 Database::Database():
   _pimpl(rmf_utils::make_impl<Implementation>())
 {
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp
index 9ed9e64e..1dc66cd8 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp
@@ -23,7 +23,46 @@
 
 namespace rmf_traffic {
 namespace reservations {
+
+class RequestQueue
+{
+public:
+  struct ReservationInfo {
+    int priority;
+    std::vector<ReservationRequest> request_options;
+  };
+
+  void enqueue_reservation(
+    ParticipantId pid,
+    RequestId req,
+    int priority,
+    std::vector<ReservationRequest>& reservation
+  )
+  {
+    _reservation_info[pid].insert({
+      req,
+      {
+        priority,
+        std::move(reservation)
+      }
+    });
+  }
+
+  void erase_participant_requests(
+    ParticipantId pid
+  ){
+    auto entry = _reservation_info.find(pid);
+    if(entry == _reservation_info.end())
+      return;
+    _reservation_info.erase(entry);
+  }
+
+private:
+  std::unordered_map<ParticipantId,
+    std::unordered_map<RequestId, ReservationInfo>
+    >  _reservation_info;
+};
+
 }
 }
-
 #endif
\ No newline at end of file
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index e69de29b..98e25c37 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -0,0 +1,80 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * 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.
+ *
+*/
+
+#ifndef RMF_TRAFFIC__RESERVATIONS__INTERNAL_STATE_HPP
+#define RMF_TRAFFIC__RESERVATIONS__INTERNAL_STATE_HPP
+
+#include <rmf_traffic/reservations/Database.hpp>
+#include <unordered_map>
+#include <unordered_set>
+#include <map>
+
+#include "RequestQueue.hpp"
+
+namespace rmf_traffic {
+namespace reservations {
+class State
+{
+  struct pair_hash {
+    template <class T1, class T2>
+    std::size_t operator () (const std::pair<T1,T2> &p) const {
+      auto h1 = std::hash<T1>{}(p.first);
+      auto h2 = std::hash<T2>{}(p.second);
+      return h1 ^ h2;
+    }
+  };
+public:
+  /// Adds a reservation to the state
+  /// Note: Assumes that reservation is valid.
+  State add_reservation(
+    ParticipantId pid,
+    RequestId reqid,
+    Reservation res)
+  {
+
+  }
+
+  State remove_request(
+    ParticipantId pid,
+    RequestId reqid)
+  {
+
+  }
+
+  State delay_reservations_after(ReservationId id, Duration dur)
+  {
+
+  }
+
+  State bring_forward_reservations_before(ReservationId id, Duration dur)
+  {
+
+  }
+
+private:
+  std::unordered_map<
+    std::pair<ParticipantId, RequestId>, ReservationId, pair_hash> _assignments;
+
+  using ResourceSchedule = std::map<rmf_traffic::Time, Reservation>;
+  using ResourceSchedules = std::unordered_map<std::string, ResourceSchedule>;
+  ResourceSchedules _resource_schedules;
+
+  std::unordered_set<std::pair<ParticipantId, RequestId>, pair_hash> _unassigned;
+};
+
+}
+}

From ce982c74c3782cd30bd83558e3b1e5172fc22d2b Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Tue, 25 May 2021 18:36:05 +0800
Subject: [PATCH 06/66] fix minor compile issues

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../include/rmf_traffic/reservations/Participant.hpp  |  2 +-
 rmf_traffic/src/rmf_traffic/reservations/Database.cpp |  2 +-
 .../src/rmf_traffic/reservations/internal/State.hpp   | 11 +++++++++--
 3 files changed, 11 insertions(+), 4 deletions(-)

diff --git a/rmf_traffic/include/rmf_traffic/reservations/Participant.hpp b/rmf_traffic/include/rmf_traffic/reservations/Participant.hpp
index 245d6642..c9d88d65 100644
--- a/rmf_traffic/include/rmf_traffic/reservations/Participant.hpp
+++ b/rmf_traffic/include/rmf_traffic/reservations/Participant.hpp
@@ -19,7 +19,7 @@
 #define RMF_TRAFFIC__PARTICIPANT_HPP
 
 #include <rmf_traffic/reservations/Reservation.hpp>
-
+#include <rmf_traffic/reservations/ReservationRequest.hpp>
 namespace rmf_traffic {
 namespace reservations {
 
diff --git a/rmf_traffic/src/rmf_traffic/reservations/Database.cpp b/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
index c7608a4f..4c0c155c 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
@@ -80,7 +80,7 @@ void Database::request_reservation(
   ParticipantId id,
   RequestId req,
   std::vector<ReservationRequest>& request_options,
-  int priority = 0)
+  int priority)
 {
   _pimpl->request_reservation(id, req, request_options, priority);
 }
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index 98e25c37..2a66fc72 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -55,12 +55,18 @@ class State
 
   }
 
-  State delay_reservations_after(ReservationId id, Duration dur)
+  State remove_reservation(ReservationId res)
   {
 
   }
 
-  State bring_forward_reservations_before(ReservationId id, Duration dur)
+  std::optional<State> delay_reservations_after(ReservationId id, Duration dur)
+  {
+
+  }
+
+  std::optional<State>
+    bring_forward_reservations_before(ReservationId id, Duration dur)
   {
 
   }
@@ -78,3 +84,4 @@ class State
 
 }
 }
+#endif
\ No newline at end of file

From 756d15877fae300cf4f60a88406acd515a2901e9 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Thu, 27 May 2021 14:40:52 +0800
Subject: [PATCH 07/66] on par with the previous attempt

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../rmf_traffic/reservations/Participant.hpp  |   7 +-
 .../src/rmf_traffic/reservations/Database.cpp |   7 +-
 .../reservations/internal/RequestQueue.hpp    |  74 ++++++++-
 .../reservations/internal/State.hpp           | 148 ++++++++++++++++--
 4 files changed, 215 insertions(+), 21 deletions(-)

diff --git a/rmf_traffic/include/rmf_traffic/reservations/Participant.hpp b/rmf_traffic/include/rmf_traffic/reservations/Participant.hpp
index c9d88d65..546f14e6 100644
--- a/rmf_traffic/include/rmf_traffic/reservations/Participant.hpp
+++ b/rmf_traffic/include/rmf_traffic/reservations/Participant.hpp
@@ -28,7 +28,12 @@ using ParticipantId = uint64_t;
 class Participant
 {
 public:
-  virtual bool request_assignment_received(
+  virtual bool request_proposal_received(
+    RequestId id,
+    Reservation& res
+  ) = 0;
+
+  virtual bool request_proposal_confirmed(
     RequestId id,
     Reservation& res
   ) = 0;
diff --git a/rmf_traffic/src/rmf_traffic/reservations/Database.cpp b/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
index 4c0c155c..f07f5e4e 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
@@ -24,7 +24,8 @@ namespace reservations {
 class Database::Implementation
 {
 public:
-  Implementation()
+  Implementation():
+    _request_queue(std::make_shared<RequestQueue>())
   {
 
   }
@@ -41,7 +42,7 @@ class Database::Implementation
   )
   {
     _participant_store.remove_participant(id);
-    _request_queue.erase_participant_requests(id);
+    _request_queue->erase_participant_requests(id);
   }
 
   void request_reservation(
@@ -58,7 +59,7 @@ class Database::Implementation
   }
 private:
   ParticipantStore _participant_store;
-  RequestQueue _request_queue;
+  std::shared_ptr<RequestQueue> _request_queue;
 };
 
 void Database::register_participant(
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp
index 1dc66cd8..66385d58 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp
@@ -18,7 +18,8 @@
 #ifndef RMF_TRAFFIC__RESERVATIONS__INTERNAL_REQUESTQUEUE_HPP
 #define RMF_TRAFFIC__RESERVATIONS__INTERNAL_REQUESTQUEUE_HPP
 
-#include <rmf_traffic/reservations/Database.hpp>
+#include <rmf_traffic/reservations/ReservationRequest.hpp>
+#include <rmf_traffic/reservations/Reservation.hpp>
 #include <unordered_map>
 
 namespace rmf_traffic {
@@ -57,6 +58,77 @@ class RequestQueue
     _reservation_info.erase(entry);
   }
 
+  bool satisfies(ReservationRequest& req, Reservation reservation)
+  {
+    //Check upper bound
+    if(req.start_time()->lower_bound().has_value())
+    {
+      if(reservation.start_time() < req.start_time()->lower_bound().value())
+      {
+        return false;
+      }
+    }
+
+    //Check upper bound
+    if(req.start_time()->upper_bound().has_value())
+    {
+      if(reservation.start_time() > req.start_time()->upper_bound().value())
+      {
+        return false;
+      }
+    }
+
+    if(req.duration().has_value())
+    {
+      if(reservation.is_indefinite())
+      {
+        return false;
+      }
+      if(req.duration().value()
+        > *reservation.actual_finish_time() - reservation.start_time())
+      {
+        return false;
+      }
+    }
+
+    if(req.finish_time().has_value())
+    {
+      if(reservation.is_indefinite())
+      {
+        return false;
+      }
+      if(req.finish_time().value() > *reservation.actual_finish_time())
+      {
+        return false;
+      }
+    }
+
+    if(req.is_indefinite() && !reservation.is_indefinite()
+      || !req.is_indefinite() && reservation.is_indefinite())
+    {
+      return false;
+    }
+
+    if(req.resource_name() != reservation.resource_name())
+    {
+      return false;
+    }
+
+    return true;
+  }
+
+  bool satisfies(
+    ParticipantId pid,
+    RequestId reqid,
+    Reservation& res
+  ){
+    for(auto req: _reservation_info[pid][reqid].request_options)
+    {
+      if(satisfies(req, res))
+        return true;
+    }
+    return false;
+  }
 private:
   std::unordered_map<ParticipantId,
     std::unordered_map<RequestId, ReservationInfo>
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index 2a66fc72..5e86a6c4 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -27,6 +27,9 @@
 
 namespace rmf_traffic {
 namespace reservations {
+
+//TODO: Currrently uses a lot of copying. In future we should use a parent based
+//hierarchy
 class State
 {
   struct pair_hash {
@@ -38,31 +41,108 @@ class State
     }
   };
 public:
-  /// Adds a reservation to the state
-  /// Note: Assumes that reservation is valid.
-  State add_reservation(
+  using ResourceSchedule = std::map<rmf_traffic::Time, Reservation>;
+  using ResourceSchedules = std::unordered_map<std::string, ResourceSchedule>;
+  using ParticipantAssignment = std::unordered_map<
+    ParticipantId, std::unordered_map<RequestId, ReservationId>>;
+  using ReservationAssignment = std::unordered_map<
+    ParticipantId, std::unordered_map<ReservationId, RequestId>>;
+
+  using ReservationTimings = std::unordered_map<ReservationId, Time>;
+  using ReservationResources = std::unordered_map<ReservationId, std::string>;
+  using UnassignedSet = std::unordered_set<
+    std::pair<ParticipantId, RequestId>, pair_hash>;
+
+  State(std::shared_ptr<const RequestQueue> queue) :
+    _queue(queue)
+  {
+    //Do nothing
+  }
+  /// Adds a request to the state
+  State add_request(
     ParticipantId pid,
-    RequestId reqid,
-    Reservation res)
+    RequestId reqid) const
   {
-
+    State new_state(*this);
+    new_state._unassigned.insert({pid, reqid});
+    return new_state;
   }
 
   State remove_request(
     ParticipantId pid,
-    RequestId reqid)
+    RequestId reqid) const
   {
+    State new_state(*this);
+    auto req = new_state._unassigned.find({pid, reqid});
 
-  }
+    if(req != new_state._unassigned.end())
+    {
+      new_state._unassigned.erase(req);
+      return new_state;
+    }
+    auto reservation_id = new_state._assignments[pid][reqid];
+    auto findings = new_state._reservation_timings.find(reservation_id);
+    if(findings == new_state._reservation_timings.end())
+    {
+      // Non existant pid-reqid pair
+      return;
+    }
+    auto time = findings->second;
+    auto resource = new_state._reservation_resources[reservation_id];
 
-  State remove_reservation(ReservationId res)
-  {
+    new_state._resource_schedules[resource].erase(time);
+    new_state._reservation_resources.erase(reservation_id);
+    new_state._reservation_assignment.erase(reservation_id);
+    new_state._reservation_timings.erase(findings);
 
+    return new_state;
   }
 
   std::optional<State> delay_reservations_after(ReservationId id, Duration dur)
   {
+    State new_state(*this);
+    auto resource = new_state._reservation_resources.find(id);
+    if(resource == new_state._reservation_resources.end())
+    {
+      // This is not possible
+      return std::nullopt;
+    }
+    auto start_time = new_state._reservation_timings[id];
+    auto res_iter = new_state._resource_schedules[resource->second]
+      .find(start_time);
+    auto prev_iter = res_iter;
+    std::next(res_iter);
+
+    // TODO: This is extremely inefficient
+    std::map<Time, ReservationId> new_times;
+    auto gap_left = dur;
+    while(
+      res_iter != new_state._resource_schedules[resource->second].end()
+      && prev_iter->second.actual_finish_time().has_value()
+      && gap_left.count() > 0)
+    {
+      auto new_time = res_iter->first + gap_left;
+      auto gap = res_iter->first - prev_iter->second.actual_finish_time().value();
+      if(gap < gap_left)
+        gap_left -= gap;
+      else
+        gap_left -= gap_left;
+      new_times[new_time] = res_iter->second.reservation_id();
+      prev_iter = res_iter;
+      std::next(res_iter);
+    }
 
+    for(auto iter = new_times.rbegin(); iter != new_times.rend(); iter++)
+    {
+      bool ok  = new_state.shift_reservation_start_time(
+        resource->second,
+        iter->second,
+        iter->first);
+
+      if(!ok) return std::nullopt;
+    }
+
+    return {new_state}; //RIP another invocation of copy constructor
   }
 
   std::optional<State>
@@ -71,15 +151,51 @@ class State
 
   }
 
+  bool operator==(State& other) const
+  {
+
+  }
+
+  std::size_t hash()
+  {
+
+  }
+
+  State(const State& other) :
+    _resource_schedules(other._resource_schedules),
+    _assignments(other._assignments),
+    _unassigned(other._unassigned),
+    _reservation_timings(other._reservation_timings),
+    _queue(other._queue),
+    _reservation_resources(other._reservation_resources),
+    _reservation_assignment(other._reservation_assignment)
+  {
+    // Do nothing
+  }
 private:
-  std::unordered_map<
-    std::pair<ParticipantId, RequestId>, ReservationId, pair_hash> _assignments;
+  // Returns true if a shift could be successfully applied. Else returns false.
+  bool shift_reservation_start_time(
+    std::string resource,
+    ReservationId res_id,
+    rmf_traffic::Time new_time)
+  {
+    auto original_time = _reservation_timings[res_id];
+    auto new_start =
+      _resource_schedules[resource][original_time]
+        .propose_new_start_time(new_time);
 
-  using ResourceSchedule = std::map<rmf_traffic::Time, Reservation>;
-  using ResourceSchedules = std::unordered_map<std::string, ResourceSchedule>;
-  ResourceSchedules _resource_schedules;
+    _resource_schedules[resource].erase(original_time);
+    _reservation_timings[res_id] = new_time;
+  }
 
-  std::unordered_set<std::pair<ParticipantId, RequestId>, pair_hash> _unassigned;
+  
+  ResourceSchedules _resource_schedules;
+  ParticipantAssignment _assignments;
+  ReservationAssignment _reservation_assignment;
+  UnassignedSet _unassigned;
+  ReservationTimings _reservation_timings;
+  ReservationResources _reservation_resources;
+  std::shared_ptr<const RequestQueue> _queue;
 };
 
 }

From b64be02cc3033cef3d52ce0030f4bdb79f9294a7 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Thu, 27 May 2021 14:48:50 +0800
Subject: [PATCH 08/66] Add equality operator

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index 5e86a6c4..b778177c 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -145,6 +145,7 @@ class State
     return {new_state}; //RIP another invocation of copy constructor
   }
 
+  // TODO: Implement later so we have more flexibility.
   std::optional<State>
     bring_forward_reservations_before(ReservationId id, Duration dur)
   {
@@ -153,7 +154,8 @@ class State
 
   bool operator==(State& other) const
   {
-
+    return _resource_schedules == other._resource_schedules
+      && _assignments == other._assignments;
   }
 
   std::size_t hash()
@@ -188,7 +190,6 @@ class State
     _reservation_timings[res_id] = new_time;
   }
 
-  
   ResourceSchedules _resource_schedules;
   ParticipantAssignment _assignments;
   ReservationAssignment _reservation_assignment;

From ab802cc2049fd7a3b5f485e528f6b6688730a949 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Fri, 28 May 2021 16:14:57 +0800
Subject: [PATCH 09/66] Add support for `NextStateGenerator` .

NextStateGenerator provides the basics for iterating in the planner.

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../reservations/internal/Optimizer.hpp       |  21 ++
 .../reservations/internal/RequestQueue.hpp    |  26 ++-
 .../reservations/internal/State.hpp           | 194 +++++++++++++++---
 3 files changed, 207 insertions(+), 34 deletions(-)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/Optimizer.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/Optimizer.hpp
index e69de29b..b51306c5 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/Optimizer.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/Optimizer.hpp
@@ -0,0 +1,21 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * 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.
+ *
+*/
+
+#ifndef RMF_TRAFFIC__OPTIMIZER__INTERNAL_REQUESTQUEUE_HPP
+#define RMF_TRAFFIC__OPTIMIZER__INTERNAL_REQUESTQUEUE_HPP
+
+#endif
\ No newline at end of file
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp
index 66385d58..8f9cce5a 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp
@@ -28,11 +28,19 @@ namespace reservations {
 class RequestQueue
 {
 public:
-  struct ReservationInfo {
+  struct RequestInfo {
     int priority;
     std::vector<ReservationRequest> request_options;
   };
 
+  RequestInfo get_request_info(
+    ParticipantId pid,
+    RequestId req)
+  {
+    //TODO: Sanitize
+    return _reservation_info[pid][req];
+  }
+
   void enqueue_reservation(
     ParticipantId pid,
     RequestId req,
@@ -117,21 +125,23 @@ class RequestQueue
     return true;
   }
 
-  bool satisfies(
+  std::optional<std::size_t> satisfies(
     ParticipantId pid,
     RequestId reqid,
     Reservation& res
-  ){
-    for(auto req: _reservation_info[pid][reqid].request_options)
+  ) {
+    for(std::size_t i = 0;
+      i < _reservation_info[pid][reqid].request_options.size();
+      i++)
     {
-      if(satisfies(req, res))
-        return true;
+      if(satisfies(_reservation_info[pid][reqid].request_options[i], res))
+        return i;
     }
-    return false;
+    return std::nullopt;
   }
 private:
   std::unordered_map<ParticipantId,
-    std::unordered_map<RequestId, ReservationInfo>
+    std::unordered_map<RequestId, RequestInfo>
     >  _reservation_info;
 };
 
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index b778177c..7a2b5add 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -28,6 +28,7 @@
 namespace rmf_traffic {
 namespace reservations {
 
+struct NextStateGenerator;
 //TODO: Currrently uses a lot of copying. In future we should use a parent based
 //hierarchy
 class State
@@ -41,23 +42,38 @@ class State
     }
   };
 public:
+
+  struct ReservationAssignmentIndex
+  {
+    ParticipantId participant;
+    RequestId reqid;
+    std::size_t index;
+  };
   using ResourceSchedule = std::map<rmf_traffic::Time, Reservation>;
   using ResourceSchedules = std::unordered_map<std::string, ResourceSchedule>;
-  using ParticipantAssignment = std::unordered_map<
-    ParticipantId, std::unordered_map<RequestId, ReservationId>>;
   using ReservationAssignment = std::unordered_map<
     ParticipantId, std::unordered_map<ReservationId, RequestId>>;
 
   using ReservationTimings = std::unordered_map<ReservationId, Time>;
   using ReservationResources = std::unordered_map<ReservationId, std::string>;
+  using ReservationRequestId = std::unordered_map<ReservationId,
+    ReservationAssignmentIndex>;
   using UnassignedSet = std::unordered_set<
     std::pair<ParticipantId, RequestId>, pair_hash>;
 
-  State(std::shared_ptr<const RequestQueue> queue) :
+  State(std::shared_ptr<RequestQueue> queue) :
     _queue(queue)
   {
     //Do nothing
   }
+
+  NextStateGenerator begin()
+  {
+  }
+
+  NextStateGenerator end()
+  {
+  }
   /// Adds a request to the state
   State add_request(
     ParticipantId pid,
@@ -80,7 +96,7 @@ class State
       new_state._unassigned.erase(req);
       return new_state;
     }
-    auto reservation_id = new_state._assignments[pid][reqid];
+    auto reservation_id = new_state._reservation_assignments[pid][reqid];
     auto findings = new_state._reservation_timings.find(reservation_id);
     if(findings == new_state._reservation_timings.end())
     {
@@ -92,9 +108,8 @@ class State
 
     new_state._resource_schedules[resource].erase(time);
     new_state._reservation_resources.erase(reservation_id);
-    new_state._reservation_assignment.erase(reservation_id);
+    new_state._reservation_assignments.erase(reservation_id);
     new_state._reservation_timings.erase(findings);
-
     return new_state;
   }
 
@@ -142,20 +157,13 @@ class State
       if(!ok) return std::nullopt;
     }
 
-    return {new_state}; //RIP another invocation of copy constructor
-  }
-
-  // TODO: Implement later so we have more flexibility.
-  std::optional<State>
-    bring_forward_reservations_before(ReservationId id, Duration dur)
-  {
-
+    return {new_state};
   }
 
   bool operator==(State& other) const
   {
     return _resource_schedules == other._resource_schedules
-      && _assignments == other._assignments;
+      && _reservation_assignments == other._reservation_assignments;
   }
 
   std::size_t hash()
@@ -165,15 +173,16 @@ class State
 
   State(const State& other) :
     _resource_schedules(other._resource_schedules),
-    _assignments(other._assignments),
     _unassigned(other._unassigned),
     _reservation_timings(other._reservation_timings),
     _queue(other._queue),
     _reservation_resources(other._reservation_resources),
-    _reservation_assignment(other._reservation_assignment)
+    _reservation_assignments(other._reservation_assignments),
+    _reservation_request_ids(other._reservation_request_ids)
   {
     // Do nothing
   }
+
 private:
   // Returns true if a shift could be successfully applied. Else returns false.
   bool shift_reservation_start_time(
@@ -182,21 +191,154 @@ class State
     rmf_traffic::Time new_time)
   {
     auto original_time = _reservation_timings[res_id];
-    auto new_start =
+    auto new_reservation =
       _resource_schedules[resource][original_time]
         .propose_new_start_time(new_time);
-
+    auto request_info = _reservation_request_ids[res_id];
+    auto index = _queue->satisfies(
+      request_info.participant,
+      request_info.reqid,
+      new_reservation);
+    if(!index.has_value())
+    {
+      return false;
+    }
+    _reservation_request_ids[res_id].reqid = *index;
     _resource_schedules[resource].erase(original_time);
+    _resource_schedules[resource][new_time] = new_reservation;
     _reservation_timings[res_id] = new_time;
+    return true;
+  }
+
+  ResourceSchedules _resource_schedules; // resource_name => Schedules
+  ReservationAssignment _reservation_assignments; // {participant, req_id} => {reservation_id
+  UnassignedSet _unassigned; // {participant, req_id}
+  ReservationTimings _reservation_timings; // reservation => time
+  ReservationResources _reservation_resources; // reservation => resource
+  ReservationRequestId _reservation_request_ids; // reservation => req_id
+
+  std::shared_ptr<RequestQueue> _queue;
+  friend NextStateGenerator;
+};
+struct NextStateGenerator
+{
+  //TODO: RAW_POINTER-FOO
+  State* start_state;
+  std::optional<State> current_state;
+  enum PotentialActionMode
+  {
+    ASSIGN_ITEMS,
+    REMOVE_ITEMS,
+    END
+  };
+  PotentialActionMode mode = ASSIGN_ITEMS;
+  State::UnassignedSet::const_iterator unassigned_iter;
+  
+  ReservationRequest curr_request;
+  State::ResourceSchedule::const_iterator insertion_point_iter;
+  State::ResourceSchedules::const_iterator remove_iter;
+  std::size_t request_index = 0;
+
+  using difference_type = std::ptrdiff_t;
+  using value_type = State;
+  using pointer = State*;
+  using reference = State;
+  using iterator_category= std::input_iterator_tag;
+
+  State operator*() const {
+    return *current_state;
+  }
+
+  State::ResourceSchedule::const_iterator
+    get_search_start(ReservationRequest req)
+  {
+    if(req.start_time().has_value() &&
+      req.start_time().value().lower_bound().has_value())
+    {
+      return start_state->
+        _resource_schedules[req.resource_name()]
+        .lower_bound(
+          curr_request.start_time().value().lower_bound().value());
+    }
+    else
+    {
+      return start_state->_resource_schedules[req.resource_name()].begin();
+    }
+  }
+
+  State::ResourceSchedule::const_iterator
+    get_search_end(ReservationRequest req)
+  {
+    if(req.start_time().has_value() &&
+      req.start_time().value().lower_bound().has_value())
+    {
+      return start_state->
+        _resource_schedules[req.resource_name()]
+        .upper_bound(
+          curr_request.start_time().value().upper_bound().value());
+    }
+    else
+    {
+      return start_state->_resource_schedules[req.resource_name()].end();
+    }
+  }
+
+  std::optional<State> next_state() {
+    if(mode == PotentialActionMode::ASSIGN_ITEMS)
+    {
+      if(unassigned_iter == start_state->_unassigned.end())
+      {
+        mode = PotentialActionMode::REMOVE_ITEMS;
+      }
+      else
+      {
+        auto [participant_id, request_id] = *unassigned_iter;
+        auto info =
+          start_state->_queue->get_request_info(participant_id, request_id);
+        if(request_index >= info.request_options.size())
+        {
+          unassigned_iter++;
+        }
+        else
+        {
+          auto curr_request = info.request_options[request_index];
+          // Get all reservations within current request and attempt insertion
+          
+          request_index++;
+        }
+      }
+    }
+    else if(mode == REMOVE_ITEMS)
+    {
+
+    }
+  }
+
+  NextStateGenerator& operator++() {
+    current_state = next_state();
+    return *this;
+  }
+
+  NextStateGenerator operator++(int) {
+    NextStateGenerator r = *this;
+    ++(*this);
+    return r;
   }
 
-  ResourceSchedules _resource_schedules;
-  ParticipantAssignment _assignments;
-  ReservationAssignment _reservation_assignment;
-  UnassignedSet _unassigned;
-  ReservationTimings _reservation_timings;
-  ReservationResources _reservation_resources;
-  std::shared_ptr<const RequestQueue> _queue;
+  bool operator==(const NextStateGenerator& other)
+  {
+    if(mode == PotentialActionMode::END
+    && other.mode == PotentialActionMode::END)
+    {
+      return true;
+    }
+    return false;
+  }
+
+  bool operator!=(const NextStateGenerator& other)
+  {
+    return !(*this == other);
+  }
 };
 
 }

From 82adb7523e8fcb4089234e659420c0979eb2eb13 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Mon, 31 May 2021 09:04:53 +0800
Subject: [PATCH 10/66] add removals

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../reservations/internal/State.hpp           | 52 +++++++++++++++++--
 1 file changed, 49 insertions(+), 3 deletions(-)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index 7a2b5add..6f52e22b 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -160,6 +160,12 @@ class State
     return {new_state};
   }
 
+  
+  void purge_up_to_time(Time time)
+  {
+
+  }
+
   bool operator==(State& other) const
   {
     return _resource_schedules == other._resource_schedules
@@ -185,6 +191,20 @@ class State
 
 private:
   // Returns true if a shift could be successfully applied. Else returns false.
+  void unassign_reservation(ResourceSchedule::const_iterator iter)
+  {
+    auto res_id = iter->second.reservation_id();
+    auto resource_name = iter->second.resource_name();
+
+    _reservation_timings.erase(res_id);
+    _reservation_resources.erase(res_id);
+    auto request = _reservation_request_ids[res_id];
+
+    _unassigned.insert({request.participant , request.reqid});
+    _reservation_assignments[request.participant].erase(request.reqid);
+    _reservation_request_ids.erase(res_id);
+  }
+
   bool shift_reservation_start_time(
     std::string resource,
     ReservationId res_id,
@@ -215,7 +235,7 @@ class State
   UnassignedSet _unassigned; // {participant, req_id}
   ReservationTimings _reservation_timings; // reservation => time
   ReservationResources _reservation_resources; // reservation => resource
-  ReservationRequestId _reservation_request_ids; // reservation => req_id
+  ReservationRequestId _reservation_request_ids; // reservation => {part_id, req_id, index}
 
   std::shared_ptr<RequestQueue> _queue;
   friend NextStateGenerator;
@@ -231,12 +251,15 @@ struct NextStateGenerator
     REMOVE_ITEMS,
     END
   };
+
   PotentialActionMode mode = ASSIGN_ITEMS;
   State::UnassignedSet::const_iterator unassigned_iter;
-  
   ReservationRequest curr_request;
   State::ResourceSchedule::const_iterator insertion_point_iter;
-  State::ResourceSchedules::const_iterator remove_iter;
+
+  // For iterating through removals
+  State::ResourceSchedules::const_iterator remove_resource_iter;
+  State::ResourceSchedule::const_iterator remove_iter;
   std::size_t request_index = 0;
 
   using difference_type = std::ptrdiff_t;
@@ -283,6 +306,29 @@ struct NextStateGenerator
     }
   }
 
+  std::optional<State> advance_assignments()
+  {
+    
+  }
+
+  std::optional<State> advance_removals()
+  {
+    if(remove_resource_iter == start_state->_resource_schedules.end())
+    {
+      return std::nullopt;
+    }
+    while(remove_iter == remove_resource_iter->second.end())
+    {
+      remove_resource_iter++;
+      remove_iter = remove_resource_iter->second.begin();
+    }
+
+    State new_state(start_state);
+    new_state.remove_request(remove_iter);
+    remove_iter++;
+    return new_state;
+  }
+
   std::optional<State> next_state() {
     if(mode == PotentialActionMode::ASSIGN_ITEMS)
     {

From f2698b25d4ddbd27e18ee5eb5e9144478dc91835 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Tue, 1 Jun 2021 14:12:32 +0800
Subject: [PATCH 11/66] Add insertion points

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../reservations/internal/State.hpp           | 232 +++++++++++-------
 1 file changed, 141 insertions(+), 91 deletions(-)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index 6f52e22b..38627035 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -113,54 +113,6 @@ class State
     return new_state;
   }
 
-  std::optional<State> delay_reservations_after(ReservationId id, Duration dur)
-  {
-    State new_state(*this);
-    auto resource = new_state._reservation_resources.find(id);
-    if(resource == new_state._reservation_resources.end())
-    {
-      // This is not possible
-      return std::nullopt;
-    }
-    auto start_time = new_state._reservation_timings[id];
-    auto res_iter = new_state._resource_schedules[resource->second]
-      .find(start_time);
-    auto prev_iter = res_iter;
-    std::next(res_iter);
-
-    // TODO: This is extremely inefficient
-    std::map<Time, ReservationId> new_times;
-    auto gap_left = dur;
-    while(
-      res_iter != new_state._resource_schedules[resource->second].end()
-      && prev_iter->second.actual_finish_time().has_value()
-      && gap_left.count() > 0)
-    {
-      auto new_time = res_iter->first + gap_left;
-      auto gap = res_iter->first - prev_iter->second.actual_finish_time().value();
-      if(gap < gap_left)
-        gap_left -= gap;
-      else
-        gap_left -= gap_left;
-      new_times[new_time] = res_iter->second.reservation_id();
-      prev_iter = res_iter;
-      std::next(res_iter);
-    }
-
-    for(auto iter = new_times.rbegin(); iter != new_times.rend(); iter++)
-    {
-      bool ok  = new_state.shift_reservation_start_time(
-        resource->second,
-        iter->second,
-        iter->first);
-
-      if(!ok) return std::nullopt;
-    }
-
-    return {new_state};
-  }
-
-  
   void purge_up_to_time(Time time)
   {
 
@@ -191,11 +143,9 @@ class State
 
 private:
   // Returns true if a shift could be successfully applied. Else returns false.
-  void unassign_reservation(ResourceSchedule::const_iterator iter)
+  void unassign_reservation(ReservationId res_id, std::string resource_name)
   {
-    auto res_id = iter->second.reservation_id();
-    auto resource_name = iter->second.resource_name();
-
+    auto time =  _reservation_timings[res_id];
     _reservation_timings.erase(res_id);
     _reservation_resources.erase(res_id);
     auto request = _reservation_request_ids[res_id];
@@ -203,6 +153,59 @@ class State
     _unassigned.insert({request.participant , request.reqid});
     _reservation_assignments[request.participant].erase(request.reqid);
     _reservation_request_ids.erase(res_id);
+    _resource_schedules[resource_name].erase(time);
+  }
+
+  bool assign_reservation(
+    ParticipantId part_id,
+    RequestId req_id,
+    Reservation res)
+  {
+    auto index = _queue->satisfies(part_id, req_id, res);
+    if(!index.has_value())
+    {
+      return false;
+    }
+
+    if(res.is_indefinite())
+    {
+      //infinite reservation. Must be last
+      auto next_reservation =
+        _resource_schedules[res.resource_name()].lower_bound(
+          res.start_time()
+        );
+      if(next_reservation != _resource_schedules[res.resource_name()].end())
+      {
+        return false;
+      }
+    }
+    else
+    {
+      // Get the insertion point
+      auto next_reservation = _resource_schedules[res.resource_name()].lower_bound(
+        res.start_time()
+      );
+      if(next_reservation->second.start_time() < res.actual_finish_time())
+      {
+
+      }
+
+    }
+
+    _resource_schedules[res.resource_name()].insert({
+      res.start_time(), res});
+    _reservation_timings.insert({res.reservation_id(), res.start_time()});
+    _reservation_assignments[part_id][req_id] = res.reservation_id();
+    _reservation_resources[res.reservation_id()] = res.resource_name();
+    _reservation_request_ids.insert({
+      res.reservation_id(),
+      {
+        part_id,
+        req_id,
+        index.value()
+      }});
+    _unassigned.erase({part_id, req_id});
+    return true;
   }
 
   bool shift_reservation_start_time(
@@ -244,7 +247,7 @@ struct NextStateGenerator
 {
   //TODO: RAW_POINTER-FOO
   State* start_state;
-  std::optional<State> current_state;
+  std::optional<State> visiting_state;
   enum PotentialActionMode
   {
     ASSIGN_ITEMS,
@@ -253,9 +256,12 @@ struct NextStateGenerator
   };
 
   PotentialActionMode mode = ASSIGN_ITEMS;
+
+  // For iterating through assignments
   State::UnassignedSet::const_iterator unassigned_iter;
-  ReservationRequest curr_request;
   State::ResourceSchedule::const_iterator insertion_point_iter;
+  State::ResourceSchedule::const_iterator insertion_point_end;
+  std::size_t request_index_iter = 0;
 
   // For iterating through removals
   State::ResourceSchedules::const_iterator remove_resource_iter;
@@ -269,11 +275,11 @@ struct NextStateGenerator
   using iterator_category= std::input_iterator_tag;
 
   State operator*() const {
-    return *current_state;
+    return *visiting_state;
   }
 
   State::ResourceSchedule::const_iterator
-    get_search_start(ReservationRequest req)
+    get_search_start(const ReservationRequest& req)
   {
     if(req.start_time().has_value() &&
       req.start_time().value().lower_bound().has_value())
@@ -281,7 +287,7 @@ struct NextStateGenerator
       return start_state->
         _resource_schedules[req.resource_name()]
         .lower_bound(
-          curr_request.start_time().value().lower_bound().value());
+          req.start_time().value().lower_bound().value());
     }
     else
     {
@@ -290,7 +296,7 @@ struct NextStateGenerator
   }
 
   State::ResourceSchedule::const_iterator
-    get_search_end(ReservationRequest req)
+    get_search_end(const ReservationRequest& req)
   {
     if(req.start_time().has_value() &&
       req.start_time().value().lower_bound().has_value())
@@ -298,7 +304,7 @@ struct NextStateGenerator
       return start_state->
         _resource_schedules[req.resource_name()]
         .upper_bound(
-          curr_request.start_time().value().upper_bound().value());
+          req.start_time().value().upper_bound().value());
     }
     else
     {
@@ -308,7 +314,76 @@ struct NextStateGenerator
 
   std::optional<State> advance_assignments()
   {
-    
+    while(true)
+    {
+      if(unassigned_iter == start_state->_unassigned.end())
+      {
+        return std::nullopt;
+      }
+      auto request_info = start_state->_queue->get_request_info(
+        unassigned_iter->first,
+        unassigned_iter->second);
+
+      while(request_index_iter >= request_info.request_options.size())
+      {
+        request_index_iter = 0;
+        unassigned_iter++;
+        request_info = start_state->_queue->get_request_info(
+          unassigned_iter->first,
+          unassigned_iter->second);
+
+        if(request_info.request_options.size() > 0)
+        {
+          insertion_point_iter =
+            get_search_start(request_info.request_options[request_index]);
+          insertion_point_end =
+            get_search_end(request_info.request_options[request_index]);
+        }
+      }
+
+      while(insertion_point_iter == insertion_point_end)
+      {
+        request_index_iter++;
+        request_info = start_state->_queue->get_request_info(
+          unassigned_iter->first,
+          unassigned_iter->second);
+
+        if(request_info.request_options.size() > 0)
+        {
+          insertion_point_iter =
+            get_search_start(request_info.request_options[request_index]);
+          insertion_point_end =
+            get_search_end(request_info.request_options[request_index]);
+        }
+      }
+
+      auto resource_name =
+        request_info.request_options[request_index_iter].resource_name();
+      auto current_request = request_info.request_options[request_index];
+
+      State new_state(*start_state);
+
+      if(insertion_point_iter->second.actual_finish_time().has_value())
+      {
+        Reservation new_res = Reservation::make_reservation(
+          insertion_point_iter->second.actual_finish_time().value(),
+          resource_name,
+          current_request.duration(),
+          current_request.finish_time()
+        );
+        bool ok = new_state.assign_reservation(
+          unassigned_iter->first,
+          unassigned_iter->second,
+          new_res
+        );
+        if(ok)
+        {
+          insertion_point_iter++;
+          return new_state;
+        }
+      }
+      insertion_point_iter++;
+    }
   }
 
   std::optional<State> advance_removals()
@@ -323,45 +398,20 @@ struct NextStateGenerator
       remove_iter = remove_resource_iter->second.begin();
     }
 
-    State new_state(start_state);
-    new_state.remove_request(remove_iter);
+    State new_state(*start_state);
+    new_state.unassign_reservation(
+      remove_iter->second.reservation_id(),
+      remove_iter->second.resource_name());
     remove_iter++;
     return new_state;
   }
 
   std::optional<State> next_state() {
-    if(mode == PotentialActionMode::ASSIGN_ITEMS)
-    {
-      if(unassigned_iter == start_state->_unassigned.end())
-      {
-        mode = PotentialActionMode::REMOVE_ITEMS;
-      }
-      else
-      {
-        auto [participant_id, request_id] = *unassigned_iter;
-        auto info =
-          start_state->_queue->get_request_info(participant_id, request_id);
-        if(request_index >= info.request_options.size())
-        {
-          unassigned_iter++;
-        }
-        else
-        {
-          auto curr_request = info.request_options[request_index];
-          // Get all reservations within current request and attempt insertion
-          
-          request_index++;
-        }
-      }
-    }
-    else if(mode == REMOVE_ITEMS)
-    {
-
-    }
+    
   }
 
   NextStateGenerator& operator++() {
-    current_state = next_state();
+    visiting_state = next_state();
     return *this;
   }
 

From 5a2c9f98706720697aee8ec36295dd8b7af73bac Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Tue, 1 Jun 2021 16:45:11 +0800
Subject: [PATCH 12/66] Add hashes

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../reservations/internal/State.hpp           | 62 ++++++++++++++++++-
 1 file changed, 60 insertions(+), 2 deletions(-)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index 38627035..58cedfe6 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -29,6 +29,14 @@ namespace rmf_traffic {
 namespace reservations {
 
 struct NextStateGenerator;
+
+// Ripped from https://stackoverflow.com/questions/2590677/how-do-i-combine-hash-values-in-c0x
+template <class T>
+inline void hash_combine(std::size_t& seed, const T& v)
+{
+    std::hash<T> hasher;
+    seed ^= hasher(v) + 0x9e3779b9 + (seed<<6) + (seed>>2);
+}
 //TODO: Currrently uses a lot of copying. In future we should use a parent based
 //hierarchy
 class State
@@ -73,6 +81,9 @@ class State
 
   NextStateGenerator end()
   {
+    NextStateGenerator gen;
+    gen.mode == NextStateGenerator::PotentialActionMode::END;
+    return gen;
   }
   /// Adds a request to the state
   State add_request(
@@ -118,6 +129,31 @@ class State
 
   }
 
+  void debug_state()
+  {
+    for(auto &[resource, schedule]: _resource_schedules)
+    {
+      std::cout << "________________________" <<std::endl;
+      std::cout << "Resource: " << std::endl;
+      for(auto &[time, reservation]: schedule)
+      {
+        std::cout << "\t======"
+        std::cout << "\tReservation: " << reservation.reservation_id() << std::endl;
+        std::cout << "\tStart time: " << time.time_since_epoch().count()/1e9<< std::endl;
+        if(!reservation.is_indefinite())
+          std::cout << "\tEnd time: " <<
+            reservation.actual_finish_time()->time_since_epoch().count()/1e9
+            << std::endl;
+        std::cout << "\tParticipant: " <<
+          _reservation_request_ids[reservation.reservation_id()].participant
+          << std::endl;
+        std::cout << "\tRequestID: " <<
+          _reservation_request_ids[reservation.resource_name()].request
+          << std::endl;
+      }
+    }
+  }
+
   bool operator==(State& other) const
   {
     return _resource_schedules == other._resource_schedules
@@ -126,7 +162,21 @@ class State
 
   std::size_t hash()
   {
-
+    std::size_t seed = 0x928193843;
+    for(auto &[resource, schedule]: _resource_schedules)
+    {
+      for(auto &[time, reservation]: schedule)
+      {
+        hash_combine(seed, time.time_since_epoch().count());
+        hash_combine(
+          seed,
+          _reservation_request_ids[reservation.reservation_id()].participant);
+        hash_combine(
+          seed,
+          _reservation_request_ids[reservation.reservation_id()].reqid);
+      }
+    }
+    return seed;
   }
 
   State(const State& other) :
@@ -407,7 +457,15 @@ struct NextStateGenerator
   }
 
   std::optional<State> next_state() {
-    
+    auto assignment = advance_assignments();
+    if(assignment.has_value())
+      return assignment;
+    mode = PotentialActionMode::REMOVE_ITEMS;
+    auto removals = advance_removals();
+    if(removals.has_value())
+      return removals;
+    mode = PotentialActionMode::END;
+    return std::nullopt;
   }
 
   NextStateGenerator& operator++() {

From e5155ee35a1e565e775be301bd09fbb5fae3b8ce Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Tue, 1 Jun 2021 18:41:01 +0800
Subject: [PATCH 13/66] add push_back

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../reservations/internal/State.hpp           | 47 ++++++++++++++++++-
 1 file changed, 45 insertions(+), 2 deletions(-)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index 58cedfe6..951a995d 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -192,6 +192,44 @@ class State
   }
 
 private:
+  void push_back(
+    std::string resource,
+    ResourceSchedule::const_iterator res_iter,
+    Duration dur
+  )
+  {
+    auto prev_iter = res_iter;
+    std::next(res_iter);
+
+    // TODO: This is extremely inefficient. Do in-place.
+    std::map<Time, ReservationId> new_times;
+    auto gap_left = dur;
+    while(
+      res_iter != new_state._resource_schedules[resource->second].end()
+      && prev_iter->second.actual_finish_time().has_value()
+      && gap_left.count() > 0)
+    {
+      auto new_time = res_iter->first + gap_left;
+      auto gap = res_iter->first - prev_iter->second.actual_finish_time().value();
+      if(gap < gap_left)
+        gap_left -= gap;
+      else
+        gap_left -= gap_left;
+      new_times[new_time] = res_iter->second.reservation_id();
+      prev_iter = res_iter;
+      std::next(res_iter);
+    }
+
+    for(auto iter = new_times.rbegin(); iter != new_times.rend(); iter++)
+    {
+      bool ok  = new_state.shift_reservation_start_time(
+        resource->second,
+        iter->second,
+        iter->first);
+
+      if(!ok) return std::nullopt;
+    }
+  }
   // Returns true if a shift could be successfully applied. Else returns false.
   void unassign_reservation(ReservationId res_id, std::string resource_name)
   {
@@ -235,9 +273,14 @@ class State
       auto next_reservation = _resource_schedules[res.resource_name()].lower_bound(
         res.start_time()
       );
-      if(next_reservation->second.start_time() < res.actual_finish_time())
+      if(next_reservation != _resource_schedules[res.resource_name()].end()
+        && res.actual_finish_time().has_value()
+        && next_reservation->second.start_time() < res.actual_finish_time().value())
       {
-
+        push_back(
+          res.resource_name(),
+          next_reservation,
+          res.actual_finish_time().value());
       }
 
     }

From 45172f6acf06bb92bda5d65b1eb170414042b31d Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Wed, 2 Jun 2021 14:12:40 +0800
Subject: [PATCH 14/66] Add begining

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../reservations/internal/State.hpp           | 54 +++++++++++++++++++
 .../unit/reservations/internal/test_State.cpp | 21 ++++++++
 2 files changed, 75 insertions(+)
 create mode 100644 rmf_traffic/test/unit/reservations/internal/test_State.cpp

diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index 951a995d..8f5a42ea 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -77,6 +77,60 @@ class State
 
   NextStateGenerator begin()
   {
+    NextStateGenerator gen;
+    gen.mode = NextStateGenerator::PotentialActionMode::ASSIGN_ITEMS;
+    gen.start_state = this;
+    if(_unassigned.size() > 0)
+    {
+      auto current_request = _queue->get_request_info(
+        _unassigned.begin()->first,
+        _unassigned.begin()->second).request_options[0];
+
+      gen.unassigned_iter = _unassigned.begin();
+      gen.insertion_point_iter = gen.get_search_start(current_request);
+      gen.insertion_point_end = gen.get_search_end(current_request);
+
+      State next_state(*this);
+      auto new_res = [=]() -> Reservation {
+        if(gen.insertion_point_end != gen.insertion_point_iter)
+        {
+          return Reservation::make_reservation(
+            gen.insertion_point_iter->second.actual_finish_time().value(),
+            resource_name,
+            current_request.duration(),
+            current_request.finish_time()
+          );
+        }
+        else
+        {
+          if(current_request.start_time().has_value())
+          {
+            return Reservation::make_reservation(
+              current_request.start_time().value(),
+              resource_name,
+              current_request.duration(),
+              current_request.finish_time()
+            );
+          }
+          else
+          {
+            //TODO some API for time syncing
+          }
+        }
+      }();
+      next_state.assign_reservation(
+        unassigned_iter->first,
+        unassigned_iter->second,
+        new_res
+      )
+      gen.next_state = next_state;
+    }
+    gen.remove_resource_iter = _resource_schedules.begin();
+    if(_resource_schedules.size() > 0)
+    {
+      gen.remove_iter = gen.remove_resource_iter->first;
+    }
+    return gen;
   }
 
   NextStateGenerator end()
diff --git a/rmf_traffic/test/unit/reservations/internal/test_State.cpp b/rmf_traffic/test/unit/reservations/internal/test_State.cpp
new file mode 100644
index 00000000..16c339a0
--- /dev/null
+++ b/rmf_traffic/test/unit/reservations/internal/test_State.cpp
@@ -0,0 +1,21 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * 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.
+ *
+*/
+
+#include "../../../../src/rmf_traffic/reservations/internal/State.hpp"
+
+#include <rmf_utils/catch.hpp>
+

From 2c825de62562a05d2bddad82c783c3705e1febf5 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Wed, 2 Jun 2021 15:42:40 +0800
Subject: [PATCH 15/66] fix le compile issues

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../rmf_traffic/reservations/Reservation.hpp  |   3 +-
 .../src/rmf_traffic/reservations/Database.cpp |   1 +
 .../reservations/internal/State.hpp           | 159 ++++++++++--------
 3 files changed, 88 insertions(+), 75 deletions(-)

diff --git a/rmf_traffic/include/rmf_traffic/reservations/Reservation.hpp b/rmf_traffic/include/rmf_traffic/reservations/Reservation.hpp
index 708364dd..3e54cde3 100644
--- a/rmf_traffic/include/rmf_traffic/reservations/Reservation.hpp
+++ b/rmf_traffic/include/rmf_traffic/reservations/Reservation.hpp
@@ -60,8 +60,9 @@ class Reservation
     std::optional<rmf_traffic::Time> finish_time);
 
   class Implementation;
-private:
+
   Reservation();
+private:
   rmf_utils::impl_ptr<Implementation> _pimpl;
 };
 } // end namespace reservations
diff --git a/rmf_traffic/src/rmf_traffic/reservations/Database.cpp b/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
index f07f5e4e..9378cdcd 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
@@ -18,6 +18,7 @@
 #include <rmf_traffic/reservations/Database.hpp>
 #include "internal/ParticipantStore.hpp"
 #include "internal/RequestQueue.hpp"
+#include "internal/State.hpp"
 namespace rmf_traffic {
 namespace reservations {
 
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index 8f5a42ea..81f5e789 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -21,6 +21,7 @@
 #include <rmf_traffic/reservations/Database.hpp>
 #include <unordered_map>
 #include <unordered_set>
+#include <iostream>
 #include <map>
 
 #include "RequestQueue.hpp"
@@ -75,70 +76,8 @@ class State
     //Do nothing
   }
 
-  NextStateGenerator begin()
-  {
-    NextStateGenerator gen;
-    gen.mode = NextStateGenerator::PotentialActionMode::ASSIGN_ITEMS;
-    gen.start_state = this;
-    if(_unassigned.size() > 0)
-    {
-      auto current_request = _queue->get_request_info(
-        _unassigned.begin()->first,
-        _unassigned.begin()->second).request_options[0];
-
-      gen.unassigned_iter = _unassigned.begin();
-      gen.insertion_point_iter = gen.get_search_start(current_request);
-      gen.insertion_point_end = gen.get_search_end(current_request);
-
-      State next_state(*this);
-      auto new_res = [=]() -> Reservation {
-        if(gen.insertion_point_end != gen.insertion_point_iter)
-        {
-          return Reservation::make_reservation(
-            gen.insertion_point_iter->second.actual_finish_time().value(),
-            resource_name,
-            current_request.duration(),
-            current_request.finish_time()
-          );
-        }
-        else
-        {
-          if(current_request.start_time().has_value())
-          {
-            return Reservation::make_reservation(
-              current_request.start_time().value(),
-              resource_name,
-              current_request.duration(),
-              current_request.finish_time()
-            );
-          }
-          else
-          {
-            //TODO some API for time syncing
-          }
-        }
-      }();
-      next_state.assign_reservation(
-        unassigned_iter->first,
-        unassigned_iter->second,
-        new_res
-      )
-      gen.next_state = next_state;
-    }
-    gen.remove_resource_iter = _resource_schedules.begin();
-    if(_resource_schedules.size() > 0)
-    {
-      gen.remove_iter = gen.remove_resource_iter->first;
-    }
-    return gen;
-  }
-
-  NextStateGenerator end()
-  {
-    NextStateGenerator gen;
-    gen.mode == NextStateGenerator::PotentialActionMode::END;
-    return gen;
-  }
+  NextStateGenerator begin();
+  NextStateGenerator end();
   /// Adds a request to the state
   State add_request(
     ParticipantId pid,
@@ -166,7 +105,7 @@ class State
     if(findings == new_state._reservation_timings.end())
     {
       // Non existant pid-reqid pair
-      return;
+      return new_state;
     }
     auto time = findings->second;
     auto resource = new_state._reservation_resources[reservation_id];
@@ -191,7 +130,7 @@ class State
       std::cout << "Resource: " << std::endl;
       for(auto &[time, reservation]: schedule)
       {
-        std::cout << "\t======"
+        std::cout << "\t======";
         std::cout << "\tReservation: " << reservation.reservation_id() << std::endl;
         std::cout << "\tStart time: " << time.time_since_epoch().count()/1e9<< std::endl;
         if(!reservation.is_indefinite())
@@ -202,7 +141,7 @@ class State
           _reservation_request_ids[reservation.reservation_id()].participant
           << std::endl;
         std::cout << "\tRequestID: " <<
-          _reservation_request_ids[reservation.resource_name()].request
+          _reservation_request_ids[reservation.reservation_id()].reqid
           << std::endl;
       }
     }
@@ -246,7 +185,7 @@ class State
   }
 
 private:
-  void push_back(
+  bool push_back(
     std::string resource,
     ResourceSchedule::const_iterator res_iter,
     Duration dur
@@ -259,7 +198,7 @@ class State
     std::map<Time, ReservationId> new_times;
     auto gap_left = dur;
     while(
-      res_iter != new_state._resource_schedules[resource->second].end()
+      res_iter != _resource_schedules[resource].end()
       && prev_iter->second.actual_finish_time().has_value()
       && gap_left.count() > 0)
     {
@@ -276,13 +215,15 @@ class State
 
     for(auto iter = new_times.rbegin(); iter != new_times.rend(); iter++)
     {
-      bool ok  = new_state.shift_reservation_start_time(
-        resource->second,
+      bool ok  = shift_reservation_start_time(
+        resource,
         iter->second,
         iter->first);
 
-      if(!ok) return std::nullopt;
+      if(!ok) return false;
     }
+
+    return true;
   }
   // Returns true if a shift could be successfully applied. Else returns false.
   void unassign_reservation(ReservationId res_id, std::string resource_name)
@@ -331,10 +272,14 @@ class State
         && res.actual_finish_time().has_value()
         && next_reservation->second.start_time() < res.actual_finish_time().value())
       {
-        push_back(
+        auto ok = push_back(
           res.resource_name(),
           next_reservation,
-          res.actual_finish_time().value());
+          res.actual_finish_time().value() - next_reservation->second.start_time());
+        if(!ok)
+        {
+          return false;
+        }
       }
 
     }
@@ -592,6 +537,72 @@ struct NextStateGenerator
   }
 };
 
+NextStateGenerator State::begin()
+{
+  NextStateGenerator gen;
+  gen.mode = NextStateGenerator::PotentialActionMode::ASSIGN_ITEMS;
+  gen.start_state = this;
+  if(_unassigned.size() > 0)
+  {
+    auto current_request = _queue->get_request_info(
+      _unassigned.begin()->first,
+      _unassigned.begin()->second).request_options[0];
+
+    gen.unassigned_iter = _unassigned.begin();
+    gen.insertion_point_iter = gen.get_search_start(current_request);
+    gen.insertion_point_end = gen.get_search_end(current_request);
+
+    State next_state(*this);
+    auto new_res = [=]() -> Reservation {
+      if(gen.insertion_point_end != gen.insertion_point_iter)
+      {
+        return Reservation::make_reservation(
+          gen.insertion_point_iter->second.actual_finish_time().value(),
+          current_request.resource_name(),
+          current_request.duration(),
+          current_request.finish_time()
+        );
+      }
+      else
+      {
+        if(current_request.start_time().has_value()
+        && current_request.start_time()->lower_bound().has_value()
+        )
+        {
+          return Reservation::make_reservation(
+            current_request.start_time()->lower_bound().value(),
+            current_request.resource_name(),
+            current_request.duration(),
+            current_request.finish_time()
+          );
+        }
+        else
+        {
+          //TODO some API for time syncing
+        }
+      }
+    }();
+    next_state.assign_reservation(
+      gen.unassigned_iter->first,
+      gen.unassigned_iter->second,
+      new_res
+    );
+    gen.visiting_state = next_state;
+  }
+  gen.remove_resource_iter = _resource_schedules.begin();
+  if(_resource_schedules.size() > 0)
+  {
+    gen.remove_iter = gen.remove_resource_iter->second.begin();
+  }
+  return gen;
+}
+
+NextStateGenerator State::end()
+{
+  NextStateGenerator gen;
+  gen.mode = NextStateGenerator::PotentialActionMode::END;
+  return gen;
+}
 }
 }
 #endif
\ No newline at end of file

From ca1aa27667cd96f9debf89077e4730a9a6c79c49 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Fri, 4 Jun 2021 15:58:32 +0800
Subject: [PATCH 16/66] Finish Iterator begin and end

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index 81f5e789..a2bdc932 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -594,6 +594,10 @@ NextStateGenerator State::begin()
   {
     gen.remove_iter = gen.remove_resource_iter->second.begin();
   }
+  else if(_unassigned.size() == 0)
+  {
+    return end();
+  }
   return gen;
 }
 

From 3f2d09207753762cb9b8d68a150436c0ccf4862c Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Mon, 7 Jun 2021 08:43:16 +0800
Subject: [PATCH 17/66] make neater print

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../src/rmf_traffic/reservations/internal/State.hpp      | 9 +++++++--
 1 file changed, 7 insertions(+), 2 deletions(-)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index a2bdc932..5862bb07 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -132,10 +132,15 @@ class State
       {
         std::cout << "\t======";
         std::cout << "\tReservation: " << reservation.reservation_id() << std::endl;
-        std::cout << "\tStart time: " << time.time_since_epoch().count()/1e9<< std::endl;
+        std::cout << "\tStart time: " << 
+          std::chrono::duration_cast<std::chrono::seconds>(
+            time.time_since_epoch()
+          ).count() << std::endl;
         if(!reservation.is_indefinite())
           std::cout << "\tEnd time: " <<
-            reservation.actual_finish_time()->time_since_epoch().count()/1e9
+            std::chrono::duration_cast<std::chrono::seconds>(
+              reservation.actual_finish_time()->time_since_epoch()
+            ).count()
             << std::endl;
         std::cout << "\tParticipant: " <<
           _reservation_request_ids[reservation.reservation_id()].participant

From af71eda7adaa5705f0af0179155d529093491a89 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Mon, 7 Jun 2021 11:46:42 +0800
Subject: [PATCH 18/66] Add tests

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../unit/reservations/internal/test_State.cpp | 34 +++++++++++++++++++
 1 file changed, 34 insertions(+)

diff --git a/rmf_traffic/test/unit/reservations/internal/test_State.cpp b/rmf_traffic/test/unit/reservations/internal/test_State.cpp
index 16c339a0..cb68d1fc 100644
--- a/rmf_traffic/test/unit/reservations/internal/test_State.cpp
+++ b/rmf_traffic/test/unit/reservations/internal/test_State.cpp
@@ -19,3 +19,37 @@
 
 #include <rmf_utils/catch.hpp>
 
+using namespace rmf_traffic;
+using namespace rmf_traffic::reservations;
+using namespace std::chrono_literals;
+
+SCENARIO("A few reservations in a state")
+{
+    auto queue = std::make_shared<RequestQueue>();
+    State start_state(queue);
+
+    auto now = std::chrono::steady_clock::now();
+    now -= now.time_since_epoch();
+
+    auto request1_alt1 = ReservationRequest::make_request(
+        "table_at_timbre",
+        ReservationRequest::TimeRange::make_time_range(
+            now,
+            now+10s
+        ),
+        {10s}
+    );
+
+    auto request1 = std::vector{request1_alt1};
+    queue->enqueue_reservation(0, 0, 1, request1);
+    start_state = start_state.add_request(0, 0);
+
+    std::cout << "Start State:" <<std::endl;
+    start_state.debug_state();
+
+    for(auto next_state: start_state)
+    {
+        std::cout << "Next State:" <<std::endl;
+        next_state.debug_state();
+    }
+}
\ No newline at end of file

From 05f5ac1ba6ca753a490242f4c56bd81a05789703 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Mon, 7 Jun 2021 18:25:22 +0800
Subject: [PATCH 19/66] manage to fix test case

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../reservations/internal/State.hpp           | 236 ++++++++++--------
 .../unit/reservations/internal/test_State.cpp |  11 +-
 2 files changed, 148 insertions(+), 99 deletions(-)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index 5862bb07..c5df6c57 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -127,12 +127,13 @@ class State
     for(auto &[resource, schedule]: _resource_schedules)
     {
       std::cout << "________________________" <<std::endl;
-      std::cout << "Resource: " << std::endl;
+      std::cout << "Resource: " << resource << std::endl;
       for(auto &[time, reservation]: schedule)
       {
-        std::cout << "\t======";
-        std::cout << "\tReservation: " << reservation.reservation_id() << std::endl;
-        std::cout << "\tStart time: " << 
+        std::cout << "\t======" << std::endl;
+        std::cout << "\tReservation: "
+          << reservation.reservation_id() << std::endl;
+        std::cout << "\tStart time: " <<
           std::chrono::duration_cast<std::chrono::seconds>(
             time.time_since_epoch()
           ).count() << std::endl;
@@ -177,6 +178,11 @@ class State
     return seed;
   }
 
+  void set_current_time(Time time)
+  {
+    _current_time = time;
+  }
+
   State(const State& other) :
     _resource_schedules(other._resource_schedules),
     _unassigned(other._unassigned),
@@ -184,7 +190,8 @@ class State
     _queue(other._queue),
     _reservation_resources(other._reservation_resources),
     _reservation_assignments(other._reservation_assignments),
-    _reservation_request_ids(other._reservation_request_ids)
+    _reservation_request_ids(other._reservation_request_ids),
+    _current_time(other._current_time)
   {
     // Do nothing
   }
@@ -338,6 +345,8 @@ class State
   ReservationRequestId _reservation_request_ids; // reservation => {part_id, req_id, index}
 
   std::shared_ptr<RequestQueue> _queue;
+
+  Time _current_time;
   friend NextStateGenerator;
 };
 struct NextStateGenerator
@@ -345,13 +354,14 @@ struct NextStateGenerator
   //TODO: RAW_POINTER-FOO
   State* start_state;
   std::optional<State> visiting_state;
+  
+  //Potential Actions
   enum PotentialActionMode
   {
     ASSIGN_ITEMS,
     REMOVE_ITEMS,
     END
   };
-
   PotentialActionMode mode = ASSIGN_ITEMS;
 
   // For iterating through assignments
@@ -359,6 +369,7 @@ struct NextStateGenerator
   State::ResourceSchedule::const_iterator insertion_point_iter;
   State::ResourceSchedule::const_iterator insertion_point_end;
   std::size_t request_index_iter = 0;
+  bool proceed_next_resource = false;
 
   // For iterating through removals
   State::ResourceSchedules::const_iterator remove_resource_iter;
@@ -409,78 +420,133 @@ struct NextStateGenerator
     }
   }
 
-  std::optional<State> advance_assignments()
+  std::optional<State> advance_within_resource()
   {
-    while(true)
-    {
-      if(unassigned_iter == start_state->_unassigned.end())
-      {
-        return std::nullopt;
-      }
-      auto request_info = start_state->_queue->get_request_info(
-        unassigned_iter->first,
-        unassigned_iter->second);
-
-      while(request_index_iter >= request_info.request_options.size())
-      {
-        request_index_iter = 0;
-        unassigned_iter++;
-        request_info = start_state->_queue->get_request_info(
-          unassigned_iter->first,
-          unassigned_iter->second);
+    if(proceed_next_resource)
+      return std::nullopt;
 
-        if(request_info.request_options.size() > 0)
-        {
-          insertion_point_iter =
-            get_search_start(request_info.request_options[request_index]);
-          insertion_point_end =
-            get_search_end(request_info.request_options[request_index]);
-        }
-      }
+    State next_state(*start_state);
 
-      while(insertion_point_iter == insertion_point_end)
-      {
-        request_index_iter++;
-        request_info = start_state->_queue->get_request_info(
-          unassigned_iter->first,
-          unassigned_iter->second);
+    auto [part_id, req_id] = *unassigned_iter;
 
-        if(request_info.request_options.size() > 0)
-        {
-          insertion_point_iter =
-            get_search_start(request_info.request_options[request_index]);
-          insertion_point_end =
-            get_search_end(request_info.request_options[request_index]);
-        }
-      }
+    auto current_request = start_state->_queue->get_request_info(
+      part_id,
+      req_id).request_options[request_index_iter];
 
-      auto resource_name =
-        request_info.request_options[request_index_iter].resource_name();
-      auto current_request = request_info.request_options[request_index];
+    bool found_slot = false;
 
-      State new_state(*start_state);
+    proceed_next_resource = false;
 
+    while(insertion_point_iter != start_state->_resource_schedules[current_request.resource_name()].end())
+    {
       if(insertion_point_iter->second.actual_finish_time().has_value())
       {
-        Reservation new_res = Reservation::make_reservation(
+        auto reservation = Reservation::make_reservation(
           insertion_point_iter->second.actual_finish_time().value(),
-          resource_name,
+          current_request.resource_name(),
           current_request.duration(),
           current_request.finish_time()
         );
-        bool ok = new_state.assign_reservation(
-          unassigned_iter->first,
-          unassigned_iter->second,
-          new_res
+
+        bool found_slot = next_state.assign_reservation(
+          part_id,
+          req_id,
+          reservation
         );
-        if(ok)
+        insertion_point_iter++;
+        if(found_slot)
         {
-          insertion_point_iter++;
-          return new_state;
+          return next_state;
         }
       }
-      insertion_point_iter++;
+      else
+      {
+        return std::nullopt;
+      }
     }
+
+    proceed_next_resource = true;
+
+    if(
+      current_request.start_time().has_value() &&
+      current_request.start_time()->lower_bound().has_value()
+    )
+    {
+      auto reservation = Reservation::make_reservation(
+        current_request.start_time().value().lower_bound().value(),
+        current_request.resource_name(),
+        current_request.duration(),
+        current_request.finish_time()
+      );
+      bool found_slot = next_state.assign_reservation(
+        part_id,
+        req_id,
+        reservation
+      );
+      if(found_slot)
+      {
+        return next_state;
+      }
+      else
+      {
+        return std::nullopt;
+      }
+    }
+    else
+    {
+      auto reservation = Reservation::make_reservation(
+        start_state->_current_time,
+        current_request.resource_name(),
+        current_request.duration(),
+        current_request.finish_time()
+      );
+
+      bool found_slot = next_state.assign_reservation(
+        part_id,
+        req_id,
+        reservation
+      );
+      if(found_slot)
+      {
+        return next_state;
+      }
+      else
+      {
+        return std::nullopt;
+      }
+    }
+  }
+
+  std::optional<State> advance_within_query()
+  {
+    auto state = advance_within_resource();
+    if(state.has_value())
+    {
+      return state;
+    }
+
+    request_index_iter++;
+    auto [part_id, req_id] = *unassigned_iter;
+    auto num_alternative = start_state->_queue->get_request_info(
+      part_id,
+      req_id).request_options.size();
+
+    while(request_index_iter < num_alternative)
+    {
+      auto state = advance_within_resource();
+      if(state.has_value())
+      {
+        return state;
+      }
+      request_index_iter++;
+    }
+
+    return std::nullopt;
+  }
+
+  std::optional<State> advance_assignments()
+  {
+    return advance_within_query();
   }
 
   std::optional<State> advance_removals()
@@ -504,13 +570,21 @@ struct NextStateGenerator
   }
 
   std::optional<State> next_state() {
+
     auto assignment = advance_assignments();
     if(assignment.has_value())
+    {
+      visiting_state = assignment;
       return assignment;
+    }
     mode = PotentialActionMode::REMOVE_ITEMS;
-    auto removals = advance_removals();
+    std::cout << "finished insertions" << std::endl;
+    /*auto removals = advance_removals();
+
     if(removals.has_value())
+    {
       return removals;
+    }*/
     mode = PotentialActionMode::END;
     return std::nullopt;
   }
@@ -547,6 +621,7 @@ NextStateGenerator State::begin()
   NextStateGenerator gen;
   gen.mode = NextStateGenerator::PotentialActionMode::ASSIGN_ITEMS;
   gen.start_state = this;
+
   if(_unassigned.size() > 0)
   {
     auto current_request = _queue->get_request_info(
@@ -556,45 +631,9 @@ NextStateGenerator State::begin()
     gen.unassigned_iter = _unassigned.begin();
     gen.insertion_point_iter = gen.get_search_start(current_request);
     gen.insertion_point_end = gen.get_search_end(current_request);
-
-    State next_state(*this);
-    auto new_res = [=]() -> Reservation {
-      if(gen.insertion_point_end != gen.insertion_point_iter)
-      {
-        return Reservation::make_reservation(
-          gen.insertion_point_iter->second.actual_finish_time().value(),
-          current_request.resource_name(),
-          current_request.duration(),
-          current_request.finish_time()
-        );
-      }
-      else
-      {
-        if(current_request.start_time().has_value()
-        && current_request.start_time()->lower_bound().has_value()
-        )
-        {
-          return Reservation::make_reservation(
-            current_request.start_time()->lower_bound().value(),
-            current_request.resource_name(),
-            current_request.duration(),
-            current_request.finish_time()
-          );
-        }
-        else
-        {
-          //TODO some API for time syncing
-        }
-      }
-    }();
-    next_state.assign_reservation(
-      gen.unassigned_iter->first,
-      gen.unassigned_iter->second,
-      new_res
-    );
-    gen.visiting_state = next_state;
   }
   gen.remove_resource_iter = _resource_schedules.begin();
+
   if(_resource_schedules.size() > 0)
   {
     gen.remove_iter = gen.remove_resource_iter->second.begin();
@@ -603,6 +642,7 @@ NextStateGenerator State::begin()
   {
     return end();
   }
+  gen.next_state();
   return gen;
 }
 
diff --git a/rmf_traffic/test/unit/reservations/internal/test_State.cpp b/rmf_traffic/test/unit/reservations/internal/test_State.cpp
index cb68d1fc..cae0b165 100644
--- a/rmf_traffic/test/unit/reservations/internal/test_State.cpp
+++ b/rmf_traffic/test/unit/reservations/internal/test_State.cpp
@@ -40,7 +40,16 @@ SCENARIO("A few reservations in a state")
         {10s}
     );
 
-    auto request1 = std::vector{request1_alt1};
+    auto request1_alt2 = ReservationRequest::make_request(
+        "table_at_koufu",
+        ReservationRequest::TimeRange::make_time_range(
+            now,
+            now+10s
+        ),
+        {10s}
+    );
+
+    auto request1 = std::vector{request1_alt1, request1_alt2};
     queue->enqueue_reservation(0, 0, 1, request1);
     start_state = start_state.add_request(0, 0);
 

From b93c32fb2ade3920c2fcdc350ba28746806777c8 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Tue, 8 Jun 2021 09:28:02 +0800
Subject: [PATCH 20/66] more segfault fixing

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../src/rmf_traffic/reservations/internal/State.hpp       | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index c5df6c57..39b89610 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -531,8 +531,16 @@ struct NextStateGenerator
       part_id,
       req_id).request_options.size();
 
+    
+
     while(request_index_iter < num_alternative)
     {
+      // Reset resource iterators
+      auto curr_request = start_state->_queue->get_request_info(part_id, req_id)
+        .request_options[request_index_iter];
+      insertion_point_iter = get_search_start(curr_request);
+      insertion_point_end = get_search_end(curr_request);
+      proceed_next_resource = false;
       auto state = advance_within_resource();
       if(state.has_value())
       {

From 61411689f2d38e18ceaef72a65774d23a0a7feb0 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Tue, 8 Jun 2021 15:04:30 +0800
Subject: [PATCH 21/66] work in removals :tada:

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../reservations/internal/State.hpp           | 62 ++++++++++++-------
 .../unit/reservations/internal/test_State.cpp | 29 ++++++++-
 2 files changed, 67 insertions(+), 24 deletions(-)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index 39b89610..683572f8 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -117,16 +117,11 @@ class State
     return new_state;
   }
 
-  void purge_up_to_time(Time time)
-  {
-
-  }
-
   void debug_state()
   {
+    std::cout << "________________________" <<std::endl;
     for(auto &[resource, schedule]: _resource_schedules)
     {
-      std::cout << "________________________" <<std::endl;
       std::cout << "Resource: " << resource << std::endl;
       for(auto &[time, reservation]: schedule)
       {
@@ -203,9 +198,10 @@ class State
     Duration dur
   )
   {
+    assert(dur.count() > 0);
+
     auto prev_iter = res_iter;
     std::next(res_iter);
-
     // TODO: This is extremely inefficient. Do in-place.
     std::map<Time, ReservationId> new_times;
     auto gap_left = dur;
@@ -231,7 +227,6 @@ class State
         resource,
         iter->second,
         iter->first);
-
       if(!ok) return false;
     }
 
@@ -280,6 +275,7 @@ class State
       auto next_reservation = _resource_schedules[res.resource_name()].lower_bound(
         res.start_time()
       );
+
       if(next_reservation != _resource_schedules[res.resource_name()].end()
         && res.actual_finish_time().has_value()
         && next_reservation->second.start_time() < res.actual_finish_time().value())
@@ -287,7 +283,7 @@ class State
         auto ok = push_back(
           res.resource_name(),
           next_reservation,
-          res.actual_finish_time().value() - next_reservation->second.start_time());
+          next_reservation->second.start_time() - res.actual_finish_time().value());
         if(!ok)
         {
           return false;
@@ -531,8 +527,6 @@ struct NextStateGenerator
       part_id,
       req_id).request_options.size();
 
-    
-
     while(request_index_iter < num_alternative)
     {
       // Reset resource iterators
@@ -554,7 +548,28 @@ struct NextStateGenerator
 
   std::optional<State> advance_assignments()
   {
-    return advance_within_query();
+    auto state = advance_within_query();
+    if(state.has_value())
+    {
+      return state;
+    }
+    unassigned_iter++;
+    while(unassigned_iter != start_state->_unassigned.end())
+    {
+      auto [part_id, req_id] = *unassigned_iter;
+      request_index_iter = 0;
+      auto curr_request = start_state->_queue->get_request_info(part_id, req_id)
+        .request_options[request_index_iter];
+      insertion_point_iter = get_search_start(curr_request);
+      insertion_point_end = get_search_end(curr_request);
+      proceed_next_resource = false;
+      auto state = advance_within_query();
+      if(state.has_value())
+      {
+        return state;
+      }
+    }
+    return std::nullopt;
   }
 
   std::optional<State> advance_removals()
@@ -566,6 +581,10 @@ struct NextStateGenerator
     while(remove_iter == remove_resource_iter->second.end())
     {
       remove_resource_iter++;
+      if(remove_resource_iter == start_state->_resource_schedules.end())
+      {
+        return std::nullopt;
+      }
       remove_iter = remove_resource_iter->second.begin();
     }
 
@@ -578,21 +597,22 @@ struct NextStateGenerator
   }
 
   std::optional<State> next_state() {
-
-    auto assignment = advance_assignments();
-    if(assignment.has_value())
+    if(mode == PotentialActionMode::ASSIGN_ITEMS)
     {
-      visiting_state = assignment;
-      return assignment;
+      auto assignment = advance_assignments();
+      if(assignment.has_value())
+      {
+        visiting_state = assignment;
+        return assignment;
+      }
+      mode = PotentialActionMode::REMOVE_ITEMS;
     }
-    mode = PotentialActionMode::REMOVE_ITEMS;
-    std::cout << "finished insertions" << std::endl;
-    /*auto removals = advance_removals();
+    auto removals = advance_removals();
 
     if(removals.has_value())
     {
       return removals;
-    }*/
+    }
     mode = PotentialActionMode::END;
     return std::nullopt;
   }
diff --git a/rmf_traffic/test/unit/reservations/internal/test_State.cpp b/rmf_traffic/test/unit/reservations/internal/test_State.cpp
index cae0b165..eef8bc16 100644
--- a/rmf_traffic/test/unit/reservations/internal/test_State.cpp
+++ b/rmf_traffic/test/unit/reservations/internal/test_State.cpp
@@ -53,12 +53,35 @@ SCENARIO("A few reservations in a state")
     queue->enqueue_reservation(0, 0, 1, request1);
     start_state = start_state.add_request(0, 0);
 
-    std::cout << "Start State:" <<std::endl;
-    start_state.debug_state();
+    auto request2_alt1 = ReservationRequest::make_request(
+        "table_at_timbre",
+        ReservationRequest::TimeRange::make_time_range(
+            now,
+            now+10s
+        ),
+        {10s}
+    );
+    auto request2 = std::vector{request2_alt1};
+    queue->enqueue_reservation(1, 0, 1, request2);
+    start_state = start_state.add_request(1, 0);
+
+    std::vector<State> child_states;
 
     for(auto next_state: start_state)
     {
-        std::cout << "Next State:" <<std::endl;
+        //std::cout << "Next State:" <<std::endl;
+        //next_state.debug_state();
+        child_states.push_back(next_state);
+    }
+
+    REQUIRE(child_states.size() == 3);
+
+    std::cout << "Start State:" <<std::endl;
+    child_states[0].debug_state();
+
+    std::cout << "Child states: " <<std::endl;
+    for(auto next_state: child_states[0])
+    {
         next_state.debug_state();
     }
 }
\ No newline at end of file

From f420ee832a9005907522f4d0ba295dde0b2505f5 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Tue, 8 Jun 2021 18:23:35 +0800
Subject: [PATCH 22/66] Add primitives for planner

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../reservations/internal/Heuristic.hpp       | 56 +++++++++++++++++++
 .../reservations/internal/Optimizer.hpp       | 42 +++++++++++++-
 .../reservations/internal/State.hpp           | 17 +++---
 3 files changed, 106 insertions(+), 9 deletions(-)
 create mode 100644 rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp

diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp
new file mode 100644
index 00000000..440c61c5
--- /dev/null
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp
@@ -0,0 +1,56 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * 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.
+ *
+*/
+
+#ifndef RMF_TRAFFIC__RESERVATIONS__INTERNAL_HEURISTIC_HPP
+#define RMF_TRAFFIC__RESERVATIONS__INTERNAL_HEURISTIC_HPP
+
+#include "State.hpp"
+
+namespace rmf_traffic {
+namespace reservations {
+
+class Heuristic
+{
+public:
+  virtual float score(State& state);
+};
+
+class PriorityBasedScorer: Heuristic
+{
+public:
+  PriorityBasedScorer(std::shared_ptr<RequestQueue> queue) :
+    _queue(queue)
+  {
+
+  }
+  float score(State& state) override
+  {
+    auto score = 0;
+    for(auto [part_id, req_id]: state.unassigned())
+    {
+      score += _queue->get_request_info(part_id, req_id).priority;
+    }
+    return score;
+  }
+private:
+  std::shared_ptr<RequestQueue> _queue;
+};
+
+}
+}
+
+#endif
\ No newline at end of file
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/Optimizer.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/Optimizer.hpp
index b51306c5..d382b959 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/Optimizer.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/Optimizer.hpp
@@ -15,7 +15,45 @@
  *
 */
 
-#ifndef RMF_TRAFFIC__OPTIMIZER__INTERNAL_REQUESTQUEUE_HPP
-#define RMF_TRAFFIC__OPTIMIZER__INTERNAL_REQUESTQUEUE_HPP
+#ifndef RMF_TRAFFIC__RESERVATIONS__INTERNAL_OPTIMIZER_HPP
+#define RMF_TRAFFIC__RESERVATIONS__INTERNAL_OPTIMIZER_HPP
+
+#include "Heuristic.hpp"
+
+namespace rmf_traffic {
+namespace reservations {
+
+struct StateHash {
+   size_t operator() (const State &state) const {
+     return state.hash();
+   }
+};
+class Optimizer
+{
+public:
+  class Solution
+  {
+  public:
+    std::optional<State> next_solution()
+    {
+      
+    }
+  private:
+    std::unordered_set<State> visited;
+    State last_solution;
+  };
+  Optimizer(std::unique_ptr<Heuristic> heuristic):
+    _heuristic(std::move(heuristic))
+  {
+  }
+
+  Solution optimize(State& state)
+  {
+    
+  }
+private:
+  std::unique_ptr<Heuristic> _heuristic;
+};
+}
 
 #endif
\ No newline at end of file
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index 683572f8..84221be5 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -154,7 +154,7 @@ class State
       && _reservation_assignments == other._reservation_assignments;
   }
 
-  std::size_t hash()
+  std::size_t hash() const
   {
     std::size_t seed = 0x928193843;
     for(auto &[resource, schedule]: _resource_schedules)
@@ -162,12 +162,10 @@ class State
       for(auto &[time, reservation]: schedule)
       {
         hash_combine(seed, time.time_since_epoch().count());
-        hash_combine(
-          seed,
-          _reservation_request_ids[reservation.reservation_id()].participant);
-        hash_combine(
-          seed,
-          _reservation_request_ids[reservation.reservation_id()].reqid);
+        auto val =
+          _reservation_request_ids.find(reservation.reservation_id())->second;
+        hash_combine(seed, val.participant);
+        hash_combine(seed, val.reqid);
       }
     }
     return seed;
@@ -178,6 +176,11 @@ class State
     _current_time = time;
   }
 
+  UnassignedSet unassigned() const
+  {
+    return _unassigned;
+  }
+
   State(const State& other) :
     _resource_schedules(other._resource_schedules),
     _unassigned(other._unassigned),

From 47dc095f084c1af4f99b8161060ffecbe5df2bc7 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Wed, 9 Jun 2021 11:48:47 +0800
Subject: [PATCH 23/66] Added initial planner implementation

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../rmf_traffic/reservations/Reservation.cpp  |  5 +-
 .../reservations/internal/Heuristic.hpp       |  5 +-
 .../reservations/internal/Optimizer.hpp       | 77 +++++++++++++++++--
 .../reservations/internal/State.hpp           | 12 ++-
 4 files changed, 86 insertions(+), 13 deletions(-)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp b/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp
index e2343b42..fe4e2d04 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp
@@ -117,8 +117,9 @@ ReservationId Reservation::reservation_id() const
 //=============================================================================
 bool Reservation::operator==(const Reservation& other) const
 {
-  return other._pimpl->_res_id == _pimpl->_res_id
-    && other._pimpl->_start_time == _pimpl->_start_time
+  // We purposely ignore the reservation id here... Lets see if we regret that
+  // later.
+  return other._pimpl->_start_time == _pimpl->_start_time
     && other._pimpl->_resource_name == _pimpl->_resource_name
     && other._pimpl->_duration == _pimpl->_duration
     && other._pimpl->_finish_time == _pimpl->_finish_time;
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp
index 440c61c5..ba21cc23 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp
@@ -26,7 +26,7 @@ namespace reservations {
 class Heuristic
 {
 public:
-  virtual float score(State& state);
+  virtual float score(const State& state);
 };
 
 class PriorityBasedScorer: Heuristic
@@ -37,11 +37,12 @@ class PriorityBasedScorer: Heuristic
   {
 
   }
-  float score(State& state) override
+  float score(const State& state) override
   {
     auto score = 0;
     for(auto [part_id, req_id]: state.unassigned())
     {
+      // TODO: Incorporate 
       score += _queue->get_request_info(part_id, req_id).priority;
     }
     return score;
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/Optimizer.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/Optimizer.hpp
index d382b959..59cfaec2 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/Optimizer.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/Optimizer.hpp
@@ -19,6 +19,7 @@
 #define RMF_TRAFFIC__RESERVATIONS__INTERNAL_OPTIMIZER_HPP
 
 #include "Heuristic.hpp"
+#include "priority_queue.hpp"
 
 namespace rmf_traffic {
 namespace reservations {
@@ -28,22 +29,82 @@ struct StateHash {
      return state.hash();
    }
 };
-class Optimizer
+class GreedyBestFirstSearchOptimizer
 {
 public:
+  class CustomComparator
+  {
+    std::shared_ptr<Heuristic> _heuristic;
+  public:
+    CustomComparator(std::shared_ptr<Heuristic> heuristic):
+      _heuristic(heuristic)
+    {
+
+    }
+    bool operator() (const State& e1, const State& e2) const
+    {
+      return _heuristic->score(e1) < _heuristic->score(e2);
+    }
+  };
   class Solution
   {
   public:
+    Solution(State& state,
+      std::shared_ptr<Heuristic> heuristic):
+      _last_solution(state),
+      _pq(heuristic),
+      _all_solutions(heuristic),
+      _heuristic(heuristic)
+    {
+      _blacklist.insert(state);
+      _visited.insert(state);
+      _pq.push(state);
+    }
     std::optional<State> next_solution()
     {
-      
+      _blacklist.insert(_last_solution);
+      while(!_pq.empty())
+      {
+        auto state = _pq.top();
+        _visited.insert(state);
+        _pq.pop();
+        for(auto next_state: state)
+        {
+          if(_visited.count(next_state) != 0)
+            continue;
+
+          if(_heuristic->score(next_state) == 0 &&
+            _blacklist.count(next_state) == 0)
+          {
+            _last_solution = next_state;
+            return next_state;
+          }
+
+          _pq.push(next_state);
+          _all_solutions.push(next_state);
+        }
+      }
+      while(!_all_solutions.empty())
+      {
+        auto res = _all_solutions.top();
+        _last_solution = res;
+        _all_solutions.pop();
+        if(_blacklist.count(res) != 0)
+          return res;
+      }
+      return std::nullopt;
     }
   private:
-    std::unordered_set<State> visited;
-    State last_solution;
+    std::unordered_set<State, StateHash> _visited;
+    std::unordered_set<State, StateHash> _blacklist;
+    std::priority_queue<State, std::vector<State>, CustomComparator> _pq;
+    std::priority_queue<State, std::vector<State>, CustomComparator>
+      _all_solutions;
+    std::shared_ptr<Heuristic> _heuristic;
+    State _last_solution;
   };
-  Optimizer(std::unique_ptr<Heuristic> heuristic):
-    _heuristic(std::move(heuristic))
+  GreedyBestFirstSearchOptimizer(std::shared_ptr<Heuristic> heuristic):
+    _heuristic(heuristic)
   {
   }
 
@@ -52,8 +113,8 @@ class Optimizer
     
   }
 private:
-  std::unique_ptr<Heuristic> _heuristic;
+  std::shared_ptr<Heuristic> _heuristic;
 };
 }
-
+}
 #endif
\ No newline at end of file
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index 84221be5..c0b9c7da 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -23,6 +23,7 @@
 #include <unordered_set>
 #include <iostream>
 #include <map>
+#include <set>
 
 #include "RequestQueue.hpp"
 
@@ -151,14 +152,23 @@ class State
   bool operator==(State& other) const
   {
     return _resource_schedules == other._resource_schedules
-      && _reservation_assignments == other._reservation_assignments;
+      && _unassigned == other._unassigned;
   }
 
   std::size_t hash() const
   {
     std::size_t seed = 0x928193843;
+    std::set<std::string> resources;
+
+    // Make sure the order of iteration is correct
     for(auto &[resource, schedule]: _resource_schedules)
     {
+      resources.insert(resource);
+    }
+
+    for(auto resource: resources)
+    {
+      auto schedule = _resource_schedules.find(resource)->second;
       for(auto &[time, reservation]: schedule)
       {
         hash_combine(seed, time.time_since_epoch().count());

From d0a3185625814880ff2c0eb3b31cca32c76517db Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Wed, 9 Jun 2021 17:09:55 +0800
Subject: [PATCH 24/66] Add planner

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../src/rmf_traffic/reservations/internal/Heuristic.hpp     | 2 +-
 .../src/rmf_traffic/reservations/internal/Optimizer.hpp     | 2 +-
 .../src/rmf_traffic/reservations/internal/RequestQueue.hpp  | 4 ++--
 rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp | 2 +-
 rmf_traffic/test/unit/reservations/internal/test_State.cpp  | 6 ++++++
 5 files changed, 11 insertions(+), 5 deletions(-)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp
index ba21cc23..5f995d3c 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp
@@ -29,7 +29,7 @@ class Heuristic
   virtual float score(const State& state);
 };
 
-class PriorityBasedScorer: Heuristic
+class PriorityBasedScorer: public Heuristic
 {
 public:
   PriorityBasedScorer(std::shared_ptr<RequestQueue> queue) :
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/Optimizer.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/Optimizer.hpp
index 59cfaec2..b86690f9 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/Optimizer.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/Optimizer.hpp
@@ -19,7 +19,7 @@
 #define RMF_TRAFFIC__RESERVATIONS__INTERNAL_OPTIMIZER_HPP
 
 #include "Heuristic.hpp"
-#include "priority_queue.hpp"
+#include <queue>
 
 namespace rmf_traffic {
 namespace reservations {
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp
index 8f9cce5a..d47eadd3 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp
@@ -111,8 +111,8 @@ class RequestQueue
       }
     }
 
-    if(req.is_indefinite() && !reservation.is_indefinite()
-      || !req.is_indefinite() && reservation.is_indefinite())
+    if((req.is_indefinite() && !reservation.is_indefinite())
+      || (!req.is_indefinite() && reservation.is_indefinite()))
     {
       return false;
     }
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index c0b9c7da..e581b4d6 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -149,7 +149,7 @@ class State
     }
   }
 
-  bool operator==(State& other) const
+  bool operator==(const State& other) const
   {
     return _resource_schedules == other._resource_schedules
       && _unassigned == other._unassigned;
diff --git a/rmf_traffic/test/unit/reservations/internal/test_State.cpp b/rmf_traffic/test/unit/reservations/internal/test_State.cpp
index eef8bc16..7b09aac2 100644
--- a/rmf_traffic/test/unit/reservations/internal/test_State.cpp
+++ b/rmf_traffic/test/unit/reservations/internal/test_State.cpp
@@ -16,6 +16,8 @@
 */
 
 #include "../../../../src/rmf_traffic/reservations/internal/State.hpp"
+#include "../../../../src/rmf_traffic/reservations/internal/Heuristic.hpp"
+#include "../../../../src/rmf_traffic/reservations/internal/Optimizer.hpp"
 
 #include <rmf_utils/catch.hpp>
 
@@ -84,4 +86,8 @@ SCENARIO("A few reservations in a state")
     {
         next_state.debug_state();
     }
+
+    /*auto heuristic = std::make_shared<PriorityBasedScorer>();
+    GreedyBestFirstSearchOptimizer opt(start_state, heuristic);*/
+
 }
\ No newline at end of file

From 23ac2e02742c5772b6b12ebd38e2d21daa760b74 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Tue, 15 Jun 2021 15:57:18 +0800
Subject: [PATCH 25/66] Add state difference

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../rmf_traffic/reservations/Reservation.hpp  |   3 +
 .../src/rmf_traffic/reservations/Database.cpp |   8 +-
 .../rmf_traffic/reservations/Reservation.cpp  |   8 +-
 .../reservations/internal/Heuristic.hpp       |   8 +-
 .../reservations/internal/Optimizer.hpp       |   3 +-
 .../reservations/internal/RequestQueue.hpp    | 157 +++++++-----------
 .../reservations/internal/RequestStore.hpp    | 155 +++++++++++++++++
 .../reservations/internal/State.hpp           |  28 +++-
 .../reservations/internal/StateDiff.hpp       | 113 +++++++++++++
 .../unit/reservations/internal/test_State.cpp |  12 +-
 10 files changed, 379 insertions(+), 116 deletions(-)
 create mode 100644 rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp
 create mode 100644 rmf_traffic/src/rmf_traffic/reservations/internal/StateDiff.hpp

diff --git a/rmf_traffic/include/rmf_traffic/reservations/Reservation.hpp b/rmf_traffic/include/rmf_traffic/reservations/Reservation.hpp
index 3e54cde3..a5b61dbf 100644
--- a/rmf_traffic/include/rmf_traffic/reservations/Reservation.hpp
+++ b/rmf_traffic/include/rmf_traffic/reservations/Reservation.hpp
@@ -53,6 +53,8 @@ class Reservation
 
   bool operator==(const Reservation& other) const;
 
+  bool operator!=(const Reservation& other) const;
+
   static Reservation make_reservation(
     rmf_traffic::Time start_time,
     std::string resource_name,
@@ -62,6 +64,7 @@ class Reservation
   class Implementation;
 
   Reservation();
+
 private:
   rmf_utils::impl_ptr<Implementation> _pimpl;
 };
diff --git a/rmf_traffic/src/rmf_traffic/reservations/Database.cpp b/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
index 9378cdcd..59b16173 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
@@ -17,7 +17,7 @@
 
 #include <rmf_traffic/reservations/Database.hpp>
 #include "internal/ParticipantStore.hpp"
-#include "internal/RequestQueue.hpp"
+#include "internal/RequestStore.hpp"
 #include "internal/State.hpp"
 namespace rmf_traffic {
 namespace reservations {
@@ -26,7 +26,7 @@ class Database::Implementation
 {
 public:
   Implementation():
-    _request_queue(std::make_shared<RequestQueue>())
+    _request_queue(std::make_shared<RequestStore>())
   {
 
   }
@@ -43,7 +43,6 @@ class Database::Implementation
   )
   {
     _participant_store.remove_participant(id);
-    _request_queue->erase_participant_requests(id);
   }
 
   void request_reservation(
@@ -57,10 +56,11 @@ class Database::Implementation
 
   void cancel_request(ParticipantId id, RequestId req)
   {
+    
   }
 private:
   ParticipantStore _participant_store;
-  std::shared_ptr<RequestQueue> _request_queue;
+  std::shared_ptr<RequestStore> _request_queue;
 };
 
 void Database::register_participant(
diff --git a/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp b/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp
index fe4e2d04..797c48da 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp
@@ -125,6 +125,12 @@ bool Reservation::operator==(const Reservation& other) const
     && other._pimpl->_finish_time == _pimpl->_finish_time;
 }
 
+//=============================================================================
+bool Reservation::operator!=(const Reservation& other) const
+{
+  return !(*this == other);
+}
+
 //=============================================================================
 Reservation Reservation::make_reservation(
   rmf_traffic::Time start_time,
@@ -149,7 +155,7 @@ Reservation Reservation::make_reservation(
 Reservation::Reservation():
   _pimpl(rmf_utils::make_impl<Implementation>())
 {
-
+  // do nothing
 }
 
 }
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp
index 5f995d3c..71388c49 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp
@@ -26,13 +26,13 @@ namespace reservations {
 class Heuristic
 {
 public:
-  virtual float score(const State& state);
+  virtual float score(const State& state) = 0;
 };
 
 class PriorityBasedScorer: public Heuristic
 {
 public:
-  PriorityBasedScorer(std::shared_ptr<RequestQueue> queue) :
+  PriorityBasedScorer(std::shared_ptr<RequestStore> queue) :
     _queue(queue)
   {
 
@@ -42,13 +42,13 @@ class PriorityBasedScorer: public Heuristic
     auto score = 0;
     for(auto [part_id, req_id]: state.unassigned())
     {
-      // TODO: Incorporate 
+      // TODO: Incorporate
       score += _queue->get_request_info(part_id, req_id).priority;
     }
     return score;
   }
 private:
-  std::shared_ptr<RequestQueue> _queue;
+  std::shared_ptr<RequestStore> _queue;
 };
 
 }
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/Optimizer.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/Optimizer.hpp
index b86690f9..888e2755 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/Optimizer.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/Optimizer.hpp
@@ -110,7 +110,8 @@ class GreedyBestFirstSearchOptimizer
 
   Solution optimize(State& state)
   {
-    
+    Solution solution(state, _heuristic);
+    return solution;
   }
 private:
   std::shared_ptr<Heuristic> _heuristic;
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp
index d47eadd3..5f820aa0 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp
@@ -20,7 +20,12 @@
 
 #include <rmf_traffic/reservations/ReservationRequest.hpp>
 #include <rmf_traffic/reservations/Reservation.hpp>
+#include <rmf_traffic/reservations/Participant.hpp>
 #include <unordered_map>
+#include <deque>
+#include <mutex>
+#include <condition_variable>
+#include "RequestStore.hpp"
 
 namespace rmf_traffic {
 namespace reservations {
@@ -28,123 +33,81 @@ namespace reservations {
 class RequestQueue
 {
 public:
-  struct RequestInfo {
-    int priority;
-    std::vector<ReservationRequest> request_options;
+  enum ActionType
+  {
+    ADD,
+    REMOVE,
+    REMOVE_PARTICIPANT
   };
-
-  RequestInfo get_request_info(
-    ParticipantId pid,
-    RequestId req)
+  struct Action
   {
-    //TODO: Sanitize
-    return _reservation_info[pid][req];
-  }
+    ActionType type;
+    ParticipantId participant_id;
+    RequestId request_id;
+    std::vector<ReservationRequest>& request_options;
+    int priority;
+  };
 
-  void enqueue_reservation(
-    ParticipantId pid,
-    RequestId req,
-    int priority,
-    std::vector<ReservationRequest>& reservation
-  )
+  struct QueueElement
   {
-    _reservation_info[pid].insert({
-      req,
-      {
-        priority,
-        std::move(reservation)
-      }
-    });
-  }
-
-  void erase_participant_requests(
-    ParticipantId pid
-  ){
-    auto entry = _reservation_info.find(pid);
-    if(entry == _reservation_info.end())
-      return;
-    _reservation_info.erase(entry);
-  }
+    Action action;
+    std::shared_ptr<RequestStore> request_store;
+  };
 
-  bool satisfies(ReservationRequest& req, Reservation reservation)
+  void add_action(Action action)
   {
-    //Check upper bound
-    if(req.start_time()->lower_bound().has_value())
-    {
-      if(reservation.start_time() < req.start_time()->lower_bound().value())
-      {
-        return false;
-      }
-    }
-
-    //Check upper bound
-    if(req.start_time()->upper_bound().has_value())
     {
-      if(reservation.start_time() > req.start_time()->upper_bound().value())
+      std::lock_guard lock(_mutex);
+
+      std::shared_ptr<RequestStore>
+        req_store = [=]() -> std::shared_ptr<RequestStore>
+        {
+          if(_request_store_queue.empty())
+            return std::make_shared<RequestStore>();
+          else
+            return std::make_shared<RequestStore>(
+              _request_store_queue.back().request_store.get()
+            );
+        }();
+
+      if(action.type == ActionType::ADD)
       {
-        return false;
+        req_store->enqueue_reservation(
+          action.participant_id,
+          action.request_id,
+          action.priority,
+          action.request_options);
       }
-    }
-
-    if(req.duration().has_value())
-    {
-      if(reservation.is_indefinite())
+      else if(action.type == ActionType::REMOVE)
       {
-        return false;
+        req_store->cancel(action.participant_id, action.request_id);
       }
-      if(req.duration().value()
-        > *reservation.actual_finish_time() - reservation.start_time())
+      else
       {
-        return false;
+        req_store->erase_participant_requests(action.participant_id);
       }
+      _request_store_queue.push_back({action, req_store});
     }
-
-    if(req.finish_time().has_value())
-    {
-      if(reservation.is_indefinite())
-      {
-        return false;
-      }
-      if(req.finish_time().value() > *reservation.actual_finish_time())
-      {
-        return false;
-      }
-    }
-
-    if((req.is_indefinite() && !reservation.is_indefinite())
-      || (!req.is_indefinite() && reservation.is_indefinite()))
-    {
-      return false;
-    }
-
-    if(req.resource_name() != reservation.resource_name())
-    {
-      return false;
-    }
-
-    return true;
+    _cond.notify_one();
   }
 
-  std::optional<std::size_t> satisfies(
-    ParticipantId pid,
-    RequestId reqid,
-    Reservation& res
-  ) {
-    for(std::size_t i = 0;
-      i < _reservation_info[pid][reqid].request_options.size();
-      i++)
-    {
-      if(satisfies(_reservation_info[pid][reqid].request_options[i], res))
-        return i;
-    }
-    return std::nullopt;
+  std::shared_ptr<QueueElement> deque()
+  {
+    std::unique_lock loc(_mutex);
+    _cond.wait(loc, [=](){return !_request_store_queue.empty();});
+    auto front = _request_store_queue.front();
+    _request_store_queue.pop_front();
+    loc.unlock();
+    return front;
   }
+
 private:
-  std::unordered_map<ParticipantId,
-    std::unordered_map<RequestId, RequestInfo>
-    >  _reservation_info;
+  std::deque<QueueElement> _request_store_queue;
+  std::mutex _mutex;
+  std::condition_variable _cond;
 };
 
 }
 }
+
 #endif
\ No newline at end of file
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp
new file mode 100644
index 00000000..4eba3e50
--- /dev/null
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp
@@ -0,0 +1,155 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * 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.
+ *
+*/
+
+#ifndef RMF_TRAFFIC__RESERVATIONS__INTERNAL_REQUESTSTORE_HPP
+#define RMF_TRAFFIC__RESERVATIONS__INTERNAL_REQUESTSTORE_HPP
+
+#include <rmf_traffic/reservations/ReservationRequest.hpp>
+#include <rmf_traffic/reservations/Reservation.hpp>
+#include <unordered_map>
+
+namespace rmf_traffic {
+namespace reservations {
+
+class RequestStore
+{
+public:
+  struct RequestInfo {
+    int priority;
+    std::vector<ReservationRequest> request_options;
+  };
+
+  RequestInfo get_request_info(
+    ParticipantId pid,
+    RequestId req)
+  {
+    //TODO: Sanitize
+    return _reservation_info[pid][req];
+  }
+
+  void enqueue_reservation(
+    ParticipantId pid,
+    RequestId req,
+    int priority,
+    std::vector<ReservationRequest>& reservation
+  )
+  {
+    _reservation_info[pid].insert({
+      req,
+      {
+        priority,
+        std::move(reservation)
+      }
+    });
+  }
+
+  void erase_participant_requests(
+    ParticipantId pid
+  ){
+    auto entry = _reservation_info.find(pid);
+    if(entry == _reservation_info.end())
+      return;
+    _reservation_info.erase(entry);
+  }
+
+  void cancel(ParticipantId pid, RequestId req)
+  {
+    _reservation_info[pid].erase(req);
+  }
+
+  bool satisfies(ReservationRequest& req, Reservation reservation)
+  {
+    //Check upper bound
+    if(req.start_time()->lower_bound().has_value())
+    {
+      if(reservation.start_time() < req.start_time()->lower_bound().value())
+      {
+        return false;
+      }
+    }
+
+    //Check upper bound
+    if(req.start_time()->upper_bound().has_value())
+    {
+      if(reservation.start_time() > req.start_time()->upper_bound().value())
+      {
+        return false;
+      }
+    }
+
+    if(req.duration().has_value())
+    {
+      if(reservation.is_indefinite())
+      {
+        return false;
+      }
+      if(req.duration().value()
+        > *reservation.actual_finish_time() - reservation.start_time())
+      {
+        return false;
+      }
+    }
+
+    if(req.finish_time().has_value())
+    {
+      if(reservation.is_indefinite())
+      {
+        return false;
+      }
+      if(req.finish_time().value() > *reservation.actual_finish_time())
+      {
+        return false;
+      }
+    }
+
+    if((req.is_indefinite() && !reservation.is_indefinite())
+      || (!req.is_indefinite() && reservation.is_indefinite()))
+    {
+      return false;
+    }
+
+    if(req.resource_name() != reservation.resource_name())
+    {
+      return false;
+    }
+
+    return true;
+  }
+
+  std::optional<std::size_t> satisfies(
+    ParticipantId pid,
+    RequestId reqid,
+    Reservation& res)
+  {
+    for(std::size_t i = 0;
+      i < _reservation_info[pid][reqid].request_options.size();
+      i++)
+    {
+      if(satisfies(_reservation_info[pid][reqid].request_options[i], res))
+        return i;
+    }
+    return std::nullopt;
+  }
+private:
+  std::unordered_map<ParticipantId,
+    std::unordered_map<RequestId, RequestInfo>
+    >  _reservation_info;
+};
+
+}
+}
+#endif
\ No newline at end of file
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index e581b4d6..096d50d6 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -25,13 +25,15 @@
 #include <map>
 #include <set>
 
-#include "RequestQueue.hpp"
+#include "RequestStore.hpp"
 
 namespace rmf_traffic {
 namespace reservations {
 
 struct NextStateGenerator;
 
+class StateDiff;
+
 // Ripped from https://stackoverflow.com/questions/2590677/how-do-i-combine-hash-values-in-c0x
 template <class T>
 inline void hash_combine(std::size_t& seed, const T& v)
@@ -62,7 +64,7 @@ class State
   using ResourceSchedule = std::map<rmf_traffic::Time, Reservation>;
   using ResourceSchedules = std::unordered_map<std::string, ResourceSchedule>;
   using ReservationAssignment = std::unordered_map<
-    ParticipantId, std::unordered_map<ReservationId, RequestId>>;
+    ParticipantId, std::unordered_map<RequestId, ReservationId>>;
 
   using ReservationTimings = std::unordered_map<ReservationId, Time>;
   using ReservationResources = std::unordered_map<ReservationId, std::string>;
@@ -71,12 +73,17 @@ class State
   using UnassignedSet = std::unordered_set<
     std::pair<ParticipantId, RequestId>, pair_hash>;
 
-  State(std::shared_ptr<RequestQueue> queue) :
+  State(std::shared_ptr<RequestStore> queue) :
     _queue(queue)
   {
     //Do nothing
   }
 
+  void update_reservation_store(std::shared_ptr<RequestStore> store)
+  {
+    _queue = store;
+  }
+
   NextStateGenerator begin();
   NextStateGenerator end();
   /// Adds a request to the state
@@ -113,7 +120,7 @@ class State
 
     new_state._resource_schedules[resource].erase(time);
     new_state._reservation_resources.erase(reservation_id);
-    new_state._reservation_assignments.erase(reservation_id);
+    new_state._reservation_assignments[pid].erase(reqid);
     new_state._reservation_timings.erase(findings);
     return new_state;
   }
@@ -191,6 +198,16 @@ class State
     return _unassigned;
   }
 
+  ResourceSchedules resource_schedule() const
+  {
+    return _resource_schedules;
+  }
+
+  ReservationRequestId request_ids() const
+  {
+    return _reservation_request_ids;
+  }
+
   State(const State& other) :
     _resource_schedules(other._resource_schedules),
     _unassigned(other._unassigned),
@@ -353,10 +370,11 @@ class State
   ReservationResources _reservation_resources; // reservation => resource
   ReservationRequestId _reservation_request_ids; // reservation => {part_id, req_id, index}
 
-  std::shared_ptr<RequestQueue> _queue;
+  std::shared_ptr<RequestStore> _queue;
 
   Time _current_time;
   friend NextStateGenerator;
+  friend StateDiff;
 };
 struct NextStateGenerator
 {
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/StateDiff.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/StateDiff.hpp
new file mode 100644
index 00000000..609b70d8
--- /dev/null
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/StateDiff.hpp
@@ -0,0 +1,113 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * 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.
+ *
+*/
+
+#ifndef RMF_TRAFFIC__RESERVATIONS__INTERNAL_STATEDIFF_HPP
+#define RMF_TRAFFIC__RESERVATIONS__INTERNAL_STATEDIFF_HPP
+
+#include <unordered_map>
+#include "State.hpp"
+
+namespace rmf_traffic {
+namespace reservations {
+
+/// Gives the difference between two states. *NOTE*: Assumes that 
+class StateDiff
+{
+public:
+
+  enum DifferenceType
+  {
+    SHIFT_RESERVATION,
+    ASSIGN_RESERVATION,
+    UNASSIGN_RESERVATION
+  };
+
+  struct Difference
+  {
+    DifferenceType diff_type;
+    ParticipantId participant_id;
+    RequestId request_id;
+    std::optional<Reservation> reservation;
+  };
+
+  /// Calaculates the StateDiff required for one to go from State2 to State1.
+  StateDiff(
+    State& state1,
+    State& state2
+  )
+  {
+    // TODO(arjo): currently method is O(nlogn). It is possible to implement
+    // in O(n) using a two pointer method, just trickier. Ideally we should 
+    // do this.
+
+    // Check for reservation shuffling/shifting and assignment
+    for(auto [participant, requests]: state1._reservation_assignments)
+    {
+      for(auto [request, reservation_id1]: requests)
+      {
+        auto resource = state1._reservation_resources[reservation_id1];
+        if(
+          state2._reservation_assignments.count(participant) != 0
+          && state2._reservation_assignments[participant].count(request) != 0)
+        {
+          auto reservation_id2 =
+            state2._reservation_assignments[participant][request];
+          auto time1 = state1._reservation_timings[reservation_id1];
+          auto time2 = state2._reservation_timings[reservation_id2];
+          auto res1 = state1._resource_schedules[resource].find(time1)->second;
+          auto res2 = state2._resource_schedules[resource].find(time2)->second;
+
+          if(res1 != res2)
+          {
+            // Request was shifted from state 2
+            Difference diff
+            {
+              DifferenceType::SHIFT_RESERVATION,
+              participant,
+              request,
+              {res1}
+            };
+            _differences.push_back(diff);
+          }
+        }
+        else
+        {
+          // Request was not there in state2 therefore we assign it.
+          auto time1 = state1._reservation_timings[reservation_id1];
+          auto res1 = state1._resource_schedules[resource].find(time1).second;
+          Difference diff
+          {
+            DifferenceType::ASSIGN_RESERVATION,
+            participant,
+            request,
+            {res1}
+          };
+          _differences.push_back(diff);
+        }
+
+      }
+    }
+
+    //Check for unassigning
+  }
+private:
+  std::vector<Difference>  _differences;
+};
+
+}
+}
+#endif
\ No newline at end of file
diff --git a/rmf_traffic/test/unit/reservations/internal/test_State.cpp b/rmf_traffic/test/unit/reservations/internal/test_State.cpp
index 7b09aac2..0e1f1d5f 100644
--- a/rmf_traffic/test/unit/reservations/internal/test_State.cpp
+++ b/rmf_traffic/test/unit/reservations/internal/test_State.cpp
@@ -27,7 +27,7 @@ using namespace std::chrono_literals;
 
 SCENARIO("A few reservations in a state")
 {
-    auto queue = std::make_shared<RequestQueue>();
+    auto queue = std::make_shared<RequestStore>();
     State start_state(queue);
 
     auto now = std::chrono::steady_clock::now();
@@ -87,7 +87,11 @@ SCENARIO("A few reservations in a state")
         next_state.debug_state();
     }
 
-    /*auto heuristic = std::make_shared<PriorityBasedScorer>();
-    GreedyBestFirstSearchOptimizer opt(start_state, heuristic);*/
-
+    auto heuristic = std::make_shared<PriorityBasedScorer>(queue);
+    GreedyBestFirstSearchOptimizer opt(heuristic);
+    auto solutions = opt.optimize(start_state);
+    while(auto solution = solutions.next_solution())
+    {
+        solution->debug_state();
+    }
 }
\ No newline at end of file

From f57d73c989de7c17abf4cba463f9426bd4053cf5 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Wed, 16 Jun 2021 09:50:42 +0800
Subject: [PATCH 26/66] Add state diff

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../reservations/internal/RequestQueue.hpp    |  2 +-
 .../reservations/internal/StateDiff.hpp       | 25 +++++++++++++++++--
 2 files changed, 24 insertions(+), 3 deletions(-)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp
index 5f820aa0..ff3b42ea 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp
@@ -91,7 +91,7 @@ class RequestQueue
     _cond.notify_one();
   }
 
-  std::shared_ptr<QueueElement> deque()
+  QueueElement deque()
   {
     std::unique_lock loc(_mutex);
     _cond.wait(loc, [=](){return !_request_store_queue.empty();});
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/StateDiff.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/StateDiff.hpp
index 609b70d8..a80c2868 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/StateDiff.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/StateDiff.hpp
@@ -24,7 +24,8 @@
 namespace rmf_traffic {
 namespace reservations {
 
-/// Gives the difference between two states. *NOTE*: Assumes that 
+/// Gives the difference between two states. *NOTE*: Assumes that bothe the
+/// states have the same
 class StateDiff
 {
 public:
@@ -44,7 +45,7 @@ class StateDiff
     std::optional<Reservation> reservation;
   };
 
-  /// Calaculates the StateDiff required for one to go from State2 to State1.
+  /// Calculates the StateDiff required for one to go from State2 to State1.
   StateDiff(
     State& state1,
     State& state2
@@ -103,6 +104,26 @@ class StateDiff
     }
 
     //Check for unassigning
+    for(auto [participant, request]: state1.unassigned())
+    {
+      if(state2._reservation_assignments.count(participant) > 0
+        && state2._reservation_assignments[participant].count(request) > 0)
+      {
+        _differences.push_back(
+          {
+            UNASSIGN_RESERVATION,
+            participant,
+            request,
+            std::nullopt
+          }
+        );
+      }
+    }
+  }
+
+  std::vector<Difference>  differences() const
+  {
+    return _differences;
   }
 private:
   std::vector<Difference>  _differences;

From 42a8967e45ea065490ab930cd7e5e2d88d0323f3 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Wed, 16 Jun 2021 13:49:03 +0800
Subject: [PATCH 27/66] Made progress on the execution engine

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../rmf_traffic/reservations/Participant.hpp  |  10 +-
 .../reservations/internal/ExecutionEngine.hpp | 142 ++++++++++++++++++
 .../reservations/internal/RequestQueue.hpp    |  12 +-
 .../reservations/internal/State.hpp           |  49 +++++-
 4 files changed, 204 insertions(+), 9 deletions(-)
 create mode 100644 rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp

diff --git a/rmf_traffic/include/rmf_traffic/reservations/Participant.hpp b/rmf_traffic/include/rmf_traffic/reservations/Participant.hpp
index 546f14e6..c7d63b65 100644
--- a/rmf_traffic/include/rmf_traffic/reservations/Participant.hpp
+++ b/rmf_traffic/include/rmf_traffic/reservations/Participant.hpp
@@ -28,15 +28,21 @@ using ParticipantId = uint64_t;
 class Participant
 {
 public:
-  virtual bool request_proposal_received(
+  virtual bool request_proposal(
     RequestId id,
     Reservation& res
   ) = 0;
 
-  virtual bool request_proposal_confirmed(
+  virtual bool request_confirmed(
     RequestId id,
     Reservation& res
   ) = 0;
+
+  virtual bool unassign_request_confirmed(
+    RequestId id
+  ) = 0;
+
+  virtual bool request_failed(RequestId id) = 0;
 };
 
 }
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp
new file mode 100644
index 00000000..a9415a1a
--- /dev/null
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp
@@ -0,0 +1,142 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * 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.
+ *
+*/
+
+#ifndef RMF_TRAFFIC__RESERVATIONS__INTERNAL_EXECUTIONENGINE_HPP
+#define RMF_TRAFFIC__RESERVATIONS__INTERNAL_EXECUTIONENGINE_HPP
+
+#include <thread>
+
+#include "RequestQueue.hpp"
+#include "ParticipantStore.hpp"
+#include "Optimizer.hpp"
+#include "State.hpp"
+#include "StateDiff.hpp"
+
+namespace rmf_traffic {
+namespace reservations {
+
+/// \brief The execution engine class is in charge of handling the requests and
+/// negotiations that occur during the planning and negotiation phase. As such,
+/// it maintains a single thread that handles both planning and negotiations
+/// while enqueueing requests upon a reservation queue.
+class ExecutionEngine
+{
+  ExecutionEngine(
+    std::shared_ptr<RequestQueue> request_queue,
+    std::shared_ptr<ParticipantStore> pstore):
+      _request_queue(request_queue),
+      _participant_store(pstore),
+      _execution_thread(&ExecutionEngine::execute, this),
+      _curr_state(std::make_shared<RequestStore>())
+  {
+    //Do nothing
+  }
+
+  ~ExecutionEngine()
+  {
+    std::vector<ReservationRequest> res;
+    // Terminate the execution thread
+    RequestQueue::Action terminate_req {
+      RequestQueue::ActionType::TERMINATE_STREAM,
+      0,
+      0,
+      res,
+      0
+    };
+    _request_queue->add_action(terminate_req);
+
+    if(_execution_thread.joinable())
+      _execution_thread.join();
+  }
+
+private:
+  void execute()
+  {
+    while(true)
+    {
+      auto element = _request_queue->deque();
+
+      if(element.action.type == RequestQueue::ActionType::TERMINATE_STREAM)
+        return;
+
+      if(element.action.type == RequestQueue::ActionType::ADD)
+      {
+        _curr_state = _curr_state.add_request(
+          element.action.participant_id,
+          element.action.request_id,
+          element.request_store
+        );
+      }
+      else if(element.action.type == RequestQueue::ActionType::REMOVE)
+      {
+        _curr_state = _curr_state.remove_request(
+          element.action.participant_id,
+          element.action.request_id,
+          element.request_store
+        );
+      }
+      // TODO(URGENT): Add participant removal
+
+      //TODO: Breakout optimizer
+      auto heuristic = std::make_shared<PriorityBasedScorer>(
+        element.request_store);
+      GreedyBestFirstSearchOptimizer optimizer(heuristic);
+      auto solutions = optimizer.optimize(_curr_state);
+      while(auto solution = solutions.next_solution())
+      {
+        StateDiff impacted_reservations(solution.value(), _curr_state);
+        auto res = impacted_reservations.differences();
+        for(auto diff: res)
+        {
+          auto impacted_participant = diff.participant_id;
+          auto participant =
+            _participant_store->get_participant(impacted_participant)
+              .value();
+
+          if(diff.diff_type == StateDiff::DifferenceType::ASSIGN_RESERVATION
+            || diff.diff_type == StateDiff::DifferenceType::SHIFT_RESERVATION)
+          {
+            auto result = participant->request_proposal(
+              diff.request_id,
+              diff.reservation.value()
+            );
+            if(!result)
+              continue;
+          }
+          else
+          {
+            auto result = participant->(
+              diff.request_id,
+              diff.reservation.value()
+            );
+            if(!result)
+              continue;
+          }
+        }
+        break;
+      }
+    }
+  }
+  std::thread _execution_thread;
+  std::shared_ptr<RequestQueue> _request_queue;
+  std::shared_ptr<ParticipantStore> _participant_store;
+  State _curr_state;
+};
+}
+}
+
+#endif
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp
index ff3b42ea..3abb42ee 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp
@@ -30,15 +30,24 @@
 namespace rmf_traffic {
 namespace reservations {
 
+/// \brief The RequestQueue is used to coordinate dispatch to a the
+/// ExecutionEngine.
 class RequestQueue
 {
 public:
   enum ActionType
   {
+    /// \brief Add a request
     ADD,
+    /// \brief Remove a request
     REMOVE,
-    REMOVE_PARTICIPANT
+    /// \brief Remove an entire participant
+    REMOVE_PARTICIPANT,
+    /// \brief Terminates the execution engine. Only the ExecutionEngine class
+    /// should use this.
+    TERMINATE_STREAM
   };
+  /// This represents the action which you wish to execute.
   struct Action
   {
     ActionType type;
@@ -47,7 +56,6 @@ class RequestQueue
     std::vector<ReservationRequest>& request_options;
     int priority;
   };
-
   struct QueueElement
   {
     Action action;
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index 096d50d6..756424db 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -79,11 +79,6 @@ class State
     //Do nothing
   }
 
-  void update_reservation_store(std::shared_ptr<RequestStore> store)
-  {
-    _queue = store;
-  }
-
   NextStateGenerator begin();
   NextStateGenerator end();
   /// Adds a request to the state
@@ -96,6 +91,18 @@ class State
     return new_state;
   }
 
+  /// Adds a request to the state
+  State add_request(
+    ParticipantId pid,
+    RequestId reqid,
+    std::shared_ptr<RequestStore> store) const
+  {
+    State new_state(*this);
+    new_state._unassigned.insert({pid, reqid});
+    new_state._queue = store;
+    return new_state;
+  }
+
   State remove_request(
     ParticipantId pid,
     RequestId reqid) const
@@ -125,6 +132,38 @@ class State
     return new_state;
   }
 
+   State remove_request(
+    ParticipantId pid,
+    RequestId reqid,
+    std::shared_ptr<RequestStore> store) const
+  {
+    State new_state(*this);
+    auto req = new_state._unassigned.find({pid, reqid});
+
+    if(req != new_state._unassigned.end())
+    {
+      new_state._unassigned.erase(req);
+      return new_state;
+    }
+    auto reservation_id = new_state._reservation_assignments[pid][reqid];
+    auto findings = new_state._reservation_timings.find(reservation_id);
+    if(findings == new_state._reservation_timings.end())
+    {
+      // Non existant pid-reqid pair
+      return new_state;
+    }
+    auto time = findings->second;
+    auto resource = new_state._reservation_resources[reservation_id];
+
+    new_state._resource_schedules[resource].erase(time);
+    new_state._reservation_resources.erase(reservation_id);
+    new_state._reservation_assignments[pid].erase(reqid);
+    new_state._reservation_timings.erase(findings);
+    
+    new_state._queue = store;
+    return new_state;
+  }
+
   void debug_state()
   {
     std::cout << "________________________" <<std::endl;

From 0ecb7af5d109dd38e5126306d8acfb2c77862630 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Fri, 18 Jun 2021 19:07:57 +0800
Subject: [PATCH 28/66] Add request list

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../rmf_traffic/reservations/Participant.hpp  |   4 +-
 .../rmf_traffic/reservations/Reservation.hpp  |   2 +
 .../rmf_traffic/reservations/Reservation.cpp  |  19 ++
 .../reservations/internal/ExecutionEngine.hpp | 181 ++++++++++++---
 .../reservations/internal/State.hpp           | 214 +++++++++++++-----
 .../reservations/internal/StateDiff.hpp       |  69 +++++-
 .../unit/reservations/test_Reservation.cpp    |   0
 7 files changed, 391 insertions(+), 98 deletions(-)
 create mode 100644 rmf_traffic/test/unit/reservations/test_Reservation.cpp

diff --git a/rmf_traffic/include/rmf_traffic/reservations/Participant.hpp b/rmf_traffic/include/rmf_traffic/reservations/Participant.hpp
index c7d63b65..a4f1d00d 100644
--- a/rmf_traffic/include/rmf_traffic/reservations/Participant.hpp
+++ b/rmf_traffic/include/rmf_traffic/reservations/Participant.hpp
@@ -42,7 +42,9 @@ class Participant
     RequestId id
   ) = 0;
 
-  virtual bool request_failed(RequestId id) = 0;
+  virtual bool unassign_request_proposal(
+    RequestId id
+  ) = 0;
 };
 
 }
diff --git a/rmf_traffic/include/rmf_traffic/reservations/Reservation.hpp b/rmf_traffic/include/rmf_traffic/reservations/Reservation.hpp
index a5b61dbf..324d4818 100644
--- a/rmf_traffic/include/rmf_traffic/reservations/Reservation.hpp
+++ b/rmf_traffic/include/rmf_traffic/reservations/Reservation.hpp
@@ -51,6 +51,8 @@ class Reservation
 
   ReservationId reservation_id() const;
 
+  bool conflicts_with(const Reservation& other) const;
+
   bool operator==(const Reservation& other) const;
 
   bool operator!=(const Reservation& other) const;
diff --git a/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp b/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp
index 797c48da..b81d107f 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp
@@ -114,6 +114,25 @@ ReservationId Reservation::reservation_id() const
   return _pimpl->_res_id;
 }
 
+//=============================================================================
+bool Reservation::conflicts_with(const Reservation& other) const
+{
+  if(other.start_time() < this->start_time())
+  {
+    if(!other.is_indefinite()
+      && other.actual_finish_time() <= this->start_time())
+      return true;
+    else return false;
+  }
+  else
+  {
+    if(!this->is_indefinite()
+      && this->actual_finish_time() <= other.start_time())
+      return true;
+    else return false;
+  }
+}
+
 //=============================================================================
 bool Reservation::operator==(const Reservation& other) const
 {
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp
index a9415a1a..2144f7c2 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp
@@ -29,12 +29,21 @@
 namespace rmf_traffic {
 namespace reservations {
 
+//==============================================================================
 /// \brief The execution engine class is in charge of handling the requests and
 /// negotiations that occur during the planning and negotiation phase. As such,
 /// it maintains a single thread that handles both planning and negotiations
-/// while enqueueing requests upon a reservation queue.
+/// while enqueueing requests upon a reservation queue. This is where the crux
+/// of the protocol is implemented
 class ExecutionEngine
 {
+  //============================================================================
+  /// \brief Constructor. Starts an execution thread engine which consumes
+  /// commands of the incoming participants.
+  /// \param[in] request_queue - the `RequestQueue` upon which you will send
+  ///   commands.
+  /// \param[in] participant_store - the `ParticipantStore` containing the list
+  ///   of participants.
   ExecutionEngine(
     std::shared_ptr<RequestQueue> request_queue,
     std::shared_ptr<ParticipantStore> pstore):
@@ -46,6 +55,9 @@ class ExecutionEngine
     //Do nothing
   }
 
+  //============================================================================
+  /// \brief Destructor. Terminates the execution engine by sending a
+  /// TERMINATE_STREAM action and waits for the destructor to join.
   ~ExecutionEngine()
   {
     std::vector<ReservationRequest> res;
@@ -64,6 +76,9 @@ class ExecutionEngine
   }
 
 private:
+  //============================================================================
+  /// \brief Main thread: blocks until a request has come in. When a request
+  /// comes in the current_state is read and updated to contain the request.
   void execute()
   {
     while(true)
@@ -89,48 +104,144 @@ class ExecutionEngine
           element.request_store
         );
       }
-      // TODO(URGENT): Add participant removal
+      else if(element.action.type == RequestQueue::ActionType::REMOVE_PARTICIPANT)
+      {
+        _curr_state = _curr_state.remove_participant(
+          element.action.participant_id,
+          element.request_store);
+      }
 
-      //TODO: Breakout optimizer
       auto heuristic = std::make_shared<PriorityBasedScorer>(
         element.request_store);
+
       GreedyBestFirstSearchOptimizer optimizer(heuristic);
-      auto solutions = optimizer.optimize(_curr_state);
-      while(auto solution = solutions.next_solution())
+      auto favored_solution = get_favored_solution(optimizer);
+      if(!favored_solution.has_value())
+        continue;
+
+      rollout(favored_solution.value());
+
+    }
+  }
+
+  //============================================================================
+  /// \brief Rolls out the changes. If for whatever reason a rollout should fail
+  /// there is no need to rollback as the StateDiff is returned in a topological
+  /// order. Instead _`curr_state` remains in a partially successful state and
+  /// we will retry optimization upon the arrival of the next request.
+  void rollout(const StateDiff& changes)
+  {
+    for(auto change: changes.differences())
+    {
+      assert(
+        _participant_store->get_participant(change.participant_id).has_value());
+
+      auto participant =
+        _participant_store->get_participant(change.participant_id).value();
+      if(change.diff_type == StateDiff::DifferenceType::SHIFT_RESERVATION)
+      {
+        assert(change.reservation.has_value());
+
+        auto result = participant->request_confirmed(
+          change.request_id,
+          change.reservation.value());
+
+        if(result == false)
+          return;
+
+        _curr_state = _curr_state.unassign_reservation(
+          change.participant_id,
+          change.request_id);
+
+        _curr_state.assign_reservation(
+          change.participant_id,
+          change.request_id,
+          change.reservation.value()
+        );
+      }
+      else if(
+        change.diff_type == StateDiff::DifferenceType::ASSIGN_RESERVATION)
+      {
+        assert(change.reservation.has_value());
+
+        auto result = participant->request_confirmed(
+          change.request_id,
+          change.reservation.value());
+
+        if(result == false)
+          return;
+
+        _curr_state.assign_reservation(
+          change.participant_id,
+          change.request_id,
+          change.reservation.value()
+        );
+      }
+      else if(
+        change.diff_type == StateDiff::DifferenceType::UNASSIGN_RESERVATION)
       {
-        StateDiff impacted_reservations(solution.value(), _curr_state);
-        auto res = impacted_reservations.differences();
-        for(auto diff: res)
-        {
-          auto impacted_participant = diff.participant_id;
-          auto participant =
-            _participant_store->get_participant(impacted_participant)
-              .value();
-
-          if(diff.diff_type == StateDiff::DifferenceType::ASSIGN_RESERVATION
-            || diff.diff_type == StateDiff::DifferenceType::SHIFT_RESERVATION)
-          {
-            auto result = participant->request_proposal(
-              diff.request_id,
-              diff.reservation.value()
-            );
-            if(!result)
-              continue;
-          }
-          else
-          {
-            auto result = participant->(
-              diff.request_id,
-              diff.reservation.value()
-            );
-            if(!result)
-              continue;
-          }
-        }
-        break;
+        assert(!change.reservation.has_value());
+        auto result =
+          participant->unassign_request_confirmed(change.request_id);
+
+        if(result == false)
+          return;
+
+        _curr_state = _curr_state.unassign_reservation(
+          change.participant_id,
+          change.request_id);
       }
     }
   }
+
+  //============================================================================
+  /// \brief Proposes the solution favoured by the optimizer and 
+  std::optional<StateDiff> get_favored_solution(
+    GreedyBestFirstSearchOptimizer& optimizer)
+  {
+    auto solutions = optimizer.optimize(_curr_state);
+    while(auto solution = solutions.next_solution())
+    {
+      StateDiff impacted_reservations(solution.value(), _curr_state);
+      if(verify_proposal(impacted_reservations))
+        return {impacted_reservations};
+    }
+
+    return std::nullopt;
+  }
+
+  bool verify_proposal(const StateDiff& impacted_reservations)
+  {
+    auto res = impacted_reservations.differences();
+    for(auto diff: res)
+    {
+      auto impacted_participant = diff.participant_id;
+      auto participant =
+        _participant_store->get_participant(impacted_participant)
+          .value();
+
+      if(diff.diff_type == StateDiff::DifferenceType::ASSIGN_RESERVATION
+        || diff.diff_type == StateDiff::DifferenceType::SHIFT_RESERVATION)
+      {
+        auto result = participant->request_proposal(
+          diff.request_id,
+          diff.reservation.value()
+        );
+        if(!result)
+          return false;
+      }
+      else
+      {
+        auto result = participant->unassign_request_proposal(
+          diff.request_id
+        );
+        if(!result)
+          return false;
+      }
+    }
+    return true;
+  }
+
   std::thread _execution_thread;
   std::shared_ptr<RequestQueue> _request_queue;
   std::shared_ptr<ParticipantStore> _participant_store;
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index 756424db..ad83a4cf 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -73,6 +73,7 @@ class State
   using UnassignedSet = std::unordered_set<
     std::pair<ParticipantId, RequestId>, pair_hash>;
 
+  //============================================================================
   State(std::shared_ptr<RequestStore> queue) :
     _queue(queue)
   {
@@ -81,6 +82,8 @@ class State
 
   NextStateGenerator begin();
   NextStateGenerator end();
+  
+  //============================================================================
   /// Adds a request to the state
   State add_request(
     ParticipantId pid,
@@ -91,6 +94,7 @@ class State
     return new_state;
   }
 
+  //============================================================================
   /// Adds a request to the state
   State add_request(
     ParticipantId pid,
@@ -132,7 +136,8 @@ class State
     return new_state;
   }
 
-   State remove_request(
+  //============================================================================
+  State remove_request(
     ParticipantId pid,
     RequestId reqid,
     std::shared_ptr<RequestStore> store) const
@@ -159,11 +164,26 @@ class State
     new_state._reservation_resources.erase(reservation_id);
     new_state._reservation_assignments[pid].erase(reqid);
     new_state._reservation_timings.erase(findings);
-    
+
     new_state._queue = store;
     return new_state;
   }
 
+  //============================================================================
+  State remove_participant(
+    ParticipantId pid,
+    std::shared_ptr<RequestStore> store)
+  {
+    //TODO(arjo): Lots of unessecary copying
+    State new_state(*this);
+    for(auto [request, reservation]: _reservation_assignments[pid])
+    {
+      new_state = new_state.remove_request(pid, request, store);
+    }
+    return new_state;
+  }
+
+  //============================================================================
   void debug_state()
   {
     std::cout << "________________________" <<std::endl;
@@ -195,12 +215,14 @@ class State
     }
   }
 
+  //============================================================================
   bool operator==(const State& other) const
   {
     return _resource_schedules == other._resource_schedules
       && _unassigned == other._unassigned;
   }
 
+  //============================================================================
   std::size_t hash() const
   {
     std::size_t seed = 0x928193843;
@@ -227,94 +249,81 @@ class State
     return seed;
   }
 
+  //============================================================================
   void set_current_time(Time time)
   {
     _current_time = time;
   }
 
+  //============================================================================
   UnassignedSet unassigned() const
   {
     return _unassigned;
   }
 
+  //============================================================================
   ResourceSchedules resource_schedule() const
   {
     return _resource_schedules;
   }
 
+  //============================================================================
   ReservationRequestId request_ids() const
   {
     return _reservation_request_ids;
   }
 
-  State(const State& other) :
-    _resource_schedules(other._resource_schedules),
-    _unassigned(other._unassigned),
-    _reservation_timings(other._reservation_timings),
-    _queue(other._queue),
-    _reservation_resources(other._reservation_resources),
-    _reservation_assignments(other._reservation_assignments),
-    _reservation_request_ids(other._reservation_request_ids),
-    _current_time(other._current_time)
-  {
-    // Do nothing
-  }
-
-private:
-  bool push_back(
-    std::string resource,
-    ResourceSchedule::const_iterator res_iter,
-    Duration dur
-  )
+  //============================================================================
+  bool check_if_conflicts(Reservation res)
   {
-    assert(dur.count() > 0);
+    auto resource = res.resource_name();
+    auto start_slot =
+      _resource_schedules[resource].upper_bound(res.start_time());
 
-    auto prev_iter = res_iter;
-    std::next(res_iter);
-    // TODO: This is extremely inefficient. Do in-place.
-    std::map<Time, ReservationId> new_times;
-    auto gap_left = dur;
-    while(
-      res_iter != _resource_schedules[resource].end()
-      && prev_iter->second.actual_finish_time().has_value()
-      && gap_left.count() > 0)
+    if(res.is_indefinite())
     {
-      auto new_time = res_iter->first + gap_left;
-      auto gap = res_iter->first - prev_iter->second.actual_finish_time().value();
-      if(gap < gap_left)
-        gap_left -= gap;
+      if(_resource_schedules[resource].size() == 0)
+        return start_slot  != _resource_schedules[resource].end();
+      else if(
+        _resource_schedules[resource].rbegin()->second.actual_finish_time().has_value()
+        && _resource_schedules[resource].rbegin()->second.actual_finish_time() < res.start_time())
+        return start_slot  != _resource_schedules[resource].end();
       else
-        gap_left -= gap_left;
-      new_times[new_time] = res_iter->second.reservation_id();
-      prev_iter = res_iter;
-      std::next(res_iter);
+        return true;
     }
 
-    for(auto iter = new_times.rbegin(); iter != new_times.rend(); iter++)
+    auto end_slot =
+      _resource_schedules[resource].upper_bound(
+        res.actual_finish_time().value());
+
+    if(start_slot != _resource_schedules[resource].begin())
     {
-      bool ok  = shift_reservation_start_time(
-        resource,
-        iter->second,
-        iter->first);
-      if(!ok) return false;
+      auto prev_iter = std::prev(start_slot);
+      // Reservation definitely has end time
+      return !(end_slot == start_slot &&
+        start_slot->first >= res.actual_finish_time().value() &&
+        prev_iter->second.actual_finish_time().has_value() &&
+        prev_iter->second.actual_finish_time().value() <= res.start_time());
+    }
+    else
+    {
+      return !(end_slot == start_slot &&
+        start_slot->first >= res.actual_finish_time().value());
     }
-
-    return true;
   }
-  // Returns true if a shift could be successfully applied. Else returns false.
-  void unassign_reservation(ReservationId res_id, std::string resource_name)
-  {
-    auto time =  _reservation_timings[res_id];
-    _reservation_timings.erase(res_id);
-    _reservation_resources.erase(res_id);
-    auto request = _reservation_request_ids[res_id];
 
-    _unassigned.insert({request.participant , request.reqid});
-    _reservation_assignments[request.participant].erase(request.reqid);
-    _reservation_request_ids.erase(res_id);
-    _resource_schedules[resource_name].erase(time);
+
+
+  //============================================================================
+  State unassign_reservation(ParticipantId pid, RequestId reqid)
+  {
+    State new_state(*this);
+    auto res_id = _reservation_assignments[pid][reqid];
+    auto resource = _reservation_resources[res_id];
+    new_state.unassign_reservation(res_id, resource);
   }
 
+  //============================================================================
   bool assign_reservation(
     ParticipantId part_id,
     RequestId req_id,
@@ -377,6 +386,78 @@ class State
     return true;
   }
 
+  //============================================================================
+  State(const State& other) :
+    _resource_schedules(other._resource_schedules),
+    _unassigned(other._unassigned),
+    _reservation_timings(other._reservation_timings),
+    _queue(other._queue),
+    _reservation_resources(other._reservation_resources),
+    _reservation_assignments(other._reservation_assignments),
+    _reservation_request_ids(other._reservation_request_ids),
+    _current_time(other._current_time)
+  {
+    // Do nothing
+  }
+
+private:
+  //============================================================================
+  void unassign_reservation(ReservationId res_id, std::string resource_name)
+  {
+    auto time =  _reservation_timings[res_id];
+    _reservation_timings.erase(res_id);
+    _reservation_resources.erase(res_id);
+    auto request = _reservation_request_ids[res_id];
+
+    _unassigned.insert({request.participant , request.reqid});
+    _reservation_assignments[request.participant].erase(request.reqid);
+    _reservation_request_ids.erase(res_id);
+    _resource_schedules[resource_name].erase(time);
+  }
+
+  //============================================================================
+  bool push_back(
+    std::string resource,
+    ResourceSchedule::const_iterator res_iter,
+    Duration dur
+  )
+  {
+    assert(dur.count() > 0);
+
+    auto prev_iter = res_iter;
+    std::next(res_iter);
+    // TODO: This is extremely inefficient. Do in-place.
+    std::map<Time, ReservationId> new_times;
+    auto gap_left = dur;
+    while(
+      res_iter != _resource_schedules[resource].end()
+      && prev_iter->second.actual_finish_time().has_value()
+      && gap_left.count() > 0)
+    {
+      auto new_time = res_iter->first + gap_left;
+      auto gap = res_iter->first - prev_iter->second.actual_finish_time().value();
+      if(gap < gap_left)
+        gap_left -= gap;
+      else
+        gap_left -= gap_left;
+      new_times[new_time] = res_iter->second.reservation_id();
+      prev_iter = res_iter;
+      std::next(res_iter);
+    }
+
+    for(auto iter = new_times.rbegin(); iter != new_times.rend(); iter++)
+    {
+      bool ok  = shift_reservation_start_time(
+        resource,
+        iter->second,
+        iter->first);
+      if(!ok) return false;
+    }
+
+    return true;
+  }
+
+  //============================================================================
   bool shift_reservation_start_time(
     std::string resource,
     ReservationId res_id,
@@ -415,6 +496,7 @@ class State
   friend NextStateGenerator;
   friend StateDiff;
 };
+//==============================================================================
 struct NextStateGenerator
 {
   //TODO: RAW_POINTER-FOO
@@ -448,10 +530,12 @@ struct NextStateGenerator
   using reference = State;
   using iterator_category= std::input_iterator_tag;
 
+  //============================================================================
   State operator*() const {
     return *visiting_state;
   }
 
+  //============================================================================
   State::ResourceSchedule::const_iterator
     get_search_start(const ReservationRequest& req)
   {
@@ -469,6 +553,7 @@ struct NextStateGenerator
     }
   }
 
+  //============================================================================
   State::ResourceSchedule::const_iterator
     get_search_end(const ReservationRequest& req)
   {
@@ -486,6 +571,7 @@ struct NextStateGenerator
     }
   }
 
+  //============================================================================
   std::optional<State> advance_within_resource()
   {
     if(proceed_next_resource)
@@ -583,6 +669,7 @@ struct NextStateGenerator
     }
   }
 
+  //============================================================================
   std::optional<State> advance_within_query()
   {
     auto state = advance_within_resource();
@@ -616,6 +703,7 @@ struct NextStateGenerator
     return std::nullopt;
   }
 
+  //============================================================================
   std::optional<State> advance_assignments()
   {
     auto state = advance_within_query();
@@ -642,6 +730,7 @@ struct NextStateGenerator
     return std::nullopt;
   }
 
+  //============================================================================
   std::optional<State> advance_removals()
   {
     if(remove_resource_iter == start_state->_resource_schedules.end())
@@ -666,6 +755,7 @@ struct NextStateGenerator
     return new_state;
   }
 
+  //============================================================================
   std::optional<State> next_state() {
     if(mode == PotentialActionMode::ASSIGN_ITEMS)
     {
@@ -687,17 +777,20 @@ struct NextStateGenerator
     return std::nullopt;
   }
 
+  //============================================================================
   NextStateGenerator& operator++() {
     visiting_state = next_state();
     return *this;
   }
 
+  //============================================================================
   NextStateGenerator operator++(int) {
     NextStateGenerator r = *this;
     ++(*this);
     return r;
   }
-
+  
+  //============================================================================
   bool operator==(const NextStateGenerator& other)
   {
     if(mode == PotentialActionMode::END
@@ -708,12 +801,14 @@ struct NextStateGenerator
     return false;
   }
 
+  //============================================================================
   bool operator!=(const NextStateGenerator& other)
   {
     return !(*this == other);
   }
 };
 
+//==============================================================================
 NextStateGenerator State::begin()
 {
   NextStateGenerator gen;
@@ -744,6 +839,7 @@ NextStateGenerator State::begin()
   return gen;
 }
 
+//==============================================================================
 NextStateGenerator State::end()
 {
   NextStateGenerator gen;
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/StateDiff.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/StateDiff.hpp
index a80c2868..7e030da5 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/StateDiff.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/StateDiff.hpp
@@ -19,12 +19,15 @@
 #define RMF_TRAFFIC__RESERVATIONS__INTERNAL_STATEDIFF_HPP
 
 #include <unordered_map>
+#include <unordered_set>
+#include <queue>
 #include "State.hpp"
 
 namespace rmf_traffic {
 namespace reservations {
 
-/// Gives the difference between two states. *NOTE*: Assumes that bothe the
+//==============================================================================
+/// Gives the difference between two states. *NOTE*: Assumes that both the
 /// states have the same
 class StateDiff
 {
@@ -44,7 +47,8 @@ class StateDiff
     RequestId request_id;
     std::optional<Reservation> reservation;
   };
-
+  
+  //============================================================================
   /// Calculates the StateDiff required for one to go from State2 to State1.
   StateDiff(
     State& state1,
@@ -89,7 +93,7 @@ class StateDiff
         {
           // Request was not there in state2 therefore we assign it.
           auto time1 = state1._reservation_timings[reservation_id1];
-          auto res1 = state1._resource_schedules[resource].find(time1).second;
+          auto res1 = state1._resource_schedules[resource].find(time1)->second;
           Difference diff
           {
             DifferenceType::ASSIGN_RESERVATION,
@@ -119,13 +123,72 @@ class StateDiff
         );
       }
     }
+
+    _differences = toposort(state2);
   }
 
+  //============================================================================
+  /// Return the differences.
   std::vector<Difference>  differences() const
   {
     return _differences;
   }
 private:
+  //============================================================================
+  /// Use something akin to Kahn's algorithm to perform a topological sort of
+  /// the shifts. This ensures that during the rollout phase, if there is a
+  /// communications failure the system will remain in a usable state.
+  std::vector<Difference> toposort(State& state)
+  {
+    std::vector<Difference> result;
+    std::queue<std::size_t> unvisited;
+    State curr_state(state);
+    //Unassignments can go first, since they will never create a conflict.
+    for(std::size_t i = 0; i <_differences.size(); i++)
+    {
+      if(_differences[i].diff_type == DifferenceType::UNASSIGN_RESERVATION)
+      {
+        result.push_back(_differences[i]);
+        curr_state = curr_state.unassign_reservation(
+          _differences[i].participant_id,
+          _differences[i].request_id);
+      }
+      else
+      {
+        unvisited.push(i);
+      }
+    }
+
+    while(!unvisited.empty())
+    {
+      auto index = unvisited.front();
+      unvisited.pop();
+      auto diff = _differences[index];
+      auto reservation = diff.reservation;
+
+      if(!curr_state.check_if_conflicts(reservation.value()))
+      {
+        if(diff.diff_type == DifferenceType::SHIFT_RESERVATION)
+          curr_state = curr_state.unassign_reservation(
+            diff.participant_id,
+            diff.request_id
+          );
+
+        curr_state.assign_reservation(
+          diff.participant_id,
+          diff.request_id,
+          reservation.value()
+        );
+      }
+      else
+      {
+        unvisited.push(index);
+      }
+    }
+
+    return result;
+  }
+
   std::vector<Difference>  _differences;
 };
 
diff --git a/rmf_traffic/test/unit/reservations/test_Reservation.cpp b/rmf_traffic/test/unit/reservations/test_Reservation.cpp
new file mode 100644
index 00000000..e69de29b

From b79a9337da7012d366f03505303645b8fce83b56 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Sat, 19 Jun 2021 19:20:23 +0800
Subject: [PATCH 29/66] Add states... Something wrong with toposort

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../reservations/internal/ExecutionEngine.hpp |   9 +-
 .../reservations/internal/State.hpp           |  23 ++--
 .../unit/reservations/internal/test_State.cpp | 109 ++++++++++++++----
 3 files changed, 108 insertions(+), 33 deletions(-)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp
index 2144f7c2..bd55f0de 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp
@@ -195,7 +195,10 @@ class ExecutionEngine
   }
 
   //============================================================================
-  /// \brief Proposes the solution favoured by the optimizer and 
+  /// \brief Proposes the solution favoured by the optimizer and ratified
+  /// by all participants.
+  /// \param[in] optimizer - Instance of the optimizer.
+  /// \returns the StateDiff or a `std::nullopt` if no Solution is agreeable
   std::optional<StateDiff> get_favored_solution(
     GreedyBestFirstSearchOptimizer& optimizer)
   {
@@ -210,6 +213,10 @@ class ExecutionEngine
     return std::nullopt;
   }
 
+  //============================================================================
+  /// \brief Verifies individual proposals.
+  /// \param[in] impacted_reservations -  The proposed changes
+  /// \return true if all impacted participants agree otherwise return false
   bool verify_proposal(const StateDiff& impacted_reservations)
   {
     auto res = impacted_reservations.differences();
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index ad83a4cf..6344fb7e 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -274,29 +274,32 @@ class State
   }
 
   //============================================================================
-  bool check_if_conflicts(Reservation res)
+  bool check_if_conflicts(Reservation res) const
   {
     auto resource = res.resource_name();
-    auto start_slot =
-      _resource_schedules[resource].upper_bound(res.start_time());
+    auto sched = _resource_schedules.find(resource);
+    if(sched == _resource_schedules.end())
+      return false;
+
+    auto start_slot = sched->second.upper_bound(res.start_time());
 
     if(res.is_indefinite())
     {
-      if(_resource_schedules[resource].size() == 0)
-        return start_slot  != _resource_schedules[resource].end();
+      if(sched->second.size() == 0)
+        return start_slot  != sched->second.end();
       else if(
-        _resource_schedules[resource].rbegin()->second.actual_finish_time().has_value()
-        && _resource_schedules[resource].rbegin()->second.actual_finish_time() < res.start_time())
-        return start_slot  != _resource_schedules[resource].end();
+        sched->second.rbegin()->second.actual_finish_time().has_value()
+        && sched->second.rbegin()->second.actual_finish_time() < res.start_time())
+        return start_slot  != sched->second.end();
       else
         return true;
     }
 
     auto end_slot =
-      _resource_schedules[resource].upper_bound(
+      sched->second.upper_bound(
         res.actual_finish_time().value());
 
-    if(start_slot != _resource_schedules[resource].begin())
+    if(start_slot != sched->second.begin())
     {
       auto prev_iter = std::prev(start_slot);
       // Reservation definitely has end time
diff --git a/rmf_traffic/test/unit/reservations/internal/test_State.cpp b/rmf_traffic/test/unit/reservations/internal/test_State.cpp
index 0e1f1d5f..dccb2a4a 100644
--- a/rmf_traffic/test/unit/reservations/internal/test_State.cpp
+++ b/rmf_traffic/test/unit/reservations/internal/test_State.cpp
@@ -16,6 +16,7 @@
 */
 
 #include "../../../../src/rmf_traffic/reservations/internal/State.hpp"
+#include "../../../../src/rmf_traffic/reservations/internal/StateDiff.hpp"
 #include "../../../../src/rmf_traffic/reservations/internal/Heuristic.hpp"
 #include "../../../../src/rmf_traffic/reservations/internal/Optimizer.hpp"
 
@@ -27,12 +28,12 @@ using namespace std::chrono_literals;
 
 SCENARIO("A few reservations in a state")
 {
+  GIVEN("An empty state with two requests pending")
+  {
     auto queue = std::make_shared<RequestStore>();
     State start_state(queue);
-
     auto now = std::chrono::steady_clock::now();
     now -= now.time_since_epoch();
-
     auto request1_alt1 = ReservationRequest::make_request(
         "table_at_timbre",
         ReservationRequest::TimeRange::make_time_range(
@@ -41,15 +42,14 @@ SCENARIO("A few reservations in a state")
         ),
         {10s}
     );
-
     auto request1_alt2 = ReservationRequest::make_request(
         "table_at_koufu",
         ReservationRequest::TimeRange::make_time_range(
             now,
             now+10s
         ),
-        {10s}
-    );
+          {10s}
+      );
 
     auto request1 = std::vector{request1_alt1, request1_alt2};
     queue->enqueue_reservation(0, 0, 1, request1);
@@ -67,31 +67,96 @@ SCENARIO("A few reservations in a state")
     queue->enqueue_reservation(1, 0, 1, request2);
     start_state = start_state.add_request(1, 0);
 
-    std::vector<State> child_states;
-
-    for(auto next_state: start_state)
+    WHEN("We request the next possible states")
     {
-        //std::cout << "Next State:" <<std::endl;
-        //next_state.debug_state();
+      std::vector<State> child_states;
+
+      for(auto next_state: start_state)
+      {
         child_states.push_back(next_state);
+      }
+      THEN("There are three possible allocations")
+      {
+        REQUIRE(child_states.size() == 3);
+      }
+      THEN("The state difference between all child states is 1 add element")
+      {
+        for(auto child_state: child_states)
+        {
+          StateDiff diff(child_state, start_state);
+          auto steps = diff.differences();
+          REQUIRE(steps.size() == 1);
+          REQUIRE(steps[0].diff_type
+            == StateDiff::DifferenceType::ASSIGN_RESERVATION);
+        }
+      }
     }
 
-    REQUIRE(child_states.size() == 3);
-
-    std::cout << "Start State:" <<std::endl;
-    child_states[0].debug_state();
+    WHEN("We attempt to solve")
+    {
+      auto heuristic = std::make_shared<PriorityBasedScorer>(queue);
+      GreedyBestFirstSearchOptimizer opt(heuristic);
+      auto solutions = opt.optimize(start_state);
+      while(auto solution = solutions.next_solution())
+      {
+        solution->debug_state();
+      }
+    }
 
-    std::cout << "Child states: " <<std::endl;
-    for(auto next_state: child_states[0])
+    WHEN("Checking for conflict against another reservation")
     {
-        next_state.debug_state();
+      Reservation res = Reservation::make_reservation(
+        now,
+        "Hawaii",
+        {},
+        {}
+      );
+
+      THEN("There should be no conflict")
+      {
+        REQUIRE(start_state.check_if_conflicts(res) == false);
+      }
     }
+  }
+}
+
+SCENARIO("Given a single item allocated in a state")
+{
+  auto queue = std::make_shared<RequestStore>();
+  State start_state(queue);
+  auto now = std::chrono::steady_clock::now();
+  now -= now.time_since_epoch();
+  auto request1_alt1 = ReservationRequest::make_request(
+    "table_at_timbre",
+    ReservationRequest::TimeRange::make_time_range(
+        now,
+        now+10s
+    ),
+    {10s}
+  );
+
+  auto request1 = std::vector{request1_alt1};
+  queue->enqueue_reservation(0, 0, 1, request1);
+  start_state = start_state.add_request(0, 0);
 
-    auto heuristic = std::make_shared<PriorityBasedScorer>(queue);
-    GreedyBestFirstSearchOptimizer opt(heuristic);
-    auto solutions = opt.optimize(start_state);
-    while(auto solution = solutions.next_solution())
+  std::vector<State> next_state;
+  for(auto state: start_state)
+  {
+    next_state.push_back(state);
+  }
+
+  WHEN("Checking for overlapping indefinite reservation")
+  {
+    Reservation res = Reservation::make_reservation(
+      now,
+      "table_at_timbre",
+      {},
+      {}
+    );
+
+    THEN("check_if_conflicts")
     {
-        solution->debug_state();
+      REQUIRE(next_state[0].check_if_conflicts(res));
     }
+  }
 }
\ No newline at end of file

From 69d1ef1cbfa30693c29341198007b75a4d885a4d Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Sat, 19 Jun 2021 19:35:14 +0800
Subject: [PATCH 30/66] const correctness, disable toposort

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../src/rmf_traffic/reservations/internal/State.hpp      | 9 +++++----
 .../src/rmf_traffic/reservations/internal/StateDiff.hpp  | 2 +-
 2 files changed, 6 insertions(+), 5 deletions(-)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index 6344fb7e..f1ebebfb 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -82,7 +82,7 @@ class State
 
   NextStateGenerator begin();
   NextStateGenerator end();
-  
+
   //============================================================================
   /// Adds a request to the state
   State add_request(
@@ -318,11 +318,12 @@ class State
 
 
   //============================================================================
-  State unassign_reservation(ParticipantId pid, RequestId reqid)
+  State unassign_reservation(ParticipantId pid, RequestId reqid) const
   {
     State new_state(*this);
-    auto res_id = _reservation_assignments[pid][reqid];
-    auto resource = _reservation_resources[res_id];
+    auto res_id =
+      _reservation_assignments.find(pid)->second.find(reqid)->second;
+    auto resource = _reservation_resources.find(res_id)->second;
     new_state.unassign_reservation(res_id, resource);
   }
 
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/StateDiff.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/StateDiff.hpp
index 7e030da5..246968fd 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/StateDiff.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/StateDiff.hpp
@@ -124,7 +124,7 @@ class StateDiff
       }
     }
 
-    _differences = toposort(state2);
+    //_differences = toposort(state2);
   }
 
   //============================================================================

From da92ad8784fb72468750a2d1d37ce0d9696a9384 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Tue, 22 Jun 2021 13:10:15 +0800
Subject: [PATCH 31/66] Documentation

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 rmf_traffic/CMakeLists.txt                    |   2 +
 .../rmf_traffic/reservations/Database.hpp     |   8 +
 .../rmf_traffic/reservations/Participant.hpp  |  32 ++++
 .../reservations/ReservationRequest.hpp       |   2 +
 .../rmf_traffic/reservations/Writer.hpp       |  33 ++++
 .../src/rmf_traffic/reservations/Database.cpp |  45 +++++-
 .../reservations/internal/ExecutionEngine.hpp |  12 +-
 .../reservations/internal/RequestQueue.hpp    |   4 +-
 .../reservations/internal/RequestStore.hpp    |  11 ++
 .../reservations/internal/State.hpp           |   2 +-
 .../test/unit/reservations/test_Database.cpp  | 147 ++++++++++++++++++
 11 files changed, 282 insertions(+), 16 deletions(-)
 create mode 100644 rmf_traffic/test/unit/reservations/test_Database.cpp

diff --git a/rmf_traffic/CMakeLists.txt b/rmf_traffic/CMakeLists.txt
index d5516187..c5668b70 100644
--- a/rmf_traffic/CMakeLists.txt
+++ b/rmf_traffic/CMakeLists.txt
@@ -46,6 +46,7 @@ add_library(rmf_traffic SHARED
 
 find_package(ament_cmake_catch2 QUIET)
 find_package(rmf_cmake_uncrustify QUIET)
+find_package (Threads)
 if(BUILD_TESTING AND ament_cmake_catch2_FOUND AND rmf_cmake_uncrustify_FOUND)
   file(GLOB_RECURSE unit_test_srcs "test/*.cpp")
 
@@ -87,6 +88,7 @@ target_link_libraries(rmf_traffic
     rmf_utils::rmf_utils
   PRIVATE
     ${FCL_LIBRARIES}
+    Threads::Threads
 )
 
 target_include_directories(rmf_traffic
diff --git a/rmf_traffic/include/rmf_traffic/reservations/Database.hpp b/rmf_traffic/include/rmf_traffic/reservations/Database.hpp
index f8c29ddb..546f5723 100644
--- a/rmf_traffic/include/rmf_traffic/reservations/Database.hpp
+++ b/rmf_traffic/include/rmf_traffic/reservations/Database.hpp
@@ -26,14 +26,20 @@ namespace reservations {
 class Database: public Writer
 {
 public:
+  ///===========================================================================
+  /// Documentation inherited from Writer
   void register_participant(
     ParticipantId id,
     std::shared_ptr<Participant> participant) override;
 
+  ///===========================================================================
+  /// Documentation inherited from Writer
   void unregister_participant(
     ParticipantId id
   ) override;
 
+  ///===========================================================================
+  /// Documentation inherited from Writer
   void request_reservation(
     ParticipantId id,
     RequestId req,
@@ -41,6 +47,8 @@ class Database: public Writer
     int priority = 0
   ) override;
 
+  ///===========================================================================
+  /// Documentation inherited from Writer
   void cancel_request(ParticipantId id, RequestId req) override;
 
   Database();
diff --git a/rmf_traffic/include/rmf_traffic/reservations/Participant.hpp b/rmf_traffic/include/rmf_traffic/reservations/Participant.hpp
index a4f1d00d..2781d657 100644
--- a/rmf_traffic/include/rmf_traffic/reservations/Participant.hpp
+++ b/rmf_traffic/include/rmf_traffic/reservations/Participant.hpp
@@ -25,23 +25,55 @@ namespace reservations {
 
 using ParticipantId = uint64_t;
 
+///=============================================================================
+/// \brief This class is used to handle the asynchronous callbacks made by the
+/// database. In order to make a reservation you will need to inherit this class
 class Participant
 {
 public:
+  ///===========================================================================
+  /// \brief This method is called whenever a change is proposed by the
+  /// reservation system to the participant. It is also called when you first
+  /// make a request.
+  /// \param[in] id is the RequestId of the request that will be impacted.
+  /// \param[in] res is the Reservation that is being proposed by the
+  /// reservation system.
+  /// \return true if the request is acceptable, false otherwise.
   virtual bool request_proposal(
     RequestId id,
     Reservation& res
   ) = 0;
 
+  ///===========================================================================
+  /// \brief This is called when the solution to a request is confirmed. It will
+  /// only be called if all the participants that are impacted by the set of
+  /// changes needed have approved of it. Note: once this has been called it is
+  /// expected that the participant will comply.
+  /// \param[in] id the request id.
+  /// \param[in] res the response.
+  /// \return true if the request has been successfully confirmed, false
+  /// otherwise
   virtual bool request_confirmed(
     RequestId id,
     Reservation& res
   ) = 0;
 
+  ///===========================================================================
+  /// \brief This method is called whenever a change is proposed by the
+  /// reservation system to the participant to cancel the request made by it.
+  /// The request will still remain in the database, just that it will be
+  /// serviced at some point later if possible.
+  /// \param[in] id is the RequestId of the request that will be impacted.
+  /// \return true if the you agree to allow your request to be cancelled, false
+  /// otherwise.
   virtual bool unassign_request_confirmed(
     RequestId id
   ) = 0;
 
+  ///===========================================================================
+  /// \brief This method is called whenever a cancellation has been allowed.
+  /// \param[in] id is the RequestId of the request that will be impacted.
+  /// \return true if the request is acceptable, false otherwise.
   virtual bool unassign_request_proposal(
     RequestId id
   ) = 0;
diff --git a/rmf_traffic/include/rmf_traffic/reservations/ReservationRequest.hpp b/rmf_traffic/include/rmf_traffic/reservations/ReservationRequest.hpp
index c496f1d4..b3f18b32 100644
--- a/rmf_traffic/include/rmf_traffic/reservations/ReservationRequest.hpp
+++ b/rmf_traffic/include/rmf_traffic/reservations/ReservationRequest.hpp
@@ -28,6 +28,8 @@ namespace reservations {
 
 using RequestId = uint64_t;
 
+///=============================================================================
+/// \brief This class 
 class ReservationRequest
 {
 
diff --git a/rmf_traffic/include/rmf_traffic/reservations/Writer.hpp b/rmf_traffic/include/rmf_traffic/reservations/Writer.hpp
index 510f7129..267f7f18 100644
--- a/rmf_traffic/include/rmf_traffic/reservations/Writer.hpp
+++ b/rmf_traffic/include/rmf_traffic/reservations/Writer.hpp
@@ -25,17 +25,47 @@
 namespace rmf_traffic {
 namespace reservations {
 
+///=============================================================================
+// Writer class 
 class Writer
 {
 public:
+
+  ///===========================================================================
+  /// Registers a participant
+  /// \param[in] id the Participant's ID. If a participant registers with an
+  /// already pre-existing id then the id should get overwritten.
+  /// (Warning: currently overwrites are not threadsafe and should be avoided.
+  /// This needs to be fixed before merging)
+  /// \param[in] participant a shared pointer pointing to the participant
+  /// handler
   virtual void register_participant(
     ParticipantId id,
     std::shared_ptr<Participant> participant) = 0;
 
+  ///===========================================================================
+  /// Removes a participant.
+  /// \param[in] id the participant's id
   virtual void unregister_participant(
     ParticipantId id
   ) = 0;
 
+  ///===========================================================================
+  /// Request a reservation.
+  /// \param[in] id the participant requesting the reservation
+  /// \param[in] req the request id. This should be unique for each request and
+  /// needs to be tracked by the client. (Warning to be fixed before merge:
+  /// If a request is duplicated there is undefined behaviour. This needs to
+  /// change to log an error or throw an exception...)
+  /// \param[in] request_options alternatives which can service this request. 
+  /// For instance, if we would like to reserve a charger we can send in a 
+  /// ReservationRequest for all the chargers. Suppose Charger1 is nearest and
+  /// Charger4 is farthest and we prefer near chargers then we would send in a
+  /// list of requests like so {Charger1, Charger2, Charger3, Charger4}. The
+  /// system should assign Charger1 lowest cost.
+  /// \param[in] priority The priority to give this reservation. Higher priority
+  /// requests will be given preference when solving/optimizing for the best
+  /// solution.
   virtual void request_reservation(
     ParticipantId id,
     RequestId req,
@@ -43,6 +73,9 @@ class Writer
     int priority = 0
   ) = 0;
 
+  /// Cancels a request.
+  /// \param[in] id - the participant cancelling the request
+  /// \param[req] req -the request id
   virtual void cancel_request(ParticipantId id, RequestId req) = 0;
 };
 }
diff --git a/rmf_traffic/src/rmf_traffic/reservations/Database.cpp b/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
index 59b16173..ae760997 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
@@ -16,8 +16,9 @@
 */
 
 #include <rmf_traffic/reservations/Database.hpp>
+#include "internal/ExecutionEngine.hpp"
 #include "internal/ParticipantStore.hpp"
-#include "internal/RequestStore.hpp"
+#include "internal/RequestQueue.hpp"
 #include "internal/State.hpp"
 namespace rmf_traffic {
 namespace reservations {
@@ -26,7 +27,9 @@ class Database::Implementation
 {
 public:
   Implementation():
-    _request_queue(std::make_shared<RequestStore>())
+    _request_queue(std::make_shared<RequestQueue>()),
+    _participant_store(std::make_shared<ParticipantStore>()),
+    _execution_engine(_request_queue, _participant_store)
   {
 
   }
@@ -35,14 +38,24 @@ class Database::Implementation
     ParticipantId id,
     std::shared_ptr<Participant> participant)
   {
-    _participant_store.add_participant(id, participant);
+    /// TODO(arjo): add some type of locking mechanism that will prevent
+    /// overwrites or otherwise use the queue.
+    _participant_store->add_participant(id, participant);
   }
 
   void unregister_participant(
     ParticipantId id
   )
   {
-    _participant_store.remove_participant(id);
+    RequestQueue::Action action
+    {
+      RequestQueue::ActionType::REMOVE_PARTICIPANT,
+      id,
+      0,
+      {},
+      0
+    };
+    _request_queue->add_action(action);
   }
 
   void request_reservation(
@@ -51,16 +64,32 @@ class Database::Implementation
     std::vector<ReservationRequest>& request_options,
     int priority)
   {
-
+    RequestQueue::Action add_action{
+      RequestQueue::ActionType::ADD,
+      id,
+      req,
+      request_options,
+      priority
+    };
+    _request_queue->add_action(add_action);
   }
 
   void cancel_request(ParticipantId id, RequestId req)
   {
-    
+    RequestQueue::Action action
+    {
+      RequestQueue::ActionType::REMOVE,
+      id,
+      req,
+      {},
+      0
+    };
+    _request_queue->add_action(action);
   }
 private:
-  ParticipantStore _participant_store;
-  std::shared_ptr<RequestStore> _request_queue;
+  std::shared_ptr<ParticipantStore> _participant_store;
+  std::shared_ptr<RequestQueue> _request_queue;
+  ExecutionEngine _execution_engine;
 };
 
 void Database::register_participant(
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp
index bd55f0de..ef014dac 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp
@@ -37,6 +37,7 @@ namespace reservations {
 /// of the protocol is implemented
 class ExecutionEngine
 {
+public:
   //============================================================================
   /// \brief Constructor. Starts an execution thread engine which consumes
   /// commands of the incoming participants.
@@ -49,10 +50,10 @@ class ExecutionEngine
     std::shared_ptr<ParticipantStore> pstore):
       _request_queue(request_queue),
       _participant_store(pstore),
-      _execution_thread(&ExecutionEngine::execute, this),
       _curr_state(std::make_shared<RequestStore>())
   {
-    //Do nothing
+    _execution_thread =
+      std::make_shared<std::thread>(&ExecutionEngine::execute, this);
   }
 
   //============================================================================
@@ -71,8 +72,8 @@ class ExecutionEngine
     };
     _request_queue->add_action(terminate_req);
 
-    if(_execution_thread.joinable())
-      _execution_thread.join();
+    if(_execution_thread->joinable())
+      _execution_thread->join();
   }
 
 private:
@@ -109,6 +110,7 @@ class ExecutionEngine
         _curr_state = _curr_state.remove_participant(
           element.action.participant_id,
           element.request_store);
+        _participant_store->remove_participant(element.action.participant_id);
       }
 
       auto heuristic = std::make_shared<PriorityBasedScorer>(
@@ -249,7 +251,7 @@ class ExecutionEngine
     return true;
   }
 
-  std::thread _execution_thread;
+  std::shared_ptr<std::thread> _execution_thread;
   std::shared_ptr<RequestQueue> _request_queue;
   std::shared_ptr<ParticipantStore> _participant_store;
   State _curr_state;
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp
index 3abb42ee..c4160d98 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp
@@ -53,7 +53,7 @@ class RequestQueue
     ActionType type;
     ParticipantId participant_id;
     RequestId request_id;
-    std::vector<ReservationRequest>& request_options;
+    std::vector<ReservationRequest> request_options;
     int priority;
   };
   struct QueueElement
@@ -74,7 +74,7 @@ class RequestQueue
             return std::make_shared<RequestStore>();
           else
             return std::make_shared<RequestStore>(
-              _request_store_queue.back().request_store.get()
+              *_request_store_queue.back().request_store.get()
             );
         }();
 
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp
index 4eba3e50..8f6b0192 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp
@@ -41,6 +41,17 @@ class RequestStore
     return _reservation_info[pid][req];
   }
 
+  RequestStore()
+  {
+    //Do nothing
+  }
+
+  RequestStore(const rmf_traffic::reservations::RequestStore& other):
+    _reservation_info(other._reservation_info)
+  {
+    //Do nothing
+  }
+
   void enqueue_reservation(
     ParticipantId pid,
     RequestId req,
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index f1ebebfb..7ef0e351 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -793,7 +793,7 @@ struct NextStateGenerator
     ++(*this);
     return r;
   }
-  
+
   //============================================================================
   bool operator==(const NextStateGenerator& other)
   {
diff --git a/rmf_traffic/test/unit/reservations/test_Database.cpp b/rmf_traffic/test/unit/reservations/test_Database.cpp
new file mode 100644
index 00000000..1ba2262f
--- /dev/null
+++ b/rmf_traffic/test/unit/reservations/test_Database.cpp
@@ -0,0 +1,147 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * 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.
+ *
+*/
+
+#include <rmf_utils/catch.hpp>
+#include <rmf_traffic/reservations/Database.hpp>
+#include <rmf_traffic/reservations/Reservation.hpp>
+
+#include <mutex>
+#include <thread>
+
+using namespace rmf_traffic;
+using namespace rmf_traffic::reservations;
+using namespace std::chrono_literals;
+
+///=============================================================================
+/// Emulates a dumb participant
+class SimpleParticipant: public Participant
+{
+public:
+  SimpleParticipant(
+    ParticipantId pid,
+    std::shared_ptr<Database> db,
+    int priority = 1):
+    _pid(pid),
+    _db(db),
+    _reqid(0),
+    _priority(priority)
+  {
+  }
+
+  bool request_proposal(
+    RequestId id,
+    Reservation& res
+  ) override
+  {
+    _received_confirmation[id] = true;
+    _proposed[id] = res;
+    return true;
+  }
+
+  bool request_confirmed(
+    RequestId id,
+    Reservation& res
+  ) override
+  {
+    return true;
+  }
+
+  bool unassign_request_confirmed(
+    RequestId id
+  ) override
+  {
+    return true;
+  }
+
+  bool unassign_request_proposal(
+    RequestId id
+  ) override
+  {
+    _received_cancellation[id] = true;
+    return true;
+  }
+
+  RequestId make_request_blocking(
+    std::vector<ReservationRequest>& request_options
+  )
+  {
+    _received_confirmation[_reqid] = false;
+    _received_cancellation[_reqid] = false;
+    _db->request_reservation(
+      _pid,
+      _reqid,
+      request_options,
+      _priority
+    );
+
+    // Busy wait... Bad but oh well...this is a unit test
+    while(
+      !_received_confirmation[_reqid] && 
+      !_received_cancellation[_reqid] ){}
+
+    return _reqid++;
+  }
+
+  std::optional<Reservation> get_proposal(ReservationId rid)
+  {
+    return _proposed[rid];
+  }
+
+private:
+  ParticipantId _pid;
+  RequestId _reqid;
+  std::shared_ptr<Database> _db;
+  std::unordered_map<RequestId, bool> _received_confirmation;
+  std::unordered_map<RequestId, bool> _received_cancellation;
+  std::unordered_map<RequestId, std::optional<Reservation>> _proposed;
+  int _priority;
+};
+
+SCENARIO("Test database behaviour at the start of our lord, the saviour, UNIX")
+{
+  auto now = std::chrono::steady_clock::now();
+  now -= now.time_since_epoch();
+  GIVEN("An empty database and a participant")
+  {
+    std::shared_ptr<Database> database = std::make_shared<Database>();
+    std::shared_ptr<SimpleParticipant> participant1 =
+      std::make_shared<SimpleParticipant>(0, database);
+    database->register_participant(1, participant1);
+
+    WHEN("the participant makes a request")
+    {
+      auto request1_alt1 = ReservationRequest::make_request(
+        "table_at_timbre",
+        ReservationRequest::TimeRange::make_time_range(
+            now,
+            now+10s
+        ),
+        {10s}
+      );
+      
+      auto request1 = std::vector{request1_alt1};
+      auto req_id = participant1->make_request_blocking(request1);
+      auto prop = participant1->get_proposal(req_id);
+
+      THEN("There should be a successful proposal")
+      {
+        REQUIRE(prop.has_value());
+        REQUIRE(prop.value().resource_name() == "table_at_timbre");
+      }
+    }
+  }
+}
\ No newline at end of file

From 70d6bb5834c561c8c982f191d0e822db9891489f Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Wed, 23 Jun 2021 09:12:06 +0800
Subject: [PATCH 32/66] fix failing test case

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../reservations/internal/ExecutionEngine.hpp  |  4 ++++
 .../unit/reservations/internal/test_State.cpp  | 18 +++++++++++++++++-
 .../test/unit/reservations/test_Database.cpp   |  8 ++++----
 3 files changed, 25 insertions(+), 5 deletions(-)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp
index ef014dac..1673fffb 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp
@@ -225,6 +225,10 @@ class ExecutionEngine
     for(auto diff: res)
     {
       auto impacted_participant = diff.participant_id;
+
+      assert (
+        _participant_store->get_participant(impacted_participant).has_value());
+
       auto participant =
         _participant_store->get_participant(impacted_participant)
           .value();
diff --git a/rmf_traffic/test/unit/reservations/internal/test_State.cpp b/rmf_traffic/test/unit/reservations/internal/test_State.cpp
index dccb2a4a..a140597c 100644
--- a/rmf_traffic/test/unit/reservations/internal/test_State.cpp
+++ b/rmf_traffic/test/unit/reservations/internal/test_State.cpp
@@ -120,6 +120,7 @@ SCENARIO("A few reservations in a state")
   }
 }
 
+// TODO: Increase test cases here to cover all edge cases
 SCENARIO("Given a single item allocated in a state")
 {
   auto queue = std::make_shared<RequestStore>();
@@ -154,9 +155,24 @@ SCENARIO("Given a single item allocated in a state")
       {}
     );
 
-    THEN("check_if_conflicts")
+    THEN("check_if_conflicts returns true")
     {
       REQUIRE(next_state[0].check_if_conflicts(res));
     }
   }
+
+  WHEN("Checking for overlapping indefinite reservation")
+  {
+    Reservation res = Reservation::make_reservation(
+      now,
+      "table_at_koufu",
+      {},
+      {}
+    );
+
+    THEN("check_if_conflicts returns false")
+    {
+      REQUIRE(!next_state[0].check_if_conflicts(res));
+    }
+  }
 }
\ No newline at end of file
diff --git a/rmf_traffic/test/unit/reservations/test_Database.cpp b/rmf_traffic/test/unit/reservations/test_Database.cpp
index 1ba2262f..51ed2def 100644
--- a/rmf_traffic/test/unit/reservations/test_Database.cpp
+++ b/rmf_traffic/test/unit/reservations/test_Database.cpp
@@ -19,8 +19,7 @@
 #include <rmf_traffic/reservations/Database.hpp>
 #include <rmf_traffic/reservations/Reservation.hpp>
 
-#include <mutex>
-#include <thread>
+#include <iostream>
 
 using namespace rmf_traffic;
 using namespace rmf_traffic::reservations;
@@ -120,7 +119,7 @@ SCENARIO("Test database behaviour at the start of our lord, the saviour, UNIX")
     std::shared_ptr<Database> database = std::make_shared<Database>();
     std::shared_ptr<SimpleParticipant> participant1 =
       std::make_shared<SimpleParticipant>(0, database);
-    database->register_participant(1, participant1);
+    database->register_participant(0, participant1);
 
     WHEN("the participant makes a request")
     {
@@ -132,9 +131,10 @@ SCENARIO("Test database behaviour at the start of our lord, the saviour, UNIX")
         ),
         {10s}
       );
-      
+
       auto request1 = std::vector{request1_alt1};
       auto req_id = participant1->make_request_blocking(request1);
+      std::cout << "Got reqid" << req_id<< std::endl;
       auto prop = participant1->get_proposal(req_id);
 
       THEN("There should be a successful proposal")

From a61e46c712c628fe9485058ec43c02dce3483ecc Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Wed, 23 Jun 2021 09:36:10 +0800
Subject: [PATCH 33/66] style

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../src/rmf_traffic/reservations/Database.cpp |   8 +-
 .../rmf_traffic/reservations/Reservation.cpp  |  33 +--
 .../reservations/ReservationRequest.cpp       |  14 +-
 .../reservations/internal/ExecutionEngine.hpp |  53 ++---
 .../reservations/internal/Heuristic.hpp       |   8 +-
 .../reservations/internal/Optimizer.hpp       |  38 ++--
 .../internal/ParticipantStore.hpp             |   8 +-
 .../reservations/internal/RequestQueue.hpp    |  12 +-
 .../reservations/internal/RequestStore.hpp    |  54 ++---
 .../reservations/internal/State.hpp           | 212 +++++++++---------
 .../reservations/internal/StateDiff.hpp       |  30 +--
 11 files changed, 244 insertions(+), 226 deletions(-)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/Database.cpp b/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
index ae760997..d7f0315e 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
@@ -26,8 +26,8 @@ namespace reservations {
 class Database::Implementation
 {
 public:
-  Implementation():
-    _request_queue(std::make_shared<RequestQueue>()),
+  Implementation()
+  : _request_queue(std::make_shared<RequestQueue>()),
     _participant_store(std::make_shared<ParticipantStore>()),
     _execution_engine(_request_queue, _participant_store)
   {
@@ -120,8 +120,8 @@ void Database::cancel_request(ParticipantId id, RequestId req)
 {
   _pimpl->cancel_request(id, req);
 }
-Database::Database():
-  _pimpl(rmf_utils::make_impl<Implementation>())
+Database::Database()
+: _pimpl(rmf_utils::make_impl<Implementation>())
 {
 }
 
diff --git a/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp b/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp
index b81d107f..48182f8c 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp
@@ -28,8 +28,8 @@ class Reservation::Implementation
   ReservationId _res_id;
   std::string _resource_name;
   rmf_traffic::Time _start_time;
-  std::optional<rmf_traffic::Duration>  _duration;
-  std::optional<rmf_traffic::Time>  _finish_time;
+  std::optional<rmf_traffic::Duration> _duration;
+  std::optional<rmf_traffic::Time> _finish_time;
 };
 
 //=============================================================================
@@ -53,7 +53,7 @@ const std::optional<rmf_traffic::Time> Reservation::finish_time() const
 //=============================================================================
 void Reservation::set_actual_finish_time(rmf_traffic::Time dur)
 {
-  if(dur < _pimpl->_start_time)
+  if (dur < _pimpl->_start_time)
     _pimpl->_start_time = dur;
 
   _pimpl->_finish_time = dur;
@@ -79,14 +79,15 @@ Reservation Reservation::propose_new_start_time(rmf_traffic::Time start_time)
 //=============================================================================
 const std::optional<rmf_traffic::Time> Reservation::actual_finish_time() const
 {
-  if(is_indefinite()) return std::nullopt;
+  if (is_indefinite())
+    return std::nullopt;
 
-  if(_pimpl->_finish_time.has_value() && _pimpl->_duration.has_value())
+  if (_pimpl->_finish_time.has_value() && _pimpl->_duration.has_value())
   {
-    return {std::max(_pimpl->_start_time + *_pimpl->_duration, 
-      *_pimpl->_finish_time)};
+    return {std::max(_pimpl->_start_time + *_pimpl->_duration,
+        *_pimpl->_finish_time)};
   }
-  else if(_pimpl->_finish_time.has_value())
+  else if (_pimpl->_finish_time.has_value())
   {
     return {std::max(_pimpl->_start_time, *_pimpl->_finish_time)};
   }
@@ -117,19 +118,21 @@ ReservationId Reservation::reservation_id() const
 //=============================================================================
 bool Reservation::conflicts_with(const Reservation& other) const
 {
-  if(other.start_time() < this->start_time())
+  if (other.start_time() < this->start_time())
   {
-    if(!other.is_indefinite()
+    if (!other.is_indefinite()
       && other.actual_finish_time() <= this->start_time())
       return true;
-    else return false;
+    else
+      return false;
   }
   else
   {
-    if(!this->is_indefinite()
+    if (!this->is_indefinite()
       && this->actual_finish_time() <= other.start_time())
       return true;
-    else return false;
+    else
+      return false;
   }
 }
 
@@ -171,8 +174,8 @@ Reservation Reservation::make_reservation(
 }
 
 //=============================================================================
-Reservation::Reservation():
-  _pimpl(rmf_utils::make_impl<Implementation>())
+Reservation::Reservation()
+: _pimpl(rmf_utils::make_impl<Implementation>())
 {
   // do nothing
 }
diff --git a/rmf_traffic/src/rmf_traffic/reservations/ReservationRequest.cpp b/rmf_traffic/src/rmf_traffic/reservations/ReservationRequest.cpp
index 0221ea2e..3defe178 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/ReservationRequest.cpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/ReservationRequest.cpp
@@ -29,12 +29,12 @@ class ReservationRequest::TimeRange::Implementation
 
 
 const std::optional<Time>
-  ReservationRequest::TimeRange::lower_bound() const
+ReservationRequest::TimeRange::lower_bound() const
 {
   return _pimpl->_lower_bound;
 }
 const std::optional<Time>
-  ReservationRequest::TimeRange::upper_bound() const
+ReservationRequest::TimeRange::upper_bound() const
 {
   return _pimpl->_upper_bound;
 }
@@ -50,8 +50,8 @@ ReservationRequest::TimeRange::make_time_range(
   return tr;
 }
 
-ReservationRequest::TimeRange::TimeRange():
-  _pimpl(rmf_utils::make_impl<Implementation>())
+ReservationRequest::TimeRange::TimeRange()
+: _pimpl(rmf_utils::make_impl<Implementation>())
 {
 
 }
@@ -67,7 +67,7 @@ class ReservationRequest::Implementation
 
 //=============================================================================
 const std::optional<ReservationRequest::TimeRange>
-    ReservationRequest::start_time() const
+ReservationRequest::start_time() const
 {
   return _pimpl->_start;
 }
@@ -113,8 +113,8 @@ ReservationRequest ReservationRequest::make_request(
 }
 
 //=============================================================================
-ReservationRequest::ReservationRequest():
-  _pimpl(rmf_utils::make_impl<Implementation>())
+ReservationRequest::ReservationRequest()
+: _pimpl(rmf_utils::make_impl<Implementation>())
 {
 }
 
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp
index 1673fffb..b8dcce68 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp
@@ -47,10 +47,10 @@ class ExecutionEngine
   ///   of participants.
   ExecutionEngine(
     std::shared_ptr<RequestQueue> request_queue,
-    std::shared_ptr<ParticipantStore> pstore):
-      _request_queue(request_queue),
-      _participant_store(pstore),
-      _curr_state(std::make_shared<RequestStore>())
+    std::shared_ptr<ParticipantStore> pstore)
+  : _request_queue(request_queue),
+    _participant_store(pstore),
+    _curr_state(std::make_shared<RequestStore>())
   {
     _execution_thread =
       std::make_shared<std::thread>(&ExecutionEngine::execute, this);
@@ -72,7 +72,7 @@ class ExecutionEngine
     };
     _request_queue->add_action(terminate_req);
 
-    if(_execution_thread->joinable())
+    if (_execution_thread->joinable())
       _execution_thread->join();
   }
 
@@ -82,14 +82,14 @@ class ExecutionEngine
   /// comes in the current_state is read and updated to contain the request.
   void execute()
   {
-    while(true)
+    while (true)
     {
       auto element = _request_queue->deque();
 
-      if(element.action.type == RequestQueue::ActionType::TERMINATE_STREAM)
+      if (element.action.type == RequestQueue::ActionType::TERMINATE_STREAM)
         return;
 
-      if(element.action.type == RequestQueue::ActionType::ADD)
+      if (element.action.type == RequestQueue::ActionType::ADD)
       {
         _curr_state = _curr_state.add_request(
           element.action.participant_id,
@@ -97,7 +97,7 @@ class ExecutionEngine
           element.request_store
         );
       }
-      else if(element.action.type == RequestQueue::ActionType::REMOVE)
+      else if (element.action.type == RequestQueue::ActionType::REMOVE)
       {
         _curr_state = _curr_state.remove_request(
           element.action.participant_id,
@@ -105,7 +105,8 @@ class ExecutionEngine
           element.request_store
         );
       }
-      else if(element.action.type == RequestQueue::ActionType::REMOVE_PARTICIPANT)
+      else if (element.action.type ==
+        RequestQueue::ActionType::REMOVE_PARTICIPANT)
       {
         _curr_state = _curr_state.remove_participant(
           element.action.participant_id,
@@ -118,7 +119,7 @@ class ExecutionEngine
 
       GreedyBestFirstSearchOptimizer optimizer(heuristic);
       auto favored_solution = get_favored_solution(optimizer);
-      if(!favored_solution.has_value())
+      if (!favored_solution.has_value())
         continue;
 
       rollout(favored_solution.value());
@@ -133,14 +134,14 @@ class ExecutionEngine
   /// we will retry optimization upon the arrival of the next request.
   void rollout(const StateDiff& changes)
   {
-    for(auto change: changes.differences())
+    for (auto change: changes.differences())
     {
       assert(
         _participant_store->get_participant(change.participant_id).has_value());
 
       auto participant =
         _participant_store->get_participant(change.participant_id).value();
-      if(change.diff_type == StateDiff::DifferenceType::SHIFT_RESERVATION)
+      if (change.diff_type == StateDiff::DifferenceType::SHIFT_RESERVATION)
       {
         assert(change.reservation.has_value());
 
@@ -148,7 +149,7 @@ class ExecutionEngine
           change.request_id,
           change.reservation.value());
 
-        if(result == false)
+        if (result == false)
           return;
 
         _curr_state = _curr_state.unassign_reservation(
@@ -161,7 +162,7 @@ class ExecutionEngine
           change.reservation.value()
         );
       }
-      else if(
+      else if (
         change.diff_type == StateDiff::DifferenceType::ASSIGN_RESERVATION)
       {
         assert(change.reservation.has_value());
@@ -170,7 +171,7 @@ class ExecutionEngine
           change.request_id,
           change.reservation.value());
 
-        if(result == false)
+        if (result == false)
           return;
 
         _curr_state.assign_reservation(
@@ -179,14 +180,14 @@ class ExecutionEngine
           change.reservation.value()
         );
       }
-      else if(
+      else if (
         change.diff_type == StateDiff::DifferenceType::UNASSIGN_RESERVATION)
       {
         assert(!change.reservation.has_value());
         auto result =
           participant->unassign_request_confirmed(change.request_id);
 
-        if(result == false)
+        if (result == false)
           return;
 
         _curr_state = _curr_state.unassign_reservation(
@@ -205,10 +206,10 @@ class ExecutionEngine
     GreedyBestFirstSearchOptimizer& optimizer)
   {
     auto solutions = optimizer.optimize(_curr_state);
-    while(auto solution = solutions.next_solution())
+    while (auto solution = solutions.next_solution())
     {
       StateDiff impacted_reservations(solution.value(), _curr_state);
-      if(verify_proposal(impacted_reservations))
+      if (verify_proposal(impacted_reservations))
         return {impacted_reservations};
     }
 
@@ -222,25 +223,25 @@ class ExecutionEngine
   bool verify_proposal(const StateDiff& impacted_reservations)
   {
     auto res = impacted_reservations.differences();
-    for(auto diff: res)
+    for (auto diff: res)
     {
       auto impacted_participant = diff.participant_id;
 
-      assert (
+      assert(
         _participant_store->get_participant(impacted_participant).has_value());
 
       auto participant =
         _participant_store->get_participant(impacted_participant)
-          .value();
+        .value();
 
-      if(diff.diff_type == StateDiff::DifferenceType::ASSIGN_RESERVATION
+      if (diff.diff_type == StateDiff::DifferenceType::ASSIGN_RESERVATION
         || diff.diff_type == StateDiff::DifferenceType::SHIFT_RESERVATION)
       {
         auto result = participant->request_proposal(
           diff.request_id,
           diff.reservation.value()
         );
-        if(!result)
+        if (!result)
           return false;
       }
       else
@@ -248,7 +249,7 @@ class ExecutionEngine
         auto result = participant->unassign_request_proposal(
           diff.request_id
         );
-        if(!result)
+        if (!result)
           return false;
       }
     }
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp
index 71388c49..2e11c415 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp
@@ -29,18 +29,18 @@ class Heuristic
   virtual float score(const State& state) = 0;
 };
 
-class PriorityBasedScorer: public Heuristic
+class PriorityBasedScorer : public Heuristic
 {
 public:
-  PriorityBasedScorer(std::shared_ptr<RequestStore> queue) :
-    _queue(queue)
+  PriorityBasedScorer(std::shared_ptr<RequestStore> queue)
+  : _queue(queue)
   {
 
   }
   float score(const State& state) override
   {
     auto score = 0;
-    for(auto [part_id, req_id]: state.unassigned())
+    for (auto [part_id, req_id]: state.unassigned())
     {
       // TODO: Incorporate
       score += _queue->get_request_info(part_id, req_id).priority;
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/Optimizer.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/Optimizer.hpp
index 888e2755..120a5ff0 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/Optimizer.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/Optimizer.hpp
@@ -24,10 +24,12 @@
 namespace rmf_traffic {
 namespace reservations {
 
-struct StateHash {
-   size_t operator() (const State &state) const {
-     return state.hash();
-   }
+struct StateHash
+{
+  size_t operator()(const State& state) const
+  {
+    return state.hash();
+  }
 };
 class GreedyBestFirstSearchOptimizer
 {
@@ -36,12 +38,12 @@ class GreedyBestFirstSearchOptimizer
   {
     std::shared_ptr<Heuristic> _heuristic;
   public:
-    CustomComparator(std::shared_ptr<Heuristic> heuristic):
-      _heuristic(heuristic)
+    CustomComparator(std::shared_ptr<Heuristic> heuristic)
+    : _heuristic(heuristic)
     {
 
     }
-    bool operator() (const State& e1, const State& e2) const
+    bool operator()(const State& e1, const State& e2) const
     {
       return _heuristic->score(e1) < _heuristic->score(e2);
     }
@@ -50,8 +52,8 @@ class GreedyBestFirstSearchOptimizer
   {
   public:
     Solution(State& state,
-      std::shared_ptr<Heuristic> heuristic):
-      _last_solution(state),
+      std::shared_ptr<Heuristic> heuristic)
+    : _last_solution(state),
       _pq(heuristic),
       _all_solutions(heuristic),
       _heuristic(heuristic)
@@ -63,17 +65,17 @@ class GreedyBestFirstSearchOptimizer
     std::optional<State> next_solution()
     {
       _blacklist.insert(_last_solution);
-      while(!_pq.empty())
+      while (!_pq.empty())
       {
         auto state = _pq.top();
         _visited.insert(state);
         _pq.pop();
-        for(auto next_state: state)
+        for (auto next_state: state)
         {
-          if(_visited.count(next_state) != 0)
+          if (_visited.count(next_state) != 0)
             continue;
 
-          if(_heuristic->score(next_state) == 0 &&
+          if (_heuristic->score(next_state) == 0 &&
             _blacklist.count(next_state) == 0)
           {
             _last_solution = next_state;
@@ -84,12 +86,12 @@ class GreedyBestFirstSearchOptimizer
           _all_solutions.push(next_state);
         }
       }
-      while(!_all_solutions.empty())
+      while (!_all_solutions.empty())
       {
         auto res = _all_solutions.top();
         _last_solution = res;
         _all_solutions.pop();
-        if(_blacklist.count(res) != 0)
+        if (_blacklist.count(res) != 0)
           return res;
       }
       return std::nullopt;
@@ -99,12 +101,12 @@ class GreedyBestFirstSearchOptimizer
     std::unordered_set<State, StateHash> _blacklist;
     std::priority_queue<State, std::vector<State>, CustomComparator> _pq;
     std::priority_queue<State, std::vector<State>, CustomComparator>
-      _all_solutions;
+    _all_solutions;
     std::shared_ptr<Heuristic> _heuristic;
     State _last_solution;
   };
-  GreedyBestFirstSearchOptimizer(std::shared_ptr<Heuristic> heuristic):
-    _heuristic(heuristic)
+  GreedyBestFirstSearchOptimizer(std::shared_ptr<Heuristic> heuristic)
+  : _heuristic(heuristic)
   {
   }
 
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/ParticipantStore.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/ParticipantStore.hpp
index ba437fa0..ca313740 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/ParticipantStore.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/ParticipantStore.hpp
@@ -41,22 +41,22 @@ class ParticipantStore
   void remove_participant(ParticipantId id)
   {
     auto participant = _participants.find(id);
-    if(participant == _participants.end())
+    if (participant == _participants.end())
       return;
     _participants.erase(participant);
   }
 
   std::optional<std::shared_ptr<Participant>>
-    get_participant(ParticipantId id)
+  get_participant(ParticipantId id)
   {
     auto participant = _participants.find(id);
-    if(participant == _participants.end())
+    if (participant == _participants.end())
       return std::nullopt;
     return {participant->second};
   }
 private:
   std::unordered_map<ParticipantId, std::shared_ptr<Participant>>
-    _participants;
+  _participants;
 };
 
 }
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp
index c4160d98..6833a437 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp
@@ -68,17 +68,17 @@ class RequestQueue
       std::lock_guard lock(_mutex);
 
       std::shared_ptr<RequestStore>
-        req_store = [=]() -> std::shared_ptr<RequestStore>
+      req_store = [=]() -> std::shared_ptr<RequestStore>
         {
-          if(_request_store_queue.empty())
+          if (_request_store_queue.empty())
             return std::make_shared<RequestStore>();
           else
             return std::make_shared<RequestStore>(
               *_request_store_queue.back().request_store.get()
             );
-        }();
+        } ();
 
-      if(action.type == ActionType::ADD)
+      if (action.type == ActionType::ADD)
       {
         req_store->enqueue_reservation(
           action.participant_id,
@@ -86,7 +86,7 @@ class RequestQueue
           action.priority,
           action.request_options);
       }
-      else if(action.type == ActionType::REMOVE)
+      else if (action.type == ActionType::REMOVE)
       {
         req_store->cancel(action.participant_id, action.request_id);
       }
@@ -102,7 +102,7 @@ class RequestQueue
   QueueElement deque()
   {
     std::unique_lock loc(_mutex);
-    _cond.wait(loc, [=](){return !_request_store_queue.empty();});
+    _cond.wait(loc, [=]() {return !_request_store_queue.empty();});
     auto front = _request_store_queue.front();
     _request_store_queue.pop_front();
     loc.unlock();
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp
index 8f6b0192..2d4f7b81 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp
@@ -28,7 +28,8 @@ namespace reservations {
 class RequestStore
 {
 public:
-  struct RequestInfo {
+  struct RequestInfo
+  {
     int priority;
     std::vector<ReservationRequest> request_options;
   };
@@ -46,8 +47,8 @@ class RequestStore
     //Do nothing
   }
 
-  RequestStore(const rmf_traffic::reservations::RequestStore& other):
-    _reservation_info(other._reservation_info)
+  RequestStore(const rmf_traffic::reservations::RequestStore& other)
+  : _reservation_info(other._reservation_info)
   {
     //Do nothing
   }
@@ -60,19 +61,20 @@ class RequestStore
   )
   {
     _reservation_info[pid].insert({
-      req,
-      {
-        priority,
-        std::move(reservation)
-      }
-    });
+        req,
+        {
+          priority,
+          std::move(reservation)
+        }
+      });
   }
 
   void erase_participant_requests(
     ParticipantId pid
-  ){
+  )
+  {
     auto entry = _reservation_info.find(pid);
-    if(entry == _reservation_info.end())
+    if (entry == _reservation_info.end())
       return;
     _reservation_info.erase(entry);
   }
@@ -85,55 +87,55 @@ class RequestStore
   bool satisfies(ReservationRequest& req, Reservation reservation)
   {
     //Check upper bound
-    if(req.start_time()->lower_bound().has_value())
+    if (req.start_time()->lower_bound().has_value())
     {
-      if(reservation.start_time() < req.start_time()->lower_bound().value())
+      if (reservation.start_time() < req.start_time()->lower_bound().value())
       {
         return false;
       }
     }
 
     //Check upper bound
-    if(req.start_time()->upper_bound().has_value())
+    if (req.start_time()->upper_bound().has_value())
     {
-      if(reservation.start_time() > req.start_time()->upper_bound().value())
+      if (reservation.start_time() > req.start_time()->upper_bound().value())
       {
         return false;
       }
     }
 
-    if(req.duration().has_value())
+    if (req.duration().has_value())
     {
-      if(reservation.is_indefinite())
+      if (reservation.is_indefinite())
       {
         return false;
       }
-      if(req.duration().value()
+      if (req.duration().value()
         > *reservation.actual_finish_time() - reservation.start_time())
       {
         return false;
       }
     }
 
-    if(req.finish_time().has_value())
+    if (req.finish_time().has_value())
     {
-      if(reservation.is_indefinite())
+      if (reservation.is_indefinite())
       {
         return false;
       }
-      if(req.finish_time().value() > *reservation.actual_finish_time())
+      if (req.finish_time().value() > *reservation.actual_finish_time())
       {
         return false;
       }
     }
 
-    if((req.is_indefinite() && !reservation.is_indefinite())
+    if ((req.is_indefinite() && !reservation.is_indefinite())
       || (!req.is_indefinite() && reservation.is_indefinite()))
     {
       return false;
     }
 
-    if(req.resource_name() != reservation.resource_name())
+    if (req.resource_name() != reservation.resource_name())
     {
       return false;
     }
@@ -146,11 +148,11 @@ class RequestStore
     RequestId reqid,
     Reservation& res)
   {
-    for(std::size_t i = 0;
+    for (std::size_t i = 0;
       i < _reservation_info[pid][reqid].request_options.size();
       i++)
     {
-      if(satisfies(_reservation_info[pid][reqid].request_options[i], res))
+      if (satisfies(_reservation_info[pid][reqid].request_options[i], res))
         return i;
     }
     return std::nullopt;
@@ -158,7 +160,7 @@ class RequestStore
 private:
   std::unordered_map<ParticipantId,
     std::unordered_map<RequestId, RequestInfo>
-    >  _reservation_info;
+  > _reservation_info;
 };
 
 }
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index 7ef0e351..a99c96c3 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -35,21 +35,23 @@ struct NextStateGenerator;
 class StateDiff;
 
 // Ripped from https://stackoverflow.com/questions/2590677/how-do-i-combine-hash-values-in-c0x
-template <class T>
+template<class T>
 inline void hash_combine(std::size_t& seed, const T& v)
 {
-    std::hash<T> hasher;
-    seed ^= hasher(v) + 0x9e3779b9 + (seed<<6) + (seed>>2);
+  std::hash<T> hasher;
+  seed ^= hasher(v) + 0x9e3779b9 + (seed<<6) + (seed>>2);
 }
 //TODO: Currrently uses a lot of copying. In future we should use a parent based
 //hierarchy
 class State
 {
-  struct pair_hash {
-    template <class T1, class T2>
-    std::size_t operator () (const std::pair<T1,T2> &p) const {
-      auto h1 = std::hash<T1>{}(p.first);
-      auto h2 = std::hash<T2>{}(p.second);
+  struct pair_hash
+  {
+    template<class T1, class T2>
+    std::size_t operator()(const std::pair<T1, T2>& p) const
+    {
+      auto h1 = std::hash<T1>{} (p.first);
+      auto h2 = std::hash<T2>{} (p.second);
       return h1 ^ h2;
     }
   };
@@ -69,13 +71,13 @@ class State
   using ReservationTimings = std::unordered_map<ReservationId, Time>;
   using ReservationResources = std::unordered_map<ReservationId, std::string>;
   using ReservationRequestId = std::unordered_map<ReservationId,
-    ReservationAssignmentIndex>;
+      ReservationAssignmentIndex>;
   using UnassignedSet = std::unordered_set<
     std::pair<ParticipantId, RequestId>, pair_hash>;
 
   //============================================================================
-  State(std::shared_ptr<RequestStore> queue) :
-    _queue(queue)
+  State(std::shared_ptr<RequestStore> queue)
+  : _queue(queue)
   {
     //Do nothing
   }
@@ -114,14 +116,14 @@ class State
     State new_state(*this);
     auto req = new_state._unassigned.find({pid, reqid});
 
-    if(req != new_state._unassigned.end())
+    if (req != new_state._unassigned.end())
     {
       new_state._unassigned.erase(req);
       return new_state;
     }
     auto reservation_id = new_state._reservation_assignments[pid][reqid];
     auto findings = new_state._reservation_timings.find(reservation_id);
-    if(findings == new_state._reservation_timings.end())
+    if (findings == new_state._reservation_timings.end())
     {
       // Non existant pid-reqid pair
       return new_state;
@@ -145,14 +147,14 @@ class State
     State new_state(*this);
     auto req = new_state._unassigned.find({pid, reqid});
 
-    if(req != new_state._unassigned.end())
+    if (req != new_state._unassigned.end())
     {
       new_state._unassigned.erase(req);
       return new_state;
     }
     auto reservation_id = new_state._reservation_assignments[pid][reqid];
     auto findings = new_state._reservation_timings.find(reservation_id);
-    if(findings == new_state._reservation_timings.end())
+    if (findings == new_state._reservation_timings.end())
     {
       // Non existant pid-reqid pair
       return new_state;
@@ -176,7 +178,7 @@ class State
   {
     //TODO(arjo): Lots of unessecary copying
     State new_state(*this);
-    for(auto [request, reservation]: _reservation_assignments[pid])
+    for (auto [request, reservation]: _reservation_assignments[pid])
     {
       new_state = new_state.remove_request(pid, request, store);
     }
@@ -187,30 +189,30 @@ class State
   void debug_state()
   {
     std::cout << "________________________" <<std::endl;
-    for(auto &[resource, schedule]: _resource_schedules)
+    for (auto&[resource, schedule]: _resource_schedules)
     {
       std::cout << "Resource: " << resource << std::endl;
-      for(auto &[time, reservation]: schedule)
+      for (auto&[time, reservation]: schedule)
       {
         std::cout << "\t======" << std::endl;
         std::cout << "\tReservation: "
-          << reservation.reservation_id() << std::endl;
+                  << reservation.reservation_id() << std::endl;
         std::cout << "\tStart time: " <<
           std::chrono::duration_cast<std::chrono::seconds>(
             time.time_since_epoch()
           ).count() << std::endl;
-        if(!reservation.is_indefinite())
+        if (!reservation.is_indefinite())
           std::cout << "\tEnd time: " <<
             std::chrono::duration_cast<std::chrono::seconds>(
               reservation.actual_finish_time()->time_since_epoch()
             ).count()
-            << std::endl;
+                    << std::endl;
         std::cout << "\tParticipant: " <<
           _reservation_request_ids[reservation.reservation_id()].participant
-          << std::endl;
+                  << std::endl;
         std::cout << "\tRequestID: " <<
           _reservation_request_ids[reservation.reservation_id()].reqid
-          << std::endl;
+                  << std::endl;
       }
     }
   }
@@ -229,15 +231,15 @@ class State
     std::set<std::string> resources;
 
     // Make sure the order of iteration is correct
-    for(auto &[resource, schedule]: _resource_schedules)
+    for (auto&[resource, schedule]: _resource_schedules)
     {
       resources.insert(resource);
     }
 
-    for(auto resource: resources)
+    for (auto resource: resources)
     {
       auto schedule = _resource_schedules.find(resource)->second;
-      for(auto &[time, reservation]: schedule)
+      for (auto&[time, reservation]: schedule)
       {
         hash_combine(seed, time.time_since_epoch().count());
         auto val =
@@ -278,18 +280,19 @@ class State
   {
     auto resource = res.resource_name();
     auto sched = _resource_schedules.find(resource);
-    if(sched == _resource_schedules.end())
+    if (sched == _resource_schedules.end())
       return false;
 
     auto start_slot = sched->second.upper_bound(res.start_time());
 
-    if(res.is_indefinite())
+    if (res.is_indefinite())
     {
-      if(sched->second.size() == 0)
+      if (sched->second.size() == 0)
         return start_slot  != sched->second.end();
-      else if(
+      else if (
         sched->second.rbegin()->second.actual_finish_time().has_value()
-        && sched->second.rbegin()->second.actual_finish_time() < res.start_time())
+        && sched->second.rbegin()->second.actual_finish_time() <
+        res.start_time())
         return start_slot  != sched->second.end();
       else
         return true;
@@ -297,9 +300,9 @@ class State
 
     auto end_slot =
       sched->second.upper_bound(
-        res.actual_finish_time().value());
+      res.actual_finish_time().value());
 
-    if(start_slot != sched->second.begin())
+    if (start_slot != sched->second.begin())
     {
       auto prev_iter = std::prev(start_slot);
       // Reservation definitely has end time
@@ -316,7 +319,6 @@ class State
   }
 
 
-
   //============================================================================
   State unassign_reservation(ParticipantId pid, RequestId reqid) const
   {
@@ -334,19 +336,19 @@ class State
     Reservation res)
   {
     auto index = _queue->satisfies(part_id, req_id, res);
-    if(!index.has_value())
+    if (!index.has_value())
     {
       return false;
     }
 
-    if(res.is_indefinite())
+    if (res.is_indefinite())
     {
       //infinite reservation. Must be last
       auto next_reservation =
         _resource_schedules[res.resource_name()].lower_bound(
-          res.start_time()
+        res.start_time()
         );
-      if(next_reservation != _resource_schedules[res.resource_name()].end())
+      if (next_reservation != _resource_schedules[res.resource_name()].end())
       {
         return false;
       }
@@ -354,19 +356,22 @@ class State
     else
     {
       // Get the insertion point
-      auto next_reservation = _resource_schedules[res.resource_name()].lower_bound(
+      auto next_reservation =
+        _resource_schedules[res.resource_name()].lower_bound(
         res.start_time()
-      );
+        );
 
-      if(next_reservation != _resource_schedules[res.resource_name()].end()
+      if (next_reservation != _resource_schedules[res.resource_name()].end()
         && res.actual_finish_time().has_value()
-        && next_reservation->second.start_time() < res.actual_finish_time().value())
+        && next_reservation->second.start_time() <
+        res.actual_finish_time().value())
       {
         auto ok = push_back(
           res.resource_name(),
           next_reservation,
-          next_reservation->second.start_time() - res.actual_finish_time().value());
-        if(!ok)
+          next_reservation->second.start_time() -
+          res.actual_finish_time().value());
+        if (!ok)
         {
           return false;
         }
@@ -375,24 +380,24 @@ class State
     }
 
     _resource_schedules[res.resource_name()].insert({
-      res.start_time(), res});
+        res.start_time(), res});
     _reservation_timings.insert({res.reservation_id(), res.start_time()});
     _reservation_assignments[part_id][req_id] = res.reservation_id();
     _reservation_resources[res.reservation_id()] = res.resource_name();
     _reservation_request_ids.insert({
-      res.reservation_id(),
-      {
-        part_id,
-        req_id,
-        index.value()
-      }});
+        res.reservation_id(),
+        {
+          part_id,
+          req_id,
+          index.value()
+        }});
     _unassigned.erase({part_id, req_id});
     return true;
   }
 
   //============================================================================
-  State(const State& other) :
-    _resource_schedules(other._resource_schedules),
+  State(const State& other)
+  : _resource_schedules(other._resource_schedules),
     _unassigned(other._unassigned),
     _reservation_timings(other._reservation_timings),
     _queue(other._queue),
@@ -408,12 +413,12 @@ class State
   //============================================================================
   void unassign_reservation(ReservationId res_id, std::string resource_name)
   {
-    auto time =  _reservation_timings[res_id];
+    auto time = _reservation_timings[res_id];
     _reservation_timings.erase(res_id);
     _reservation_resources.erase(res_id);
     auto request = _reservation_request_ids[res_id];
 
-    _unassigned.insert({request.participant , request.reqid});
+    _unassigned.insert({request.participant, request.reqid});
     _reservation_assignments[request.participant].erase(request.reqid);
     _reservation_request_ids.erase(res_id);
     _resource_schedules[resource_name].erase(time);
@@ -433,14 +438,15 @@ class State
     // TODO: This is extremely inefficient. Do in-place.
     std::map<Time, ReservationId> new_times;
     auto gap_left = dur;
-    while(
+    while (
       res_iter != _resource_schedules[resource].end()
       && prev_iter->second.actual_finish_time().has_value()
       && gap_left.count() > 0)
     {
       auto new_time = res_iter->first + gap_left;
-      auto gap = res_iter->first - prev_iter->second.actual_finish_time().value();
-      if(gap < gap_left)
+      auto gap = res_iter->first -
+        prev_iter->second.actual_finish_time().value();
+      if (gap < gap_left)
         gap_left -= gap;
       else
         gap_left -= gap_left;
@@ -449,13 +455,13 @@ class State
       std::next(res_iter);
     }
 
-    for(auto iter = new_times.rbegin(); iter != new_times.rend(); iter++)
+    for (auto iter = new_times.rbegin(); iter != new_times.rend(); iter++)
     {
-      bool ok  = shift_reservation_start_time(
+      bool ok = shift_reservation_start_time(
         resource,
         iter->second,
         iter->first);
-      if(!ok) return false;
+      if (!ok) return false;
     }
 
     return true;
@@ -470,13 +476,13 @@ class State
     auto original_time = _reservation_timings[res_id];
     auto new_reservation =
       _resource_schedules[resource][original_time]
-        .propose_new_start_time(new_time);
+      .propose_new_start_time(new_time);
     auto request_info = _reservation_request_ids[res_id];
     auto index = _queue->satisfies(
       request_info.participant,
       request_info.reqid,
       new_reservation);
-    if(!index.has_value())
+    if (!index.has_value())
     {
       return false;
     }
@@ -506,7 +512,7 @@ struct NextStateGenerator
   //TODO: RAW_POINTER-FOO
   State* start_state;
   std::optional<State> visiting_state;
-  
+
   //Potential Actions
   enum PotentialActionMode
   {
@@ -532,24 +538,25 @@ struct NextStateGenerator
   using value_type = State;
   using pointer = State*;
   using reference = State;
-  using iterator_category= std::input_iterator_tag;
+  using iterator_category = std::input_iterator_tag;
 
   //============================================================================
-  State operator*() const {
+  State operator*() const
+  {
     return *visiting_state;
   }
 
   //============================================================================
   State::ResourceSchedule::const_iterator
-    get_search_start(const ReservationRequest& req)
+  get_search_start(const ReservationRequest& req)
   {
-    if(req.start_time().has_value() &&
+    if (req.start_time().has_value() &&
       req.start_time().value().lower_bound().has_value())
     {
       return start_state->
         _resource_schedules[req.resource_name()]
         .lower_bound(
-          req.start_time().value().lower_bound().value());
+        req.start_time().value().lower_bound().value());
     }
     else
     {
@@ -559,15 +566,15 @@ struct NextStateGenerator
 
   //============================================================================
   State::ResourceSchedule::const_iterator
-    get_search_end(const ReservationRequest& req)
+  get_search_end(const ReservationRequest& req)
   {
-    if(req.start_time().has_value() &&
+    if (req.start_time().has_value() &&
       req.start_time().value().lower_bound().has_value())
     {
       return start_state->
         _resource_schedules[req.resource_name()]
         .upper_bound(
-          req.start_time().value().upper_bound().value());
+        req.start_time().value().upper_bound().value());
     }
     else
     {
@@ -578,7 +585,7 @@ struct NextStateGenerator
   //============================================================================
   std::optional<State> advance_within_resource()
   {
-    if(proceed_next_resource)
+    if (proceed_next_resource)
       return std::nullopt;
 
     State next_state(*start_state);
@@ -593,9 +600,10 @@ struct NextStateGenerator
 
     proceed_next_resource = false;
 
-    while(insertion_point_iter != start_state->_resource_schedules[current_request.resource_name()].end())
+    while (insertion_point_iter !=
+      start_state->_resource_schedules[current_request.resource_name()].end())
     {
-      if(insertion_point_iter->second.actual_finish_time().has_value())
+      if (insertion_point_iter->second.actual_finish_time().has_value())
       {
         auto reservation = Reservation::make_reservation(
           insertion_point_iter->second.actual_finish_time().value(),
@@ -610,7 +618,7 @@ struct NextStateGenerator
           reservation
         );
         insertion_point_iter++;
-        if(found_slot)
+        if (found_slot)
         {
           return next_state;
         }
@@ -623,10 +631,9 @@ struct NextStateGenerator
 
     proceed_next_resource = true;
 
-    if(
+    if (
       current_request.start_time().has_value() &&
-      current_request.start_time()->lower_bound().has_value()
-    )
+      current_request.start_time()->lower_bound().has_value())
     {
       auto reservation = Reservation::make_reservation(
         current_request.start_time().value().lower_bound().value(),
@@ -639,7 +646,7 @@ struct NextStateGenerator
         req_id,
         reservation
       );
-      if(found_slot)
+      if (found_slot)
       {
         return next_state;
       }
@@ -662,7 +669,7 @@ struct NextStateGenerator
         req_id,
         reservation
       );
-      if(found_slot)
+      if (found_slot)
       {
         return next_state;
       }
@@ -677,7 +684,7 @@ struct NextStateGenerator
   std::optional<State> advance_within_query()
   {
     auto state = advance_within_resource();
-    if(state.has_value())
+    if (state.has_value())
     {
       return state;
     }
@@ -688,7 +695,7 @@ struct NextStateGenerator
       part_id,
       req_id).request_options.size();
 
-    while(request_index_iter < num_alternative)
+    while (request_index_iter < num_alternative)
     {
       // Reset resource iterators
       auto curr_request = start_state->_queue->get_request_info(part_id, req_id)
@@ -697,7 +704,7 @@ struct NextStateGenerator
       insertion_point_end = get_search_end(curr_request);
       proceed_next_resource = false;
       auto state = advance_within_resource();
-      if(state.has_value())
+      if (state.has_value())
       {
         return state;
       }
@@ -711,12 +718,12 @@ struct NextStateGenerator
   std::optional<State> advance_assignments()
   {
     auto state = advance_within_query();
-    if(state.has_value())
+    if (state.has_value())
     {
       return state;
     }
     unassigned_iter++;
-    while(unassigned_iter != start_state->_unassigned.end())
+    while (unassigned_iter != start_state->_unassigned.end())
     {
       auto [part_id, req_id] = *unassigned_iter;
       request_index_iter = 0;
@@ -726,7 +733,7 @@ struct NextStateGenerator
       insertion_point_end = get_search_end(curr_request);
       proceed_next_resource = false;
       auto state = advance_within_query();
-      if(state.has_value())
+      if (state.has_value())
       {
         return state;
       }
@@ -737,14 +744,14 @@ struct NextStateGenerator
   //============================================================================
   std::optional<State> advance_removals()
   {
-    if(remove_resource_iter == start_state->_resource_schedules.end())
+    if (remove_resource_iter == start_state->_resource_schedules.end())
     {
       return std::nullopt;
     }
-    while(remove_iter == remove_resource_iter->second.end())
+    while (remove_iter == remove_resource_iter->second.end())
     {
       remove_resource_iter++;
-      if(remove_resource_iter == start_state->_resource_schedules.end())
+      if (remove_resource_iter == start_state->_resource_schedules.end())
       {
         return std::nullopt;
       }
@@ -760,11 +767,12 @@ struct NextStateGenerator
   }
 
   //============================================================================
-  std::optional<State> next_state() {
-    if(mode == PotentialActionMode::ASSIGN_ITEMS)
+  std::optional<State> next_state()
+  {
+    if (mode == PotentialActionMode::ASSIGN_ITEMS)
     {
       auto assignment = advance_assignments();
-      if(assignment.has_value())
+      if (assignment.has_value())
       {
         visiting_state = assignment;
         return assignment;
@@ -773,7 +781,7 @@ struct NextStateGenerator
     }
     auto removals = advance_removals();
 
-    if(removals.has_value())
+    if (removals.has_value())
     {
       return removals;
     }
@@ -782,13 +790,15 @@ struct NextStateGenerator
   }
 
   //============================================================================
-  NextStateGenerator& operator++() {
+  NextStateGenerator& operator++()
+  {
     visiting_state = next_state();
     return *this;
   }
 
   //============================================================================
-  NextStateGenerator operator++(int) {
+  NextStateGenerator operator++(int)
+  {
     NextStateGenerator r = *this;
     ++(*this);
     return r;
@@ -797,8 +807,8 @@ struct NextStateGenerator
   //============================================================================
   bool operator==(const NextStateGenerator& other)
   {
-    if(mode == PotentialActionMode::END
-    && other.mode == PotentialActionMode::END)
+    if (mode == PotentialActionMode::END
+      && other.mode == PotentialActionMode::END)
     {
       return true;
     }
@@ -819,7 +829,7 @@ NextStateGenerator State::begin()
   gen.mode = NextStateGenerator::PotentialActionMode::ASSIGN_ITEMS;
   gen.start_state = this;
 
-  if(_unassigned.size() > 0)
+  if (_unassigned.size() > 0)
   {
     auto current_request = _queue->get_request_info(
       _unassigned.begin()->first,
@@ -831,11 +841,11 @@ NextStateGenerator State::begin()
   }
   gen.remove_resource_iter = _resource_schedules.begin();
 
-  if(_resource_schedules.size() > 0)
+  if (_resource_schedules.size() > 0)
   {
     gen.remove_iter = gen.remove_resource_iter->second.begin();
   }
-  else if(_unassigned.size() == 0)
+  else if (_unassigned.size() == 0)
   {
     return end();
   }
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/StateDiff.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/StateDiff.hpp
index 246968fd..d1f56652 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/StateDiff.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/StateDiff.hpp
@@ -47,7 +47,7 @@ class StateDiff
     RequestId request_id;
     std::optional<Reservation> reservation;
   };
-  
+
   //============================================================================
   /// Calculates the StateDiff required for one to go from State2 to State1.
   StateDiff(
@@ -56,16 +56,16 @@ class StateDiff
   )
   {
     // TODO(arjo): currently method is O(nlogn). It is possible to implement
-    // in O(n) using a two pointer method, just trickier. Ideally we should 
+    // in O(n) using a two pointer method, just trickier. Ideally we should
     // do this.
 
     // Check for reservation shuffling/shifting and assignment
-    for(auto [participant, requests]: state1._reservation_assignments)
+    for (auto [participant, requests]: state1._reservation_assignments)
     {
-      for(auto [request, reservation_id1]: requests)
+      for (auto [request, reservation_id1]: requests)
       {
         auto resource = state1._reservation_resources[reservation_id1];
-        if(
+        if (
           state2._reservation_assignments.count(participant) != 0
           && state2._reservation_assignments[participant].count(request) != 0)
         {
@@ -76,7 +76,7 @@ class StateDiff
           auto res1 = state1._resource_schedules[resource].find(time1)->second;
           auto res2 = state2._resource_schedules[resource].find(time2)->second;
 
-          if(res1 != res2)
+          if (res1 != res2)
           {
             // Request was shifted from state 2
             Difference diff
@@ -108,9 +108,9 @@ class StateDiff
     }
 
     //Check for unassigning
-    for(auto [participant, request]: state1.unassigned())
+    for (auto [participant, request]: state1.unassigned())
     {
-      if(state2._reservation_assignments.count(participant) > 0
+      if (state2._reservation_assignments.count(participant) > 0
         && state2._reservation_assignments[participant].count(request) > 0)
       {
         _differences.push_back(
@@ -129,7 +129,7 @@ class StateDiff
 
   //============================================================================
   /// Return the differences.
-  std::vector<Difference>  differences() const
+  std::vector<Difference> differences() const
   {
     return _differences;
   }
@@ -144,9 +144,9 @@ class StateDiff
     std::queue<std::size_t> unvisited;
     State curr_state(state);
     //Unassignments can go first, since they will never create a conflict.
-    for(std::size_t i = 0; i <_differences.size(); i++)
+    for (std::size_t i = 0; i < _differences.size(); i++)
     {
-      if(_differences[i].diff_type == DifferenceType::UNASSIGN_RESERVATION)
+      if (_differences[i].diff_type == DifferenceType::UNASSIGN_RESERVATION)
       {
         result.push_back(_differences[i]);
         curr_state = curr_state.unassign_reservation(
@@ -159,16 +159,16 @@ class StateDiff
       }
     }
 
-    while(!unvisited.empty())
+    while (!unvisited.empty())
     {
       auto index = unvisited.front();
       unvisited.pop();
       auto diff = _differences[index];
       auto reservation = diff.reservation;
 
-      if(!curr_state.check_if_conflicts(reservation.value()))
+      if (!curr_state.check_if_conflicts(reservation.value()))
       {
-        if(diff.diff_type == DifferenceType::SHIFT_RESERVATION)
+        if (diff.diff_type == DifferenceType::SHIFT_RESERVATION)
           curr_state = curr_state.unassign_reservation(
             diff.participant_id,
             diff.request_id
@@ -189,7 +189,7 @@ class StateDiff
     return result;
   }
 
-  std::vector<Difference>  _differences;
+  std::vector<Difference> _differences;
 };
 
 }

From b3c08fe8180b11adca55b5c078929a476981e4e1 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Wed, 23 Jun 2021 09:36:46 +0800
Subject: [PATCH 34/66] style

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 rmf_traffic/include/rmf_traffic/reservations/Database.hpp   | 4 ++--
 .../include/rmf_traffic/reservations/ReservationRequest.hpp | 4 ++--
 rmf_traffic/include/rmf_traffic/reservations/Writer.hpp     | 6 +++---
 3 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/rmf_traffic/include/rmf_traffic/reservations/Database.hpp b/rmf_traffic/include/rmf_traffic/reservations/Database.hpp
index 546f5723..77bedc6e 100644
--- a/rmf_traffic/include/rmf_traffic/reservations/Database.hpp
+++ b/rmf_traffic/include/rmf_traffic/reservations/Database.hpp
@@ -23,7 +23,7 @@
 namespace rmf_traffic {
 namespace reservations {
 
-class Database: public Writer
+class Database : public Writer
 {
 public:
   ///===========================================================================
@@ -55,7 +55,7 @@ class Database: public Writer
 
   class Implementation;
 private:
-  rmf_utils::impl_ptr<Implementation>  _pimpl;
+  rmf_utils::impl_ptr<Implementation> _pimpl;
 };
 
 }
diff --git a/rmf_traffic/include/rmf_traffic/reservations/ReservationRequest.hpp b/rmf_traffic/include/rmf_traffic/reservations/ReservationRequest.hpp
index b3f18b32..6a3459f1 100644
--- a/rmf_traffic/include/rmf_traffic/reservations/ReservationRequest.hpp
+++ b/rmf_traffic/include/rmf_traffic/reservations/ReservationRequest.hpp
@@ -29,7 +29,7 @@ namespace reservations {
 using RequestId = uint64_t;
 
 ///=============================================================================
-/// \brief This class 
+/// \brief This class
 class ReservationRequest
 {
 
@@ -73,7 +73,7 @@ class ReservationRequest
 
 private:
   ReservationRequest();
-  rmf_utils::impl_ptr<Implementation>  _pimpl;
+  rmf_utils::impl_ptr<Implementation> _pimpl;
 };
 }
 }
diff --git a/rmf_traffic/include/rmf_traffic/reservations/Writer.hpp b/rmf_traffic/include/rmf_traffic/reservations/Writer.hpp
index 267f7f18..6498c42d 100644
--- a/rmf_traffic/include/rmf_traffic/reservations/Writer.hpp
+++ b/rmf_traffic/include/rmf_traffic/reservations/Writer.hpp
@@ -26,7 +26,7 @@ namespace rmf_traffic {
 namespace reservations {
 
 ///=============================================================================
-// Writer class 
+// Writer class
 class Writer
 {
 public:
@@ -57,8 +57,8 @@ class Writer
   /// needs to be tracked by the client. (Warning to be fixed before merge:
   /// If a request is duplicated there is undefined behaviour. This needs to
   /// change to log an error or throw an exception...)
-  /// \param[in] request_options alternatives which can service this request. 
-  /// For instance, if we would like to reserve a charger we can send in a 
+  /// \param[in] request_options alternatives which can service this request.
+  /// For instance, if we would like to reserve a charger we can send in a
   /// ReservationRequest for all the chargers. Suppose Charger1 is nearest and
   /// Charger4 is farthest and we prefer near chargers then we would send in a
   /// list of requests like so {Charger1, Charger2, Charger3, Charger4}. The

From 9c0101238feb9fdb9c52129a9c8811ee1128606c Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Wed, 23 Jun 2021 10:10:02 +0800
Subject: [PATCH 35/66] fix divergences in test state

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../unit/reservations/internal/test_State.cpp | 50 +++++++++----------
 .../test/unit/reservations/test_Database.cpp  | 16 +++---
 2 files changed, 33 insertions(+), 33 deletions(-)

diff --git a/rmf_traffic/test/unit/reservations/internal/test_State.cpp b/rmf_traffic/test/unit/reservations/internal/test_State.cpp
index a140597c..00d64c1c 100644
--- a/rmf_traffic/test/unit/reservations/internal/test_State.cpp
+++ b/rmf_traffic/test/unit/reservations/internal/test_State.cpp
@@ -35,33 +35,33 @@ SCENARIO("A few reservations in a state")
     auto now = std::chrono::steady_clock::now();
     now -= now.time_since_epoch();
     auto request1_alt1 = ReservationRequest::make_request(
-        "table_at_timbre",
-        ReservationRequest::TimeRange::make_time_range(
-            now,
-            now+10s
-        ),
-        {10s}
+      "table_at_timbre",
+      ReservationRequest::TimeRange::make_time_range(
+        now,
+        now+10s
+      ),
+      {10s}
     );
     auto request1_alt2 = ReservationRequest::make_request(
-        "table_at_koufu",
-        ReservationRequest::TimeRange::make_time_range(
-            now,
-            now+10s
-        ),
-          {10s}
-      );
+      "table_at_koufu",
+      ReservationRequest::TimeRange::make_time_range(
+        now,
+        now+10s
+      ),
+      {10s}
+    );
 
     auto request1 = std::vector{request1_alt1, request1_alt2};
     queue->enqueue_reservation(0, 0, 1, request1);
     start_state = start_state.add_request(0, 0);
 
     auto request2_alt1 = ReservationRequest::make_request(
-        "table_at_timbre",
-        ReservationRequest::TimeRange::make_time_range(
-            now,
-            now+10s
-        ),
-        {10s}
+      "table_at_timbre",
+      ReservationRequest::TimeRange::make_time_range(
+        now,
+        now+10s
+      ),
+      {10s}
     );
     auto request2 = std::vector{request2_alt1};
     queue->enqueue_reservation(1, 0, 1, request2);
@@ -71,7 +71,7 @@ SCENARIO("A few reservations in a state")
     {
       std::vector<State> child_states;
 
-      for(auto next_state: start_state)
+      for (auto next_state: start_state)
       {
         child_states.push_back(next_state);
       }
@@ -81,7 +81,7 @@ SCENARIO("A few reservations in a state")
       }
       THEN("The state difference between all child states is 1 add element")
       {
-        for(auto child_state: child_states)
+        for (auto child_state: child_states)
         {
           StateDiff diff(child_state, start_state);
           auto steps = diff.differences();
@@ -97,7 +97,7 @@ SCENARIO("A few reservations in a state")
       auto heuristic = std::make_shared<PriorityBasedScorer>(queue);
       GreedyBestFirstSearchOptimizer opt(heuristic);
       auto solutions = opt.optimize(start_state);
-      while(auto solution = solutions.next_solution())
+      while (auto solution = solutions.next_solution())
       {
         solution->debug_state();
       }
@@ -130,8 +130,8 @@ SCENARIO("Given a single item allocated in a state")
   auto request1_alt1 = ReservationRequest::make_request(
     "table_at_timbre",
     ReservationRequest::TimeRange::make_time_range(
-        now,
-        now+10s
+      now,
+      now+10s
     ),
     {10s}
   );
@@ -141,7 +141,7 @@ SCENARIO("Given a single item allocated in a state")
   start_state = start_state.add_request(0, 0);
 
   std::vector<State> next_state;
-  for(auto state: start_state)
+  for (auto state: start_state)
   {
     next_state.push_back(state);
   }
diff --git a/rmf_traffic/test/unit/reservations/test_Database.cpp b/rmf_traffic/test/unit/reservations/test_Database.cpp
index 51ed2def..18cb050d 100644
--- a/rmf_traffic/test/unit/reservations/test_Database.cpp
+++ b/rmf_traffic/test/unit/reservations/test_Database.cpp
@@ -27,14 +27,14 @@ using namespace std::chrono_literals;
 
 ///=============================================================================
 /// Emulates a dumb participant
-class SimpleParticipant: public Participant
+class SimpleParticipant : public Participant
 {
 public:
   SimpleParticipant(
     ParticipantId pid,
     std::shared_ptr<Database> db,
-    int priority = 1):
-    _pid(pid),
+    int priority = 1)
+  : _pid(pid),
     _db(db),
     _reqid(0),
     _priority(priority)
@@ -88,9 +88,9 @@ class SimpleParticipant: public Participant
     );
 
     // Busy wait... Bad but oh well...this is a unit test
-    while(
-      !_received_confirmation[_reqid] && 
-      !_received_cancellation[_reqid] ){}
+    while (
+      !_received_confirmation[_reqid] &&
+      !_received_cancellation[_reqid]) {}
 
     return _reqid++;
   }
@@ -126,8 +126,8 @@ SCENARIO("Test database behaviour at the start of our lord, the saviour, UNIX")
       auto request1_alt1 = ReservationRequest::make_request(
         "table_at_timbre",
         ReservationRequest::TimeRange::make_time_range(
-            now,
-            now+10s
+          now,
+          now+10s
         ),
         {10s}
       );

From 0fb669d5b8282d217a5b7bc8e1775f7ed927fa97 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Fri, 25 Jun 2021 13:57:44 +0800
Subject: [PATCH 36/66] Add Versioning Scheme

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../rmf_traffic/reservations/Participant.hpp  | 16 +++++++---
 .../reservations/internal/ExecutionEngine.hpp | 29 ++++++++++++-------
 .../test/unit/reservations/test_Database.cpp  | 12 +++++---
 3 files changed, 38 insertions(+), 19 deletions(-)

diff --git a/rmf_traffic/include/rmf_traffic/reservations/Participant.hpp b/rmf_traffic/include/rmf_traffic/reservations/Participant.hpp
index 2781d657..5614e86b 100644
--- a/rmf_traffic/include/rmf_traffic/reservations/Participant.hpp
+++ b/rmf_traffic/include/rmf_traffic/reservations/Participant.hpp
@@ -38,10 +38,12 @@ class Participant
   /// \param[in] id is the RequestId of the request that will be impacted.
   /// \param[in] res is the Reservation that is being proposed by the
   /// reservation system.
+  /// \param[in] proposal_version is used to track the proposal's id
   /// \return true if the request is acceptable, false otherwise.
   virtual bool request_proposal(
     RequestId id,
-    Reservation& res
+    Reservation& res,
+    uint64_t proposal_version
   ) = 0;
 
   ///===========================================================================
@@ -51,11 +53,13 @@ class Participant
   /// expected that the participant will comply.
   /// \param[in] id the request id.
   /// \param[in] res the response.
+  /// \param[in] proposal_version is used to track the proposal's id
   /// \return true if the request has been successfully confirmed, false
   /// otherwise
   virtual bool request_confirmed(
     RequestId id,
-    Reservation& res
+    Reservation& res,
+    uint64_t proposal_version
   ) = 0;
 
   ///===========================================================================
@@ -64,18 +68,22 @@ class Participant
   /// The request will still remain in the database, just that it will be
   /// serviced at some point later if possible.
   /// \param[in] id is the RequestId of the request that will be impacted.
+  /// \param[in] proposal_version is used to track the proposal's id
   /// \return true if the you agree to allow your request to be cancelled, false
   /// otherwise.
   virtual bool unassign_request_confirmed(
-    RequestId id
+    RequestId id,
+    uint64_t proposal_version
   ) = 0;
 
   ///===========================================================================
   /// \brief This method is called whenever a cancellation has been allowed.
   /// \param[in] id is the RequestId of the request that will be impacted.
+  /// \param[in] proposal_version is used to track the proposal's id
   /// \return true if the request is acceptable, false otherwise.
   virtual bool unassign_request_proposal(
-    RequestId id
+    RequestId id,
+    uint64_t proposal_version
   ) = 0;
 };
 
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp
index b8dcce68..630d37b6 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp
@@ -50,7 +50,8 @@ class ExecutionEngine
     std::shared_ptr<ParticipantStore> pstore)
   : _request_queue(request_queue),
     _participant_store(pstore),
-    _curr_state(std::make_shared<RequestStore>())
+    _curr_state(std::make_shared<RequestStore>()),
+    _proposal_version(0)
   {
     _execution_thread =
       std::make_shared<std::thread>(&ExecutionEngine::execute, this);
@@ -128,10 +129,8 @@ class ExecutionEngine
   }
 
   //============================================================================
-  /// \brief Rolls out the changes. If for whatever reason a rollout should fail
-  /// there is no need to rollback as the StateDiff is returned in a topological
-  /// order. Instead _`curr_state` remains in a partially successful state and
-  /// we will retry optimization upon the arrival of the next request.
+  /// \brief Rolls out the proposed changes.
+  /// TODO: Consider making asynchronous
   void rollout(const StateDiff& changes)
   {
     for (auto change: changes.differences())
@@ -147,7 +146,8 @@ class ExecutionEngine
 
         auto result = participant->request_confirmed(
           change.request_id,
-          change.reservation.value());
+          change.reservation.value(),
+          _proposal_version);
 
         if (result == false)
           return;
@@ -169,7 +169,8 @@ class ExecutionEngine
 
         auto result = participant->request_confirmed(
           change.request_id,
-          change.reservation.value());
+          change.reservation.value(),
+          _proposal_version);
 
         if (result == false)
           return;
@@ -185,11 +186,12 @@ class ExecutionEngine
       {
         assert(!change.reservation.has_value());
         auto result =
-          participant->unassign_request_confirmed(change.request_id);
+          participant->unassign_request_confirmed(
+            change.request_id,
+            _proposal_version);
 
         if (result == false)
           return;
-
         _curr_state = _curr_state.unassign_reservation(
           change.participant_id,
           change.request_id);
@@ -200,6 +202,7 @@ class ExecutionEngine
   //============================================================================
   /// \brief Proposes the solution favoured by the optimizer and ratified
   /// by all participants.
+  /// TODO(arjo): async-ify
   /// \param[in] optimizer - Instance of the optimizer.
   /// \returns the StateDiff or a `std::nullopt` if no Solution is agreeable
   std::optional<StateDiff> get_favored_solution(
@@ -211,6 +214,7 @@ class ExecutionEngine
       StateDiff impacted_reservations(solution.value(), _curr_state);
       if (verify_proposal(impacted_reservations))
         return {impacted_reservations};
+      _proposal_version++;
     }
 
     return std::nullopt;
@@ -239,7 +243,8 @@ class ExecutionEngine
       {
         auto result = participant->request_proposal(
           diff.request_id,
-          diff.reservation.value()
+          diff.reservation.value(),
+          _proposal_version
         );
         if (!result)
           return false;
@@ -247,7 +252,8 @@ class ExecutionEngine
       else
       {
         auto result = participant->unassign_request_proposal(
-          diff.request_id
+          diff.request_id,
+          _proposal_version
         );
         if (!result)
           return false;
@@ -259,6 +265,7 @@ class ExecutionEngine
   std::shared_ptr<std::thread> _execution_thread;
   std::shared_ptr<RequestQueue> _request_queue;
   std::shared_ptr<ParticipantStore> _participant_store;
+  uint64_t _proposal_version;
   State _curr_state;
 };
 }
diff --git a/rmf_traffic/test/unit/reservations/test_Database.cpp b/rmf_traffic/test/unit/reservations/test_Database.cpp
index 18cb050d..3fb68c2f 100644
--- a/rmf_traffic/test/unit/reservations/test_Database.cpp
+++ b/rmf_traffic/test/unit/reservations/test_Database.cpp
@@ -43,7 +43,8 @@ class SimpleParticipant : public Participant
 
   bool request_proposal(
     RequestId id,
-    Reservation& res
+    Reservation& res,
+    uint64_t proposal_version
   ) override
   {
     _received_confirmation[id] = true;
@@ -53,21 +54,24 @@ class SimpleParticipant : public Participant
 
   bool request_confirmed(
     RequestId id,
-    Reservation& res
+    Reservation& res,
+    uint64_t proposal_version
   ) override
   {
     return true;
   }
 
   bool unassign_request_confirmed(
-    RequestId id
+    RequestId id,
+    uint64_t proposal_version
   ) override
   {
     return true;
   }
 
   bool unassign_request_proposal(
-    RequestId id
+    RequestId id,
+    uint64_t proposal_version
   ) override
   {
     _received_cancellation[id] = true;

From bc5ad1f0099798cbd17b87057024554bc85755ff Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Thu, 15 Jul 2021 14:11:59 +0800
Subject: [PATCH 37/66] minor clean ups

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp b/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp
index 48182f8c..0464df8c 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp
@@ -17,6 +17,7 @@
 
 #include <rmf_traffic/reservations/Reservation.hpp>
 #include <atomic>
+#include <string>
 
 namespace rmf_traffic {
 namespace reservations {
@@ -156,7 +157,7 @@ bool Reservation::operator!=(const Reservation& other) const
 //=============================================================================
 Reservation Reservation::make_reservation(
   rmf_traffic::Time start_time,
-  std::string resource_name,
+  std::string _resource_name,
   std::optional<rmf_traffic::Duration> duration,
   std::optional<rmf_traffic::Time> finish_time)
 {
@@ -165,7 +166,7 @@ Reservation Reservation::make_reservation(
 
   res._pimpl->_res_id = counter;
   res._pimpl->_start_time = start_time;
-  res._pimpl->_resource_name = resource_name;
+  res._pimpl->_resource_name = _resource_name;
   res._pimpl->_duration = duration;
   res._pimpl->_finish_time = finish_time;
   counter++;

From 5126d7c1dce26c3300d55b41aa8838467f672d58 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Thu, 15 Jul 2021 14:39:56 +0800
Subject: [PATCH 38/66] more cleanups

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../include/rmf_traffic/reservations/Reservation.hpp      | 1 +
 .../rmf_traffic/reservations/ReservationRequest.hpp       | 1 +
 .../src/rmf_traffic/reservations/internal/Heuristic.hpp   | 8 ++++++++
 .../src/rmf_traffic/reservations/internal/State.hpp       | 4 ++--
 4 files changed, 12 insertions(+), 2 deletions(-)

diff --git a/rmf_traffic/include/rmf_traffic/reservations/Reservation.hpp b/rmf_traffic/include/rmf_traffic/reservations/Reservation.hpp
index 324d4818..f81ef8d8 100644
--- a/rmf_traffic/include/rmf_traffic/reservations/Reservation.hpp
+++ b/rmf_traffic/include/rmf_traffic/reservations/Reservation.hpp
@@ -20,6 +20,7 @@
 
 #include <optional>
 #include <vector>
+#include <string>
 #include <rmf_traffic/Time.hpp>
 #include <rmf_utils/impl_ptr.hpp>
 
diff --git a/rmf_traffic/include/rmf_traffic/reservations/ReservationRequest.hpp b/rmf_traffic/include/rmf_traffic/reservations/ReservationRequest.hpp
index 6a3459f1..203f3917 100644
--- a/rmf_traffic/include/rmf_traffic/reservations/ReservationRequest.hpp
+++ b/rmf_traffic/include/rmf_traffic/reservations/ReservationRequest.hpp
@@ -20,6 +20,7 @@
 
 #include <optional>
 #include <vector>
+#include <string>
 #include <rmf_traffic/Time.hpp>
 #include <rmf_utils/impl_ptr.hpp>
 
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp
index 2e11c415..f611f5b8 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp
@@ -27,6 +27,8 @@ class Heuristic
 {
 public:
   virtual float score(const State& state) = 0;
+
+  virtual ~Heuristic() = default;
 };
 
 class PriorityBasedScorer : public Heuristic
@@ -35,8 +37,14 @@ class PriorityBasedScorer : public Heuristic
   PriorityBasedScorer(std::shared_ptr<RequestStore> queue)
   : _queue(queue)
   {
+    // Do nothing
+  }
 
+  ~PriorityBasedScorer()
+  {
+    // Do nothing
   }
+
   float score(const State& state) override
   {
     auto score = 0;
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index a99c96c3..38e2e5c5 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -396,7 +396,7 @@ class State
   }
 
   //============================================================================
-  State(const State& other)
+  /*State(const State& other)
   : _resource_schedules(other._resource_schedules),
     _unassigned(other._unassigned),
     _reservation_timings(other._reservation_timings),
@@ -407,7 +407,7 @@ class State
     _current_time(other._current_time)
   {
     // Do nothing
-  }
+  }*/
 
 private:
   //============================================================================

From 648360b4c6c5e63dca5913d8a2870d3037877d09 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Thu, 15 Jul 2021 14:50:10 +0800
Subject: [PATCH 39/66] remove some crust

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../src/rmf_traffic/reservations/internal/ExecutionEngine.hpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp
index 630d37b6..d4589a40 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp
@@ -187,8 +187,8 @@ class ExecutionEngine
         assert(!change.reservation.has_value());
         auto result =
           participant->unassign_request_confirmed(
-            change.request_id,
-            _proposal_version);
+          change.request_id,
+          _proposal_version);
 
         if (result == false)
           return;

From 23817a7f8c926ce19bd02b8372a3eea1ad5e9a90 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Mon, 19 Jul 2021 16:27:09 +0800
Subject: [PATCH 40/66] fixed memory issues; still have wierd planner
 behaviour.

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../reservations/internal/RequestStore.hpp    |  3 +-
 .../reservations/internal/State.hpp           | 41 +++++++++++-----
 .../unit/reservations/internal/test_State.cpp | 49 ++++++++++++++++++-
 3 files changed, 77 insertions(+), 16 deletions(-)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp
index 2d4f7b81..d8bb0e1a 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp
@@ -38,7 +38,6 @@ class RequestStore
     ParticipantId pid,
     RequestId req)
   {
-    //TODO: Sanitize
     return _reservation_info[pid][req];
   }
 
@@ -64,7 +63,7 @@ class RequestStore
         req,
         {
           priority,
-          std::move(reservation)
+          reservation
         }
       });
   }
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index a99c96c3..ca8305ae 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -396,21 +396,22 @@ class State
   }
 
   //============================================================================
-  State(const State& other)
-  : _resource_schedules(other._resource_schedules),
-    _unassigned(other._unassigned),
-    _reservation_timings(other._reservation_timings),
-    _queue(other._queue),
-    _reservation_resources(other._reservation_resources),
-    _reservation_assignments(other._reservation_assignments),
-    _reservation_request_ids(other._reservation_request_ids),
-    _current_time(other._current_time)
-  {
-    // Do nothing
-  }
+  //State(const State& other)
+  //: _resource_schedules(other._resource_schedules),
+  //  _unassigned(other._unassigned),
+  //  _reservation_timings(other._reservation_timings),
+  //  _queue(other._queue),
+  //  _reservation_resources(other._reservation_resources),
+  //  _reservation_assignments(other._reservation_assignments),
+  //  _reservation_request_ids(other._reservation_request_ids),
+  //  _current_time(other._current_time)
+  //{
+  //  // Do nothing
+  //}
 
 private:
   //============================================================================
+  /// \brief Unassign a reservation from the current state. Mutates the state.
   void unassign_reservation(ReservationId res_id, std::string resource_name)
   {
     auto time = _reservation_timings[res_id];
@@ -425,13 +426,21 @@ class State
   }
 
   //============================================================================
+  /// \brief Performs a pushback operation on the current schedule. This is used
+  /// to make space for new reservations.
+  /// \param resource - the resource for which we are trying to make more space
+  /// \param res_iter = the point at which to perform the pushback
+  /// \param dur - the duration of the pushback
+  /// \returns true if the pushback is possible, false otherwise. Reasons for
+  /// returning false include the fact that some constraints are not satisfied,
+  /// or the duration is negative.
   bool push_back(
     std::string resource,
     ResourceSchedule::const_iterator res_iter,
     Duration dur
   )
   {
-    assert(dur.count() > 0);
+    if(dur.count() < 0) return false;
 
     auto prev_iter = res_iter;
     std::next(res_iter);
@@ -468,6 +477,7 @@ class State
   }
 
   //============================================================================
+  /// \brief Move the start time of a reservation.
   bool shift_reservation_start_time(
     std::string resource,
     ReservationId res_id,
@@ -507,6 +517,10 @@ class State
   friend StateDiff;
 };
 //==============================================================================
+/// \brief This iterator is used to iterate over the next possible states of the
+/// schedule. It does this by first attempting to assign one item from the
+/// unassigned list. After that it gives states by individually removing items
+/// from the schedule. This is used by the planner to get the next state.
 struct NextStateGenerator
 {
   //TODO: RAW_POINTER-FOO
@@ -783,6 +797,7 @@ struct NextStateGenerator
 
     if (removals.has_value())
     {
+      visiting_state = removals;
       return removals;
     }
     mode = PotentialActionMode::END;
diff --git a/rmf_traffic/test/unit/reservations/internal/test_State.cpp b/rmf_traffic/test/unit/reservations/internal/test_State.cpp
index 00d64c1c..359764cd 100644
--- a/rmf_traffic/test/unit/reservations/internal/test_State.cpp
+++ b/rmf_traffic/test/unit/reservations/internal/test_State.cpp
@@ -28,6 +28,53 @@ using namespace std::chrono_literals;
 
 SCENARIO("A few reservations in a state")
 {
+  GIVEN("A state with one reservation")
+  {
+    auto queue = std::make_shared<RequestStore>();
+    State start_state(queue);
+    auto now = std::chrono::steady_clock::now();
+    now -= now.time_since_epoch();
+    auto request1_alt1 = ReservationRequest::make_request(
+      "table_at_timbre",
+      ReservationRequest::TimeRange::make_time_range(
+        now,
+        now+5s
+      ),
+      {10s}
+    );
+
+    auto request1 = std::vector{request1_alt1};
+    queue->enqueue_reservation(0, 0, 1, request1);
+    start_state = start_state.add_request(0, 0);
+
+    std::vector<State> child_states;
+    for (auto child_state: start_state)
+    {
+      child_states.push_back(child_state);
+    }
+
+    REQUIRE(child_states.size() == 1);
+
+    queue->enqueue_reservation(1,0,20,request1);
+    auto allocated_state = child_states[0].add_request(1, 0);
+
+    WHEN("We have a conflicting request with higher priority")
+    {
+      auto heuristic = std::make_shared<PriorityBasedScorer>(queue);
+      GreedyBestFirstSearchOptimizer opt(heuristic);
+      auto solutions = opt.optimize(allocated_state);
+      std::vector<State> result;
+      std::cout << "[Current test]=============" <<std::endl;
+      while (auto solution = solutions.next_solution())
+      {
+        std::cout << "New Solution Found" << std::endl;
+        //result.push_back(solution.value());
+        solution->debug_state();
+      }
+      std::cout << "[end]=============" <<std::endl;
+    }
+  }
+
   GIVEN("An empty state with two requests pending")
   {
     auto queue = std::make_shared<RequestStore>();
@@ -175,4 +222,4 @@ SCENARIO("Given a single item allocated in a state")
       REQUIRE(!next_state[0].check_if_conflicts(res));
     }
   }
-}
\ No newline at end of file
+}

From 4a2f438803f8802268b8199f450e0b498e156ce7 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Tue, 20 Jul 2021 10:40:44 +0800
Subject: [PATCH 41/66] fix ordering?

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../reservations/internal/Optimizer.hpp       | 12 +++-----
 .../unit/reservations/internal/test_State.cpp | 28 +++++++++++++++----
 2 files changed, 26 insertions(+), 14 deletions(-)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/Optimizer.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/Optimizer.hpp
index 120a5ff0..dfba4e2d 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/Optimizer.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/Optimizer.hpp
@@ -45,7 +45,7 @@ class GreedyBestFirstSearchOptimizer
     }
     bool operator()(const State& e1, const State& e2) const
     {
-      return _heuristic->score(e1) < _heuristic->score(e2);
+      return _heuristic->score(e1) >= _heuristic->score(e2);
     }
   };
   class Solution
@@ -58,13 +58,11 @@ class GreedyBestFirstSearchOptimizer
       _all_solutions(heuristic),
       _heuristic(heuristic)
     {
-      _blacklist.insert(state);
       _visited.insert(state);
       _pq.push(state);
     }
     std::optional<State> next_solution()
     {
-      _blacklist.insert(_last_solution);
       while (!_pq.empty())
       {
         auto state = _pq.top();
@@ -75,8 +73,7 @@ class GreedyBestFirstSearchOptimizer
           if (_visited.count(next_state) != 0)
             continue;
 
-          if (_heuristic->score(next_state) == 0 &&
-            _blacklist.count(next_state) == 0)
+          if (_heuristic->score(next_state) == 0)
           {
             _last_solution = next_state;
             return next_state;
@@ -86,19 +83,18 @@ class GreedyBestFirstSearchOptimizer
           _all_solutions.push(next_state);
         }
       }
+
       while (!_all_solutions.empty())
       {
         auto res = _all_solutions.top();
         _last_solution = res;
         _all_solutions.pop();
-        if (_blacklist.count(res) != 0)
-          return res;
+        return res;
       }
       return std::nullopt;
     }
   private:
     std::unordered_set<State, StateHash> _visited;
-    std::unordered_set<State, StateHash> _blacklist;
     std::priority_queue<State, std::vector<State>, CustomComparator> _pq;
     std::priority_queue<State, std::vector<State>, CustomComparator>
     _all_solutions;
diff --git a/rmf_traffic/test/unit/reservations/internal/test_State.cpp b/rmf_traffic/test/unit/reservations/internal/test_State.cpp
index 359764cd..82a933d0 100644
--- a/rmf_traffic/test/unit/reservations/internal/test_State.cpp
+++ b/rmf_traffic/test/unit/reservations/internal/test_State.cpp
@@ -64,14 +64,30 @@ SCENARIO("A few reservations in a state")
       GreedyBestFirstSearchOptimizer opt(heuristic);
       auto solutions = opt.optimize(allocated_state);
       std::vector<State> result;
-      std::cout << "[Current test]=============" <<std::endl;
       while (auto solution = solutions.next_solution())
       {
-        std::cout << "New Solution Found" << std::endl;
-        //result.push_back(solution.value());
-        solution->debug_state();
+        result.push_back(solution.value());
+      }
+
+      REQUIRE(result.size() == 2);
+
+      THEN("The request with a higher priority will be serviced first")
+      {
+        StateDiff diff(result[0], allocated_state);
+        auto steps = diff.differences();
+        for(auto step: steps)
+        {
+          if(step.diff_type == StateDiff::DifferenceType::ASSIGN_RESERVATION)
+          {
+            REQUIRE(step.participant_id == 1);
+          }
+          else if(step.diff_type == StateDiff::DifferenceType::UNASSIGN_RESERVATION)
+          {
+            REQUIRE(step.participant_id == 0);
+          }
+        }
+        REQUIRE(steps.size() == 2);
       }
-      std::cout << "[end]=============" <<std::endl;
     }
   }
 
@@ -146,7 +162,7 @@ SCENARIO("A few reservations in a state")
       auto solutions = opt.optimize(start_state);
       while (auto solution = solutions.next_solution())
       {
-        solution->debug_state();
+        //solution->debug_state();
       }
     }
 

From 70ce90a9e5fdc959c7353b235c01d0dbea406ca0 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Tue, 20 Jul 2021 11:42:01 +0800
Subject: [PATCH 42/66] Style fixes

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../src/rmf_traffic/reservations/internal/State.hpp        | 2 +-
 rmf_traffic/test/unit/reservations/internal/test_State.cpp | 7 ++++---
 2 files changed, 5 insertions(+), 4 deletions(-)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index ca8305ae..e7d204c1 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -440,7 +440,7 @@ class State
     Duration dur
   )
   {
-    if(dur.count() < 0) return false;
+    if (dur.count() < 0) return false;
 
     auto prev_iter = res_iter;
     std::next(res_iter);
diff --git a/rmf_traffic/test/unit/reservations/internal/test_State.cpp b/rmf_traffic/test/unit/reservations/internal/test_State.cpp
index 82a933d0..7c292d4f 100644
--- a/rmf_traffic/test/unit/reservations/internal/test_State.cpp
+++ b/rmf_traffic/test/unit/reservations/internal/test_State.cpp
@@ -75,13 +75,14 @@ SCENARIO("A few reservations in a state")
       {
         StateDiff diff(result[0], allocated_state);
         auto steps = diff.differences();
-        for(auto step: steps)
+        for (auto step: steps)
         {
-          if(step.diff_type == StateDiff::DifferenceType::ASSIGN_RESERVATION)
+          if (step.diff_type == StateDiff::DifferenceType::ASSIGN_RESERVATION)
           {
             REQUIRE(step.participant_id == 1);
           }
-          else if(step.diff_type == StateDiff::DifferenceType::UNASSIGN_RESERVATION)
+          else if (step.diff_type ==
+            StateDiff::DifferenceType::UNASSIGN_RESERVATION)
           {
             REQUIRE(step.participant_id == 0);
           }

From c271ae5827019418f656cbcb170f456b2d1b5df0 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Tue, 20 Jul 2021 11:53:11 +0800
Subject: [PATCH 43/66] fix state based style issues.

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 rmf_traffic/test/unit/reservations/internal/test_State.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rmf_traffic/test/unit/reservations/internal/test_State.cpp b/rmf_traffic/test/unit/reservations/internal/test_State.cpp
index 7c292d4f..ac616bd0 100644
--- a/rmf_traffic/test/unit/reservations/internal/test_State.cpp
+++ b/rmf_traffic/test/unit/reservations/internal/test_State.cpp
@@ -55,7 +55,7 @@ SCENARIO("A few reservations in a state")
 
     REQUIRE(child_states.size() == 1);
 
-    queue->enqueue_reservation(1,0,20,request1);
+    queue->enqueue_reservation(1, 0, 20, request1);
     auto allocated_state = child_states[0].add_request(1, 0);
 
     WHEN("We have a conflicting request with higher priority")

From 5838c36323f1a172182e45d4cb30a6954d427a80 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Wed, 21 Jul 2021 16:57:19 +0800
Subject: [PATCH 44/66] update state api

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../reservations/ReservationRequest.hpp       |  28 ++++-
 .../reservations/internal/ExecutionEngine.hpp |  13 +--
 .../reservations/internal/Heuristic.hpp       |   9 +-
 .../reservations/internal/RequestQueue.hpp    |  32 +-----
 .../reservations/internal/RequestStore.hpp    |   9 +-
 .../reservations/internal/State.hpp           | 107 ++++++------------
 .../unit/reservations/internal/test_State.cpp |  33 +++---
 .../test/unit/reservations/test_Database.cpp  |  35 +++++-
 8 files changed, 132 insertions(+), 134 deletions(-)

diff --git a/rmf_traffic/include/rmf_traffic/reservations/ReservationRequest.hpp b/rmf_traffic/include/rmf_traffic/reservations/ReservationRequest.hpp
index 203f3917..1340e985 100644
--- a/rmf_traffic/include/rmf_traffic/reservations/ReservationRequest.hpp
+++ b/rmf_traffic/include/rmf_traffic/reservations/ReservationRequest.hpp
@@ -30,11 +30,15 @@ namespace reservations {
 using RequestId = uint64_t;
 
 ///=============================================================================
-/// \brief This class
+/// \brief This class represents a reservation request.
 class ReservationRequest
 {
 
 public:
+  ///===========================================================================
+  /// \brief Time range for describing the starting time. The starting time may
+  /// either have a lower bound, upper bound, both or none. This constraints the
+  /// possible starting times for a given reservation.
   class TimeRange
   {
   public:
@@ -53,16 +57,38 @@ class ReservationRequest
     rmf_utils::impl_ptr<Implementation> _pimpl;
   };
 
+  ///===========================================================================
+  /// \brief Time range for describing the starting time.
   const std::optional<TimeRange> start_time() const;
 
+  ///===========================================================================
+  /// \brief Minimum duration for which the reservation must be held.
   const std::optional<Duration> duration() const;
 
+  ///===========================================================================
+  /// \brief The finish time for the reservation. If the finish time is not set.
   const std::optional<Time> finish_time() const;
 
+  ///===========================================================================
+  /// \brief Thre resource name
   const std::string resource_name() const;
 
+  ///===========================================================================
+  /// \brief \returns true if this is an indefinite reservation. An indefinite
+  /// reservation is a reservation that does not have a finish time or a
+  /// duration.
   bool is_indefinite() const;
 
+  ///===========================================================================
+  /// \brief Create a reservation request.
+  /// \param[in] resource_name The name of the resource to reserve.
+  /// \param[in] start The time range constraints for the start time of the
+  ///   reservation.
+  /// \param[in] duration The duration of the reservation.
+  /// \param[in] finish The finish time for the reservation.
+  /// If both duration and finish are `std::nullopt` then the reservation is
+  /// indefinite. If both duration and finish are not `std::nullopt` then the
+  /// max(start_time + duration, finish_time) is used as the actual finish time.
   static ReservationRequest make_request(
     std::string resource_name,
     std::optional<TimeRange> start = std::nullopt,
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp
index d4589a40..2f11c71a 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp
@@ -95,28 +95,25 @@ class ExecutionEngine
         _curr_state = _curr_state.add_request(
           element.action.participant_id,
           element.action.request_id,
-          element.request_store
+          element.action.priority,
+          element.action.request_options
         );
       }
       else if (element.action.type == RequestQueue::ActionType::REMOVE)
       {
         _curr_state = _curr_state.remove_request(
           element.action.participant_id,
-          element.action.request_id,
-          element.request_store
-        );
+          element.action.request_id);
       }
       else if (element.action.type ==
         RequestQueue::ActionType::REMOVE_PARTICIPANT)
       {
         _curr_state = _curr_state.remove_participant(
-          element.action.participant_id,
-          element.request_store);
+          element.action.participant_id);
         _participant_store->remove_participant(element.action.participant_id);
       }
 
-      auto heuristic = std::make_shared<PriorityBasedScorer>(
-        element.request_store);
+      auto heuristic = std::make_shared<PriorityBasedScorer>();
 
       GreedyBestFirstSearchOptimizer optimizer(heuristic);
       auto favored_solution = get_favored_solution(optimizer);
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp
index f611f5b8..158cb1d2 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp
@@ -34,8 +34,7 @@ class Heuristic
 class PriorityBasedScorer : public Heuristic
 {
 public:
-  PriorityBasedScorer(std::shared_ptr<RequestStore> queue)
-  : _queue(queue)
+  PriorityBasedScorer()
   {
     // Do nothing
   }
@@ -50,13 +49,11 @@ class PriorityBasedScorer : public Heuristic
     auto score = 0;
     for (auto [part_id, req_id]: state.unassigned())
     {
-      // TODO: Incorporate
-      score += _queue->get_request_info(part_id, req_id).priority;
+      // TODO: Incorporate the order into the heuristic
+      score += state.requests()->get_request_info(part_id, req_id)->priority;
     }
     return score;
   }
-private:
-  std::shared_ptr<RequestStore> _queue;
 };
 
 }
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp
index 6833a437..e9d37f18 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp
@@ -59,42 +59,14 @@ class RequestQueue
   struct QueueElement
   {
     Action action;
-    std::shared_ptr<RequestStore> request_store;
+
   };
 
   void add_action(Action action)
   {
     {
       std::lock_guard lock(_mutex);
-
-      std::shared_ptr<RequestStore>
-      req_store = [=]() -> std::shared_ptr<RequestStore>
-        {
-          if (_request_store_queue.empty())
-            return std::make_shared<RequestStore>();
-          else
-            return std::make_shared<RequestStore>(
-              *_request_store_queue.back().request_store.get()
-            );
-        } ();
-
-      if (action.type == ActionType::ADD)
-      {
-        req_store->enqueue_reservation(
-          action.participant_id,
-          action.request_id,
-          action.priority,
-          action.request_options);
-      }
-      else if (action.type == ActionType::REMOVE)
-      {
-        req_store->cancel(action.participant_id, action.request_id);
-      }
-      else
-      {
-        req_store->erase_participant_requests(action.participant_id);
-      }
-      _request_store_queue.push_back({action, req_store});
+      _request_store_queue.push_back({action});
     }
     _cond.notify_one();
   }
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp
index d8bb0e1a..c9006da5 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp
@@ -22,6 +22,8 @@
 #include <rmf_traffic/reservations/Reservation.hpp>
 #include <unordered_map>
 
+#include <iostream>
+
 namespace rmf_traffic {
 namespace reservations {
 
@@ -34,11 +36,14 @@ class RequestStore
     std::vector<ReservationRequest> request_options;
   };
 
-  RequestInfo get_request_info(
+  std::optional<RequestInfo> get_request_info(
     ParticipantId pid,
     RequestId req)
   {
-    return _reservation_info[pid][req];
+    if(_reservation_info.count(pid) != 0
+      && _reservation_info.at(pid).count(req) != 0)
+      return _reservation_info[pid][req];
+    return std::nullopt;
   }
 
   RequestStore()
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index e7d204c1..3e5ab201 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -41,8 +41,11 @@ inline void hash_combine(std::size_t& seed, const T& v)
   std::hash<T> hasher;
   seed ^= hasher(v) + 0x9e3779b9 + (seed<<6) + (seed>>2);
 }
-//TODO: Currrently uses a lot of copying. In future we should use a parent based
-//hierarchy
+///=============================================================================
+/// \brief This class represents the current schedule. It is used by the planner
+/// to explore the space of possible schedules. Iterating over the State allows
+/// one to find out the next possible schedule based on the current schedule
+/// stored in the state.
 class State
 {
   struct pair_hash
@@ -82,67 +85,33 @@ class State
     //Do nothing
   }
 
+  //============================================================================
   NextStateGenerator begin();
-  NextStateGenerator end();
 
   //============================================================================
-  /// Adds a request to the state
-  State add_request(
-    ParticipantId pid,
-    RequestId reqid) const
-  {
-    State new_state(*this);
-    new_state._unassigned.insert({pid, reqid});
-    return new_state;
-  }
+  NextStateGenerator end();
 
   //============================================================================
   /// Adds a request to the state
+  /// Note: This modifies the ReservationStore.
   State add_request(
     ParticipantId pid,
     RequestId reqid,
-    std::shared_ptr<RequestStore> store) const
+    int priority,
+    std::vector<ReservationRequest>& reservation) const
   {
     State new_state(*this);
     new_state._unassigned.insert({pid, reqid});
-    new_state._queue = store;
-    return new_state;
-  }
-
-  State remove_request(
-    ParticipantId pid,
-    RequestId reqid) const
-  {
-    State new_state(*this);
-    auto req = new_state._unassigned.find({pid, reqid});
-
-    if (req != new_state._unassigned.end())
-    {
-      new_state._unassigned.erase(req);
-      return new_state;
-    }
-    auto reservation_id = new_state._reservation_assignments[pid][reqid];
-    auto findings = new_state._reservation_timings.find(reservation_id);
-    if (findings == new_state._reservation_timings.end())
-    {
-      // Non existant pid-reqid pair
-      return new_state;
-    }
-    auto time = findings->second;
-    auto resource = new_state._reservation_resources[reservation_id];
-
-    new_state._resource_schedules[resource].erase(time);
-    new_state._reservation_resources.erase(reservation_id);
-    new_state._reservation_assignments[pid].erase(reqid);
-    new_state._reservation_timings.erase(findings);
+    new_state._queue->enqueue_reservation(pid, reqid, priority, reservation);
     return new_state;
   }
 
   //============================================================================
+  /// Removes a request from the current state and returns the new state.
+  /// Note: This modifies the ReservationStore.
   State remove_request(
     ParticipantId pid,
-    RequestId reqid,
-    std::shared_ptr<RequestStore> store) const
+    RequestId reqid) const
   {
     State new_state(*this);
     auto req = new_state._unassigned.find({pid, reqid});
@@ -166,21 +135,19 @@ class State
     new_state._reservation_resources.erase(reservation_id);
     new_state._reservation_assignments[pid].erase(reqid);
     new_state._reservation_timings.erase(findings);
-
-    new_state._queue = store;
+    new_state._queue->cancel(pid, reqid);
     return new_state;
   }
 
   //============================================================================
   State remove_participant(
-    ParticipantId pid,
-    std::shared_ptr<RequestStore> store)
+    ParticipantId pid)
   {
     //TODO(arjo): Lots of unessecary copying
     State new_state(*this);
     for (auto [request, reservation]: _reservation_assignments[pid])
     {
-      new_state = new_state.remove_request(pid, request, store);
+      new_state = new_state.remove_request(pid, request);
     }
     return new_state;
   }
@@ -395,20 +362,10 @@ class State
     return true;
   }
 
-  //============================================================================
-  //State(const State& other)
-  //: _resource_schedules(other._resource_schedules),
-  //  _unassigned(other._unassigned),
-  //  _reservation_timings(other._reservation_timings),
-  //  _queue(other._queue),
-  //  _reservation_resources(other._reservation_resources),
-  //  _reservation_assignments(other._reservation_assignments),
-  //  _reservation_request_ids(other._reservation_request_ids),
-  //  _current_time(other._current_time)
-  //{
-  //  // Do nothing
-  //}
-
+  std::shared_ptr<RequestStore> requests() const
+  {
+    return _queue;
+  }
 private:
   //============================================================================
   /// \brief Unassign a reservation from the current state. Mutates the state.
@@ -608,7 +565,7 @@ struct NextStateGenerator
 
     auto current_request = start_state->_queue->get_request_info(
       part_id,
-      req_id).request_options[request_index_iter];
+      req_id)->request_options[request_index_iter];
 
     bool found_slot = false;
 
@@ -707,13 +664,13 @@ struct NextStateGenerator
     auto [part_id, req_id] = *unassigned_iter;
     auto num_alternative = start_state->_queue->get_request_info(
       part_id,
-      req_id).request_options.size();
+      req_id)->request_options.size();
 
     while (request_index_iter < num_alternative)
     {
       // Reset resource iterators
       auto curr_request = start_state->_queue->get_request_info(part_id, req_id)
-        .request_options[request_index_iter];
+        ->request_options[request_index_iter];
       insertion_point_iter = get_search_start(curr_request);
       insertion_point_end = get_search_end(curr_request);
       proceed_next_resource = false;
@@ -742,7 +699,7 @@ struct NextStateGenerator
       auto [part_id, req_id] = *unassigned_iter;
       request_index_iter = 0;
       auto curr_request = start_state->_queue->get_request_info(part_id, req_id)
-        .request_options[request_index_iter];
+        ->request_options[request_index_iter];
       insertion_point_iter = get_search_start(curr_request);
       insertion_point_end = get_search_end(curr_request);
       proceed_next_resource = false;
@@ -846,9 +803,19 @@ NextStateGenerator State::begin()
 
   if (_unassigned.size() > 0)
   {
-    auto current_request = _queue->get_request_info(
+    auto request = _queue->get_request_info(
       _unassigned.begin()->first,
-      _unassigned.begin()->second).request_options[0];
+      _unassigned.begin()->second);
+
+    if(!request.has_value())
+    {
+      throw std::runtime_error(std::string("Got nullopt for participant ") +
+        std::to_string(_unassigned.begin()->first) +
+        std::string(" request id #")+ 
+        std::to_string(_unassigned.begin()->second));
+    }
+
+    auto current_request = request->request_options[0];
 
     gen.unassigned_iter = _unassigned.begin();
     gen.insertion_point_iter = gen.get_search_start(current_request);
diff --git a/rmf_traffic/test/unit/reservations/internal/test_State.cpp b/rmf_traffic/test/unit/reservations/internal/test_State.cpp
index ac616bd0..10f7d092 100644
--- a/rmf_traffic/test/unit/reservations/internal/test_State.cpp
+++ b/rmf_traffic/test/unit/reservations/internal/test_State.cpp
@@ -44,8 +44,7 @@ SCENARIO("A few reservations in a state")
     );
 
     auto request1 = std::vector{request1_alt1};
-    queue->enqueue_reservation(0, 0, 1, request1);
-    start_state = start_state.add_request(0, 0);
+    start_state = start_state.add_request(0, 0, 1, request1);
 
     std::vector<State> child_states;
     for (auto child_state: start_state)
@@ -54,13 +53,11 @@ SCENARIO("A few reservations in a state")
     }
 
     REQUIRE(child_states.size() == 1);
-
-    queue->enqueue_reservation(1, 0, 20, request1);
-    auto allocated_state = child_states[0].add_request(1, 0);
+    auto allocated_state = child_states[0].add_request(1, 0, 20, request1);
 
     WHEN("We have a conflicting request with higher priority")
     {
-      auto heuristic = std::make_shared<PriorityBasedScorer>(queue);
+      auto heuristic = std::make_shared<PriorityBasedScorer>();
       GreedyBestFirstSearchOptimizer opt(heuristic);
       auto solutions = opt.optimize(allocated_state);
       std::vector<State> result;
@@ -116,8 +113,7 @@ SCENARIO("A few reservations in a state")
     );
 
     auto request1 = std::vector{request1_alt1, request1_alt2};
-    queue->enqueue_reservation(0, 0, 1, request1);
-    start_state = start_state.add_request(0, 0);
+    start_state = start_state.add_request(0, 0, 1, request1);
 
     auto request2_alt1 = ReservationRequest::make_request(
       "table_at_timbre",
@@ -128,8 +124,7 @@ SCENARIO("A few reservations in a state")
       {10s}
     );
     auto request2 = std::vector{request2_alt1};
-    queue->enqueue_reservation(1, 0, 1, request2);
-    start_state = start_state.add_request(1, 0);
+    start_state = start_state.add_request(1, 0, 1, request2);
 
     WHEN("We request the next possible states")
     {
@@ -158,16 +153,25 @@ SCENARIO("A few reservations in a state")
 
     WHEN("We attempt to solve")
     {
-      auto heuristic = std::make_shared<PriorityBasedScorer>(queue);
+      auto heuristic = std::make_shared<PriorityBasedScorer>();
       GreedyBestFirstSearchOptimizer opt(heuristic);
       auto solutions = opt.optimize(start_state);
+      std::vector<float> scores;
       while (auto solution = solutions.next_solution())
       {
-        //solution->debug_state();
+        scores.push_back(heuristic->score(solution.value()));
+      }
+
+      THEN("The scores come back in ascending order")
+      {
+        for(std::size_t i = 1; i < scores.size(); ++i)
+        {
+          REQUIRE(scores[i-1] <= scores[i]);
+        }
       }
     }
 
-    WHEN("Checking for conflict against another reservation")
+    WHEN("Checking for conflict against another non-conflicting reservation")
     {
       Reservation res = Reservation::make_reservation(
         now,
@@ -201,8 +205,7 @@ SCENARIO("Given a single item allocated in a state")
   );
 
   auto request1 = std::vector{request1_alt1};
-  queue->enqueue_reservation(0, 0, 1, request1);
-  start_state = start_state.add_request(0, 0);
+  start_state = start_state.add_request(0, 0, 1, request1);
 
   std::vector<State> next_state;
   for (auto state: start_state)
diff --git a/rmf_traffic/test/unit/reservations/test_Database.cpp b/rmf_traffic/test/unit/reservations/test_Database.cpp
index 3fb68c2f..e3801c17 100644
--- a/rmf_traffic/test/unit/reservations/test_Database.cpp
+++ b/rmf_traffic/test/unit/reservations/test_Database.cpp
@@ -47,6 +47,7 @@ class SimpleParticipant : public Participant
     uint64_t proposal_version
   ) override
   {
+    std::cout << "Participant " << _pid << " Recieved proposal for " << id <<std::endl;
     _received_confirmation[id] = true;
     _proposed[id] = res;
     return true;
@@ -74,6 +75,9 @@ class SimpleParticipant : public Participant
     uint64_t proposal_version
   ) override
   {
+    std::cout << "Recieved unassign proposal " <<std::endl;
+
+    _proposed[id] = std::nullopt;
     _received_cancellation[id] = true;
     return true;
   }
@@ -131,14 +135,14 @@ SCENARIO("Test database behaviour at the start of our lord, the saviour, UNIX")
         "table_at_timbre",
         ReservationRequest::TimeRange::make_time_range(
           now,
-          now+10s
+          now+5s
         ),
         {10s}
       );
 
       auto request1 = std::vector{request1_alt1};
       auto req_id = participant1->make_request_blocking(request1);
-      std::cout << "Got reqid" << req_id<< std::endl;
+      std::cout << "Request: " << req_id << std::endl;
       auto prop = participant1->get_proposal(req_id);
 
       THEN("There should be a successful proposal")
@@ -147,5 +151,32 @@ SCENARIO("Test database behaviour at the start of our lord, the saviour, UNIX")
         REQUIRE(prop.value().resource_name() == "table_at_timbre");
       }
     }
+
+    WHEN("A second participant makes a request with a higher priority")
+    {
+      auto request1_alt1 = ReservationRequest::make_request(
+        "table_at_timbre",
+        ReservationRequest::TimeRange::make_time_range(
+          now,
+          now+5s
+        ),
+        {10s}
+      );
+
+      auto request1 = std::vector{request1_alt1};
+      auto req_id = participant1->make_request_blocking(request1);
+      auto prop = participant1->get_proposal(req_id);
+
+      std::shared_ptr<SimpleParticipant> participant2 =
+        std::make_shared<SimpleParticipant>(1, database, 20);
+      database->register_participant(1, participant2);
+      auto req_id2 = participant2->make_request_blocking(request1);
+      auto prop2 = participant2->get_proposal(req_id2);
+      THEN("The participant with a higher priority should be serviced")
+      {
+        REQUIRE(prop2.has_value());
+        REQUIRE(!participant1->get_proposal(req_id).has_value());
+      }
+    }
   }
 }
\ No newline at end of file

From c008dc67629e465f981f9782ceb261c10d4e9166 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Thu, 22 Jul 2021 09:54:15 +0800
Subject: [PATCH 45/66] Removed threading

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 rmf_traffic/CMakeLists.txt                    |   2 -
 .../src/rmf_traffic/reservations/Database.cpp |  46 ++-----
 .../reservations/internal/ExecutionEngine.hpp | 129 ++++++++----------
 .../reservations/internal/RequestQueue.hpp    |  93 -------------
 .../reservations/internal/State.hpp           |   4 +-
 .../test/unit/reservations/test_Database.cpp  |   5 -
 6 files changed, 74 insertions(+), 205 deletions(-)
 delete mode 100644 rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp

diff --git a/rmf_traffic/CMakeLists.txt b/rmf_traffic/CMakeLists.txt
index c5668b70..24e4ae57 100644
--- a/rmf_traffic/CMakeLists.txt
+++ b/rmf_traffic/CMakeLists.txt
@@ -36,7 +36,6 @@ endif()
 
 find_package(rmf_utils REQUIRED)
 find_package(Eigen3 REQUIRED)
-find_package(Threads)
 
 # ===== Traffic control library
 file(GLOB_RECURSE core_lib_srcs "src/rmf_traffic/*.cpp")
@@ -88,7 +87,6 @@ target_link_libraries(rmf_traffic
     rmf_utils::rmf_utils
   PRIVATE
     ${FCL_LIBRARIES}
-    Threads::Threads
 )
 
 target_include_directories(rmf_traffic
diff --git a/rmf_traffic/src/rmf_traffic/reservations/Database.cpp b/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
index d7f0315e..50dbc55d 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
@@ -18,22 +18,25 @@
 #include <rmf_traffic/reservations/Database.hpp>
 #include "internal/ExecutionEngine.hpp"
 #include "internal/ParticipantStore.hpp"
-#include "internal/RequestQueue.hpp"
 #include "internal/State.hpp"
 namespace rmf_traffic {
 namespace reservations {
 
+//==============================================================================
 class Database::Implementation
 {
 public:
+  //============================================================================
+  // \Constructor
   Implementation()
-  : _request_queue(std::make_shared<RequestQueue>()),
-    _participant_store(std::make_shared<ParticipantStore>()),
-    _execution_engine(_request_queue, _participant_store)
+  : _participant_store(std::make_shared<ParticipantStore>()),
+    _protocol( _participant_store)
   {
 
   }
 
+  //============================================================================]
+  // 
   void register_participant(
     ParticipantId id,
     std::shared_ptr<Participant> participant)
@@ -44,18 +47,9 @@ class Database::Implementation
   }
 
   void unregister_participant(
-    ParticipantId id
-  )
+    ParticipantId id)
   {
-    RequestQueue::Action action
-    {
-      RequestQueue::ActionType::REMOVE_PARTICIPANT,
-      id,
-      0,
-      {},
-      0
-    };
-    _request_queue->add_action(action);
+    _protocol.remove_participant(id);
   }
 
   void request_reservation(
@@ -64,32 +58,16 @@ class Database::Implementation
     std::vector<ReservationRequest>& request_options,
     int priority)
   {
-    RequestQueue::Action add_action{
-      RequestQueue::ActionType::ADD,
-      id,
-      req,
-      request_options,
-      priority
-    };
-    _request_queue->add_action(add_action);
+    _protocol.add_request(id, req, request_options, priority);
   }
 
   void cancel_request(ParticipantId id, RequestId req)
   {
-    RequestQueue::Action action
-    {
-      RequestQueue::ActionType::REMOVE,
-      id,
-      req,
-      {},
-      0
-    };
-    _request_queue->add_action(action);
+    _protocol.remove_request(id, req);
   }
 private:
   std::shared_ptr<ParticipantStore> _participant_store;
-  std::shared_ptr<RequestQueue> _request_queue;
-  ExecutionEngine _execution_engine;
+  Protocol _protocol;
 };
 
 void Database::register_participant(
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp
index 2f11c71a..5efdd7be 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp
@@ -18,9 +18,6 @@
 #ifndef RMF_TRAFFIC__RESERVATIONS__INTERNAL_EXECUTIONENGINE_HPP
 #define RMF_TRAFFIC__RESERVATIONS__INTERNAL_EXECUTIONENGINE_HPP
 
-#include <thread>
-
-#include "RequestQueue.hpp"
 #include "ParticipantStore.hpp"
 #include "Optimizer.hpp"
 #include "State.hpp"
@@ -30,12 +27,9 @@ namespace rmf_traffic {
 namespace reservations {
 
 //==============================================================================
-/// \brief The execution engine class is in charge of handling the requests and
-/// negotiations that occur during the planning and negotiation phase. As such,
-/// it maintains a single thread that handles both planning and negotiations
-/// while enqueueing requests upon a reservation queue. This is where the crux
-/// of the protocol is implemented
-class ExecutionEngine
+/// \brief The Protocol class is in charge of handling the requests and
+/// negotiations that occur during the planning and negotiation phase.
+class Protocol
 {
 public:
   //============================================================================
@@ -45,86 +39,85 @@ class ExecutionEngine
   ///   commands.
   /// \param[in] participant_store - the `ParticipantStore` containing the list
   ///   of participants.
-  ExecutionEngine(
-    std::shared_ptr<RequestQueue> request_queue,
+  Protocol(
     std::shared_ptr<ParticipantStore> pstore)
-  : _request_queue(request_queue),
-    _participant_store(pstore),
+  : _participant_store(pstore),
     _curr_state(std::make_shared<RequestStore>()),
     _proposal_version(0)
   {
-    _execution_thread =
-      std::make_shared<std::thread>(&ExecutionEngine::execute, this);
+
   }
 
   //============================================================================
   /// \brief Destructor. Terminates the execution engine by sending a
   /// TERMINATE_STREAM action and waits for the destructor to join.
-  ~ExecutionEngine()
+  ~Protocol()
   {
-    std::vector<ReservationRequest> res;
-    // Terminate the execution thread
-    RequestQueue::Action terminate_req {
-      RequestQueue::ActionType::TERMINATE_STREAM,
-      0,
-      0,
-      res,
-      0
-    };
-    _request_queue->add_action(terminate_req);
-
-    if (_execution_thread->joinable())
-      _execution_thread->join();
+
   }
 
-private:
   //============================================================================
-  /// \brief Main thread: blocks until a request has come in. When a request
-  /// comes in the current_state is read and updated to contain the request.
-  void execute()
+  /// \brief Add a request to the queue.
+  void add_request(
+    ParticipantId participant_id,
+    RequestId request_id,
+    std::vector<ReservationRequest>& reservation,
+    int priority)
   {
-    while (true)
-    {
-      auto element = _request_queue->deque();
+    _curr_state = _curr_state.add_request(
+        participant_id,
+        request_id,
+        priority,
+        reservation
+      );
 
-      if (element.action.type == RequestQueue::ActionType::TERMINATE_STREAM)
-        return;
+    auto heuristic = std::make_shared<PriorityBasedScorer>();
 
-      if (element.action.type == RequestQueue::ActionType::ADD)
-      {
-        _curr_state = _curr_state.add_request(
-          element.action.participant_id,
-          element.action.request_id,
-          element.action.priority,
-          element.action.request_options
-        );
-      }
-      else if (element.action.type == RequestQueue::ActionType::REMOVE)
-      {
-        _curr_state = _curr_state.remove_request(
-          element.action.participant_id,
-          element.action.request_id);
-      }
-      else if (element.action.type ==
-        RequestQueue::ActionType::REMOVE_PARTICIPANT)
-      {
-        _curr_state = _curr_state.remove_participant(
-          element.action.participant_id);
-        _participant_store->remove_participant(element.action.participant_id);
-      }
+    GreedyBestFirstSearchOptimizer optimizer(heuristic);
+    auto favored_solution = get_favored_solution(optimizer);
+    if (!favored_solution.has_value())
+      return;
 
-      auto heuristic = std::make_shared<PriorityBasedScorer>();
+    rollout(favored_solution.value());
+  }
 
-      GreedyBestFirstSearchOptimizer optimizer(heuristic);
-      auto favored_solution = get_favored_solution(optimizer);
-      if (!favored_solution.has_value())
-        continue;
+  //============================================================================
+  /// \brief Remove a request
+  void remove_request(
+    ParticipantId participant_id,
+    RequestId request_id)
+  {
+    _curr_state = _curr_state.remove_request(
+      participant_id, request_id);
 
-      rollout(favored_solution.value());
+    auto heuristic = std::make_shared<PriorityBasedScorer>();
 
-    }
+    GreedyBestFirstSearchOptimizer optimizer(heuristic);
+    auto favored_solution = get_favored_solution(optimizer);
+    if (!favored_solution.has_value())
+      return;
+
+    rollout(favored_solution.value());
+  }
+
+  //============================================================================
+  /// \brief Remove a participant
+  void remove_participant(ParticipantId participant_id)
+  {
+    _curr_state = _curr_state.remove_participant(participant_id);
+    _participant_store->remove_participant(participant_id);
+
+    auto heuristic = std::make_shared<PriorityBasedScorer>();
+
+    GreedyBestFirstSearchOptimizer optimizer(heuristic);
+    auto favored_solution = get_favored_solution(optimizer);
+    if (!favored_solution.has_value())
+      return;
+
+    rollout(favored_solution.value());
   }
 
+private:
   //============================================================================
   /// \brief Rolls out the proposed changes.
   /// TODO: Consider making asynchronous
@@ -259,8 +252,6 @@ class ExecutionEngine
     return true;
   }
 
-  std::shared_ptr<std::thread> _execution_thread;
-  std::shared_ptr<RequestQueue> _request_queue;
   std::shared_ptr<ParticipantStore> _participant_store;
   uint64_t _proposal_version;
   State _curr_state;
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp
deleted file mode 100644
index e9d37f18..00000000
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestQueue.hpp
+++ /dev/null
@@ -1,93 +0,0 @@
-/*
- * Copyright (C) 2020 Open Source Robotics Foundation
- *
- * 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.
- *
-*/
-
-#ifndef RMF_TRAFFIC__RESERVATIONS__INTERNAL_REQUESTQUEUE_HPP
-#define RMF_TRAFFIC__RESERVATIONS__INTERNAL_REQUESTQUEUE_HPP
-
-#include <rmf_traffic/reservations/ReservationRequest.hpp>
-#include <rmf_traffic/reservations/Reservation.hpp>
-#include <rmf_traffic/reservations/Participant.hpp>
-#include <unordered_map>
-#include <deque>
-#include <mutex>
-#include <condition_variable>
-#include "RequestStore.hpp"
-
-namespace rmf_traffic {
-namespace reservations {
-
-/// \brief The RequestQueue is used to coordinate dispatch to a the
-/// ExecutionEngine.
-class RequestQueue
-{
-public:
-  enum ActionType
-  {
-    /// \brief Add a request
-    ADD,
-    /// \brief Remove a request
-    REMOVE,
-    /// \brief Remove an entire participant
-    REMOVE_PARTICIPANT,
-    /// \brief Terminates the execution engine. Only the ExecutionEngine class
-    /// should use this.
-    TERMINATE_STREAM
-  };
-  /// This represents the action which you wish to execute.
-  struct Action
-  {
-    ActionType type;
-    ParticipantId participant_id;
-    RequestId request_id;
-    std::vector<ReservationRequest> request_options;
-    int priority;
-  };
-  struct QueueElement
-  {
-    Action action;
-
-  };
-
-  void add_action(Action action)
-  {
-    {
-      std::lock_guard lock(_mutex);
-      _request_store_queue.push_back({action});
-    }
-    _cond.notify_one();
-  }
-
-  QueueElement deque()
-  {
-    std::unique_lock loc(_mutex);
-    _cond.wait(loc, [=]() {return !_request_store_queue.empty();});
-    auto front = _request_store_queue.front();
-    _request_store_queue.pop_front();
-    loc.unlock();
-    return front;
-  }
-
-private:
-  std::deque<QueueElement> _request_store_queue;
-  std::mutex _mutex;
-  std::condition_variable _cond;
-};
-
-}
-}
-
-#endif
\ No newline at end of file
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index 3e5ab201..7f98e813 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -140,6 +140,7 @@ class State
   }
 
   //============================================================================
+  /// Remove a participant
   State remove_participant(
     ParticipantId pid)
   {
@@ -294,6 +295,7 @@ class State
       _reservation_assignments.find(pid)->second.find(reqid)->second;
     auto resource = _reservation_resources.find(res_id)->second;
     new_state.unassign_reservation(res_id, resource);
+    return new_state;
   }
 
   //============================================================================
@@ -567,8 +569,6 @@ struct NextStateGenerator
       part_id,
       req_id)->request_options[request_index_iter];
 
-    bool found_slot = false;
-
     proceed_next_resource = false;
 
     while (insertion_point_iter !=
diff --git a/rmf_traffic/test/unit/reservations/test_Database.cpp b/rmf_traffic/test/unit/reservations/test_Database.cpp
index e3801c17..57b6a6bf 100644
--- a/rmf_traffic/test/unit/reservations/test_Database.cpp
+++ b/rmf_traffic/test/unit/reservations/test_Database.cpp
@@ -95,11 +95,6 @@ class SimpleParticipant : public Participant
       _priority
     );
 
-    // Busy wait... Bad but oh well...this is a unit test
-    while (
-      !_received_confirmation[_reqid] &&
-      !_received_cancellation[_reqid]) {}
-
     return _reqid++;
   }
 

From 341be05d2e8b0396cb003af32353ba246d11cbc3 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Thu, 22 Jul 2021 09:59:03 +0800
Subject: [PATCH 46/66] Rename ExecutionEngine

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 rmf_traffic/src/rmf_traffic/reservations/Database.cpp           | 2 +-
 .../reservations/internal/{ExecutionEngine.hpp => Protocol.hpp} | 0
 2 files changed, 1 insertion(+), 1 deletion(-)
 rename rmf_traffic/src/rmf_traffic/reservations/internal/{ExecutionEngine.hpp => Protocol.hpp} (100%)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/Database.cpp b/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
index 50dbc55d..e8790e93 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
@@ -16,7 +16,7 @@
 */
 
 #include <rmf_traffic/reservations/Database.hpp>
-#include "internal/ExecutionEngine.hpp"
+#include "internal/Protocol.hpp"
 #include "internal/ParticipantStore.hpp"
 #include "internal/State.hpp"
 namespace rmf_traffic {
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/Protocol.hpp
similarity index 100%
rename from rmf_traffic/src/rmf_traffic/reservations/internal/ExecutionEngine.hpp
rename to rmf_traffic/src/rmf_traffic/reservations/internal/Protocol.hpp

From 184749a609726031cda78dba51c93abb2341e347 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Thu, 22 Jul 2021 10:44:43 +0800
Subject: [PATCH 47/66] Rename header

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 rmf_traffic/src/rmf_traffic/reservations/Database.cpp         | 4 ++--
 .../src/rmf_traffic/reservations/internal/Protocol.hpp        | 4 ++--
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/Database.cpp b/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
index e8790e93..a32f94a6 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
@@ -36,7 +36,7 @@ class Database::Implementation
   }
 
   //============================================================================]
-  // 
+  // Register a participant
   void register_participant(
     ParticipantId id,
     std::shared_ptr<Participant> participant)
@@ -45,7 +45,7 @@ class Database::Implementation
     /// overwrites or otherwise use the queue.
     _participant_store->add_participant(id, participant);
   }
-
+  
   void unregister_participant(
     ParticipantId id)
   {
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/Protocol.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/Protocol.hpp
index 5efdd7be..156d2491 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/Protocol.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/Protocol.hpp
@@ -15,8 +15,8 @@
  *
 */
 
-#ifndef RMF_TRAFFIC__RESERVATIONS__INTERNAL_EXECUTIONENGINE_HPP
-#define RMF_TRAFFIC__RESERVATIONS__INTERNAL_EXECUTIONENGINE_HPP
+#ifndef RMF_TRAFFIC__RESERVATIONS__INTERNAL_PROTOCOL_HPP
+#define RMF_TRAFFIC__RESERVATIONS__INTERNAL_PROTOCOL_HPP
 
 #include "ParticipantStore.hpp"
 #include "Optimizer.hpp"

From f40bfceae3ced2e26d2df2dd2cca0a7b672c717f Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Thu, 22 Jul 2021 10:58:25 +0800
Subject: [PATCH 48/66] Style fixes

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../src/rmf_traffic/reservations/Database.cpp        | 12 +++++-------
 .../rmf_traffic/reservations/internal/Protocol.hpp   | 10 +++++-----
 .../reservations/internal/RequestStore.hpp           |  2 +-
 .../src/rmf_traffic/reservations/internal/State.hpp  |  5 +++--
 .../test/unit/reservations/internal/test_State.cpp   |  2 +-
 rmf_traffic/test/unit/reservations/test_Database.cpp |  4 ----
 6 files changed, 15 insertions(+), 20 deletions(-)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/Database.cpp b/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
index a32f94a6..74ce52a4 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
@@ -27,25 +27,23 @@ class Database::Implementation
 {
 public:
   //============================================================================
-  // \Constructor
+  /// \constructor
   Implementation()
   : _participant_store(std::make_shared<ParticipantStore>()),
-    _protocol( _participant_store)
+    _protocol(_participant_store)
   {
-
+    // Do nothing
   }
 
   //============================================================================]
-  // Register a participant
+  /// \brief Register a participant
   void register_participant(
     ParticipantId id,
     std::shared_ptr<Participant> participant)
   {
-    /// TODO(arjo): add some type of locking mechanism that will prevent
-    /// overwrites or otherwise use the queue.
     _participant_store->add_participant(id, participant);
   }
-  
+
   void unregister_participant(
     ParticipantId id)
   {
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/Protocol.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/Protocol.hpp
index 156d2491..8adef38c 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/Protocol.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/Protocol.hpp
@@ -65,11 +65,11 @@ class Protocol
     int priority)
   {
     _curr_state = _curr_state.add_request(
-        participant_id,
-        request_id,
-        priority,
-        reservation
-      );
+      participant_id,
+      request_id,
+      priority,
+      reservation
+    );
 
     auto heuristic = std::make_shared<PriorityBasedScorer>();
 
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp
index c9006da5..2815114c 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp
@@ -40,7 +40,7 @@ class RequestStore
     ParticipantId pid,
     RequestId req)
   {
-    if(_reservation_info.count(pid) != 0
+    if (_reservation_info.count(pid) != 0
       && _reservation_info.at(pid).count(req) != 0)
       return _reservation_info[pid][req];
     return std::nullopt;
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index 7f98e813..86ae3291 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -807,11 +807,12 @@ NextStateGenerator State::begin()
       _unassigned.begin()->first,
       _unassigned.begin()->second);
 
-    if(!request.has_value())
+    if (!request.has_value())
     {
+      // This should *never* happen
       throw std::runtime_error(std::string("Got nullopt for participant ") +
         std::to_string(_unassigned.begin()->first) +
-        std::string(" request id #")+ 
+        std::string(" request id #") +
         std::to_string(_unassigned.begin()->second));
     }
 
diff --git a/rmf_traffic/test/unit/reservations/internal/test_State.cpp b/rmf_traffic/test/unit/reservations/internal/test_State.cpp
index 10f7d092..2dd05657 100644
--- a/rmf_traffic/test/unit/reservations/internal/test_State.cpp
+++ b/rmf_traffic/test/unit/reservations/internal/test_State.cpp
@@ -164,7 +164,7 @@ SCENARIO("A few reservations in a state")
 
       THEN("The scores come back in ascending order")
       {
-        for(std::size_t i = 1; i < scores.size(); ++i)
+        for (std::size_t i = 1; i < scores.size(); ++i)
         {
           REQUIRE(scores[i-1] <= scores[i]);
         }
diff --git a/rmf_traffic/test/unit/reservations/test_Database.cpp b/rmf_traffic/test/unit/reservations/test_Database.cpp
index 57b6a6bf..3b4b3672 100644
--- a/rmf_traffic/test/unit/reservations/test_Database.cpp
+++ b/rmf_traffic/test/unit/reservations/test_Database.cpp
@@ -47,7 +47,6 @@ class SimpleParticipant : public Participant
     uint64_t proposal_version
   ) override
   {
-    std::cout << "Participant " << _pid << " Recieved proposal for " << id <<std::endl;
     _received_confirmation[id] = true;
     _proposed[id] = res;
     return true;
@@ -75,8 +74,6 @@ class SimpleParticipant : public Participant
     uint64_t proposal_version
   ) override
   {
-    std::cout << "Recieved unassign proposal " <<std::endl;
-
     _proposed[id] = std::nullopt;
     _received_cancellation[id] = true;
     return true;
@@ -137,7 +134,6 @@ SCENARIO("Test database behaviour at the start of our lord, the saviour, UNIX")
 
       auto request1 = std::vector{request1_alt1};
       auto req_id = participant1->make_request_blocking(request1);
-      std::cout << "Request: " << req_id << std::endl;
       auto prop = participant1->get_proposal(req_id);
 
       THEN("There should be a successful proposal")

From 2a13f44a1c409e434e3a764cd226db9795b0b456 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Thu, 22 Jul 2021 11:17:04 +0800
Subject: [PATCH 49/66] style fixes

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../src/rmf_traffic/reservations/internal/State.hpp       | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index 86ae3291..cd08ab1e 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -811,10 +811,10 @@ NextStateGenerator State::begin()
     {
       // This should *never* happen
       throw std::runtime_error(std::string("Got nullopt for participant ") +
-        std::to_string(_unassigned.begin()->first) +
-        std::string(" request id #") +
-        std::to_string(_unassigned.begin()->second));
-    }
+              std::to_string(_unassigned.begin()->first) +
+              std::string(" request id #") +
+              std::to_string(_unassigned.begin()->second));
+      }
 
     auto current_request = request->request_options[0];
 

From 3ba60064c9fdfaf2b9203cc8b57c1d84efd58dc3 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Thu, 22 Jul 2021 11:20:12 +0800
Subject: [PATCH 50/66] add debug flags to asan, for some reason cmake-args is
 being ignored.

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .github/workflows/asan.yaml | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/.github/workflows/asan.yaml b/.github/workflows/asan.yaml
index 2e450a40..2405709a 100644
--- a/.github/workflows/asan.yaml
+++ b/.github/workflows/asan.yaml
@@ -18,8 +18,8 @@ jobs:
       - name: build_and_test
         uses: ros-tooling/action-ros-ci@v0.2
         env:
-          CC: clang -fsanitize-blacklist=/home/runner/work/blacklist.txt
-          CXX: clang++ -fsanitize-blacklist=/home/runner/work/blacklist.txt
+          CC: clang -fsanitize-blacklist=/home/runner/work/blacklist.txt -g3
+          CXX: clang++ -fsanitize-blacklist=/home/runner/work/blacklist.txt -g3
         with:
           target-ros2-distro: foxy
           # build all packages listed in the meta package

From b2e318ded782204e39f3ff886eaa80f8d437bd39 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Thu, 22 Jul 2021 11:32:59 +0800
Subject: [PATCH 51/66] style

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index cd08ab1e..83ee8362 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -814,7 +814,7 @@ NextStateGenerator State::begin()
               std::to_string(_unassigned.begin()->first) +
               std::string(" request id #") +
               std::to_string(_unassigned.begin()->second));
-      }
+    }
 
     auto current_request = request->request_options[0];
 

From c231cae284e2e4b5c5f88d50ac6af972da472446 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Thu, 22 Jul 2021 13:43:41 +0800
Subject: [PATCH 52/66] Adding test cases for cancellation

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../reservations/internal/Protocol.hpp        | 10 +--
 .../test/unit/reservations/test_Database.cpp  | 66 +++++++++++++++++++
 2 files changed, 72 insertions(+), 4 deletions(-)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/Protocol.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/Protocol.hpp
index 8adef38c..e82a721e 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/Protocol.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/Protocol.hpp
@@ -125,8 +125,10 @@ class Protocol
   {
     for (auto change: changes.differences())
     {
-      assert(
-        _participant_store->get_participant(change.participant_id).has_value());
+      if(!_participant_store->get_participant(change.participant_id).has_value())
+      {
+        continue;
+      }
 
       auto participant =
         _participant_store->get_participant(change.participant_id).value();
@@ -221,8 +223,8 @@ class Protocol
     {
       auto impacted_participant = diff.participant_id;
 
-      assert(
-        _participant_store->get_participant(impacted_participant).has_value());
+      if(!_participant_store->get_participant(impacted_participant).has_value())
+        continue;
 
       auto participant =
         _participant_store->get_participant(impacted_participant)
diff --git a/rmf_traffic/test/unit/reservations/test_Database.cpp b/rmf_traffic/test/unit/reservations/test_Database.cpp
index 3b4b3672..f3637cf9 100644
--- a/rmf_traffic/test/unit/reservations/test_Database.cpp
+++ b/rmf_traffic/test/unit/reservations/test_Database.cpp
@@ -100,6 +100,11 @@ class SimpleParticipant : public Participant
     return _proposed[rid];
   }
 
+  ParticipantId get_id() const
+  {
+    return _pid;
+  }
+
 private:
   ParticipantId _pid;
   RequestId _reqid;
@@ -169,5 +174,66 @@ SCENARIO("Test database behaviour at the start of our lord, the saviour, UNIX")
         REQUIRE(!participant1->get_proposal(req_id).has_value());
       }
     }
+
+    WHEN("We remove the second participant")
+    {
+      auto request1_alt1 = ReservationRequest::make_request(
+        "table_at_timbre",
+        ReservationRequest::TimeRange::make_time_range(
+          now,
+          now+5s
+        ),
+        {10s}
+      );
+
+      auto request1 = std::vector{request1_alt1};
+      auto req_id = participant1->make_request_blocking(request1);
+      auto prop = participant1->get_proposal(req_id);
+
+      std::shared_ptr<SimpleParticipant> participant2 =
+        std::make_shared<SimpleParticipant>(1, database, 20);
+      database->register_participant(1, participant2);
+      auto req_id2 = participant2->make_request_blocking(request1);
+      auto prop2 = participant2->get_proposal(req_id2);
+      REQUIRE(prop2.has_value());
+      REQUIRE(!participant1->get_proposal(req_id).has_value());
+
+      database->unregister_participant(participant2->get_id());
+      THEN("Participant 1 will regain its reservation")
+      {
+        //REQUIRE(participant1->get_proposal(req_id).has_value());
+      }
+    }
+
+
+    WHEN("We cancel the second reservation")
+    {
+      auto request1_alt1 = ReservationRequest::make_request(
+        "table_at_timbre",
+        ReservationRequest::TimeRange::make_time_range(
+          now,
+          now+5s
+        ),
+        {10s}
+      );
+
+      auto request1 = std::vector{request1_alt1};
+      auto req_id = participant1->make_request_blocking(request1);
+      auto prop = participant1->get_proposal(req_id);
+
+      std::shared_ptr<SimpleParticipant> participant2 =
+        std::make_shared<SimpleParticipant>(1, database, 20);
+      database->register_participant(1, participant2);
+      auto req_id2 = participant2->make_request_blocking(request1);
+      auto prop2 = participant2->get_proposal(req_id2);
+      REQUIRE(prop2.has_value());
+      REQUIRE(!participant1->get_proposal(req_id).has_value());
+
+      database->cancel_request(participant2->get_id(), req_id2);
+      THEN("Participant 1 will regain its reservation.")
+      {
+        REQUIRE(participant1->get_proposal(req_id).has_value());
+      }
+    }
   }
 }
\ No newline at end of file

From 2d6f3f20fcf26aa4f30c654d3988eaacc62dc797 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Thu, 22 Jul 2021 13:59:51 +0800
Subject: [PATCH 53/66] fix style

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 rmf_traffic/src/rmf_traffic/reservations/internal/Protocol.hpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/Protocol.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/Protocol.hpp
index e82a721e..754ec0a2 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/Protocol.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/Protocol.hpp
@@ -125,7 +125,7 @@ class Protocol
   {
     for (auto change: changes.differences())
     {
-      if(!_participant_store->get_participant(change.participant_id).has_value())
+      if (!_participant_store->get_participant(change.participant_id).has_value())
       {
         continue;
       }

From 02401ef1fca3d5d7367b26d3b0eca319fd6aa0f5 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Thu, 22 Jul 2021 14:27:53 +0800
Subject: [PATCH 54/66] Fix unregistration

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp | 6 ++++++
 rmf_traffic/test/unit/reservations/test_Database.cpp        | 2 +-
 2 files changed, 7 insertions(+), 1 deletion(-)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index 83ee8362..1a03a6a6 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -150,6 +150,12 @@ class State
     {
       new_state = new_state.remove_request(pid, request);
     }
+
+    for (auto [participant_id, reqid] : _unassigned)
+    {
+      if(participant_id == pid)
+        new_state = new_state.remove_request(participant_id, reqid);
+    }
     return new_state;
   }
 
diff --git a/rmf_traffic/test/unit/reservations/test_Database.cpp b/rmf_traffic/test/unit/reservations/test_Database.cpp
index f3637cf9..b368a7c5 100644
--- a/rmf_traffic/test/unit/reservations/test_Database.cpp
+++ b/rmf_traffic/test/unit/reservations/test_Database.cpp
@@ -201,7 +201,7 @@ SCENARIO("Test database behaviour at the start of our lord, the saviour, UNIX")
       database->unregister_participant(participant2->get_id());
       THEN("Participant 1 will regain its reservation")
       {
-        //REQUIRE(participant1->get_proposal(req_id).has_value());
+        REQUIRE(participant1->get_proposal(req_id).has_value());
       }
     }
 

From 658ba42a408396eca416a3823719060b2fdee7b3 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Thu, 22 Jul 2021 14:28:14 +0800
Subject: [PATCH 55/66] style fixes

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 rmf_traffic/src/rmf_traffic/reservations/internal/Protocol.hpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/Protocol.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/Protocol.hpp
index 754ec0a2..6abcb844 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/Protocol.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/Protocol.hpp
@@ -223,7 +223,7 @@ class Protocol
     {
       auto impacted_participant = diff.participant_id;
 
-      if(!_participant_store->get_participant(impacted_participant).has_value())
+      if (!_participant_store->get_participant(impacted_participant).has_value())
         continue;
 
       auto participant =

From eb0c34b6ae4c02235fb8f084ac945fe9a0044894 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Thu, 22 Jul 2021 15:00:41 +0800
Subject: [PATCH 56/66] documentation and style fixes.

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../rmf_traffic/reservations/Reservation.hpp  | 47 +++++++++++++++++++
 .../src/rmf_traffic/reservations/Database.cpp | 11 ++++-
 .../reservations/internal/State.hpp           |  2 +-
 3 files changed, 58 insertions(+), 2 deletions(-)

diff --git a/rmf_traffic/include/rmf_traffic/reservations/Reservation.hpp b/rmf_traffic/include/rmf_traffic/reservations/Reservation.hpp
index f81ef8d8..15627361 100644
--- a/rmf_traffic/include/rmf_traffic/reservations/Reservation.hpp
+++ b/rmf_traffic/include/rmf_traffic/reservations/Reservation.hpp
@@ -29,35 +29,80 @@ namespace reservations {
 
 using ReservationId = uint64_t;
 //==============================================================================
+/// \brief The Reservation class represents a proposed solution to a given
+/// ReservationRequest.
 class Reservation
 {
 public:
+
+  ///===========================================================================
+  /// \brief The time at which the Reservation will start.
   const rmf_traffic::Time start_time() const;
 
+  ///===========================================================================
+  /// \brief The minimum duration of the Reservation.
   const std::optional<rmf_traffic::Duration> duration() const;
 
+  ///===========================================================================
+  /// \brief The earliest finish time
   const std::optional<rmf_traffic::Time> finish_time() const;
 
+  ///===========================================================================
+  /// \brief Shifts the actual finish time
   void set_actual_finish_time(rmf_traffic::Time dur);
 
+  ///===========================================================================
+  /// \brief creates a new reservation with a different finish time based on
+  /// this reservation.
   Reservation propose_new_finish_time(rmf_traffic::Time dur);
 
+  ///===========================================================================
+  /// \brief creates a new reservation with a different start time based on
+  /// this reservation.
   Reservation propose_new_start_time(rmf_traffic::Time dur);
 
+  ///===========================================================================
+  /// \brief Returns true if the reservation is indefinite. An indefinite
+  /// reservation has no duration or finish time.
   bool is_indefinite() const;
 
+  ///===========================================================================
+  /// \brief Returns the actual finish time of the reservation. If the
+  /// reservation is indefinite then it returns a `std::nullopt`.
   const std::optional<rmf_traffic::Time> actual_finish_time() const;
 
+  ///===========================================================================
+  /// \brief Returns the resource which is being reserved by the reservation.
   const std::string resource_name() const;
 
+  ///===========================================================================
+  /// \brief Returns a reservation id. Used internally to
   ReservationId reservation_id() const;
 
+  ///===========================================================================
+  /// \brief Returns true if two reservations are overlapping.
   bool conflicts_with(const Reservation& other) const;
 
+  ///===========================================================================
+  /// \brief Checks equality of two reservations. Note: reservation ids are not
+  /// compared.
   bool operator==(const Reservation& other) const;
 
+  ///===========================================================================
+  /// \brief Checks inequality of two reservations. Note: reservation ids are
+  /// not compared.
   bool operator!=(const Reservation& other) const;
 
+  ///===========================================================================
+  /// \brief Creates a reservation.
+  /// \param[in] start_time The time at which the reservation will start.
+  /// \param[in] resource_name The name of the resource which is being reserved.
+  /// \param[in] duration The minimum duration of the reservation.
+  /// \param[in] finish_time The earliest finish time.
+  /// Note: If both `duration` and `finish_time` are `std::nullopt` then the
+  /// reservation is indefinite. If both `duration` and `finish_time` are not
+  /// `std::nullopt` then the actual finish time is
+  /// $max(finish_time, start_time + duration)$
   static Reservation make_reservation(
     rmf_traffic::Time start_time,
     std::string resource_name,
@@ -66,6 +111,8 @@ class Reservation
 
   class Implementation;
 
+  ///===========================================================================
+  /// \brief Constructor
   Reservation();
 
 private:
diff --git a/rmf_traffic/src/rmf_traffic/reservations/Database.cpp b/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
index 74ce52a4..8eefe824 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
@@ -35,7 +35,7 @@ class Database::Implementation
     // Do nothing
   }
 
-  //============================================================================]
+  //============================================================================
   /// \brief Register a participant
   void register_participant(
     ParticipantId id,
@@ -44,12 +44,14 @@ class Database::Implementation
     _participant_store->add_participant(id, participant);
   }
 
+  //============================================================================
   void unregister_participant(
     ParticipantId id)
   {
     _protocol.remove_participant(id);
   }
 
+  //============================================================================
   void request_reservation(
     ParticipantId id,
     RequestId req,
@@ -59,6 +61,7 @@ class Database::Implementation
     _protocol.add_request(id, req, request_options, priority);
   }
 
+  //============================================================================
   void cancel_request(ParticipantId id, RequestId req)
   {
     _protocol.remove_request(id, req);
@@ -68,6 +71,7 @@ class Database::Implementation
   Protocol _protocol;
 };
 
+//==============================================================================
 void Database::register_participant(
   ParticipantId id,
   std::shared_ptr<Participant> participant)
@@ -78,11 +82,13 @@ void Database::register_participant(
   );
 }
 
+//==============================================================================
 void Database::unregister_participant(ParticipantId id)
 {
   _pimpl->unregister_participant(id);
 }
 
+//==============================================================================
 void Database::request_reservation(
   ParticipantId id,
   RequestId req,
@@ -92,10 +98,13 @@ void Database::request_reservation(
   _pimpl->request_reservation(id, req, request_options, priority);
 }
 
+//==============================================================================
 void Database::cancel_request(ParticipantId id, RequestId req)
 {
   _pimpl->cancel_request(id, req);
 }
+
+//==============================================================================
 Database::Database()
 : _pimpl(rmf_utils::make_impl<Implementation>())
 {
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index 1a03a6a6..78ce2b36 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -153,7 +153,7 @@ class State
 
     for (auto [participant_id, reqid] : _unassigned)
     {
-      if(participant_id == pid)
+      if (participant_id == pid)
         new_state = new_state.remove_request(participant_id, reqid);
     }
     return new_state;

From 2e550097399a544a0e9193b7f1bb6326cdc1d6ed Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Fri, 23 Jul 2021 08:26:45 +0800
Subject: [PATCH 57/66] remove circular dependency

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../test/unit/reservations/test_Database.cpp  | 33 ++++++++-----------
 1 file changed, 13 insertions(+), 20 deletions(-)

diff --git a/rmf_traffic/test/unit/reservations/test_Database.cpp b/rmf_traffic/test/unit/reservations/test_Database.cpp
index b368a7c5..437b9ca1 100644
--- a/rmf_traffic/test/unit/reservations/test_Database.cpp
+++ b/rmf_traffic/test/unit/reservations/test_Database.cpp
@@ -32,10 +32,8 @@ class SimpleParticipant : public Participant
 public:
   SimpleParticipant(
     ParticipantId pid,
-    std::shared_ptr<Database> db,
     int priority = 1)
   : _pid(pid),
-    _db(db),
     _reqid(0),
     _priority(priority)
   {
@@ -47,7 +45,6 @@ class SimpleParticipant : public Participant
     uint64_t proposal_version
   ) override
   {
-    _received_confirmation[id] = true;
     _proposed[id] = res;
     return true;
   }
@@ -75,16 +72,14 @@ class SimpleParticipant : public Participant
   ) override
   {
     _proposed[id] = std::nullopt;
-    _received_cancellation[id] = true;
     return true;
   }
 
   RequestId make_request_blocking(
+    std::shared_ptr<Database>& _db,
     std::vector<ReservationRequest>& request_options
   )
   {
-    _received_confirmation[_reqid] = false;
-    _received_cancellation[_reqid] = false;
     _db->request_reservation(
       _pid,
       _reqid,
@@ -108,9 +103,7 @@ class SimpleParticipant : public Participant
 private:
   ParticipantId _pid;
   RequestId _reqid;
-  std::shared_ptr<Database> _db;
-  std::unordered_map<RequestId, bool> _received_confirmation;
-  std::unordered_map<RequestId, bool> _received_cancellation;
+
   std::unordered_map<RequestId, std::optional<Reservation>> _proposed;
   int _priority;
 };
@@ -123,7 +116,7 @@ SCENARIO("Test database behaviour at the start of our lord, the saviour, UNIX")
   {
     std::shared_ptr<Database> database = std::make_shared<Database>();
     std::shared_ptr<SimpleParticipant> participant1 =
-      std::make_shared<SimpleParticipant>(0, database);
+      std::make_shared<SimpleParticipant>(0);
     database->register_participant(0, participant1);
 
     WHEN("the participant makes a request")
@@ -138,7 +131,7 @@ SCENARIO("Test database behaviour at the start of our lord, the saviour, UNIX")
       );
 
       auto request1 = std::vector{request1_alt1};
-      auto req_id = participant1->make_request_blocking(request1);
+      auto req_id = participant1->make_request_blocking(database, request1);
       auto prop = participant1->get_proposal(req_id);
 
       THEN("There should be a successful proposal")
@@ -160,13 +153,13 @@ SCENARIO("Test database behaviour at the start of our lord, the saviour, UNIX")
       );
 
       auto request1 = std::vector{request1_alt1};
-      auto req_id = participant1->make_request_blocking(request1);
+      auto req_id = participant1->make_request_blocking(database, request1);
       auto prop = participant1->get_proposal(req_id);
 
       std::shared_ptr<SimpleParticipant> participant2 =
-        std::make_shared<SimpleParticipant>(1, database, 20);
+        std::make_shared<SimpleParticipant>(1, 20);
       database->register_participant(1, participant2);
-      auto req_id2 = participant2->make_request_blocking(request1);
+      auto req_id2 = participant2->make_request_blocking(database, request1);
       auto prop2 = participant2->get_proposal(req_id2);
       THEN("The participant with a higher priority should be serviced")
       {
@@ -187,13 +180,13 @@ SCENARIO("Test database behaviour at the start of our lord, the saviour, UNIX")
       );
 
       auto request1 = std::vector{request1_alt1};
-      auto req_id = participant1->make_request_blocking(request1);
+      auto req_id = participant1->make_request_blocking(database, request1);
       auto prop = participant1->get_proposal(req_id);
 
       std::shared_ptr<SimpleParticipant> participant2 =
-        std::make_shared<SimpleParticipant>(1, database, 20);
+        std::make_shared<SimpleParticipant>(1, 20);
       database->register_participant(1, participant2);
-      auto req_id2 = participant2->make_request_blocking(request1);
+      auto req_id2 = participant2->make_request_blocking(database, request1);
       auto prop2 = participant2->get_proposal(req_id2);
       REQUIRE(prop2.has_value());
       REQUIRE(!participant1->get_proposal(req_id).has_value());
@@ -218,13 +211,13 @@ SCENARIO("Test database behaviour at the start of our lord, the saviour, UNIX")
       );
 
       auto request1 = std::vector{request1_alt1};
-      auto req_id = participant1->make_request_blocking(request1);
+      auto req_id = participant1->make_request_blocking(database, request1);
       auto prop = participant1->get_proposal(req_id);
 
       std::shared_ptr<SimpleParticipant> participant2 =
-        std::make_shared<SimpleParticipant>(1, database, 20);
+        std::make_shared<SimpleParticipant>(1, 20);
       database->register_participant(1, participant2);
-      auto req_id2 = participant2->make_request_blocking(request1);
+      auto req_id2 = participant2->make_request_blocking(database, request1);
       auto prop2 = participant2->get_proposal(req_id2);
       REQUIRE(prop2.has_value());
       REQUIRE(!participant1->get_proposal(req_id).has_value());

From 5240770aebccb16e3e50fe71a08f37abaa41d7e3 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Fri, 23 Jul 2021 10:10:13 +0800
Subject: [PATCH 58/66] fix `conflicts_with`

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../rmf_traffic/reservations/Reservation.cpp  | 17 +++-
 .../test/unit/reservations/test_Database.cpp  | 65 +++++++++++++-
 .../unit/reservations/test_Reservation.cpp    | 88 +++++++++++++++++++
 3 files changed, 163 insertions(+), 7 deletions(-)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp b/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp
index 0464df8c..226ff6a0 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp
@@ -119,21 +119,30 @@ ReservationId Reservation::reservation_id() const
 //=============================================================================
 bool Reservation::conflicts_with(const Reservation& other) const
 {
+  if (other.resource_name() != this->resource_name())
+    return false;
+
   if (other.start_time() < this->start_time())
   {
+    // other starts before this
     if (!other.is_indefinite()
       && other.actual_finish_time() <= this->start_time())
-      return true;
-    else
+      //other is not indefinite and start time of other is after this one's
+      //finish time
       return false;
+    else
+      //Either other is indefinite in which case it will conflict with this
+      //or the actual start time of this is before other finishes.
+      return true;
   }
   else
   {
+    // this starts before other
     if (!this->is_indefinite()
       && this->actual_finish_time() <= other.start_time())
-      return true;
-    else
       return false;
+    else
+      return true ;
   }
 }
 
diff --git a/rmf_traffic/test/unit/reservations/test_Database.cpp b/rmf_traffic/test/unit/reservations/test_Database.cpp
index 437b9ca1..293c107f 100644
--- a/rmf_traffic/test/unit/reservations/test_Database.cpp
+++ b/rmf_traffic/test/unit/reservations/test_Database.cpp
@@ -19,8 +19,6 @@
 #include <rmf_traffic/reservations/Database.hpp>
 #include <rmf_traffic/reservations/Reservation.hpp>
 
-#include <iostream>
-
 using namespace rmf_traffic;
 using namespace rmf_traffic::reservations;
 using namespace std::chrono_literals;
@@ -199,7 +197,7 @@ SCENARIO("Test database behaviour at the start of our lord, the saviour, UNIX")
     }
 
 
-    WHEN("We cancel the second reservation")
+    WHEN("We cancel the reservation of the second participant")
     {
       auto request1_alt1 = ReservationRequest::make_request(
         "table_at_timbre",
@@ -228,5 +226,66 @@ SCENARIO("Test database behaviour at the start of our lord, the saviour, UNIX")
         REQUIRE(participant1->get_proposal(req_id).has_value());
       }
     }
+
+    WHEN("We make three requests")
+    {
+      auto request1_alt1 = ReservationRequest::make_request(
+        "table_at_timbre",
+        ReservationRequest::TimeRange::make_time_range(
+          now,
+          now+5s
+        ),
+        {10s}
+      );
+      auto request1 = std::vector{request1_alt1};
+      auto req1_id = participant1->make_request_blocking(database, request1);
+
+      REQUIRE(participant1->get_proposal(req1_id).has_value());
+
+      auto request2_alt1 = ReservationRequest::make_request(
+        "table_at_timbre",
+        ReservationRequest::TimeRange::make_time_range(
+          now+10s,
+          now+30s
+        ),
+        {10s}
+      );
+      auto request2 = std::vector{request2_alt1};
+      auto req2_id = participant1->make_request_blocking(database, request2);
+
+      REQUIRE(participant1->get_proposal(req2_id).has_value());
+
+      auto request3_alt1 = ReservationRequest::make_request(
+        "table_at_timbre",
+        ReservationRequest::TimeRange::make_time_range(
+          now+10s,
+          now+20s
+        ),
+        {10s}
+      );
+      auto request3 = std::vector{request3_alt1};
+      auto req3_id = participant1->make_request_blocking(database, request3);
+      THEN("All three proposals are accomodated")
+      {
+        REQUIRE(participant1->get_proposal(req1_id).has_value());
+        REQUIRE(participant1->get_proposal(req2_id).has_value());
+        REQUIRE(participant1->get_proposal(req3_id).has_value());
+
+        REQUIRE(
+          !participant1->get_proposal(req3_id)->conflicts_with(
+            participant1->get_proposal(req2_id).value()
+          ));
+
+        REQUIRE(
+          !participant1->get_proposal(req1_id)->conflicts_with(
+            participant1->get_proposal(req2_id).value()
+          ));
+
+        REQUIRE(
+          !participant1->get_proposal(req3_id)->conflicts_with(
+            participant1->get_proposal(req1_id).value()
+          ));
+      }
+    }
   }
 }
\ No newline at end of file
diff --git a/rmf_traffic/test/unit/reservations/test_Reservation.cpp b/rmf_traffic/test/unit/reservations/test_Reservation.cpp
index e69de29b..fff465a0 100644
--- a/rmf_traffic/test/unit/reservations/test_Reservation.cpp
+++ b/rmf_traffic/test/unit/reservations/test_Reservation.cpp
@@ -0,0 +1,88 @@
+#include <rmf_utils/catch.hpp>
+#include <rmf_traffic/reservations/Reservation.hpp>
+
+using namespace rmf_traffic;
+using namespace rmf_traffic::reservations;
+using namespace std::chrono_literals;
+
+SCENARIO(
+  "Test reservation conflicts_with")
+{
+  auto now = std::chrono::steady_clock::now();
+  GIVEN("Two overlapping reservations for same resource")
+  {
+    Reservation res1 = Reservation::make_reservation(
+      now,
+      "table_at_koufu",
+      20s,
+      std::nullopt
+    );
+    Reservation res2 = Reservation::make_reservation(
+      now + 10s,
+      "table_at_koufu",
+      20s,
+      std::nullopt
+    );
+    THEN("reservations conflict with each other")
+    {
+      REQUIRE(res1.conflicts_with(res2));
+      REQUIRE(res2.conflicts_with(res1));
+    }
+  }
+
+  GIVEN("Two non-overlapping reservations for same resource")
+  {
+    Reservation res1 = Reservation::make_reservation(
+      now,
+      "table_at_koufu",
+      20s,
+      std::nullopt
+    );
+    Reservation res2 = Reservation::make_reservation(
+      now + 20s,
+      "table_at_koufu",
+      20s,
+      std::nullopt
+    );
+    THEN("reservations do not conflict with each other")
+    {
+      REQUIRE(!res1.conflicts_with(res2));
+      REQUIRE(!res2.conflicts_with(res1));
+    }
+  }
+
+  GIVEN("Two overlapping reservations for different resource")
+  {
+    Reservation res1 = Reservation::make_reservation(
+      now,
+      "table_at_koufu",
+      20s,
+      std::nullopt
+    );
+    Reservation res2 = Reservation::make_reservation(
+      now + 10s,
+      "table_at_timbre",
+      20s,
+      std::nullopt
+    );
+    THEN("reservations do not conflict with each other")
+    {
+      REQUIRE(!res1.conflicts_with(res2));
+      REQUIRE(!res2.conflicts_with(res1));
+    }
+  }
+
+  GIVEN("One reservation")
+  {
+    Reservation res1 = Reservation::make_reservation(
+      now,
+      "table_at_koufu",
+      20s,
+      std::nullopt
+    );
+    THEN("reservation conflicts with itself")
+    {
+      REQUIRE(res1.conflicts_with(res1));
+    }
+  }
+}
\ No newline at end of file

From 9df78e862e9ba46b22b0f01783571025a9cad897 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Fri, 23 Jul 2021 10:23:20 +0800
Subject: [PATCH 59/66] style

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 rmf_traffic/include/rmf_traffic/reservations/Database.hpp   | 2 +-
 .../include/rmf_traffic/reservations/Participant.hpp        | 2 +-
 .../include/rmf_traffic/reservations/Reservation.hpp        | 2 +-
 .../include/rmf_traffic/reservations/ReservationRequest.hpp | 2 +-
 rmf_traffic/include/rmf_traffic/reservations/Writer.hpp     | 2 +-
 rmf_traffic/src/rmf_traffic/reservations/Database.cpp       | 5 +++--
 rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp    | 4 ++--
 .../src/rmf_traffic/reservations/ReservationRequest.cpp     | 5 ++---
 .../src/rmf_traffic/reservations/internal/Heuristic.hpp     | 2 +-
 .../src/rmf_traffic/reservations/internal/Optimizer.hpp     | 2 +-
 .../rmf_traffic/reservations/internal/ParticipantStore.hpp  | 2 +-
 .../src/rmf_traffic/reservations/internal/RequestStore.hpp  | 6 +++---
 rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp | 6 +++---
 .../src/rmf_traffic/reservations/internal/StateDiff.hpp     | 6 +++---
 14 files changed, 24 insertions(+), 24 deletions(-)

diff --git a/rmf_traffic/include/rmf_traffic/reservations/Database.hpp b/rmf_traffic/include/rmf_traffic/reservations/Database.hpp
index 77bedc6e..5d34ebbb 100644
--- a/rmf_traffic/include/rmf_traffic/reservations/Database.hpp
+++ b/rmf_traffic/include/rmf_traffic/reservations/Database.hpp
@@ -61,4 +61,4 @@ class Database : public Writer
 }
 }
 
-#endif
\ No newline at end of file
+#endif
diff --git a/rmf_traffic/include/rmf_traffic/reservations/Participant.hpp b/rmf_traffic/include/rmf_traffic/reservations/Participant.hpp
index 5614e86b..2287f96a 100644
--- a/rmf_traffic/include/rmf_traffic/reservations/Participant.hpp
+++ b/rmf_traffic/include/rmf_traffic/reservations/Participant.hpp
@@ -89,4 +89,4 @@ class Participant
 
 }
 }
-#endif
\ No newline at end of file
+#endif
diff --git a/rmf_traffic/include/rmf_traffic/reservations/Reservation.hpp b/rmf_traffic/include/rmf_traffic/reservations/Reservation.hpp
index 15627361..3cf9ee82 100644
--- a/rmf_traffic/include/rmf_traffic/reservations/Reservation.hpp
+++ b/rmf_traffic/include/rmf_traffic/reservations/Reservation.hpp
@@ -120,4 +120,4 @@ class Reservation
 };
 } // end namespace reservations
 } // end namespace rmf_traffic
-#endif
\ No newline at end of file
+#endif
diff --git a/rmf_traffic/include/rmf_traffic/reservations/ReservationRequest.hpp b/rmf_traffic/include/rmf_traffic/reservations/ReservationRequest.hpp
index 1340e985..9f0a0a94 100644
--- a/rmf_traffic/include/rmf_traffic/reservations/ReservationRequest.hpp
+++ b/rmf_traffic/include/rmf_traffic/reservations/ReservationRequest.hpp
@@ -105,4 +105,4 @@ class ReservationRequest
 }
 }
 
-#endif
\ No newline at end of file
+#endif
diff --git a/rmf_traffic/include/rmf_traffic/reservations/Writer.hpp b/rmf_traffic/include/rmf_traffic/reservations/Writer.hpp
index 6498c42d..74a8526c 100644
--- a/rmf_traffic/include/rmf_traffic/reservations/Writer.hpp
+++ b/rmf_traffic/include/rmf_traffic/reservations/Writer.hpp
@@ -81,4 +81,4 @@ class Writer
 }
 }
 
-#endif
\ No newline at end of file
+#endif
diff --git a/rmf_traffic/src/rmf_traffic/reservations/Database.cpp b/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
index 8eefe824..95e90f34 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
@@ -108,7 +108,8 @@ void Database::cancel_request(ParticipantId id, RequestId req)
 Database::Database()
 : _pimpl(rmf_utils::make_impl<Implementation>())
 {
+  // Do nothing
 }
 
-}
-}
\ No newline at end of file
+} // end namespace reservations
+} // end namespace rmf_traffic
diff --git a/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp b/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp
index 226ff6a0..08ca2e77 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp
@@ -190,5 +190,5 @@ Reservation::Reservation()
   // do nothing
 }
 
-}
-}
\ No newline at end of file
+} // end namespace reservations
+} // end namespace rmf_traffic
diff --git a/rmf_traffic/src/rmf_traffic/reservations/ReservationRequest.cpp b/rmf_traffic/src/rmf_traffic/reservations/ReservationRequest.cpp
index 3defe178..4bf21def 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/ReservationRequest.cpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/ReservationRequest.cpp
@@ -118,6 +118,5 @@ ReservationRequest::ReservationRequest()
 {
 }
 
-}
-}
-
+} // end namespace reservations
+} // end namespace rmf_traffic
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp
index 158cb1d2..d3fcc4fe 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp
@@ -59,4 +59,4 @@ class PriorityBasedScorer : public Heuristic
 }
 }
 
-#endif
\ No newline at end of file
+#endif
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/Optimizer.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/Optimizer.hpp
index dfba4e2d..9f883bb0 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/Optimizer.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/Optimizer.hpp
@@ -116,4 +116,4 @@ class GreedyBestFirstSearchOptimizer
 };
 }
 }
-#endif
\ No newline at end of file
+#endif
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/ParticipantStore.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/ParticipantStore.hpp
index ca313740..a7f0a4b6 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/ParticipantStore.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/ParticipantStore.hpp
@@ -61,4 +61,4 @@ class ParticipantStore
 
 }
 }
-#endif
\ No newline at end of file
+#endif
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp
index 2815114c..8d4cc233 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp
@@ -167,6 +167,6 @@ class RequestStore
   > _reservation_info;
 };
 
-}
-}
-#endif
\ No newline at end of file
+} // end namespace reservations
+} // end namespace rmf_traffic
+#endif
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
index 78ce2b36..6d8c4025 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -849,6 +849,6 @@ NextStateGenerator State::end()
   gen.mode = NextStateGenerator::PotentialActionMode::END;
   return gen;
 }
-}
-}
-#endif
\ No newline at end of file
+} // end namespace reservations
+} // end namespace rmf_traffic
+#endif
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/StateDiff.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/StateDiff.hpp
index d1f56652..b85945f8 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/StateDiff.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/StateDiff.hpp
@@ -192,6 +192,6 @@ class StateDiff
   std::vector<Difference> _differences;
 };
 
-}
-}
-#endif
\ No newline at end of file
+} //end namespace reservations
+} //end namespace rmf_traffic
+#endif

From d2e06ccb051dcf87756235b2151abcb20d5d9d83 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Fri, 23 Jul 2021 10:25:27 +0800
Subject: [PATCH 60/66] style

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp | 2 +-
 rmf_traffic/test/unit/reservations/test_Database.cpp     | 6 +++---
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp b/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp
index 08ca2e77..d721d147 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp
@@ -142,7 +142,7 @@ bool Reservation::conflicts_with(const Reservation& other) const
       && this->actual_finish_time() <= other.start_time())
       return false;
     else
-      return true ;
+      return true;
   }
 }
 
diff --git a/rmf_traffic/test/unit/reservations/test_Database.cpp b/rmf_traffic/test/unit/reservations/test_Database.cpp
index 293c107f..5f17074f 100644
--- a/rmf_traffic/test/unit/reservations/test_Database.cpp
+++ b/rmf_traffic/test/unit/reservations/test_Database.cpp
@@ -274,17 +274,17 @@ SCENARIO("Test database behaviour at the start of our lord, the saviour, UNIX")
         REQUIRE(
           !participant1->get_proposal(req3_id)->conflicts_with(
             participant1->get_proposal(req2_id).value()
-          ));
+        ));
 
         REQUIRE(
           !participant1->get_proposal(req1_id)->conflicts_with(
             participant1->get_proposal(req2_id).value()
-          ));
+        ));
 
         REQUIRE(
           !participant1->get_proposal(req3_id)->conflicts_with(
             participant1->get_proposal(req1_id).value()
-          ));
+        ));
       }
     }
   }

From d4f7b9ef3a1bc34c4b7ef300ad2d648cd652bb44 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Fri, 23 Jul 2021 10:29:10 +0800
Subject: [PATCH 61/66] remove dead code

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../reservations/internal/StateDiff.hpp       | 61 -------------------
 1 file changed, 61 deletions(-)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/StateDiff.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/StateDiff.hpp
index b85945f8..66013792 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/StateDiff.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/StateDiff.hpp
@@ -55,10 +55,6 @@ class StateDiff
     State& state2
   )
   {
-    // TODO(arjo): currently method is O(nlogn). It is possible to implement
-    // in O(n) using a two pointer method, just trickier. Ideally we should
-    // do this.
-
     // Check for reservation shuffling/shifting and assignment
     for (auto [participant, requests]: state1._reservation_assignments)
     {
@@ -123,8 +119,6 @@ class StateDiff
         );
       }
     }
-
-    //_differences = toposort(state2);
   }
 
   //============================================================================
@@ -134,61 +128,6 @@ class StateDiff
     return _differences;
   }
 private:
-  //============================================================================
-  /// Use something akin to Kahn's algorithm to perform a topological sort of
-  /// the shifts. This ensures that during the rollout phase, if there is a
-  /// communications failure the system will remain in a usable state.
-  std::vector<Difference> toposort(State& state)
-  {
-    std::vector<Difference> result;
-    std::queue<std::size_t> unvisited;
-    State curr_state(state);
-    //Unassignments can go first, since they will never create a conflict.
-    for (std::size_t i = 0; i < _differences.size(); i++)
-    {
-      if (_differences[i].diff_type == DifferenceType::UNASSIGN_RESERVATION)
-      {
-        result.push_back(_differences[i]);
-        curr_state = curr_state.unassign_reservation(
-          _differences[i].participant_id,
-          _differences[i].request_id);
-      }
-      else
-      {
-        unvisited.push(i);
-      }
-    }
-
-    while (!unvisited.empty())
-    {
-      auto index = unvisited.front();
-      unvisited.pop();
-      auto diff = _differences[index];
-      auto reservation = diff.reservation;
-
-      if (!curr_state.check_if_conflicts(reservation.value()))
-      {
-        if (diff.diff_type == DifferenceType::SHIFT_RESERVATION)
-          curr_state = curr_state.unassign_reservation(
-            diff.participant_id,
-            diff.request_id
-          );
-
-        curr_state.assign_reservation(
-          diff.participant_id,
-          diff.request_id,
-          reservation.value()
-        );
-      }
-      else
-      {
-        unvisited.push(index);
-      }
-    }
-
-    return result;
-  }
-
   std::vector<Difference> _differences;
 };
 

From 1e207e8644909b768cb9e2edfd6a706e08632598 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Fri, 23 Jul 2021 11:03:24 +0800
Subject: [PATCH 62/66] update heuristic

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../reservations/internal/Heuristic.hpp         | 17 ++++++++++++++++-
 .../reservations/internal/RequestStore.hpp      | 13 +++++++++++++
 2 files changed, 29 insertions(+), 1 deletion(-)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp
index d3fcc4fe..0ef767a9 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp
@@ -46,12 +46,27 @@ class PriorityBasedScorer : public Heuristic
 
   float score(const State& state) override
   {
+    // TODO: This is not really priority based planning.
+    // Currently we first minimize unassigned priorities, then we
+    // minimize the cost...
+    // Cost itself is determined by the order of the alternatives presented.
     auto score = 0;
     for (auto [part_id, req_id]: state.unassigned())
     {
-      // TODO: Incorporate the order into the heuristic
+      // Incorporate priority
       score += state.requests()->get_request_info(part_id, req_id)->priority;
     }
+
+    auto num_alternatives = state.requests()->num_alternatives();
+    // Enforces ordering
+    for(auto &[resource, schedule] : state.resource_schedule())
+    {
+      for(auto &[time, reservation]: schedule)
+      {
+        double index = state.request_ids()[reservation.reservation_id()].index; 
+        score += index / (double)num_alternatives;
+      }
+    }
     return score;
   }
 };
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp
index 8d4cc233..9b6740ec 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp
@@ -161,6 +161,19 @@ class RequestStore
     }
     return std::nullopt;
   }
+
+  std::size_t num_alternatives() const
+  {
+    std::size_t alternatives = 0;
+    for(auto &[participant, requests] : _reservation_info)
+    {
+      for(auto &[request, request_info] : requests)
+      {
+        alternatives += request_info.request_options.size();
+      }
+    }
+    return alternatives;
+  }
 private:
   std::unordered_map<ParticipantId,
     std::unordered_map<RequestId, RequestInfo>

From db8b747bd299d679a43c80548110125e77c3db41 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Fri, 23 Jul 2021 11:05:38 +0800
Subject: [PATCH 63/66] function separators

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../rmf_traffic/reservations/internal/RequestStore.hpp   | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp
index 9b6740ec..bce61aa5 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp
@@ -36,6 +36,7 @@ class RequestStore
     std::vector<ReservationRequest> request_options;
   };
 
+  //============================================================================
   std::optional<RequestInfo> get_request_info(
     ParticipantId pid,
     RequestId req)
@@ -46,17 +47,20 @@ class RequestStore
     return std::nullopt;
   }
 
+  //============================================================================
   RequestStore()
   {
     //Do nothing
   }
 
+  //============================================================================
   RequestStore(const rmf_traffic::reservations::RequestStore& other)
   : _reservation_info(other._reservation_info)
   {
     //Do nothing
   }
 
+  //============================================================================
   void enqueue_reservation(
     ParticipantId pid,
     RequestId req,
@@ -73,6 +77,7 @@ class RequestStore
       });
   }
 
+  //============================================================================
   void erase_participant_requests(
     ParticipantId pid
   )
@@ -83,11 +88,13 @@ class RequestStore
     _reservation_info.erase(entry);
   }
 
+  //============================================================================
   void cancel(ParticipantId pid, RequestId req)
   {
     _reservation_info[pid].erase(req);
   }
 
+  //============================================================================
   bool satisfies(ReservationRequest& req, Reservation reservation)
   {
     //Check upper bound
@@ -147,6 +154,7 @@ class RequestStore
     return true;
   }
 
+  //============================================================================
   std::optional<std::size_t> satisfies(
     ParticipantId pid,
     RequestId reqid,
@@ -162,6 +170,7 @@ class RequestStore
     return std::nullopt;
   }
 
+  //============================================================================
   std::size_t num_alternatives() const
   {
     std::size_t alternatives = 0;

From c2cbe94a3c65df6fb0a1ac44c63a1e281b6fa45e Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Fri, 23 Jul 2021 11:17:57 +0800
Subject: [PATCH 64/66] style

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../src/rmf_traffic/reservations/internal/Heuristic.hpp     | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp
index 0ef767a9..a3ef43ff 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp
@@ -59,11 +59,11 @@ class PriorityBasedScorer : public Heuristic
 
     auto num_alternatives = state.requests()->num_alternatives();
     // Enforces ordering
-    for(auto &[resource, schedule] : state.resource_schedule())
+    for (auto &[resource, schedule] : state.resource_schedule())
     {
-      for(auto &[time, reservation]: schedule)
+      for (auto &[time, reservation]: schedule)
       {
-        double index = state.request_ids()[reservation.reservation_id()].index; 
+        double index = state.request_ids()[reservation.reservation_id()].index;
         score += index / (double)num_alternatives;
       }
     }

From 2b459261619a976187b87e3c0bb8393248a6b2e5 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Fri, 23 Jul 2021 11:37:09 +0800
Subject: [PATCH 65/66] a little less crusty?

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../src/rmf_traffic/reservations/internal/Heuristic.hpp       | 4 ++--
 .../src/rmf_traffic/reservations/internal/RequestStore.hpp    | 4 ++--
 rmf_traffic/test/unit/reservations/test_Database.cpp          | 2 +-
 rmf_traffic/test/unit/reservations/test_Reservation.cpp       | 2 +-
 4 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp
index a3ef43ff..685da764 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.hpp
@@ -59,9 +59,9 @@ class PriorityBasedScorer : public Heuristic
 
     auto num_alternatives = state.requests()->num_alternatives();
     // Enforces ordering
-    for (auto &[resource, schedule] : state.resource_schedule())
+    for (auto&[resource, schedule] : state.resource_schedule())
     {
-      for (auto &[time, reservation]: schedule)
+      for (auto&[time, reservation]: schedule)
       {
         double index = state.request_ids()[reservation.reservation_id()].index;
         score += index / (double)num_alternatives;
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp
index bce61aa5..3fff7f56 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp
@@ -174,9 +174,9 @@ class RequestStore
   std::size_t num_alternatives() const
   {
     std::size_t alternatives = 0;
-    for(auto &[participant, requests] : _reservation_info)
+    for(auto&[participant, requests] : _reservation_info)
     {
-      for(auto &[request, request_info] : requests)
+      for(auto&[request, request_info] : requests)
       {
         alternatives += request_info.request_options.size();
       }
diff --git a/rmf_traffic/test/unit/reservations/test_Database.cpp b/rmf_traffic/test/unit/reservations/test_Database.cpp
index 5f17074f..401abd47 100644
--- a/rmf_traffic/test/unit/reservations/test_Database.cpp
+++ b/rmf_traffic/test/unit/reservations/test_Database.cpp
@@ -288,4 +288,4 @@ SCENARIO("Test database behaviour at the start of our lord, the saviour, UNIX")
       }
     }
   }
-}
\ No newline at end of file
+}
diff --git a/rmf_traffic/test/unit/reservations/test_Reservation.cpp b/rmf_traffic/test/unit/reservations/test_Reservation.cpp
index fff465a0..aff513bb 100644
--- a/rmf_traffic/test/unit/reservations/test_Reservation.cpp
+++ b/rmf_traffic/test/unit/reservations/test_Reservation.cpp
@@ -85,4 +85,4 @@ SCENARIO(
       REQUIRE(res1.conflicts_with(res1));
     }
   }
-}
\ No newline at end of file
+}

From a4f58d0ced25c81d612781cdb0aae6ca287e668a Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Fri, 23 Jul 2021 11:49:10 +0800
Subject: [PATCH 66/66] style.

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../src/rmf_traffic/reservations/internal/RequestStore.hpp    | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp
index 3fff7f56..d29afac3 100644
--- a/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp
@@ -174,9 +174,9 @@ class RequestStore
   std::size_t num_alternatives() const
   {
     std::size_t alternatives = 0;
-    for(auto&[participant, requests] : _reservation_info)
+    for (auto&[participant, requests] : _reservation_info)
     {
-      for(auto&[request, request_info] : requests)
+      for (auto&[request, request_info] : requests)
       {
         alternatives += request_info.request_options.size();
       }