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
diff --git a/rmf_traffic/CMakeLists.txt b/rmf_traffic/CMakeLists.txt
index d5516187..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")
@@ -46,6 +45,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")
 
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..5d34ebbb
--- /dev/null
+++ b/rmf_traffic/include/rmf_traffic/reservations/Database.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__DATABASE_HPP
+#define RMF_TRAFFIC__DATABASE_HPP
+
+#include <rmf_traffic/reservations/Writer.hpp>
+
+namespace rmf_traffic {
+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,
+    std::vector<ReservationRequest>& request_options,
+    int priority = 0
+  ) override;
+
+  ///===========================================================================
+  /// Documentation inherited from Writer
+  void cancel_request(ParticipantId id, RequestId req) override;
+
+  Database();
+
+  class Implementation;
+private:
+  rmf_utils::impl_ptr<Implementation> _pimpl;
+};
+
+}
+}
+
+#endif
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..2287f96a
--- /dev/null
+++ b/rmf_traffic/include/rmf_traffic/reservations/Participant.hpp
@@ -0,0 +1,92 @@
+/*
+ * 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>
+#include <rmf_traffic/reservations/ReservationRequest.hpp>
+namespace rmf_traffic {
+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.
+  /// \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,
+    uint64_t proposal_version
+  ) = 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.
+  /// \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,
+    uint64_t proposal_version
+  ) = 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.
+  /// \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,
+    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,
+    uint64_t proposal_version
+  ) = 0;
+};
+
+}
+}
+#endif
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..3cf9ee82
--- /dev/null
+++ b/rmf_traffic/include/rmf_traffic/reservations/Reservation.hpp
@@ -0,0 +1,123 @@
+/*
+ * 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 <string>
+#include <rmf_traffic/Time.hpp>
+#include <rmf_utils/impl_ptr.hpp>
+
+namespace rmf_traffic {
+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,
+    std::optional<rmf_traffic::Duration> duration,
+    std::optional<rmf_traffic::Time> finish_time);
+
+  class Implementation;
+
+  ///===========================================================================
+  /// \brief Constructor
+  Reservation();
+
+private:
+  rmf_utils::impl_ptr<Implementation> _pimpl;
+};
+} // end namespace reservations
+} // end namespace rmf_traffic
+#endif
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..9f0a0a94
--- /dev/null
+++ b/rmf_traffic/include/rmf_traffic/reservations/ReservationRequest.hpp
@@ -0,0 +1,108 @@
+/*
+ * 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 <string>
+#include <rmf_traffic/Time.hpp>
+#include <rmf_utils/impl_ptr.hpp>
+
+namespace rmf_traffic {
+namespace reservations {
+
+using RequestId = uint64_t;
+
+///=============================================================================
+/// \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:
+    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;
+  };
+
+  ///===========================================================================
+  /// \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,
+    std::optional<Duration> duration = std::nullopt,
+    std::optional<Time> finish = std::nullopt
+  );
+
+  class Implementation;
+
+private:
+  ReservationRequest();
+  rmf_utils::impl_ptr<Implementation> _pimpl;
+};
+}
+}
+
+#endif
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..74a8526c
--- /dev/null
+++ b/rmf_traffic/include/rmf_traffic/reservations/Writer.hpp
@@ -0,0 +1,84 @@
+/*
+ * 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 {
+
+///=============================================================================
+// 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,
+    std::vector<ReservationRequest>& request_options,
+    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;
+};
+}
+}
+
+#endif
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..95e90f34
--- /dev/null
+++ b/rmf_traffic/src/rmf_traffic/reservations/Database.cpp
@@ -0,0 +1,115 @@
+/*
+ * 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/Protocol.hpp"
+#include "internal/ParticipantStore.hpp"
+#include "internal/State.hpp"
+namespace rmf_traffic {
+namespace reservations {
+
+//==============================================================================
+class Database::Implementation
+{
+public:
+  //============================================================================
+  /// \constructor
+  Implementation()
+  : _participant_store(std::make_shared<ParticipantStore>()),
+    _protocol(_participant_store)
+  {
+    // Do nothing
+  }
+
+  //============================================================================
+  /// \brief Register a participant
+  void register_participant(
+    ParticipantId id,
+    std::shared_ptr<Participant> participant)
+  {
+    _participant_store->add_participant(id, participant);
+  }
+
+  //============================================================================
+  void unregister_participant(
+    ParticipantId id)
+  {
+    _protocol.remove_participant(id);
+  }
+
+  //============================================================================
+  void request_reservation(
+    ParticipantId id,
+    RequestId req,
+    std::vector<ReservationRequest>& request_options,
+    int priority)
+  {
+    _protocol.add_request(id, req, request_options, priority);
+  }
+
+  //============================================================================
+  void cancel_request(ParticipantId id, RequestId req)
+  {
+    _protocol.remove_request(id, req);
+  }
+private:
+  std::shared_ptr<ParticipantStore> _participant_store;
+  Protocol _protocol;
+};
+
+//==============================================================================
+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)
+{
+  _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>())
+{
+  // Do nothing
+}
+
+} // 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
new file mode 100644
index 00000000..d721d147
--- /dev/null
+++ b/rmf_traffic/src/rmf_traffic/reservations/Reservation.cpp
@@ -0,0 +1,194 @@
+/*
+ * 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>
+#include <string>
+
+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::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())
+      //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 false;
+    else
+      return true;
+  }
+}
+
+//=============================================================================
+bool Reservation::operator==(const Reservation& other) const
+{
+  // 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;
+}
+
+//=============================================================================
+bool Reservation::operator!=(const Reservation& other) const
+{
+  return !(*this == other);
+}
+
+//=============================================================================
+Reservation Reservation::make_reservation(
+  rmf_traffic::Time start_time,
+  std::string _resource_name,
+  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->_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>())
+{
+  // do nothing
+}
+
+} // 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
new file mode 100644
index 00000000..4bf21def
--- /dev/null
+++ b/rmf_traffic/src/rmf_traffic/reservations/ReservationRequest.cpp
@@ -0,0 +1,122 @@
+/*
+ * 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,
+  std::optional<TimeRange> start,
+  std::optional<Duration> duration,
+  std::optional<Time> finish)
+{
+  ReservationRequest req;
+  req._pimpl->_resource_name = resource_name;
+  req._pimpl->_start = start;
+  req._pimpl->_finish = finish;
+  req._pimpl->_duration = duration;
+
+  return req;
+}
+
+//=============================================================================
+ReservationRequest::ReservationRequest()
+: _pimpl(rmf_utils::make_impl<Implementation>())
+{
+}
+
+} // 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
new file mode 100644
index 00000000..685da764
--- /dev/null
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/Heuristic.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__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(const State& state) = 0;
+
+  virtual ~Heuristic() = default;
+};
+
+class PriorityBasedScorer : public Heuristic
+{
+public:
+  PriorityBasedScorer()
+  {
+    // Do nothing
+  }
+
+  ~PriorityBasedScorer()
+  {
+    // Do nothing
+  }
+
+  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())
+    {
+      // 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;
+  }
+};
+
+}
+}
+
+#endif
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..9f883bb0
--- /dev/null
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/Optimizer.hpp
@@ -0,0 +1,119 @@
+/*
+ * 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_OPTIMIZER_HPP
+#define RMF_TRAFFIC__RESERVATIONS__INTERNAL_OPTIMIZER_HPP
+
+#include "Heuristic.hpp"
+#include <queue>
+
+namespace rmf_traffic {
+namespace reservations {
+
+struct StateHash
+{
+  size_t operator()(const State& state) const
+  {
+    return state.hash();
+  }
+};
+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)
+    {
+      _visited.insert(state);
+      _pq.push(state);
+    }
+    std::optional<State> next_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)
+          {
+            _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();
+        return res;
+      }
+      return std::nullopt;
+    }
+  private:
+    std::unordered_set<State, StateHash> _visited;
+    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;
+  };
+  GreedyBestFirstSearchOptimizer(std::shared_ptr<Heuristic> heuristic)
+  : _heuristic(heuristic)
+  {
+  }
+
+  Solution optimize(State& state)
+  {
+    Solution solution(state, _heuristic);
+    return solution;
+  }
+private:
+  std::shared_ptr<Heuristic> _heuristic;
+};
+}
+}
+#endif
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..a7f0a4b6
--- /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
diff --git a/rmf_traffic/src/rmf_traffic/reservations/internal/Protocol.hpp b/rmf_traffic/src/rmf_traffic/reservations/internal/Protocol.hpp
new file mode 100644
index 00000000..6abcb844
--- /dev/null
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/Protocol.hpp
@@ -0,0 +1,264 @@
+/*
+ * 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_PROTOCOL_HPP
+#define RMF_TRAFFIC__RESERVATIONS__INTERNAL_PROTOCOL_HPP
+
+#include "ParticipantStore.hpp"
+#include "Optimizer.hpp"
+#include "State.hpp"
+#include "StateDiff.hpp"
+
+namespace rmf_traffic {
+namespace reservations {
+
+//==============================================================================
+/// \brief The Protocol class is in charge of handling the requests and
+/// negotiations that occur during the planning and negotiation phase.
+class Protocol
+{
+public:
+  //============================================================================
+  /// \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.
+  Protocol(
+    std::shared_ptr<ParticipantStore> pstore)
+  : _participant_store(pstore),
+    _curr_state(std::make_shared<RequestStore>()),
+    _proposal_version(0)
+  {
+
+  }
+
+  //============================================================================
+  /// \brief Destructor. Terminates the execution engine by sending a
+  /// TERMINATE_STREAM action and waits for the destructor to join.
+  ~Protocol()
+  {
+
+  }
+
+  //============================================================================
+  /// \brief Add a request to the queue.
+  void add_request(
+    ParticipantId participant_id,
+    RequestId request_id,
+    std::vector<ReservationRequest>& reservation,
+    int priority)
+  {
+    _curr_state = _curr_state.add_request(
+      participant_id,
+      request_id,
+      priority,
+      reservation
+    );
+
+    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 request
+  void remove_request(
+    ParticipantId participant_id,
+    RequestId request_id)
+  {
+    _curr_state = _curr_state.remove_request(
+      participant_id, request_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());
+  }
+
+  //============================================================================
+  /// \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
+  void rollout(const StateDiff& changes)
+  {
+    for (auto change: changes.differences())
+    {
+      if (!_participant_store->get_participant(change.participant_id).has_value())
+      {
+        continue;
+      }
+
+      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(),
+          _proposal_version);
+
+        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(),
+          _proposal_version);
+
+        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)
+      {
+        assert(!change.reservation.has_value());
+        auto result =
+          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);
+      }
+    }
+  }
+
+  //============================================================================
+  /// \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(
+    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};
+      _proposal_version++;
+    }
+
+    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();
+    for (auto diff: res)
+    {
+      auto impacted_participant = diff.participant_id;
+
+      if (!_participant_store->get_participant(impacted_participant).has_value())
+        continue;
+
+      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(),
+          _proposal_version
+        );
+        if (!result)
+          return false;
+      }
+      else
+      {
+        auto result = participant->unassign_request_proposal(
+          diff.request_id,
+          _proposal_version
+        );
+        if (!result)
+          return false;
+      }
+    }
+    return true;
+  }
+
+  std::shared_ptr<ParticipantStore> _participant_store;
+  uint64_t _proposal_version;
+  State _curr_state;
+};
+}
+}
+
+#endif
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..d29afac3
--- /dev/null
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/RequestStore.hpp
@@ -0,0 +1,194 @@
+/*
+ * 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>
+
+#include <iostream>
+
+namespace rmf_traffic {
+namespace reservations {
+
+class RequestStore
+{
+public:
+  struct RequestInfo
+  {
+    int priority;
+    std::vector<ReservationRequest> request_options;
+  };
+
+  //============================================================================
+  std::optional<RequestInfo> get_request_info(
+    ParticipantId pid,
+    RequestId req)
+  {
+    if (_reservation_info.count(pid) != 0
+      && _reservation_info.at(pid).count(req) != 0)
+      return _reservation_info[pid][req];
+    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,
+    int priority,
+    std::vector<ReservationRequest>& reservation
+  )
+  {
+    _reservation_info[pid].insert({
+        req,
+        {
+          priority,
+          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;
+  }
+
+  //============================================================================
+  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>
+  > _reservation_info;
+};
+
+} // 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
new file mode 100644
index 00000000..6d8c4025
--- /dev/null
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/State.hpp
@@ -0,0 +1,854 @@
+/*
+ * 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 <iostream>
+#include <map>
+#include <set>
+
+#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)
+{
+  std::hash<T> hasher;
+  seed ^= hasher(v) + 0x9e3779b9 + (seed<<6) + (seed>>2);
+}
+///=============================================================================
+/// \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
+  {
+    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:
+
+  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 ReservationAssignment = std::unordered_map<
+    ParticipantId, std::unordered_map<RequestId, ReservationId>>;
+
+  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<RequestStore> queue)
+  : _queue(queue)
+  {
+    //Do nothing
+  }
+
+  //============================================================================
+  NextStateGenerator begin();
+
+  //============================================================================
+  NextStateGenerator end();
+
+  //============================================================================
+  /// Adds a request to the state
+  /// Note: This modifies the ReservationStore.
+  State add_request(
+    ParticipantId pid,
+    RequestId reqid,
+    int priority,
+    std::vector<ReservationRequest>& reservation) const
+  {
+    State new_state(*this);
+    new_state._unassigned.insert({pid, reqid});
+    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) 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->cancel(pid, reqid);
+    return new_state;
+  }
+
+  //============================================================================
+  /// Remove a participant
+  State remove_participant(
+    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);
+    }
+
+    for (auto [participant_id, reqid] : _unassigned)
+    {
+      if (participant_id == pid)
+        new_state = new_state.remove_request(participant_id, reqid);
+    }
+    return new_state;
+  }
+
+  //============================================================================
+  void debug_state()
+  {
+    std::cout << "________________________" <<std::endl;
+    for (auto&[resource, schedule]: _resource_schedules)
+    {
+      std::cout << "Resource: " << resource << std::endl;
+      for (auto&[time, reservation]: schedule)
+      {
+        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;
+        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::cout << "\tParticipant: " <<
+          _reservation_request_ids[reservation.reservation_id()].participant
+                  << std::endl;
+        std::cout << "\tRequestID: " <<
+          _reservation_request_ids[reservation.reservation_id()].reqid
+                  << std::endl;
+      }
+    }
+  }
+
+  //============================================================================
+  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;
+    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());
+        auto val =
+          _reservation_request_ids.find(reservation.reservation_id())->second;
+        hash_combine(seed, val.participant);
+        hash_combine(seed, val.reqid);
+      }
+    }
+    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;
+  }
+
+  //============================================================================
+  bool check_if_conflicts(Reservation res) const
+  {
+    auto resource = res.resource_name();
+    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 (sched->second.size() == 0)
+        return start_slot  != sched->second.end();
+      else if (
+        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 =
+      sched->second.upper_bound(
+      res.actual_finish_time().value());
+
+    if (start_slot != sched->second.begin())
+    {
+      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());
+    }
+  }
+
+
+  //============================================================================
+  State unassign_reservation(ParticipantId pid, RequestId reqid) const
+  {
+    State new_state(*this);
+    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);
+    return new_state;
+  }
+
+  //============================================================================
+  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 != _resource_schedules[res.resource_name()].end()
+        && res.actual_finish_time().has_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)
+        {
+          return false;
+        }
+      }
+
+    }
+
+    _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;
+  }
+
+  std::shared_ptr<RequestStore> requests() const
+  {
+    return _queue;
+  }
+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];
+    _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);
+  }
+
+  //============================================================================
+  /// \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
+  )
+  {
+    if (dur.count() < 0) return false;
+
+    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;
+  }
+
+  //============================================================================
+  /// \brief Move the start time of a reservation.
+  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_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 => {part_id, req_id, index}
+
+  std::shared_ptr<RequestStore> _queue;
+
+  Time _current_time;
+  friend NextStateGenerator;
+  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
+  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
+  State::UnassignedSet::const_iterator unassigned_iter;
+  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;
+  State::ResourceSchedule::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 *visiting_state;
+  }
+
+  //============================================================================
+  State::ResourceSchedule::const_iterator
+  get_search_start(const 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(
+        req.start_time().value().lower_bound().value());
+    }
+    else
+    {
+      return start_state->_resource_schedules[req.resource_name()].begin();
+    }
+  }
+
+  //============================================================================
+  State::ResourceSchedule::const_iterator
+  get_search_end(const 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(
+        req.start_time().value().upper_bound().value());
+    }
+    else
+    {
+      return start_state->_resource_schedules[req.resource_name()].end();
+    }
+  }
+
+  //============================================================================
+  std::optional<State> advance_within_resource()
+  {
+    if (proceed_next_resource)
+      return std::nullopt;
+
+    State next_state(*start_state);
+
+    auto [part_id, req_id] = *unassigned_iter;
+
+    auto current_request = start_state->_queue->get_request_info(
+      part_id,
+      req_id)->request_options[request_index_iter];
+
+    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())
+      {
+        auto reservation = Reservation::make_reservation(
+          insertion_point_iter->second.actual_finish_time().value(),
+          current_request.resource_name(),
+          current_request.duration(),
+          current_request.finish_time()
+        );
+
+        bool found_slot = next_state.assign_reservation(
+          part_id,
+          req_id,
+          reservation
+        );
+        insertion_point_iter++;
+        if (found_slot)
+        {
+          return next_state;
+        }
+      }
+      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)
+    {
+      // 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())
+      {
+        return state;
+      }
+      request_index_iter++;
+    }
+
+    return std::nullopt;
+  }
+
+  //============================================================================
+  std::optional<State> advance_assignments()
+  {
+    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()
+  {
+    if (remove_resource_iter == start_state->_resource_schedules.end())
+    {
+      return std::nullopt;
+    }
+    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();
+    }
+
+    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)
+    {
+      auto assignment = advance_assignments();
+      if (assignment.has_value())
+      {
+        visiting_state = assignment;
+        return assignment;
+      }
+      mode = PotentialActionMode::REMOVE_ITEMS;
+    }
+    auto removals = advance_removals();
+
+    if (removals.has_value())
+    {
+      visiting_state = removals;
+      return removals;
+    }
+    mode = PotentialActionMode::END;
+    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
+      && other.mode == PotentialActionMode::END)
+    {
+      return true;
+    }
+    return false;
+  }
+
+  //============================================================================
+  bool operator!=(const NextStateGenerator& other)
+  {
+    return !(*this == other);
+  }
+};
+
+//==============================================================================
+NextStateGenerator State::begin()
+{
+  NextStateGenerator gen;
+  gen.mode = NextStateGenerator::PotentialActionMode::ASSIGN_ITEMS;
+  gen.start_state = this;
+
+  if (_unassigned.size() > 0)
+  {
+    auto request = _queue->get_request_info(
+      _unassigned.begin()->first,
+      _unassigned.begin()->second);
+
+    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::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);
+    gen.insertion_point_end = gen.get_search_end(current_request);
+  }
+  gen.remove_resource_iter = _resource_schedules.begin();
+
+  if (_resource_schedules.size() > 0)
+  {
+    gen.remove_iter = gen.remove_resource_iter->second.begin();
+  }
+  else if (_unassigned.size() == 0)
+  {
+    return end();
+  }
+  gen.next_state();
+  return gen;
+}
+
+//==============================================================================
+NextStateGenerator State::end()
+{
+  NextStateGenerator gen;
+  gen.mode = NextStateGenerator::PotentialActionMode::END;
+  return gen;
+}
+} // 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
new file mode 100644
index 00000000..66013792
--- /dev/null
+++ b/rmf_traffic/src/rmf_traffic/reservations/internal/StateDiff.hpp
@@ -0,0 +1,136 @@
+/*
+ * 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 <unordered_set>
+#include <queue>
+#include "State.hpp"
+
+namespace rmf_traffic {
+namespace reservations {
+
+//==============================================================================
+/// Gives the difference between two states. *NOTE*: Assumes that both the
+/// states have the same
+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;
+  };
+
+  //============================================================================
+  /// Calculates the StateDiff required for one to go from State2 to State1.
+  StateDiff(
+    State& state1,
+    State& state2
+  )
+  {
+    // 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
+    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
+          }
+        );
+      }
+    }
+  }
+
+  //============================================================================
+  /// Return the differences.
+  std::vector<Difference> differences() const
+  {
+    return _differences;
+  }
+private:
+  std::vector<Difference> _differences;
+};
+
+} //end namespace reservations
+} //end namespace rmf_traffic
+#endif
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..2dd05657
--- /dev/null
+++ b/rmf_traffic/test/unit/reservations/internal/test_State.cpp
@@ -0,0 +1,245 @@
+/*
+ * 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 "../../../../src/rmf_traffic/reservations/internal/StateDiff.hpp"
+#include "../../../../src/rmf_traffic/reservations/internal/Heuristic.hpp"
+#include "../../../../src/rmf_traffic/reservations/internal/Optimizer.hpp"
+
+#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")
+{
+  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};
+    start_state = start_state.add_request(0, 0, 1, request1);
+
+    std::vector<State> child_states;
+    for (auto child_state: start_state)
+    {
+      child_states.push_back(child_state);
+    }
+
+    REQUIRE(child_states.size() == 1);
+    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>();
+      GreedyBestFirstSearchOptimizer opt(heuristic);
+      auto solutions = opt.optimize(allocated_state);
+      std::vector<State> result;
+      while (auto solution = solutions.next_solution())
+      {
+        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);
+      }
+    }
+  }
+
+  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(
+        now,
+        now+10s
+      ),
+      {10s}
+    );
+    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};
+    start_state = start_state.add_request(0, 0, 1, request1);
+
+    auto request2_alt1 = ReservationRequest::make_request(
+      "table_at_timbre",
+      ReservationRequest::TimeRange::make_time_range(
+        now,
+        now+10s
+      ),
+      {10s}
+    );
+    auto request2 = std::vector{request2_alt1};
+    start_state = start_state.add_request(1, 0, 1, request2);
+
+    WHEN("We request the next possible states")
+    {
+      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);
+        }
+      }
+    }
+
+    WHEN("We attempt to solve")
+    {
+      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())
+      {
+        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 non-conflicting reservation")
+    {
+      Reservation res = Reservation::make_reservation(
+        now,
+        "Hawaii",
+        {},
+        {}
+      );
+
+      THEN("There should be no conflict")
+      {
+        REQUIRE(start_state.check_if_conflicts(res) == false);
+      }
+    }
+  }
+}
+
+// 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>();
+  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};
+  start_state = start_state.add_request(0, 0, 1, request1);
+
+  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 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));
+    }
+  }
+}
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..401abd47
--- /dev/null
+++ b/rmf_traffic/test/unit/reservations/test_Database.cpp
@@ -0,0 +1,291 @@
+/*
+ * 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>
+
+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,
+    int priority = 1)
+  : _pid(pid),
+    _reqid(0),
+    _priority(priority)
+  {
+  }
+
+  bool request_proposal(
+    RequestId id,
+    Reservation& res,
+    uint64_t proposal_version
+  ) override
+  {
+    _proposed[id] = res;
+    return true;
+  }
+
+  bool request_confirmed(
+    RequestId id,
+    Reservation& res,
+    uint64_t proposal_version
+  ) override
+  {
+    return true;
+  }
+
+  bool unassign_request_confirmed(
+    RequestId id,
+    uint64_t proposal_version
+  ) override
+  {
+    return true;
+  }
+
+  bool unassign_request_proposal(
+    RequestId id,
+    uint64_t proposal_version
+  ) override
+  {
+    _proposed[id] = std::nullopt;
+    return true;
+  }
+
+  RequestId make_request_blocking(
+    std::shared_ptr<Database>& _db,
+    std::vector<ReservationRequest>& request_options
+  )
+  {
+    _db->request_reservation(
+      _pid,
+      _reqid,
+      request_options,
+      _priority
+    );
+
+    return _reqid++;
+  }
+
+  std::optional<Reservation> get_proposal(ReservationId rid)
+  {
+    return _proposed[rid];
+  }
+
+  ParticipantId get_id() const
+  {
+    return _pid;
+  }
+
+private:
+  ParticipantId _pid;
+  RequestId _reqid;
+
+  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->register_participant(0, participant1);
+
+    WHEN("the participant makes a request")
+    {
+      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(database, 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");
+      }
+    }
+
+    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(database, request1);
+      auto prop = participant1->get_proposal(req_id);
+
+      std::shared_ptr<SimpleParticipant> participant2 =
+        std::make_shared<SimpleParticipant>(1, 20);
+      database->register_participant(1, participant2);
+      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")
+      {
+        REQUIRE(prop2.has_value());
+        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(database, request1);
+      auto prop = participant1->get_proposal(req_id);
+
+      std::shared_ptr<SimpleParticipant> participant2 =
+        std::make_shared<SimpleParticipant>(1, 20);
+      database->register_participant(1, participant2);
+      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());
+
+      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 reservation of 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(database, request1);
+      auto prop = participant1->get_proposal(req_id);
+
+      std::shared_ptr<SimpleParticipant> participant2 =
+        std::make_shared<SimpleParticipant>(1, 20);
+      database->register_participant(1, participant2);
+      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());
+
+      database->cancel_request(participant2->get_id(), req_id2);
+      THEN("Participant 1 will regain its reservation.")
+      {
+        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()
+        ));
+      }
+    }
+  }
+}
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..aff513bb
--- /dev/null
+++ 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));
+    }
+  }
+}