From 5944a2220af5142099377f3af9acbdd316faae90 Mon Sep 17 00:00:00 2001
From: Yadunund <yadunund@openrobotics.org>
Date: Mon, 6 Dec 2021 18:21:36 +0800
Subject: [PATCH 01/79] Added skeleton for broadcast client

Signed-off-by: Yadunund <yadunund@openrobotics.org>
---
 rmf_fleet_adapter/CMakeLists.txt              |  9 ++-
 rmf_fleet_adapter/package.xml                 |  4 ++
 .../src/rmf_fleet_adapter/BroadcastClient.cpp | 26 ++++++++
 .../src/rmf_fleet_adapter/BroadcastClient.hpp | 66 +++++++++++++++++++
 4 files changed, 104 insertions(+), 1 deletion(-)
 create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.cpp
 create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.hpp

diff --git a/rmf_fleet_adapter/CMakeLists.txt b/rmf_fleet_adapter/CMakeLists.txt
index ad93b290b..30455b2d7 100644
--- a/rmf_fleet_adapter/CMakeLists.txt
+++ b/rmf_fleet_adapter/CMakeLists.txt
@@ -36,6 +36,8 @@ set(dep_pkgs
   rmf_battery
   rmf_task
   std_msgs
+  websocketpp
+  nlohmann_json
 )
 foreach(pkg ${dep_pkgs})
   find_package(${pkg} REQUIRED)
@@ -76,10 +78,12 @@ target_link_libraries(rmf_fleet_adapter
     ${rmf_task_msgs_LIBRARIES}
   PRIVATE
     rmf_rxcpp
+    nlohmann_json::nlohmann_json
     ${rmf_door_msgs_LIBRARIES}
     ${rmf_lift_msgs_LIBRARIES}
     ${rmf_dispenser_msgs_LIBRARIES}
     ${rmf_ingestor_msgs_LIBRARIES}
+    ${websocketpp_LIBRARIES}
 )
 
 target_include_directories(rmf_fleet_adapter
@@ -96,6 +100,7 @@ target_include_directories(rmf_fleet_adapter
     ${rmf_lift_msgs_INCLUDE_DIRS}
     ${rmf_dispenser_msgs_INCLUDE_DIRS}
     ${rmf_ingestor_msgs_INCLUDE_DIRS}
+    ${WEBSOCKETPP_INCLUDE_DIR}
 )
 
 if (BUILD_TESTING)
@@ -131,6 +136,7 @@ if (BUILD_TESTING)
       ${rmf_dispenser_msgs_INCLUDE_DIRS}
       ${rmf_ingestor_msgs_INCLUDE_DIRS}
       ${std_msgs_INCLUDE_DIRS}
+      ${WEBSOCKETPP_INCLUDE_DIR}
   )
   target_link_libraries(test_rmf_fleet_adapter
     PRIVATE
@@ -144,6 +150,7 @@ if (BUILD_TESTING)
       rmf_fleet_adapter
       rmf_utils::rmf_utils
       ${std_msgs_LIBRARIES}
+      ${websocketpp_LIBRARIES}
   )
 
   target_compile_definitions(test_rmf_fleet_adapter
@@ -462,6 +469,7 @@ ament_export_dependencies(
   rmf_lift_msgs
   rmf_ingestor_msgs
   rmf_dispenser_msgs
+  websocketpp
 )
 
 install(
@@ -499,4 +507,3 @@ install(
 )
 
 ament_package()
-
diff --git a/rmf_fleet_adapter/package.xml b/rmf_fleet_adapter/package.xml
index 731ff6b82..53c998690 100644
--- a/rmf_fleet_adapter/package.xml
+++ b/rmf_fleet_adapter/package.xml
@@ -28,6 +28,10 @@
   <depend condition="$RMF_ENABLE_FAILOVER == 1">stubborn_buddies</depend>
   <depend condition="$RMF_ENABLE_FAILOVER == 1">stubborn_buddies_msgs</depend>
 
+  <depend>libwebsocketpp-dev</depend>
+  <depend>nlohmann-json3-dev</depend>
+  <!-- <depend>nlohmann_json_schema_validator_vendor</depend> -->
+
   <build_depend>eigen</build_depend>
   <build_depend>yaml-cpp</build_depend>
 
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.cpp
new file mode 100644
index 000000000..a996e2766
--- /dev/null
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.cpp
@@ -0,0 +1,26 @@
+/*
+ * 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 "TaskStateServer.hpp"
+
+
+namespace rmf_fleet_adapter {
+
+
+
+
+} // namespace rmf_fleet_adapter
\ No newline at end of file
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.hpp
new file mode 100644
index 000000000..e323ed078
--- /dev/null
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.hpp
@@ -0,0 +1,66 @@
+/*
+ * 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 SRC__RMF_FLEET_ADAPTER__BROADCASTCLIENT_HPP
+#define SRC__RMF_FLEET_ADAPTER__BROADCASTCLIENT_HPP
+
+#include <websocketpp/config/asio_no_tls_client.hpp>
+#include <websocketpp/client.hpp>
+
+#include <nlohmann/json.hpp>
+
+#include <rmf_fleet_adapter/agv/FleetUpdateHandle.hpp>
+
+#include <memory>
+#include <set>
+#include <queue>
+#include <mutex>
+
+namespace rmf_fleet_adapter {
+//==============================================================================
+// A wrapper around a websocket client for broadcasting states and logs for
+// fleets, robots and tasks. A queue of json msgs is maintained and published
+// in an internal thread whenever connection to a server is established.
+class BroadcastClient : public std::enable_shared_from_this<BroadcastClient>
+{
+public:
+  using WebsocketClient =
+    websocketpp::client<websocketpp::config::asio_client>;
+  using WebsocketMessagePtr = WebsocketClient::message_ptr;
+  using ConnectionHDL = websocketpp::connection_hdl;
+  using Connections = std::set<ConnectionHDL, std::owner_less<ConnectionHDL>>;
+
+  static std::shared_ptr<BroadcastClient> make(
+    std::size_t port,
+    std::weak_ptr<TaskManager> task_managers);
+
+  // This will add the json message to the internal _queue
+  void publish(const nlohmann::json& msg);
+
+  ~BroadcastClient();
+
+private:
+  BroadcastClient();
+  std::weak_ptr<FleetUpdateHandle> _fleet_handle;
+  std::shared_ptr<WebsocketClient> _server;
+  Connections _connections;
+  std::mutex _mutex;
+};
+
+} // namespace rmf_fleet_adapter
+
+#endif // SRC__RMF_FLEET_ADAPTER__BROADCASTCLIENT_HPP
\ No newline at end of file

From ae176b5e95b6bd5236093d4b15185a6b8e95ea44 Mon Sep 17 00:00:00 2001
From: Yadunund <yadunund@openrobotics.org>
Date: Tue, 7 Dec 2021 04:57:32 +0800
Subject: [PATCH 02/79] wip

Signed-off-by: Yadunund <yadunund@openrobotics.org>
---
 .../src/rmf_fleet_adapter/BroadcastClient.cpp | 26 +++++++++++++++++--
 .../src/rmf_fleet_adapter/BroadcastClient.hpp | 20 +++++++++-----
 2 files changed, 37 insertions(+), 9 deletions(-)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.cpp
index a996e2766..8dad2aab8 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.cpp
@@ -15,12 +15,34 @@
  *
 */
 
-#include "TaskStateServer.hpp"
+#include "BroadcastClient.hpp"
 
 
 namespace rmf_fleet_adapter {
+//==============================================================================
+std::shared_ptr<BroadcastClient> BroadcastClient::make(
+  const std::string& uri,
+  std::shared_ptr<agv::FleetUpdateHandle> fleet_handle)
+{
+  std::shared_ptr<BroadcastClient> client(new BroadcastClient());
+  client->_fleet_handle = std::move(fleet_handle);
+  client->_shutdown = false;
 
+}
 
+//==============================================================================
+BroadcastClient::BroadcastClient()
+{
+  // Do nothing
+}
 
+//==============================================================================
+BroadcastClient::~BroadcastClient()
+{
+  if (_thread.joinable)
+  {
+    _thread.join();
+  }
+}
 
-} // namespace rmf_fleet_adapter
\ No newline at end of file
+} // namespace rmf_fleet_adapter
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.hpp
index e323ed078..6b36bdd9b 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.hpp
@@ -29,6 +29,8 @@
 #include <set>
 #include <queue>
 #include <mutex>
+#include <thread>
+#include <atomic>
 
 namespace rmf_fleet_adapter {
 //==============================================================================
@@ -44,23 +46,27 @@ class BroadcastClient : public std::enable_shared_from_this<BroadcastClient>
   using ConnectionHDL = websocketpp::connection_hdl;
   using Connections = std::set<ConnectionHDL, std::owner_less<ConnectionHDL>>;
 
+  /// \param[in] uri
+  ///   "ws://localhost:9000"
   static std::shared_ptr<BroadcastClient> make(
-    std::size_t port,
-    std::weak_ptr<TaskManager> task_managers);
+    const std::string& uri,
+    std::shared_ptr<agv::FleetUpdateHandle> fleet_handle);
 
-  // This will add the json message to the internal _queue
+  // Publish message
   void publish(const nlohmann::json& msg);
 
   ~BroadcastClient();
 
 private:
   BroadcastClient();
-  std::weak_ptr<FleetUpdateHandle> _fleet_handle;
-  std::shared_ptr<WebsocketClient> _server;
-  Connections _connections;
+  std::weak_ptr<agv::FleetUpdateHandle> _fleet_handle;
+  std::shared_ptr<WebsocketClient> _client;
   std::mutex _mutex;
+  std::queue<nlohmann::json> _queue;
+  std::thread _thread;
+  std::atomic_bool _shutdown;
 };
 
 } // namespace rmf_fleet_adapter
 
-#endif // SRC__RMF_FLEET_ADAPTER__BROADCASTCLIENT_HPP
\ No newline at end of file
+#endif // SRC__RMF_FLEET_ADAPTER__BROADCASTCLIENT_HPP

From 62315197c8b48cb12aee4bb1387f7fa10fe07255 Mon Sep 17 00:00:00 2001
From: Yadunund <yadunund@openrobotics.org>
Date: Tue, 7 Dec 2021 18:51:26 +0800
Subject: [PATCH 03/79] Implemented make

Signed-off-by: Yadunund <yadunund@openrobotics.org>
---
 .../src/rmf_fleet_adapter/BroadcastClient.cpp | 106 ++++++++++++++++++
 .../src/rmf_fleet_adapter/BroadcastClient.hpp |   4 +
 2 files changed, 110 insertions(+)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.cpp
index 8dad2aab8..aa07aff8c 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.cpp
@@ -17,6 +17,7 @@
 
 #include "BroadcastClient.hpp"
 
+#include "agv/internal_FleetUpdateHandle.hpp"
 
 namespace rmf_fleet_adapter {
 //==============================================================================
@@ -25,9 +26,113 @@ std::shared_ptr<BroadcastClient> BroadcastClient::make(
   std::shared_ptr<agv::FleetUpdateHandle> fleet_handle)
 {
   std::shared_ptr<BroadcastClient> client(new BroadcastClient());
+  client->_uri = std::move(uri);
   client->_fleet_handle = std::move(fleet_handle);
   client->_shutdown = false;
+  client->_connected = false;
 
+  client->_client = std::make_shared<WebsocketClient>();
+  // Initialize the Asio transport policy
+  client->_client->init_asio();
+
+  client->_client->set_open_handler(
+    [c = client](websocketpp::connection_hdl)
+    {
+      c->_connected = true;
+      const auto fleet = c->_fleet_handle.lock();
+      if (!fleet)
+        return;
+      const auto impl = agv::FleetUpdateHandle::get(*fleet);
+      for (const auto& [conext, mgr] : impl.task_managers)
+      {
+        // TODO(YV): Publish latest state and log
+      }
+      RCLCPP_INFO(
+        impl.node->get_logger(),
+        "BroadcastClient successfully connected to uri: [%s]",
+        c->_uri.c_str());
+    });
+
+  client->_client->set_close_handler(
+    [c = client](websocketpp::connection_hdl)
+    {
+      c->_connected = false;
+    });
+
+  client->_client->set_fail_handler()
+  [c = client](websocketpp::connection_hdl)
+  {
+    c->_connected = false;
+  };
+
+  client->_thread = std::thread(
+    [c = client]()
+    {
+      while (!shutdown)
+      {
+        const auto fleet = c->_fleet_handle.lock();
+        if (!fleet)
+          continue;
+        const auto impl = agv::FleetUpdateHandle::get(*fleet);
+
+        // Try to connect to the server if we are not connected yet
+        if (!c->_connected)
+        {
+          websocketpp::lib::error_code ec;
+          WebsocketClient::connection_ptr con = c->_client->get_connection(
+            c->_uri, ec);
+          if (ec)
+          {
+            RCLCPP_WARN(
+              impl.node->get_logger(),
+              "BroadcastClient unable to connect to [%s]. Please make sure "
+              "server is running.",
+              c->_uri.c_str());
+            c->_connected = false;
+            std::this_thread::sleep_for(std::chrono::milliseconds(2000));
+            continue;
+          }
+
+          c->_handle = con->get_handle();
+          c->_client->connect(con);
+          // TODO(YV): Start asio io_service event loop
+          c->_connected = true;
+        }
+
+        std::unique_lock<std::mutex> lock(c->_mutex);
+        c->_cv.wait(lock,
+          [c]()
+          {
+            return !c->_queue.empty();
+          });
+
+        while (!c->_queue.empty())
+        {
+          websocketpp::lib::error_code ec;
+          const std::string& msg = c->_queue.front().dump();
+          c->_client->send(c->_hdl, msg, websocketpp::frame::opcode::text, ec);
+          if (ec)
+          {
+            RCLCPP_ERROR(
+              impl.node->get_logger(),
+              "BroadcastClient unable to publish message");
+            // TODO(YV): Check if we should re-connect to server
+            break;
+          }
+          c->_queue.pop();
+        }
+      }
+    });
+
+  return client;
+
+}
+
+//==============================================================================
+void BroadcastClient::publish(const nlohmann::json& msg)
+{
+  // TODO(YV): lock a mutex
+  _queue.push(msg);
 }
 
 //==============================================================================
@@ -39,6 +144,7 @@ BroadcastClient::BroadcastClient()
 //==============================================================================
 BroadcastClient::~BroadcastClient()
 {
+  _shutdown = true;
   if (_thread.joinable)
   {
     _thread.join();
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.hpp
index 6b36bdd9b..4932d8efb 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.hpp
@@ -59,11 +59,15 @@ class BroadcastClient : public std::enable_shared_from_this<BroadcastClient>
 
 private:
   BroadcastClient();
+  std::string _uri;
   std::weak_ptr<agv::FleetUpdateHandle> _fleet_handle;
   std::shared_ptr<WebsocketClient> _client;
+  websocketpp::connection_hdl _hdl;
   std::mutex _mutex;
+  std::condition_variable _cv;
   std::queue<nlohmann::json> _queue;
   std::thread _thread;
+  std::atomic_bool _connected;
   std::atomic_bool _shutdown;
 };
 

From 43db070208e26a22e56059e6592cd9cd9b9bd6e3 Mon Sep 17 00:00:00 2001
From: Yadunund <yadunund@openrobotics.org>
Date: Tue, 7 Dec 2021 19:00:31 +0800
Subject: [PATCH 04/79] Initialize broadcast client

Signed-off-by: Yadunund <yadunund@openrobotics.org>
---
 .../agv/internal_FleetUpdateHandle.hpp                | 11 +++++++++++
 1 file changed, 11 insertions(+)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
index 3c9eb87ba..88bb254b9 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
@@ -38,6 +38,7 @@
 #include "Node.hpp"
 #include "RobotContext.hpp"
 #include "../TaskManager.hpp"
+#include "../BroadcastClient.hpp"
 
 #include <rmf_traffic/schedule/Snapshot.hpp>
 #include <rmf_traffic/agv/Interpolate.hpp>
@@ -138,6 +139,9 @@ class FleetUpdateHandle::Implementation
   std::shared_ptr<rmf_traffic::schedule::Snappable> snappable;
   std::shared_ptr<rmf_traffic_ros2::schedule::Negotiation> negotiation;
 
+  // TODO(YV): Remove from default args
+  std::string broadcast_client_uri = "ws://localhost:9007";
+
   // Task planner params
   std::shared_ptr<rmf_task::CostCalculator> cost_calculator =
     rmf_task::BinaryPriorityScheme::make_cost_calculator();
@@ -150,6 +154,8 @@ class FleetUpdateHandle::Implementation
   std::unordered_map<RobotContextPtr,
     std::shared_ptr<TaskManager>> task_managers = {};
 
+  std::shared_ptr<BroadcastClient> broadcast_client = nullptr;
+
   rclcpp::Publisher<rmf_fleet_msgs::msg::FleetState>::SharedPtr
     fleet_state_pub = nullptr;
   rclcpp::TimerBase::SharedPtr fleet_state_timer = nullptr;
@@ -270,6 +276,11 @@ class FleetUpdateHandle::Implementation
     }
 
     return handle;
+
+    // Start the BroadcastClient
+    handle->_pimpl->broadcast_client = BroadcastClient::make(
+      handle->_pimpl->broadcast_client_uri,
+      handle);
   }
 
   void dock_summary_cb(const DockSummary::SharedPtr& msg);

From fb32c33f0e64b40b92074a96aab4ce1f0b817c80 Mon Sep 17 00:00:00 2001
From: Yadunund <yadunund@openrobotics.org>
Date: Wed, 8 Dec 2021 01:26:37 +0800
Subject: [PATCH 05/79] Debugging

Signed-off-by: Yadunund <yadunund@openrobotics.org>
---
 .../src/rmf_fleet_adapter/BroadcastClient.cpp | 88 ++++++++++++++-----
 .../src/rmf_fleet_adapter/BroadcastClient.hpp |  9 +-
 .../agv/internal_FleetUpdateHandle.hpp        |  8 +-
 3 files changed, 73 insertions(+), 32 deletions(-)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.cpp
index aa07aff8c..1ef6419cf 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.cpp
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2020 Open Source Robotics Foundation
+ * 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.
@@ -19,30 +19,35 @@
 
 #include "agv/internal_FleetUpdateHandle.hpp"
 
+#include <iostream>
+
 namespace rmf_fleet_adapter {
 //==============================================================================
 std::shared_ptr<BroadcastClient> BroadcastClient::make(
   const std::string& uri,
-  std::shared_ptr<agv::FleetUpdateHandle> fleet_handle)
+  std::weak_ptr<agv::FleetUpdateHandle> fleet_handle)
 {
   std::shared_ptr<BroadcastClient> client(new BroadcastClient());
   client->_uri = std::move(uri);
-  client->_fleet_handle = std::move(fleet_handle);
+  client->_fleet_handle = fleet_handle;
   client->_shutdown = false;
   client->_connected = false;
 
-  client->_client = std::make_shared<WebsocketClient>();
   // Initialize the Asio transport policy
-  client->_client->init_asio();
+  client->_client.init_asio();
+  client->_client.start_perpetual();
 
-  client->_client->set_open_handler(
-    [c = client](websocketpp::connection_hdl)
+  client->_client.set_open_handler(
+    [w = client->weak_from_this()](websocketpp::connection_hdl)
     {
+      const auto c = w.lock();
+      if (!c)
+        return;
       c->_connected = true;
       const auto fleet = c->_fleet_handle.lock();
       if (!fleet)
         return;
-      const auto impl = agv::FleetUpdateHandle::get(*fleet);
+      const auto impl = agv::FleetUpdateHandle::Implementation::get(*fleet);
       for (const auto& [conext, mgr] : impl.task_managers)
       {
         // TODO(YV): Publish latest state and log
@@ -53,64 +58,95 @@ std::shared_ptr<BroadcastClient> BroadcastClient::make(
         c->_uri.c_str());
     });
 
-  client->_client->set_close_handler(
-    [c = client](websocketpp::connection_hdl)
+  client->_client.set_close_handler(
+    [w = client->weak_from_this()](websocketpp::connection_hdl)
     {
+      const auto c = w.lock();
+      if (!c)
+        return;
       c->_connected = false;
     });
 
-  client->_client->set_fail_handler()
-  [c = client](websocketpp::connection_hdl)
+  client->_client.set_fail_handler(
+  [w = client->weak_from_this()](websocketpp::connection_hdl)
   {
+    const auto c = w.lock();
+    if (!c)
+      return;
     c->_connected = false;
-  };
+  });
 
-  client->_thread = std::thread(
-    [c = client]()
+  client->_processing_thread = std::thread(
+    [w = client->weak_from_this()]()
     {
-      while (!shutdown)
+      const auto c = w.lock();
+      if (!c)
+      {
+        std::cout << "Unable to lock weak_from_this()" << std::endl;
+        return;
+      }
+      std::cout << "Able to lock weak_from_this()" << std::endl;
+      while (!c->_shutdown)
       {
+        std::cout << "Inside while" << std::endl;
         const auto fleet = c->_fleet_handle.lock();
         if (!fleet)
+        {
+          std::cout << "Unable to lock fleet handle" << std::endl;
           continue;
-        const auto impl = agv::FleetUpdateHandle::get(*fleet);
+        }
+        std::cout << "Able to lock fleet handle" << std::endl;
+        const auto impl = agv::FleetUpdateHandle::Implementation::get(*fleet);
 
         // Try to connect to the server if we are not connected yet
         if (!c->_connected)
         {
+          std::cout << "Trying to connect to server " << c->_uri << std::endl;
           websocketpp::lib::error_code ec;
-          WebsocketClient::connection_ptr con = c->_client->get_connection(
+          WebsocketClient::connection_ptr con = c->_client.get_connection(
             c->_uri, ec);
+          std::cout << "ec: " << ec << std::endl;
           if (ec)
           {
-            RCLCPP_WARN(
+            std::cout << "Inside ec connection" << std::endl;
+            RCLCPP_ERROR(
               impl.node->get_logger(),
               "BroadcastClient unable to connect to [%s]. Please make sure "
               "server is running.",
               c->_uri.c_str());
             c->_connected = false;
+            std::cout << "Sleeping thread" << std::endl;
             std::this_thread::sleep_for(std::chrono::milliseconds(2000));
+            std::cout << "Done sleeping thread" << std::endl;
             continue;
           }
 
-          c->_handle = con->get_handle();
-          c->_client->connect(con);
+          std::cout << "Connection successful" << std::endl;
+          c->_client.send(c->_hdl, "Hello", websocketpp::frame::opcode::text, ec);
+          std::cout << "ec: " << ec << std::endl;
+
+          c->_hdl = con->get_handle();
+          c->_client.connect(con);
           // TODO(YV): Start asio io_service event loop
           c->_connected = true;
         }
 
+        std::cout << "Attending to items in queue" << std::endl;
         std::unique_lock<std::mutex> lock(c->_mutex);
+        std::cout << "Locked mutex" << std::endl;
         c->_cv.wait(lock,
           [c]()
           {
             return !c->_queue.empty();
           });
 
+        std::cout << "_queue not empty" << std::endl;
+
         while (!c->_queue.empty())
         {
           websocketpp::lib::error_code ec;
           const std::string& msg = c->_queue.front().dump();
-          c->_client->send(c->_hdl, msg, websocketpp::frame::opcode::text, ec);
+          c->_client.send(c->_hdl, msg, websocketpp::frame::opcode::text, ec);
           if (ec)
           {
             RCLCPP_ERROR(
@@ -124,6 +160,8 @@ std::shared_ptr<BroadcastClient> BroadcastClient::make(
       }
     });
 
+  std::cout << "Returning client" << std::endl;
+
   return client;
 
 }
@@ -133,6 +171,7 @@ void BroadcastClient::publish(const nlohmann::json& msg)
 {
   // TODO(YV): lock a mutex
   _queue.push(msg);
+  _cv.notify_all();
 }
 
 //==============================================================================
@@ -145,10 +184,11 @@ BroadcastClient::BroadcastClient()
 BroadcastClient::~BroadcastClient()
 {
   _shutdown = true;
-  if (_thread.joinable)
+  if (_processing_thread.joinable())
   {
-    _thread.join();
+    _processing_thread.join();
   }
+  _client.stop_perpetual();
 }
 
 } // namespace rmf_fleet_adapter
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.hpp
index 4932d8efb..cf70142d8 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.hpp
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2020 Open Source Robotics Foundation
+ * 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.
@@ -50,7 +50,7 @@ class BroadcastClient : public std::enable_shared_from_this<BroadcastClient>
   ///   "ws://localhost:9000"
   static std::shared_ptr<BroadcastClient> make(
     const std::string& uri,
-    std::shared_ptr<agv::FleetUpdateHandle> fleet_handle);
+    std::weak_ptr<agv::FleetUpdateHandle> fleet_handle);
 
   // Publish message
   void publish(const nlohmann::json& msg);
@@ -61,12 +61,13 @@ class BroadcastClient : public std::enable_shared_from_this<BroadcastClient>
   BroadcastClient();
   std::string _uri;
   std::weak_ptr<agv::FleetUpdateHandle> _fleet_handle;
-  std::shared_ptr<WebsocketClient> _client;
+  WebsocketClient _client;
   websocketpp::connection_hdl _hdl;
   std::mutex _mutex;
   std::condition_variable _cv;
   std::queue<nlohmann::json> _queue;
-  std::thread _thread;
+  std::thread _processing_thread;
+  std::thread _client_thread;
   std::atomic_bool _connected;
   std::atomic_bool _shutdown;
 };
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
index 88bb254b9..040471900 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
@@ -140,7 +140,7 @@ class FleetUpdateHandle::Implementation
   std::shared_ptr<rmf_traffic_ros2::schedule::Negotiation> negotiation;
 
   // TODO(YV): Remove from default args
-  std::string broadcast_client_uri = "ws://localhost:9007";
+  std::string broadcast_client_uri = "ws://127.0.0.1:9007";
 
   // Task planner params
   std::shared_ptr<rmf_task::CostCalculator> cost_calculator =
@@ -275,12 +275,12 @@ class FleetUpdateHandle::Implementation
         handle->_pimpl->charging_waypoints.insert(i);
     }
 
-    return handle;
-
     // Start the BroadcastClient
     handle->_pimpl->broadcast_client = BroadcastClient::make(
       handle->_pimpl->broadcast_client_uri,
-      handle);
+      handle->weak_from_this());
+
+    return handle;
   }
 
   void dock_summary_cb(const DockSummary::SharedPtr& msg);

From 0591869a16d74d14ab5a715aa1ade7ad87a86e40 Mon Sep 17 00:00:00 2001
From: Yadunund <yadunund@openrobotics.org>
Date: Wed, 8 Dec 2021 14:14:05 +0800
Subject: [PATCH 06/79] Cleanup

Signed-off-by: Yadunund <yadunund@openrobotics.org>
---
 .../include/rmf_fleet_adapter/agv/Adapter.hpp |  7 +-
 .../launch/fleet_adapter.launch.xml           |  3 +
 rmf_fleet_adapter/src/full_control/main.cpp   | 15 +++-
 .../src/rmf_fleet_adapter/BroadcastClient.cpp | 72 ++++++++-----------
 .../src/rmf_fleet_adapter/BroadcastClient.hpp |  3 +-
 .../src/rmf_fleet_adapter/agv/Adapter.cpp     |  5 +-
 .../agv/internal_FleetUpdateHandle.hpp        | 13 ++--
 7 files changed, 66 insertions(+), 52 deletions(-)

diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp
index 594259f8c..ca39142f4 100644
--- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp
+++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp
@@ -93,10 +93,15 @@ class Adapter : public std::enable_shared_from_this<Adapter>
   ///
   /// \param[in] navigation_graph
   ///   Specify the navigation graph used by the vehicles in this fleet.
+  ///
+  /// \param[in] server_uri
+  ///   Specify the URI for the websocket server that receives updates on tasks
+  ///   and states. If nullopt, data will not be published.
   std::shared_ptr<FleetUpdateHandle> add_fleet(
     const std::string& fleet_name,
     rmf_traffic::agv::VehicleTraits traits,
-    rmf_traffic::agv::Graph navigation_graph);
+    rmf_traffic::agv::Graph navigation_graph,
+    std::optional<std::string> server_uri = std::nullopt);
 
   /// Create a traffic light to help manage robots that can only support pause
   /// and resume commands.
diff --git a/rmf_fleet_adapter/launch/fleet_adapter.launch.xml b/rmf_fleet_adapter/launch/fleet_adapter.launch.xml
index 85e71a976..212b7410b 100644
--- a/rmf_fleet_adapter/launch/fleet_adapter.launch.xml
+++ b/rmf_fleet_adapter/launch/fleet_adapter.launch.xml
@@ -39,6 +39,7 @@
   <arg name="recharge_threshold" default="0.2" description="The fraction of total battery capacity below which the robot must return to its charger"/>
   <arg name="recharge_soc" default="1.0" description="The fraction of total battery capacity to which the robot should be charged"/>
   <arg name="experimental_lift_watchdog_service" default="" description="(Experimental) The name of a service to check whether a robot can enter a lift"/>
+  <arg name="server_uri" default="ws://127.0.0.1:8006" description="The URI of the api server to trasnmit state and task infromation. If empty, information will not be transmitted"/>
 
 
   <node pkg="rmf_fleet_adapter"
@@ -84,6 +85,8 @@
 
     <param name="experimental_lift_watchdog_service" value="$(var experimental_lift_watchdog_service)"/>
 
+    <param name="server_uri" value="$(var server_uri)"/>
+
     <param name="use_sim_time" value="$(var use_sim_time)"/>
   </node>
 
diff --git a/rmf_fleet_adapter/src/full_control/main.cpp b/rmf_fleet_adapter/src/full_control/main.cpp
index d4186bfac..b5590f37b 100644
--- a/rmf_fleet_adapter/src/full_control/main.cpp
+++ b/rmf_fleet_adapter/src/full_control/main.cpp
@@ -59,6 +59,7 @@
 
 #include <Eigen/Geometry>
 #include <unordered_set>
+#include <optional>
 
 //==============================================================================
 rmf_fleet_adapter::agv::RobotUpdateHandle::Unstable::Decision
@@ -839,8 +840,20 @@ std::shared_ptr<Connections> make_fleet(
   for (const auto& key : connections->graph->keys())
     std::cout << " -- " << key.first << std::endl;
 
+  std::optional<std::string> server_uri = std::nullopt;
+  const std::string uri =
+    node->declare_parameter("server_uri", std::string());
+  if (!uri.empty())
+  {
+    RCLCPP_ERROR(
+      node->get_logger(),
+      "API server URI: [%s]", uri.c_str());
+
+    server_uri = uri;
+  }
+
   connections->fleet = adapter->add_fleet(
-    fleet_name, *connections->traits, *connections->graph);
+    fleet_name, *connections->traits, *connections->graph, server_uri);
 
   // We disable fleet state publishing for this fleet adapter because we expect
   // the fleet drivers to publish these messages.
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.cpp
index 1ef6419cf..5a8dd10ba 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.cpp
@@ -34,15 +34,20 @@ std::shared_ptr<BroadcastClient> BroadcastClient::make(
   client->_connected = false;
 
   // Initialize the Asio transport policy
+  client->_client.clear_access_channels(websocketpp::log::alevel::all);
+  client->_client.clear_error_channels(websocketpp::log::elevel::all);
   client->_client.init_asio();
   client->_client.start_perpetual();
+  client->_client_thread = std::thread(
+    [c = client]()
+    {
+      c->_client.run();
+    });
 
   client->_client.set_open_handler(
-    [w = client->weak_from_this()](websocketpp::connection_hdl)
+    [c = client](websocketpp::connection_hdl)
     {
-      const auto c = w.lock();
-      if (!c)
-        return;
+      std::cout << "Open handler called" << std::endl;
       c->_connected = true;
       const auto fleet = c->_fleet_handle.lock();
       if (!fleet)
@@ -59,43 +64,29 @@ std::shared_ptr<BroadcastClient> BroadcastClient::make(
     });
 
   client->_client.set_close_handler(
-    [w = client->weak_from_this()](websocketpp::connection_hdl)
+    [c = client](websocketpp::connection_hdl)
     {
-      const auto c = w.lock();
-      if (!c)
-        return;
+      std::cout << " Close handler called" << std::endl;
       c->_connected = false;
     });
 
   client->_client.set_fail_handler(
-  [w = client->weak_from_this()](websocketpp::connection_hdl)
+  [c = client](websocketpp::connection_hdl)
   {
-    const auto c = w.lock();
-    if (!c)
-      return;
+    std::cout << "Fail handler called" << std::endl;
     c->_connected = false;
   });
 
   client->_processing_thread = std::thread(
-    [w = client->weak_from_this()]()
+    [c = client]()
     {
-      const auto c = w.lock();
-      if (!c)
-      {
-        std::cout << "Unable to lock weak_from_this()" << std::endl;
-        return;
-      }
-      std::cout << "Able to lock weak_from_this()" << std::endl;
       while (!c->_shutdown)
       {
-        std::cout << "Inside while" << std::endl;
         const auto fleet = c->_fleet_handle.lock();
         if (!fleet)
         {
-          std::cout << "Unable to lock fleet handle" << std::endl;
           continue;
         }
-        std::cout << "Able to lock fleet handle" << std::endl;
         const auto impl = agv::FleetUpdateHandle::Implementation::get(*fleet);
 
         // Try to connect to the server if we are not connected yet
@@ -105,35 +96,31 @@ std::shared_ptr<BroadcastClient> BroadcastClient::make(
           websocketpp::lib::error_code ec;
           WebsocketClient::connection_ptr con = c->_client.get_connection(
             c->_uri, ec);
-          std::cout << "ec: " << ec << std::endl;
+          c->_hdl = con->get_handle();
+          c->_client.connect(con);
+          // TOD(YV): Without sending a test payload, ec seems to be 0 even
+          // when the client has not connected. Avoid sending this message.
+          c->_client.send(c->_hdl, "Hello", websocketpp::frame::opcode::text, ec);
           if (ec)
           {
-            std::cout << "Inside ec connection" << std::endl;
-            RCLCPP_ERROR(
+            RCLCPP_WARN(
               impl.node->get_logger(),
               "BroadcastClient unable to connect to [%s]. Please make sure "
               "server is running.",
               c->_uri.c_str());
             c->_connected = false;
-            std::cout << "Sleeping thread" << std::endl;
             std::this_thread::sleep_for(std::chrono::milliseconds(2000));
-            std::cout << "Done sleeping thread" << std::endl;
             continue;
           }
-
-          std::cout << "Connection successful" << std::endl;
-          c->_client.send(c->_hdl, "Hello", websocketpp::frame::opcode::text, ec);
-          std::cout << "ec: " << ec << std::endl;
-
-          c->_hdl = con->get_handle();
-          c->_client.connect(con);
-          // TODO(YV): Start asio io_service event loop
+          RCLCPP_INFO(
+            impl.node->get_logger(),
+            "BroadcastClient successfully connected to [%s]",
+            c->_uri.c_str());
           c->_connected = true;
         }
 
         std::cout << "Attending to items in queue" << std::endl;
-        std::unique_lock<std::mutex> lock(c->_mutex);
-        std::cout << "Locked mutex" << std::endl;
+        std::unique_lock<std::mutex> lock(c->_wait_mutex);
         c->_cv.wait(lock,
           [c]()
           {
@@ -144,6 +131,7 @@ std::shared_ptr<BroadcastClient> BroadcastClient::make(
 
         while (!c->_queue.empty())
         {
+          std::lock_guard<std::mutex> lock(c->_queue_mutex);
           websocketpp::lib::error_code ec;
           const std::string& msg = c->_queue.front().dump();
           c->_client.send(c->_hdl, msg, websocketpp::frame::opcode::text, ec);
@@ -160,8 +148,6 @@ std::shared_ptr<BroadcastClient> BroadcastClient::make(
       }
     });
 
-  std::cout << "Returning client" << std::endl;
-
   return client;
 
 }
@@ -169,7 +155,7 @@ std::shared_ptr<BroadcastClient> BroadcastClient::make(
 //==============================================================================
 void BroadcastClient::publish(const nlohmann::json& msg)
 {
-  // TODO(YV): lock a mutex
+  std::lock_guard<std::mutex> lock(_queue_mutex);
   _queue.push(msg);
   _cv.notify_all();
 }
@@ -188,6 +174,10 @@ BroadcastClient::~BroadcastClient()
   {
     _processing_thread.join();
   }
+  if (_client_thread.joinable())
+  {
+    _client_thread.join();
+  }
   _client.stop_perpetual();
 }
 
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.hpp
index cf70142d8..3e2f8ca57 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.hpp
@@ -63,7 +63,8 @@ class BroadcastClient : public std::enable_shared_from_this<BroadcastClient>
   std::weak_ptr<agv::FleetUpdateHandle> _fleet_handle;
   WebsocketClient _client;
   websocketpp::connection_hdl _hdl;
-  std::mutex _mutex;
+  std::mutex _wait_mutex;
+  std::mutex _queue_mutex;
   std::condition_variable _cv;
   std::queue<nlohmann::json> _queue;
   std::thread _processing_thread;
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp
index 5ccba5234..b467f6f18 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp
@@ -208,7 +208,8 @@ std::shared_ptr<Adapter> Adapter::make(
 std::shared_ptr<FleetUpdateHandle> Adapter::add_fleet(
   const std::string& fleet_name,
   rmf_traffic::agv::VehicleTraits traits,
-  rmf_traffic::agv::Graph navigation_graph)
+  rmf_traffic::agv::Graph navigation_graph,
+  std::optional<std::string> server_uri)
 {
   auto planner =
     std::make_shared<std::shared_ptr<const rmf_traffic::agv::Planner>>(
@@ -221,7 +222,7 @@ std::shared_ptr<FleetUpdateHandle> Adapter::add_fleet(
   auto fleet = FleetUpdateHandle::Implementation::make(
     fleet_name, std::move(planner), _pimpl->node, _pimpl->worker,
     _pimpl->schedule_writer, _pimpl->mirror_manager.snapshot_handle(),
-    _pimpl->negotiation);
+    _pimpl->negotiation, server_uri);
 
   _pimpl->fleets.push_back(fleet);
   return fleet;
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
index 040471900..b885bd10b 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
@@ -138,9 +138,7 @@ class FleetUpdateHandle::Implementation
   std::shared_ptr<ParticipantFactory> writer;
   std::shared_ptr<rmf_traffic::schedule::Snappable> snappable;
   std::shared_ptr<rmf_traffic_ros2::schedule::Negotiation> negotiation;
-
-  // TODO(YV): Remove from default args
-  std::string broadcast_client_uri = "ws://127.0.0.1:9007";
+  std::optional<std::string> server_uri;
 
   // Task planner params
   std::shared_ptr<rmf_task::CostCalculator> cost_calculator =
@@ -276,9 +274,12 @@ class FleetUpdateHandle::Implementation
     }
 
     // Start the BroadcastClient
-    handle->_pimpl->broadcast_client = BroadcastClient::make(
-      handle->_pimpl->broadcast_client_uri,
-      handle->weak_from_this());
+    if (handle->_pimpl->server_uri.has_value())
+    {
+      handle->_pimpl->broadcast_client = BroadcastClient::make(
+        handle->_pimpl->server_uri.value(),
+        handle->weak_from_this());
+    }
 
     return handle;
   }

From bfd85a0192391cf14e1f37c6b644eaafe252eec3 Mon Sep 17 00:00:00 2001
From: Yadunund <yadunund@openrobotics.org>
Date: Wed, 8 Dec 2021 23:15:06 +0800
Subject: [PATCH 07/79] Publish fleet_state message

Signed-off-by: Yadunund <yadunund@openrobotics.org>
---
 rmf_fleet_adapter/CMakeLists.txt              |  8 +++
 .../agv/test/MockAdapter.hpp                  |  3 +-
 rmf_fleet_adapter/package.xml                 |  3 +-
 rmf_fleet_adapter/src/full_control/main.cpp   |  2 +-
 .../src/rmf_fleet_adapter/BroadcastClient.cpp |  9 ---
 .../agv/FleetUpdateHandle.cpp                 | 70 +++++++++++++++++++
 .../agv/test/MockAdapter.cpp                  |  5 +-
 7 files changed, 86 insertions(+), 14 deletions(-)

diff --git a/rmf_fleet_adapter/CMakeLists.txt b/rmf_fleet_adapter/CMakeLists.txt
index 30455b2d7..07d33d02a 100644
--- a/rmf_fleet_adapter/CMakeLists.txt
+++ b/rmf_fleet_adapter/CMakeLists.txt
@@ -36,8 +36,10 @@ set(dep_pkgs
   rmf_battery
   rmf_task
   std_msgs
+  rmf_api_msgs
   websocketpp
   nlohmann_json
+  nlohmann_json_schema_validator_vendor
 )
 foreach(pkg ${dep_pkgs})
   find_package(${pkg} REQUIRED)
@@ -79,11 +81,13 @@ target_link_libraries(rmf_fleet_adapter
   PRIVATE
     rmf_rxcpp
     nlohmann_json::nlohmann_json
+    rmf_api_msgs::rmf_api_msgs
     ${rmf_door_msgs_LIBRARIES}
     ${rmf_lift_msgs_LIBRARIES}
     ${rmf_dispenser_msgs_LIBRARIES}
     ${rmf_ingestor_msgs_LIBRARIES}
     ${websocketpp_LIBRARIES}
+    nlohmann_json_schema_validator
 )
 
 target_include_directories(rmf_fleet_adapter
@@ -100,7 +104,9 @@ target_include_directories(rmf_fleet_adapter
     ${rmf_lift_msgs_INCLUDE_DIRS}
     ${rmf_dispenser_msgs_INCLUDE_DIRS}
     ${rmf_ingestor_msgs_INCLUDE_DIRS}
+    ${rmf_api_msgs_INCLUDE_DIRS}
     ${WEBSOCKETPP_INCLUDE_DIR}
+    ${nlohmann_json_schema_validator_INCLUDE_DIRS}
 )
 
 if (BUILD_TESTING)
@@ -135,6 +141,7 @@ if (BUILD_TESTING)
       ${rmf_lift_msgs_INCLUDE_DIRS}
       ${rmf_dispenser_msgs_INCLUDE_DIRS}
       ${rmf_ingestor_msgs_INCLUDE_DIRS}
+      ${rmf_api_msgs_INCLUDE_DIRS}
       ${std_msgs_INCLUDE_DIRS}
       ${WEBSOCKETPP_INCLUDE_DIR}
   )
@@ -149,6 +156,7 @@ if (BUILD_TESTING)
       ${rmf_ingestor_msgs_LIBRARIES}
       rmf_fleet_adapter
       rmf_utils::rmf_utils
+      rmf_api_msgs::rmf_api_msgs
       ${std_msgs_LIBRARIES}
       ${websocketpp_LIBRARIES}
   )
diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/test/MockAdapter.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/test/MockAdapter.hpp
index 182145305..225730b29 100644
--- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/test/MockAdapter.hpp
+++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/test/MockAdapter.hpp
@@ -47,7 +47,8 @@ class MockAdapter : public std::enable_shared_from_this<MockAdapter>
   std::shared_ptr<FleetUpdateHandle> add_fleet(
     const std::string& fleet_name,
     rmf_traffic::agv::VehicleTraits traits,
-    rmf_traffic::agv::Graph navigation_graph);
+    rmf_traffic::agv::Graph navigation_graph,
+    std::optional<std::string> server_uri = std::nullopt);
 
   TrafficLight::UpdateHandlePtr add_traffic_light(
     std::shared_ptr<TrafficLight::CommandHandle> command,
diff --git a/rmf_fleet_adapter/package.xml b/rmf_fleet_adapter/package.xml
index 53c998690..575575c8a 100644
--- a/rmf_fleet_adapter/package.xml
+++ b/rmf_fleet_adapter/package.xml
@@ -25,12 +25,13 @@
   <depend>rmf_battery</depend>
   <depend>rmf_task</depend>
   <depend>std_msgs</depend>
+  <depend>rmf_api_msgs</depend>
   <depend condition="$RMF_ENABLE_FAILOVER == 1">stubborn_buddies</depend>
   <depend condition="$RMF_ENABLE_FAILOVER == 1">stubborn_buddies_msgs</depend>
 
   <depend>libwebsocketpp-dev</depend>
   <depend>nlohmann-json3-dev</depend>
-  <!-- <depend>nlohmann_json_schema_validator_vendor</depend> -->
+  <depend>nlohmann_json_schema_validator_vendor</depend>
 
   <build_depend>eigen</build_depend>
   <build_depend>yaml-cpp</build_depend>
diff --git a/rmf_fleet_adapter/src/full_control/main.cpp b/rmf_fleet_adapter/src/full_control/main.cpp
index b5590f37b..fd062d175 100644
--- a/rmf_fleet_adapter/src/full_control/main.cpp
+++ b/rmf_fleet_adapter/src/full_control/main.cpp
@@ -857,7 +857,7 @@ std::shared_ptr<Connections> make_fleet(
 
   // We disable fleet state publishing for this fleet adapter because we expect
   // the fleet drivers to publish these messages.
-  connections->fleet->fleet_state_publish_period(std::nullopt);
+  // connections->fleet->fleet_state_publish_period(std::nullopt);
 
   connections->closed_lanes_pub =
     adapter->node()->create_publisher<rmf_fleet_msgs::msg::ClosedLanes>(
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.cpp
index 5a8dd10ba..d7a7aa84f 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.cpp
@@ -19,8 +19,6 @@
 
 #include "agv/internal_FleetUpdateHandle.hpp"
 
-#include <iostream>
-
 namespace rmf_fleet_adapter {
 //==============================================================================
 std::shared_ptr<BroadcastClient> BroadcastClient::make(
@@ -47,7 +45,6 @@ std::shared_ptr<BroadcastClient> BroadcastClient::make(
   client->_client.set_open_handler(
     [c = client](websocketpp::connection_hdl)
     {
-      std::cout << "Open handler called" << std::endl;
       c->_connected = true;
       const auto fleet = c->_fleet_handle.lock();
       if (!fleet)
@@ -66,14 +63,12 @@ std::shared_ptr<BroadcastClient> BroadcastClient::make(
   client->_client.set_close_handler(
     [c = client](websocketpp::connection_hdl)
     {
-      std::cout << " Close handler called" << std::endl;
       c->_connected = false;
     });
 
   client->_client.set_fail_handler(
   [c = client](websocketpp::connection_hdl)
   {
-    std::cout << "Fail handler called" << std::endl;
     c->_connected = false;
   });
 
@@ -92,7 +87,6 @@ std::shared_ptr<BroadcastClient> BroadcastClient::make(
         // Try to connect to the server if we are not connected yet
         if (!c->_connected)
         {
-          std::cout << "Trying to connect to server " << c->_uri << std::endl;
           websocketpp::lib::error_code ec;
           WebsocketClient::connection_ptr con = c->_client.get_connection(
             c->_uri, ec);
@@ -119,7 +113,6 @@ std::shared_ptr<BroadcastClient> BroadcastClient::make(
           c->_connected = true;
         }
 
-        std::cout << "Attending to items in queue" << std::endl;
         std::unique_lock<std::mutex> lock(c->_wait_mutex);
         c->_cv.wait(lock,
           [c]()
@@ -127,8 +120,6 @@ std::shared_ptr<BroadcastClient> BroadcastClient::make(
             return !c->_queue.empty();
           });
 
-        std::cout << "_queue not empty" << std::endl;
-
         while (!c->_queue.empty())
         {
           std::lock_guard<std::mutex> lock(c->_queue_mutex);
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
index acd37c4a8..66aa3a2c2 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
@@ -38,6 +38,11 @@
 #include <rmf_task_msgs/msg/delivery.hpp>
 #include <rmf_task_msgs/msg/loop.hpp>
 
+#include <nlohmann/json.hpp>
+#include <nlohmann/json-schema.hpp>
+
+#include <rmf_api_msgs/schemas/fleet_state.hpp>
+
 #include <sstream>
 #include <unordered_map>
 #include <unordered_set>
@@ -854,11 +859,76 @@ void FleetUpdateHandle::Implementation::publish_fleet_state() const
   for (const auto& [context, mgr] : task_managers)
     robot_states.emplace_back(convert_state(*mgr));
 
+  const auto mode_to_status = [](const uint32_t mode) -> std::string
+    {
+      // uninitialized, offline, shutdown ,idle, charging, working, error
+      if (mode == 0)
+        return "idle";
+      else if (mode == 1)
+        return "charging";
+      else if (mode == 2)
+        return "working";
+      else
+        return "error";
+    };
+
+  // Publish to API server
+  if (broadcast_client)
+  {
+    nlohmann::json fleet_state_msg = { {"name", {}}, {"robots", {}}};
+    fleet_state_msg["name"] = name;
+    for (const auto& state : robot_states)
+    {
+      nlohmann::json json;
+      json["name"] = state.name;
+      json["status"] = mode_to_status(state.mode.mode);
+      json["task_id"] = state.task_id;
+      json["unix_millis_time"] =
+        std::chrono::duration_cast<std::chrono::milliseconds>(
+        std::chrono::seconds(state.location.t.sec) +
+        std::chrono::nanoseconds(state.location.t.nanosec)).count();
+      nlohmann::json location;
+      location["map"] = state.location.level_name;
+      location["x"] = state.location.x;
+      location["y"] = state.location.y;
+      location["yaw"] = state.location.yaw;
+      json["location"] = location;
+      json["battery"] = state.battery_percent / 100.0;
+      // TODO(YV): json["issues"]
+      fleet_state_msg["robots"].push_back(json);
+    }
+    // TODO(YV): Validating the schema requires a loader to be defined. This is
+    // commented out temporarily until the proper loader can be defined.
+    // const auto schema = rmf_api_msgs::schemas::fleet_state;
+    // nlohmann::json_uri json_uri = nlohmann::json_uri{schema["$id"]};
+    // const auto loader =
+    //   [](const nlohmann::json_uri& id, nlohmann::json& value)
+    //   {
+
+    //   };
+    // try
+    // {
+    //   nlohmann::json_schema::json_validator validator(schema, loader);
+    //   validator.validate(fleet_state_msg);
+    // }
+    // catch (const std::exception& e)
+    // {
+    //   RCLCPP_ERROR(
+    //     node->get_logger(),
+    //     "Unable to publish fleet state json message: %s",
+    //     e.what());
+    //   return;
+    // }
+
+    broadcast_client->publish(fleet_state_msg);
+  }
+
   auto fleet_state = rmf_fleet_msgs::build<rmf_fleet_msgs::msg::FleetState>()
     .name(name)
     .robots(std::move(robot_states));
 
   fleet_state_pub->publish(std::move(fleet_state));
+
 }
 
 //==============================================================================
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp
index d7d8fa419..05284e5dd 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp
@@ -227,7 +227,8 @@ MockAdapter::MockAdapter(
 std::shared_ptr<FleetUpdateHandle> MockAdapter::add_fleet(
   const std::string& fleet_name,
   rmf_traffic::agv::VehicleTraits traits,
-  rmf_traffic::agv::Graph navigation_graph)
+  rmf_traffic::agv::Graph navigation_graph,
+  std::optional<std::string> server_uri)
 {
   auto planner =
     std::make_shared<std::shared_ptr<const rmf_traffic::agv::Planner>>(
@@ -240,7 +241,7 @@ std::shared_ptr<FleetUpdateHandle> MockAdapter::add_fleet(
   auto fleet = FleetUpdateHandle::Implementation::make(
     fleet_name, std::move(planner), _pimpl->node, _pimpl->worker,
     std::make_shared<SimpleParticipantFactory>(_pimpl->schedule),
-    _pimpl->schedule->snappable(), nullptr);
+    _pimpl->schedule->snappable(), nullptr, server_uri);
 
   _pimpl->fleets.push_back(fleet);
   return fleet;

From e4bf9079d800f0af02620a8ed14a556607cc37cd Mon Sep 17 00:00:00 2001
From: Yadunund <yadunund@openrobotics.org>
Date: Wed, 8 Dec 2021 23:37:38 +0800
Subject: [PATCH 08/79] Store broadcast_client within TaskManager

Signed-off-by: Yadunund <yadunund@openrobotics.org>
---
 .../src/rmf_fleet_adapter/TaskManager.cpp     | 20 +++++++++++++++----
 .../src/rmf_fleet_adapter/TaskManager.hpp     | 12 +++++++++--
 .../agv/FleetUpdateHandle.cpp                 |  2 +-
 3 files changed, 27 insertions(+), 7 deletions(-)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
index 34b1b78d3..8268ec810 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
@@ -36,9 +36,12 @@
 namespace rmf_fleet_adapter {
 
 //==============================================================================
-TaskManagerPtr TaskManager::make(agv::RobotContextPtr context)
+TaskManagerPtr TaskManager::make(
+  agv::RobotContextPtr context,
+  std::weak_ptr<BroadcastClient> broadcast_client)
 {
-  auto mgr = TaskManagerPtr(new TaskManager(std::move(context)));
+  auto mgr = TaskManagerPtr(new TaskManager(
+    std::move(context), std::move(broadcast_client)));
   mgr->_emergency_sub = mgr->_context->node()->emergency_notice()
     .observe_on(rxcpp::identity_same_worker(mgr->_context->worker()))
     .subscribe(
@@ -80,8 +83,11 @@ TaskManagerPtr TaskManager::make(agv::RobotContextPtr context)
 }
 
 //==============================================================================
-TaskManager::TaskManager(agv::RobotContextPtr context)
-: _context(std::move(context))
+TaskManager::TaskManager(
+  agv::RobotContextPtr context,
+  std::weak_ptr<BroadcastClient> broadcast_client)
+: _context(std::move(context)),
+  _broadcast_client(std::move(broadcast_client))
 {
   // Do nothing. The make() function does all further initialization.
 }
@@ -133,6 +139,12 @@ agv::ConstRobotContextPtr TaskManager::context() const
   return _context;
 }
 
+//==============================================================================
+std::weak_ptr<BroadcastClient> TaskManager::broadcast_client() const
+{
+  return _broadcast_client;
+}
+
 //==============================================================================
 void TaskManager::set_queue(
   const std::vector<TaskManager::Assignment>& assignments,
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
index f6c764dc5..22f149e52 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
@@ -20,6 +20,7 @@
 
 #include "Task.hpp"
 #include "agv/RobotContext.hpp"
+#include "BroadcastClient.hpp"
 
 #include <rmf_traffic/agv/Planner.hpp>
 
@@ -42,7 +43,9 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
 {
 public:
 
-  static std::shared_ptr<TaskManager> make(agv::RobotContextPtr context);
+  static std::shared_ptr<TaskManager> make(
+    agv::RobotContextPtr context,
+    std::weak_ptr<BroadcastClient> broadcast_client);
 
   using Start = rmf_traffic::agv::Plan::Start;
   using StartSet = rmf_traffic::agv::Plan::StartSet;
@@ -61,6 +64,8 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
 
   agv::ConstRobotContextPtr context() const;
 
+  std::weak_ptr<BroadcastClient> broadcast_client() const;
+
   const Task* current_task() const;
 
   /// Set the queue for this task manager with assignments generated from the
@@ -87,9 +92,12 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
 
 private:
 
-  TaskManager(agv::RobotContextPtr context);
+  TaskManager(
+    agv::RobotContextPtr context,
+    std::weak_ptr<BroadcastClient> broadcast_client);
 
   agv::RobotContextPtr _context;
+  std::weak_ptr<BroadcastClient> _broadcast_client;
   std::shared_ptr<Task> _active_task;
   std::vector<std::shared_ptr<Task>> _queue;
   rmf_utils::optional<Start> _expected_finish_location;
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
index 66aa3a2c2..96478a754 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
@@ -1188,7 +1188,7 @@ void FleetUpdateHandle::add_robot(
           }
 
           fleet->_pimpl->task_managers.insert({context,
-            TaskManager::make(context)});
+            TaskManager::make(context, fleet->_pimpl->broadcast_client)});
         });
     });
 }

From c8e0517c42f3ccfe0f5c64e3c96f291c4d02ca5e Mon Sep 17 00:00:00 2001
From: Yadunund <yadunund@openrobotics.org>
Date: Thu, 9 Dec 2021 01:17:31 +0800
Subject: [PATCH 09/79] Changes to support rmf_task/redesign_v2

Signed-off-by: Yadunund <yadunund@openrobotics.org>
---
 .../src/rmf_fleet_adapter/Task.cpp            |   6 +-
 .../src/rmf_fleet_adapter/Task.hpp            |  10 +-
 .../src/rmf_fleet_adapter/TaskManager.cpp     | 106 +++++++-----------
 .../src/rmf_fleet_adapter/TaskManager.hpp     |  10 +-
 .../agv/FleetUpdateHandle.cpp                 |  52 ++++-----
 .../rmf_fleet_adapter/agv/RobotContext.cpp    |  12 +-
 .../rmf_fleet_adapter/agv/RobotContext.hpp    |  22 ++--
 .../agv/RobotUpdateHandle.cpp                 |   2 +-
 .../agv/internal_FleetUpdateHandle.hpp        |   6 +-
 .../rmf_fleet_adapter/phases/GoToPlace.cpp    |   7 +-
 .../rmf_fleet_adapter/tasks/ChargeBattery.cpp |   7 +-
 .../rmf_fleet_adapter/tasks/ChargeBattery.hpp |   4 +-
 .../src/rmf_fleet_adapter/tasks/Clean.cpp     |   6 +-
 .../src/rmf_fleet_adapter/tasks/Clean.hpp     |   4 +-
 .../src/rmf_fleet_adapter/tasks/Delivery.cpp  |   8 +-
 .../src/rmf_fleet_adapter/tasks/Delivery.hpp  |   2 +-
 .../src/rmf_fleet_adapter/tasks/Loop.cpp      |   4 +-
 .../src/rmf_fleet_adapter/tasks/Loop.hpp      |   2 +-
 rmf_fleet_adapter/test/test_Task.cpp          |  17 ++-
 19 files changed, 138 insertions(+), 149 deletions(-)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp
index 9eec4e68c..b3c561858 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp
@@ -32,7 +32,7 @@ std::shared_ptr<Task> Task::make(
   PendingPhases phases,
   rxcpp::schedulers::worker worker,
   rmf_traffic::Time deployment_time,
-  rmf_task::agv::State finish_state,
+  rmf_task::State finish_state,
   rmf_task::ConstRequestPtr request)
 {
   return std::make_shared<Task>(
@@ -102,7 +102,7 @@ const rmf_traffic::Time Task::deployment_time() const
 }
 
 //==============================================================================
-const rmf_task::agv::State Task::finish_state() const
+const rmf_task::State Task::finish_state() const
 {
   return _finish_state;
 }
@@ -126,7 +126,7 @@ Task::Task(
   std::vector<std::unique_ptr<PendingPhase>> phases,
   rxcpp::schedulers::worker worker,
   rmf_traffic::Time deployment_time,
-  rmf_task::agv::State finish_state,
+  rmf_task::State finish_state,
   rmf_task::ConstRequestPtr request)
 : _id(std::move(id)),
   _pending_phases(std::move(phases)),
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp
index 118d57271..0939950b1 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp
@@ -27,7 +27,7 @@
 #include <rmf_task_msgs/msg/task_summary.hpp>
 
 #include <rmf_task/Request.hpp>
-#include <rmf_task/agv/State.hpp>
+#include <rmf_task/State.hpp>
 
 #include <rmf_rxcpp/RxJobs.hpp>
 #include <rmf_rxcpp/Publisher.hpp>
@@ -103,7 +103,7 @@ class Task : public std::enable_shared_from_this<Task>
     PendingPhases phases,
     rxcpp::schedulers::worker worker,
     rmf_traffic::Time deployment_time,
-    rmf_task::agv::State finish_state,
+    rmf_task::State finish_state,
     rmf_task::ConstRequestPtr request);
 
   void begin();
@@ -132,7 +132,7 @@ class Task : public std::enable_shared_from_this<Task>
   const rmf_traffic::Time deployment_time() const;
 
   /// Get the finish state of this Task
-  const rmf_task::agv::State finish_state() const;
+  const rmf_task::State finish_state() const;
 
   /// Set the TaskProfile of this task
   void task_profile(TaskProfileMsg profile);
@@ -147,7 +147,7 @@ class Task : public std::enable_shared_from_this<Task>
     PendingPhases phases,
     rxcpp::schedulers::worker worker,
     rmf_traffic::Time deployment_time,
-    rmf_task::agv::State finish_state,
+    rmf_task::State finish_state,
     rmf_task::ConstRequestPtr request);
 
   std::string _id;
@@ -167,7 +167,7 @@ class Task : public std::enable_shared_from_this<Task>
   rmf_utils::optional<builtin_interfaces::msg::Time> _initial_time;
 
   rmf_traffic::Time _deployment_time;
-  rmf_task::agv::State _finish_state;
+  rmf_task::State _finish_state;
   rmf_task::ConstRequestPtr _request;
 
   TaskProfileMsg _profile;
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
index 8268ec810..d796464df 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
@@ -79,6 +79,9 @@ TaskManagerPtr TaskManager::make(
 
   mgr->_begin_waiting();
 
+  mgr->_travel_estimator = std::make_shared<rmf_task::TravelEstimator>(
+    mgr->_context->task_planner()->configuration().parameters());
+
   return mgr;
 }
 
@@ -111,9 +114,8 @@ auto TaskManager::expected_finish_state() const -> State
 
   // Update battery soc and finish time in the current state
   auto finish_state = _context->current_task_end_state();
-  auto location = finish_state.location();
-  location.time(rmf_traffic_ros2::convert(_context->node()->now()));
-  finish_state.location(location);
+  auto location = finish_state.extract_plan_start().value();
+  finish_state.time(rmf_traffic_ros2::convert(_context->node()->now()));
 
   const double current_battery_soc = _context->current_battery_soc();
   finish_state.battery_soc(current_battery_soc);
@@ -161,15 +163,16 @@ void TaskManager::set_queue(
     for (std::size_t i = 0; i < assignments.size(); ++i)
     {
       const auto& a = assignments[i];
-      auto start = _context->current_task_end_state().location();
+      auto start =
+        _context->current_task_end_state().extract_plan_start().value();
       if (i != 0)
-        start = assignments[i-1].state().location();
+        start = assignments[i-1].finish_state().extract_plan_start().value();
       start.time(a.deployment_time());
       const auto request = a.request();
 
       TaskProfileMsg task_profile;
-      bool auto_request = request->automatic();
-      const auto it = task_profiles.find(request->id());
+      bool auto_request = request->booking()->automatic();
+      const auto it = task_profiles.find(request->booking()->id());
       if (it != task_profiles.end())
       {
         task_profile = it->second;
@@ -178,10 +181,10 @@ void TaskManager::set_queue(
       {
         assert(auto_request);
         // We may have an auto-generated request
-        task_profile.task_id = request->id();
+        task_profile.task_id = request->booking()->id();
         task_profile.submission_time = _context->node()->now();
         task_profile.description.start_time = rmf_traffic_ros2::convert(
-          request->earliest_start_time());
+          request->booking()->earliest_start_time());
       }
 
       using namespace rmf_task::requests;
@@ -194,7 +197,7 @@ void TaskManager::set_queue(
           _context,
           start,
           a.deployment_time(),
-          a.state());
+          a.finish_state());
 
         // Populate task_profile for auto-generated Clean request
         if (auto_request)
@@ -223,7 +226,7 @@ void TaskManager::set_queue(
           _context,
           start,
           a.deployment_time(),
-          a.state());
+          a.finish_state());
 
         // Populate task_profile for auto-generated ChargeBattery request
         if (auto_request)
@@ -244,7 +247,7 @@ void TaskManager::set_queue(
           _context,
           start,
           a.deployment_time(),
-          a.state(),
+          a.finish_state(),
           task_profile.description.delivery);
 
         // Populate task_profile for auto-generated Delivery request
@@ -280,7 +283,7 @@ void TaskManager::set_queue(
           _context,
           start,
           a.deployment_time(),
-          a.state());
+          a.finish_state());
 
         // Populate task_profile for auto-generated Loop request
         if (auto_request)
@@ -315,7 +318,7 @@ void TaskManager::set_queue(
           "[TaskManager] Un-supported request type in assignment list. "
           "Please update the implementation of TaskManager::set_queue() to "
           "support request with task_id:[%s]",
-          a.request()->id().c_str());
+          a.request()->booking()->id().c_str());
 
         continue;
       }
@@ -338,7 +341,7 @@ const std::vector<rmf_task::ConstRequestPtr> TaskManager::requests() const
   requests.reserve(_queue.size());
   for (const auto& task : _queue)
   {
-    if (task->request()->automatic())
+    if (task->request()->booking()->automatic())
     {
       continue;
     }
@@ -393,7 +396,7 @@ void TaskManager::_begin_next_task()
   // time as task is completed.
   const auto deployment_time = std::min(
     next_task->deployment_time(),
-    next_task->request()->earliest_start_time());
+    next_task->request()->booking()->earliest_start_time());
 
   if (now >= deployment_time)
   {
@@ -557,7 +560,9 @@ void TaskManager::retreat_to_charger()
     return;
 
   const auto current_state = expected_finish_state();
-  if (current_state.waypoint() == current_state.charging_waypoint())
+  const auto charging_waypoint =
+    current_state.dedicated_charging_waypoint().value();
+  if (current_state.waypoint() == charging_waypoint)
     return;
 
   const auto& constraints = task_planner->configuration().constraints();
@@ -566,76 +571,45 @@ void TaskManager::retreat_to_charger()
   const double current_battery_soc = _context->current_battery_soc();
 
   const auto& parameters = task_planner->configuration().parameters();
-  auto& estimate_cache = *(task_planner->estimate_cache());
-
-  double retreat_battery_drain = 0.0;
-  const auto endpoints = std::make_pair(current_state.waypoint(),
-      current_state.charging_waypoint());
-  const auto& cache_result = estimate_cache.get(endpoints);
-
-  if (cache_result)
-  {
-    retreat_battery_drain = cache_result->dsoc;
-  }
-  else
+  // TODO(YV): Expose the TravelEstimator in the TaskPlanner to benefit from
+  // caching
+  const rmf_traffic::agv::Planner::Goal retreat_goal{charging_waypoint};
+  const auto result = _travel_estimator->estimate(
+    current_state.extract_plan_start().value(), retreat_goal);
+  if (!result.has_value())
   {
-    const rmf_traffic::agv::Planner::Goal retreat_goal{
-      current_state.charging_waypoint()};
-    const auto result_to_charger = parameters.planner()->plan(
-      current_state.location(), retreat_goal);
-
-    // We assume we can always compute a plan
-    double dSOC_motion = 0.0;
-    double dSOC_device = 0.0;
-    rmf_traffic::Duration retreat_duration = rmf_traffic::Duration{0};
-    rmf_traffic::Time itinerary_start_time = current_state.finish_time();
-
-    for (const auto& itinerary : result_to_charger->get_itinerary())
-    {
-      const auto& trajectory = itinerary.trajectory();
-      const auto& finish_time = *trajectory.finish_time();
-      const rmf_traffic::Duration itinerary_duration =
-        finish_time - itinerary_start_time;
-
-      dSOC_motion =
-        parameters.motion_sink()->compute_change_in_charge(
-        trajectory);
-      dSOC_device =
-        parameters.ambient_sink()->compute_change_in_charge(
-        rmf_traffic::time::to_seconds(itinerary_duration));
-      retreat_battery_drain += dSOC_motion + dSOC_device;
-      retreat_duration += itinerary_duration;
-      itinerary_start_time = finish_time;
-    }
-    estimate_cache.set(endpoints, retreat_duration,
-      retreat_battery_drain);
+    RCLCPP_WARN(
+      _context->node()->get_logger(),
+      "Unable to compute estimate of journey back to charger for robot [%s]",
+      _context->name().c_str());
+    return;
   }
 
   const double battery_soc_after_retreat =
-    current_battery_soc - retreat_battery_drain;
+    current_battery_soc - result->change_in_charge();
 
   if ((battery_soc_after_retreat < retreat_threshold) &&
     (battery_soc_after_retreat > threshold_soc))
   {
     // Add a new charging task to the task queue
     const auto charging_request = rmf_task::requests::ChargeBattery::make(
-      current_state.finish_time());
+      current_state.time().value());
     const auto model = charging_request->description()->make_model(
-      current_state.finish_time(),
+      current_state.time().value(),
       parameters);
 
     const auto finish = model->estimate_finish(
       current_state,
       constraints,
-      estimate_cache);
+      *_travel_estimator);
 
     if (!finish)
       return;
 
-    rmf_task::agv::TaskPlanner::Assignment charging_assignment(
+    rmf_task::TaskPlanner::Assignment charging_assignment(
       charging_request,
       finish.value().finish_state(),
-      current_state.finish_time());
+      current_state.time().value());
 
     set_queue({charging_assignment});
 
@@ -698,7 +672,7 @@ void TaskManager::_populate_task_summary(
     msg.start_time = rmf_traffic_ros2::convert(
       task->deployment_time());
     msg.end_time = rmf_traffic_ros2::convert(
-      task->finish_state().finish_time());
+      task->finish_state().time().value());
     msg.task_profile = task->task_profile();
   }
 
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
index 22f149e52..28d9634f3 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
@@ -24,7 +24,7 @@
 
 #include <rmf_traffic/agv/Planner.hpp>
 
-#include <rmf_task/agv/TaskPlanner.hpp>
+#include <rmf_task/TaskPlanner.hpp>
 
 #include <rmf_fleet_msgs/msg/robot_mode.hpp>
 #include <rmf_task_msgs/msg/task_summary.hpp>
@@ -49,8 +49,8 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
 
   using Start = rmf_traffic::agv::Plan::Start;
   using StartSet = rmf_traffic::agv::Plan::StartSet;
-  using Assignment = rmf_task::agv::TaskPlanner::Assignment;
-  using State = rmf_task::agv::State;
+  using Assignment = rmf_task::TaskPlanner::Assignment;
+  using State = rmf_task::State;
   using RobotModeMsg = rmf_fleet_msgs::msg::RobotMode;
   using TaskProfileMsg = rmf_task_msgs::msg::TaskProfile;
   using TaskProfiles = std::unordered_map<std::string, TaskProfileMsg>;
@@ -120,6 +120,10 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
   // Use the _register_executed_task() to populate this container.
   std::vector<std::string> _executed_task_registry;
 
+  // TravelEstimator for caching travel estimates for automatic charging
+  // retreat. TODO(YV): Expose the TaskPlanner's TravelEstimator.
+  std::shared_ptr<rmf_task::TravelEstimator> _travel_estimator;
+
   /// Callback for task timer which begins next task if its deployment time has passed
   void _begin_next_task();
 
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
index 96478a754..3ae257a5c 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
@@ -28,8 +28,8 @@
 #include "../tasks/Delivery.hpp"
 #include "../tasks/Loop.hpp"
 
-#include <rmf_task/agv/Constraints.hpp>
-#include <rmf_task/agv/Parameters.hpp>
+#include <rmf_task/Constraints.hpp>
+#include <rmf_task/Parameters.hpp>
 #include <rmf_task/requests/Clean.hpp>
 #include <rmf_task/requests/Delivery.hpp>
 #include <rmf_task/requests/Loop.hpp>
@@ -470,16 +470,16 @@ void FleetUpdateHandle::Implementation::bid_notice_cb(
     debug_stream << "--Agent: " << i << std::endl;
     for (const auto& a : assignments[i])
     {
-      const auto& s = a.state();
+      const auto& s = a.finish_state();
       const double request_seconds =
-        a.request()->earliest_start_time().time_since_epoch().count()/1e9;
+        a.request()->booking()->earliest_start_time().time_since_epoch().count()/1e9;
       const double start_seconds =
         a.deployment_time().time_since_epoch().count()/1e9;
-      const rmf_traffic::Time finish_time = s.finish_time();
+      const rmf_traffic::Time finish_time = s.time().value();
       const double finish_seconds = finish_time.time_since_epoch().count()/1e9;
-      debug_stream << "    <" << a.request()->id() << ": " << request_seconds
+      debug_stream << "    <" << a.request()->booking()->id() << ": " << request_seconds
                    << ", " << start_seconds
-                   << ", "<< finish_seconds << ", " << 100* s.battery_soc()
+                   << ", "<< finish_seconds << ", " << s.battery_soc().value()
                    << "%>" << std::endl;
     }
   }
@@ -508,10 +508,10 @@ void FleetUpdateHandle::Implementation::bid_notice_cb(
   {
     for (const auto& assignment : agent)
     {
-      if (assignment.request()->id() == id)
+      if (assignment.request()->booking()->id() == id)
       {
         bid_proposal.finish_time = rmf_traffic_ros2::convert(
-          assignment.state().finish_time());
+          assignment.finish_state().time().value());
         if (robot_name_map.find(index) != robot_name_map.end())
           bid_proposal.robot_name = robot_name_map[index];
         break;
@@ -748,7 +748,7 @@ auto FleetUpdateHandle::Implementation::is_valid_assignments(
   {
     for (const auto& a : agent)
     {
-      if (executed_tasks.find(a.request()->id()) != executed_tasks.end())
+      if (executed_tasks.find(a.request()->booking()->id()) != executed_tasks.end())
         return false;
     }
   }
@@ -938,14 +938,14 @@ auto FleetUpdateHandle::Implementation::allocate_tasks(
 {
   // Collate robot states, constraints and combine new requestptr with
   // requestptr of non-charging tasks in task manager queues
-  std::vector<rmf_task::agv::State> states;
+  std::vector<rmf_task::State> states;
   std::vector<rmf_task::ConstRequestPtr> pending_requests;
   std::string id = "";
 
   if (new_request)
   {
     pending_requests.push_back(new_request);
-    id = new_request->id();
+    id = new_request->booking()->id();
   }
 
   for (const auto& t : task_managers)
@@ -963,7 +963,7 @@ auto FleetUpdateHandle::Implementation::allocate_tasks(
     for (auto it = pending_requests.begin(); it != pending_requests.end(); ++it)
     {
       auto pending_request = *it;
-      if (pending_request->id() == ignore_request->id())
+      if (pending_request->booking()->id() == ignore_request->booking()->id())
         ignore_request_it = it;
     }
     if (ignore_request_it != pending_requests.end())
@@ -972,14 +972,14 @@ auto FleetUpdateHandle::Implementation::allocate_tasks(
       RCLCPP_INFO(
         node->get_logger(),
         "Request with task_id:[%s] will be ignored during task allocation.",
-        ignore_request->id().c_str());
+        ignore_request->booking()->id().c_str());
     }
     else
     {
       RCLCPP_WARN(
         node->get_logger(),
         "Request with task_id:[%s] is not present in any of the task queues.",
-        ignore_request->id().c_str());
+        ignore_request->booking()->id().c_str());
     }
   }
 
@@ -996,14 +996,14 @@ auto FleetUpdateHandle::Implementation::allocate_tasks(
     pending_requests);
 
   auto assignments_ptr = std::get_if<
-    rmf_task::agv::TaskPlanner::Assignments>(&result);
+    rmf_task::TaskPlanner::Assignments>(&result);
 
   if (!assignments_ptr)
   {
     auto error = std::get_if<
-      rmf_task::agv::TaskPlanner::TaskPlannerError>(&result);
+      rmf_task::TaskPlanner::TaskPlannerError>(&result);
 
-    if (*error == rmf_task::agv::TaskPlanner::TaskPlannerError::low_battery)
+    if (*error == rmf_task::TaskPlanner::TaskPlannerError::low_battery)
     {
       RCLCPP_ERROR(
         node->get_logger(),
@@ -1013,7 +1013,7 @@ auto FleetUpdateHandle::Implementation::allocate_tasks(
     }
 
     else if (*error ==
-      rmf_task::agv::TaskPlanner::TaskPlannerError::limited_capacity)
+      rmf_task::TaskPlanner::TaskPlannerError::limited_capacity)
     {
       RCLCPP_ERROR(
         node->get_logger(),
@@ -1098,8 +1098,8 @@ void FleetUpdateHandle::add_robot(
         // *INDENT-ON*
       }
 
-      rmf_task::agv::State state = rmf_task::agv::State{
-        start[0], charger_wp.value(), 1.0};
+      rmf_task::State state;
+      state.load_basic(start[0], charger_wp.value(), 1.0);
 
       auto context = std::make_shared<RobotContext>(
         RobotContext
@@ -1344,25 +1344,25 @@ bool FleetUpdateHandle::set_task_planner_params(
     (recharge_threshold >= 0.0 && recharge_threshold <= 1.0) &&
     (recharge_soc >= 0.0 && recharge_threshold <= 1.0))
   {
-    const rmf_task::agv::Parameters parameters{
+    const rmf_task::Parameters parameters{
       *_pimpl->planner,
       *battery_system,
       motion_sink,
       ambient_sink,
       tool_sink};
-    const rmf_task::agv::Constraints constraints{
+    const rmf_task::Constraints constraints{
       recharge_threshold,
       recharge_soc,
       account_for_battery_drain};
-    const rmf_task::agv::TaskPlanner::Configuration task_config{
+    const rmf_task::TaskPlanner::Configuration task_config{
       parameters,
       constraints,
       _pimpl->cost_calculator};
-    const rmf_task::agv::TaskPlanner::Options options{
+    const rmf_task::TaskPlanner::Options options{
       false,
       nullptr,
       finishing_request};
-    _pimpl->task_planner = std::make_shared<rmf_task::agv::TaskPlanner>(
+    _pimpl->task_planner = std::make_shared<rmf_task::TaskPlanner>(
       std::move(task_config), std::move(options));
 
     // Here we update the task planner in all the RobotContexts.
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp
index bc6ed324a..53e0caf5e 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp
@@ -216,14 +216,14 @@ RobotContext& RobotContext::maximum_delay(
 }
 
 //==============================================================================
-const rmf_task::agv::State& RobotContext::current_task_end_state() const
+const rmf_task::State& RobotContext::current_task_end_state() const
 {
   return _current_task_end_state;
 }
 
 //==============================================================================
 RobotContext& RobotContext::current_task_end_state(
-  const rmf_task::agv::State& state)
+  const rmf_task::State& state)
 {
   _current_task_end_state = state;
   return *this;
@@ -250,7 +250,7 @@ const rxcpp::observable<double>& RobotContext::observe_battery_soc() const
 }
 
 //==============================================================================
-const std::shared_ptr<const rmf_task::agv::TaskPlanner>&
+const std::shared_ptr<const rmf_task::TaskPlanner>&
 RobotContext::task_planner() const
 {
   return _task_planner;
@@ -258,7 +258,7 @@ RobotContext::task_planner() const
 
 //==============================================================================
 auto RobotContext::task_planner(
-  const std::shared_ptr<const rmf_task::agv::TaskPlanner> task_planner)
+  const std::shared_ptr<const rmf_task::TaskPlanner> task_planner)
 -> RobotContext&
 {
   _task_planner = task_planner;
@@ -328,8 +328,8 @@ RobotContext::RobotContext(
   std::shared_ptr<rmf_fleet_adapter::agv::Node> node,
   const rxcpp::schedulers::worker& worker,
   rmf_utils::optional<rmf_traffic::Duration> maximum_delay,
-  rmf_task::agv::State state,
-  std::shared_ptr<const rmf_task::agv::TaskPlanner> task_planner)
+  rmf_task::State state,
+  std::shared_ptr<const rmf_task::TaskPlanner> task_planner)
 : _command_handle(std::move(command_handle)),
   _location(std::move(_initial_location)),
   _itinerary(std::move(itinerary)),
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp
index 6f2a003f2..c0ac0209d 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp
@@ -9,9 +9,9 @@
 #include <rmf_traffic/schedule/Participant.hpp>
 #include <rmf_traffic/schedule/Snapshot.hpp>
 
-#include <rmf_task/agv/State.hpp>
-#include <rmf_task/agv/Constraints.hpp>
-#include <rmf_task/agv/TaskPlanner.hpp>
+#include <rmf_task/State.hpp>
+#include <rmf_task/Constraints.hpp>
+#include <rmf_task/TaskPlanner.hpp>
 
 #include <rclcpp/node.hpp>
 
@@ -117,11 +117,11 @@ class RobotContext
     const ResponderPtr& responder) final;
 
   /// Set the state of this robot at the end of its current task
-  RobotContext& current_task_end_state(const rmf_task::agv::State& state);
+  RobotContext& current_task_end_state(const rmf_task::State& state);
 
   /// Get a mutable reference to the state of this robot at the end of its
   // current task
-  const rmf_task::agv::State& current_task_end_state() const;
+  const rmf_task::State& current_task_end_state() const;
 
   /// Get the current battery state of charge
   double current_battery_soc() const;
@@ -134,11 +134,11 @@ class RobotContext
   const rxcpp::observable<double>& observe_battery_soc() const;
 
   /// Get a mutable reference to the task planner for this robot
-  const std::shared_ptr<const rmf_task::agv::TaskPlanner>& task_planner() const;
+  const std::shared_ptr<const rmf_task::TaskPlanner>& task_planner() const;
 
   /// Set the task planner for this robot
   RobotContext& task_planner(
-    const std::shared_ptr<const rmf_task::agv::TaskPlanner> task_planner);
+    const std::shared_ptr<const rmf_task::TaskPlanner> task_planner);
 
   void set_lift_entry_watchdog(
     RobotUpdateHandle::Unstable::Watchdog watchdog,
@@ -168,8 +168,8 @@ class RobotContext
     std::shared_ptr<Node> node,
     const rxcpp::schedulers::worker& worker,
     rmf_utils::optional<rmf_traffic::Duration> maximum_delay,
-    rmf_task::agv::State state,
-    std::shared_ptr<const rmf_task::agv::TaskPlanner> task_planner);
+    rmf_task::State state,
+    std::shared_ptr<const rmf_task::TaskPlanner> task_planner);
 
   std::weak_ptr<RobotCommandHandle> _command_handle;
   std::vector<rmf_traffic::agv::Plan::Start> _location;
@@ -194,8 +194,8 @@ class RobotContext
   double _current_battery_soc = 1.0;
   rxcpp::subjects::subject<double> _battery_soc_publisher;
   rxcpp::observable<double> _battery_soc_obs;
-  rmf_task::agv::State _current_task_end_state;
-  std::shared_ptr<const rmf_task::agv::TaskPlanner> _task_planner;
+  rmf_task::State _current_task_end_state;
+  std::shared_ptr<const rmf_task::TaskPlanner> _task_planner;
 
   RobotUpdateHandle::Unstable::Watchdog _lift_watchdog;
   rmf_traffic::Duration _lift_rewait_duration = std::chrono::seconds(0);
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp
index 148c21d44..3b3caf5f0 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp
@@ -173,7 +173,7 @@ RobotUpdateHandle& RobotUpdateHandle::set_charger_waypoint(
   if (const auto context = _pimpl->get_context())
   {
     auto end_state = context->current_task_end_state();
-    end_state.charging_waypoint(charger_wp);
+    end_state.dedicated_charging_waypoint(charger_wp);
     context->current_task_end_state(end_state);
     RCLCPP_INFO(
       context->node()->get_logger(),
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
index b885bd10b..a437ea140 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
@@ -25,7 +25,7 @@
 #include <rmf_task_msgs/msg/dispatch_request.hpp>
 #include <rmf_task_msgs/msg/dispatch_ack.hpp>
 
-#include <rmf_task/agv/TaskPlanner.hpp>
+#include <rmf_task/TaskPlanner.hpp>
 #include <rmf_task/Request.hpp>
 #include <rmf_task/requests/Clean.hpp>
 #include <rmf_task/BinaryPriorityScheme.hpp>
@@ -143,7 +143,7 @@ class FleetUpdateHandle::Implementation
   // Task planner params
   std::shared_ptr<rmf_task::CostCalculator> cost_calculator =
     rmf_task::BinaryPriorityScheme::make_cost_calculator();
-  std::shared_ptr<rmf_task::agv::TaskPlanner> task_planner = nullptr;
+  std::shared_ptr<rmf_task::TaskPlanner> task_planner = nullptr;
 
   rmf_utils::optional<rmf_traffic::Duration> default_maximum_delay =
     std::chrono::nanoseconds(std::chrono::seconds(10));
@@ -159,7 +159,7 @@ class FleetUpdateHandle::Implementation
   rclcpp::TimerBase::SharedPtr fleet_state_timer = nullptr;
 
   // Map task id to pair of <RequestPtr, Assignments>
-  using Assignments = rmf_task::agv::TaskPlanner::Assignments;
+  using Assignments = rmf_task::TaskPlanner::Assignments;
 
   // Map of dock name to dock parameters
   std::unordered_map<std::string,
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.cpp
index 02980fe2b..55e987c1a 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.cpp
@@ -553,7 +553,12 @@ void GoToPlace::Active::execute_plan(rmf_traffic::agv::Plan new_plan)
   // TODO: Make distinctions between task and subtasks to avoid passing
   // dummy parameters for subtasks
   rmf_traffic::Time dummy_time;
-  rmf_task::agv::State dummy_state{{dummy_time, 0, 0.0}, 0, 1.0};
+  rmf_task::State dummy_state;
+  dummy_state.waypoint(0)
+  .orientation(0.0)
+  .time(dummy_time)
+  .dedicated_charging_waypoint(0)
+  .battery_soc(1.0);
   _subtasks = Task::make(
     _description,
     std::move(sub_phases),
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp
index dfd37a01a..90096e99d 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp
@@ -30,7 +30,7 @@ std::shared_ptr<Task> make_charge_battery(
   const agv::RobotContextPtr& context,
   const rmf_traffic::agv::Plan::Start start,
   const rmf_traffic::Time deployment_time,
-  const rmf_task::agv::State finish_state)
+  const rmf_task::State finish_state)
 {
   std::shared_ptr<const rmf_task::requests::ChargeBattery::Description>
   description = std::dynamic_pointer_cast<
@@ -39,7 +39,8 @@ std::shared_ptr<Task> make_charge_battery(
   if (description == nullptr)
     return nullptr;
 
-  rmf_traffic::agv::Planner::Goal goal{finish_state.charging_waypoint()};
+  rmf_traffic::agv::Planner::Goal goal{
+    finish_state.dedicated_charging_waypoint().value()};
 
   Task::PendingPhases phases;
   phases.push_back(
@@ -51,7 +52,7 @@ std::shared_ptr<Task> make_charge_battery(
       context->task_planner()->configuration().constraints().recharge_soc()));
 
   return Task::make(
-    request->id(),
+    request->booking()->id(),
     std::move(phases),
     context->worker(),
     deployment_time,
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp
index e938190d5..81a350a56 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp
@@ -24,7 +24,7 @@
 #include <rmf_traffic/agv/Planner.hpp>
 #include <rmf_traffic/Time.hpp>
 
-#include <rmf_task/agv/State.hpp>
+#include <rmf_task/State.hpp>
 #include <rmf_task/requests/ChargeBattery.hpp>
 
 namespace rmf_fleet_adapter {
@@ -36,7 +36,7 @@ std::shared_ptr<Task> make_charge_battery(
   const agv::RobotContextPtr& context,
   const rmf_traffic::agv::Plan::Start start,
   const rmf_traffic::Time deployment_time,
-  const rmf_task::agv::State finish_state);
+  const rmf_task::State finish_state);
 
 } // namespace tasks
 } // namespace rmf_fleet_adapter
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp
index 4a2980969..d08bcdcfe 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp
@@ -28,7 +28,7 @@ std::shared_ptr<Task> make_clean(
   const agv::RobotContextPtr& context,
   const rmf_traffic::agv::Plan::Start clean_start,
   const rmf_traffic::Time deployment_time,
-  const rmf_task::agv::State finish_state)
+  const rmf_task::State finish_state)
 {
   std::shared_ptr<const rmf_task::requests::Clean::Description> description =
     std::dynamic_pointer_cast<
@@ -51,7 +51,7 @@ std::shared_ptr<Task> make_clean(
   // performed. To avoid this, we request the robot to re-enter the lane.
   // This should be fixed when we define a new phase cleaning and not rely on
   // docking
-  if (context->current_task_end_state().location().waypoint() == start_waypoint)
+  if (context->current_task_end_state().waypoint().value() == start_waypoint)
   {
     const auto& graph = context->navigation_graph();
     const auto& lane_from_index = graph.lanes_from(start_waypoint)[0];
@@ -67,7 +67,7 @@ std::shared_ptr<Task> make_clean(
     phases::GoToPlace::make(context, std::move(clean_start), clean_goal));
 
   return Task::make(
-    request->id(),
+    request->booking()->id(),
     std::move(phases),
     context->worker(),
     deployment_time,
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp
index 1782c2405..1d0f3c319 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp
@@ -24,7 +24,7 @@
 #include <rmf_traffic/agv/Planner.hpp>
 #include <rmf_traffic/Time.hpp>
 
-#include <rmf_task/agv/State.hpp>
+#include <rmf_task/State.hpp>
 #include <rmf_task/requests/Clean.hpp>
 
 namespace rmf_fleet_adapter {
@@ -36,7 +36,7 @@ std::shared_ptr<Task> make_clean(
   const agv::RobotContextPtr& context,
   const rmf_traffic::agv::Plan::Start clean_start,
   const rmf_traffic::Time deployment_time,
-  const rmf_task::agv::State finish_state);
+  const rmf_task::State finish_state);
 
 } // namespace tasks
 } // namespace rmf_fleet_adapter
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp
index 0bf55d5d2..9fc7864b8 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp
@@ -32,7 +32,7 @@ std::shared_ptr<Task> make_delivery(
   const agv::RobotContextPtr& context,
   const rmf_traffic::agv::Plan::Start pickup_start,
   const rmf_traffic::Time deployment_time,
-  const rmf_task::agv::State finish_state,
+  const rmf_task::State finish_state,
   const rmf_task_msgs::msg::Delivery delivery_profile)
 {
 
@@ -56,7 +56,7 @@ std::shared_ptr<Task> make_delivery(
   phases.push_back(
     std::make_unique<phases::DispenseItem::PendingPhase>(
       context,
-      request->id(),
+      request->booking()->id(),
       delivery_profile.pickup_dispenser,
       context->itinerary().description().owner(),
       delivery_profile.items));
@@ -102,13 +102,13 @@ std::shared_ptr<Task> make_delivery(
   phases.push_back(
     std::make_unique<phases::IngestItem::PendingPhase>(
       context,
-      request->id(),
+      request->booking()->id(),
       delivery_profile.dropoff_ingestor,
       context->itinerary().description().owner(),
       ingestor_items));
 
   return Task::make(
-    request->id(),
+    request->booking()->id(),
     std::move(phases),
     context->worker(),
     deployment_time,
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp
index f7f718b40..f00a36389 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp
@@ -34,7 +34,7 @@ std::shared_ptr<Task> make_delivery(
   const agv::RobotContextPtr& context,
   const rmf_traffic::agv::Plan::Start pickup_start,
   const rmf_traffic::Time deployment_time,
-  const rmf_task::agv::State finish_state,
+  const rmf_task::State finish_state,
   const rmf_task_msgs::msg::Delivery delivery_profile);
 
 } // namespace tasks
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp
index cdd0b4b48..5a78555bd 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp
@@ -28,7 +28,7 @@ std::shared_ptr<Task> make_loop(
   const agv::RobotContextPtr& context,
   const rmf_traffic::agv::Plan::Start start,
   const rmf_traffic::Time deployment_time,
-  const rmf_task::agv::State finish_state)
+  const rmf_task::State finish_state)
 {
   std::shared_ptr<const rmf_task::requests::Loop::Description> description =
     std::dynamic_pointer_cast<
@@ -101,7 +101,7 @@ std::shared_ptr<Task> make_loop(
   }
 
   return Task::make(
-    request->id(),
+    request->booking()->id(),
     std::move(phases),
     context->worker(),
     deployment_time,
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.hpp
index 15dd7d306..3813100de 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.hpp
@@ -32,7 +32,7 @@ std::shared_ptr<Task> make_loop(
   const agv::RobotContextPtr& context,
   const rmf_traffic::agv::Plan::Start start,
   const rmf_traffic::Time deployment_time,
-  const rmf_task::agv::State finish_state);
+  const rmf_task::State finish_state);
 
 } // namespace tasks
 } // namespace rmf_fleet_adapter
diff --git a/rmf_fleet_adapter/test/test_Task.cpp b/rmf_fleet_adapter/test/test_Task.cpp
index 9fe23155e..8e6dfaa7e 100644
--- a/rmf_fleet_adapter/test/test_Task.cpp
+++ b/rmf_fleet_adapter/test/test_Task.cpp
@@ -177,14 +177,17 @@ class MockSubtaskPhase
   public:
 
     Active(PendingPhases phases)
-    : _subtasks(
+    {
+      rmf_task::State state;
+      state.load_basic(
+        {std::chrono::steady_clock::now(), 0, 0.0}, 0, 1.0);
+      _subtasks =
         rmf_fleet_adapter::Task::make(
           "subtasks", std::move(phases),
           rxcpp::schedulers::make_event_loop().create_worker(),
           std::chrono::steady_clock::now(),
-          {{std::chrono::steady_clock::now(), 0, 0.0}, 0, 1.0},
-          nullptr))
-    {
+          state,
+          nullptr);
       _desc = "subtasks";
       _status_obs = _status_publisher.get_observable();
     }
@@ -295,7 +298,8 @@ SCENARIO("Test simple task")
 
   // Dummy parameters
   rmf_traffic::Time deployment_time;
-  rmf_task::agv::State finish_state{{deployment_time, 0, 0.0}, 0, 1.0};
+  rmf_task::State finish_state;
+  finish_state.load_basic({deployment_time, 0, 0.0}, 0, 1.0);
 
   std::shared_ptr<rmf_fleet_adapter::Task> task =
     rmf_fleet_adapter::Task::make(
@@ -395,7 +399,8 @@ SCENARIO("Test nested task")
 
   // Dummy parameters
   rmf_traffic::Time deployment_time;
-  rmf_task::agv::State finish_state{{deployment_time, 0, 0.0}, 0, 1.0};
+  rmf_task::State finish_state;
+  finish_state.load_basic({deployment_time, 0, 0.0}, 0, 1.0);
 
   const auto task = rmf_fleet_adapter::Task::make(
     "test_NestedTask", std::move(phases),

From c89e534b387e51e54efb910ca29bf992061cbc02 Mon Sep 17 00:00:00 2001
From: Yadunund <yadunund@openrobotics.org>
Date: Thu, 9 Dec 2021 12:02:24 +0800
Subject: [PATCH 10/79] Schema validator loader uses internal dictionary

Signed-off-by: Yadunund <yadunund@openrobotics.org>
---
 rmf_fleet_adapter/CMakeLists.txt              |  2 +
 .../agv/FleetUpdateHandle.cpp                 | 56 ++++++++++---------
 .../agv/internal_FleetUpdateHandle.hpp        | 20 +++++++
 3 files changed, 51 insertions(+), 27 deletions(-)

diff --git a/rmf_fleet_adapter/CMakeLists.txt b/rmf_fleet_adapter/CMakeLists.txt
index 07d33d02a..7ebca2497 100644
--- a/rmf_fleet_adapter/CMakeLists.txt
+++ b/rmf_fleet_adapter/CMakeLists.txt
@@ -144,6 +144,7 @@ if (BUILD_TESTING)
       ${rmf_api_msgs_INCLUDE_DIRS}
       ${std_msgs_INCLUDE_DIRS}
       ${WEBSOCKETPP_INCLUDE_DIR}
+      ${nlohmann_json_schema_validator_INCLUDE_DIRS}
   )
   target_link_libraries(test_rmf_fleet_adapter
     PRIVATE
@@ -159,6 +160,7 @@ if (BUILD_TESTING)
       rmf_api_msgs::rmf_api_msgs
       ${std_msgs_LIBRARIES}
       ${websocketpp_LIBRARIES}
+      nlohmann_json_schema_validator
   )
 
   target_compile_definitions(test_rmf_fleet_adapter
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
index 3ae257a5c..fee9d1b46 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
@@ -38,11 +38,6 @@
 #include <rmf_task_msgs/msg/delivery.hpp>
 #include <rmf_task_msgs/msg/loop.hpp>
 
-#include <nlohmann/json.hpp>
-#include <nlohmann/json-schema.hpp>
-
-#include <rmf_api_msgs/schemas/fleet_state.hpp>
-
 #include <sstream>
 #include <unordered_map>
 #include <unordered_set>
@@ -897,28 +892,35 @@ void FleetUpdateHandle::Implementation::publish_fleet_state() const
       // TODO(YV): json["issues"]
       fleet_state_msg["robots"].push_back(json);
     }
-    // TODO(YV): Validating the schema requires a loader to be defined. This is
-    // commented out temporarily until the proper loader can be defined.
-    // const auto schema = rmf_api_msgs::schemas::fleet_state;
-    // nlohmann::json_uri json_uri = nlohmann::json_uri{schema["$id"]};
-    // const auto loader =
-    //   [](const nlohmann::json_uri& id, nlohmann::json& value)
-    //   {
-
-    //   };
-    // try
-    // {
-    //   nlohmann::json_schema::json_validator validator(schema, loader);
-    //   validator.validate(fleet_state_msg);
-    // }
-    // catch (const std::exception& e)
-    // {
-    //   RCLCPP_ERROR(
-    //     node->get_logger(),
-    //     "Unable to publish fleet state json message: %s",
-    //     e.what());
-    //   return;
-    // }
+    const auto fleet_schema = rmf_api_msgs::schemas::fleet_state;
+    const auto loader =
+      [n = node, s = schema_dictionary](const nlohmann::json_uri& id, nlohmann::json& value)
+      {
+        const auto it = s.find(id.url());
+        if (it == s.end())
+        {
+          RCLCPP_ERROR(
+          n->get_logger(),
+          "url: %s not found in schema dictionary", id.url().c_str());
+          return;
+        }
+
+        value = it->second;
+      };
+
+    try
+    {
+      nlohmann::json_schema::json_validator validator(fleet_schema, loader);
+      validator.validate(fleet_state_msg);
+    }
+    catch (const std::exception& e)
+    {
+      RCLCPP_ERROR(
+        node->get_logger(),
+        "Unable to publish fleet state json message: %s",
+        e.what());
+      return;
+    }
 
     broadcast_client->publish(fleet_state_msg);
   }
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
index a437ea140..1b94aa9d8 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
@@ -49,6 +49,13 @@
 #include <rmf_traffic_ros2/schedule/Negotiation.hpp>
 #include <rmf_traffic_ros2/Time.hpp>
 
+#include <nlohmann/json.hpp>
+#include <nlohmann/json-schema.hpp>
+#include <rmf_api_msgs/schemas/fleet_state.hpp>
+#include <rmf_api_msgs/schemas/robot_state.hpp>
+#include <rmf_api_msgs/schemas/location_2D.hpp>
+
+
 #include <iostream>
 #include <unordered_set>
 #include <optional>
@@ -153,6 +160,8 @@ class FleetUpdateHandle::Implementation
     std::shared_ptr<TaskManager>> task_managers = {};
 
   std::shared_ptr<BroadcastClient> broadcast_client = nullptr;
+  // Map uri to schema for validator loader function
+  std::unordered_map<std::string, nlohmann::json> schema_dictionary = {};
 
   rclcpp::Publisher<rmf_fleet_msgs::msg::FleetState>::SharedPtr
     fleet_state_pub = nullptr;
@@ -273,6 +282,17 @@ class FleetUpdateHandle::Implementation
         handle->_pimpl->charging_waypoints.insert(i);
     }
 
+    // Initialize schema dictionary
+    auto schema = rmf_api_msgs::schemas::fleet_state;
+    nlohmann::json_uri json_uri = nlohmann::json_uri{schema["$id"]};
+    handle->_pimpl->schema_dictionary.insert({json_uri.url(), schema});
+    schema = rmf_api_msgs::schemas::robot_state;
+    json_uri = nlohmann::json_uri{schema["$id"]};
+    handle->_pimpl->schema_dictionary.insert({json_uri.url(), schema});
+    schema = rmf_api_msgs::schemas::location_2D;
+    json_uri = nlohmann::json_uri{schema["$id"]};
+    handle->_pimpl->schema_dictionary.insert({json_uri.url(), schema});
+
     // Start the BroadcastClient
     if (handle->_pimpl->server_uri.has_value())
     {

From aaf008ede7f91344749556a60e1a79457024940c Mon Sep 17 00:00:00 2001
From: Yadunund <yadunund@openrobotics.org>
Date: Thu, 9 Dec 2021 12:44:22 +0800
Subject: [PATCH 11/79] Fixed fleet_state message

Signed-off-by: Yadunund <yadunund@openrobotics.org>
---
 .../src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp             | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
index fee9d1b46..5f96f7fe4 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
@@ -890,7 +890,7 @@ void FleetUpdateHandle::Implementation::publish_fleet_state() const
       json["location"] = location;
       json["battery"] = state.battery_percent / 100.0;
       // TODO(YV): json["issues"]
-      fleet_state_msg["robots"].push_back(json);
+      fleet_state_msg["robots"][state.name] = json;
     }
     const auto fleet_schema = rmf_api_msgs::schemas::fleet_state;
     const auto loader =

From 46fe559a4097f4e1f86307819ec33a32d49310cf Mon Sep 17 00:00:00 2001
From: Yadunund <yadunund@openrobotics.org>
Date: Thu, 9 Dec 2021 13:45:06 +0800
Subject: [PATCH 12/79] Update python binding

Signed-off-by: Yadunund <yadunund@openrobotics.org>
---
 rmf_fleet_adapter_python/src/adapter.cpp | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/rmf_fleet_adapter_python/src/adapter.cpp b/rmf_fleet_adapter_python/src/adapter.cpp
index 929fcbc72..ea35b1906 100644
--- a/rmf_fleet_adapter_python/src/adapter.cpp
+++ b/rmf_fleet_adapter_python/src/adapter.cpp
@@ -353,7 +353,8 @@ PYBIND11_MODULE(rmf_adapter, m) {
   .def("add_fleet", &agv::Adapter::add_fleet,
     py::arg("fleet_name"),
     py::arg("traits"),
-    py::arg("navigation_graph"))
+    py::arg("navigation_graph"),
+    py::arg("server_uri"))
   .def("add_easy_traffic_light", &agv::Adapter::add_easy_traffic_light,
     py::arg("handle_callback"),
     py::arg("fleet_name"),
@@ -383,7 +384,8 @@ PYBIND11_MODULE(rmf_adapter, m) {
   .def("add_fleet", &agv::test::MockAdapter::add_fleet,
     py::arg("fleet_name"),
     py::arg("traits"),
-    py::arg("navigation_graph"))
+    py::arg("navigation_graph"),
+    py::arg("server_uri"))
   .def_property_readonly("node",
     py::overload_cast<>(
       &agv::test::MockAdapter::node))

From 129f66b2ee01b07dac00d73833bd3f6337054387 Mon Sep 17 00:00:00 2001
From: Yadunund <yadunund@openrobotics.org>
Date: Thu, 9 Dec 2021 18:15:21 +0800
Subject: [PATCH 13/79] Publish _update jsons

Signed-off-by: Yadunund <yadunund@openrobotics.org>
---
 .../src/rmf_fleet_adapter/TaskManager.cpp     | 87 +++++++++++++++++++
 .../src/rmf_fleet_adapter/TaskManager.hpp     | 63 ++++++++++++++
 .../agv/FleetUpdateHandle.cpp                 | 11 ++-
 .../agv/internal_FleetUpdateHandle.hpp        |  6 +-
 4 files changed, 162 insertions(+), 5 deletions(-)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
index d796464df..1d5893eeb 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
@@ -33,6 +33,12 @@
 
 #include "phases/ResponsiveWait.hpp"
 
+#include <rmf_api_msgs/schemas/task_state_update.hpp>
+#include <rmf_api_msgs/schemas/task_state.hpp>
+#include <rmf_api_msgs/schemas/task_log_update.hpp>
+#include <rmf_api_msgs/schemas/task_log.hpp>
+#include <rmf_api_msgs/schemas/log_entry.hpp>
+
 namespace rmf_fleet_adapter {
 
 //==============================================================================
@@ -82,6 +88,24 @@ TaskManagerPtr TaskManager::make(
   mgr->_travel_estimator = std::make_shared<rmf_task::TravelEstimator>(
     mgr->_context->task_planner()->configuration().parameters());
 
+  mgr->_activator = std::make_shared<rmf_task::Activator>();
+
+  auto schema = rmf_api_msgs::schemas::task_state;
+  nlohmann::json_uri json_uri = nlohmann::json_uri{schema["$id"]};
+  mgr->_schema_dictionary.insert({json_uri.url(), schema});
+  schema = rmf_api_msgs::schemas::task_log;
+  json_uri = nlohmann::json_uri{schema["$id"]};
+  mgr->_schema_dictionary.insert({json_uri.url(), schema});
+  schema = rmf_api_msgs::schemas::log_entry;
+  json_uri = nlohmann::json_uri{schema["$id"]};
+  mgr->_schema_dictionary.insert({json_uri.url(), schema});
+  schema = rmf_api_msgs::schemas::task_state_update;
+  json_uri = nlohmann::json_uri{schema["$id"]};
+  mgr->_schema_dictionary.insert({json_uri.url(), schema});
+  schema = rmf_api_msgs::schemas::task_log_update;
+  json_uri = nlohmann::json_uri{schema["$id"]};
+  mgr->_schema_dictionary.insert({json_uri.url(), schema});
+
   return mgr;
 }
 
@@ -682,5 +706,68 @@ void TaskManager::_populate_task_summary(
   msg.state = task_summary_state;
 }
 
+//==============================================================================
+void TaskManager::_schema_loader(
+  const nlohmann::json_uri& id, nlohmann::json& value)
+{
+  const auto it = _schema_dictionary.find(id.url());
+  if (it == _schema_dictionary.end())
+  {
+    RCLCPP_ERROR(
+    _context->node()->get_logger(),
+    "[TaskManager] url: %s not found in schema dictionary", id.url().c_str());
+    return;
+  }
+
+  value = it->second;
+}
+
+//==============================================================================
+void TaskManager::_publish_validated_message(const nlohmann::json& msg) const
+{
+  const auto client = _broadcast_client.lock();
+  if (!client)
+  {
+    RCLCPP_ERROR(
+      _context->node()->get_logger(),
+      "Unable to lock BroadcastClient within TaskManager of robot [%s]",
+      _context->name().c_str());
+    return;
+  }
+  client->publish(msg);
+}
+
+
+//==============================================================================
+rmf_task::State TaskManager::_get_state() const
+{
+  return _context->current_task_end_state();
+}
+
+//==============================================================================
+void TaskManager::_update(rmf_task::Phase::ConstSnapshotPtr snapshot)
+{
+
+  // Update task state and log
+}
+
+//==============================================================================
+void TaskManager::_checkpoint(rmf_task::Task::Active::Backup backup)
+{
+
+}
+
+//==============================================================================
+void TaskManager::_phase_finished(
+  rmf_task::Phase::ConstCompletedPtr completed_phase)
+{
+
+}
+
+//==============================================================================
+void TaskManager::_task_finished()
+{
+
+}
 
 } // namespace rmf_fleet_adapter
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
index 28d9634f3..d71bff2a5 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
@@ -25,10 +25,14 @@
 #include <rmf_traffic/agv/Planner.hpp>
 
 #include <rmf_task/TaskPlanner.hpp>
+#include <rmf_task/Activator.hpp>
 
 #include <rmf_fleet_msgs/msg/robot_mode.hpp>
 #include <rmf_task_msgs/msg/task_summary.hpp>
 
+#include <nlohmann/json.hpp>
+#include <nlohmann/json-schema.hpp>
+
 #include <mutex>
 
 namespace rmf_fleet_adapter {
@@ -124,12 +128,71 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
   // retreat. TODO(YV): Expose the TaskPlanner's TravelEstimator.
   std::shared_ptr<rmf_task::TravelEstimator> _travel_estimator;
 
+  // The activator to generate rmf_task::Task::Active instances from requests
+  std::shared_ptr<rmf_task::Activator> _activator;
+
+  // Map schema url to schema for validator.
+  // TODO: Get this and loader from FleetUpdateHandle
+  std::unordered_map<std::string, nlohmann::json> _schema_dictionary = {};
+
+  // Constant jsons with validated schemas for internal use
+  // TODO(YV): Replace these with codegen tools
+  const nlohmann::json _task_log_update_msg =
+    {{"type", "task_log_update"}, {"data", {}}};
+  const nlohmann::json _task_log_json =
+    {{"task_id", {}}, {"log", {}}, {"phases", {{"log", {}}, {"events, {}"}}}};
+  const nlohmann::json _task_state_update_json =
+    {{"type", "task_state_update"}, {"data", {}}};
+  const nlohmann::json _task_state_json =
+    {{"booking", {}}, {"category", {}}, {"detail", {}},
+    {"unix_millis_start_time", {}}, {"unix_millis_finish_time", {}},
+    {"estimate_millis", {}}, {"phases", {}}, {"completed", {}}, {"active", {}},
+    {"pending", {}}, {"interruptions", {}}, {"cancellation", {}},
+    {"killed", {}}};
+
+  // The task_state.json for the active task. This should be initialized when
+  // a request is activated
+  nlohmann::json _active_task_state;
+
+  // Map task_id to task_log.json for all tasks managed by this TaskManager
+  std::unordered_map<std::string, nlohmann::json> _task_log = {};
+
   /// Callback for task timer which begins next task if its deployment time has passed
   void _begin_next_task();
 
   /// Begin responsively waiting for the next task
   void _begin_waiting();
 
+  /// Get the current state of the robot
+  rmf_task::State _get_state() const;
+
+  /// Schema loader for validating jsons
+  void _schema_loader(const nlohmann::json_uri& id, nlohmann::json& value);
+
+  /// Publish a json after it has been validated
+  void _publish_validated_message(const nlohmann::json& msg) const;
+
+  /// Callback for when the task has a significat update
+  void _update(rmf_task::Phase::ConstSnapshotPtr snapshot);
+
+  /// Callback for when the task reaches a checkpoint
+  void _checkpoint(rmf_task::Task::Active::Backup backup);
+
+  /// Callback for when a phase within a task has finished
+  void _phase_finished(rmf_task::Phase::ConstCompletedPtr completed_phase);
+
+  /// Callback for when the task has finished
+  void _task_finished();
+
+  // TODO: Assuming each Task::Active instance stores a weak_ptr to this
+  // TaskManager, the implementations of the corresponding functions in
+  // Task::Active can call these methods to publish state/log updates
+  // void _interrupt_active_task();
+  // void _cancel_active_task();
+  // void _kill_active_task();
+  // void _skip_phase_in_active_task(uint64_t phase_id, bool value = true);
+  // void _rewind_active_task(uint64_t phase_id);
+
   /// Function to register the task id of a task that has begun execution
   /// The input task id will be inserted into the registry such that the max
   /// size of the registry is 100.
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
index 5f96f7fe4..22b4a5492 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
@@ -870,7 +870,7 @@ void FleetUpdateHandle::Implementation::publish_fleet_state() const
   // Publish to API server
   if (broadcast_client)
   {
-    nlohmann::json fleet_state_msg = { {"name", {}}, {"robots", {}}};
+    nlohmann::json fleet_state_msg = {{"name", {}}, {"robots", {}}};
     fleet_state_msg["name"] = name;
     for (const auto& state : robot_states)
     {
@@ -892,7 +892,7 @@ void FleetUpdateHandle::Implementation::publish_fleet_state() const
       // TODO(YV): json["issues"]
       fleet_state_msg["robots"][state.name] = json;
     }
-    const auto fleet_schema = rmf_api_msgs::schemas::fleet_state;
+    const auto fleet_schema = rmf_api_msgs::schemas::fleet_state_update;
     const auto loader =
       [n = node, s = schema_dictionary](const nlohmann::json_uri& id, nlohmann::json& value)
       {
@@ -908,10 +908,13 @@ void FleetUpdateHandle::Implementation::publish_fleet_state() const
         value = it->second;
       };
 
+    nlohmann::json fleet_state_update_msg =
+      {{"type", "fleet_state_update"}, {"data", fleet_state_msg}};
     try
     {
+
       nlohmann::json_schema::json_validator validator(fleet_schema, loader);
-      validator.validate(fleet_state_msg);
+      validator.validate(fleet_state_update_msg);
     }
     catch (const std::exception& e)
     {
@@ -922,7 +925,7 @@ void FleetUpdateHandle::Implementation::publish_fleet_state() const
       return;
     }
 
-    broadcast_client->publish(fleet_state_msg);
+    broadcast_client->publish(fleet_state_update_msg);
   }
 
   auto fleet_state = rmf_fleet_msgs::build<rmf_fleet_msgs::msg::FleetState>()
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
index 1b94aa9d8..c95a27792 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
@@ -51,6 +51,7 @@
 
 #include <nlohmann/json.hpp>
 #include <nlohmann/json-schema.hpp>
+#include <rmf_api_msgs/schemas/fleet_state_update.hpp>
 #include <rmf_api_msgs/schemas/fleet_state.hpp>
 #include <rmf_api_msgs/schemas/robot_state.hpp>
 #include <rmf_api_msgs/schemas/location_2D.hpp>
@@ -283,9 +284,12 @@ class FleetUpdateHandle::Implementation
     }
 
     // Initialize schema dictionary
-    auto schema = rmf_api_msgs::schemas::fleet_state;
+    auto schema = rmf_api_msgs::schemas::fleet_state_update;
     nlohmann::json_uri json_uri = nlohmann::json_uri{schema["$id"]};
     handle->_pimpl->schema_dictionary.insert({json_uri.url(), schema});
+    schema = rmf_api_msgs::schemas::fleet_state;
+    json_uri = nlohmann::json_uri{schema["$id"]};
+    handle->_pimpl->schema_dictionary.insert({json_uri.url(), schema});
     schema = rmf_api_msgs::schemas::robot_state;
     json_uri = nlohmann::json_uri{schema["$id"]};
     handle->_pimpl->schema_dictionary.insert({json_uri.url(), schema});

From a274b49876202d67ef01eaa7b42ebe93f2f932f4 Mon Sep 17 00:00:00 2001
From: Yadunund <yadunund@openrobotics.org>
Date: Thu, 9 Dec 2021 19:49:19 +0800
Subject: [PATCH 14/79] Add helper functions to validate and publish task
 updates

Signed-off-by: Yadunund <yadunund@openrobotics.org>
---
 .../launch/fleet_adapter.launch.xml           |  1 +
 .../src/rmf_fleet_adapter/BroadcastClient.cpp | 12 ++-
 .../src/rmf_fleet_adapter/BroadcastClient.hpp |  5 +-
 .../src/rmf_fleet_adapter/TaskManager.cpp     | 96 ++++++++++++++++++-
 .../src/rmf_fleet_adapter/TaskManager.hpp     | 27 ++++--
 5 files changed, 130 insertions(+), 11 deletions(-)

diff --git a/rmf_fleet_adapter/launch/fleet_adapter.launch.xml b/rmf_fleet_adapter/launch/fleet_adapter.launch.xml
index 212b7410b..2e35889a1 100644
--- a/rmf_fleet_adapter/launch/fleet_adapter.launch.xml
+++ b/rmf_fleet_adapter/launch/fleet_adapter.launch.xml
@@ -39,6 +39,7 @@
   <arg name="recharge_threshold" default="0.2" description="The fraction of total battery capacity below which the robot must return to its charger"/>
   <arg name="recharge_soc" default="1.0" description="The fraction of total battery capacity to which the robot should be charged"/>
   <arg name="experimental_lift_watchdog_service" default="" description="(Experimental) The name of a service to check whether a robot can enter a lift"/>
+  <!-- The uri is set to the TrajectoryServer uri for debugging purposes. The default port needs to be updated before merging -->
   <arg name="server_uri" default="ws://127.0.0.1:8006" description="The URI of the api server to trasnmit state and task infromation. If empty, information will not be transmitted"/>
 
 
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.cpp
index d7a7aa84f..08d214183 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.cpp
@@ -52,7 +52,8 @@ std::shared_ptr<BroadcastClient> BroadcastClient::make(
       const auto impl = agv::FleetUpdateHandle::Implementation::get(*fleet);
       for (const auto& [conext, mgr] : impl.task_managers)
       {
-        // TODO(YV): Publish latest state and log
+        // Publish all task logs to the server
+        c->publish(mgr->task_log_updates());
       }
       RCLCPP_INFO(
         impl.node->get_logger(),
@@ -151,6 +152,15 @@ void BroadcastClient::publish(const nlohmann::json& msg)
   _cv.notify_all();
 }
 
+//==============================================================================
+void BroadcastClient::publish(const std::vector<nlohmann::json>& msgs)
+{
+  std::lock_guard<std::mutex> lock(_queue_mutex);
+  for (const auto& msg : msgs)
+    _queue.push(msg);
+  _cv.notify_all();
+}
+
 //==============================================================================
 BroadcastClient::BroadcastClient()
 {
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.hpp
index 3e2f8ca57..3ef1beecd 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.hpp
@@ -52,9 +52,12 @@ class BroadcastClient : public std::enable_shared_from_this<BroadcastClient>
     const std::string& uri,
     std::weak_ptr<agv::FleetUpdateHandle> fleet_handle);
 
-  // Publish message
+  // Publish a single message
   void publish(const nlohmann::json& msg);
 
+  // Publish a vector of messages
+  void publish(const std::vector<nlohmann::json>& msgs);
+
   ~BroadcastClient();
 
 private:
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
index 1d5893eeb..7587baa00 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
@@ -387,6 +387,30 @@ TaskManager::RobotModeMsg TaskManager::robot_mode() const
   return mode;
 }
 
+//==============================================================================
+std::vector<nlohmann::json> TaskManager::task_log_updates() const
+{
+  std::vector<nlohmann::json> logs;
+  for (const auto& it : _task_logs)
+  {
+    nlohmann::json update_msg = _task_log_update_msg;
+    update_msg["data"] = it.second;
+    std::string error = "";
+    if (_validate_json(
+      update_msg, rmf_api_msgs::schemas::task_log_update, error))
+    {
+      logs.push_back(update_msg);
+    }
+    else
+    {
+      RCLCPP_ERROR(
+        _context->node()->get_logger(),
+        "%s", error.c_str());
+    }
+  }
+  return logs;
+}
+
 //==============================================================================
 void TaskManager::_begin_next_task()
 {
@@ -708,7 +732,7 @@ void TaskManager::_populate_task_summary(
 
 //==============================================================================
 void TaskManager::_schema_loader(
-  const nlohmann::json_uri& id, nlohmann::json& value)
+  const nlohmann::json_uri& id, nlohmann::json& value) const
 {
   const auto it = _schema_dictionary.find(id.url());
   if (it == _schema_dictionary.end())
@@ -723,8 +747,20 @@ void TaskManager::_schema_loader(
 }
 
 //==============================================================================
-void TaskManager::_publish_validated_message(const nlohmann::json& msg) const
+void TaskManager::_validate_and_publish_json(
+  const nlohmann::json& msg,
+  const nlohmann::json& schema) const
 {
+  std::string error = "";
+  if (!_validate_json(msg, schema, error))
+  {
+    RCLCPP_ERROR(
+      _context->node()->get_logger(),
+      "[%s]",
+      error.c_str());
+    return;
+  }
+
   const auto client = _broadcast_client.lock();
   if (!client)
   {
@@ -744,30 +780,84 @@ rmf_task::State TaskManager::_get_state() const
   return _context->current_task_end_state();
 }
 
+//==============================================================================
+bool TaskManager::_validate_json(
+  const nlohmann::json& json,
+  const nlohmann::json& schema,
+  std::string& error) const
+{
+  try
+  {
+    nlohmann::json_schema::json_validator validator(
+      schema,
+      [w = weak_from_this()](const nlohmann::json_uri& id, nlohmann::json& value)
+      {
+        const auto self = w.lock();
+        if (!self)
+          return;
+        self->_schema_loader(id, value);
+      });
+    validator.validate(json);
+  }
+  catch(const std::exception& e)
+  {
+    error = e.what();
+    return false;
+  }
+
+  return true;
+}
+
 //==============================================================================
 void TaskManager::_update(rmf_task::Phase::ConstSnapshotPtr snapshot)
 {
+  // TODO
 
-  // Update task state and log
+  auto task_state_update = _task_state_update_json;
+  task_state_update["data"] = _active_task_state;
+  _validate_and_publish_json(
+    task_state_update, rmf_api_msgs::schemas::task_state_update);
 }
 
 //==============================================================================
 void TaskManager::_checkpoint(rmf_task::Task::Active::Backup backup)
 {
+  // TODO
 
+  auto task_state_update = _task_state_update_json;
+  task_state_update["data"] = _active_task_state;
+  _validate_and_publish_json(
+    task_state_update, rmf_api_msgs::schemas::task_state_update);
 }
 
 //==============================================================================
 void TaskManager::_phase_finished(
   rmf_task::Phase::ConstCompletedPtr completed_phase)
 {
+  // TODO
 
+  auto task_state_update = _task_state_update_json;
+  task_state_update["data"] = _active_task_state;
+  _validate_and_publish_json(
+    task_state_update, rmf_api_msgs::schemas::task_state_update);
 }
 
 //==============================================================================
 void TaskManager::_task_finished()
 {
+  // TODO
+
+  auto task_state_update = _task_state_update_json;
+  task_state_update["data"] = _active_task_state;
+  _validate_and_publish_json(
+    task_state_update, rmf_api_msgs::schemas::task_state_update);
+
+  auto task_log_update = _task_log_update_msg;
+  task_log_update["data"] = _task_logs[_active_task_state["booking"]["id"]];
+  _validate_and_publish_json(
+    task_log_update, rmf_api_msgs::schemas::task_log_update);
 
+  _active_task_state = {};
 }
 
 } // namespace rmf_fleet_adapter
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
index d71bff2a5..7318cd02c 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
@@ -94,6 +94,9 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
 
   RobotModeMsg robot_mode() const;
 
+  /// Get a vector of task logs that are validaed against the schema
+  std::vector<nlohmann::json> task_log_updates() const;
+
 private:
 
   TaskManager(
@@ -151,11 +154,12 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
     {"killed", {}}};
 
   // The task_state.json for the active task. This should be initialized when
-  // a request is activated
+  // a request is activated.
+  // TODO: Should this be a shared_ptr to pass down to Task::Active?
   nlohmann::json _active_task_state;
 
   // Map task_id to task_log.json for all tasks managed by this TaskManager
-  std::unordered_map<std::string, nlohmann::json> _task_log = {};
+  std::unordered_map<std::string, nlohmann::json> _task_logs = {};
 
   /// Callback for task timer which begins next task if its deployment time has passed
   void _begin_next_task();
@@ -167,10 +171,21 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
   rmf_task::State _get_state() const;
 
   /// Schema loader for validating jsons
-  void _schema_loader(const nlohmann::json_uri& id, nlohmann::json& value);
-
-  /// Publish a json after it has been validated
-  void _publish_validated_message(const nlohmann::json& msg) const;
+  void _schema_loader(
+    const nlohmann::json_uri& id, nlohmann::json& value) const;
+
+  /// Returns true if json is valid.
+  // TODO: Move this into a utils?
+  bool _validate_json(
+    const nlohmann::json& json,
+    const nlohmann::json& schema,
+    std::string& error) const;
+
+  /// Validate and publish a json. This can be used for task
+  /// state and log updates
+  void _validate_and_publish_json(
+    const nlohmann::json& msg,
+    const nlohmann::json& schema) const;
 
   /// Callback for when the task has a significat update
   void _update(rmf_task::Phase::ConstSnapshotPtr snapshot);

From 5becba19e22baa72139982a5acb36b2781014e60 Mon Sep 17 00:00:00 2001
From: Yadunund <yadunund@openrobotics.org>
Date: Thu, 9 Dec 2021 19:51:57 +0800
Subject: [PATCH 15/79] Uncrustify

Signed-off-by: Yadunund <yadunund@openrobotics.org>
---
 .../src/rmf_fleet_adapter/BroadcastClient.cpp | 19 ++++++++++---------
 .../src/rmf_fleet_adapter/TaskManager.cpp     | 13 +++++++------
 .../src/rmf_fleet_adapter/TaskManager.hpp     |  8 ++++----
 .../agv/FleetUpdateHandle.cpp                 | 18 +++++++++++-------
 4 files changed, 32 insertions(+), 26 deletions(-)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.cpp
index 08d214183..38e4e2b84 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.cpp
@@ -68,10 +68,10 @@ std::shared_ptr<BroadcastClient> BroadcastClient::make(
     });
 
   client->_client.set_fail_handler(
-  [c = client](websocketpp::connection_hdl)
-  {
-    c->_connected = false;
-  });
+    [c = client](websocketpp::connection_hdl)
+    {
+      c->_connected = false;
+    });
 
   client->_processing_thread = std::thread(
     [c = client]()
@@ -95,7 +95,8 @@ std::shared_ptr<BroadcastClient> BroadcastClient::make(
           c->_client.connect(con);
           // TOD(YV): Without sending a test payload, ec seems to be 0 even
           // when the client has not connected. Avoid sending this message.
-          c->_client.send(c->_hdl, "Hello", websocketpp::frame::opcode::text, ec);
+          c->_client.send(c->_hdl, "Hello", websocketpp::frame::opcode::text,
+          ec);
           if (ec)
           {
             RCLCPP_WARN(
@@ -116,10 +117,10 @@ std::shared_ptr<BroadcastClient> BroadcastClient::make(
 
         std::unique_lock<std::mutex> lock(c->_wait_mutex);
         c->_cv.wait(lock,
-          [c]()
-          {
-            return !c->_queue.empty();
-          });
+        [c]()
+        {
+          return !c->_queue.empty();
+        });
 
         while (!c->_queue.empty())
         {
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
index 7587baa00..c5880a602 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
@@ -47,7 +47,7 @@ TaskManagerPtr TaskManager::make(
   std::weak_ptr<BroadcastClient> broadcast_client)
 {
   auto mgr = TaskManagerPtr(new TaskManager(
-    std::move(context), std::move(broadcast_client)));
+        std::move(context), std::move(broadcast_client)));
   mgr->_emergency_sub = mgr->_context->node()->emergency_notice()
     .observe_on(rxcpp::identity_same_worker(mgr->_context->worker()))
     .subscribe(
@@ -397,7 +397,7 @@ std::vector<nlohmann::json> TaskManager::task_log_updates() const
     update_msg["data"] = it.second;
     std::string error = "";
     if (_validate_json(
-      update_msg, rmf_api_msgs::schemas::task_log_update, error))
+        update_msg, rmf_api_msgs::schemas::task_log_update, error))
     {
       logs.push_back(update_msg);
     }
@@ -738,8 +738,8 @@ void TaskManager::_schema_loader(
   if (it == _schema_dictionary.end())
   {
     RCLCPP_ERROR(
-    _context->node()->get_logger(),
-    "[TaskManager] url: %s not found in schema dictionary", id.url().c_str());
+      _context->node()->get_logger(),
+      "[TaskManager] url: %s not found in schema dictionary", id.url().c_str());
     return;
   }
 
@@ -790,7 +790,8 @@ bool TaskManager::_validate_json(
   {
     nlohmann::json_schema::json_validator validator(
       schema,
-      [w = weak_from_this()](const nlohmann::json_uri& id, nlohmann::json& value)
+      [w = weak_from_this()](const nlohmann::json_uri& id,
+      nlohmann::json& value)
       {
         const auto self = w.lock();
         if (!self)
@@ -799,7 +800,7 @@ bool TaskManager::_validate_json(
       });
     validator.validate(json);
   }
-  catch(const std::exception& e)
+  catch (const std::exception& e)
   {
     error = e.what();
     return false;
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
index 7318cd02c..34750122f 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
@@ -141,13 +141,13 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
   // Constant jsons with validated schemas for internal use
   // TODO(YV): Replace these with codegen tools
   const nlohmann::json _task_log_update_msg =
-    {{"type", "task_log_update"}, {"data", {}}};
+  {{"type", "task_log_update"}, {"data", {}}};
   const nlohmann::json _task_log_json =
-    {{"task_id", {}}, {"log", {}}, {"phases", {{"log", {}}, {"events, {}"}}}};
+  {{"task_id", {}}, {"log", {}}, {"phases", {{"log", {}}, {"events, {}"}}}};
   const nlohmann::json _task_state_update_json =
-    {{"type", "task_state_update"}, {"data", {}}};
+  {{"type", "task_state_update"}, {"data", {}}};
   const nlohmann::json _task_state_json =
-    {{"booking", {}}, {"category", {}}, {"detail", {}},
+  {{"booking", {}}, {"category", {}}, {"detail", {}},
     {"unix_millis_start_time", {}}, {"unix_millis_finish_time", {}},
     {"estimate_millis", {}}, {"phases", {}}, {"completed", {}}, {"active", {}},
     {"pending", {}}, {"interruptions", {}}, {"cancellation", {}},
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
index 22b4a5492..9c74f218d 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
@@ -467,12 +467,14 @@ void FleetUpdateHandle::Implementation::bid_notice_cb(
     {
       const auto& s = a.finish_state();
       const double request_seconds =
-        a.request()->booking()->earliest_start_time().time_since_epoch().count()/1e9;
+        a.request()->booking()->earliest_start_time().time_since_epoch().count()
+        /1e9;
       const double start_seconds =
         a.deployment_time().time_since_epoch().count()/1e9;
       const rmf_traffic::Time finish_time = s.time().value();
       const double finish_seconds = finish_time.time_since_epoch().count()/1e9;
-      debug_stream << "    <" << a.request()->booking()->id() << ": " << request_seconds
+      debug_stream << "    <" << a.request()->booking()->id() << ": " <<
+        request_seconds
                    << ", " << start_seconds
                    << ", "<< finish_seconds << ", " << s.battery_soc().value()
                    << "%>" << std::endl;
@@ -743,7 +745,8 @@ auto FleetUpdateHandle::Implementation::is_valid_assignments(
   {
     for (const auto& a : agent)
     {
-      if (executed_tasks.find(a.request()->booking()->id()) != executed_tasks.end())
+      if (executed_tasks.find(a.request()->booking()->id()) !=
+        executed_tasks.end())
         return false;
     }
   }
@@ -894,14 +897,15 @@ void FleetUpdateHandle::Implementation::publish_fleet_state() const
     }
     const auto fleet_schema = rmf_api_msgs::schemas::fleet_state_update;
     const auto loader =
-      [n = node, s = schema_dictionary](const nlohmann::json_uri& id, nlohmann::json& value)
+      [n = node, s = schema_dictionary](const nlohmann::json_uri& id,
+        nlohmann::json& value)
       {
         const auto it = s.find(id.url());
         if (it == s.end())
         {
           RCLCPP_ERROR(
-          n->get_logger(),
-          "url: %s not found in schema dictionary", id.url().c_str());
+            n->get_logger(),
+            "url: %s not found in schema dictionary", id.url().c_str());
           return;
         }
 
@@ -909,7 +913,7 @@ void FleetUpdateHandle::Implementation::publish_fleet_state() const
       };
 
     nlohmann::json fleet_state_update_msg =
-      {{"type", "fleet_state_update"}, {"data", fleet_state_msg}};
+    {{"type", "fleet_state_update"}, {"data", fleet_state_msg}};
     try
     {
 

From 03de72a09286ca5be12fd8456ab627059d2855e7 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Fri, 10 Dec 2021 18:28:27 +0800
Subject: [PATCH 16/79] Creating a shim to migrate the legacy task phases to
 the new system

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 rmf_fleet_adapter/CMakeLists.txt              |   2 +
 rmf_fleet_adapter/package.xml                 |   1 +
 .../{Task.cpp => LegacyTask.cpp}              |  42 ++---
 .../{Task.hpp => LegacyTask.hpp}              |  24 +--
 .../src/rmf_fleet_adapter/TaskManager.cpp     |   8 +-
 .../src/rmf_fleet_adapter/TaskManager.hpp     |  18 +-
 .../agv/FleetUpdateHandle.cpp                 |   2 +-
 .../agv/internal_FleetUpdateHandle.hpp        |   2 +-
 .../events/LegacyPhaseShim.cpp                | 175 ++++++++++++++++++
 .../events/LegacyPhaseShim.hpp                | 104 +++++++++++
 .../rmf_fleet_adapter/phases/DispenseItem.cpp |  20 +-
 .../rmf_fleet_adapter/phases/DispenseItem.hpp |  14 +-
 .../rmf_fleet_adapter/phases/DockRobot.cpp    |   6 +-
 .../rmf_fleet_adapter/phases/DockRobot.hpp    |  20 +-
 .../rmf_fleet_adapter/phases/DoorClose.cpp    |  12 +-
 .../rmf_fleet_adapter/phases/DoorClose.hpp    |  14 +-
 .../src/rmf_fleet_adapter/phases/DoorOpen.cpp |  14 +-
 .../src/rmf_fleet_adapter/phases/DoorOpen.hpp |  14 +-
 .../phases/EndLiftSession.cpp                 |  14 +-
 .../phases/EndLiftSession.hpp                 |  12 +-
 .../rmf_fleet_adapter/phases/GoToPlace.cpp    |  10 +-
 .../rmf_fleet_adapter/phases/GoToPlace.hpp    |  14 +-
 .../rmf_fleet_adapter/phases/IngestItem.cpp   |  20 +-
 .../rmf_fleet_adapter/phases/IngestItem.hpp   |  14 +-
 .../rmf_fleet_adapter/phases/MoveRobot.cpp    |   8 +-
 .../rmf_fleet_adapter/phases/MoveRobot.hpp    |  20 +-
 .../rmf_fleet_adapter/phases/RequestLift.cpp  |  24 +--
 .../rmf_fleet_adapter/phases/RequestLift.hpp  |  14 +-
 .../phases/ResponsiveWait.cpp                 |   2 +-
 .../phases/ResponsiveWait.hpp                 |  10 +-
 .../rmf_fleet_adapter/phases/RxOperators.hpp  |  18 +-
 .../phases/WaitForCharge.cpp                  |   2 +-
 .../phases/WaitForCharge.hpp                  |  12 +-
 .../rmf_fleet_adapter/tasks/ChargeBattery.cpp |   6 +-
 .../rmf_fleet_adapter/tasks/ChargeBattery.hpp |   4 +-
 .../src/rmf_fleet_adapter/tasks/Clean.cpp     |   6 +-
 .../src/rmf_fleet_adapter/tasks/Clean.hpp     |   4 +-
 .../src/rmf_fleet_adapter/tasks/Delivery.cpp  |   6 +-
 .../src/rmf_fleet_adapter/tasks/Delivery.hpp  |   4 +-
 .../src/rmf_fleet_adapter/tasks/Loop.cpp      |   6 +-
 .../src/rmf_fleet_adapter/tasks/Loop.hpp      |   4 +-
 .../test/phases/test_DispenseItem.cpp         |  12 +-
 .../test/phases/test_DoorClose.cpp            |  10 +-
 .../test/phases/test_DoorOpen.cpp             |   8 +-
 .../test/phases/test_GoToPlace.cpp            |   4 +-
 .../test/phases/test_IngestItem.cpp           |  12 +-
 .../test/phases/test_RequestLift.cpp          |   4 +-
 .../test/tasks/test_Delivery.cpp              |   2 +-
 rmf_fleet_adapter/test/tasks/test_Loop.cpp    |   4 +-
 rmf_fleet_adapter/test/test_Task.cpp          |  34 ++--
 50 files changed, 549 insertions(+), 267 deletions(-)
 rename rmf_fleet_adapter/src/rmf_fleet_adapter/{Task.cpp => LegacyTask.cpp} (84%)
 rename rmf_fleet_adapter/src/rmf_fleet_adapter/{Task.hpp => LegacyTask.hpp} (87%)
 create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.cpp
 create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.hpp

diff --git a/rmf_fleet_adapter/CMakeLists.txt b/rmf_fleet_adapter/CMakeLists.txt
index 7ebca2497..3ee53d4f3 100644
--- a/rmf_fleet_adapter/CMakeLists.txt
+++ b/rmf_fleet_adapter/CMakeLists.txt
@@ -35,6 +35,7 @@ set(dep_pkgs
   rmf_traffic_ros2
   rmf_battery
   rmf_task
+  rmf_task_sequence
   std_msgs
   rmf_api_msgs
   websocketpp
@@ -74,6 +75,7 @@ target_link_libraries(rmf_fleet_adapter
     rmf_traffic_ros2::rmf_traffic_ros2
     rmf_battery::rmf_battery
     rmf_task::rmf_task
+    rmf_task_sequence::rmf_task_sequence
     yaml-cpp
     ${rmf_fleet_msgs_LIBRARIES}
     ${rclcpp_LIBRARIES}
diff --git a/rmf_fleet_adapter/package.xml b/rmf_fleet_adapter/package.xml
index 575575c8a..2870fbfcd 100644
--- a/rmf_fleet_adapter/package.xml
+++ b/rmf_fleet_adapter/package.xml
@@ -24,6 +24,7 @@
   <depend>rmf_traffic_ros2</depend>
   <depend>rmf_battery</depend>
   <depend>rmf_task</depend>
+  <depend>rmf_task_sequence</depend>
   <depend>std_msgs</depend>
   <depend>rmf_api_msgs</depend>
   <depend condition="$RMF_ENABLE_FAILOVER == 1">stubborn_buddies</depend>
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/LegacyTask.cpp
similarity index 84%
rename from rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp
rename to rmf_fleet_adapter/src/rmf_fleet_adapter/LegacyTask.cpp
index b3c561858..bdbb1847c 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/LegacyTask.cpp
@@ -15,7 +15,7 @@
  *
 */
 
-#include "Task.hpp"
+#include "LegacyTask.hpp"
 
 #include <rclcpp/time.hpp>
 #include <rclcpp/duration.hpp>
@@ -27,7 +27,7 @@
 namespace rmf_fleet_adapter {
 
 //==============================================================================
-std::shared_ptr<Task> Task::make(
+std::shared_ptr<LegacyTask> LegacyTask::make(
   std::string id,
   PendingPhases phases,
   rxcpp::schedulers::worker worker,
@@ -35,8 +35,8 @@ std::shared_ptr<Task> Task::make(
   rmf_task::State finish_state,
   rmf_task::ConstRequestPtr request)
 {
-  return std::make_shared<Task>(
-    Task(std::move(id),
+  return std::make_shared<LegacyTask>(
+    LegacyTask(std::move(id),
     std::move(phases),
     std::move(worker),
     deployment_time,
@@ -45,38 +45,38 @@ std::shared_ptr<Task> Task::make(
 }
 
 //==============================================================================
-void Task::begin()
+void LegacyTask::begin()
 {
   if (!_active_phase)
     _start_next_phase();
 }
 
 //==============================================================================
-auto Task::observe() const -> const rxcpp::observable<StatusMsg>&
+auto LegacyTask::observe() const -> const rxcpp::observable<StatusMsg>&
 {
   return _status_obs;
 }
 
 //==============================================================================
-auto Task::current_phase() -> const std::shared_ptr<ActivePhase>&
+auto LegacyTask::current_phase() -> const std::shared_ptr<ActivePhase>&
 {
   return _active_phase;
 }
 
 //==============================================================================
-auto Task::current_phase() const -> std::shared_ptr<const ActivePhase>
+auto LegacyTask::current_phase() const -> std::shared_ptr<const ActivePhase>
 {
   return _active_phase;
 }
 
 //==============================================================================
-auto Task::pending_phases() const -> const PendingPhases&
+auto LegacyTask::pending_phases() const -> const PendingPhases&
 {
   return _pending_phases;
 }
 
 //==============================================================================
-void Task::cancel()
+void LegacyTask::cancel()
 {
   _pending_phases.clear();
   if (_active_phase)
@@ -84,44 +84,44 @@ void Task::cancel()
 }
 
 //==============================================================================
-const std::string& Task::id() const
+const std::string& LegacyTask::id() const
 {
   return _id;
 }
 
 //==============================================================================
-const rmf_task::ConstRequestPtr Task::request() const
+const rmf_task::ConstRequestPtr LegacyTask::request() const
 {
   return _request;
 }
 
 //==============================================================================
-const rmf_traffic::Time Task::deployment_time() const
+const rmf_traffic::Time LegacyTask::deployment_time() const
 {
   return _deployment_time;
 }
 
 //==============================================================================
-const rmf_task::State Task::finish_state() const
+const rmf_task::State LegacyTask::finish_state() const
 {
   return _finish_state;
 }
 
 //==============================================================================
-void Task::task_profile(Task::TaskProfileMsg profile)
+void LegacyTask::task_profile(LegacyTask::TaskProfileMsg profile)
 {
   _profile = profile;
   return;
 }
 
 //==============================================================================
-const Task::TaskProfileMsg& Task::task_profile() const
+const LegacyTask::TaskProfileMsg& LegacyTask::task_profile() const
 {
   return _profile;
 }
 
 //==============================================================================
-Task::Task(
+LegacyTask::LegacyTask(
   std::string id,
   std::vector<std::unique_ptr<PendingPhase>> phases,
   rxcpp::schedulers::worker worker,
@@ -140,7 +140,7 @@ Task::Task(
 }
 
 //==============================================================================
-void Task::_start_next_phase()
+void LegacyTask::_start_next_phase()
 {
   if (_pending_phases.empty())
   {
@@ -170,7 +170,7 @@ void Task::_start_next_phase()
   {
     // *INDENT-OFF*
     throw std::runtime_error(
-      "[Task::_start_next_phase] INTERNAL ERROR: Next phase has a null value");
+      "[LegacyTask::_start_next_phase] INTERNAL ERROR: Next phase has a null value");
     // *INDENT-ON*
   }
   _active_phase = next_pending->begin();
@@ -189,7 +189,7 @@ void Task::_start_next_phase()
 
       auto summary = msg;
       // We have received a status update from the phase. We will forward
-      // this to whoever is subscribing to the Task.
+      // this to whoever is subscribing to the LegacyTask.
       summary.task_id = task->_id;
 
       // We don't want to say that the task is complete until the very end.
@@ -239,7 +239,7 @@ void Task::_start_next_phase()
 }
 
 //==============================================================================
-auto Task::_process_summary(const StatusMsg& input_msg) -> StatusMsg
+auto LegacyTask::_process_summary(const StatusMsg& input_msg) -> StatusMsg
 {
   auto output = input_msg;
   if (!_initial_time.has_value())
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/LegacyTask.hpp
similarity index 87%
rename from rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp
rename to rmf_fleet_adapter/src/rmf_fleet_adapter/LegacyTask.hpp
index 0939950b1..c580924da 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/LegacyTask.hpp
@@ -15,8 +15,8 @@
  *
 */
 
-#ifndef SRC__RMF_FLEET_ADAPTER__TASK_HPP
-#define SRC__RMF_FLEET_ADAPTER__TASK_HPP
+#ifndef SRC__RMF_FLEET_ADAPTER__LEGACYTASK_HPP
+#define SRC__RMF_FLEET_ADAPTER__LEGACYTASK_HPP
 
 #include <string>
 #include <memory>
@@ -35,14 +35,14 @@
 namespace rmf_fleet_adapter {
 
 //==============================================================================
-class Task : public std::enable_shared_from_this<Task>
+class LegacyTask : public std::enable_shared_from_this<LegacyTask>
 {
 public:
 
   using StatusMsg = rmf_task_msgs::msg::TaskSummary;
 
-  /// This class represents the active phase of a Task. It provides an
-  /// observable that the Task can track to stay up-to-date on the status and to
+  /// This class represents the active phase of a LegacyTask. It provides an
+  /// observable that the LegacyTask can track to stay up-to-date on the status and to
   /// know when to begin the next phase.
   ///
   /// The ActivePhase class must be a schedule Negotiator so that it can
@@ -52,7 +52,7 @@ class Task : public std::enable_shared_from_this<Task>
   {
   public:
 
-    using StatusMsg = Task::StatusMsg;
+    using StatusMsg = LegacyTask::StatusMsg;
 
     /// Get a reference to an observable for the status of this ActivePhase.
     /// When this phase is complete, it will trigger on_completed()
@@ -98,7 +98,7 @@ class Task : public std::enable_shared_from_this<Task>
   using TaskProfileMsg = rmf_task_msgs::msg::TaskProfile;
 
   // Make a new task
-  static std::shared_ptr<Task> make(
+  static std::shared_ptr<LegacyTask> make(
     std::string id,
     PendingPhases phases,
     rxcpp::schedulers::worker worker,
@@ -108,7 +108,7 @@ class Task : public std::enable_shared_from_this<Task>
 
   void begin();
 
-  /// Get a reference to an observable for the status of this Task
+  /// Get a reference to an observable for the status of this LegacyTask
   const rxcpp::observable<StatusMsg>& observe() const;
 
   /// Get the current phase of the task
@@ -128,10 +128,10 @@ class Task : public std::enable_shared_from_this<Task>
   /// Get the request used to generate this task
   const rmf_task::ConstRequestPtr request() const;
 
-  /// Get the deployment time of this Task
+  /// Get the deployment time of this LegacyTask
   const rmf_traffic::Time deployment_time() const;
 
-  /// Get the finish state of this Task
+  /// Get the finish state of this LegacyTask
   const rmf_task::State finish_state() const;
 
   /// Set the TaskProfile of this task
@@ -142,7 +142,7 @@ class Task : public std::enable_shared_from_this<Task>
 
 private:
 
-  Task(
+  LegacyTask(
     std::string id,
     PendingPhases phases,
     rxcpp::schedulers::worker worker,
@@ -179,4 +179,4 @@ class Task : public std::enable_shared_from_this<Task>
 
 }
 
-#endif // SRC__RMF_FLEET_ADAPTER__TASK_HPP
+#endif // SRC__RMF_FLEET_ADAPTER__LEGACYTASK_HPP
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
index c5880a602..6c4947de9 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
@@ -154,7 +154,7 @@ const agv::RobotContextPtr& TaskManager::context()
 }
 
 //==============================================================================
-const Task* TaskManager::current_task() const
+const LegacyTask* TaskManager::current_task() const
 {
   return _active_task.get();
 }
@@ -463,7 +463,7 @@ void TaskManager::_begin_next_task()
     _task_sub = _active_task->observe()
       .observe_on(rxcpp::identity_same_worker(_context->worker()))
       .subscribe(
-      [me = weak_from_this(), active_task = _active_task](Task::StatusMsg msg)
+      [me = weak_from_this(), active_task = _active_task](LegacyTask::StatusMsg msg)
       {
         const auto self = me.lock();
         if (!self)
@@ -543,7 +543,7 @@ void TaskManager::_begin_waiting()
   _task_sub = _waiting->observe()
     .observe_on(rxcpp::identity_same_worker(_context->worker()))
     .subscribe(
-    [me = weak_from_this()](Task::StatusMsg msg)
+    [me = weak_from_this()](LegacyTask::StatusMsg msg)
     {
       const auto self = me.lock();
       if (!self)
@@ -698,7 +698,7 @@ void TaskManager::_register_executed_task(const std::string& id)
 
 //==============================================================================
 void TaskManager::_populate_task_summary(
-  std::shared_ptr<Task> task,
+  std::shared_ptr<LegacyTask> task,
   uint32_t task_summary_state,
   TaskManager::TaskSummaryMsg& msg)
 {
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
index 34750122f..c07080e39 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
@@ -18,7 +18,7 @@
 #ifndef SRC__RMF_FLEET_ADAPTER__TASKMANAGER_HPP
 #define SRC__RMF_FLEET_ADAPTER__TASKMANAGER_HPP
 
-#include "Task.hpp"
+#include "LegacyTask.hpp"
 #include "agv/RobotContext.hpp"
 #include "BroadcastClient.hpp"
 
@@ -70,7 +70,7 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
 
   std::weak_ptr<BroadcastClient> broadcast_client() const;
 
-  const Task* current_task() const;
+  const LegacyTask* current_task() const;
 
   /// Set the queue for this task manager with assignments generated from the
   /// task planner
@@ -105,8 +105,8 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
 
   agv::RobotContextPtr _context;
   std::weak_ptr<BroadcastClient> _broadcast_client;
-  std::shared_ptr<Task> _active_task;
-  std::vector<std::shared_ptr<Task>> _queue;
+  std::shared_ptr<LegacyTask> _active_task;
+  std::vector<std::shared_ptr<LegacyTask>> _queue;
   rmf_utils::optional<Start> _expected_finish_location;
   rxcpp::subscription _task_sub;
   rxcpp::subscription _emergency_sub;
@@ -114,7 +114,7 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
   /// This phase will kick in automatically when no task is being executed. It
   /// will ensure that the agent continues to respond to traffic negotiations so
   /// it does not become a blocker for other traffic participants.
-  std::shared_ptr<Task::ActivePhase> _waiting;
+  std::shared_ptr<LegacyTask::ActivePhase> _waiting;
 
   // TODO: Eliminate the need for a mutex by redesigning the use of the task
   // manager so that modifications of shared data only happen on designated
@@ -155,7 +155,7 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
 
   // The task_state.json for the active task. This should be initialized when
   // a request is activated.
-  // TODO: Should this be a shared_ptr to pass down to Task::Active?
+  // TODO: Should this be a shared_ptr to pass down to LegacyTask::Active?
   nlohmann::json _active_task_state;
 
   // Map task_id to task_log.json for all tasks managed by this TaskManager
@@ -199,9 +199,9 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
   /// Callback for when the task has finished
   void _task_finished();
 
-  // TODO: Assuming each Task::Active instance stores a weak_ptr to this
+  // TODO: Assuming each LegacyTask::Active instance stores a weak_ptr to this
   // TaskManager, the implementations of the corresponding functions in
-  // Task::Active can call these methods to publish state/log updates
+  // LegacyTask::Active can call these methods to publish state/log updates
   // void _interrupt_active_task();
   // void _cancel_active_task();
   // void _kill_active_task();
@@ -214,7 +214,7 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
   void _register_executed_task(const std::string& id);
 
   void _populate_task_summary(
-    std::shared_ptr<Task> task,
+    std::shared_ptr<LegacyTask> task,
     uint32_t task_summary_state,
     TaskSummaryMsg& msg);
 };
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
index 9c74f218d..9d2c72e00 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
@@ -713,7 +713,7 @@ void FleetUpdateHandle::Implementation::dispatch_request_cb(
 
     RCLCPP_INFO(
       node->get_logger(),
-      "Task with task_id:[%s] has successfully been cancelled. Assignments "
+      "LegacyTask with task_id:[%s] has successfully been cancelled. Assignments "
       "updated for robots in fleet [%s].",
       id.c_str(), name.c_str());
   }
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
index c95a27792..3f5726c1e 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
@@ -148,7 +148,7 @@ class FleetUpdateHandle::Implementation
   std::shared_ptr<rmf_traffic_ros2::schedule::Negotiation> negotiation;
   std::optional<std::string> server_uri;
 
-  // Task planner params
+  // LegacyTask planner params
   std::shared_ptr<rmf_task::CostCalculator> cost_calculator =
     rmf_task::BinaryPriorityScheme::make_cost_calculator();
   std::shared_ptr<rmf_task::TaskPlanner> task_planner = nullptr;
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.cpp
new file mode 100644
index 000000000..7c0ade7bb
--- /dev/null
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.cpp
@@ -0,0 +1,175 @@
+/*
+ * 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 "LegacyPhaseShim.hpp"
+
+namespace rmf_fleet_adapter {
+namespace events {
+
+//==============================================================================
+auto LegacyPhaseShim::Standby::make(
+  std::unique_ptr<LegacyTask::PendingPhase> legacy,
+  rxcpp::schedulers::worker worker,
+  const AssignIDPtr& id,
+  std::function<void()> parent_update) -> std::shared_ptr<Standby>
+{
+  auto standby = std::make_shared<Standby>();
+  standby->_legacy = std::move(legacy);
+  standby->_worker = std::move(worker);
+  standby->_state = rmf_task::events::SimpleEventState::make(
+    id->assign(),
+    standby->_legacy->description(),
+    "",
+    rmf_task::Event::Status::Standby);
+
+  standby->_parent_update = std::move(parent_update);
+}
+
+//==============================================================================
+auto LegacyPhaseShim::Standby::state() const -> ConstStatePtr
+{
+  return _state;
+}
+
+//==============================================================================
+rmf_traffic::Duration LegacyPhaseShim::Standby::duration_estimate() const
+{
+  _legacy->estimate_phase_duration();
+}
+
+//==============================================================================
+auto LegacyPhaseShim::Standby::begin(
+  std::function<void()> /* none of the legacy phases have checkpoints */,
+  std::function<void()> finished) -> ActivePtr
+{
+  if (!_legacy)
+  {
+    throw std::runtime_error(
+      "[rmf_fleet_adapter::events::LegacyPhaseShim::begin] "
+      "Triggering begin twice!");
+  }
+
+  return Active::make(
+    std::move(_legacy),
+    std::move(_worker),
+    std::move(_state),
+    std::move(_parent_update),
+    std::move(finished));
+}
+
+//==============================================================================
+auto LegacyPhaseShim::Active::make(
+  std::unique_ptr<LegacyTask::PendingPhase> legacy,
+  rxcpp::schedulers::worker worker,
+  rmf_task::events::SimpleEventStatePtr state,
+  std::function<void()> parent_update,
+  std::function<void()> finished) -> std::shared_ptr<Active>
+{
+  using rmf_task_msgs::msg::TaskSummary;
+  using rmf_task::Event;
+
+  auto active = std::make_shared<Active>();
+  active->_state = std::move(state);
+  active->_parent_update = std::move(parent_update);
+  active->_finished = std::move(finished);
+  active->_legacy = legacy->begin();
+  active->_subscription = active->_legacy->observe()
+    .observe_on(rxcpp::identity_same_worker(worker))
+    .subscribe(
+    [w = active->weak_from_this()](
+      const rmf_task_msgs::msg::TaskSummary& msg)
+    {
+      if (const auto self = w.lock())
+      {
+        bool need_update = false;
+        if (msg.status != self->_last_status_message)
+        {
+          need_update = true;
+          self->_last_status_message = msg.status;
+          auto& log = self->_state->update_log();
+          if (msg.state == TaskSummary::STATE_FAILED)
+            log.error(msg.status);
+          else
+            log.info(msg.status);
+        }
+
+        if (!self->_last_state_value.has_value()
+            || *self->_last_state_value != msg.state)
+        {
+          need_update = true;
+          self->_last_state_value = msg.state;
+          if (msg.state == TaskSummary::STATE_QUEUED
+              || msg.state == TaskSummary::STATE_PENDING)
+            self->_state->update_status(Event::Status::Standby);
+          else if (msg.state == TaskSummary::STATE_ACTIVE)
+            self->_state->update_status(Event::Status::Underway);
+          else if (msg.state == TaskSummary::STATE_COMPLETED)
+            self->_state->update_status(Event::Status::Completed);
+          else if (msg.state == TaskSummary::STATE_FAILED)
+            self->_state->update_status(Event::Status::Failed);
+          else if (msg.state == TaskSummary::STATE_CANCELED)
+            self->_state->update_status(Event::Status::Canceled);
+          else
+            self->_state->update_status(Event::Status::Uninitialized);
+        }
+
+        if (need_update)
+          self->_parent_update();
+      }
+    },
+    [w = active->weak_from_this()]()
+    {
+      if (const auto self = w.lock())
+      {
+        if (self->_finished)
+          self->_finished();
+
+        self->_finished = nullptr;
+      }
+    });
+
+  return active;
+}
+
+//==============================================================================
+auto LegacyPhaseShim::Active::state() const -> ConstStatePtr
+{
+  return _state;
+}
+
+//==============================================================================
+rmf_traffic::Duration LegacyPhaseShim::Active::remaining_time_estimate() const
+{
+  return _legacy->estimate_remaining_time();
+}
+
+//==============================================================================
+auto LegacyPhaseShim::Active::backup() const -> Backup
+{
+  // Legacy phases don't have backups
+  return Backup::make(0, nlohmann::json());
+}
+
+//==============================================================================
+auto LegacyPhaseShim::Active::interrupt(
+  std::function<void()> task_is_interrupted) -> Resume
+{
+
+}
+
+} // namespace events
+} // namespace rmf_fleet_adapter
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.hpp
new file mode 100644
index 000000000..9e51bb0f1
--- /dev/null
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.hpp
@@ -0,0 +1,104 @@
+/*
+ * 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 SRC__RMF_FLEET_ADAPTER__EVENTS__LEGACYPHASESHIM_HPP
+#define SRC__RMF_FLEET_ADAPTER__EVENTS__LEGACYPHASESHIM_HPP
+
+#include "../LegacyTask.hpp"
+
+#include <rmf_task_sequence/Event.hpp>
+#include <rmf_task/events/SimpleEventState.hpp>
+
+namespace rmf_fleet_adapter {
+namespace events {
+
+//==============================================================================
+/// This class is being used to gradually migrate the legacy task phase system
+/// into the new task event system. It provides a shim for legacy "Phase"
+/// implementations to be turned into Event objects.
+///
+/// We should endeavor to deprecate the use of this class over time as we work
+/// on improving the code quality of RMF.
+class LegacyPhaseShim : public rmf_task_sequence::Event
+{
+public:
+
+  class Standby : public rmf_task_sequence::Event::Standby
+  {
+  public:
+
+    static std::shared_ptr<Standby> make(
+      std::unique_ptr<LegacyTask::PendingPhase> legacy,
+      rxcpp::schedulers::worker worker,
+      const AssignIDPtr& id,
+      std::function<void()> parent_update);
+
+    ConstStatePtr state() const final;
+
+    rmf_traffic::Duration duration_estimate() const final;
+
+    ActivePtr begin(
+      std::function<void()> checkpoint,
+      std::function<void()> finished) final;
+
+  private:
+    std::unique_ptr<LegacyTask::PendingPhase> _legacy;
+    rxcpp::schedulers::worker _worker;
+    rmf_task::events::SimpleEventStatePtr _state;
+    std::function<void()> _parent_update;
+  };
+
+  class Active
+    : public rmf_task_sequence::Event::Active,
+      public std::enable_shared_from_this<Active>
+  {
+  public:
+
+    static std::shared_ptr<Active> make(
+      std::unique_ptr<LegacyTask::PendingPhase> legacy,
+      rxcpp::schedulers::worker worker,
+      rmf_task::events::SimpleEventStatePtr state,
+      std::function<void()> parent_update,
+      std::function<void()> finished);
+
+    ConstStatePtr state() const final;
+
+    rmf_traffic::Duration remaining_time_estimate() const final;
+
+    Backup backup() const final;
+
+    Resume interrupt(std::function<void()> task_is_interrupted) final;
+
+    void cancel() final;
+
+    void kill() final;
+
+  private:
+    std::shared_ptr<LegacyTask::ActivePhase> _legacy;
+    rmf_task::events::SimpleEventStatePtr _state;
+    std::string _last_status_message;
+    std::optional<uint32_t> _last_state_value;
+    std::function<void()> _parent_update;
+    std::function<void()> _finished;
+    rmf_rxcpp::subscription_guard _subscription;
+  };
+};
+
+} // namespace events
+} // namespace rmf_fleet_adapter
+
+#endif // SRC__RMF_FLEET_ADAPTER__EVENTS__LEGACYPHASESHIM_HPP
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DispenseItem.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DispenseItem.cpp
index cf22582cd..e9a78813e 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DispenseItem.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DispenseItem.cpp
@@ -42,7 +42,7 @@ std::shared_ptr<DispenseItem::ActivePhase> DispenseItem::ActivePhase::make(
 }
 
 //==============================================================================
-const rxcpp::observable<Task::StatusMsg>&
+const rxcpp::observable<LegacyTask::StatusMsg>&
 DispenseItem::ActivePhase::observe() const
 {
   return _obs;
@@ -135,11 +135,11 @@ void DispenseItem::ActivePhase::_init_obs()
       {
         auto me = weak.lock();
         if (!me)
-          return Task::StatusMsg();
+          return LegacyTask::StatusMsg();
 
         return me->_get_status(std::get<0>(v), std::get<1>(v));
       })
-    .lift<Task::StatusMsg>(grab_while_active())
+    .lift<LegacyTask::StatusMsg>(grab_while_active())
     .finally([weak = weak_from_this()]()
       {
         auto me = weak.lock();
@@ -152,12 +152,12 @@ void DispenseItem::ActivePhase::_init_obs()
 }
 
 //==============================================================================
-Task::StatusMsg DispenseItem::ActivePhase::_get_status(
+LegacyTask::StatusMsg DispenseItem::ActivePhase::_get_status(
   const rmf_dispenser_msgs::msg::DispenserResult::SharedPtr& dispenser_result,
   const rmf_dispenser_msgs::msg::DispenserState::SharedPtr& dispenser_state)
 {
-  Task::StatusMsg status{};
-  status.state = Task::StatusMsg::STATE_ACTIVE;
+  LegacyTask::StatusMsg status{};
+  status.state = LegacyTask::StatusMsg::STATE_ACTIVE;
 
   using rmf_dispenser_msgs::msg::DispenserResult;
   if (dispenser_result
@@ -171,10 +171,10 @@ Task::StatusMsg DispenseItem::ActivePhase::_get_status(
         _request_acknowledged = true;
         break;
       case DispenserResult::SUCCESS:
-        status.state = Task::StatusMsg::STATE_COMPLETED;
+        status.state = LegacyTask::StatusMsg::STATE_COMPLETED;
         break;
       case DispenserResult::FAILED:
-        status.state = Task::StatusMsg::STATE_FAILED;
+        status.state = LegacyTask::StatusMsg::STATE_FAILED;
         break;
     }
   }
@@ -200,7 +200,7 @@ Task::StatusMsg DispenseItem::ActivePhase::_get_status(
     {
       // The request has been received, so if it's no longer in the queue,
       // then we'll assume it's finished.
-      status.state = Task::StatusMsg::STATE_COMPLETED;
+      status.state = LegacyTask::StatusMsg::STATE_COMPLETED;
     }
   }
 
@@ -245,7 +245,7 @@ DispenseItem::PendingPhase::PendingPhase(
 }
 
 //==============================================================================
-std::shared_ptr<Task::ActivePhase> DispenseItem::PendingPhase::begin()
+std::shared_ptr<LegacyTask::ActivePhase> DispenseItem::PendingPhase::begin()
 {
   return DispenseItem::ActivePhase::make(
     _context,
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DispenseItem.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DispenseItem.hpp
index 946d6d257..7b2d9757d 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DispenseItem.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DispenseItem.hpp
@@ -19,7 +19,7 @@
 #define SRC__RMF_FLEET_ADAPTER__PHASES__DISPENSEITEM_HPP
 
 #include "RxOperators.hpp"
-#include "../Task.hpp"
+#include "../LegacyTask.hpp"
 #include "../agv/RobotContext.hpp"
 #include "rmf_fleet_adapter/StandardNames.hpp"
 
@@ -33,7 +33,7 @@ namespace phases {
 
 struct DispenseItem
 {
-  class ActivePhase : public Task::ActivePhase,
+  class ActivePhase : public LegacyTask::ActivePhase,
     public std::enable_shared_from_this<ActivePhase>
   {
   public:
@@ -45,7 +45,7 @@ struct DispenseItem
       std::string transporter_type,
       std::vector<rmf_dispenser_msgs::msg::DispenserRequestItem> items);
 
-    const rxcpp::observable<Task::StatusMsg>& observe() const override;
+    const rxcpp::observable<LegacyTask::StatusMsg>& observe() const override;
 
     rmf_traffic::Duration estimate_remaining_time() const override;
 
@@ -63,7 +63,7 @@ struct DispenseItem
     std::string _transporter_type;
     std::vector<rmf_dispenser_msgs::msg::DispenserRequestItem> _items;
     std::string _description;
-    rxcpp::observable<Task::StatusMsg> _obs;
+    rxcpp::observable<LegacyTask::StatusMsg> _obs;
     rclcpp::TimerBase::SharedPtr _timer;
     bool _request_acknowledged = false;
     builtin_interfaces::msg::Time _last_msg;
@@ -77,14 +77,14 @@ struct DispenseItem
 
     void _init_obs();
 
-    Task::StatusMsg _get_status(
+    LegacyTask::StatusMsg _get_status(
       const rmf_dispenser_msgs::msg::DispenserResult::SharedPtr& dispenser_result,
       const rmf_dispenser_msgs::msg::DispenserState::SharedPtr& dispenser_state);
 
     void _do_publish();
   };
 
-  class PendingPhase : public Task::PendingPhase
+  class PendingPhase : public LegacyTask::PendingPhase
   {
   public:
 
@@ -95,7 +95,7 @@ struct DispenseItem
       std::string transporter_type,
       std::vector<rmf_dispenser_msgs::msg::DispenserRequestItem> items);
 
-    std::shared_ptr<Task::ActivePhase> begin() override;
+    std::shared_ptr<LegacyTask::ActivePhase> begin() override;
 
     rmf_traffic::Duration estimate_phase_duration() const override;
 
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.cpp
index e93fcccbb..fbeb15e11 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.cpp
@@ -32,13 +32,13 @@ DockRobot::ActivePhase::ActivePhase(
   _description = oss.str();
 
   _action = std::make_shared<Action>(this);
-  _obs = rmf_rxcpp::make_job<Task::StatusMsg>(_action);
+  _obs = rmf_rxcpp::make_job<LegacyTask::StatusMsg>(_action);
 
   _context->current_mode(rmf_fleet_msgs::msg::RobotMode::MODE_DOCKING);
 }
 
 //==============================================================================
-const rxcpp::observable<Task::StatusMsg>&
+const rxcpp::observable<LegacyTask::StatusMsg>&
 DockRobot::ActivePhase::observe() const
 {
   return _obs;
@@ -82,7 +82,7 @@ DockRobot::PendingPhase::PendingPhase(
 }
 
 //==============================================================================
-std::shared_ptr<Task::ActivePhase> DockRobot::PendingPhase::begin()
+std::shared_ptr<LegacyTask::ActivePhase> DockRobot::PendingPhase::begin()
 {
   return std::make_shared<DockRobot::ActivePhase>(_context, _dock_name);
 }
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.hpp
index e584a5cf5..9d49cfd45 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.hpp
@@ -18,7 +18,7 @@
 #ifndef SRC__RMF_FLEET_ADAPTER__PHASES__DOCKROBOT_HPP
 #define SRC__RMF_FLEET_ADAPTER__PHASES__DOCKROBOT_HPP
 
-#include "../Task.hpp"
+#include "../LegacyTask.hpp"
 #include "../agv/RobotContext.hpp"
 
 namespace rmf_fleet_adapter {
@@ -29,7 +29,7 @@ struct DockRobot
 {
   class Action;
 
-  class ActivePhase : public Task::ActivePhase
+  class ActivePhase : public LegacyTask::ActivePhase
   {
   public:
 
@@ -37,7 +37,7 @@ struct DockRobot
       agv::RobotContextPtr context,
       std::string dock_name);
 
-    const rxcpp::observable<Task::StatusMsg>& observe() const override;
+    const rxcpp::observable<LegacyTask::StatusMsg>& observe() const override;
 
     rmf_traffic::Duration estimate_remaining_time() const override;
 
@@ -54,10 +54,10 @@ struct DockRobot
     std::string _dock_name;
     std::string _description;
     std::shared_ptr<Action> _action;
-    rxcpp::observable<Task::StatusMsg> _obs;
+    rxcpp::observable<LegacyTask::StatusMsg> _obs;
   };
 
-  class PendingPhase : public Task::PendingPhase
+  class PendingPhase : public LegacyTask::PendingPhase
   {
   public:
 
@@ -65,7 +65,7 @@ struct DockRobot
       agv::RobotContextPtr context,
       std::string dock_name);
 
-    std::shared_ptr<Task::ActivePhase> begin() override;
+    std::shared_ptr<LegacyTask::ActivePhase> begin() override;
 
     rmf_traffic::Duration estimate_phase_duration() const override;
 
@@ -96,8 +96,8 @@ struct DockRobot
 template<typename Subscriber>
 void DockRobot::Action::operator()(const Subscriber& s)
 {
-  Task::StatusMsg status;
-  status.state = Task::StatusMsg::STATE_ACTIVE;
+  LegacyTask::StatusMsg status;
+  status.state = LegacyTask::StatusMsg::STATE_ACTIVE;
   status.status = "Docking [" + _phase->_context->requester_id() +
     "] into dock ["
     + _phase->_dock_name + "]";
@@ -107,10 +107,10 @@ void DockRobot::Action::operator()(const Subscriber& s)
     _phase->_dock_name,
     [s, dock_name = _phase->_dock_name, context = _phase->_context]()
     {
-      Task::StatusMsg status;
+      LegacyTask::StatusMsg status;
       status.status = "Finished docking [" + context->requester_id()
       + "] into dock [" + dock_name + "]";
-      status.state = Task::StatusMsg::STATE_COMPLETED;
+      status.state = LegacyTask::StatusMsg::STATE_COMPLETED;
       s.on_next(status);
       s.on_completed();
     });
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorClose.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorClose.cpp
index 2d9aec95d..7f12f6841 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorClose.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorClose.cpp
@@ -39,7 +39,7 @@ std::shared_ptr<DoorClose::ActivePhase> DoorClose::ActivePhase::make(
 }
 
 //==============================================================================
-const rxcpp::observable<Task::StatusMsg>&
+const rxcpp::observable<LegacyTask::StatusMsg>&
 DoorClose::ActivePhase::observe() const
 {
   return _obs;
@@ -84,7 +84,7 @@ void DoorClose::ActivePhase::_init_obs()
         if (!me)
           return;
 
-        me->_status.state = Task::StatusMsg::STATE_ACTIVE;
+        me->_status.state = LegacyTask::StatusMsg::STATE_ACTIVE;
         me->_publish_close_door();
         me->_timer = me->_context->node()->try_create_wall_timer(
           std::chrono::milliseconds(1000),
@@ -101,12 +101,12 @@ void DoorClose::ActivePhase::_init_obs()
       {
         auto me = weak.lock();
         if (!me)
-          return Task::StatusMsg();
+          return LegacyTask::StatusMsg();
 
         me->_update_status(heartbeat);
         return me->_status;
       })
-    .lift<Task::StatusMsg>(grab_while_active())
+    .lift<LegacyTask::StatusMsg>(grab_while_active())
     .finally([weak = weak_from_this()]()
       {
         auto me = weak.lock();
@@ -135,7 +135,7 @@ void DoorClose::ActivePhase::_update_status(
   if (!supervisor_has_session(*heartbeat, _request_id, _door_name))
   {
     _status.status = "success";
-    _status.state = Task::StatusMsg::STATE_COMPLETED;
+    _status.state = LegacyTask::StatusMsg::STATE_COMPLETED;
   }
   else
   {
@@ -169,7 +169,7 @@ DoorClose::PendingPhase::PendingPhase(
 }
 
 //==============================================================================
-std::shared_ptr<Task::ActivePhase> DoorClose::PendingPhase::begin()
+std::shared_ptr<LegacyTask::ActivePhase> DoorClose::PendingPhase::begin()
 {
   return DoorClose::ActivePhase::make(
     _context,
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorClose.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorClose.hpp
index 43dafc073..ab389c82a 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorClose.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorClose.hpp
@@ -18,7 +18,7 @@
 #ifndef SRC__RMF_FLEET_ADAPTER__PHASES__DOORCLOSE_HPP
 #define SRC__RMF_FLEET_ADAPTER__PHASES__DOORCLOSE_HPP
 
-#include "../Task.hpp"
+#include "../LegacyTask.hpp"
 #include "../agv/RobotContext.hpp"
 
 #include <rmf_rxcpp/Transport.hpp>
@@ -38,7 +38,7 @@ struct DoorClose
    * 3. It is completed when the supervisor state does NOT contains the requester_id, regardless of the door state
    * 4. Cancellation requests are ignored
    */
-  class ActivePhase : public Task::ActivePhase,
+  class ActivePhase : public LegacyTask::ActivePhase,
     public std::enable_shared_from_this<ActivePhase>
   {
   public:
@@ -48,7 +48,7 @@ struct DoorClose
       std::string door_name,
       std::string request_id);
 
-    const rxcpp::observable<Task::StatusMsg>& observe() const override;
+    const rxcpp::observable<LegacyTask::StatusMsg>& observe() const override;
 
     rmf_traffic::Duration estimate_remaining_time() const override;
 
@@ -63,10 +63,10 @@ struct DoorClose
     agv::RobotContextPtr _context;
     std::string _door_name;
     std::string _request_id;
-    rxcpp::observable<Task::StatusMsg> _obs;
+    rxcpp::observable<LegacyTask::StatusMsg> _obs;
     std::string _description;
     rclcpp::TimerBase::SharedPtr _timer;
-    Task::StatusMsg _status;
+    LegacyTask::StatusMsg _status;
 
     ActivePhase(
       agv::RobotContextPtr context,
@@ -81,7 +81,7 @@ struct DoorClose
       const rmf_door_msgs::msg::SupervisorHeartbeat::SharedPtr& heartbeat);
   };
 
-  class PendingPhase : public Task::PendingPhase
+  class PendingPhase : public LegacyTask::PendingPhase
   {
   public:
 
@@ -90,7 +90,7 @@ struct DoorClose
       std::string door_name,
       std::string request_id);
 
-    std::shared_ptr<Task::ActivePhase> begin() override;
+    std::shared_ptr<LegacyTask::ActivePhase> begin() override;
 
     rmf_traffic::Duration estimate_phase_duration() const override;
 
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorOpen.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorOpen.cpp
index 61d13325f..c69a4da95 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorOpen.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorOpen.cpp
@@ -58,7 +58,7 @@ DoorOpen::ActivePhase::ActivePhase(
 }
 
 //==============================================================================
-const rxcpp::observable<Task::StatusMsg>& DoorOpen::ActivePhase::observe() const
+const rxcpp::observable<LegacyTask::StatusMsg>& DoorOpen::ActivePhase::observe() const
 {
   return _obs;
 }
@@ -107,7 +107,7 @@ void DoorOpen::ActivePhase::_init_obs()
         if (!me)
           return;
 
-        me->_status.state = Task::StatusMsg::STATE_ACTIVE;
+        me->_status.state = LegacyTask::StatusMsg::STATE_ACTIVE;
         me->_publish_open_door();
         me->_timer =
         transport->try_create_wall_timer(std::chrono::milliseconds(1000),
@@ -139,12 +139,12 @@ void DoorOpen::ActivePhase::_init_obs()
       {
         auto me = weak.lock();
         if (!me)
-          return Task::StatusMsg();
+          return LegacyTask::StatusMsg();
 
         me->_update_status(std::get<0>(v), std::get<1>(v));
         return me->_status;
       })
-    .lift<Task::StatusMsg>(grab_while_active())
+    .lift<LegacyTask::StatusMsg>(grab_while_active())
     .finally([weak = weak_from_this()]()
       {
         auto me = weak.lock();
@@ -156,7 +156,7 @@ void DoorOpen::ActivePhase::_init_obs()
     // When the phase is cancelled, queue a door close phase to make sure that there is no hanging
     // open doors
     .take_until(_cancelled.get_observable().filter([](auto b) { return b; }))
-    .concat(rxcpp::observable<>::create<Task::StatusMsg>(
+    .concat(rxcpp::observable<>::create<LegacyTask::StatusMsg>(
         [weak = weak_from_this()](const auto& s)
         {
           auto me = weak.lock();
@@ -201,7 +201,7 @@ void DoorOpen::ActivePhase::_update_status(
     && supervisor_has_session(*heartbeat, _request_id, _door_name))
   {
     _status.status = "success";
-    _status.state = Task::StatusMsg::STATE_COMPLETED;
+    _status.state = LegacyTask::StatusMsg::STATE_COMPLETED;
   }
   else
   {
@@ -225,7 +225,7 @@ DoorOpen::PendingPhase::PendingPhase(
 }
 
 //==============================================================================
-std::shared_ptr<Task::ActivePhase> DoorOpen::PendingPhase::begin()
+std::shared_ptr<LegacyTask::ActivePhase> DoorOpen::PendingPhase::begin()
 {
   return ActivePhase::make(_context, _door_name, _request_id, _expected_finish);
 }
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorOpen.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorOpen.hpp
index 1c5ba3488..fa5f71922 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorOpen.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorOpen.hpp
@@ -19,7 +19,7 @@
 #define SRC__RMF_FLEET_ADAPTER__PHASES__DOOROPEN_HPP
 
 #include "DoorClose.hpp"
-#include "../Task.hpp"
+#include "../LegacyTask.hpp"
 #include "../agv/RobotContext.hpp"
 
 #include <rmf_rxcpp/Transport.hpp>
@@ -39,7 +39,7 @@ struct DoorOpen
    * 2. It is completed when the supervisor state contains the requester_id and the door has an OPEN mode
    * 4. If cancelled, should start a door close phase
    */
-  class ActivePhase : public Task::ActivePhase,
+  class ActivePhase : public LegacyTask::ActivePhase,
     public std::enable_shared_from_this<ActivePhase>
   {
   public:
@@ -50,7 +50,7 @@ struct DoorOpen
       std::string request_id,
       rmf_traffic::Time expected_finish);
 
-    const rxcpp::observable<Task::StatusMsg>& observe() const override;
+    const rxcpp::observable<LegacyTask::StatusMsg>& observe() const override;
 
     rmf_traffic::Duration estimate_remaining_time() const override;
 
@@ -68,10 +68,10 @@ struct DoorOpen
     rmf_traffic::Time _expected_finish;
     rxcpp::subjects::behavior<bool> _cancelled =
       rxcpp::subjects::behavior<bool>(false);
-    rxcpp::observable<Task::StatusMsg> _obs;
+    rxcpp::observable<LegacyTask::StatusMsg> _obs;
     std::string _description;
     rclcpp::TimerBase::SharedPtr _timer;
-    Task::StatusMsg _status;
+    LegacyTask::StatusMsg _status;
     std::shared_ptr<DoorClose::ActivePhase> _door_close_phase;
 
     ActivePhase(
@@ -89,7 +89,7 @@ struct DoorOpen
       const rmf_door_msgs::msg::SupervisorHeartbeat::SharedPtr& heartbeat);
   };
 
-  class PendingPhase : public Task::PendingPhase
+  class PendingPhase : public LegacyTask::PendingPhase
   {
   public:
 
@@ -99,7 +99,7 @@ struct DoorOpen
       std::string request_id,
       rmf_traffic::Time expected_finish);
 
-    std::shared_ptr<Task::ActivePhase> begin() override;
+    std::shared_ptr<LegacyTask::ActivePhase> begin() override;
 
     rmf_traffic::Duration estimate_phase_duration() const override;
 
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.cpp
index ae281ed5d..a4224e938 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.cpp
@@ -49,7 +49,7 @@ EndLiftSession::Active::Active(
 }
 
 //==============================================================================
-const rxcpp::observable<Task::StatusMsg>&
+const rxcpp::observable<LegacyTask::StatusMsg>&
 EndLiftSession::Active::observe() const
 {
   return _obs;
@@ -108,10 +108,10 @@ void EndLiftSession::Active::_init_obs()
       {
         const auto me = weak.lock();
         if (!me)
-          return Task::StatusMsg();
+          return LegacyTask::StatusMsg();
 
-        Task::StatusMsg msg;
-        msg.state = Task::StatusMsg::STATE_ACTIVE;
+        LegacyTask::StatusMsg msg;
+        msg.state = LegacyTask::StatusMsg::STATE_ACTIVE;
 
         if (state->lift_name != me->_lift_name)
           return msg;
@@ -119,12 +119,12 @@ void EndLiftSession::Active::_init_obs()
         if (state->session_id != me->_context->requester_id())
         {
           msg.status = "success";
-          msg.state = Task::StatusMsg::STATE_COMPLETED;
+          msg.state = LegacyTask::StatusMsg::STATE_COMPLETED;
         }
 
         return msg;
       })
-    .lift<Task::StatusMsg>(grab_while_active())
+    .lift<LegacyTask::StatusMsg>(grab_while_active())
     .finally([weak = weak_from_this()]()
       {
         const auto me = weak.lock();
@@ -160,7 +160,7 @@ EndLiftSession::Pending::Pending(
 }
 
 //==============================================================================
-std::shared_ptr<Task::ActivePhase> EndLiftSession::Pending::begin()
+std::shared_ptr<LegacyTask::ActivePhase> EndLiftSession::Pending::begin()
 {
   return Active::make(
     std::move(_context),
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.hpp
index 727f2ed65..cfa2373fa 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.hpp
@@ -18,7 +18,7 @@
 #ifndef SRC__RMF_FLEET_ADAPTER__PHASES__ENDLIFTSESSION_HPP
 #define SRC__RMF_FLEET_ADAPTER__PHASES__ENDLIFTSESSION_HPP
 
-#include "../Task.hpp"
+#include "../LegacyTask.hpp"
 #include "../agv/RobotContext.hpp"
 #include "rmf_fleet_adapter/StandardNames.hpp"
 
@@ -27,7 +27,7 @@ namespace phases {
 
 struct EndLiftSession
 {
-  class Active : public Task::ActivePhase,
+  class Active : public LegacyTask::ActivePhase,
     public std::enable_shared_from_this<Active>
   {
   public:
@@ -42,7 +42,7 @@ struct EndLiftSession
       std::string lift_name,
       std::string destination);
 
-    const rxcpp::observable<Task::StatusMsg>& observe() const override;
+    const rxcpp::observable<LegacyTask::StatusMsg>& observe() const override;
 
     rmf_traffic::Duration estimate_remaining_time() const override;
 
@@ -58,14 +58,14 @@ struct EndLiftSession
     std::string _lift_name;
     std::string _destination;
     std::string _description;
-    rxcpp::observable<Task::StatusMsg> _obs;
+    rxcpp::observable<LegacyTask::StatusMsg> _obs;
     rclcpp::TimerBase::SharedPtr _timer;
 
     void _init_obs();
     void _publish_session_end();
   };
 
-  class Pending : public Task::PendingPhase
+  class Pending : public LegacyTask::PendingPhase
   {
   public:
 
@@ -74,7 +74,7 @@ struct EndLiftSession
       std::string lift_name,
       std::string destination);
 
-    std::shared_ptr<Task::ActivePhase> begin() final;
+    std::shared_ptr<LegacyTask::ActivePhase> begin() final;
 
     rmf_traffic::Duration estimate_phase_duration() const final;
 
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.cpp
index 55e987c1a..c2c0bddb5 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.cpp
@@ -337,7 +337,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor
 
   EventPhaseFactory(
     agv::RobotContextPtr context,
-    Task::PendingPhases& phases,
+    LegacyTask::PendingPhases& phases,
     rmf_traffic::Time event_start_time,
     bool& continuous)
   : _context(std::move(context)),
@@ -451,7 +451,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor
 
 private:
   agv::RobotContextPtr _context;
-  Task::PendingPhases& _phases;
+  LegacyTask::PendingPhases& _phases;
   rmf_traffic::Time _event_start_time;
   bool& _continuous;
   bool _moving_lift = false;
@@ -468,7 +468,7 @@ void GoToPlace::Active::execute_plan(rmf_traffic::agv::Plan new_plan)
     _plan->get_waypoints();
   std::vector<rmf_traffic::agv::Plan::Waypoint> move_through;
 
-  Task::PendingPhases sub_phases;
+  LegacyTask::PendingPhases sub_phases;
   while (!waypoints.empty())
   {
     auto it = waypoints.begin();
@@ -559,7 +559,7 @@ void GoToPlace::Active::execute_plan(rmf_traffic::agv::Plan new_plan)
   .time(dummy_time)
   .dedicated_charging_waypoint(0)
   .battery_soc(1.0);
-  _subtasks = Task::make(
+  _subtasks = LegacyTask::make(
     _description,
     std::move(sub_phases),
     _context->worker(),
@@ -602,7 +602,7 @@ void GoToPlace::Active::execute_plan(rmf_traffic::agv::Plan new_plan)
 }
 
 //==============================================================================
-std::shared_ptr<Task::ActivePhase> GoToPlace::Pending::begin()
+std::shared_ptr<LegacyTask::ActivePhase> GoToPlace::Pending::begin()
 {
   auto active =
     std::shared_ptr<Active>(
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.hpp
index a969131f7..7930ccf87 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.hpp
@@ -18,7 +18,7 @@
 #ifndef SRC__RMF_FLEET_ADAPTER__PHASES__GOTOPLACE_HPP
 #define SRC__RMF_FLEET_ADAPTER__PHASES__GOTOPLACE_HPP
 
-#include "../Task.hpp"
+#include "../LegacyTask.hpp"
 #include "../agv/RobotContext.hpp"
 
 #include "../services/FindPath.hpp"
@@ -33,11 +33,11 @@ class GoToPlace
 {
 public:
 
-  using StatusMsg = Task::StatusMsg;
+  using StatusMsg = LegacyTask::StatusMsg;
   class Pending;
 
   class Active
-    : public Task::ActivePhase,
+    : public LegacyTask::ActivePhase,
     public rmf_traffic::schedule::Negotiator,
     public std::enable_shared_from_this<Active>
   {
@@ -82,7 +82,7 @@ class GoToPlace
     double _latest_time_estimate;
     std::string _description;
     rmf_utils::optional<rmf_traffic::agv::Plan> _plan;
-    std::shared_ptr<Task> _subtasks;
+    std::shared_ptr<LegacyTask> _subtasks;
     bool _emergency_active = false;
     bool _performing_emergency_task = false;
     rxcpp::subjects::subject<StatusMsg> _status_publisher;
@@ -110,12 +110,12 @@ class GoToPlace
     std::shared_ptr<void> _negotiator_license;
   };
 
-  class Pending : public Task::PendingPhase
+  class Pending : public LegacyTask::PendingPhase
   {
   public:
 
     // Documentation inherited
-    std::shared_ptr<Task::ActivePhase> begin() final;
+    std::shared_ptr<LegacyTask::ActivePhase> begin() final;
 
     // Documentation inherited
     rmf_traffic::Duration estimate_phase_duration() const final;
@@ -138,7 +138,7 @@ class GoToPlace
     std::string _description;
   };
 
-  /// Make a Task Phase for going to a place
+  /// Make a LegacyTask Phase for going to a place
   static std::unique_ptr<Pending> make(
     agv::RobotContextPtr context,
     rmf_traffic::agv::Plan::Start start_estimate,
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/IngestItem.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/IngestItem.cpp
index 158af77a5..3db97ec62 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/IngestItem.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/IngestItem.cpp
@@ -42,7 +42,7 @@ std::shared_ptr<IngestItem::ActivePhase> IngestItem::ActivePhase::make(
 }
 
 //==============================================================================
-const rxcpp::observable<Task::StatusMsg>&
+const rxcpp::observable<LegacyTask::StatusMsg>&
 IngestItem::ActivePhase::observe() const
 {
   return _obs;
@@ -134,11 +134,11 @@ void IngestItem::ActivePhase::_init_obs()
       {
         auto me = weak.lock();
         if (!me)
-          return Task::StatusMsg();
+          return LegacyTask::StatusMsg();
 
         return me->_get_status(std::get<0>(v), std::get<1>(v));
       })
-    .lift<Task::StatusMsg>(grab_while_active())
+    .lift<LegacyTask::StatusMsg>(grab_while_active())
     .finally([weak = weak_from_this()]()
       {
         auto me = weak.lock();
@@ -151,12 +151,12 @@ void IngestItem::ActivePhase::_init_obs()
 }
 
 //==============================================================================
-Task::StatusMsg IngestItem::ActivePhase::_get_status(
+LegacyTask::StatusMsg IngestItem::ActivePhase::_get_status(
   const rmf_ingestor_msgs::msg::IngestorResult::SharedPtr& ingestor_result,
   const rmf_ingestor_msgs::msg::IngestorState::SharedPtr& ingestor_state)
 {
-  Task::StatusMsg status{};
-  status.state = Task::StatusMsg::STATE_ACTIVE;
+  LegacyTask::StatusMsg status{};
+  status.state = LegacyTask::StatusMsg::STATE_ACTIVE;
 
   using rmf_ingestor_msgs::msg::IngestorResult;
   if (ingestor_result
@@ -170,10 +170,10 @@ Task::StatusMsg IngestItem::ActivePhase::_get_status(
         _request_acknowledged = true;
         break;
       case IngestorResult::SUCCESS:
-        status.state = Task::StatusMsg::STATE_COMPLETED;
+        status.state = LegacyTask::StatusMsg::STATE_COMPLETED;
         break;
       case IngestorResult::FAILED:
-        status.state = Task::StatusMsg::STATE_FAILED;
+        status.state = LegacyTask::StatusMsg::STATE_FAILED;
         break;
     }
   }
@@ -199,7 +199,7 @@ Task::StatusMsg IngestItem::ActivePhase::_get_status(
     {
       // The request has been received, so if it's no longer in the queue,
       // then we'll assume it's finished.
-      status.state = Task::StatusMsg::STATE_COMPLETED;
+      status.state = LegacyTask::StatusMsg::STATE_COMPLETED;
     }
   }
 
@@ -244,7 +244,7 @@ IngestItem::PendingPhase::PendingPhase(
 }
 
 //==============================================================================
-std::shared_ptr<Task::ActivePhase> IngestItem::PendingPhase::begin()
+std::shared_ptr<LegacyTask::ActivePhase> IngestItem::PendingPhase::begin()
 {
   return IngestItem::ActivePhase::make(
     _context,
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/IngestItem.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/IngestItem.hpp
index 0e4cd16d2..802a533bb 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/IngestItem.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/IngestItem.hpp
@@ -19,7 +19,7 @@
 #define SRC__RMF_FLEET_ADAPTER__PHASES__INGESTITEM_HPP
 
 #include "RxOperators.hpp"
-#include "../Task.hpp"
+#include "../LegacyTask.hpp"
 #include "../agv/RobotContext.hpp"
 #include "rmf_fleet_adapter/StandardNames.hpp"
 
@@ -33,7 +33,7 @@ namespace phases {
 
 struct IngestItem
 {
-  class ActivePhase : public Task::ActivePhase,
+  class ActivePhase : public LegacyTask::ActivePhase,
     public std::enable_shared_from_this<ActivePhase>
   {
   public:
@@ -45,7 +45,7 @@ struct IngestItem
       std::string transporter_type,
       std::vector<rmf_ingestor_msgs::msg::IngestorRequestItem> items);
 
-    const rxcpp::observable<Task::StatusMsg>& observe() const override;
+    const rxcpp::observable<LegacyTask::StatusMsg>& observe() const override;
 
     rmf_traffic::Duration estimate_remaining_time() const override;
 
@@ -63,7 +63,7 @@ struct IngestItem
     std::string _transporter_type;
     std::vector<rmf_ingestor_msgs::msg::IngestorRequestItem> _items;
     std::string _description;
-    rxcpp::observable<Task::StatusMsg> _obs;
+    rxcpp::observable<LegacyTask::StatusMsg> _obs;
     rclcpp::TimerBase::SharedPtr _timer;
     bool _request_acknowledged = false;
     builtin_interfaces::msg::Time _last_msg;
@@ -77,14 +77,14 @@ struct IngestItem
 
     void _init_obs();
 
-    Task::StatusMsg _get_status(
+    LegacyTask::StatusMsg _get_status(
       const rmf_ingestor_msgs::msg::IngestorResult::SharedPtr& ingestor_result,
       const rmf_ingestor_msgs::msg::IngestorState::SharedPtr& ingestor_state);
 
     void _do_publish();
   };
 
-  class PendingPhase : public Task::PendingPhase
+  class PendingPhase : public LegacyTask::PendingPhase
   {
   public:
 
@@ -95,7 +95,7 @@ struct IngestItem
       std::string transporter_type,
       std::vector<rmf_ingestor_msgs::msg::IngestorRequestItem> items);
 
-    std::shared_ptr<Task::ActivePhase> begin() override;
+    std::shared_ptr<LegacyTask::ActivePhase> begin() override;
 
     rmf_traffic::Duration estimate_phase_duration() const override;
 
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.cpp
index 4a7224261..a0f197bb2 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.cpp
@@ -38,17 +38,17 @@ MoveRobot::ActivePhase::ActivePhase(
   _action = std::make_shared<MoveRobot::Action>(
     _context, waypoints, _tail_period);
 
-  auto job = rmf_rxcpp::make_job<Task::StatusMsg>(_action);
+  auto job = rmf_rxcpp::make_job<LegacyTask::StatusMsg>(_action);
 
   _obs = make_cancellable(job, _cancel_subject.get_observable())
-    .lift<Task::StatusMsg>(grab_while_active())
+    .lift<LegacyTask::StatusMsg>(grab_while_active())
     .observe_on(rxcpp::identity_same_worker(_context->worker()));
 
   _context->current_mode(rmf_fleet_msgs::msg::RobotMode::MODE_MOVING);
 }
 
 //==============================================================================
-const rxcpp::observable<Task::StatusMsg>& MoveRobot::ActivePhase::observe()
+const rxcpp::observable<LegacyTask::StatusMsg>& MoveRobot::ActivePhase::observe()
 const
 {
   return _obs;
@@ -96,7 +96,7 @@ MoveRobot::PendingPhase::PendingPhase(
 }
 
 //==============================================================================
-std::shared_ptr<Task::ActivePhase> MoveRobot::PendingPhase::begin()
+std::shared_ptr<LegacyTask::ActivePhase> MoveRobot::PendingPhase::begin()
 {
   return std::make_shared<MoveRobot::ActivePhase>(
     _context, _waypoints, _tail_period);
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp
index 8fdf0f0b0..08cc8c7e7 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp
@@ -18,7 +18,7 @@
 #ifndef SRC__RMF_FLEET_ADAPTER__PHASES__MOVEROBOT_HPP
 #define SRC__RMF_FLEET_ADAPTER__PHASES__MOVEROBOT_HPP
 
-#include "../Task.hpp"
+#include "../LegacyTask.hpp"
 #include "../agv/RobotContext.hpp"
 
 #include <rmf_traffic_ros2/Time.hpp>
@@ -30,7 +30,7 @@ struct MoveRobot
 {
   class Action;
 
-  class ActivePhase : public Task::ActivePhase
+  class ActivePhase : public LegacyTask::ActivePhase
   {
   public:
 
@@ -39,7 +39,7 @@ struct MoveRobot
       std::vector<rmf_traffic::agv::Plan::Waypoint> waypoints,
       std::optional<rmf_traffic::Duration> tail_period);
 
-    const rxcpp::observable<Task::StatusMsg>& observe() const override;
+    const rxcpp::observable<LegacyTask::StatusMsg>& observe() const override;
 
     rmf_traffic::Duration estimate_remaining_time() const override;
 
@@ -54,12 +54,12 @@ struct MoveRobot
     agv::RobotContextPtr _context;
     std::string _description;
     std::shared_ptr<Action> _action;
-    rxcpp::observable<Task::StatusMsg> _obs;
+    rxcpp::observable<LegacyTask::StatusMsg> _obs;
     rxcpp::subjects::subject<bool> _cancel_subject;
     std::optional<rmf_traffic::Duration> _tail_period;
   };
 
-  class PendingPhase : public Task::PendingPhase
+  class PendingPhase : public LegacyTask::PendingPhase
   {
   public:
 
@@ -68,7 +68,7 @@ struct MoveRobot
       std::vector<rmf_traffic::agv::Plan::Waypoint> waypoints,
       std::optional<rmf_traffic::Duration> tail_period);
 
-    std::shared_ptr<Task::ActivePhase> begin() override;
+    std::shared_ptr<LegacyTask::ActivePhase> begin() override;
 
     rmf_traffic::Duration estimate_phase_duration() const override;
 
@@ -142,8 +142,8 @@ void MoveRobot::Action::operator()(const Subscriber& s)
       if (path_index != action->_next_path_index)
       {
         action->_next_path_index = path_index;
-        Task::StatusMsg msg;
-        msg.state = Task::StatusMsg::STATE_ACTIVE;
+        LegacyTask::StatusMsg msg;
+        msg.state = LegacyTask::StatusMsg::STATE_ACTIVE;
 
         if (path_index < action->_waypoints.size())
         {
@@ -217,8 +217,8 @@ void MoveRobot::Action::operator()(const Subscriber& s)
     },
     [s]()
     {
-      Task::StatusMsg msg;
-      msg.state = Task::StatusMsg::STATE_COMPLETED;
+      LegacyTask::StatusMsg msg;
+      msg.state = LegacyTask::StatusMsg::STATE_COMPLETED;
       msg.status = "move robot success";
       s.on_next(msg);
 
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp
index 437cdd231..bc76dc870 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp
@@ -43,7 +43,7 @@ std::shared_ptr<RequestLift::ActivePhase> RequestLift::ActivePhase::make(
 }
 
 //==============================================================================
-const rxcpp::observable<Task::StatusMsg>&
+const rxcpp::observable<LegacyTask::StatusMsg>&
 RequestLift::ActivePhase::observe() const
 {
   return _obs;
@@ -138,20 +138,20 @@ void RequestLift::ActivePhase::_init_obs()
       {
         auto me = weak.lock();
         if (!me)
-          return Task::StatusMsg();
+          return LegacyTask::StatusMsg();
 
         return me->_get_status(v);
       })
-    .lift<Task::StatusMsg>(grab_while([weak =
-      weak_from_this()](const Task::StatusMsg& status)
+    .lift<LegacyTask::StatusMsg>(grab_while([weak =
+      weak_from_this()](const LegacyTask::StatusMsg& status)
       {
         auto me = weak.lock();
         if (!me)
           return false;
 
         if (
-          status.state == Task::StatusMsg::STATE_COMPLETED ||
-          status.state == Task::StatusMsg::STATE_FAILED)
+          status.state == LegacyTask::StatusMsg::STATE_COMPLETED ||
+          status.state == LegacyTask::StatusMsg::STATE_FAILED)
         {
           me->_timer.reset();
           return false;
@@ -159,7 +159,7 @@ void RequestLift::ActivePhase::_init_obs()
         return true;
       }))
     .take_until(_cancelled.get_observable().filter([](auto b) { return b; }))
-    .concat(rxcpp::observable<>::create<Task::StatusMsg>(
+    .concat(rxcpp::observable<>::create<LegacyTask::StatusMsg>(
         [weak = weak_from_this()](const auto& s)
         {
           auto me = weak.lock();
@@ -185,13 +185,13 @@ void RequestLift::ActivePhase::_init_obs()
 }
 
 //==============================================================================
-Task::StatusMsg RequestLift::ActivePhase::_get_status(
+LegacyTask::StatusMsg RequestLift::ActivePhase::_get_status(
   const rmf_lift_msgs::msg::LiftState::SharedPtr& lift_state)
 {
   using rmf_lift_msgs::msg::LiftState;
   using rmf_lift_msgs::msg::LiftRequest;
-  Task::StatusMsg status{};
-  status.state = Task::StatusMsg::STATE_ACTIVE;
+  LegacyTask::StatusMsg status{};
+  status.state = LegacyTask::StatusMsg::STATE_ACTIVE;
   if (!_rewaiting &&
     lift_state->lift_name == _lift_name &&
     lift_state->current_floor == _destination &&
@@ -273,7 +273,7 @@ Task::StatusMsg RequestLift::ActivePhase::_get_status(
 
     if (completed)
     {
-      status.state = Task::StatusMsg::STATE_COMPLETED;
+      status.state = LegacyTask::StatusMsg::STATE_COMPLETED;
       status.status = "success";
       _timer.reset();
     }
@@ -334,7 +334,7 @@ RequestLift::PendingPhase::PendingPhase(
 }
 
 //==============================================================================
-std::shared_ptr<Task::ActivePhase> RequestLift::PendingPhase::begin()
+std::shared_ptr<LegacyTask::ActivePhase> RequestLift::PendingPhase::begin()
 {
   return ActivePhase::make(
     _context,
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.hpp
index 5c2f35057..e269b8245 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.hpp
@@ -18,7 +18,7 @@
 #ifndef SRC__RMF_FLEET_ADAPTER__PHASES__REQUESTLIFT_HPP
 #define SRC__RMF_FLEET_ADAPTER__PHASES__REQUESTLIFT_HPP
 
-#include "../Task.hpp"
+#include "../LegacyTask.hpp"
 #include "../agv/RobotContext.hpp"
 #include "rmf_fleet_adapter/StandardNames.hpp"
 #include "EndLiftSession.hpp"
@@ -34,7 +34,7 @@ struct RequestLift
     Outside
   };
 
-  class ActivePhase : public Task::ActivePhase,
+  class ActivePhase : public LegacyTask::ActivePhase,
     public std::enable_shared_from_this<ActivePhase>
   {
   public:
@@ -46,7 +46,7 @@ struct RequestLift
       rmf_traffic::Time expected_finish,
       Located located);
 
-    const rxcpp::observable<Task::StatusMsg>& observe() const override;
+    const rxcpp::observable<LegacyTask::StatusMsg>& observe() const override;
 
     rmf_traffic::Duration estimate_remaining_time() const override;
 
@@ -65,7 +65,7 @@ struct RequestLift
     rxcpp::subjects::behavior<bool> _cancelled =
       rxcpp::subjects::behavior<bool>(false);
     std::string _description;
-    rxcpp::observable<Task::StatusMsg> _obs;
+    rxcpp::observable<LegacyTask::StatusMsg> _obs;
     rclcpp::TimerBase::SharedPtr _timer;
     std::shared_ptr<EndLiftSession::Active> _lift_end_phase;
     Located _located;
@@ -90,13 +90,13 @@ struct RequestLift
 
     void _init_obs();
 
-    Task::StatusMsg _get_status(
+    LegacyTask::StatusMsg _get_status(
       const rmf_lift_msgs::msg::LiftState::SharedPtr& lift_state);
 
     void _do_publish();
   };
 
-  class PendingPhase : public Task::PendingPhase
+  class PendingPhase : public LegacyTask::PendingPhase
   {
   public:
 
@@ -107,7 +107,7 @@ struct RequestLift
       rmf_traffic::Time expected_finish,
       Located located);
 
-    std::shared_ptr<Task::ActivePhase> begin() override;
+    std::shared_ptr<LegacyTask::ActivePhase> begin() override;
 
     rmf_traffic::Duration estimate_phase_duration() const override;
 
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ResponsiveWait.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ResponsiveWait.cpp
index 23dc0256e..540bf7633 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ResponsiveWait.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ResponsiveWait.cpp
@@ -154,7 +154,7 @@ void ResponsiveWait::Active::_begin_movement()
 }
 
 //==============================================================================
-std::shared_ptr<Task::ActivePhase> ResponsiveWait::Pending::begin()
+std::shared_ptr<LegacyTask::ActivePhase> ResponsiveWait::Pending::begin()
 {
   auto active = std::shared_ptr<Active>(new Active(_info));
   active->_begin_movement();
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ResponsiveWait.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ResponsiveWait.hpp
index 8d2b1ad4d..3ff0dd573 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ResponsiveWait.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ResponsiveWait.hpp
@@ -38,11 +38,11 @@ class ResponsiveWait
 
 public:
 
-  using StatusMsg = Task::StatusMsg;
+  using StatusMsg = LegacyTask::StatusMsg;
   class Pending;
 
   class Active
-    : public Task::ActivePhase,
+    : public LegacyTask::ActivePhase,
     public std::enable_shared_from_this<Active>
   {
   public:
@@ -72,15 +72,15 @@ class ResponsiveWait
     rxcpp::observable<StatusMsg> _status_obs;
     rxcpp::subjects::subject<StatusMsg> _status_publisher;
     rmf_rxcpp::subscription_guard _movement_subscription;
-    std::shared_ptr<Task::ActivePhase> _movement;
+    std::shared_ptr<LegacyTask::ActivePhase> _movement;
   };
 
-  class Pending : public Task::PendingPhase
+  class Pending : public LegacyTask::PendingPhase
   {
   public:
 
     // Documentation inherited
-    std::shared_ptr<Task::ActivePhase> begin() final;
+    std::shared_ptr<LegacyTask::ActivePhase> begin() final;
 
     // Documentation inherited
     rmf_traffic::Duration estimate_phase_duration() const final;
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RxOperators.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RxOperators.hpp
index a284a22ac..438aec528 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RxOperators.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RxOperators.hpp
@@ -18,7 +18,7 @@
 #ifndef SRC__RMF_FLEET_ADAPTER__PHASES__RXOPERATORS_HPP
 #define SRC__RMF_FLEET_ADAPTER__PHASES__RXOPERATORS_HPP
 
-#include "../Task.hpp"
+#include "../LegacyTask.hpp"
 
 #include <rxcpp/rx.hpp>
 
@@ -53,10 +53,10 @@ auto grab_while(Predicate pred)
 
 inline auto grab_while_active()
 {
-  return grab_while([](const Task::StatusMsg& status)
+  return grab_while([](const LegacyTask::StatusMsg& status)
       {
-        return !(status.state == Task::StatusMsg::STATE_COMPLETED ||
-        status.state == Task::StatusMsg::STATE_FAILED);
+        return !(status.state == LegacyTask::StatusMsg::STATE_COMPLETED ||
+        status.state == LegacyTask::StatusMsg::STATE_FAILED);
       });
 }
 
@@ -80,10 +80,10 @@ auto on_subscribe(F f)
 const std::string status_msg_cancelled = "cancelled";
 
 /**
- * Makes an observable cancellable, the observable must be of type `Task::StatusMsg`.
+ * Makes an observable cancellable, the observable must be of type `LegacyTask::StatusMsg`.
  * TODO: Add a "CANCELLED" task state?
  * Use the `cancelled_msg` status text to find if the result of the observable is cancelled.
- * @tparam Observable An observable of type `Task::StatusMsg`
+ * @tparam Observable An observable of type `LegacyTask::StatusMsg`
  * @tparam CancelObservable An observable of type `bool`
  * @param obs
  * @param cancel_obs
@@ -96,14 +96,14 @@ auto make_cancellable(const Observable& obs, const CancelObservable& cancel_obs)
     .filter([](const auto& b) { return b; })
     .map([](const auto&)
       {
-        Task::StatusMsg status;
-        status.state = Task::StatusMsg::STATE_COMPLETED;
+        LegacyTask::StatusMsg status;
+        status.state = LegacyTask::StatusMsg::STATE_COMPLETED;
         status.status = status_msg_cancelled;
         return status;
       });
   return obs
     .merge(rxcpp::observe_on_event_loop(), cancelled_obs)
-    .template lift<Task::StatusMsg>(grab_while_active());
+    .template lift<LegacyTask::StatusMsg>(grab_while_active());
 }
 
 } // namespace phases
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp
index 2822b206a..b19b4206d 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp
@@ -92,7 +92,7 @@ WaitForCharge::Active::Active(
 }
 
 //==============================================================================
-std::shared_ptr<Task::ActivePhase> WaitForCharge::Pending::begin()
+std::shared_ptr<LegacyTask::ActivePhase> WaitForCharge::Pending::begin()
 {
   const auto& now = std::chrono::steady_clock::now();
 
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp
index e6d8801e7..b603fcd92 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp
@@ -18,7 +18,7 @@
 #ifndef SRC__RMF_FLEET_ADAPTER__PHASES__WAITFORCHARGE_HPP
 #define SRC__RMF_FLEET_ADAPTER__PHASES__WAITFORCHARGE_HPP
 
-#include "../Task.hpp"
+#include "../LegacyTask.hpp"
 #include "../agv/RobotContext.hpp"
 
 #include <rmf_battery/agv/BatterySystem.hpp>
@@ -33,11 +33,11 @@ class WaitForCharge
 {
 public:
 
-  using StatusMsg = Task::StatusMsg;
+  using StatusMsg = LegacyTask::StatusMsg;
   class Pending;
 
   class Active
-    : public Task::ActivePhase,
+    : public LegacyTask::ActivePhase,
     public std::enable_shared_from_this<Active>
   {
   public:
@@ -79,11 +79,11 @@ class WaitForCharge
 
   };
 
-  class Pending : public Task::PendingPhase
+  class Pending : public LegacyTask::PendingPhase
   {
   public:
     // Documentation inherited
-    std::shared_ptr<Task::ActivePhase> begin() final;
+    std::shared_ptr<LegacyTask::ActivePhase> begin() final;
 
     // Documentation inherited
     rmf_traffic::Duration estimate_phase_duration() const final;
@@ -116,4 +116,4 @@ class WaitForCharge
 } // namespace phases
 } // namespace rmf_fleet_adapter
 
-#endif // SRC__RMF_FLEET_ADAPTER__PHASES__WAITFORCHARGE_HPP
\ No newline at end of file
+#endif // SRC__RMF_FLEET_ADAPTER__PHASES__WAITFORCHARGE_HPP
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp
index 90096e99d..1badab34a 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp
@@ -25,7 +25,7 @@ namespace rmf_fleet_adapter {
 namespace tasks {
 
 //==============================================================================
-std::shared_ptr<Task> make_charge_battery(
+std::shared_ptr<LegacyTask> make_charge_battery(
   const rmf_task::ConstRequestPtr request,
   const agv::RobotContextPtr& context,
   const rmf_traffic::agv::Plan::Start start,
@@ -42,7 +42,7 @@ std::shared_ptr<Task> make_charge_battery(
   rmf_traffic::agv::Planner::Goal goal{
     finish_state.dedicated_charging_waypoint().value()};
 
-  Task::PendingPhases phases;
+  LegacyTask::PendingPhases phases;
   phases.push_back(
     phases::GoToPlace::make(context, std::move(start), goal));
   phases.push_back(
@@ -51,7 +51,7 @@ std::shared_ptr<Task> make_charge_battery(
       context->task_planner()->configuration().parameters().battery_system(),
       context->task_planner()->configuration().constraints().recharge_soc()));
 
-  return Task::make(
+  return LegacyTask::make(
     request->booking()->id(),
     std::move(phases),
     context->worker(),
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp
index 81a350a56..225b6877c 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp
@@ -18,7 +18,7 @@
 #ifndef SRC__RMF_FLEET_ADAPTER__TASKS__CHARGEBATTERY_HPP
 #define SRC__RMF_FLEET_ADAPTER__TASKS__CHARGEBATTERY_HPP
 
-#include "../Task.hpp"
+#include "../LegacyTask.hpp"
 #include "../agv/RobotContext.hpp"
 
 #include <rmf_traffic/agv/Planner.hpp>
@@ -31,7 +31,7 @@ namespace rmf_fleet_adapter {
 namespace tasks {
 
 //==============================================================================
-std::shared_ptr<Task> make_charge_battery(
+std::shared_ptr<LegacyTask> make_charge_battery(
   const rmf_task::ConstRequestPtr request,
   const agv::RobotContextPtr& context,
   const rmf_traffic::agv::Plan::Start start,
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp
index d08bcdcfe..2f310e4b4 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp
@@ -23,7 +23,7 @@ namespace rmf_fleet_adapter {
 namespace tasks {
 
 //==============================================================================
-std::shared_ptr<Task> make_clean(
+std::shared_ptr<LegacyTask> make_clean(
   const rmf_task::ConstRequestPtr request,
   const agv::RobotContextPtr& context,
   const rmf_traffic::agv::Plan::Start clean_start,
@@ -45,7 +45,7 @@ std::shared_ptr<Task> make_clean(
   const auto start_waypoint = description->start_waypoint();
   const rmf_traffic::agv::Planner::Goal clean_goal{start_waypoint};
 
-  Task::PendingPhases phases;
+  LegacyTask::PendingPhases phases;
   // TODO(YV): If the robot is already at the start_waypoint, the Dock entry event will
   // not be triggered and the task will be competed without any cleaning
   // performed. To avoid this, we request the robot to re-enter the lane.
@@ -66,7 +66,7 @@ std::shared_ptr<Task> make_clean(
   phases.push_back(
     phases::GoToPlace::make(context, std::move(clean_start), clean_goal));
 
-  return Task::make(
+  return LegacyTask::make(
     request->booking()->id(),
     std::move(phases),
     context->worker(),
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp
index 1d0f3c319..7d95dc381 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp
@@ -18,7 +18,7 @@
 #ifndef SRC__RMF_FLEET_ADAPTER__TASKS__CLEAN_HPP
 #define SRC__RMF_FLEET_ADAPTER__TASKS__CLEAN_HPP
 
-#include "../Task.hpp"
+#include "../LegacyTask.hpp"
 #include "../agv/RobotContext.hpp"
 
 #include <rmf_traffic/agv/Planner.hpp>
@@ -31,7 +31,7 @@ namespace rmf_fleet_adapter {
 namespace tasks {
 
 //==============================================================================
-std::shared_ptr<Task> make_clean(
+std::shared_ptr<LegacyTask> make_clean(
   const rmf_task::ConstRequestPtr request,
   const agv::RobotContextPtr& context,
   const rmf_traffic::agv::Plan::Start clean_start,
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp
index 9fc7864b8..6fa0218f5 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp
@@ -27,7 +27,7 @@ namespace rmf_fleet_adapter {
 namespace tasks {
 
 //==============================================================================
-std::shared_ptr<Task> make_delivery(
+std::shared_ptr<LegacyTask> make_delivery(
   const rmf_task::ConstRequestPtr request,
   const agv::RobotContextPtr& context,
   const rmf_traffic::agv::Plan::Start pickup_start,
@@ -46,7 +46,7 @@ std::shared_ptr<Task> make_delivery(
   const auto pickup_waypoint = description->pickup_waypoint();
   const auto dropoff_waypoint = description->dropoff_waypoint();
 
-  Task::PendingPhases phases;
+  LegacyTask::PendingPhases phases;
   phases.push_back(
     phases::GoToPlace::make(
       context,
@@ -107,7 +107,7 @@ std::shared_ptr<Task> make_delivery(
       context->itinerary().description().owner(),
       ingestor_items));
 
-  return Task::make(
+  return LegacyTask::make(
     request->booking()->id(),
     std::move(phases),
     context->worker(),
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp
index f00a36389..18f340d66 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp
@@ -18,7 +18,7 @@
 #ifndef SRC__RMF_FLEET_ADAPTER__TASKS__DELIVERY_HPP
 #define SRC__RMF_FLEET_ADAPTER__TASKS__DELIVERY_HPP
 
-#include "../Task.hpp"
+#include "../LegacyTask.hpp"
 #include "../agv/RobotContext.hpp"
 
 #include <rmf_task/requests/Delivery.hpp>
@@ -29,7 +29,7 @@ namespace rmf_fleet_adapter {
 namespace tasks {
 
 //==============================================================================
-std::shared_ptr<Task> make_delivery(
+std::shared_ptr<LegacyTask> make_delivery(
   const rmf_task::ConstRequestPtr request,
   const agv::RobotContextPtr& context,
   const rmf_traffic::agv::Plan::Start pickup_start,
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp
index 5a78555bd..ba8e39d52 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp
@@ -23,7 +23,7 @@ namespace rmf_fleet_adapter {
 namespace tasks {
 
 //==============================================================================
-std::shared_ptr<Task> make_loop(
+std::shared_ptr<LegacyTask> make_loop(
   const rmf_task::ConstRequestPtr request,
   const agv::RobotContextPtr& context,
   const rmf_traffic::agv::Plan::Start start,
@@ -80,7 +80,7 @@ std::shared_ptr<Task> make_loop(
       orientation};
     } ();
 
-  Task::PendingPhases phases;
+  LegacyTask::PendingPhases phases;
   phases.push_back(
     phases::GoToPlace::make(
       context, std::move(start), start_waypoint));
@@ -100,7 +100,7 @@ std::shared_ptr<Task> make_loop(
         context, loop_start, finish_waypoint));
   }
 
-  return Task::make(
+  return LegacyTask::make(
     request->booking()->id(),
     std::move(phases),
     context->worker(),
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.hpp
index 3813100de..99ee01248 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.hpp
@@ -18,7 +18,7 @@
 #ifndef SRC__RMF_FLEET_ADAPTER__TASKS__LOOP_HPP
 #define SRC__RMF_FLEET_ADAPTER__TASKS__LOOP_HPP
 
-#include "../Task.hpp"
+#include "../LegacyTask.hpp"
 #include "../agv/RobotContext.hpp"
 
 #include <rmf_task/requests/Loop.hpp>
@@ -27,7 +27,7 @@ namespace rmf_fleet_adapter {
 namespace tasks {
 
 //==============================================================================
-std::shared_ptr<Task> make_loop(
+std::shared_ptr<LegacyTask> make_loop(
   const rmf_task::ConstRequestPtr request,
   const agv::RobotContextPtr& context,
   const rmf_traffic::agv::Plan::Start start,
diff --git a/rmf_fleet_adapter/test/phases/test_DispenseItem.cpp b/rmf_fleet_adapter/test/phases/test_DispenseItem.cpp
index 38f7dbfb8..877a7ad90 100644
--- a/rmf_fleet_adapter/test/phases/test_DispenseItem.cpp
+++ b/rmf_fleet_adapter/test/phases/test_DispenseItem.cpp
@@ -38,7 +38,7 @@ struct TestData
   std::list<DispenserRequest> received_requests;
 
   std::condition_variable status_updates_cv;
-  std::list<Task::StatusMsg> status_updates;
+  std::list<LegacyTask::StatusMsg> status_updates;
 
   std::optional<uint32_t> last_state_value() const
   {
@@ -141,7 +141,7 @@ SCENARIO_METHOD(MockAdapterFixture, "dispense item phase", "[phases]")
         {
           for (const auto& status : test->status_updates)
           {
-            if (status.state == Task::StatusMsg::STATE_COMPLETED)
+            if (status.state == LegacyTask::StatusMsg::STATE_COMPLETED)
               return true;
           }
           test->status_updates.clear();
@@ -204,7 +204,7 @@ SCENARIO_METHOD(MockAdapterFixture, "dispense item phase", "[phases]")
         bool completed = test->status_updates_cv.wait_for(
           lk, std::chrono::milliseconds(10), [test]()
           {
-            return test->last_state_value() == Task::StatusMsg::STATE_COMPLETED;
+            return test->last_state_value() == LegacyTask::StatusMsg::STATE_COMPLETED;
           });
         CHECK(completed);
       }
@@ -269,7 +269,7 @@ SCENARIO_METHOD(MockAdapterFixture, "dispense item phase", "[phases]")
           test->status_updates_cv.wait_for(lk, std::chrono::milliseconds(
               10), [test]()
             {
-              return test->last_state_value() == Task::StatusMsg::STATE_FAILED;
+              return test->last_state_value() == LegacyTask::StatusMsg::STATE_FAILED;
             });
         CHECK(failed);
       }
@@ -332,7 +332,7 @@ SCENARIO_METHOD(MockAdapterFixture, "dispense item phase", "[phases]")
         bool completed = test->status_updates_cv.wait_for(lk, std::chrono::milliseconds(
               10), [test]()
             {
-              return test->last_state_value() == Task::StatusMsg::STATE_COMPLETED;
+              return test->last_state_value() == LegacyTask::StatusMsg::STATE_COMPLETED;
             });
         CHECK(completed);
       }
@@ -398,7 +398,7 @@ SCENARIO_METHOD(MockAdapterFixture, "dispense item phase", "[phases]")
         bool completed = test->status_updates_cv.wait_for(lk, std::chrono::milliseconds(
               10), [test]()
             {
-              return test->last_state_value() == Task::StatusMsg::STATE_COMPLETED;
+              return test->last_state_value() == LegacyTask::StatusMsg::STATE_COMPLETED;
             });
         CHECK(!completed);
       }
diff --git a/rmf_fleet_adapter/test/phases/test_DoorClose.cpp b/rmf_fleet_adapter/test/phases/test_DoorClose.cpp
index 053c74007..0b6bf8edf 100644
--- a/rmf_fleet_adapter/test/phases/test_DoorClose.cpp
+++ b/rmf_fleet_adapter/test/phases/test_DoorClose.cpp
@@ -39,7 +39,7 @@ struct TestData
   std::list<DoorRequest::UniquePtr> received_requests;
 
   std::condition_variable status_updates_cv;
-  std::list<Task::StatusMsg> status_updates;
+  std::list<LegacyTask::StatusMsg> status_updates;
 
   std::optional<uint32_t> last_state_value() const
   {
@@ -207,7 +207,7 @@ SCENARIO_METHOD(MockAdapterFixture, "door close phase", "[phases]")
           lk, std::chrono::milliseconds(10),
           [test]()
           {
-            return test->last_state_value() == Task::StatusMsg::STATE_COMPLETED;
+            return test->last_state_value() == LegacyTask::StatusMsg::STATE_COMPLETED;
           });
         CHECK(completed);
       }
@@ -230,7 +230,7 @@ SCENARIO_METHOD(MockAdapterFixture, "door close phase", "[phases]")
         bool completed = test->status_updates_cv.wait_for(
           lk, std::chrono::milliseconds(10), [test]()
           {
-            return test->last_state_value() == Task::StatusMsg::STATE_COMPLETED;
+            return test->last_state_value() == LegacyTask::StatusMsg::STATE_COMPLETED;
           });
         CHECK(completed);
       }
@@ -254,7 +254,7 @@ SCENARIO_METHOD(MockAdapterFixture, "door close phase", "[phases]")
         bool completed = test->status_updates_cv.wait_for(
           lk, std::chrono::milliseconds(10), [test]()
           {
-            return test->last_state_value() == Task::StatusMsg::STATE_COMPLETED;
+            return test->last_state_value() == LegacyTask::StatusMsg::STATE_COMPLETED;
           });
         CHECK(!completed);
       }
@@ -271,7 +271,7 @@ SCENARIO_METHOD(MockAdapterFixture, "door close phase", "[phases]")
         {
           for (const auto& status : test->status_updates)
           {
-            if (status.state == Task::StatusMsg::STATE_COMPLETED)
+            if (status.state == LegacyTask::StatusMsg::STATE_COMPLETED)
               return true;
           }
           test->status_updates.clear();
diff --git a/rmf_fleet_adapter/test/phases/test_DoorOpen.cpp b/rmf_fleet_adapter/test/phases/test_DoorOpen.cpp
index c9e97fd08..20c79f9f9 100644
--- a/rmf_fleet_adapter/test/phases/test_DoorOpen.cpp
+++ b/rmf_fleet_adapter/test/phases/test_DoorOpen.cpp
@@ -41,7 +41,7 @@ struct TestData
   std::list<DoorRequest::UniquePtr> received_requests;
 
   std::condition_variable status_updates_cv;
-  std::list<Task::StatusMsg> status_updates;
+  std::list<LegacyTask::StatusMsg> status_updates;
 
   std::optional<uint32_t> last_state_value() const
   {
@@ -226,7 +226,7 @@ SCENARIO_METHOD(MockAdapterFixture, "door open phase", "[phases]")
           lk, std::chrono::milliseconds(10),
           [test]()
           {
-            return test->last_state_value() == Task::StatusMsg::STATE_COMPLETED;
+            return test->last_state_value() == LegacyTask::StatusMsg::STATE_COMPLETED;
           });
         CHECK(completed);
       }
@@ -250,7 +250,7 @@ SCENARIO_METHOD(MockAdapterFixture, "door open phase", "[phases]")
           lk, std::chrono::milliseconds(10),
           [test]()
           {
-            return test->last_state_value() == Task::StatusMsg::STATE_COMPLETED;
+            return test->last_state_value() == LegacyTask::StatusMsg::STATE_COMPLETED;
           });
         CHECK(!completed);
       }
@@ -274,7 +274,7 @@ SCENARIO_METHOD(MockAdapterFixture, "door open phase", "[phases]")
           lk, std::chrono::milliseconds(10),
           [test]()
           {
-            return test->last_state_value() == Task::StatusMsg::STATE_COMPLETED;
+            return test->last_state_value() == LegacyTask::StatusMsg::STATE_COMPLETED;
           });
         CHECK(!completed);
       }
diff --git a/rmf_fleet_adapter/test/phases/test_GoToPlace.cpp b/rmf_fleet_adapter/test/phases/test_GoToPlace.cpp
index b55ad55ee..2513618ac 100644
--- a/rmf_fleet_adapter/test/phases/test_GoToPlace.cpp
+++ b/rmf_fleet_adapter/test/phases/test_GoToPlace.cpp
@@ -188,12 +188,12 @@ SCENARIO_METHOD(MockAdapterFixture, "go to place negotiation", "[phases]")
 
   const auto pending_0 = phases::GoToPlace::make(context_0, {start_0}, goal_0);
   const auto active_0 = info_0.schedule_and_wait(
-    std::function<std::shared_ptr<Task::ActivePhase>()>(
+    std::function<std::shared_ptr<LegacyTask::ActivePhase>()>(
       [&]() { return pending_0->begin(); }));
 
   const auto pending_1 = phases::GoToPlace::make(context_1, {start_1}, goal_1);
   const auto active_1 = info_1.schedule_and_wait(
-    std::function<std::shared_ptr<Task::ActivePhase>()>(
+    std::function<std::shared_ptr<LegacyTask::ActivePhase>()>(
       [&]() { return pending_1->begin(); }));
 
   const auto negotiators = std::make_shared<NegotiatorMap>();
diff --git a/rmf_fleet_adapter/test/phases/test_IngestItem.cpp b/rmf_fleet_adapter/test/phases/test_IngestItem.cpp
index 719f14890..41fa5d21d 100644
--- a/rmf_fleet_adapter/test/phases/test_IngestItem.cpp
+++ b/rmf_fleet_adapter/test/phases/test_IngestItem.cpp
@@ -38,7 +38,7 @@ struct TestData
   std::list<IngestorRequest> received_requests;
 
   std::condition_variable status_updates_cv;
-  std::list<Task::StatusMsg> status_updates;
+  std::list<LegacyTask::StatusMsg> status_updates;
 
   std::optional<uint32_t> last_state_value() const
   {
@@ -147,7 +147,7 @@ SCENARIO_METHOD(MockAdapterFixture, "ingest item phase", "[phases]")
         {
           for (const auto& status : test->status_updates)
           {
-            if (status.state == Task::StatusMsg::STATE_COMPLETED)
+            if (status.state == LegacyTask::StatusMsg::STATE_COMPLETED)
               return true;
           }
           test->status_updates.clear();
@@ -202,7 +202,7 @@ SCENARIO_METHOD(MockAdapterFixture, "ingest item phase", "[phases]")
         bool completed = test->status_updates_cv.wait_for(
           lk, std::chrono::milliseconds(1000), [test]()
           {
-            return test->last_state_value() == Task::StatusMsg::STATE_COMPLETED;
+            return test->last_state_value() == LegacyTask::StatusMsg::STATE_COMPLETED;
           });
         CHECK(completed);
       }
@@ -258,7 +258,7 @@ SCENARIO_METHOD(MockAdapterFixture, "ingest item phase", "[phases]")
           test->status_updates_cv.wait_for(lk, std::chrono::milliseconds(
               1000), [test]()
             {
-              return test->last_state_value() == Task::StatusMsg::STATE_FAILED;
+              return test->last_state_value() == LegacyTask::StatusMsg::STATE_FAILED;
             });
         CHECK(failed);
       }
@@ -314,7 +314,7 @@ SCENARIO_METHOD(MockAdapterFixture, "ingest item phase", "[phases]")
         bool completed = test->status_updates_cv.wait_for(lk, std::chrono::milliseconds(
               1000), [test]()
             {
-              return test->last_state_value() == Task::StatusMsg::STATE_COMPLETED;
+              return test->last_state_value() == LegacyTask::StatusMsg::STATE_COMPLETED;
             });
         CHECK(completed);
       }
@@ -370,7 +370,7 @@ SCENARIO_METHOD(MockAdapterFixture, "ingest item phase", "[phases]")
         bool completed = test->status_updates_cv.wait_for(
           lk, std::chrono::milliseconds(30), [test]()
           {
-            return test->last_state_value() == Task::StatusMsg::STATE_COMPLETED;
+            return test->last_state_value() == LegacyTask::StatusMsg::STATE_COMPLETED;
           });
         CHECK(!completed);
       }
diff --git a/rmf_fleet_adapter/test/phases/test_RequestLift.cpp b/rmf_fleet_adapter/test/phases/test_RequestLift.cpp
index b97d7a17e..6e2a76c1f 100644
--- a/rmf_fleet_adapter/test/phases/test_RequestLift.cpp
+++ b/rmf_fleet_adapter/test/phases/test_RequestLift.cpp
@@ -38,7 +38,7 @@ struct TestData
   std::string session_id;
 
   std::condition_variable status_updates_cv;
-  std::list<Task::StatusMsg> status_updates;
+  std::list<LegacyTask::StatusMsg> status_updates;
 };
 } // anonymous namespace
 
@@ -191,7 +191,7 @@ SCENARIO_METHOD(MockAdapterFixture, "request lift phase", "[phases]")
             if (test->status_updates.empty())
               return false;
             const auto& state = test->status_updates.back().state;
-            return state == Task::StatusMsg::STATE_COMPLETED;
+            return state == LegacyTask::StatusMsg::STATE_COMPLETED;
           });
         CHECK(completed);
       }
diff --git a/rmf_fleet_adapter/test/tasks/test_Delivery.cpp b/rmf_fleet_adapter/test/tasks/test_Delivery.cpp
index 3b4b6902f..480aeb858 100644
--- a/rmf_fleet_adapter/test/tasks/test_Delivery.cpp
+++ b/rmf_fleet_adapter/test/tasks/test_Delivery.cpp
@@ -446,7 +446,7 @@ SCENARIO("Test Delivery")
   // Note: wait for task_manager to start, else TM will be suspicously "empty"
   std::this_thread::sleep_for(1s);
 
-  // Dispatch Delivery Task
+  // Dispatch Delivery LegacyTask
   rmf_task_msgs::msg::TaskProfile task_profile;
   task_profile.task_id = delivery_id;
   task_profile.description.start_time = adapter.node()->now();
diff --git a/rmf_fleet_adapter/test/tasks/test_Loop.cpp b/rmf_fleet_adapter/test/tasks/test_Loop.cpp
index 41f8b3b7a..5c4d6198b 100644
--- a/rmf_fleet_adapter/test/tasks/test_Loop.cpp
+++ b/rmf_fleet_adapter/test/tasks/test_Loop.cpp
@@ -301,7 +301,7 @@ SCENARIO("Test loop requests", "[.flaky]")
   task_profile.description.task_type.type =
     rmf_task_msgs::msg::TaskType::TYPE_LOOP;
 
-  // Dsipatch Loop 0 Task
+  // Dsipatch Loop 0 LegacyTask
   task_profile.task_id = loop_0;
   task_profile.description.loop.num_loops = n_loops;
   task_profile.description.loop.robot_type = fleet_type;
@@ -309,7 +309,7 @@ SCENARIO("Test loop requests", "[.flaky]")
   task_profile.description.loop.finish_name = east;
   adapter.dispatch_task(task_profile);
 
-  // Dispatch Loop 1 Task
+  // Dispatch Loop 1 LegacyTask
   task_profile.task_id = loop_1;
   task_profile.description.loop.start_name = north;
   task_profile.description.loop.finish_name = east;
diff --git a/rmf_fleet_adapter/test/test_Task.cpp b/rmf_fleet_adapter/test/test_Task.cpp
index 8e6dfaa7e..069ff22b2 100644
--- a/rmf_fleet_adapter/test/test_Task.cpp
+++ b/rmf_fleet_adapter/test/test_Task.cpp
@@ -17,7 +17,7 @@
 
 #include <rmf_utils/catch.hpp>
 
-#include <Task.hpp>
+#include <LegacyTask.hpp>
 
 #include "thread_cooldown.hpp"
 
@@ -67,7 +67,7 @@ class MockPhase
 
   };
 
-  class Active : public rmf_fleet_adapter::Task::ActivePhase
+  class Active : public rmf_fleet_adapter::LegacyTask::ActivePhase
   {
   public:
 
@@ -124,7 +124,7 @@ class MockPhase
   };
 
 
-  class Pending : public rmf_fleet_adapter::Task::PendingPhase
+  class Pending : public rmf_fleet_adapter::LegacyTask::PendingPhase
   {
   public:
 
@@ -141,7 +141,7 @@ class MockPhase
       // Do nothing
     }
 
-    std::shared_ptr<rmf_fleet_adapter::Task::ActivePhase> begin() final
+    std::shared_ptr<rmf_fleet_adapter::LegacyTask::ActivePhase> begin() final
     {
       return std::make_shared<Active>(_desc, _count, _count_length, _period);
     }
@@ -168,10 +168,10 @@ class MockSubtaskPhase
 {
 public:
 
-  using PendingPhases = rmf_fleet_adapter::Task::PendingPhases;
+  using PendingPhases = rmf_fleet_adapter::LegacyTask::PendingPhases;
 
   class Active
-    : public rmf_fleet_adapter::Task::ActivePhase,
+    : public rmf_fleet_adapter::LegacyTask::ActivePhase,
     public std::enable_shared_from_this<Active>
   {
   public:
@@ -182,7 +182,7 @@ class MockSubtaskPhase
       state.load_basic(
         {std::chrono::steady_clock::now(), 0, 0.0}, 0, 1.0);
       _subtasks =
-        rmf_fleet_adapter::Task::make(
+        rmf_fleet_adapter::LegacyTask::make(
           "subtasks", std::move(phases),
           rxcpp::schedulers::make_event_loop().create_worker(),
           std::chrono::steady_clock::now(),
@@ -240,7 +240,7 @@ class MockSubtaskPhase
 
   private:
 
-    std::shared_ptr<rmf_fleet_adapter::Task> _subtasks;
+    std::shared_ptr<rmf_fleet_adapter::LegacyTask> _subtasks;
     rxcpp::subscription _subscription;
     rxcpp::subjects::subject<StatusMsg> _status_publisher;
     rxcpp::observable<StatusMsg> _status_obs;
@@ -249,7 +249,7 @@ class MockSubtaskPhase
   };
 
 
-  class Pending : public rmf_fleet_adapter::Task::PendingPhase
+  class Pending : public rmf_fleet_adapter::LegacyTask::PendingPhase
   {
   public:
 
@@ -259,7 +259,7 @@ class MockSubtaskPhase
       _desc = "subtasks";
     }
 
-    std::shared_ptr<rmf_fleet_adapter::Task::ActivePhase> begin() final
+    std::shared_ptr<rmf_fleet_adapter::LegacyTask::ActivePhase> begin() final
     {
       auto active = std::make_shared<Active>(std::move(_phases));
       active->begin();
@@ -291,7 +291,7 @@ SCENARIO("Test simple task")
   std::shared_ptr<std::size_t> count = std::make_shared<std::size_t>(0);
   const auto dt = 10ms;
 
-  rmf_fleet_adapter::Task::PendingPhases phases;
+  rmf_fleet_adapter::LegacyTask::PendingPhases phases;
   phases.push_back(std::make_unique<MockPhase::Pending>("A", count, 5, dt));
   phases.push_back(std::make_unique<MockPhase::Pending>("B", count, 15, dt));
   phases.push_back(std::make_unique<MockPhase::Pending>("C", count, 18, dt));
@@ -301,8 +301,8 @@ SCENARIO("Test simple task")
   rmf_task::State finish_state;
   finish_state.load_basic({deployment_time, 0, 0.0}, 0, 1.0);
 
-  std::shared_ptr<rmf_fleet_adapter::Task> task =
-    rmf_fleet_adapter::Task::make(
+  std::shared_ptr<rmf_fleet_adapter::LegacyTask> task =
+    rmf_fleet_adapter::LegacyTask::make(
     "test_Task", std::move(phases),
     rxcpp::schedulers::make_event_loop().create_worker(), deployment_time,
     finish_state, nullptr);
@@ -311,7 +311,7 @@ SCENARIO("Test simple task")
   auto completed_future = completed_promise.get_future();
   auto status_sub = task->observe()
     .subscribe(
-    [](const rmf_fleet_adapter::Task::StatusMsg& msg)
+    [](const rmf_fleet_adapter::LegacyTask::StatusMsg& msg)
     {
       if (msg.status.find("A") != std::string::npos)
       {
@@ -348,7 +348,7 @@ SCENARIO("Test nested task")
 {
   rmf_fleet_adapter_test::thread_cooldown = true;
 
-  using PendingPhases = rmf_fleet_adapter::Task::PendingPhases;
+  using PendingPhases = rmf_fleet_adapter::LegacyTask::PendingPhases;
 
   std::shared_ptr<std::size_t> count = std::make_shared<std::size_t>(0);
   const auto dt = 10ms;
@@ -402,7 +402,7 @@ SCENARIO("Test nested task")
   rmf_task::State finish_state;
   finish_state.load_basic({deployment_time, 0, 0.0}, 0, 1.0);
 
-  const auto task = rmf_fleet_adapter::Task::make(
+  const auto task = rmf_fleet_adapter::LegacyTask::make(
     "test_NestedTask", std::move(phases),
     rxcpp::schedulers::make_event_loop().create_worker(), deployment_time,
     finish_state, nullptr);
@@ -411,7 +411,7 @@ SCENARIO("Test nested task")
   auto completed_future = completed_promise.get_future();
   auto status_sub = task->observe()
     .subscribe(
-    [&count_limits](const rmf_fleet_adapter::Task::StatusMsg& msg)
+    [&count_limits](const rmf_fleet_adapter::LegacyTask::StatusMsg& msg)
     {
       std::pair<std::size_t, std::size_t> limits = {0, 0};
       for (const auto& element : count_limits)

From d62e94c8d70a3fcbd0ee2bdd9122ec6dfd58029c Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Tue, 14 Dec 2021 18:32:12 +0800
Subject: [PATCH 17/79] Implementing the GoToPlace event

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 .../src/rmf_fleet_adapter/Negotiator.cpp      | 103 ++++++++++++
 .../src/rmf_fleet_adapter/Negotiator.hpp      |  68 ++++++++
 .../rmf_fleet_adapter/agv/RobotContext.cpp    |  13 ++
 .../rmf_fleet_adapter/agv/RobotContext.hpp    |  14 ++
 .../rmf_fleet_adapter/events/GoToPlace.cpp    | 154 ++++++++++++++++++
 .../rmf_fleet_adapter/events/GoToPlace.hpp    | 128 +++++++++++++++
 .../events/LegacyPhaseShim.cpp                |  42 ++++-
 .../rmf_fleet_adapter/phases/DockRobot.cpp    |   3 +-
 .../rmf_fleet_adapter/phases/DockRobot.hpp    |   1 +
 9 files changed, 519 insertions(+), 7 deletions(-)
 create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/Negotiator.cpp
 create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/Negotiator.hpp
 create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp
 create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/Negotiator.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/Negotiator.cpp
new file mode 100644
index 000000000..4524e89c5
--- /dev/null
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/Negotiator.cpp
@@ -0,0 +1,103 @@
+/*
+ * 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 "Negotiator.hpp"
+
+namespace rmf_fleet_adapter {
+
+//==============================================================================
+std::shared_ptr<Negotiator> Negotiator::make(
+  agv::RobotContextPtr context,
+  Respond respond)
+{
+  auto negotiator = std::make_shared<Negotiator>();
+  negotiator->_context = std::move(context);
+  negotiator->_license = negotiator->_context->set_negotiator(negotiator.get());
+  negotiator->_respond = std::move(respond);
+
+  return negotiator;
+}
+
+//==============================================================================
+void Negotiator::respond(
+  const TableViewerPtr& table_viewer,
+  const ResponderPtr& responder)
+{
+  auto service = _respond(table_viewer, responder);
+  if (!service)
+  {
+    // If we didn't get a service back then we will trust that the response was
+    // already submitted upstream.
+    return;
+  }
+
+  auto negotiate_sub =
+    rmf_rxcpp::make_job<services::Negotiate::Result>(service)
+    .observe_on(rxcpp::identity_same_worker(_context->worker()))
+    .subscribe(
+      [w = weak_from_this()](const auto& result)
+      {
+        if (auto self = w.lock())
+        {
+          result.respond();
+          self->_negotiate_services.erase(result.service);
+        }
+        else
+        {
+          // We need to make sure we respond in some way so that we don't risk
+          // making a negotiation hang forever. If this task is dead, then we
+          // should at least respond by forfeiting.
+          const auto service = result.service;
+          const auto responder = service->responder();
+          responder->forfeit({});
+        }
+      });
+
+  using namespace std::chrono_literals;
+  const auto wait_duration = 2s + table_viewer->sequence().back().version * 10s;
+  auto negotiation_timer = _context->node()->try_create_wall_timer(
+    wait_duration,
+    [s = service->weak_from_this()]
+    {
+      if (const auto service = s.lock())
+        service->interrupt();
+    });
+
+  _negotiate_services[service] =
+    NegotiationManagers{
+      std::move(negotiate_sub),
+      std::move(negotiation_timer)
+    };
+}
+
+//==============================================================================
+services::ProgressEvaluator Negotiator::make_evaluator(
+  const TableViewerPtr& table_viewer)
+{
+  services::ProgressEvaluator evaluator;
+  if (table_viewer->parent_id())
+  {
+    const auto& s = table_viewer->sequence();
+    assert(s.size() >= 2);
+    evaluator.compliant_leeway_base *= s[s.size()-2].version + 1;
+    evaluator.max_cost_threshold = 90.0 + 30.0*s[s.size()-2].version;
+  }
+
+  return evaluator;
+}
+
+} // namespace rmf_fleet_adapter
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/Negotiator.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/Negotiator.hpp
new file mode 100644
index 000000000..03d0e4c21
--- /dev/null
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/Negotiator.hpp
@@ -0,0 +1,68 @@
+/*
+ * 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 SRC__RMF_FLEET_ADAPTER__NEGOTIATOR_HPP
+#define SRC__RMF_FLEET_ADAPTER__NEGOTIATOR_HPP
+
+#include <rmf_traffic/schedule/Negotiator.hpp>
+
+#include "agv/RobotContext.hpp"
+#include "services/Negotiate.hpp"
+
+namespace rmf_fleet_adapter {
+
+//==============================================================================
+class Negotiator
+  : public rmf_traffic::schedule::Negotiator,
+    std::enable_shared_from_this<Negotiator>
+{
+public:
+
+  using NegotiatePtr = std::shared_ptr<services::Negotiate>;
+  using Respond =
+    std::function<NegotiatePtr(const TableViewerPtr&, const ResponderPtr&)>;
+
+  static std::shared_ptr<Negotiator> make(
+    agv::RobotContextPtr context,
+    Respond respond);
+
+  void respond(
+    const TableViewerPtr& table_viewer,
+    const ResponderPtr& responder) final;
+
+  static services::ProgressEvaluator make_evaluator(
+    const TableViewerPtr& table_viewer);
+
+private:
+
+  struct NegotiationManagers
+  {
+    rmf_rxcpp::subscription_guard subscription;
+    rclcpp::TimerBase::SharedPtr timer;
+  };
+  using NegotiateServiceMap =
+    std::unordered_map<NegotiatePtr, NegotiationManagers>;
+  NegotiateServiceMap _negotiate_services;
+
+  agv::RobotContextPtr _context;
+  std::shared_ptr<void> _license;
+  Respond _respond;
+};
+
+} // namespace rmf_fleet_adapter
+
+#endif // SRC__RMF_FLEET_ADAPTER__NEGOTIATOR_HPP
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp
index 53e0caf5e..7d6ca21ba 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp
@@ -170,6 +170,18 @@ auto RobotContext::set_negotiator(
     shared_from_this(), negotiator);
 }
 
+//==============================================================================
+std::shared_ptr<void> RobotContext::be_stubborn()
+{
+  return _stubbornness;
+}
+
+//==============================================================================
+bool RobotContext::is_stubborn() const
+{
+  return _stubbornness.use_count() > 1;
+}
+
 //==============================================================================
 const rxcpp::observable<RobotContext::Empty>&
 RobotContext::observe_interrupt() const
@@ -335,6 +347,7 @@ RobotContext::RobotContext(
   _itinerary(std::move(itinerary)),
   _schedule(std::move(schedule)),
   _planner(std::move(planner)),
+  _stubbornness(std::make_shared<int>(0)),
   _node(std::move(node)),
   _worker(worker),
   _maximum_delay(maximum_delay),
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp
index c0ac0209d..b3fd86ff7 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp
@@ -90,6 +90,13 @@ class RobotContext
   std::shared_ptr<NegotiatorLicense> set_negotiator(
     rmf_traffic::schedule::Negotiator* negotiator);
 
+  /// This function will indicate that GoToPlace should have a stubborn
+  /// negotiation behavior for as long as the returned handle is alive.
+  std::shared_ptr<void> be_stubborn();
+
+  /// If anything is holding onto a be_stubborn handle, this will return true.
+  bool is_stubborn() const;
+
   struct Empty {};
   const rxcpp::observable<Empty>& observe_interrupt() const;
 
@@ -179,6 +186,7 @@ class RobotContext
   std::shared_ptr<const rmf_traffic::Profile> _profile;
 
   std::shared_ptr<void> _negotiation_license;
+  std::shared_ptr<void> _stubbornness;
 
   rxcpp::subjects::subject<Empty> _interrupt_publisher;
   rxcpp::observable<Empty> _interrupt_obs;
@@ -207,6 +215,12 @@ class RobotContext
 using RobotContextPtr = std::shared_ptr<RobotContext>;
 using ConstRobotContextPtr = std::shared_ptr<const RobotContext>;
 
+//==============================================================================
+struct GetContext
+{
+  RobotContextPtr value;
+};
+
 } // namespace agv
 } // namespace rmf_fleet_adapter
 
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp
new file mode 100644
index 000000000..bd37475d1
--- /dev/null
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp
@@ -0,0 +1,154 @@
+/*
+ * 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 "GoToPlace.hpp"
+
+#include <rmf_traffic/schedule/StubbornNegotiator.hpp>
+
+namespace rmf_fleet_adapter {
+namespace events {
+
+//==============================================================================
+auto GoToPlace::Standby::make(
+  const AssignIDPtr& id,
+  const std::function<rmf_task::State()>& get_state,
+  const rmf_task::ConstParametersPtr& parameters,
+  const rmf_task_sequence::events::GoToPlace::Description& description,
+  std::function<void()> update,
+  std::optional<rmf_traffic::Duration> tail_period)
+-> std::shared_ptr<Standby>
+{
+  const auto state = get_state();
+  const auto context = state.get<agv::GetContext>()->value;
+  const auto header = description.generate_header(state, *parameters);
+
+  auto standby = std::make_shared<Standby>();
+  standby->_assign_id = id;
+  standby->_context = context;
+  standby->_goal = description.destination();
+  standby->_time_estimate = header.original_duration_estimate();
+  standby->_tail_period = tail_period;
+  standby->_update = std::move(update);
+  standby->_state = rmf_task::events::SimpleEventState::make(
+    id->assign(),
+    header.category(),
+    header.detail(),
+    rmf_task::Event::Status::Standby);
+
+  return standby;
+}
+
+//==============================================================================
+auto GoToPlace::Standby::state() const -> ConstStatePtr
+{
+  return _state;
+}
+
+//==============================================================================
+rmf_traffic::Duration GoToPlace::Standby::duration_estimate() const
+{
+  return _time_estimate;
+}
+
+//==============================================================================
+auto GoToPlace::Standby::begin(
+  std::function<void()>,
+  std::function<void()> finished) -> ActivePtr
+{
+  if (!_active)
+  {
+    _active = Active::make(
+      _assign_id,
+      _context,
+      _goal,
+      _tail_period,
+      _state,
+      _update,
+      std::move(finished));
+  }
+
+  return _active;
+}
+
+//==============================================================================
+auto GoToPlace::Active::make(
+  const AssignIDPtr& id,
+  agv::RobotContextPtr context,
+  rmf_traffic::agv::Plan::Goal goal,
+  std::optional<rmf_traffic::Duration> tail_period,
+  rmf_task::events::SimpleEventStatePtr state,
+  std::function<void()> update,
+  std::function<void()> finished) -> std::shared_ptr<Active>
+{
+  auto active = std::make_shared<Active>();
+  active->_assign_id = id;
+  active->_context = std::move(context);
+  active->_goal = std::move(goal);
+  active->_tail_period = tail_period;
+  active->_update = std::move(update);
+  active->_finished = std::move(finished);
+  active->_state = std::move(state);
+  active->_negotiator =
+    Negotiator::make(
+      active->_context,
+      [w = active->weak_from_this()](
+        const auto& t, const auto& r) -> Negotiator::NegotiatePtr
+      {
+        if (const auto self = w.lock())
+          return self->_respond(t, r);
+
+        r->forfeit({});
+        return nullptr;
+      });
+
+  active->_find_plan();
+  return active;
+}
+
+//==============================================================================
+Negotiator::NegotiatePtr GoToPlace::Active::_respond(
+  const Negotiator::TableViewerPtr& table_view,
+  const Negotiator::ResponderPtr& responder)
+{
+  if (_context->is_stubborn())
+  {
+    rmf_traffic::schedule::StubbornNegotiator(_context->itinerary())
+    .respond(table_view, responder);
+    return nullptr;
+  }
+
+  auto approval_cb = [w = weak_from_this()](
+    const rmf_traffic::agv::Plan& plan)
+    -> std::optional<rmf_traffic::schedule::ItineraryVersion>
+    {
+      if (auto self = w.lock())
+      {
+        self->_execute_plan(plan);
+        return self->_context->itinerary().version();
+      }
+
+      return std::nullopt;
+    };
+
+  const auto evaluator = Negotiator::make_evaluator(table_view);
+  return services::Negotiate::path(
+    _context->planner(), _context->location(), _goal, table_view,
+    responder, std::move(approval_cb), std::move(evaluator));
+}
+
+} // namespace events
+} // namespace rmf_fleet_adapter
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp
new file mode 100644
index 000000000..e974d47c2
--- /dev/null
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp
@@ -0,0 +1,128 @@
+/*
+ * 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 SRC__RMF_FLEET_ADAPTER__EVENTS__GOTOPLACE_HPP
+#define SRC__RMF_FLEET_ADAPTER__EVENTS__GOTOPLACE_HPP
+
+#include "../agv/RobotContext.hpp"
+#include "../Negotiator.hpp"
+
+#include "../services/FindPath.hpp"
+
+#include <rmf_task_sequence/Event.hpp>
+#include <rmf_task_sequence/events/GoToPlace.hpp>
+#include <rmf_task/events/SimpleEventState.hpp>
+
+#include <rmf_task_sequence/events/Bundle.hpp>
+
+namespace rmf_fleet_adapter {
+namespace events {
+
+//==============================================================================
+class GoToPlace : public rmf_task_sequence::Event
+{
+public:
+
+  class Standby : public rmf_task_sequence::Event::Standby
+  {
+  public:
+
+    static std::shared_ptr<Standby> make(
+      const AssignIDPtr& id,
+      const std::function<rmf_task::State()>& get_state,
+      const rmf_task::ConstParametersPtr& parameters,
+      const rmf_task_sequence::events::GoToPlace::Description& description,
+      std::function<void()> update,
+      std::optional<rmf_traffic::Duration> tail_period = std::nullopt);
+
+    ConstStatePtr state() const final;
+
+    rmf_traffic::Duration duration_estimate() const final;
+
+    ActivePtr begin(
+      std::function<void()> checkpoint,
+      std::function<void()> finished) final;
+
+  private:
+    AssignIDPtr _assign_id;
+    agv::RobotContextPtr _context;
+    rmf_traffic::agv::Plan::Goal _goal;
+    rmf_traffic::Duration _time_estimate;
+    std::optional<rmf_traffic::Duration> _tail_period;
+    std::function<void()> _update;
+    rmf_task::events::SimpleEventStatePtr _state;
+    ActivePtr _active = nullptr;
+  };
+
+  class Active
+    : public rmf_task_sequence::Event::Active,
+      public std::enable_shared_from_this<Active>
+  {
+  public:
+
+    static std::shared_ptr<Active> make(
+      const AssignIDPtr& id,
+      agv::RobotContextPtr context,
+      rmf_traffic::agv::Plan::Goal goal,
+      std::optional<rmf_traffic::Duration> tail_period,
+      rmf_task::events::SimpleEventStatePtr state,
+      std::function<void()> update,
+      std::function<void()> finished);
+
+    ConstStatePtr state() const final;
+
+    rmf_traffic::Duration remaining_time_estimate() const final;
+
+    Backup backup() const final;
+
+    Resume interrupt(std::function<void()> task_is_interrupted) final;
+
+    void cancel() final;
+
+    void kill() final;
+
+  private:
+
+    void _find_plan();
+
+    void _execute_plan(rmf_traffic::agv::Plan new_plan);
+
+    Negotiator::NegotiatePtr _respond(
+      const Negotiator::TableViewerPtr& table_view,
+      const Negotiator::ResponderPtr& responder);
+
+    AssignIDPtr _assign_id;
+    agv::RobotContextPtr _context;
+    rmf_traffic::agv::Plan::Goal _goal;
+    std::optional<rmf_traffic::Duration> _tail_period;
+    std::function<void()> _update;
+    std::function<void()> _finished;
+    rmf_task::events::SimpleEventStatePtr _state;
+    std::shared_ptr<Negotiator> _negotiator;
+    std::optional<rmf_traffic::agv::Plan> _plan;
+    rmf_task_sequence::Event::ActivePtr _sequence;
+    std::shared_ptr<services::FindPath> _find_path_service;
+    rclcpp::TimerBase::SharedPtr _find_path_timer;
+
+    bool _is_interrupted = false;
+  };
+};
+
+} // namespace events
+} // namespace rmf_fleet_adapter
+
+#endif // SRC__RMF_FLEET_ADAPTER__EVENTS__GOTOPLACE_HPP
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.cpp
index 7c0ade7bb..165c10ca4 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.cpp
@@ -37,6 +37,8 @@ auto LegacyPhaseShim::Standby::make(
     rmf_task::Event::Status::Standby);
 
   standby->_parent_update = std::move(parent_update);
+
+  return standby;
 }
 
 //==============================================================================
@@ -48,7 +50,7 @@ auto LegacyPhaseShim::Standby::state() const -> ConstStatePtr
 //==============================================================================
 rmf_traffic::Duration LegacyPhaseShim::Standby::duration_estimate() const
 {
-  _legacy->estimate_phase_duration();
+  return _legacy->estimate_phase_duration();
 }
 
 //==============================================================================
@@ -136,9 +138,15 @@ auto LegacyPhaseShim::Active::make(
       if (const auto self = w.lock())
       {
         if (self->_finished)
-          self->_finished();
-
-        self->_finished = nullptr;
+        {
+          // We don't change the event status here because that should have been
+          // done in the task summary update, and from here we don't know what
+          // kind of finish the legacy phase may have had (e.g. completed,
+          // failed, or canceled).
+          const auto finished = self->_finished;
+          self->_finished = nullptr;
+          finished();
+        }
       }
     });
 
@@ -165,10 +173,32 @@ auto LegacyPhaseShim::Active::backup() const -> Backup
 }
 
 //==============================================================================
-auto LegacyPhaseShim::Active::interrupt(
-  std::function<void()> task_is_interrupted) -> Resume
+auto LegacyPhaseShim::Active::interrupt(std::function<void()>) -> Resume
 {
+  // Legacy phases cannot be interrupted. We need to let the phase just run to
+  // completion. We will never be able to safely trigger the task_is_interrupted
+  // callback.
+  return Resume::make([](){ /* do nothing */});
+}
 
+//==============================================================================
+void LegacyPhaseShim::Active::cancel()
+{
+  _legacy->cancel();
+}
+
+//==============================================================================
+void LegacyPhaseShim::Active::kill()
+{
+  // If we get a kill command then we'll just give up on whatever we're supposed
+  // to do without concern for consequences. When an operator kills a task, it's
+  // up to the operator to sort out the consequences.
+  if (_finished)
+  {
+    const auto finished = _finished;
+    _finished = nullptr;
+    finished();
+  }
 }
 
 } // namespace events
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.cpp
index fbeb15e11..6f97efb97 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.cpp
@@ -25,7 +25,8 @@ DockRobot::ActivePhase::ActivePhase(
   agv::RobotContextPtr context,
   std::string dock_name)
 : _context{std::move(context)},
-  _dock_name{std::move(dock_name)}
+  _dock_name{std::move(dock_name)},
+  _be_stubborn(_context->be_stubborn())
 {
   std::ostringstream oss;
   oss << "Docking robot to " << _dock_name;
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.hpp
index 9d49cfd45..f76a9d90b 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.hpp
@@ -55,6 +55,7 @@ struct DockRobot
     std::string _description;
     std::shared_ptr<Action> _action;
     rxcpp::observable<LegacyTask::StatusMsg> _obs;
+    std::shared_ptr<void> _be_stubborn;
   };
 
   class PendingPhase : public LegacyTask::PendingPhase

From 1e0bfb9eb249abc2461476d7607cf919446a5c5c Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Wed, 15 Dec 2021 18:59:30 +0800
Subject: [PATCH 18/79] Finish implementation of GoToPlace and
 EmergencyPullover

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 .../src/rmf_fleet_adapter/Negotiator.cpp      |  14 +-
 .../src/rmf_fleet_adapter/Negotiator.hpp      |   4 +
 .../rmf_fleet_adapter/agv/RobotContext.cpp    |  11 +
 .../rmf_fleet_adapter/agv/RobotContext.hpp    |   3 +
 .../events/EmergencyPullover.cpp              | 319 +++++++++++
 .../events/EmergencyPullover.hpp              | 122 ++++
 .../rmf_fleet_adapter/events/ExecutePlan.cpp  | 520 ++++++++++++++++++
 .../rmf_fleet_adapter/events/ExecutePlan.hpp  |  49 ++
 .../rmf_fleet_adapter/events/GoToPlace.cpp    | 252 ++++++++-
 .../rmf_fleet_adapter/events/GoToPlace.hpp    |  24 +-
 .../events/LegacyPhaseShim.cpp                |   9 +-
 .../events/LegacyPhaseShim.hpp                |   7 +-
 .../rmf_fleet_adapter/phases/DoorClose.hpp    |   5 +
 .../src/rmf_fleet_adapter/phases/DoorOpen.hpp |   5 +
 .../phases/EndLiftSession.hpp                 |  10 +
 .../rmf_fleet_adapter/phases/RequestLift.hpp  |   5 +
 .../rmf_fleet_adapter/services/FindPath.hpp   |   2 +-
 17 files changed, 1340 insertions(+), 21 deletions(-)
 create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp
 create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.hpp
 create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp
 create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.hpp

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/Negotiator.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/Negotiator.cpp
index 4524e89c5..2e05056e1 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/Negotiator.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/Negotiator.cpp
@@ -26,8 +26,8 @@ std::shared_ptr<Negotiator> Negotiator::make(
 {
   auto negotiator = std::make_shared<Negotiator>();
   negotiator->_context = std::move(context);
-  negotiator->_license = negotiator->_context->set_negotiator(negotiator.get());
   negotiator->_respond = std::move(respond);
+  negotiator->claim_license();
 
   return negotiator;
 }
@@ -100,4 +100,16 @@ services::ProgressEvaluator Negotiator::make_evaluator(
   return evaluator;
 }
 
+//==============================================================================
+void Negotiator::clear_license()
+{
+  _license = nullptr;
+}
+
+//==============================================================================
+void Negotiator::claim_license()
+{
+  _license = _context->set_negotiator(this);
+}
+
 } // namespace rmf_fleet_adapter
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/Negotiator.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/Negotiator.hpp
index 03d0e4c21..1974dbf7c 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/Negotiator.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/Negotiator.hpp
@@ -47,6 +47,10 @@ class Negotiator
   static services::ProgressEvaluator make_evaluator(
     const TableViewerPtr& table_viewer);
 
+  void clear_license();
+
+  void claim_license();
+
 private:
 
   struct NegotiationManagers
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp
index 7d6ca21ba..45da69d7a 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp
@@ -68,6 +68,17 @@ rmf_traffic::Time RobotContext::now() const
   return rmf_traffic_ros2::convert(_node->now());
 }
 
+//==============================================================================
+std::function<std::chrono::system_clock::time_point()>
+RobotContext::clock() const
+{
+  return [self = shared_from_this()]()
+    {
+      return std::chrono::system_clock::time_point(
+        std::chrono::system_clock::duration(self->node()->now().nanoseconds()));
+    };
+}
+
 //==============================================================================
 const std::vector<rmf_traffic::agv::Plan::Start>& RobotContext::location() const
 {
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp
index b3fd86ff7..b910445d1 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp
@@ -49,6 +49,9 @@ class RobotContext
   /// Get the current time
   rmf_traffic::Time now() const;
 
+  /// Get a clock that can be used by task loggers
+  std::function<std::chrono::system_clock::time_point()> clock() const;
+
   /// This is the current "location" of the robot, which can be used to initiate
   /// a planning job
   const std::vector<rmf_traffic::agv::Plan::Start>& location() const;
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp
new file mode 100644
index 000000000..121fac64a
--- /dev/null
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp
@@ -0,0 +1,319 @@
+/*
+ * 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 "EmergencyPullover.hpp"
+
+#include <rmf_traffic/schedule/StubbornNegotiator.hpp>
+
+namespace rmf_fleet_adapter {
+namespace events {
+
+//==============================================================================
+auto EmergencyPullover::Standby::make(
+  const AssignIDPtr& id,
+  const agv::RobotContextPtr& context,
+  std::function<void()> update) -> std::shared_ptr<Standby>
+{
+  auto standby = std::make_shared<Standby>();
+  standby->_assign_id = id;
+  standby->_context = context;
+  standby->_update = std::move(update);
+  standby->_state = rmf_task::events::SimpleEventState::make(
+    id->assign(),
+    "Emergency pullover",
+    "",
+    rmf_task::Event::Status::Standby,
+    {},
+    context->clock());
+
+  return standby;
+}
+
+//==============================================================================
+auto EmergencyPullover::Standby::state() const -> ConstStatePtr
+{
+  return _state;
+}
+
+//==============================================================================
+rmf_traffic::Duration EmergencyPullover::Standby::duration_estimate() const
+{
+  // An emergency pullover will last indefinitely until it gets cancelled, which
+  // may happen at any time.
+  return rmf_traffic::Duration(0);
+}
+
+//==============================================================================
+auto EmergencyPullover::Standby::begin(
+  std::function<void()>,
+  std::function<void()> finished) -> ActivePtr
+{
+  if (!_active)
+  {
+    _active = Active::make(
+      _assign_id,
+      _context,
+      _state,
+      _update,
+      std::move(finished));
+  }
+
+  return _active;
+}
+
+//==============================================================================
+auto EmergencyPullover::Active::make(
+  const AssignIDPtr& id,
+  agv::RobotContextPtr context,
+  rmf_task::events::SimpleEventStatePtr state,
+  std::function<void()> update,
+  std::function<void()> finished) -> std::shared_ptr<Active>
+{
+  auto active = std::make_shared<Active>();
+  active->_assign_id = id;
+  active->_context = std::move(context);
+  active->_update = std::move(update);
+  active->_finished = std::move(finished);
+  active->_state = std::move(state);
+  active->_negotiator =
+    Negotiator::make(
+      active->_context,
+      [w = active->weak_from_this()](
+        const auto& t, const auto& r) -> Negotiator::NegotiatePtr
+      {
+        if (const auto self = w.lock())
+          return self->_respond(t, r);
+
+        r->forfeit({});
+        return nullptr;
+      });
+
+  active->_find_plan();
+  return active;
+}
+
+//==============================================================================
+auto EmergencyPullover::Active::state() const -> ConstStatePtr
+{
+  return _state;
+}
+
+//==============================================================================
+rmf_traffic::Duration EmergencyPullover::Active::remaining_time_estimate() const
+{
+  // An emergency pullover will last indefinitely until it gets cancelled, which
+  // may happen at any time.
+  return rmf_traffic::Duration(0);
+}
+
+//==============================================================================
+auto EmergencyPullover::Active::backup() const -> Backup
+{
+  // EmergencyPullover doesn't need to be backed up
+  return Backup::make(0, nlohmann::json());
+}
+
+//==============================================================================
+auto EmergencyPullover::Active::interrupt(
+  std::function<void()> task_is_interrupted) -> Resume
+{
+  _negotiator->clear_license();
+  _is_interrupted = true;
+  _execution = std::nullopt;
+
+  _state->update_status(Status::Standby);
+  _state->update_log().info("Going into standby for an interruption");
+  _state->update_dependencies({});
+
+  _context->worker().schedule(
+    [task_is_interrupted](const auto&)
+    {
+      task_is_interrupted();
+    });
+
+  return Resume::make(
+    [w = weak_from_this()]()
+    {
+      if (const auto self = w.lock())
+      {
+        self->_negotiator->claim_license();
+        self->_is_interrupted = false;
+        self->_find_plan();
+      }
+    });
+}
+
+//==============================================================================
+void EmergencyPullover::Active::cancel()
+{
+  _execution = std::nullopt;
+  _state->update_status(Status::Canceled);
+  _state->update_log().info("Received signal to cancel");
+  _finished();
+}
+
+//==============================================================================
+void EmergencyPullover::Active::kill()
+{
+  _execution = std::nullopt;
+  _state->update_status(Status::Killed);
+  _state->update_log().info("Received signal to kill");
+  _finished();
+}
+
+//==============================================================================
+void EmergencyPullover::Active::_find_plan()
+{
+  if (_is_interrupted)
+    return;
+
+  _state->update_status(Status::Underway);
+  _state->update_log().info("Searching for an emergency pullover");
+
+  _find_pullover_service = std::make_shared<services::FindEmergencyPullover>(
+    _context->planner(), _context->location(), _context->schedule()->snapshot(),
+    _context->itinerary().id(), _context->profile());
+
+  _pullover_subscription =
+    rmf_rxcpp::make_job<services::FindEmergencyPullover::Result>(
+      _find_pullover_service)
+    .observe_on(rxcpp::identity_same_worker(_context->worker()))
+    .subscribe(
+    [w = weak_from_this()](
+      const services::FindEmergencyPullover::Result& result)
+    {
+      const auto self = w.lock();
+      if (!self)
+        return;
+
+      if (!result)
+      {
+        // The planner could not find any pullover
+        self->_state->update_status(Status::Error);
+        self->_state->update_log().error("Failed to find a pullover");
+
+        self->_execution = std::nullopt;
+        self->_schedule_retry();
+
+        self->_context->worker().schedule(
+          [update = self->_update](const auto&){ update(); });
+
+        return;
+      }
+
+      self->_state->update_status(Status::Underway);
+      self->_state->update_log().info("Found an emergency pullover");
+
+      self->_execute_plan(*std::move(result));
+      self->_find_pullover_service = nullptr;
+      self->_retry_timer = nullptr;
+    });
+
+  _find_pullover_timeout = _context->node()->try_create_wall_timer(
+    std::chrono::seconds(10),
+    [
+      weak_service = _find_pullover_service->weak_from_this(),
+      weak_self = weak_from_this()
+    ]()
+    {
+      if (const auto service = weak_service.lock())
+        service->interrupt();
+
+      if (const auto self = weak_self.lock())
+        self->_find_pullover_timeout = nullptr;
+    });
+
+  _update();
+}
+
+//==============================================================================
+void EmergencyPullover::Active::_schedule_retry()
+{
+  if (_retry_timer)
+    return;
+
+  // TODO(MXG): Make the retry timing configurable
+  _retry_timer = _context->node()->try_create_wall_timer(
+    std::chrono::seconds(5),
+    [w = weak_from_this()]()
+    {
+      const auto self = w.lock();
+      if (!self)
+        return;
+
+      self->_retry_timer = nullptr;
+      if (self->_execution.has_value())
+        return;
+
+      self->_find_plan();
+    });
+}
+
+//==============================================================================
+void EmergencyPullover::Active::_execute_plan(rmf_traffic::agv::Plan plan)
+{
+  if (_is_interrupted)
+    return;
+
+  _execution = ExecutePlan::make(
+    _context, std::move(plan), _assign_id, _state,
+    _update, _finished, std::nullopt);
+
+  if (!_execution.has_value())
+  {
+    _state->update_status(Status::Error);
+    _state->update_log().error(
+      "Invalid (empty) plan generated. Will retry soon. "
+      "Please report this incident to the Open-RMF developers.");
+    _schedule_retry();
+  }
+}
+
+//==============================================================================
+Negotiator::NegotiatePtr EmergencyPullover::Active::_respond(
+  const Negotiator::TableViewerPtr& table_view,
+  const Negotiator::ResponderPtr& responder)
+{
+  if (_context->is_stubborn())
+  {
+    rmf_traffic::schedule::StubbornNegotiator(_context->itinerary())
+    .respond(table_view, responder);
+    return nullptr;
+  }
+
+  auto approval_cb = [w = weak_from_this()](
+    const rmf_traffic::agv::Plan& plan)
+    -> std::optional<rmf_traffic::schedule::ItineraryVersion>
+    {
+      if (auto self = w.lock())
+      {
+        self->_execute_plan(plan);
+        return self->_context->itinerary().version();
+      }
+
+      return std::nullopt;
+    };
+
+  const auto evaluator = Negotiator::make_evaluator(table_view);
+  return services::Negotiate::emergency_pullover(
+    _context->planner(), _context->location(), table_view,
+    responder, std::move(approval_cb), std::move(evaluator));
+}
+
+
+} // namespace events
+} // namespace rmf_fleet_adapter
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.hpp
new file mode 100644
index 000000000..c6ca1db58
--- /dev/null
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.hpp
@@ -0,0 +1,122 @@
+/*
+ * 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 SRC__RMF_FLEET_ADAPTER__EVENTS__EMERGENCYPULLOVER_HPP
+#define SRC__RMF_FLEET_ADAPTER__EVENTS__EMERGENCYPULLOVER_HPP
+
+#include "../agv/RobotContext.hpp"
+#include "../Negotiator.hpp"
+
+#include "../services/FindEmergencyPullover.hpp"
+
+#include "ExecutePlan.hpp"
+
+#include <rmf_task_sequence/Event.hpp>
+#include <rmf_task/events/SimpleEventState.hpp>
+
+namespace rmf_fleet_adapter {
+namespace events {
+
+//==============================================================================
+class EmergencyPullover : public rmf_task_sequence::Event
+{
+public:
+
+  class Standby : public rmf_task_sequence::Event::Standby
+  {
+  public:
+
+    static std::shared_ptr<Standby> make(
+      const AssignIDPtr& id,
+      const agv::RobotContextPtr& context,
+      std::function<void()> update);
+
+    ConstStatePtr state() const;
+
+    rmf_traffic::Duration duration_estimate() const final;
+
+    ActivePtr begin(
+      std::function<void()> checkpoint,
+      std::function<void()> finished) final;
+
+  private:
+
+    AssignIDPtr _assign_id;
+    agv::RobotContextPtr _context;
+    std::function<void()> _update;
+    rmf_task::events::SimpleEventStatePtr _state;
+    ActivePtr _active = nullptr;
+
+  };
+
+  class Active
+    : public rmf_task_sequence::Event::Active,
+      public std::enable_shared_from_this<Active>
+  {
+  public:
+
+    static std::shared_ptr<Active> make(
+      const AssignIDPtr& id,
+      agv::RobotContextPtr context,
+      rmf_task::events::SimpleEventStatePtr state,
+      std::function<void()> update,
+      std::function<void()> finished);
+
+    ConstStatePtr state() const final;
+
+    rmf_traffic::Duration remaining_time_estimate() const final;
+
+    Backup backup() const final;
+
+    Resume interrupt(std::function<void()> task_is_interrupted) final;
+
+    void cancel() final;
+
+    void kill() final;
+
+  private:
+
+    void _schedule_retry();
+
+    void _find_plan();
+
+    void _execute_plan(rmf_traffic::agv::Plan plan);
+
+    Negotiator::NegotiatePtr _respond(
+      const Negotiator::TableViewerPtr& table_view,
+      const Negotiator::ResponderPtr& responder);
+
+    AssignIDPtr _assign_id;
+    agv::RobotContextPtr _context;
+    std::function<void()> _update;
+    std::function<void()> _finished;
+    rmf_task::events::SimpleEventStatePtr _state;
+    std::shared_ptr<Negotiator> _negotiator;
+    std::optional<ExecutePlan> _execution;
+    std::shared_ptr<services::FindEmergencyPullover> _find_pullover_service;
+    rmf_rxcpp::subscription_guard _pullover_subscription;
+    rclcpp::TimerBase::SharedPtr _find_pullover_timeout;
+    rclcpp::TimerBase::SharedPtr _retry_timer;
+
+    bool _is_interrupted = false;
+  };
+};
+
+} // namespace events
+} // namespace rmf_fleet_adapter
+
+#endif // SRC__RMF_FLEET_ADAPTER__EVENTS__EMERGENCYPULLOVER_HPP
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp
new file mode 100644
index 000000000..73038ce73
--- /dev/null
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp
@@ -0,0 +1,520 @@
+/*
+ * 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 "ExecutePlan.hpp"
+#include "LegacyPhaseShim.hpp"
+
+#include "../phases/MoveRobot.hpp"
+#include "../phases/DoorOpen.hpp"
+#include "../phases/DoorClose.hpp"
+#include "../phases/RequestLift.hpp"
+#include "../phases/DockRobot.hpp"
+
+#include <rmf_task_sequence/events/Bundle.hpp>
+
+namespace rmf_fleet_adapter {
+namespace events {
+
+namespace {
+//==============================================================================
+using StandbyPtr = rmf_task_sequence::Event::StandbyPtr;
+using UpdateFn = std::function<void()>;
+using MakeStandby = std::function<StandbyPtr(UpdateFn)>;
+using LegacyPhases = std::vector<std::shared_ptr<LegacyTask::PendingPhase>>;
+
+using DockRobot = phases::DockRobot::PendingPhase;
+using DoorOpen = phases::DoorOpen::PendingPhase;
+using DoorClose = phases::DoorClose::PendingPhase;
+using RequestLift = phases::RequestLift::PendingPhase;
+using EndLift = phases::EndLiftSession::Pending;
+using Move = phases::MoveRobot::PendingPhase;
+
+//==============================================================================
+class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor
+{
+public:
+
+  using Lane = rmf_traffic::agv::Graph::Lane;
+
+  EventPhaseFactory(
+    agv::RobotContextPtr context,
+    LegacyPhases& phases,
+    rmf_traffic::Time event_start_time,
+    bool& continuous)
+  : _context(std::move(context)),
+    _phases(phases),
+    _event_start_time(event_start_time),
+    _continuous(continuous)
+  {
+    // Do nothing
+  }
+
+  void execute(const Dock& dock) final
+  {
+    assert(!_moving_lift);
+    _phases.push_back(
+      std::make_shared<phases::DockRobot::PendingPhase>(
+        _context, dock.dock_name()));
+    _continuous = false;
+  }
+
+  void execute(const DoorOpen& open) final
+  {
+    assert(!_moving_lift);
+    const auto node = _context->node();
+    _phases.push_back(
+      std::make_shared<phases::DoorOpen::PendingPhase>(
+        _context,
+        open.name(),
+        _context->requester_id(),
+        _event_start_time + open.duration()));
+    _continuous = true;
+  }
+
+  void execute(const DoorClose& close) final
+  {
+    assert(!_moving_lift);
+
+    // TODO(MXG): Account for event duration in this phase
+    const auto node = _context->node();
+    _phases.push_back(
+      std::make_shared<phases::DoorClose::PendingPhase>(
+        _context,
+        close.name(),
+        _context->requester_id()));
+    _continuous = true;
+  }
+
+  void execute(const LiftSessionBegin& open) final
+  {
+    assert(!_moving_lift);
+    const auto node = _context->node();
+    _phases.push_back(
+      std::make_shared<phases::RequestLift::PendingPhase>(
+        _context,
+        open.lift_name(),
+        open.floor_name(),
+        _event_start_time,
+        phases::RequestLift::Located::Outside));
+
+    _continuous = true;
+  }
+
+  void execute(const LiftMove& move) final
+  {
+    // TODO(MXG): We should probably keep track of what lift is being moved to
+    // make sure we weren't given a broken nav graph
+    _lifting_duration += move.duration();
+    _moving_lift = true;
+
+    _continuous = true;
+  }
+
+  void execute(const LiftDoorOpen& open) final
+  {
+    const auto node = _context->node();
+
+    // TODO(MXG): The time calculation here should be considered more carefully.
+    _phases.push_back(
+      std::make_shared<phases::RequestLift::PendingPhase>(
+        _context,
+        open.lift_name(),
+        open.floor_name(),
+        _event_start_time + open.duration() + _lifting_duration,
+        phases::RequestLift::Located::Inside));
+    _moving_lift = false;
+
+    _continuous = true;
+  }
+
+  void execute(const LiftSessionEnd& close) final
+  {
+    assert(!_moving_lift);
+    const auto node = _context->node();
+    _phases.push_back(
+      std::make_shared<phases::EndLiftSession::Pending>(
+        _context,
+        close.lift_name(),
+        close.floor_name()));
+
+    _continuous = true;
+  }
+
+  void execute(const Wait&) final
+  {
+    // Do nothing
+  }
+
+  bool moving_lift() const
+  {
+    return _moving_lift;
+  }
+
+private:
+  agv::RobotContextPtr _context;
+  std::vector<std::shared_ptr<LegacyTask::PendingPhase>>& _phases;
+  rmf_traffic::Time _event_start_time;
+  bool& _continuous;
+  bool _moving_lift = false;
+  rmf_traffic::Duration _lifting_duration = rmf_traffic::Duration(0);
+};
+
+
+//==============================================================================
+struct EventGroupInfo
+{
+  MakeStandby group;
+  LegacyPhases::const_iterator tail;
+};
+
+//==============================================================================
+std::optional<EventGroupInfo> search_for_door_group(
+  LegacyPhases::const_iterator head,
+  LegacyPhases::const_iterator end,
+  const agv::RobotContextPtr& context,
+  const rmf_task::Event::AssignIDPtr& id)
+{
+  const auto* door_open = dynamic_cast<const phases::DoorOpen::PendingPhase*>(
+    head->get());
+
+  if (!door_open)
+    return std::nullopt;
+
+  // Look for a door close event for this same door
+  auto tail = head;
+  ++tail;
+  auto moving_duration = rmf_traffic::Duration(0);
+  while (tail != end)
+  {
+    const auto* tail_event = tail->get();
+    if (const auto* door_close = dynamic_cast<const DoorClose*>(tail_event))
+    {
+      if (door_open->door_name() != door_close->door_name())
+      {
+        // A different door is being closed, so we should not lump this all
+        // together
+        return std::nullopt;
+      }
+
+      // We have found the event where the robot is finished using the door.
+      // Let's lump these events together.
+
+      auto group_state = rmf_task::events::SimpleEventState::make(
+        id->assign(), "Pass through [door:" + door_open->door_name() + "]",
+        "", rmf_task::Event::Status::Standby, {}, context->clock());
+
+      std::vector<MakeStandby> door_group;
+      ++tail;
+      for (auto it = head; it != tail; ++it)
+      {
+        door_group.push_back(
+          [legacy = *it, context, id](UpdateFn update)
+          {
+            return LegacyPhaseShim::Standby::make(
+                  legacy, context->worker(), context->clock(), id, update);
+          });
+      }
+
+      return EventGroupInfo{
+        [
+          door_group = std::move(door_group),
+          group_state = std::move(group_state)
+        ](UpdateFn update)
+        {
+          return rmf_task_sequence::events::Bundle::standby(
+            rmf_task_sequence::events::Bundle::Type::Sequence,
+            door_group, group_state, update);
+        },
+        tail
+      };
+    }
+    else if (const auto* move = dynamic_cast<const Move*>(tail_event))
+    {
+      moving_duration += move->estimate_phase_duration();
+      if (std::chrono::minutes(1) < moving_duration)
+      {
+        // There is a lot of moving happening here, so we should not lump
+        // this all together
+        return std::nullopt;
+      }
+    }
+    else
+    {
+      // If any other type of event is happening, we should not lump this
+      // all together
+      return std::nullopt;
+    }
+
+    ++tail;
+  }
+
+  return std::nullopt;
+}
+
+//==============================================================================
+std::optional<EventGroupInfo> search_for_lift_group(
+  LegacyPhases::const_iterator head,
+  LegacyPhases::const_iterator end,
+  const agv::RobotContextPtr& context,
+  const rmf_task::Event::AssignIDPtr& id,
+  const rmf_task::events::SimpleEventStatePtr& state)
+{
+  const auto lift_begin = dynamic_cast<const RequestLift*>(head->get());
+
+  if (!lift_begin)
+    return std::nullopt;
+
+  const auto& lift_name = lift_begin->lift_name();
+  auto tail = head;
+  ++tail;
+  while (tail != end)
+  {
+    const auto* tail_event = tail->get();
+    if (const auto* lift_request = dynamic_cast<const RequestLift*>(tail_event))
+    {
+      if (lift_request->lift_name() != lift_name)
+      {
+        // A different lift is being interacted with before the current lift
+        // interaction has finished. This is weird, so let's report it.
+        state->update_log().warn(
+          "Plan involves using [lift:" + lift_request->lift_name()
+          + "] while the robot is already in a session with [lift:"
+          + lift_name + "]. This may indicate a broken navigation graph. "
+          "Please report this to the system integrator.");
+        return std::nullopt;
+      }
+    }
+    else if (const auto* lift_end = dynamic_cast<const EndLift*>(tail_event))
+    {
+      if (lift_end->lift_name() != lift_name)
+      {
+        // A different lift session is being ended before this one. This is
+        // weird, so let's report it.
+        state->update_log().warn(
+          "Plan involves ending a session with [lift:" + lift_end->lift_name()
+          + "] while [lift:" + lift_name + "] is in use. This may indicate "
+          "a broken navigation graph. Please report this to the system "
+          "integrator.");
+        return std::nullopt;
+      }
+
+      auto category = "Take [lift:" + lift_name
+        + "] to [floor:" + lift_end->destination() + "]";
+
+      auto group_state = rmf_task::events::SimpleEventState::make(
+        id->assign(), std::move(category),
+        "", rmf_task::Event::Status::Standby, {}, context->clock());
+
+      std::vector<MakeStandby> lift_group;
+      ++tail;
+      for (auto it = head; it != tail; ++it)
+      {
+        lift_group.push_back(
+          [legacy = *it, context, id](UpdateFn update)
+          {
+            return LegacyPhaseShim::Standby::make(
+              legacy, context->worker(), context->clock(), id, update);
+          });
+      }
+
+      return EventGroupInfo{
+        [
+          lift_group = std::move(lift_group),
+          group_state = std::move(group_state)
+        ](UpdateFn update)
+        {
+          return rmf_task_sequence::events::Bundle::standby(
+            rmf_task_sequence::events::Bundle::Type::Sequence,
+            lift_group, group_state, update);
+        },
+        tail
+      };
+    }
+
+    ++tail;
+  }
+
+  if (tail == end)
+  {
+    state->update_log().warn(
+      "Plan neglects to end a session with [lift:" + lift_name + "]. This may "
+      "indicate a broken navigation graph. Please report this to the system "
+      "integrator.");
+  }
+
+  return std::nullopt;
+}
+
+} // anonymous namespace
+
+//==============================================================================
+std::optional<ExecutePlan> ExecutePlan::make(
+  agv::RobotContextPtr context,
+  rmf_traffic::agv::Plan plan,
+  const rmf_task::Event::AssignIDPtr& id,
+  rmf_task::events::SimpleEventStatePtr state,
+  std::function<void()> update,
+  std::function<void()> finished,
+  std::optional<rmf_traffic::Duration> tail_period)
+{
+  std::optional<rmf_traffic::Time> finish_time_estimate;
+  for (const auto& r : plan.get_itinerary())
+  {
+    const auto check = r.trajectory().back().time();
+    if (!finish_time_estimate.has_value() || *finish_time_estimate < check)
+      finish_time_estimate = check;
+  }
+
+  if (!finish_time_estimate.has_value())
+  {
+    // If this is empty then the entire plan is empty... that's not supposed to
+    // happen...
+    return std::nullopt;
+  }
+
+  std::vector<rmf_traffic::agv::Plan::Waypoint> waypoints =
+    plan.get_waypoints();
+
+  std::vector<rmf_traffic::agv::Plan::Waypoint> move_through;
+
+  LegacyPhases legacy_phases;
+  while (!waypoints.empty())
+  {
+    auto it = waypoints.begin();
+    bool event_occurred = false;
+    for (; it != waypoints.end(); ++it)
+    {
+      move_through.push_back(*it);
+
+      if (it->event())
+      {
+        if (move_through.size() > 1)
+        {
+          legacy_phases.push_back(
+            std::make_shared<phases::MoveRobot::PendingPhase>(
+              context, move_through, tail_period));
+        }
+
+        move_through.clear();
+        bool continuous = true;
+        EventPhaseFactory factory(
+          context, legacy_phases, it->time(), continuous);
+        it->event()->execute(factory);
+        while (factory.moving_lift())
+        {
+          const auto last_it = it;
+          ++it;
+          if (!it->event())
+          {
+            const double dist =
+              (it->position().block<2, 1>(0, 0)
+              - last_it->position().block<2, 1>(0, 0)).norm();
+
+            if (dist < 0.5)
+            {
+              // We'll assume that this is just a misalignment in the maps
+              continue;
+            }
+
+            state->update_log().warn(
+              "Plan involves a translation of [" + std::to_string(dist)
+              + "m] while inside a lift. This may indicate an error in the "
+              "navigation graph. Please report this to the system integrator.");
+          }
+
+          it->event()->execute(factory);
+        }
+
+        if (continuous)
+        {
+          // Have the next sequence of waypoints begin with the event waypoint
+          // of this sequence.
+          move_through.push_back(*it);
+        }
+
+        waypoints.erase(waypoints.begin(), it+1);
+        event_occurred = true;
+        break;
+      }
+    }
+
+    if (move_through.size() > 1)
+    {
+      // If we have more than one waypoint to move through, then create a
+      // moving phase.
+      legacy_phases.push_back(
+        std::make_shared<phases::MoveRobot::PendingPhase>(
+          context, move_through, tail_period));
+    }
+
+    if (!event_occurred)
+    {
+      // If no event occurred on this loop, then we have reached the end of the
+      // waypoint sequence, and we should simply clear it out.
+      waypoints.clear();
+    }
+  }
+
+  // Convert the legacy phases into task events.
+
+  // We take the extra step of lumping related events into groups when we can
+  // manage to identify such groups, e.g. passing through a door or taking a
+  // lift.
+  std::vector<MakeStandby> standbys;
+  auto head = legacy_phases.cbegin();
+  const auto end = legacy_phases.cend();
+  while (head != end)
+  {
+    if (const auto door = search_for_door_group(head, end, context, id))
+    {
+      standbys.push_back(door->group);
+      head = door->tail;
+    }
+    else if (const auto lift =
+             search_for_lift_group(head, end, context, id, state))
+    {
+      standbys.push_back(lift->group);
+      head = lift->tail;
+    }
+    else
+    {
+      standbys.push_back(
+        [legacy = *head, context, id](UpdateFn update)
+        {
+          return LegacyPhaseShim::Standby::make(
+            legacy, context->worker(), context->clock(), id, update);
+        });
+
+      ++head;
+    }
+  }
+
+  auto sequence = rmf_task_sequence::events::Bundle::standby(
+    rmf_task_sequence::events::Bundle::Type::Sequence,
+    standbys, state, std::move(update))->begin([](){}, std::move(finished));
+
+  context->itinerary().set(plan.get_itinerary());
+
+  return ExecutePlan{
+    std::move(plan),
+    finish_time_estimate.value(),
+    std::move(sequence)
+  };
+}
+
+} // namespace events
+} // namespace rmf_fleet_adapter
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.hpp
new file mode 100644
index 000000000..429822b1a
--- /dev/null
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.hpp
@@ -0,0 +1,49 @@
+/*
+ * 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 SRC__RMF_FLEET_ADAPTER__EVENTS__EXECUTEPLAN_HPP
+#define SRC__RMF_FLEET_ADAPTER__EVENTS__EXECUTEPLAN_HPP
+
+#include "../agv/RobotContext.hpp"
+
+#include <rmf_task/events/SimpleEventState.hpp>
+#include <rmf_task_sequence/Event.hpp>
+
+namespace rmf_fleet_adapter {
+namespace events {
+
+//==============================================================================
+struct ExecutePlan
+{
+  static std::optional<ExecutePlan> make(
+    agv::RobotContextPtr context,
+    rmf_traffic::agv::Plan plan,
+    const rmf_task_sequence::Event::AssignIDPtr& id,
+    rmf_task::events::SimpleEventStatePtr state,
+    std::function<void()> update,
+    std::function<void()> finished,
+    std::optional<rmf_traffic::Duration> tail_period);
+
+  rmf_traffic::agv::Plan plan;
+  rmf_traffic::Time finish_time_estimate;
+  rmf_task_sequence::Event::ActivePtr sequence;
+};
+
+} // namespace events
+} // namespace rmf_fleet_adapter
+
+#endif // SRC__RMF_FLEET_ADAPTER__EVENTS__EXECUTEPLAN_HPP
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp
index bd37475d1..308a9833d 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp
@@ -36,10 +36,9 @@ auto GoToPlace::Standby::make(
   const auto context = state.get<agv::GetContext>()->value;
   const auto header = description.generate_header(state, *parameters);
 
-  auto standby = std::make_shared<Standby>();
+  auto standby = std::make_shared<Standby>(Standby{description.destination()});
   standby->_assign_id = id;
   standby->_context = context;
-  standby->_goal = description.destination();
   standby->_time_estimate = header.original_duration_estimate();
   standby->_tail_period = tail_period;
   standby->_update = std::move(update);
@@ -47,11 +46,20 @@ auto GoToPlace::Standby::make(
     id->assign(),
     header.category(),
     header.detail(),
-    rmf_task::Event::Status::Standby);
+    rmf_task::Event::Status::Standby,
+    {},
+    context->clock());
 
   return standby;
 }
 
+//==============================================================================
+GoToPlace::Standby::Standby(rmf_traffic::agv::Plan::Goal goal)
+: _goal(std::move(goal))
+{
+  // Do nothin
+}
+
 //==============================================================================
 auto GoToPlace::Standby::state() const -> ConstStatePtr
 {
@@ -94,10 +102,9 @@ auto GoToPlace::Active::make(
   std::function<void()> update,
   std::function<void()> finished) -> std::shared_ptr<Active>
 {
-  auto active = std::make_shared<Active>();
+  auto active = std::make_shared<Active>(Active(std::move(goal)));
   active->_assign_id = id;
   active->_context = std::move(context);
-  active->_goal = std::move(goal);
   active->_tail_period = tail_period;
   active->_update = std::move(update);
   active->_finished = std::move(finished);
@@ -119,6 +126,241 @@ auto GoToPlace::Active::make(
   return active;
 }
 
+//==============================================================================
+auto GoToPlace::Active::state() const -> ConstStatePtr
+{
+  return _state;
+}
+
+//==============================================================================
+rmf_traffic::Duration GoToPlace::Active::remaining_time_estimate() const
+{
+  if (_execution.has_value())
+  {
+    const auto finish = _execution->finish_time_estimate;
+    const auto now = _context->now();
+    return finish - now + _context->itinerary().delay();
+  }
+
+  const auto& estimate =
+    _context->planner()->setup(_context->location(), _goal);
+
+  if (estimate.ideal_cost().has_value())
+    return rmf_traffic::time::from_seconds(*estimate.ideal_cost());
+
+  // It would be very suspicious if this happens... probably indicates that the
+  // task is impossible.
+  return rmf_traffic::Duration(0);
+}
+
+//==============================================================================
+auto GoToPlace::Active::backup() const -> Backup
+{
+  // GoToPlace doesn't need to be backed up
+  return Backup::make(0, nlohmann::json());
+}
+
+//==============================================================================
+auto GoToPlace::Active::interrupt(std::function<void()> task_is_interrupted)
+-> Resume
+{
+  _negotiator->clear_license();
+  _is_interrupted = true;
+  _execution = std::nullopt;
+
+  _state->update_status(Status::Standby);
+  _state->update_log().info("Going into standby for an interruption");
+  _state->update_dependencies({});
+
+  _context->worker().schedule(
+    [task_is_interrupted](const auto&)
+    {
+      task_is_interrupted();
+    });
+
+  return Resume::make(
+    [w = weak_from_this()]()
+    {
+      if (const auto self = w.lock())
+      {
+        self->_negotiator->claim_license();
+        self->_is_interrupted = false;
+        self->_find_plan();
+      }
+    });
+}
+
+//==============================================================================
+void GoToPlace::Active::cancel()
+{
+  _execution = std::nullopt;
+  _state->update_status(Status::Canceled);
+  _state->update_log().info("Received signal to cancel");
+  _finished();
+}
+
+//==============================================================================
+void GoToPlace::Active::kill()
+{
+  _execution = std::nullopt;
+  _state->update_status(Status::Killed);
+  _state->update_log().info("Received signal to kill");
+  _finished();
+}
+
+//==============================================================================
+std::string wp_name(
+  const agv::RobotContext& context,
+  const rmf_traffic::agv::Plan::Goal& goal)
+{
+  const auto& g = context.planner()->get_configuration().graph();
+  const auto& wp = g.get_waypoint(goal.waypoint());
+  if (wp.name())
+    return *wp.name();
+
+  return "#" + std::to_string(goal.waypoint());
+}
+
+//==============================================================================
+std::string wp_name(const agv::RobotContext& context)
+{
+  const auto& g = context.planner()->get_configuration().graph();
+  const auto& locations = context.location();
+  for (const auto& l : locations)
+  {
+    const auto& wp = g.get_waypoint(l.waypoint());
+    if (wp.name())
+      return *wp.name();
+  }
+
+  if (locations.empty())
+    return "<null>";
+
+  return "#" + std::to_string(locations.front().waypoint());
+}
+
+//==============================================================================
+void GoToPlace::Active::_find_plan()
+{
+  if (_is_interrupted)
+    return;
+
+  _state->update_status(Status::Underway);
+  const auto start_name = wp_name(*_context);
+  const auto goal_name = wp_name(*_context, _goal);
+  _state->update_log().info(
+    "Generating plan to move from [" + start_name + "] to [" + goal_name + "]");
+
+  _find_path_service = std::make_shared<services::FindPath>(
+    _context->planner(), _context->location(), _goal,
+    _context->schedule()->snapshot(), _context->itinerary().id(),
+    _context->profile());
+
+  _plan_subscription = rmf_rxcpp::make_job<services::FindPath::Result>(
+    _find_path_service)
+    .observe_on(rxcpp::identity_same_worker(_context->worker()))
+    .subscribe(
+    [w = weak_from_this(), start_name, goal_name](
+      const services::FindPath::Result& result)
+    {
+      const auto self = w.lock();
+      if (!self)
+        return;
+
+      if (!result)
+      {
+        // The planner could not find a way to reach the goal
+        self->_state->update_status(Status::Error);
+        self->_state->update_log().error(
+          "Failed to find a plan to move from ["
+          + start_name + "] to [" + goal_name + "]. Will retry soon.");
+
+        self->_execution = std::nullopt;
+        self->_schedule_retry();
+
+        self->_context->worker()
+          .schedule([update = self->_update](const auto&){ update(); });
+
+        return;
+      }
+
+      self->_state->update_status(Status::Underway);
+      self->_state->update_log().info(
+        "Found a plan to move from ["
+        + start_name + "] to [" + goal_name + "]");
+
+      self->_execute_plan(*std::move(result));
+      self->_find_path_service = nullptr;
+      self->_retry_timer = nullptr;
+    });
+
+  _find_path_timeout = _context->node()->try_create_wall_timer(
+    std::chrono::seconds(10),
+    [
+      weak_service = _find_path_service->weak_from_this(),
+      weak_self = weak_from_this()
+    ]()
+    {
+      if (const auto service = weak_service.lock())
+        service->interrupt();
+
+      if (const auto self = weak_self.lock())
+        self->_find_path_timeout = nullptr;
+    });
+
+  _update();
+}
+
+//==============================================================================
+GoToPlace::Active::Active(rmf_traffic::agv::Plan::Goal goal)
+: _goal(std::move(goal))
+{
+  // Do nothing
+}
+
+//==============================================================================
+void GoToPlace::Active::_schedule_retry()
+{
+  if (_retry_timer)
+    return;
+
+  // TODO(MXG): Make the retry timing configurable
+  _retry_timer = _context->node()->try_create_wall_timer(
+    std::chrono::seconds(5),
+    [w = weak_from_this()]()
+  {
+    const auto self = w.lock();
+    if (!self)
+      return;
+
+    self->_retry_timer = nullptr;
+    if (self->_execution.has_value())
+      return;
+
+    self->_find_plan();
+  });
+}
+
+//==============================================================================
+void GoToPlace::Active::_execute_plan(rmf_traffic::agv::Plan plan)
+{
+  if (_is_interrupted)
+    return;
+
+  _execution = ExecutePlan::make(
+    _context, std::move(plan), _assign_id, _state,
+    _update, _finished, _tail_period);
+
+  if (!_execution.has_value())
+  {
+    _state->update_status(Status::Error);
+    _state->update_log().error(
+      "Invalid (empty) plan generated. Will retry soon. "
+      "Please report this incident to the Open-RMF developers.");
+    _schedule_retry();
+  }
+}
+
 //==============================================================================
 Negotiator::NegotiatePtr GoToPlace::Active::_respond(
   const Negotiator::TableViewerPtr& table_view,
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp
index e974d47c2..90b38744b 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp
@@ -23,12 +23,12 @@
 
 #include "../services/FindPath.hpp"
 
+#include "ExecutePlan.hpp"
+
 #include <rmf_task_sequence/Event.hpp>
 #include <rmf_task_sequence/events/GoToPlace.hpp>
 #include <rmf_task/events/SimpleEventState.hpp>
 
-#include <rmf_task_sequence/events/Bundle.hpp>
-
 namespace rmf_fleet_adapter {
 namespace events {
 
@@ -58,9 +58,12 @@ class GoToPlace : public rmf_task_sequence::Event
       std::function<void()> finished) final;
 
   private:
+
+    Standby(rmf_traffic::agv::Plan::Goal goal);
+
+    rmf_traffic::agv::Plan::Goal _goal;
     AssignIDPtr _assign_id;
     agv::RobotContextPtr _context;
-    rmf_traffic::agv::Plan::Goal _goal;
     rmf_traffic::Duration _time_estimate;
     std::optional<rmf_traffic::Duration> _tail_period;
     std::function<void()> _update;
@@ -97,26 +100,31 @@ class GoToPlace : public rmf_task_sequence::Event
 
   private:
 
+    Active(rmf_traffic::agv::Plan::Goal goal);
+
+    void _schedule_retry();
+
     void _find_plan();
 
-    void _execute_plan(rmf_traffic::agv::Plan new_plan);
+    void _execute_plan(rmf_traffic::agv::Plan plan);
 
     Negotiator::NegotiatePtr _respond(
       const Negotiator::TableViewerPtr& table_view,
       const Negotiator::ResponderPtr& responder);
 
+    rmf_traffic::agv::Plan::Goal _goal;
     AssignIDPtr _assign_id;
     agv::RobotContextPtr _context;
-    rmf_traffic::agv::Plan::Goal _goal;
     std::optional<rmf_traffic::Duration> _tail_period;
     std::function<void()> _update;
     std::function<void()> _finished;
     rmf_task::events::SimpleEventStatePtr _state;
     std::shared_ptr<Negotiator> _negotiator;
-    std::optional<rmf_traffic::agv::Plan> _plan;
-    rmf_task_sequence::Event::ActivePtr _sequence;
+    std::optional<ExecutePlan> _execution;
     std::shared_ptr<services::FindPath> _find_path_service;
-    rclcpp::TimerBase::SharedPtr _find_path_timer;
+    rmf_rxcpp::subscription_guard _plan_subscription;
+    rclcpp::TimerBase::SharedPtr _find_path_timeout;
+    rclcpp::TimerBase::SharedPtr _retry_timer;
 
     bool _is_interrupted = false;
   };
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.cpp
index 165c10ca4..180cc61d3 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.cpp
@@ -22,8 +22,9 @@ namespace events {
 
 //==============================================================================
 auto LegacyPhaseShim::Standby::make(
-  std::unique_ptr<LegacyTask::PendingPhase> legacy,
+  std::shared_ptr<LegacyTask::PendingPhase> legacy,
   rxcpp::schedulers::worker worker,
+  std::function<std::chrono::system_clock::time_point()> clock,
   const AssignIDPtr& id,
   std::function<void()> parent_update) -> std::shared_ptr<Standby>
 {
@@ -34,7 +35,9 @@ auto LegacyPhaseShim::Standby::make(
     id->assign(),
     standby->_legacy->description(),
     "",
-    rmf_task::Event::Status::Standby);
+    rmf_task::Event::Status::Standby,
+    {},
+    std::move(clock));
 
   standby->_parent_update = std::move(parent_update);
 
@@ -75,7 +78,7 @@ auto LegacyPhaseShim::Standby::begin(
 
 //==============================================================================
 auto LegacyPhaseShim::Active::make(
-  std::unique_ptr<LegacyTask::PendingPhase> legacy,
+  std::shared_ptr<LegacyTask::PendingPhase> legacy,
   rxcpp::schedulers::worker worker,
   rmf_task::events::SimpleEventStatePtr state,
   std::function<void()> parent_update,
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.hpp
index 9e51bb0f1..2cfe01573 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.hpp
@@ -42,8 +42,9 @@ class LegacyPhaseShim : public rmf_task_sequence::Event
   public:
 
     static std::shared_ptr<Standby> make(
-      std::unique_ptr<LegacyTask::PendingPhase> legacy,
+      std::shared_ptr<LegacyTask::PendingPhase> legacy,
       rxcpp::schedulers::worker worker,
+      std::function<std::chrono::system_clock::time_point()> clock,
       const AssignIDPtr& id,
       std::function<void()> parent_update);
 
@@ -56,7 +57,7 @@ class LegacyPhaseShim : public rmf_task_sequence::Event
       std::function<void()> finished) final;
 
   private:
-    std::unique_ptr<LegacyTask::PendingPhase> _legacy;
+    std::shared_ptr<LegacyTask::PendingPhase> _legacy;
     rxcpp::schedulers::worker _worker;
     rmf_task::events::SimpleEventStatePtr _state;
     std::function<void()> _parent_update;
@@ -69,7 +70,7 @@ class LegacyPhaseShim : public rmf_task_sequence::Event
   public:
 
     static std::shared_ptr<Active> make(
-      std::unique_ptr<LegacyTask::PendingPhase> legacy,
+      std::shared_ptr<LegacyTask::PendingPhase> legacy,
       rxcpp::schedulers::worker worker,
       rmf_task::events::SimpleEventStatePtr state,
       std::function<void()> parent_update,
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorClose.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorClose.hpp
index ab389c82a..df9faf763 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorClose.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorClose.hpp
@@ -96,6 +96,11 @@ struct DoorClose
 
     const std::string& description() const override;
 
+    const std::string& door_name() const
+    {
+      return _door_name;
+    }
+
   private:
 
     agv::RobotContextPtr _context;
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorOpen.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorOpen.hpp
index fa5f71922..9e7064fef 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorOpen.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorOpen.hpp
@@ -105,6 +105,11 @@ struct DoorOpen
 
     const std::string& description() const override;
 
+    const std::string& door_name() const
+    {
+      return _door_name;
+    }
+
   private:
 
     agv::RobotContextPtr _context;
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.hpp
index cfa2373fa..fdd2e339b 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.hpp
@@ -80,6 +80,16 @@ struct EndLiftSession
 
     const std::string& description() const override;
 
+    const std::string& lift_name() const
+    {
+      return _lift_name;
+    }
+
+    const std::string& destination() const
+    {
+      return _destination;
+    }
+
   private:
     agv::RobotContextPtr _context;
     std::string _lift_name;
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.hpp
index e269b8245..6197ee306 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.hpp
@@ -113,6 +113,11 @@ struct RequestLift
 
     const std::string& description() const override;
 
+    const std::string& lift_name() const
+    {
+      return _lift_name;
+    }
+
   private:
     agv::RobotContextPtr _context;
     std::string _lift_name;
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/services/FindPath.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/services/FindPath.hpp
index 7c7d5664f..8ed682884 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/services/FindPath.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/services/FindPath.hpp
@@ -26,7 +26,7 @@ namespace services {
 //==============================================================================
 /// Find a path that gets from the start to the goal. It might or might not
 /// comply with the given schedule, depending on what is feasible.
-class FindPath
+class FindPath : public std::enable_shared_from_this<FindPath>
 {
 public:
 

From 31c095a7dfb2f78121e887c1279cee07533de0e1 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Thu, 16 Dec 2021 23:50:45 +0800
Subject: [PATCH 19/79] Implement delivery

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 .../src/rmf_fleet_adapter/TaskManager.cpp     |   2 +
 .../src/rmf_fleet_adapter/TaskManager.hpp     |   2 +-
 .../agv/FleetUpdateHandle.cpp                 |  35 ++-
 .../src/rmf_fleet_adapter/agv/Node.cpp        |  11 +
 .../src/rmf_fleet_adapter/agv/Node.hpp        |   4 +
 .../rmf_fleet_adapter/agv/RobotContext.cpp    |  28 +-
 .../rmf_fleet_adapter/agv/RobotContext.hpp    |  17 +-
 .../agv/internal_FleetUpdateHandle.hpp        |  17 ++
 .../src/rmf_fleet_adapter/events/Error.cpp    | 120 +++++++++
 .../src/rmf_fleet_adapter/events/Error.hpp    |  98 +++++++
 .../rmf_fleet_adapter/events/GoToPlace.cpp    |  30 +++
 .../rmf_fleet_adapter/events/GoToPlace.hpp    |   4 +
 .../events/LegacyPhaseShim.cpp                |  23 +-
 .../events/LegacyPhaseShim.hpp                |   5 +-
 .../rmf_fleet_adapter/phases/DispenseItem.cpp |   2 +-
 .../src/rmf_fleet_adapter/tasks/Delivery.cpp  | 242 ++++++++++++++++++
 .../src/rmf_fleet_adapter/tasks/Delivery.hpp  |   9 +
 17 files changed, 627 insertions(+), 22 deletions(-)
 create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/events/Error.cpp
 create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/events/Error.hpp

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
index 6c4947de9..a994cf2cf 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
@@ -450,6 +450,7 @@ void TaskManager::_begin_next_task()
   {
     // Update state in RobotContext and Assign active task
     _context->current_task_end_state(_queue.front()->finish_state());
+    _context->current_task_id(_queue.front()->id());
     _active_task = std::move(_queue.front());
     _queue.erase(_queue.begin());
 
@@ -504,6 +505,7 @@ void TaskManager::_begin_next_task()
         self->_context->node()->task_summary()->publish(msg);
 
         self->_active_task = nullptr;
+        self->_context->current_task_id(std::nullopt);
       });
 
     _active_task->begin();
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
index c07080e39..ecd62bac2 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
@@ -94,7 +94,7 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
 
   RobotModeMsg robot_mode() const;
 
-  /// Get a vector of task logs that are validaed against the schema
+  /// Get a vector of task logs that are validated against the schema
   std::vector<nlohmann::json> task_log_updates() const;
 
 private:
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
index 9d2c72e00..3ff96b638 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
@@ -27,6 +27,7 @@
 
 #include "../tasks/Delivery.hpp"
 #include "../tasks/Loop.hpp"
+#include "../tasks/Clean.hpp"
 
 #include <rmf_task/Constraints.hpp>
 #include <rmf_task/Parameters.hpp>
@@ -34,6 +35,8 @@
 #include <rmf_task/requests/Delivery.hpp>
 #include <rmf_task/requests/Loop.hpp>
 
+#include <rmf_task_sequence/phases/SimplePhase.hpp>
+
 #include <rmf_task_msgs/msg/clean.hpp>
 #include <rmf_task_msgs/msg/delivery.hpp>
 #include <rmf_task_msgs/msg/loop.hpp>
@@ -346,6 +349,14 @@ void FleetUpdateHandle::Implementation::bid_notice_cb(
       return;
     }
 
+    std::vector<rmf_task::Payload::Component> components;
+    components.reserve(delivery.items.size());
+    for (const auto& item : delivery.items)
+    {
+      components.push_back(
+        {item.type_guid, (uint32_t)item.quantity, item.compartment_name});
+    }
+
     // TODO: We set the waiting duration at the pickup and dropoff locations to
     // 0s as the cycle time of the dispensers and ingestors are not available.
     // We should implement a means to lookup these values for each system.
@@ -354,9 +365,13 @@ void FleetUpdateHandle::Implementation::bid_notice_cb(
       rmf_traffic::time::from_seconds(0),
       dropoff_wp->index(),
       rmf_traffic::time::from_seconds(0),
+      rmf_task::Payload(std::move(components)),
       id,
       start_time,
-      priority);
+      priority,
+      false,
+      delivery.pickup_dispenser,
+      delivery.dropoff_ingestor);
 
     RCLCPP_INFO(
       node->get_logger(),
@@ -937,7 +952,23 @@ void FleetUpdateHandle::Implementation::publish_fleet_state() const
     .robots(std::move(robot_states));
 
   fleet_state_pub->publish(std::move(fleet_state));
+}
 
+//==============================================================================
+void FleetUpdateHandle::Implementation::add_standard_tasks()
+{
+  activation.task = std::make_shared<rmf_task::Activator>();
+  activation.phase = std::make_shared<rmf_task_sequence::Phase::Activator>();
+  activation.event = std::make_shared<rmf_task_sequence::Event::Initializer>();
+
+  rmf_task_sequence::phases::SimplePhase::add(
+    *activation.phase, activation.event);
+
+  tasks::add_delivery(
+    *activation.task,
+    activation.phase,
+    *activation.event,
+    node->clock());
 }
 
 //==============================================================================
@@ -1065,7 +1096,6 @@ void FleetUpdateHandle::add_robot(
   rmf_traffic::agv::Plan::StartSet start,
   std::function<void(std::shared_ptr<RobotUpdateHandle>)> handle_cb)
 {
-
   if (start.empty())
   {
     // *INDENT-OFF*
@@ -1118,6 +1148,7 @@ void FleetUpdateHandle::add_robot(
           std::move(participant),
           fleet->_pimpl->snappable,
           fleet->_pimpl->planner,
+          fleet->_pimpl->activation.task,
           fleet->_pimpl->node,
           fleet->_pimpl->worker,
           fleet->_pimpl->default_maximum_delay,
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp
index 4dfc66d34..d1cd875bd 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp
@@ -20,6 +20,8 @@
 #include <rmf_fleet_adapter/StandardNames.hpp>
 #include <rmf_traffic_ros2/StandardNames.hpp>
 
+#include <rmf_traffic_ros2/Time.hpp>
+
 namespace rmf_fleet_adapter {
 namespace agv {
 
@@ -104,6 +106,15 @@ Node::Node(
   // Do nothing
 }
 
+//==============================================================================
+std::function<rmf_traffic::Time()> Node::clock() const
+{
+  return [self = shared_from_this()]()
+    {
+      return rmf_traffic_ros2::convert(self->now());
+    };
+}
+
 //==============================================================================
 auto Node::door_state() const -> const DoorStateObs&
 {
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp
index 30c0c5b28..58b0a11d4 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp
@@ -36,6 +36,8 @@
 
 #include <rmf_fleet_msgs/msg/fleet_state.hpp>
 
+#include <rmf_traffic/Time.hpp>
+
 namespace rmf_fleet_adapter {
 namespace agv {
 
@@ -49,6 +51,8 @@ class Node : public rmf_rxcpp::Transport
     const std::string& node_name,
     const rclcpp::NodeOptions& options);
 
+  std::function<rmf_traffic::Time()> clock() const;
+
   using DoorState = rmf_door_msgs::msg::DoorState;
   using DoorStateObs = rxcpp::observable<DoorState::SharedPtr>;
   const DoorStateObs& door_state() const;
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp
index 45da69d7a..38437dd61 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp
@@ -69,14 +69,9 @@ rmf_traffic::Time RobotContext::now() const
 }
 
 //==============================================================================
-std::function<std::chrono::system_clock::time_point()>
-RobotContext::clock() const
+std::function<rmf_traffic::Time()> RobotContext::clock() const
 {
-  return [self = shared_from_this()]()
-    {
-      return std::chrono::system_clock::time_point(
-        std::chrono::system_clock::duration(self->node()->now().nanoseconds()));
-    };
+  return [self = shared_from_this()](){ return self->now(); };
 }
 
 //==============================================================================
@@ -252,6 +247,22 @@ RobotContext& RobotContext::current_task_end_state(
   return *this;
 }
 
+//==============================================================================
+const std::string* RobotContext::current_task_id() const
+{
+  if (_current_task_id.has_value())
+    return &(*_current_task_id);
+
+  return nullptr;
+}
+
+//==============================================================================
+RobotContext& RobotContext::current_task_id(std::optional<std::string> id)
+{
+  _current_task_id = std::move(id);
+  return *this;
+}
+
 //==============================================================================
 double RobotContext::current_battery_soc() const
 {
@@ -348,6 +359,7 @@ RobotContext::RobotContext(
   rmf_traffic::schedule::Participant itinerary,
   std::shared_ptr<const Snappable> schedule,
   std::shared_ptr<std::shared_ptr<const rmf_traffic::agv::Planner>> planner,
+  rmf_task::ConstActivatorPtr activator,
   std::shared_ptr<rmf_fleet_adapter::agv::Node> node,
   const rxcpp::schedulers::worker& worker,
   rmf_utils::optional<rmf_traffic::Duration> maximum_delay,
@@ -358,6 +370,7 @@ RobotContext::RobotContext(
   _itinerary(std::move(itinerary)),
   _schedule(std::move(schedule)),
   _planner(std::move(planner)),
+  _task_activator(std::move(activator)),
   _stubbornness(std::make_shared<int>(0)),
   _node(std::move(node)),
   _worker(worker),
@@ -365,6 +378,7 @@ RobotContext::RobotContext(
   _requester_id(
     _itinerary.description().owner() + "/" + _itinerary.description().name()),
   _current_task_end_state(state),
+  _current_task_id(std::nullopt),
   _task_planner(std::move(task_planner))
 {
   _profile = std::make_shared<rmf_traffic::Profile>(
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp
index b910445d1..23b8536d4 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp
@@ -12,6 +12,7 @@
 #include <rmf_task/State.hpp>
 #include <rmf_task/Constraints.hpp>
 #include <rmf_task/TaskPlanner.hpp>
+#include <rmf_task/Activator.hpp>
 
 #include <rclcpp/node.hpp>
 
@@ -50,7 +51,7 @@ class RobotContext
   rmf_traffic::Time now() const;
 
   /// Get a clock that can be used by task loggers
-  std::function<std::chrono::system_clock::time_point()> clock() const;
+  std::function<rmf_traffic::Time()> clock() const;
 
   /// This is the current "location" of the robot, which can be used to initiate
   /// a planning job
@@ -126,6 +127,9 @@ class RobotContext
     const TableViewerPtr& table_viewer,
     const ResponderPtr& responder) final;
 
+  /// Get the task activator for this robot
+  const rmf_task::ConstActivatorPtr& task_activator() const;
+
   /// Set the state of this robot at the end of its current task
   RobotContext& current_task_end_state(const rmf_task::State& state);
 
@@ -133,6 +137,14 @@ class RobotContext
   // current task
   const rmf_task::State& current_task_end_state() const;
 
+  /// Get the current task ID of the robot, or a nullptr if the robot is not
+  /// performing any task
+  const std::string* current_task_id() const;
+
+  /// Set the current task ID of the robot, or give a nullopt if a task is not
+  /// being performed.
+  RobotContext& current_task_id(std::optional<std::string> id);
+
   /// Get the current battery state of charge
   double current_battery_soc() const;
 
@@ -175,6 +187,7 @@ class RobotContext
     rmf_traffic::schedule::Participant itinerary,
     std::shared_ptr<const Snappable> schedule,
     std::shared_ptr<std::shared_ptr<const rmf_traffic::agv::Planner>> planner,
+    rmf_task::ConstActivatorPtr activator,
     std::shared_ptr<Node> node,
     const rxcpp::schedulers::worker& worker,
     rmf_utils::optional<rmf_traffic::Duration> maximum_delay,
@@ -186,6 +199,7 @@ class RobotContext
   rmf_traffic::schedule::Participant _itinerary;
   std::shared_ptr<const Snappable> _schedule;
   std::shared_ptr<std::shared_ptr<const rmf_traffic::agv::Planner>> _planner;
+  rmf_task::ConstActivatorPtr _task_activator;
   std::shared_ptr<const rmf_traffic::Profile> _profile;
 
   std::shared_ptr<void> _negotiation_license;
@@ -206,6 +220,7 @@ class RobotContext
   rxcpp::subjects::subject<double> _battery_soc_publisher;
   rxcpp::observable<double> _battery_soc_obs;
   rmf_task::State _current_task_end_state;
+  std::optional<std::string> _current_task_id;
   std::shared_ptr<const rmf_task::TaskPlanner> _task_planner;
 
   RobotUpdateHandle::Unstable::Watchdog _lift_watchdog;
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
index 3f5726c1e..5688ea829 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
@@ -30,6 +30,10 @@
 #include <rmf_task/requests/Clean.hpp>
 #include <rmf_task/BinaryPriorityScheme.hpp>
 
+#include <rmf_task_sequence/Task.hpp>
+#include <rmf_task_sequence/Phase.hpp>
+#include <rmf_task_sequence/Event.hpp>
+
 #include <rmf_fleet_msgs/msg/dock_summary.hpp>
 
 #include <rmf_fleet_adapter/agv/FleetUpdateHandle.hpp>
@@ -64,6 +68,13 @@
 namespace rmf_fleet_adapter {
 namespace agv {
 
+struct TaskActivation
+{
+  rmf_task::ActivatorPtr task;
+  rmf_task_sequence::Phase::ActivatorPtr phase;
+  rmf_task_sequence::Event::InitializerPtr event;
+};
+
 //==============================================================================
 /// This abstract interface class allows us to use the same implementation of
 /// FleetUpdateHandle whether we are running it in a distributed system or in a
@@ -148,6 +159,8 @@ class FleetUpdateHandle::Implementation
   std::shared_ptr<rmf_traffic_ros2::schedule::Negotiation> negotiation;
   std::optional<std::string> server_uri;
 
+  TaskActivation activation = TaskActivation();
+
   // LegacyTask planner params
   std::shared_ptr<rmf_task::CostCalculator> cost_calculator =
     rmf_task::BinaryPriorityScheme::make_cost_calculator();
@@ -219,6 +232,8 @@ class FleetUpdateHandle::Implementation
     handle->_pimpl = rmf_utils::make_unique_impl<Implementation>(
       Implementation{std::forward<Args>(args)...});
 
+    handle->_pimpl->add_standard_tasks();
+
     handle->_pimpl->fleet_state_pub = handle->_pimpl->node->fleet_state();
     handle->_pimpl->fleet_state_timer =
       handle->_pimpl->node->try_create_wall_timer(
@@ -342,6 +357,8 @@ class FleetUpdateHandle::Implementation
     std::optional<rmf_traffic::Duration> value);
 
   void publish_fleet_state() const;
+
+  void add_standard_tasks();
 };
 
 } // namespace agv
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/Error.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/Error.cpp
new file mode 100644
index 000000000..3f95abef3
--- /dev/null
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/Error.cpp
@@ -0,0 +1,120 @@
+/*
+ * 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 "Error.hpp"
+
+namespace rmf_fleet_adapter {
+namespace events {
+
+//==============================================================================
+auto Error::Standby::make(
+  rmf_task::events::SimpleEventStatePtr state,
+  Behavior behavior) -> std::shared_ptr<Standby>
+{
+  auto standby = std::make_shared<Standby>();
+  standby->_state = std::move(state);
+  standby->_behavior = behavior;
+
+  // Make sure the status is set to an error
+  standby->_state->update_status(Status::Error);
+
+  return standby;
+}
+
+//==============================================================================
+auto Error::Standby::state() const -> ConstStatePtr
+{
+  return _state;
+}
+
+//==============================================================================
+rmf_traffic::Duration Error::Standby::duration_estimate() const
+{
+  return rmf_traffic::Duration(0);
+}
+
+//==============================================================================
+auto Error::Standby::begin(
+  std::function<void()>,
+  std::function<void()> finished) -> ActivePtr
+{
+  if (!_active)
+  {
+    _active = Active::make(_state, std::move(finished), _behavior);
+  }
+
+  return _active;
+}
+
+//==============================================================================
+auto Error::Active::make(
+  rmf_task::events::SimpleEventStatePtr state,
+  std::function<void()> finished,
+  Behavior behavior) -> std::shared_ptr<Active>
+{
+  auto active = std::make_shared<Active>();
+  active->_state = std::move(state);
+  active->_finished = std::move(finished);
+
+  active->_state->update_status(Status::Error);
+
+  if (behavior == Behavior::Continue)
+    active->_finished();
+
+  return active;
+}
+
+//==============================================================================
+auto Error::Active::state() const -> ConstStatePtr
+{
+  return _state;
+}
+
+//==============================================================================
+rmf_traffic::Duration Error::Active::remaining_time_estimate() const
+{
+  return rmf_traffic::Duration(0);
+}
+
+//==============================================================================
+auto Error::Active::backup() const -> Backup
+{
+  return Backup::make(0, nlohmann::json());
+}
+
+//==============================================================================
+auto Error::Active::interrupt(std::function<void()> task_is_interrupted)
+-> Resume
+{
+  task_is_interrupted();
+  return Resume::make([](){});
+}
+
+//==============================================================================
+void Error::Active::cancel()
+{
+  _finished();
+}
+
+//==============================================================================
+void Error::Active::kill()
+{
+  _finished();
+}
+
+} // namespace events
+} // namespace rmf_fleet_adapter
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/Error.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/Error.hpp
new file mode 100644
index 000000000..56138e630
--- /dev/null
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/Error.hpp
@@ -0,0 +1,98 @@
+/*
+ * 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 SRC__RMF_FLEET_ADAPTER__EVENTS__ERROR_HPP
+#define SRC__RMF_FLEET_ADAPTER__EVENTS__ERROR_HPP
+
+#include <rmf_task_sequence/Event.hpp>
+#include <rmf_task/events/SimpleEventState.hpp>
+
+namespace rmf_fleet_adapter {
+namespace events {
+
+//==============================================================================
+class Error : public rmf_task_sequence::Event
+{
+public:
+
+  enum class Behavior
+  {
+    /// This Error event will block until an operator explicitly cancels,
+    /// kills, skips, or rewinds the task. This is the recommended behavior
+    /// because an event error usually means something in the task process went
+    /// wrong and needs to be retried.
+    Block = 0,
+
+    /// This Error event will report that the error occurred but then finish as
+    /// soon as it is activated.
+    Continue
+  };
+
+  class Standby : public rmf_task_sequence::Event::Standby
+  {
+  public:
+
+    static std::shared_ptr<Standby> make(
+      rmf_task::events::SimpleEventStatePtr state,
+      Behavior behavior = Behavior::Block);
+
+    ConstStatePtr state() const final;
+
+    rmf_traffic::Duration duration_estimate() const final;
+
+    ActivePtr begin(
+      std::function<void()> checkpoint,
+      std::function<void()> finished) final;
+
+  private:
+    rmf_task::events::SimpleEventStatePtr _state;
+    Behavior _behavior;
+    ActivePtr _active;
+  };
+
+  class Active : public rmf_task_sequence::Event::Active
+  {
+  public:
+
+    static std::shared_ptr<Active> make(
+      rmf_task::events::SimpleEventStatePtr state,
+      std::function<void()> finished,
+      Behavior behavior = Behavior::Block);
+
+    ConstStatePtr state() const final;
+
+    rmf_traffic::Duration remaining_time_estimate() const final;
+
+    Backup backup() const final;
+
+    Resume interrupt(std::function<void()> task_is_interrupted) final;
+
+    void cancel() final;
+
+    void kill() final;
+
+  private:
+    rmf_task::events::SimpleEventStatePtr _state;
+    std::function<void()> _finished;
+  };
+
+};
+
+} // namespace task
+} // namespace rmf_fleet_adapter
+
+#endif // SRC__RMF_FLEET_ADAPTER__EVENTS__ERROR_HPP
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp
index 308a9833d..7938ab2ce 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp
@@ -22,6 +22,36 @@
 namespace rmf_fleet_adapter {
 namespace events {
 
+//==============================================================================
+void GoToPlace::add(rmf_task_sequence::Event::Initializer &initializer)
+{
+  initializer.add<Description>(
+    [](
+      const AssignIDPtr& id,
+      const std::function<rmf_task::State()>& get_state,
+      const rmf_task::ConstParametersPtr& parameters,
+      const Description& description,
+      std::function<void()> update) -> StandbyPtr
+    {
+      return Standby::make(
+        id, get_state, parameters, description, std::move(update));
+    },
+    [](
+      const AssignIDPtr& id,
+      const std::function<rmf_task::State()>& get_state,
+      const rmf_task::ConstParametersPtr& parameters,
+      const Description& description,
+      const nlohmann::json&,
+      std::function<void()> update,
+      std::function<void()> checkpoint,
+      std::function<void()> finished) -> ActivePtr
+    {
+      return Standby::make(
+        id, get_state, parameters, description, std::move(update))
+        ->begin(std::move(checkpoint), std::move(finished));
+    });
+}
+
 //==============================================================================
 auto GoToPlace::Standby::make(
   const AssignIDPtr& id,
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp
index 90b38744b..56e1c9bae 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp
@@ -37,6 +37,10 @@ class GoToPlace : public rmf_task_sequence::Event
 {
 public:
 
+  using Description = rmf_task_sequence::events::GoToPlace::Description;
+
+  static void add(rmf_task_sequence::Event::Initializer& initializer);
+
   class Standby : public rmf_task_sequence::Event::Standby
   {
   public:
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.cpp
index 180cc61d3..82b4e8fb4 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.cpp
@@ -24,20 +24,27 @@ namespace events {
 auto LegacyPhaseShim::Standby::make(
   std::shared_ptr<LegacyTask::PendingPhase> legacy,
   rxcpp::schedulers::worker worker,
-  std::function<std::chrono::system_clock::time_point()> clock,
+  std::function<rmf_traffic::Time()> clock,
   const AssignIDPtr& id,
-  std::function<void()> parent_update) -> std::shared_ptr<Standby>
+  std::function<void()> parent_update,
+  std::optional<std::string> name) -> std::shared_ptr<Standby>
 {
   auto standby = std::make_shared<Standby>();
   standby->_legacy = std::move(legacy);
   standby->_worker = std::move(worker);
   standby->_state = rmf_task::events::SimpleEventState::make(
-    id->assign(),
-    standby->_legacy->description(),
-    "",
-    rmf_task::Event::Status::Standby,
-    {},
-    std::move(clock));
+    id->assign(), "", "",
+    rmf_task::Event::Status::Standby, {}, std::move(clock));
+
+  if (name.has_value())
+  {
+    standby->_state->update_name(*name);
+    standby->_state->update_detail(standby->_legacy->description());
+  }
+  else
+  {
+    standby->_state->update_name(standby->_legacy->description());
+  }
 
   standby->_parent_update = std::move(parent_update);
 
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.hpp
index 2cfe01573..0037c71a4 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.hpp
@@ -44,9 +44,10 @@ class LegacyPhaseShim : public rmf_task_sequence::Event
     static std::shared_ptr<Standby> make(
       std::shared_ptr<LegacyTask::PendingPhase> legacy,
       rxcpp::schedulers::worker worker,
-      std::function<std::chrono::system_clock::time_point()> clock,
+      std::function<rmf_traffic::Time()> clock,
       const AssignIDPtr& id,
-      std::function<void()> parent_update);
+      std::function<void()> parent_update,
+      std::optional<std::string> name = std::nullopt);
 
     ConstStatePtr state() const final;
 
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DispenseItem.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DispenseItem.cpp
index e9a78813e..8cebd1aa5 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DispenseItem.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DispenseItem.cpp
@@ -87,7 +87,7 @@ DispenseItem::ActivePhase::ActivePhase(
   _items(std::move(items))
 {
   std::ostringstream oss;
-  oss << "Dispense items (";
+  oss << "Receive items (";
   for (size_t i = 0; i < _items.size(); i++)
   {
     oss << _items[i].type_guid;
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp
index 6fa0218f5..479b2e525 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp
@@ -19,9 +19,21 @@
 #include "../phases/IngestItem.hpp"
 #include "../phases/GoToPlace.hpp"
 
+#include "../events/LegacyPhaseShim.hpp"
+#include "../events/Error.hpp"
+#include "../events/GoToPlace.hpp"
+
 #include "Delivery.hpp"
 
 #include <rmf_ingestor_msgs/msg/ingestor_request_item.hpp>
+#include <rmf_dispenser_msgs/msg/dispenser_request_item.hpp>
+
+#include <rmf_task_sequence/events/PickUp.hpp>
+#include <rmf_task_sequence/events/DropOff.hpp>
+#include <rmf_task_sequence/events/WaitFor.hpp>
+#include <rmf_task_sequence/events/Bundle.hpp>
+#include <rmf_task_sequence/Task.hpp>
+#include <rmf_task_sequence/phases/SimplePhase.hpp>
 
 namespace rmf_fleet_adapter {
 namespace tasks {
@@ -116,5 +128,235 @@ std::shared_ptr<LegacyTask> make_delivery(
     request);
 }
 
+//==============================================================================
+struct TransferItems : public rmf_task_sequence::events::WaitFor::Description
+{
+  enum class Dir
+  {
+    Load,
+    Unload
+  };
+
+  TransferItems(const rmf_task_sequence::events::PickUp::Description& pickup)
+  : rmf_task_sequence::events::WaitFor::Description(
+      pickup.loading_duration_estimate()),
+    direction(Dir::Load),
+    target(pickup.from_dispenser()),
+    payload(pickup.payload())
+  {
+    // Do nothing
+  }
+
+  TransferItems(const rmf_task_sequence::events::DropOff::Description& dropoff)
+  : rmf_task_sequence::events::WaitFor::Description(
+      dropoff.unloading_duration_estimate()),
+    direction(Dir::Unload),
+    target(dropoff.into_ingestor()),
+    payload(dropoff.payload())
+  {
+    // Do nothing
+  }
+
+  template<template<class> class Build, typename T>
+  std::vector<T> collect_items() const
+  {
+    std::vector<T> items;
+    for (const auto& c : payload.components())
+    {
+      items.push_back(
+        Build<T>()
+        .type_guid(c.sku())
+        .quantity(c.quantity())
+        .compartment_name(c.compartment()));
+    }
+  }
+
+  Dir direction;
+  std::string target;
+  rmf_task::Payload payload;
+
+  static rmf_task_sequence::Event::StandbyPtr standby(
+    const rmf_task_sequence::Event::AssignIDPtr& id,
+    const std::function<rmf_task::State()>& get_state,
+    const rmf_task::ConstParametersPtr&,
+    const TransferItems& description,
+    std::function<void()> update)
+  {
+    const auto state = get_state();
+    const auto context = state.get<agv::GetContext>()->value;
+    const auto* task_id = context->current_task_id();
+
+    std::string name;
+    if (description.direction == Dir::Unload)
+      name = "Unload";
+    else
+      name = "Load";
+
+    if (!task_id)
+    {
+      const auto error_state = rmf_task::events::SimpleEventState::make(
+        id->assign(), name, "", rmf_task::Event::Status::Error, {},
+        context->clock());
+
+      error_state->update_log().error(
+        "Task ID is null while trying to perform a delivery. This indicates a "
+        "serious software error. Please try to reset the task, and contact the "
+        "system integrator to inform them of this issue.");
+
+      return events::Error::Standby::make(std::move(error_state));
+    }
+
+    std::shared_ptr<LegacyTask::PendingPhase> legacy;
+    if (description.direction == Dir::Unload)
+    {
+      using IngestorItem = rmf_ingestor_msgs::msg::IngestorRequestItem;
+      std::vector<IngestorItem> items;
+      for (const auto& c : description.payload.components())
+      {
+        items.push_back(
+          rmf_ingestor_msgs::build<IngestorItem>()
+          .type_guid(c.sku())
+          .quantity(c.quantity())
+          .compartment_name(c.compartment()));
+      }
+
+      legacy = std::make_shared<phases::IngestItem::PendingPhase>(
+        context, *task_id, description.target,
+        context->itinerary().description().owner(),
+        std::move(items));
+
+      name = "Unload";
+    }
+    else
+    {
+      using DispenserItem = rmf_dispenser_msgs::msg::DispenserRequestItem;
+      std::vector<DispenserItem> items;
+      for (const auto& c : description.payload.components())
+      {
+        items.push_back(
+          rmf_dispenser_msgs::build<DispenserItem>()
+          .type_guid(c.sku())
+          .quantity(c.quantity())
+          .compartment_name(c.compartment()));
+      }
+
+      legacy = std::make_shared<phases::DispenseItem::PendingPhase>(
+        context, *task_id, description.target,
+        context->itinerary().description().owner(),
+        std::move(items));
+
+      name = "Load";
+    }
+
+    if (description.payload.components().size() == 1)
+      name += "item";
+    else
+      name += "items";
+
+    return events::LegacyPhaseShim::Standby::make(
+      legacy, context->worker(), context->clock(), id, std::move(update), name);
+  }
+
+  static void add(rmf_task_sequence::Event::Initializer& initializer)
+  {
+    initializer.add<TransferItems>(
+      [](
+        const rmf_task_sequence::Event::AssignIDPtr& id,
+        const std::function<rmf_task::State()>& get_state,
+        const rmf_task::ConstParametersPtr& parameters,
+        const TransferItems& description,
+        std::function<void()> update) -> rmf_task_sequence::Event::StandbyPtr
+      {
+        return standby(
+          id, get_state, parameters, description, std::move(update));
+      },
+      [](
+        const rmf_task_sequence::Event::AssignIDPtr& id,
+        const std::function<rmf_task::State()>& get_state,
+        const rmf_task::ConstParametersPtr& parameters,
+        const TransferItems& description,
+        const nlohmann::json&,
+        std::function<void()> update,
+        std::function<void()> checkpoint,
+        std::function<void()> finished) -> rmf_task_sequence::Event::ActivePtr
+      {
+        return standby(
+          id, get_state, parameters, description, std::move(update))
+          ->begin(std::move(checkpoint), std::move(finished));
+      });
+  }
+};
+
+//==============================================================================
+void add_delivery(
+  rmf_task::Activator& task_activator,
+  const rmf_task_sequence::Phase::ConstActivatorPtr& phase_activator,
+  rmf_task_sequence::Event::Initializer& event_initializer,
+  std::function<rmf_traffic::Time()> clock)
+{
+  using Bundle = rmf_task_sequence::events::Bundle;
+  using PickUp = rmf_task_sequence::events::PickUp;
+  using DropOff = rmf_task_sequence::events::DropOff;
+  using Phase = rmf_task_sequence::phases::SimplePhase;
+
+  auto private_initializer =
+    std::make_shared<rmf_task_sequence::Event::Initializer>();
+
+  TransferItems::add(*private_initializer);
+  events::GoToPlace::add(*private_initializer);
+
+  auto pickup_unfolder =
+    [](const PickUp::Description& pickup)
+    {
+      return Bundle::Description({
+        events::GoToPlace::Description::make(pickup.pickup_location()),
+        std::make_shared<TransferItems>(pickup)
+      }, Bundle::Type::Sequence, "Pick up");
+    };
+
+  Bundle::unfold<PickUp::Description>(
+    std::move(pickup_unfolder), event_initializer, private_initializer);
+
+  auto dropoff_unfolder =
+    [](const DropOff::Description& dropoff)
+    {
+      return Bundle::Description({
+        events::GoToPlace::Description::make(dropoff.drop_off_location()),
+        std::make_shared<TransferItems>(dropoff)
+      }, Bundle::Type::Sequence, "Drop Off");
+    };
+
+  Bundle::unfold<DropOff::Description>(
+    std::move(dropoff_unfolder), event_initializer, private_initializer);
+
+  auto delivery_unfolder =
+    [](const rmf_task::requests::Delivery::Description& delivery)
+    {
+      rmf_task_sequence::Task::Builder builder;
+      builder
+        .add_phase(
+          Phase::Description::make(
+            PickUp::Description::make(
+              delivery.pickup_waypoint(),
+              delivery.pickup_from_dispenser(),
+              delivery.payload(),
+              delivery.pickup_wait())), {})
+        .add_phase(
+          Phase::Description::make(
+            DropOff::Description::make(
+              delivery.dropoff_waypoint(),
+              delivery.dropoff_to_ingestor(),
+              delivery.payload(),
+              delivery.dropoff_wait())), {});
+
+      // TODO(MXG): Consider making the category and detail more detailed
+      return *builder.build("Delivery", "");
+    };
+
+  rmf_task_sequence::Task::unfold<rmf_task::requests::Delivery::Description>(
+    std::move(delivery_unfolder), task_activator,
+    phase_activator, std::move(clock));
+}
+
 } // namespace task
 } // namespace rmf_fleet_adapter
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp
index 18f340d66..d1dd477fe 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp
@@ -25,6 +25,9 @@
 
 #include <rmf_task_msgs/msg/delivery.hpp>
 
+#include <rmf_task_sequence/Phase.hpp>
+#include <rmf_task_sequence/Event.hpp>
+
 namespace rmf_fleet_adapter {
 namespace tasks {
 
@@ -37,6 +40,12 @@ std::shared_ptr<LegacyTask> make_delivery(
   const rmf_task::State finish_state,
   const rmf_task_msgs::msg::Delivery delivery_profile);
 
+void add_delivery(
+  rmf_task::Activator& task_activator,
+  const rmf_task_sequence::Phase::ConstActivatorPtr& phase_activator,
+  rmf_task_sequence::Event::Initializer& event_initializer,
+  std::function<rmf_traffic::Time()> clock);
+
 } // namespace tasks
 } // namespace rmf_fleet_adapter
 

From a072869747656dc2bba4893737139628fb1b58b7 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Fri, 17 Dec 2021 18:26:54 +0800
Subject: [PATCH 20/79] Finished refactoring to new task system -- need to
 finish comms implementation

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 .../src/rmf_fleet_adapter/TaskManager.cpp     | 454 +++++++-----------
 .../src/rmf_fleet_adapter/TaskManager.hpp     |  38 +-
 .../agv/FleetUpdateHandle.cpp                 |  25 +-
 .../rmf_fleet_adapter/agv/RobotContext.cpp    |  26 +
 .../rmf_fleet_adapter/agv/RobotContext.hpp    |   7 +
 .../agv/internal_FleetUpdateHandle.hpp        |  14 +
 .../src/rmf_fleet_adapter/tasks/Clean.cpp     | 205 +++++++-
 .../src/rmf_fleet_adapter/tasks/Clean.hpp     |   9 +
 .../src/rmf_fleet_adapter/tasks/Delivery.cpp  |  14 +-
 .../src/rmf_fleet_adapter/tasks/Loop.cpp      |  38 ++
 .../src/rmf_fleet_adapter/tasks/Loop.hpp      |   8 +
 11 files changed, 535 insertions(+), 303 deletions(-)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
index a994cf2cf..81904f2c8 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
@@ -32,6 +32,7 @@
 #include "tasks/Loop.hpp"
 
 #include "phases/ResponsiveWait.hpp"
+#include "events/EmergencyPullover.hpp"
 
 #include <rmf_api_msgs/schemas/task_state_update.hpp>
 #include <rmf_api_msgs/schemas/task_state.hpp>
@@ -46,19 +47,59 @@ TaskManagerPtr TaskManager::make(
   agv::RobotContextPtr context,
   std::weak_ptr<BroadcastClient> broadcast_client)
 {
-  auto mgr = TaskManagerPtr(new TaskManager(
-        std::move(context), std::move(broadcast_client)));
+  auto mgr = TaskManagerPtr(
+    new TaskManager(
+      std::move(context),
+      std::move(broadcast_client)));
+
+  auto begin_pullover = [w = mgr->weak_from_this()]()
+    {
+      const auto self = w.lock();
+      if (!self)
+        return;
+
+      self->_context->worker().schedule(
+        [w = self->weak_from_this()](const auto&)
+      {
+        const auto self = w.lock();
+
+        if (!self->_emergency_active)
+          return;
+
+        // TODO(MXG): Consider subscribing to the emergency pullover update
+        self->_emergency_pullover = events::EmergencyPullover::Standby::make(
+          rmf_task_sequence::Event::AssignID::make(), self->_context, [](){})
+            ->begin([](){}, self->_make_resume());
+      });
+    };
+
   mgr->_emergency_sub = mgr->_context->node()->emergency_notice()
     .observe_on(rxcpp::identity_same_worker(mgr->_context->worker()))
     .subscribe(
-    [w = mgr->weak_from_this()](const auto& msg)
+    [w = mgr->weak_from_this(), begin_pullover](const auto& msg)
     {
       if (auto mgr = w.lock())
       {
-        if (auto task = mgr->_active_task)
+        if (mgr->_emergency_active == msg->data)
+          return;
+
+        mgr->_emergency_active = msg->data;
+        if (msg->data)
+        {
+          if (auto task = mgr->_active_task)
+          {
+            mgr->_resume_task = task->interrupt(begin_pullover);
+          }
+          else
+          {
+            mgr->_resume_task = std::nullopt;
+            begin_pullover();
+          }
+        }
+        else
         {
-          if (auto phase = task->current_phase())
-            phase->emergency_alarm(msg->data);
+          if (auto pullover = mgr->_emergency_pullover)
+            pullover->cancel();
         }
       }
     });
@@ -88,8 +129,6 @@ TaskManagerPtr TaskManager::make(
   mgr->_travel_estimator = std::make_shared<rmf_task::TravelEstimator>(
     mgr->_context->task_planner()->configuration().parameters());
 
-  mgr->_activator = std::make_shared<rmf_task::Activator>();
-
   auto schema = rmf_api_msgs::schemas::task_state;
   nlohmann::json_uri json_uri = nlohmann::json_uri{schema["$id"]};
   mgr->_schema_dictionary.insert({json_uri.url(), schema});
@@ -128,6 +167,15 @@ auto TaskManager::expected_finish_location() const -> StartSet
   return _context->location();
 }
 
+//==============================================================================
+std::optional<std::string> TaskManager::current_task_id() const
+{
+  if (_active_task)
+    return _active_task->tag()->booking()->id();
+
+  return std::nullopt;
+}
+
 //==============================================================================
 auto TaskManager::expected_finish_state() const -> State
 {
@@ -153,12 +201,6 @@ const agv::RobotContextPtr& TaskManager::context()
   return _context;
 }
 
-//==============================================================================
-const LegacyTask* TaskManager::current_task() const
-{
-  return _active_task.get();
-}
-
 //==============================================================================
 agv::ConstRobotContextPtr TaskManager::context() const
 {
@@ -173,185 +215,14 @@ std::weak_ptr<BroadcastClient> TaskManager::broadcast_client() const
 
 //==============================================================================
 void TaskManager::set_queue(
-  const std::vector<TaskManager::Assignment>& assignments,
-  const TaskManager::TaskProfiles& task_profiles)
+  const std::vector<TaskManager::Assignment>& assignments)
 {
   // We indent this block as _mutex is also locked in the _begin_next_task()
   // function that is called at the end of this function.
   {
     std::lock_guard<std::mutex> guard(_mutex);
-    _queue.clear();
-
-    // We use dynamic cast to determine the type of request and then call the
-    // appropriate make(~) function to convert the request into a task
-    for (std::size_t i = 0; i < assignments.size(); ++i)
-    {
-      const auto& a = assignments[i];
-      auto start =
-        _context->current_task_end_state().extract_plan_start().value();
-      if (i != 0)
-        start = assignments[i-1].finish_state().extract_plan_start().value();
-      start.time(a.deployment_time());
-      const auto request = a.request();
-
-      TaskProfileMsg task_profile;
-      bool auto_request = request->booking()->automatic();
-      const auto it = task_profiles.find(request->booking()->id());
-      if (it != task_profiles.end())
-      {
-        task_profile = it->second;
-      }
-      else
-      {
-        assert(auto_request);
-        // We may have an auto-generated request
-        task_profile.task_id = request->booking()->id();
-        task_profile.submission_time = _context->node()->now();
-        task_profile.description.start_time = rmf_traffic_ros2::convert(
-          request->booking()->earliest_start_time());
-      }
-
-      using namespace rmf_task::requests;
-
-      if (std::dynamic_pointer_cast<const Clean::Description>(
-          request->description()) != nullptr)
-      {
-        auto task = rmf_fleet_adapter::tasks::make_clean(
-          request,
-          _context,
-          start,
-          a.deployment_time(),
-          a.finish_state());
-
-        // Populate task_profile for auto-generated Clean request
-        if (auto_request)
-        {
-          std::shared_ptr<const rmf_task::requests::Clean::Description>
-          description = std::dynamic_pointer_cast<
-            const Clean::Description>(request->description());
-          const auto start_waypoint = description->start_waypoint();
-          const auto waypoint_name =
-            _context->navigation_graph().get_waypoint(start_waypoint).name();
-          task_profile.description.task_type.type =
-            rmf_task_msgs::msg::TaskType::TYPE_CLEAN;
-          task_profile.description.clean.start_waypoint =
-            waypoint_name != nullptr ? *waypoint_name : "";
-        }
-        task->task_profile(task_profile);
-
-        _queue.push_back(task);
-      }
-
-      else if (std::dynamic_pointer_cast<const ChargeBattery::Description>(
-          request->description()) != nullptr)
-      {
-        const auto task = tasks::make_charge_battery(
-          request,
-          _context,
-          start,
-          a.deployment_time(),
-          a.finish_state());
-
-        // Populate task_profile for auto-generated ChargeBattery request
-        if (auto_request)
-        {
-          task_profile.description.task_type.type =
-            rmf_task_msgs::msg::TaskType::TYPE_CHARGE_BATTERY;
-        }
-        task->task_profile(task_profile);
-
-        _queue.push_back(task);
-      }
-
-      else if (std::dynamic_pointer_cast<const Delivery::Description>(
-          request->description()) != nullptr)
-      {
-        const auto task = tasks::make_delivery(
-          request,
-          _context,
-          start,
-          a.deployment_time(),
-          a.finish_state(),
-          task_profile.description.delivery);
-
-        // Populate task_profile for auto-generated Delivery request
-        if (auto_request)
-        {
-          std::shared_ptr<const rmf_task::requests::Delivery::Description>
-          description = std::dynamic_pointer_cast<
-            const Delivery::Description>(request->description());
-          const auto& graph = _context->navigation_graph();
-          const auto pickup_waypoint = description->pickup_waypoint();
-          const auto pickup_name =
-            graph.get_waypoint(pickup_waypoint).name();
-          const auto dropoff_waypoint = description->dropoff_waypoint();
-          const auto dropoff_name =
-            graph.get_waypoint(dropoff_waypoint).name();
-          task_profile.description.task_type.type =
-            rmf_task_msgs::msg::TaskType::TYPE_DELIVERY;
-          task_profile.description.delivery.pickup_place_name =
-            pickup_name != nullptr ? *pickup_name : "";
-          task_profile.description.delivery.dropoff_place_name =
-            dropoff_name != nullptr ? *dropoff_name : "";
-        }
-        task->task_profile(task_profile);
-
-        _queue.push_back(task);
-      }
-
-      else if (std::dynamic_pointer_cast<const Loop::Description>(request->
-        description()) != nullptr)
-      {
-        const auto task = tasks::make_loop(
-          request,
-          _context,
-          start,
-          a.deployment_time(),
-          a.finish_state());
-
-        // Populate task_profile for auto-generated Loop request
-        if (auto_request)
-        {
-          std::shared_ptr<const rmf_task::requests::Loop::Description>
-          description = std::dynamic_pointer_cast<
-            const Loop::Description>(request->description());
-          const auto& graph = _context->navigation_graph();
-          const auto start_waypoint = description->start_waypoint();
-          const auto start_name =
-            graph.get_waypoint(start_waypoint).name();
-          const auto finish_waypoint = description->finish_waypoint();
-          const auto finish_name =
-            graph.get_waypoint(finish_waypoint).name();
-          task_profile.description.loop.num_loops = description->num_loops();
-          task_profile.description.task_type.type =
-            rmf_task_msgs::msg::TaskType::TYPE_LOOP;
-          task_profile.description.loop.start_name =
-            start_name != nullptr ? *start_name : "";
-          task_profile.description.loop.finish_name =
-            finish_name != nullptr ? *finish_name : "";
-        }
-        task->task_profile(task_profile);
-
-        _queue.push_back(task);
-      }
-
-      else
-      {
-        RCLCPP_WARN(
-          _context->node()->get_logger(),
-          "[TaskManager] Un-supported request type in assignment list. "
-          "Please update the implementation of TaskManager::set_queue() to "
-          "support request with task_id:[%s]",
-          a.request()->booking()->id().c_str());
-
-        continue;
-      }
-
-      // publish queued task
-      TaskSummaryMsg msg;
-      _populate_task_summary(_queue.back(), msg.STATE_QUEUED, msg);
-      _context->node()->task_summary()->publish(msg);
-    }
+    _queue = assignments;
+    _publish_task_queue();
   }
 
   _begin_next_task();
@@ -365,12 +236,12 @@ const std::vector<rmf_task::ConstRequestPtr> TaskManager::requests() const
   requests.reserve(_queue.size());
   for (const auto& task : _queue)
   {
-    if (task->request()->booking()->automatic())
+    if (task.request()->booking()->automatic())
     {
       continue;
     }
 
-    requests.push_back(task->request());
+    requests.push_back(task.request());
   }
   return requests;
 }
@@ -443,73 +314,35 @@ void TaskManager::_begin_next_task()
   // TODO: Reactively replan task assignments across agents in a fleet every
   // time as task is completed.
   const auto deployment_time = std::min(
-    next_task->deployment_time(),
-    next_task->request()->booking()->earliest_start_time());
+    next_task.deployment_time(),
+    next_task.request()->booking()->earliest_start_time());
 
   if (now >= deployment_time)
   {
     // Update state in RobotContext and Assign active task
-    _context->current_task_end_state(_queue.front()->finish_state());
-    _context->current_task_id(_queue.front()->id());
-    _active_task = std::move(_queue.front());
+    const auto id = _queue.front().request()->booking()->id();
+    _context->current_task_end_state(_queue.front().finish_state());
+    _context->current_task_id(id);
+    const auto assignment = _queue.front();
+    _active_task = _context->task_activator()->activate(
+      _context->make_get_state(),
+      _context->task_parameters(),
+      *assignment.request(),
+      _update_cb(),
+      _checkpoint_cb(),
+      _phase_finished_cb(),
+      _task_finished(id));
+
     _queue.erase(_queue.begin());
 
     RCLCPP_INFO(
       _context->node()->get_logger(),
       "Beginning new task [%s] for [%s]. Remaining queue size: %ld",
-      _active_task->id().c_str(),
+      _active_task->tag()->booking()->id().c_str(),
       _context->requester_id().c_str(),
       _queue.size());
 
-    _task_sub = _active_task->observe()
-      .observe_on(rxcpp::identity_same_worker(_context->worker()))
-      .subscribe(
-      [me = weak_from_this(), active_task = _active_task](LegacyTask::StatusMsg msg)
-      {
-        const auto self = me.lock();
-        if (!self)
-          return;
-
-        self->_populate_task_summary(active_task, msg.state, msg);
-        self->_context->node()->task_summary()->publish(msg);
-      },
-      [me = weak_from_this(), active_task = _active_task](std::exception_ptr e)
-      {
-        const auto self = me.lock();
-        if (!self)
-          return;
-
-        TaskSummaryMsg msg;
-
-        try
-        {
-          std::rethrow_exception(e);
-        }
-        catch (const std::exception& e)
-        {
-          msg.status = e.what();
-        }
-
-        self->_populate_task_summary(active_task, msg.STATE_FAILED, msg);
-        self->_context->node()->task_summary()->publish(msg);
-      },
-      [me = weak_from_this(), active_task = _active_task]()
-      {
-        const auto self = me.lock();
-        if (!self)
-          return;
-
-        TaskSummaryMsg msg;
-        self->_populate_task_summary(
-          active_task, msg.STATE_COMPLETED, msg);
-        self->_context->node()->task_summary()->publish(msg);
-
-        self->_active_task = nullptr;
-        self->_context->current_task_id(std::nullopt);
-      });
-
-    _active_task->begin();
-    _register_executed_task(_active_task->id());
+    _register_executed_task(_active_task->tag()->booking()->id());
   }
   else
   {
@@ -593,6 +426,46 @@ void TaskManager::_begin_waiting()
     });
 }
 
+//==============================================================================
+std::function<void()> TaskManager::_make_resume()
+{
+  return [w = weak_from_this()]()
+  {
+    const auto self = w.lock();
+    if (!self)
+      return;
+
+    self->_resume();
+  };
+}
+
+//==============================================================================
+void TaskManager::_resume()
+{
+  _context->worker().schedule(
+    [w = weak_from_this()](const auto&)
+    {
+      const auto self = w.lock();
+      if (!self)
+        return;
+
+      if (self->_emergency_active)
+        return;
+
+      self->_emergency_pullover = nullptr;
+      if (self->_resume_task.has_value())
+      {
+        auto resume = *std::move(self->_resume_task);
+        self->_resume_task = std::nullopt;
+        resume();
+      }
+      else
+      {
+        self->_begin_next_task();
+      }
+    });
+}
+
 //==============================================================================
 void TaskManager::retreat_to_charger()
 {
@@ -710,7 +583,7 @@ void TaskManager::_populate_task_summary(
 
     msg.start_time = _context->node()->now();
     msg.end_time = _queue.empty() ? msg.start_time : rmf_traffic_ros2::convert(
-      _queue.front()->deployment_time());
+      _queue.front().deployment_time());
     // Make the task type explicit
     msg.task_profile.description.task_type.type =
       rmf_task_msgs::msg::TaskType::TYPE_STATION;
@@ -812,55 +685,84 @@ bool TaskManager::_validate_json(
 }
 
 //==============================================================================
-void TaskManager::_update(rmf_task::Phase::ConstSnapshotPtr snapshot)
+std::function<void(rmf_task::Phase::ConstSnapshotPtr)>
+TaskManager::_update_cb()
 {
-  // TODO
+  return [w = weak_from_this()](rmf_task::Phase::ConstSnapshotPtr snapshot)
+    {
+      const auto self = w.lock();
+      if (!self)
+        return;
 
-  auto task_state_update = _task_state_update_json;
-  task_state_update["data"] = _active_task_state;
-  _validate_and_publish_json(
-    task_state_update, rmf_api_msgs::schemas::task_state_update);
+      // TODO
+      auto task_state_update = self->_task_state_update_json;
+      task_state_update["data"] = self->_active_task_state;
+      self->_validate_and_publish_json(
+        task_state_update, rmf_api_msgs::schemas::task_state_update);
+    };
 }
 
 //==============================================================================
-void TaskManager::_checkpoint(rmf_task::Task::Active::Backup backup)
+std::function<void(rmf_task::Task::Active::Backup)>
+TaskManager::_checkpoint_cb()
 {
-  // TODO
+  return [w = weak_from_this()](rmf_task::Task::Active::Backup backup)
+  {
+    const auto self = w.lock();
+    if (!self)
+      return;
 
-  auto task_state_update = _task_state_update_json;
-  task_state_update["data"] = _active_task_state;
-  _validate_and_publish_json(
-    task_state_update, rmf_api_msgs::schemas::task_state_update);
+    // TODO
+
+    auto task_state_update = self->_task_state_update_json;
+    task_state_update["data"] = self->_active_task_state;
+    self->_validate_and_publish_json(
+      task_state_update, rmf_api_msgs::schemas::task_state_update);
+  };
 }
 
 //==============================================================================
-void TaskManager::_phase_finished(
-  rmf_task::Phase::ConstCompletedPtr completed_phase)
+std::function<void(rmf_task::Phase::ConstCompletedPtr)>
+TaskManager::_phase_finished_cb()
 {
-  // TODO
+  return [w = weak_from_this()](rmf_task::Phase::ConstCompletedPtr phase)
+    {
+      const auto self = w.lock();
+      if (!self)
+        return;
 
-  auto task_state_update = _task_state_update_json;
-  task_state_update["data"] = _active_task_state;
-  _validate_and_publish_json(
-    task_state_update, rmf_api_msgs::schemas::task_state_update);
+      // TODO
+
+      auto task_state_update = self->_task_state_update_json;
+      task_state_update["data"] = self->_active_task_state;
+      self->_validate_and_publish_json(
+        task_state_update, rmf_api_msgs::schemas::task_state_update);
+    };
 }
 
 //==============================================================================
-void TaskManager::_task_finished()
+std::function<void()> TaskManager::_task_finished(std::string id)
 {
-  // TODO
+  return [w = weak_from_this(), id]()
+    {
+      const auto self = w.lock();
+      if (!self)
+        return;
+
+      // TODO
 
-  auto task_state_update = _task_state_update_json;
-  task_state_update["data"] = _active_task_state;
-  _validate_and_publish_json(
-    task_state_update, rmf_api_msgs::schemas::task_state_update);
+      auto task_state_update = self->_task_state_update_json;
+      task_state_update["data"] = self->_active_task_state;
+      self->_validate_and_publish_json(
+        task_state_update, rmf_api_msgs::schemas::task_state_update);
 
-  auto task_log_update = _task_log_update_msg;
-  task_log_update["data"] = _task_logs[_active_task_state["booking"]["id"]];
-  _validate_and_publish_json(
-    task_log_update, rmf_api_msgs::schemas::task_log_update);
+      auto task_log_update = self->_task_log_update_msg;
+      task_log_update["data"] = self->_task_logs[self->_active_task_state["booking"]["id"]];
+      self->_validate_and_publish_json(
+        task_log_update, rmf_api_msgs::schemas::task_log_update);
 
-  _active_task_state = {};
+      self->_active_task_state = {};
+    };
 }
 
 } // namespace rmf_fleet_adapter
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
index ecd62bac2..41fa8d05e 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
@@ -27,6 +27,8 @@
 #include <rmf_task/TaskPlanner.hpp>
 #include <rmf_task/Activator.hpp>
 
+#include <rmf_task_sequence/Event.hpp>
+
 #include <rmf_fleet_msgs/msg/robot_mode.hpp>
 #include <rmf_task_msgs/msg/task_summary.hpp>
 
@@ -70,17 +72,15 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
 
   std::weak_ptr<BroadcastClient> broadcast_client() const;
 
-  const LegacyTask* current_task() const;
-
   /// Set the queue for this task manager with assignments generated from the
   /// task planner
-  void set_queue(
-    const std::vector<Assignment>& assignments,
-    const TaskProfiles& task_profiles = {});
+  void set_queue(const std::vector<Assignment>& assignments);
 
   /// Get the non-charging requests among pending tasks
   const std::vector<rmf_task::ConstRequestPtr> requests() const;
 
+  std::optional<std::string> current_task_id() const;
+
   /// The state of the robot.
   State expected_finish_state() const;
 
@@ -105,8 +105,12 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
 
   agv::RobotContextPtr _context;
   std::weak_ptr<BroadcastClient> _broadcast_client;
-  std::shared_ptr<LegacyTask> _active_task;
-  std::vector<std::shared_ptr<LegacyTask>> _queue;
+  rmf_task::ConstActivatorPtr _task_activator;
+  rmf_task::Task::ActivePtr _active_task;
+  std::optional<rmf_task::Task::Active::Resume> _resume_task;
+  bool _emergency_active = false;
+  rmf_task_sequence::Event::ActivePtr _emergency_pullover;
+  std::vector<Assignment> _queue;
   rmf_utils::optional<Start> _expected_finish_location;
   rxcpp::subscription _task_sub;
   rxcpp::subscription _emergency_sub;
@@ -131,9 +135,6 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
   // retreat. TODO(YV): Expose the TaskPlanner's TravelEstimator.
   std::shared_ptr<rmf_task::TravelEstimator> _travel_estimator;
 
-  // The activator to generate rmf_task::Task::Active instances from requests
-  std::shared_ptr<rmf_task::Activator> _activator;
-
   // Map schema url to schema for validator.
   // TODO: Get this and loader from FleetUpdateHandle
   std::unordered_map<std::string, nlohmann::json> _schema_dictionary = {};
@@ -167,9 +168,18 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
   /// Begin responsively waiting for the next task
   void _begin_waiting();
 
+  /// Make the callback for resuming
+  std::function<void()> _make_resume();
+
+  /// Resume whatever the task manager should be doing
+  void _resume();
+
   /// Get the current state of the robot
   rmf_task::State _get_state() const;
 
+  /// Publish the current pending task list
+  void _publish_task_queue();
+
   /// Schema loader for validating jsons
   void _schema_loader(
     const nlohmann::json_uri& id, nlohmann::json& value) const;
@@ -188,16 +198,16 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
     const nlohmann::json& schema) const;
 
   /// Callback for when the task has a significat update
-  void _update(rmf_task::Phase::ConstSnapshotPtr snapshot);
+  std::function<void(rmf_task::Phase::ConstSnapshotPtr)> _update_cb();
 
   /// Callback for when the task reaches a checkpoint
-  void _checkpoint(rmf_task::Task::Active::Backup backup);
+  std::function<void(rmf_task::Task::Active::Backup)> _checkpoint_cb();
 
   /// Callback for when a phase within a task has finished
-  void _phase_finished(rmf_task::Phase::ConstCompletedPtr completed_phase);
+  std::function<void(rmf_task::Phase::ConstCompletedPtr)> _phase_finished_cb();
 
   /// Callback for when the task has finished
-  void _task_finished();
+  std::function<void()> _task_finished(std::string id);
 
   // TODO: Assuming each LegacyTask::Active instance stores a weak_ptr to this
   // TaskManager, the implementations of the corresponding functions in
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
index 3ff96b638..6a706fd36 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
@@ -28,6 +28,7 @@
 #include "../tasks/Delivery.hpp"
 #include "../tasks/Loop.hpp"
 #include "../tasks/Clean.hpp"
+#include "../events/GoToPlace.hpp"
 
 #include <rmf_task/Constraints.hpp>
 #include <rmf_task/Parameters.hpp>
@@ -629,7 +630,7 @@ void FleetUpdateHandle::Implementation::dispatch_request_cb(
     std::size_t index = 0;
     for (auto& t : task_managers)
     {
-      t.second->set_queue(assignments[index], task_profile_map);
+      t.second->set_queue(assignments[index]);
       ++index;
     }
 
@@ -716,7 +717,7 @@ void FleetUpdateHandle::Implementation::dispatch_request_cb(
     std::size_t index = 0;
     for (auto& t : task_managers)
     {
-      t.second->set_queue(assignments[index], task_profile_map);
+      t.second->set_queue(assignments[index]);
       ++index;
     }
 
@@ -848,7 +849,7 @@ rmf_fleet_msgs::msg::RobotState convert_state(const TaskManager& mgr)
   return rmf_fleet_msgs::build<rmf_fleet_msgs::msg::RobotState>()
     .name(context.name())
     .model(context.description().owner())
-    .task_id(mgr.current_task() ? mgr.current_task()->id() : "")
+    .task_id(mgr.current_task_id().value_or(""))
     // TODO(MXG): We could keep track of the seq value and increment it once
     // with each publication. This is not currently an important feature
     // outside of the fleet driver, so for now we just set it to zero.
@@ -964,11 +965,24 @@ void FleetUpdateHandle::Implementation::add_standard_tasks()
   rmf_task_sequence::phases::SimplePhase::add(
     *activation.phase, activation.event);
 
+  events::GoToPlace::add(*activation.event);
+
   tasks::add_delivery(
     *activation.task,
     activation.phase,
     *activation.event,
     node->clock());
+
+  tasks::add_loop(
+    *activation.task,
+    activation.phase,
+    node->clock());
+
+  tasks::add_clean(
+    *activation.task,
+    activation.phase,
+    activation.event,
+    node->clock());
 }
 
 //==============================================================================
@@ -1149,6 +1163,7 @@ void FleetUpdateHandle::add_robot(
           fleet->_pimpl->snappable,
           fleet->_pimpl->planner,
           fleet->_pimpl->activation.task,
+          fleet->_pimpl->task_parameters,
           fleet->_pimpl->node,
           fleet->_pimpl->worker,
           fleet->_pimpl->default_maximum_delay,
@@ -1270,6 +1285,8 @@ void FleetUpdateHandle::close_lanes(std::vector<std::size_t> lane_indices)
       *self->_pimpl->planner =
       std::make_shared<const rmf_traffic::agv::Planner>(
         new_config, rmf_traffic::agv::Planner::Options(nullptr));
+
+      self->_pimpl->task_parameters->planner(*self->_pimpl->planner);
     });
 }
 
@@ -1310,6 +1327,8 @@ void FleetUpdateHandle::open_lanes(std::vector<std::size_t> lane_indices)
       *self->_pimpl->planner =
       std::make_shared<const rmf_traffic::agv::Planner>(
         new_config, rmf_traffic::agv::Planner::Options(nullptr));
+
+      self->_pimpl->task_parameters->planner(*self->_pimpl->planner);
     });
 }
 
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp
index 38437dd61..975fe52e7 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp
@@ -233,6 +233,18 @@ RobotContext& RobotContext::maximum_delay(
   return *this;
 }
 
+//==============================================================================
+const rmf_task::ConstActivatorPtr& RobotContext::task_activator() const
+{
+  return _task_activator;
+}
+
+//==============================================================================
+const rmf_task::ConstParametersPtr& RobotContext::task_parameters() const
+{
+  return _task_parameters;
+}
+
 //==============================================================================
 const rmf_task::State& RobotContext::current_task_end_state() const
 {
@@ -247,6 +259,18 @@ RobotContext& RobotContext::current_task_end_state(
   return *this;
 }
 
+//==============================================================================
+std::function<rmf_task::State()> RobotContext::make_get_state()
+{
+  return [self = shared_from_this()]()
+    {
+      rmf_task::State state;
+      state.load(self->_location.front());
+      state.insert<GetContext>(GetContext{self->shared_from_this()});
+      return state;
+    };
+}
+
 //==============================================================================
 const std::string* RobotContext::current_task_id() const
 {
@@ -360,6 +384,7 @@ RobotContext::RobotContext(
   std::shared_ptr<const Snappable> schedule,
   std::shared_ptr<std::shared_ptr<const rmf_traffic::agv::Planner>> planner,
   rmf_task::ConstActivatorPtr activator,
+  rmf_task::ConstParametersPtr parameters,
   std::shared_ptr<rmf_fleet_adapter::agv::Node> node,
   const rxcpp::schedulers::worker& worker,
   rmf_utils::optional<rmf_traffic::Duration> maximum_delay,
@@ -371,6 +396,7 @@ RobotContext::RobotContext(
   _schedule(std::move(schedule)),
   _planner(std::move(planner)),
   _task_activator(std::move(activator)),
+  _task_parameters(std::move(parameters)),
   _stubbornness(std::make_shared<int>(0)),
   _node(std::move(node)),
   _worker(worker),
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp
index 23b8536d4..b91979ba9 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp
@@ -130,6 +130,8 @@ class RobotContext
   /// Get the task activator for this robot
   const rmf_task::ConstActivatorPtr& task_activator() const;
 
+  const rmf_task::ConstParametersPtr& task_parameters() const;
+
   /// Set the state of this robot at the end of its current task
   RobotContext& current_task_end_state(const rmf_task::State& state);
 
@@ -137,6 +139,9 @@ class RobotContext
   // current task
   const rmf_task::State& current_task_end_state() const;
 
+  /// Make a way to get the state for this robot
+  std::function<rmf_task::State()> make_get_state();
+
   /// Get the current task ID of the robot, or a nullptr if the robot is not
   /// performing any task
   const std::string* current_task_id() const;
@@ -188,6 +193,7 @@ class RobotContext
     std::shared_ptr<const Snappable> schedule,
     std::shared_ptr<std::shared_ptr<const rmf_traffic::agv::Planner>> planner,
     rmf_task::ConstActivatorPtr activator,
+    rmf_task::ConstParametersPtr parameters,
     std::shared_ptr<Node> node,
     const rxcpp::schedulers::worker& worker,
     rmf_utils::optional<rmf_traffic::Duration> maximum_delay,
@@ -200,6 +206,7 @@ class RobotContext
   std::shared_ptr<const Snappable> _schedule;
   std::shared_ptr<std::shared_ptr<const rmf_traffic::agv::Planner>> _planner;
   rmf_task::ConstActivatorPtr _task_activator;
+  rmf_task::ConstParametersPtr _task_parameters;
   std::shared_ptr<const rmf_traffic::Profile> _profile;
 
   std::shared_ptr<void> _negotiation_license;
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
index 5688ea829..046238144 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
@@ -164,6 +164,7 @@ class FleetUpdateHandle::Implementation
   // LegacyTask planner params
   std::shared_ptr<rmf_task::CostCalculator> cost_calculator =
     rmf_task::BinaryPriorityScheme::make_cost_calculator();
+  std::shared_ptr<rmf_task::Parameters> task_parameters = nullptr;
   std::shared_ptr<rmf_task::TaskPlanner> task_planner = nullptr;
 
   rmf_utils::optional<rmf_traffic::Duration> default_maximum_delay =
@@ -234,6 +235,19 @@ class FleetUpdateHandle::Implementation
 
     handle->_pimpl->add_standard_tasks();
 
+    // TODO(MXG): This is a very crude implementation. We create a dummy set of
+    // task planner parameters to stand in until the user sets the task planner
+    // parameters. We'll distribute this shared_ptr to the robot contexts and
+    // update it with the proper values once they're available.
+    //
+    // Note that we also need to manually update the traffic planner when it
+    // changes.
+    handle->_pimpl->task_parameters =
+      std::make_shared<rmf_task::Parameters>(
+        *handle->_pimpl->planner,
+        *rmf_battery::agv::BatterySystem::make(1.0, 1.0, 1.0),
+        nullptr, nullptr, nullptr);
+
     handle->_pimpl->fleet_state_pub = handle->_pimpl->node->fleet_state();
     handle->_pimpl->fleet_state_timer =
       handle->_pimpl->node->try_create_wall_timer(
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp
index 2f310e4b4..046aa4197 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp
@@ -19,6 +19,14 @@
 
 #include "Clean.hpp"
 
+#include <rmf_task_sequence/Task.hpp>
+#include <rmf_task_sequence/events/WaitFor.hpp>
+#include <rmf_task_sequence/events/Bundle.hpp>
+#include <rmf_task_sequence/phases/SimplePhase.hpp>
+
+#include "../events/Error.hpp"
+#include "../events/GoToPlace.hpp"
+
 namespace rmf_fleet_adapter {
 namespace tasks {
 
@@ -47,7 +55,7 @@ std::shared_ptr<LegacyTask> make_clean(
 
   LegacyTask::PendingPhases phases;
   // TODO(YV): If the robot is already at the start_waypoint, the Dock entry event will
-  // not be triggered and the task will be competed without any cleaning
+  // not be triggered and the task will be completed without any cleaning
   // performed. To avoid this, we request the robot to re-enter the lane.
   // This should be fixed when we define a new phase cleaning and not rely on
   // docking
@@ -75,5 +83,200 @@ std::shared_ptr<LegacyTask> make_clean(
     request);
 }
 
+//==============================================================================
+// TODO(MXG): This implementation that uses the Dock command is crude and
+// should be replaced with an explicit cleaning activation command.
+struct CleanEvent : public rmf_task_sequence::events::WaitFor::Description
+{
+  CleanEvent(const rmf_task::requests::Clean::Description& clean)
+  : rmf_task_sequence::events::WaitFor::Description(rmf_traffic::Duration(0)),
+    start_waypoint(clean.start_waypoint()),
+    end_waypoint(clean.end_waypoint())
+  {
+    // Do nothing
+  }
+
+  struct DockChecker : public rmf_traffic::agv::Graph::Lane::Executor
+  {
+    void execute(const Dock& dock) final { found_dock = dock.dock_name(); };
+    void execute(const Wait&) final {};
+    void execute(const DoorOpen&) final {};
+    void execute(const LiftMove&) final {};
+    void execute(const DoorClose&) final {};
+    void execute(const LiftDoorOpen&) final {};
+    void execute(const LiftSessionEnd&) final {};
+    void execute(const LiftSessionBegin&) final {};
+
+    std::optional<std::string> found_dock = std::nullopt;
+  };
+
+  static rmf_task_sequence::Event::StandbyPtr standby(
+    const rmf_task_sequence::Event::AssignIDPtr& id,
+    const std::function<rmf_task::State()>& get_state,
+    const rmf_task::ConstParametersPtr& parameters,
+    const CleanEvent& description,
+    std::function<void()> update)
+  {
+    const auto state = get_state();
+    const auto context = state.get<agv::GetContext>()->value;
+
+    // Check if going to the start waypoint from the robot's current location
+    // will successfully pass through the dock event
+    const auto result = context->planner()->plan(
+      context->location(),
+      description.start_waypoint, {nullptr});
+
+    // TODO(MXG): Make this name more detailed
+    const std::string name = "Clean";
+
+    if (!result.success())
+    {
+      const auto error_state = rmf_task::events::SimpleEventState::make(
+        id->assign(), name, "", rmf_task::Event::Status::Error, {},
+        context->clock());
+
+      error_state->update_log().error(
+        "Could not find a path to the cleaning zone from the robot's current "
+        "location");
+
+      return events::Error::Standby::make(std::move(error_state));
+    }
+
+    bool will_pass_through_dock = false;
+    const auto& graph = context->planner()->get_configuration().graph();
+    for (const auto& wp : result->get_waypoints())
+    {
+      if (!wp.graph_index().has_value())
+        continue;
+
+      if (*wp.graph_index() == description.start_waypoint)
+      {
+        for (const auto& l : wp.approach_lanes())
+        {
+          DockChecker check;
+          const auto& lane = graph.get_lane(l);
+          if (lane.entry().event())
+          {
+            if (lane.entry().event()->execute(check).found_dock.has_value())
+            {
+              will_pass_through_dock = true;
+              break;
+            }
+          }
+
+          if (lane.exit().event())
+          {
+            if (lane.exit().event()->execute(check).found_dock.has_value())
+            {
+              will_pass_through_dock = true;
+              break;
+            }
+          }
+        }
+      }
+    }
+
+    using StandbyPtr = rmf_task_sequence::Event::StandbyPtr;
+    using UpdateFn = std::function<void()>;
+    using MakeStandby = std::function<StandbyPtr(UpdateFn)>;
+    using GoToPlaceDesc = rmf_task_sequence::events::GoToPlace::Description;
+
+    auto go_to_place = [id, get_state, parameters](std::size_t wp)
+    {
+      return [id, get_state, parameters, wp](UpdateFn update)
+      {
+        return events::GoToPlace::Standby::make(
+          id, get_state, parameters,
+          *GoToPlaceDesc::make(wp), std::move(update));
+      };
+    };
+
+    std::vector<MakeStandby> events;
+    if (!will_pass_through_dock)
+    {
+      // If the robot does not pass through the dock on its way to the start
+      // location then it is likely already sitting on the start location. We
+      // should order the robot to the exit first and then back to the start.
+      //
+      // This strategy still has flaws because we can't rule out that by the
+      // time the robot is generating its plan to reach the exit it might
+      // accidentally trigger the docking action. This is one of the reasons we
+      // should fix the overall implementation.
+      events.push_back(go_to_place(description.end_waypoint));
+    }
+
+    events.push_back(go_to_place(description.start_waypoint));
+    events.push_back(go_to_place(description.end_waypoint));
+
+    auto sequence_state = rmf_task::events::SimpleEventState::make(
+      id->assign(), name, "", rmf_task::Event::Status::Standby, {},
+      context->clock());
+
+    return rmf_task_sequence::events::Bundle::standby(
+      rmf_task_sequence::events::Bundle::Type::Sequence,
+      events, std::move(sequence_state), std::move(update));
+  }
+
+  static void add(rmf_task_sequence::Event::Initializer& initializer)
+  {
+    initializer.add<CleanEvent>(
+      [](
+        const rmf_task_sequence::Event::AssignIDPtr& id,
+        const std::function<rmf_task::State()>& get_state,
+        const rmf_task::ConstParametersPtr& parameters,
+        const CleanEvent& description,
+        std::function<void()> update) -> rmf_task_sequence::Event::StandbyPtr
+      {
+        return standby(
+          id, get_state, parameters, description, std::move(update));
+      },
+      [](
+        const rmf_task_sequence::Event::AssignIDPtr& id,
+        const std::function<rmf_task::State()>& get_state,
+        const rmf_task::ConstParametersPtr& parameters,
+        const CleanEvent& description,
+        const nlohmann::json&,
+        std::function<void()> update,
+        std::function<void()> checkpoint,
+        std::function<void()> finished) -> rmf_task_sequence::Event::ActivePtr
+      {
+        return standby(
+          id, get_state, parameters, description, std::move(update))
+          ->begin(std::move(checkpoint), std::move(finished));
+      });
+  }
+
+  std::size_t start_waypoint;
+  std::size_t end_waypoint;
+};
+
+//==============================================================================
+void add_clean(
+  rmf_task::Activator& task_activator,
+  const rmf_task_sequence::Phase::ConstActivatorPtr& phase_activator,
+  const rmf_task_sequence::Event::InitializerPtr& event_initializer,
+  std::function<rmf_traffic::Time()> clock)
+{
+  using Clean = rmf_task::requests::Clean;
+  using Phase = rmf_task_sequence::phases::SimplePhase;
+
+  CleanEvent::add(*event_initializer);
+
+  auto clean_unfolder =
+    [](const Clean::Description& clean)
+  {
+    rmf_task_sequence::Task::Builder builder;
+    builder.add_phase(
+      Phase::Description::make(std::make_shared<CleanEvent>(clean)), {});
+
+    // TODO(MXG): Make the name and detail more detailed
+    return *builder.build("Clean", "");
+  };
+
+  rmf_task_sequence::Task::unfold<rmf_task::requests::Clean::Description>(
+    std::move(clean_unfolder), task_activator,
+    phase_activator, std::move(clock));
+}
+
 } // namespace task
 } // namespace rmf_fleet_adapter
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp
index 7d95dc381..ea18f0e24 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp
@@ -27,6 +27,9 @@
 #include <rmf_task/State.hpp>
 #include <rmf_task/requests/Clean.hpp>
 
+#include <rmf_task_sequence/Phase.hpp>
+#include <rmf_task_sequence/Event.hpp>
+
 namespace rmf_fleet_adapter {
 namespace tasks {
 
@@ -38,6 +41,12 @@ std::shared_ptr<LegacyTask> make_clean(
   const rmf_traffic::Time deployment_time,
   const rmf_task::State finish_state);
 
+void add_clean(
+  rmf_task::Activator& task_activator,
+  const rmf_task_sequence::Phase::ConstActivatorPtr& phase_activator,
+  const rmf_task_sequence::Event::InitializerPtr& event_initializer,
+  std::function<rmf_traffic::Time()> clock);
+
 } // namespace tasks
 } // namespace rmf_fleet_adapter
 
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp
index 479b2e525..e008a6e6b 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp
@@ -192,6 +192,11 @@ struct TransferItems : public rmf_task_sequence::events::WaitFor::Description
     else
       name = "Load";
 
+    if (description.payload.components().size() == 1)
+      name += "item";
+    else
+      name += "items";
+
     if (!task_id)
     {
       const auto error_state = rmf_task::events::SimpleEventState::make(
@@ -224,8 +229,6 @@ struct TransferItems : public rmf_task_sequence::events::WaitFor::Description
         context, *task_id, description.target,
         context->itinerary().description().owner(),
         std::move(items));
-
-      name = "Unload";
     }
     else
     {
@@ -244,15 +247,8 @@ struct TransferItems : public rmf_task_sequence::events::WaitFor::Description
         context, *task_id, description.target,
         context->itinerary().description().owner(),
         std::move(items));
-
-      name = "Load";
     }
 
-    if (description.payload.components().size() == 1)
-      name += "item";
-    else
-      name += "items";
-
     return events::LegacyPhaseShim::Standby::make(
       legacy, context->worker(), context->clock(), id, std::move(update), name);
   }
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp
index ba8e39d52..39b24ead2 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp
@@ -19,6 +19,10 @@
 
 #include "../phases/GoToPlace.hpp"
 
+#include <rmf_task_sequence/Task.hpp>
+#include <rmf_task_sequence/phases/SimplePhase.hpp>
+#include <rmf_task_sequence/events/GoToPlace.hpp>
+
 namespace rmf_fleet_adapter {
 namespace tasks {
 
@@ -109,5 +113,39 @@ std::shared_ptr<LegacyTask> make_loop(
     request);
 }
 
+//==============================================================================
+void add_loop(
+  rmf_task::Activator& task_activator,
+  const rmf_task_sequence::Phase::ConstActivatorPtr& phase_activator,
+  std::function<rmf_traffic::Time()> clock)
+{
+  using Loop = rmf_task::requests::Loop;
+  using Phase = rmf_task_sequence::phases::SimplePhase;
+  using GoToPlace = rmf_task_sequence::events::GoToPlace;
+
+  auto loop_unfolder =
+    [](const Loop::Description& loop)
+    {
+      rmf_task_sequence::Task::Builder builder;
+      for (std::size_t i=0; i < loop.num_loops(); ++i)
+      {
+        builder
+          .add_phase(
+            Phase::Description::make(
+              GoToPlace::Description::make(loop.start_waypoint())), {})
+          .add_phase(
+            Phase::Description::make(
+              GoToPlace::Description::make(loop.finish_waypoint())), {});
+      }
+
+      // TODO(MXG): Consider making the category and detail more details
+      return *builder.build("Loop", "");
+    };
+
+  rmf_task_sequence::Task::unfold<rmf_task::requests::Loop::Description>(
+    std::move(loop_unfolder), task_activator,
+    phase_activator, std::move(clock));
+}
+
 } // namespace tasks
 } // namespace rmf_fleet_adapter
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.hpp
index 99ee01248..b02d5b1e7 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.hpp
@@ -23,6 +23,8 @@
 
 #include <rmf_task/requests/Loop.hpp>
 
+#include <rmf_task_sequence/Phase.hpp>
+
 namespace rmf_fleet_adapter {
 namespace tasks {
 
@@ -34,6 +36,12 @@ std::shared_ptr<LegacyTask> make_loop(
   const rmf_traffic::Time deployment_time,
   const rmf_task::State finish_state);
 
+//==============================================================================
+void add_loop(
+  rmf_task::Activator& task_activator,
+  const rmf_task_sequence::Phase::ConstActivatorPtr& phase_activator,
+  std::function<rmf_traffic::Time()> clock);
+
 } // namespace tasks
 } // namespace rmf_fleet_adapter
 

From 7664c7686a0f27fa42eac147718bc5d7ef56126f Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Mon, 20 Dec 2021 22:31:09 +0800
Subject: [PATCH 21/79] Publish task states

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 rmf_fleet_adapter/CMakeLists.txt              |   1 +
 .../src/rmf_fleet_adapter/TaskManager.cpp     | 278 ++++++++++++++++--
 .../src/rmf_fleet_adapter/TaskManager.hpp     |  11 +-
 3 files changed, 270 insertions(+), 20 deletions(-)

diff --git a/rmf_fleet_adapter/CMakeLists.txt b/rmf_fleet_adapter/CMakeLists.txt
index 3ee53d4f3..96a0dd67f 100644
--- a/rmf_fleet_adapter/CMakeLists.txt
+++ b/rmf_fleet_adapter/CMakeLists.txt
@@ -475,6 +475,7 @@ target_include_directories(close_lanes
 ament_export_targets(rmf_fleet_adapter HAS_LIBRARY_TARGET)
 ament_export_dependencies(
   rmf_task
+  rmf_task_sequence
   rmf_traffic_ros2
   rmf_fleet_msgs
   rmf_door_msgs
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
index 81904f2c8..f4e6267b0 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
@@ -129,6 +129,14 @@ TaskManagerPtr TaskManager::make(
   mgr->_travel_estimator = std::make_shared<rmf_task::TravelEstimator>(
     mgr->_context->task_planner()->configuration().parameters());
 
+  mgr->_update_timer = mgr->_context->node()->try_create_wall_timer(
+    std::chrono::milliseconds(100),
+    [w = mgr->weak_from_this()]()
+  {
+    if (const auto self = w.lock())
+      self->_consider_publishing_updates();
+  });
+
   auto schema = rmf_api_msgs::schemas::task_state;
   nlohmann::json_uri json_uri = nlohmann::json_uri{schema["$id"]};
   mgr->_schema_dictionary.insert({json_uri.url(), schema});
@@ -153,7 +161,8 @@ TaskManager::TaskManager(
   agv::RobotContextPtr context,
   std::weak_ptr<BroadcastClient> broadcast_client)
 : _context(std::move(context)),
-  _broadcast_client(std::move(broadcast_client))
+  _broadcast_client(std::move(broadcast_client)),
+  _last_update_time(std::chrono::steady_clock::now() - std::chrono::seconds(1))
 {
   // Do nothing. The make() function does all further initialization.
 }
@@ -655,6 +664,249 @@ rmf_task::State TaskManager::_get_state() const
   return _context->current_task_end_state();
 }
 
+//==============================================================================
+void TaskManager::_consider_publishing_updates()
+{
+  const auto now = std::chrono::steady_clock::now();
+  const auto time_elapsed = now - _last_update_time;
+  // TODO(MXG): Make max elapsed time configurable
+  const auto max_time_elapsed = std::chrono::seconds(1);
+  if (_task_state_update_available || time_elapsed > max_time_elapsed)
+  {
+    _task_state_update_available = false;
+    _last_update_time = now;
+    _publish_task_state();
+  }
+}
+
+namespace {
+//==============================================================================
+std::chrono::milliseconds to_millis(rmf_traffic::Duration duration)
+{
+  return std::chrono::duration_cast<std::chrono::milliseconds>(duration);
+}
+
+//==============================================================================
+std::string status_to_string(rmf_task::Event::Status status)
+{
+  using Status = rmf_task::Event::Status;
+  switch (status)
+  {
+  case Status::Uninitialized:
+    return "uninitialized";
+  case Status::Blocked:
+    return "blocked";
+  case Status::Error:
+    return "error";
+  case Status::Failed:
+    return "failed";
+  case Status::Standby:
+    return "standby";
+  case Status::Underway:
+    return "underway";
+  case Status::Delayed:
+    return "delayed";
+  case Status::Skipped:
+    return "skipped";
+  case Status::Canceled:
+    return "canceled";
+  case Status::Killed:
+    return "killed";
+  case Status::Completed:
+    return "completed";
+  default:
+    return "uninitialized";
+  }
+}
+
+//==============================================================================
+std::string tier_to_string(rmf_task::Log::Entry::Tier tier)
+{
+  using Tier = rmf_task::Log::Entry::Tier;
+  switch (tier)
+  {
+  case Tier::Info:
+    return "info";
+  case Tier::Warning:
+    return "warning";
+  case Tier::Error:
+    return "error";
+  default:
+    return "uninitialized";
+  }
+}
+
+//==============================================================================
+nlohmann::json log_to_json(const rmf_task::Log::Entry& entry)
+{
+  nlohmann::json output;
+  output["seq"] = entry.seq();
+  output["tier"] = tier_to_string(entry.tier());
+  output["unix_millis_time"] =
+    to_millis(entry.time().time_since_epoch()).count();
+  output["text"] = entry.text();
+
+  return output;
+}
+
+//==============================================================================
+nlohmann::json& copy_phase_data(
+  nlohmann::json& phases,
+  const rmf_task::Phase::Active& snapshot,
+  rmf_task::Log::Reader& reader,
+  nlohmann::json& all_phase_logs)
+{
+  const auto& tag = *snapshot.tag();
+  const auto& header = tag.header();
+  const auto id = tag.id();
+  auto& phase = phases[std::to_string(id)];
+  phase["id"] = id;
+  phase["category"] = header.category();
+  phase["detail"] = header.detail();
+  phase["original_estimate_millis"] =
+    std::max(0l, to_millis(header.original_duration_estimate()).count());
+  phase["estimate_millis"] =
+    std::max(0l, to_millis(snapshot.estimate_remaining_time()).count());
+  phase["final_event_id"] = snapshot.final_event()->id();
+
+  // TODO(MXG): Add in skip request information
+
+  std::vector<rmf_task::Event::ConstStatePtr> event_queue;
+  event_queue.push_back(snapshot.final_event());
+
+  auto& phase_logs = all_phase_logs[std::to_string(id)];
+  auto& event_logs = phase_logs["events"];
+
+  while (!event_queue.empty())
+  {
+    const auto top = event_queue.back();
+    event_queue.pop_back();
+
+    nlohmann::json event_state;
+    event_state["id"] = top->id();
+    event_state["status"] = status_to_string(top->status());
+
+    // TODO(MXG): Keep a VersionedString Reader to know when to actually update
+    // this string
+    event_state["name"] =
+      *rmf_task::VersionedString::Reader().read(top->name());
+
+    event_state["detail"] =
+      *rmf_task::VersionedString::Reader().read(top->detail());
+
+    std::vector<nlohmann::json> logs;
+    std::size_t log_count = 0;
+    for (const auto& log : reader.read(top->log()))
+    {
+      std::cout << "LOG COUNT: " << log_count++ << std::endl;
+      logs.push_back(log_to_json(log));
+    }
+
+    if (!logs.empty())
+      event_logs[std::to_string(top->id())] = std::move(logs);
+
+    std::vector<uint32_t> deps;
+    deps.reserve(top->dependencies().size());
+    for (const auto& dep : top->dependencies())
+    {
+      event_queue.push_back(dep);
+      deps.push_back(dep->id());
+    }
+
+    event_state["deps"] = std::move(deps);
+  }
+
+  return phase;
+}
+
+//==============================================================================
+void copy_phase_data(
+  nlohmann::json& phases,
+  const rmf_task::Phase::Pending& pending)
+{
+  const auto id = pending.tag()->id();
+  auto& phase = phases[std::to_string(id)];
+  phase["id"] = id;
+
+  const auto& header = pending.tag()->header();
+  phase["category"] = header.category();
+  phase["detail"] = header.detail();
+  phase["estimate_millis"] =
+    std::max(0l, to_millis(header.original_duration_estimate()).count());
+}
+} // anonymous namespace
+
+//==============================================================================
+void TaskManager::_publish_task_state()
+{
+  if (!_active_task)
+    return;
+
+  auto task_state_update = _task_state_update_json;
+
+  const auto& booking = *_active_task->tag()->booking();
+  auto& booking_json = _active_task_state["booking"];
+  booking_json["id"] = booking.id();
+  booking_json["unix_millis_earliest_start_time"] =
+    to_millis(booking.earliest_start_time().time_since_epoch()).count();
+  // TODO(MXG): Add priority and labels
+
+  const auto& header = _active_task->tag()->header();
+  _active_task_state["category"] = header.category();
+  _active_task_state["detail"] = header.detail();
+  // TODO(MXG): Add unix_millis_start_time and unix_millis_finish_time
+  _active_task_state["original_estimate_millis"] =
+    std::max(0l, to_millis(header.original_duration_estimate()).count());
+  _active_task_state["estimate_millis"] =
+    std::max(0l, to_millis(_active_task->estimate_remaining_time()).count());
+
+  auto& phases = _active_task_state["phases"];
+
+  nlohmann::json task_logs;
+  auto& phase_logs = task_logs["phases"];
+
+  std::vector<uint64_t> completed_ids;
+  completed_ids.reserve(_active_task->completed_phases().size());
+  for (const auto& completed : _active_task->completed_phases())
+  {
+    const auto& snapshot = completed->snapshot();
+    auto& phase = copy_phase_data(phases, *snapshot, _log_reader, phase_logs);
+
+    phase["unix_millis_start_time"] =
+      completed->start_time().time_since_epoch().count();
+
+    phase["unix_millis_finish_time"] =
+      completed->finish_time().time_since_epoch().count();
+
+    completed_ids.push_back(snapshot->tag()->id());
+  }
+  _active_task_state["completed"] = std::move(completed_ids);
+
+  const auto active_phase = _active_task->active_phase();
+  copy_phase_data(phases, *active_phase, _log_reader, phase_logs);
+
+  _active_task_state["active"] = active_phase->tag()->id();
+
+  std::vector<uint64_t> pending_ids;
+  pending_ids.reserve(_active_task->pending_phases().size());
+  for (const auto& pending : _active_task->pending_phases())
+  {
+    copy_phase_data(phases, pending);
+    pending_ids.push_back(pending.tag()->id());
+  }
+  _active_task_state["pending"] = std::move(pending_ids);
+
+  task_state_update["data"] = _active_task_state;
+  _validate_and_publish_json(
+    task_state_update, rmf_api_msgs::schemas::task_state_update);
+}
+
+//==============================================================================
+void TaskManager::_publish_task_queue()
+{
+  // TODO(MXG): Come up with a schema for task queues
+}
+
 //==============================================================================
 bool TaskManager::_validate_json(
   const nlohmann::json& json,
@@ -688,17 +940,14 @@ bool TaskManager::_validate_json(
 std::function<void(rmf_task::Phase::ConstSnapshotPtr)>
 TaskManager::_update_cb()
 {
-  return [w = weak_from_this()](rmf_task::Phase::ConstSnapshotPtr snapshot)
+  return [w = weak_from_this()](rmf_task::Phase::ConstSnapshotPtr)
     {
       const auto self = w.lock();
       if (!self)
         return;
 
-      // TODO
-      auto task_state_update = self->_task_state_update_json;
-      task_state_update["data"] = self->_active_task_state;
-      self->_validate_and_publish_json(
-        task_state_update, rmf_api_msgs::schemas::task_state_update);
+      self->_task_state_update_available = true;
+      // TODO(MXG): Use this callback to make the state updates more efficient
     };
 }
 
@@ -712,12 +961,7 @@ TaskManager::_checkpoint_cb()
     if (!self)
       return;
 
-    // TODO
-
-    auto task_state_update = self->_task_state_update_json;
-    task_state_update["data"] = self->_active_task_state;
-    self->_validate_and_publish_json(
-      task_state_update, rmf_api_msgs::schemas::task_state_update);
+    // TODO(MXG): Save the backup
   };
 }
 
@@ -731,12 +975,8 @@ TaskManager::_phase_finished_cb()
       if (!self)
         return;
 
-      // TODO
-
-      auto task_state_update = self->_task_state_update_json;
-      task_state_update["data"] = self->_active_task_state;
-      self->_validate_and_publish_json(
-        task_state_update, rmf_api_msgs::schemas::task_state_update);
+      self->_task_state_update_available = true;
+      // TODO(MXG): Use this callback to make the state updates more efficient
     };
 }
 
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
index 41fa8d05e..c7c92f2f2 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
@@ -126,6 +126,9 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
   std::mutex _mutex;
   rclcpp::TimerBase::SharedPtr _task_timer;
   rclcpp::TimerBase::SharedPtr _retreat_timer;
+  rclcpp::TimerBase::SharedPtr _update_timer;
+  bool _task_state_update_available = true;
+  std::chrono::steady_clock::time_point _last_update_time;
 
   // Container to keep track of tasks that have been started by this TaskManager
   // Use the _register_executed_task() to populate this container.
@@ -156,8 +159,8 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
 
   // The task_state.json for the active task. This should be initialized when
   // a request is activated.
-  // TODO: Should this be a shared_ptr to pass down to LegacyTask::Active?
   nlohmann::json _active_task_state;
+  rmf_task::Log::Reader _log_reader;
 
   // Map task_id to task_log.json for all tasks managed by this TaskManager
   std::unordered_map<std::string, nlohmann::json> _task_logs = {};
@@ -177,6 +180,12 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
   /// Get the current state of the robot
   rmf_task::State _get_state() const;
 
+  /// Check whether publishing should happen
+  void _consider_publishing_updates();
+
+  /// Publish the current task state
+  void _publish_task_state();
+
   /// Publish the current pending task list
   void _publish_task_queue();
 

From 0cd4cc046ab34ad6dcc0827371768d077f8ca6a2 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Tue, 21 Dec 2021 23:20:56 +0800
Subject: [PATCH 22/79] Continue implementing json updates, fix a subtle bug

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 .../agv/FleetUpdateHandle.hpp                 |  17 +-
 .../launch/fleet_adapter.launch.xml           |   2 +-
 rmf_fleet_adapter/src/full_control/main.cpp   |   2 +-
 .../src/rmf_fleet_adapter/Negotiator.hpp      |   2 +-
 .../src/rmf_fleet_adapter/TaskManager.cpp     |  70 +++++++-
 .../src/rmf_fleet_adapter/TaskManager.hpp     |   2 +
 .../agv/FleetUpdateHandle.cpp                 | 163 +++++++++---------
 .../rmf_fleet_adapter/agv/RobotContext.cpp    |   6 +
 .../rmf_fleet_adapter/agv/RobotContext.hpp    |   3 +
 .../agv/internal_FleetUpdateHandle.hpp        |  16 +-
 10 files changed, 185 insertions(+), 98 deletions(-)

diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp
index 97401637c..c13214fad 100644
--- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp
@@ -176,10 +176,25 @@ class FleetUpdateHandle : public std::enable_shared_from_this<FleetUpdateHandle>
   /// Get the default value for the maximum acceptable delay.
   std::optional<rmf_traffic::Duration> default_maximum_delay() const;
 
+  /// The behavior is identical to fleet_state_topic_publish_period
+  [[deprecated("Use fleet_state_topic_publish_period instead")]]
+  FleetUpdateHandle& fleet_state_publish_period(
+    std::optional<rmf_traffic::Duration> value);
+
   /// Specify a period for how often the fleet state message is published for
   /// this fleet. Passing in std::nullopt will disable the fleet state message
   /// publishing. The default value is 1s.
-  FleetUpdateHandle& fleet_state_publish_period(
+  FleetUpdateHandle& fleet_state_topic_publish_period(
+    std::optional<rmf_traffic::Duration> value);
+
+  /// Specify a period for how often the fleet state is updated in the database
+  /// and to the API server. This is separate from publishing the fleet state
+  /// over the ROS2 fleet state topic. Passing in std::nullopt will disable
+  /// the updating, but this is not recommended unless you intend to provide the
+  /// API server with the fleet states through some other means.
+  ///
+  /// The default value is 1s.
+  FleetUpdateHandle& fleet_state_update_period(
     std::optional<rmf_traffic::Duration> value);
 
   class Implementation;
diff --git a/rmf_fleet_adapter/launch/fleet_adapter.launch.xml b/rmf_fleet_adapter/launch/fleet_adapter.launch.xml
index 2e35889a1..e1e0fc727 100644
--- a/rmf_fleet_adapter/launch/fleet_adapter.launch.xml
+++ b/rmf_fleet_adapter/launch/fleet_adapter.launch.xml
@@ -40,7 +40,7 @@
   <arg name="recharge_soc" default="1.0" description="The fraction of total battery capacity to which the robot should be charged"/>
   <arg name="experimental_lift_watchdog_service" default="" description="(Experimental) The name of a service to check whether a robot can enter a lift"/>
   <!-- The uri is set to the TrajectoryServer uri for debugging purposes. The default port needs to be updated before merging -->
-  <arg name="server_uri" default="ws://127.0.0.1:8006" description="The URI of the api server to trasnmit state and task infromation. If empty, information will not be transmitted"/>
+  <arg name="server_uri" default="" description="The URI of the api server to trasnmit state and task infromation. If empty, information will not be transmitted"/>
 
 
   <node pkg="rmf_fleet_adapter"
diff --git a/rmf_fleet_adapter/src/full_control/main.cpp b/rmf_fleet_adapter/src/full_control/main.cpp
index fd062d175..daab9ab7d 100644
--- a/rmf_fleet_adapter/src/full_control/main.cpp
+++ b/rmf_fleet_adapter/src/full_control/main.cpp
@@ -857,7 +857,7 @@ std::shared_ptr<Connections> make_fleet(
 
   // We disable fleet state publishing for this fleet adapter because we expect
   // the fleet drivers to publish these messages.
-  // connections->fleet->fleet_state_publish_period(std::nullopt);
+  connections->fleet->fleet_state_topic_publish_period(std::nullopt);
 
   connections->closed_lanes_pub =
     adapter->node()->create_publisher<rmf_fleet_msgs::msg::ClosedLanes>(
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/Negotiator.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/Negotiator.hpp
index 1974dbf7c..551da40b6 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/Negotiator.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/Negotiator.hpp
@@ -28,7 +28,7 @@ namespace rmf_fleet_adapter {
 //==============================================================================
 class Negotiator
   : public rmf_traffic::schedule::Negotiator,
-    std::enable_shared_from_this<Negotiator>
+    public std::enable_shared_from_this<Negotiator>
 {
 public:
 
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
index f4e6267b0..5f11f4d07 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
@@ -185,6 +185,16 @@ std::optional<std::string> TaskManager::current_task_id() const
   return std::nullopt;
 }
 
+//==============================================================================
+std::string TaskManager::robot_status() const
+{
+  if (!_active_task)
+    return "idle";
+
+  // TODO(MXG): Identify if the robot is charging and report that status here
+  return "working";
+}
+
 //==============================================================================
 auto TaskManager::expected_finish_state() const -> State
 {
@@ -834,6 +844,27 @@ void copy_phase_data(
   phase["estimate_millis"] =
     std::max(0l, to_millis(header.original_duration_estimate()).count());
 }
+
+//==============================================================================
+void copy_booking_data(
+  nlohmann::json& booking_json,
+  const rmf_task::Task::Booking& booking)
+{
+  booking_json["id"] = booking.id();
+  booking_json["unix_millis_earliest_start_time"] =
+    to_millis(booking.earliest_start_time().time_since_epoch()).count();
+  // TODO(MXG): Add priority and labels
+}
+
+//==============================================================================
+void copy_assignment(
+  nlohmann::json& assigned_to_json,
+  const agv::RobotContext& context)
+{
+  assigned_to_json["group"] = context.group();
+  assigned_to_json["name"] = context.name();
+}
+
 } // anonymous namespace
 
 //==============================================================================
@@ -845,11 +876,7 @@ void TaskManager::_publish_task_state()
   auto task_state_update = _task_state_update_json;
 
   const auto& booking = *_active_task->tag()->booking();
-  auto& booking_json = _active_task_state["booking"];
-  booking_json["id"] = booking.id();
-  booking_json["unix_millis_earliest_start_time"] =
-    to_millis(booking.earliest_start_time().time_since_epoch()).count();
-  // TODO(MXG): Add priority and labels
+  copy_booking_data(_active_task_state["booking"], booking);
 
   const auto& header = _active_task->tag()->header();
   _active_task_state["category"] = header.category();
@@ -859,6 +886,9 @@ void TaskManager::_publish_task_state()
     std::max(0l, to_millis(header.original_duration_estimate()).count());
   _active_task_state["estimate_millis"] =
     std::max(0l, to_millis(_active_task->estimate_remaining_time()).count());
+  copy_assignment(_active_task_state["assigned_to"], *_context);
+  _active_task_state["status"] =
+    status_to_string(_active_task->status_overview());
 
   auto& phases = _active_task_state["phases"];
 
@@ -904,7 +934,35 @@ void TaskManager::_publish_task_state()
 //==============================================================================
 void TaskManager::_publish_task_queue()
 {
-  // TODO(MXG): Come up with a schema for task queues
+  rmf_task::State expected_state = _context->current_task_end_state();
+  const auto& parameters = *_context->task_parameters();
+  for (const auto& pending : _queue)
+  {
+    const auto info = pending.request()->description()->generate_info(
+      expected_state, parameters);
+
+    nlohmann::json pending_json;
+    const auto& booking = *pending.request()->booking();
+    copy_booking_data(pending_json["booking"], booking);
+
+    pending_json["category"] = info.category;
+    pending_json["detail"] = info.detail;
+
+    const auto estimate =
+      pending.finish_state().time().value() - pending.deployment_time();
+    pending_json["original_estimate_millis"] =
+      std::max(0l, to_millis(estimate).count());
+    copy_assignment(pending_json["assigned_to"], *_context);
+    pending_json["status"] = "standby";
+
+    auto task_state_update = _task_state_update_json;
+    task_state_update["data"] = pending_json;
+
+    _validate_and_publish_json(
+      task_state_update, rmf_api_msgs::schemas::task_state_update);
+
+    expected_state = pending.finish_state();
+  }
 }
 
 //==============================================================================
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
index c7c92f2f2..66314d547 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
@@ -81,6 +81,8 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
 
   std::optional<std::string> current_task_id() const;
 
+  std::string robot_status() const;
+
   /// The state of the robot.
   State expected_finish_state() const;
 
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
index 6a706fd36..74ae3564b 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
@@ -795,22 +795,35 @@ get_nearest_charger(
   return nearest_charger;
 }
 
+namespace {
 //==============================================================================
-void FleetUpdateHandle::Implementation::fleet_state_publish_period(
-  std::optional<rmf_traffic::Duration> value)
+rmf_fleet_msgs::msg::Location convert_location(const agv::RobotContext& context)
 {
-  if (value.has_value())
+  if (context.location().empty())
   {
-    fleet_state_timer = node->create_wall_timer(
-      std::chrono::seconds(1), [this]() { this->publish_fleet_state(); });
-  }
-  else
-  {
-    fleet_state_timer = nullptr;
+    // TODO(MXG): We should emit some kind of critical error if this ever
+    // happens
+    return rmf_fleet_msgs::msg::Location();
   }
+
+  const auto& graph = context.planner()->get_configuration().graph();
+  const auto& l = context.location().front();
+  const auto& wp = graph.get_waypoint(l.waypoint());
+  const Eigen::Vector2d p = l.location().value_or(wp.get_location());
+
+  return rmf_fleet_msgs::build<rmf_fleet_msgs::msg::Location>()
+    .t(rmf_traffic_ros2::convert(l.time()))
+    .x(p.x())
+    .y(p.y())
+    .yaw(l.orientation())
+    .obey_approach_speed_limit(false)
+    .approach_speed_limit(0.0)
+    .level_name(wp.get_map_name())
+    // NOTE(MXG): This field is only used by the fleet drivers. For now, we
+    // will just fill it with a zero.
+    .index(0);
 }
 
-namespace {
 //==============================================================================
 rmf_fleet_msgs::msg::RobotState convert_state(const TaskManager& mgr)
 {
@@ -818,34 +831,6 @@ rmf_fleet_msgs::msg::RobotState convert_state(const TaskManager& mgr)
 
   const auto mode = mgr.robot_mode();
 
-  auto location = [&]() -> rmf_fleet_msgs::msg::Location
-    {
-      if (context.location().empty())
-      {
-        // TODO(MXG): We should emit some kind of critical error if this ever
-        // happens
-        return rmf_fleet_msgs::msg::Location();
-      }
-
-      const auto& graph = context.planner()->get_configuration().graph();
-      const auto& l = context.location().front();
-      const auto& wp = graph.get_waypoint(l.waypoint());
-      const Eigen::Vector2d p = l.location().value_or(wp.get_location());
-
-      return rmf_fleet_msgs::build<rmf_fleet_msgs::msg::Location>()
-        .t(rmf_traffic_ros2::convert(l.time()))
-        .x(p.x())
-        .y(p.y())
-        .yaw(l.orientation())
-        .obey_approach_speed_limit(false)
-        .approach_speed_limit(0.0)
-        .level_name(wp.get_map_name())
-        // NOTE(MXG): This field is only used by the fleet drivers. For now, we
-        // will just fill it with a zero.
-        .index(0);
-    } ();
-
-
   return rmf_fleet_msgs::build<rmf_fleet_msgs::msg::RobotState>()
     .name(context.name())
     .model(context.description().owner())
@@ -857,7 +842,7 @@ rmf_fleet_msgs::msg::RobotState convert_state(const TaskManager& mgr)
     .mode(std::move(mode))
     // We multiply by 100 to convert from the [0.0, 1.0] range to percentage
     .battery_percent(context.current_battery_soc()*100.0)
-    .location(std::move(location))
+    .location(convert_location(context))
     // NOTE(MXG): The path field is only used by the fleet drivers. For now,
     // we will just fill it with a zero. We could consider filling it in based
     // on the robot's plan, but that seems redundant with the traffic schedule
@@ -867,50 +852,49 @@ rmf_fleet_msgs::msg::RobotState convert_state(const TaskManager& mgr)
 } // anonymous namespace
 
 //==============================================================================
-void FleetUpdateHandle::Implementation::publish_fleet_state() const
+void FleetUpdateHandle::Implementation::publish_fleet_state_topic() const
 {
   std::vector<rmf_fleet_msgs::msg::RobotState> robot_states;
   for (const auto& [context, mgr] : task_managers)
     robot_states.emplace_back(convert_state(*mgr));
 
-  const auto mode_to_status = [](const uint32_t mode) -> std::string
-    {
-      // uninitialized, offline, shutdown ,idle, charging, working, error
-      if (mode == 0)
-        return "idle";
-      else if (mode == 1)
-        return "charging";
-      else if (mode == 2)
-        return "working";
-      else
-        return "error";
-    };
+  auto fleet_state = rmf_fleet_msgs::build<rmf_fleet_msgs::msg::FleetState>()
+    .name(name)
+    .robots(std::move(robot_states));
+
+  fleet_state_pub->publish(std::move(fleet_state));
+}
 
+//==============================================================================
+void FleetUpdateHandle::Implementation::update_fleet_state() const
+{
   // Publish to API server
   if (broadcast_client)
   {
     nlohmann::json fleet_state_msg = {{"name", {}}, {"robots", {}}};
     fleet_state_msg["name"] = name;
-    for (const auto& state : robot_states)
+    for (const auto& [context, mgr] : task_managers)
     {
-      nlohmann::json json;
-      json["name"] = state.name;
-      json["status"] = mode_to_status(state.mode.mode);
-      json["task_id"] = state.task_id;
+      const auto& name = context->name();
+      nlohmann::json& json = fleet_state_msg["robots"][name];
+      json["name"] = name;
+      json["status"] = mgr->robot_status();
+      json["task_id"] = mgr->current_task_id().value_or("");
       json["unix_millis_time"] =
         std::chrono::duration_cast<std::chrono::milliseconds>(
-        std::chrono::seconds(state.location.t.sec) +
-        std::chrono::nanoseconds(state.location.t.nanosec)).count();
-      nlohmann::json location;
-      location["map"] = state.location.level_name;
-      location["x"] = state.location.x;
-      location["y"] = state.location.y;
-      location["yaw"] = state.location.yaw;
-      json["location"] = location;
-      json["battery"] = state.battery_percent / 100.0;
+        context->now().time_since_epoch()).count();
+      json["battery"] = context->current_battery_soc();
+
+      nlohmann::json& location = json["location"];
+      const auto location_msg = convert_location(*context);
+      location["map"] = location_msg.level_name;
+      location["x"] = location_msg.x;
+      location["y"] = location_msg.y;
+      location["yaw"] = location_msg.yaw;
+
       // TODO(YV): json["issues"]
-      fleet_state_msg["robots"][state.name] = json;
     }
+
     const auto fleet_schema = rmf_api_msgs::schemas::fleet_state_update;
     const auto loader =
       [n = node, s = schema_dictionary](const nlohmann::json_uri& id,
@@ -932,7 +916,7 @@ void FleetUpdateHandle::Implementation::publish_fleet_state() const
     {{"type", "fleet_state_update"}, {"data", fleet_state_msg}};
     try
     {
-
+      // TODO(MXG): We should make this validator static
       nlohmann::json_schema::json_validator validator(fleet_schema, loader);
       validator.validate(fleet_state_update_msg);
     }
@@ -947,12 +931,6 @@ void FleetUpdateHandle::Implementation::publish_fleet_state() const
 
     broadcast_client->publish(fleet_state_update_msg);
   }
-
-  auto fleet_state = rmf_fleet_msgs::build<rmf_fleet_msgs::msg::FleetState>()
-    .name(name)
-    .robots(std::move(robot_states));
-
-  fleet_state_pub->publish(std::move(fleet_state));
 }
 
 //==============================================================================
@@ -1366,20 +1344,51 @@ FleetUpdateHandle::default_maximum_delay() const
 //==============================================================================
 FleetUpdateHandle& FleetUpdateHandle::fleet_state_publish_period(
   std::optional<rmf_traffic::Duration> value)
+{
+  return fleet_state_topic_publish_period(value);
+}
+
+//==============================================================================
+FleetUpdateHandle& FleetUpdateHandle::fleet_state_topic_publish_period(
+  std::optional<rmf_traffic::Duration> value)
 {
   if (value.has_value())
   {
-    _pimpl->fleet_state_timer = _pimpl->node->try_create_wall_timer(
+    _pimpl->fleet_state_topic_publish_timer =
+      _pimpl->node->try_create_wall_timer(
       value.value(),
       [me = weak_from_this()]()
       {
         if (const auto self = me.lock())
-          self->_pimpl->publish_fleet_state();
+          self->_pimpl->publish_fleet_state_topic();
       });
   }
   else
   {
-    _pimpl->fleet_state_timer = nullptr;
+    _pimpl->fleet_state_topic_publish_timer = nullptr;
+  }
+
+  return *this;
+}
+
+//==============================================================================
+FleetUpdateHandle& FleetUpdateHandle::fleet_state_update_period(
+  std::optional<rmf_traffic::Duration> value)
+{
+  if (value.has_value())
+  {
+    _pimpl->fleet_state_update_timer =
+      _pimpl->node->try_create_wall_timer(
+      value.value(),
+      [me = weak_from_this()]
+    {
+      if (const auto self = me.lock())
+        self->_pimpl->update_fleet_state();
+    });
+  }
+  else
+  {
+    _pimpl->fleet_state_update_timer = nullptr;
   }
 
   return *this;
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp
index 975fe52e7..41b21191b 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp
@@ -117,6 +117,12 @@ const std::string& RobotContext::name() const
   return _itinerary.description().name();
 }
 
+//==============================================================================
+const std::string& RobotContext::group() const
+{
+  return _itinerary.description().owner();
+}
+
 //==============================================================================
 const std::string& RobotContext::requester_id() const
 {
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp
index b91979ba9..3d91dd94a 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp
@@ -77,6 +77,9 @@ class RobotContext
   /// Get the name of this robot
   const std::string& name() const;
 
+  /// Get the group (fleet) of this robot
+  const std::string& group() const;
+
   /// Get the requester ID to use for this robot when sending requests
   const std::string& requester_id() const;
 
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
index 046238144..6d7bad26c 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
@@ -180,7 +180,8 @@ class FleetUpdateHandle::Implementation
 
   rclcpp::Publisher<rmf_fleet_msgs::msg::FleetState>::SharedPtr
     fleet_state_pub = nullptr;
-  rclcpp::TimerBase::SharedPtr fleet_state_timer = nullptr;
+  rclcpp::TimerBase::SharedPtr fleet_state_topic_publish_timer = nullptr;
+  rclcpp::TimerBase::SharedPtr fleet_state_update_timer = nullptr;
 
   // Map task id to pair of <RequestPtr, Assignments>
   using Assignments = rmf_task::TaskPlanner::Assignments;
@@ -249,13 +250,7 @@ class FleetUpdateHandle::Implementation
         nullptr, nullptr, nullptr);
 
     handle->_pimpl->fleet_state_pub = handle->_pimpl->node->fleet_state();
-    handle->_pimpl->fleet_state_timer =
-      handle->_pimpl->node->try_create_wall_timer(
-      std::chrono::seconds(1), [me = handle->weak_from_this()]()
-      {
-        if (const auto self = me.lock())
-          self->_pimpl->publish_fleet_state();
-      });
+    handle->fleet_state_topic_publish_period(std::chrono::seconds(1));
 
     // Create subs and pubs for bidding
     auto default_qos = rclcpp::SystemDefaultsQoS();
@@ -367,10 +362,9 @@ class FleetUpdateHandle::Implementation
     return *fleet._pimpl;
   }
 
-  void fleet_state_publish_period(
-    std::optional<rmf_traffic::Duration> value);
+  void publish_fleet_state_topic() const;
 
-  void publish_fleet_state() const;
+  void update_fleet_state() const;
 
   void add_standard_tasks();
 };

From 2ca24c34ea688b62d51dc8325dd90a35865da330 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Wed, 22 Dec 2021 15:57:05 +0800
Subject: [PATCH 23/79] Support battery charging tasks

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 .../agv/FleetUpdateHandle.cpp                 |  17 ++
 .../rmf_fleet_adapter/agv/RobotContext.cpp    |   8 +
 .../rmf_fleet_adapter/agv/RobotContext.hpp    |   3 +
 .../rmf_fleet_adapter/tasks/ChargeBattery.cpp | 190 +++++++++++++++++-
 .../rmf_fleet_adapter/tasks/ChargeBattery.hpp |  10 +
 5 files changed, 227 insertions(+), 1 deletion(-)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
index 74ae3564b..3b0875d80 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
@@ -1185,6 +1185,23 @@ void FleetUpdateHandle::add_robot(
               {
                 if (const auto c = w.lock())
                 {
+                  std::stringstream ss;
+                  ss << "Failed negotiation for [" << c->requester_id()
+                     << "] with these starts:";
+                  for (const auto& l : c->location())
+                  {
+                    ss << "\n -- t:" << l.time().time_since_epoch().count()
+                       << " | wp:" << l.waypoint() << " | ori:"
+                       << l.orientation();
+                    if (l.location().has_value())
+                    {
+                      const auto& p = *l.location();
+                      ss << " | pos:(" << p.x() << ", " << p.y() << ")";
+                    }
+                  }
+                  ss << "\n -- Fin --";
+                  std::cout << ss.str() << std::endl;
+
                   auto& last_time = *last_interrupt_time;
                   const auto now = std::chrono::steady_clock::now();
                   if (last_time.has_value())
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp
index 41b21191b..f68ff8429 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp
@@ -299,6 +299,7 @@ double RobotContext::current_battery_soc() const
   return _current_battery_soc;
 }
 
+//==============================================================================
 RobotContext& RobotContext::current_battery_soc(const double battery_soc)
 {
   _current_battery_soc = battery_soc;
@@ -307,6 +308,12 @@ RobotContext& RobotContext::current_battery_soc(const double battery_soc)
   return *this;
 }
 
+//==============================================================================
+std::size_t RobotContext::dedicated_charger_wp() const
+{
+  return _charger_wp;
+}
+
 //==============================================================================
 const rxcpp::observable<double>& RobotContext::observe_battery_soc() const
 {
@@ -409,6 +416,7 @@ RobotContext::RobotContext(
   _maximum_delay(maximum_delay),
   _requester_id(
     _itinerary.description().owner() + "/" + _itinerary.description().name()),
+  _charger_wp(state.dedicated_charging_waypoint().value()),
   _current_task_end_state(state),
   _current_task_id(std::nullopt),
   _task_planner(std::move(task_planner))
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp
index 3d91dd94a..075fb8878 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp
@@ -160,6 +160,8 @@ class RobotContext
   /// publishes the battery soc via _battery_soc_publisher.
   RobotContext& current_battery_soc(const double battery_soc);
 
+  std::size_t dedicated_charger_wp() const;
+
   // Get a reference to the battery soc observer of this robot.
   const rxcpp::observable<double>& observe_battery_soc() const;
 
@@ -227,6 +229,7 @@ class RobotContext
 
   /// Always call the current_battery_soc() setter to set a new value
   double _current_battery_soc = 1.0;
+  std::size_t _charger_wp;
   rxcpp::subjects::subject<double> _battery_soc_publisher;
   rxcpp::observable<double> _battery_soc_obs;
   rmf_task::State _current_task_end_state;
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp
index 1badab34a..be5cd3dbf 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp
@@ -16,11 +16,19 @@
 */
 
 #include "../phases/GoToPlace.hpp"
-
 #include "../phases/WaitForCharge.hpp"
 
+#include "../events/GoToPlace.hpp"
+#include "../events/LegacyPhaseShim.hpp"
+
 #include "ChargeBattery.hpp"
 
+#include <rmf_task_sequence/events/Bundle.hpp>
+#include <rmf_task_sequence/phases/SimplePhase.hpp>
+#include <rmf_task_sequence/events/WaitFor.hpp>
+
+#include <rmf_task_sequence/Task.hpp>
+
 namespace rmf_fleet_adapter {
 namespace tasks {
 
@@ -60,5 +68,185 @@ std::shared_ptr<LegacyTask> make_charge_battery(
     request);
 }
 
+//==============================================================================
+struct GoToChargerDescription
+  : public rmf_task_sequence::events::WaitFor::Description
+{
+  GoToChargerDescription()
+    : rmf_task_sequence::events::WaitFor::Description(rmf_traffic::Duration(0))
+  {
+    // Do nothing
+  }
+
+  static rmf_task_sequence::Event::StandbyPtr standby(
+    const rmf_task_sequence::Event::AssignIDPtr& id,
+    const std::function<rmf_task::State()>& get_state,
+    const rmf_task::ConstParametersPtr& parameters,
+    const GoToChargerDescription&,
+    std::function<void()> update)
+  {
+    using GoToPlace = rmf_task_sequence::events::GoToPlace::Description;
+    const auto state = get_state();
+    const auto context = state.get<agv::GetContext>()->value;
+
+    return events::GoToPlace::Standby::make(
+      id, get_state, parameters,
+      *GoToPlace::make(context->dedicated_charger_wp()),
+      std::move(update));
+  }
+
+  static void add(rmf_task_sequence::Event::Initializer& initializer)
+  {
+    initializer.add<GoToChargerDescription>(
+      [](
+        const rmf_task_sequence::Event::AssignIDPtr& id,
+        const std::function<rmf_task::State()>& get_state,
+        const rmf_task::ConstParametersPtr& parameters,
+        const GoToChargerDescription& description,
+        std::function<void()> update) -> rmf_task_sequence::Event::StandbyPtr
+      {
+        return standby(
+          id, get_state, parameters, description, std::move(update));
+      },
+      [](
+        const rmf_task_sequence::Event::AssignIDPtr& id,
+        const std::function<rmf_task::State()>& get_state,
+        const rmf_task::ConstParametersPtr& parameters,
+        const GoToChargerDescription& description,
+        const nlohmann::json&,
+        std::function<void()> update,
+        std::function<void()> checkpoint,
+        std::function<void()> finished) -> rmf_task_sequence::Event::ActivePtr
+      {
+        return standby(
+          id, get_state, parameters, description, std::move(update))
+          ->begin(std::move(checkpoint), std::move(finished));
+      });
+  }
+};
+
+//==============================================================================
+struct WaitForChargeDescription
+  : public rmf_task_sequence::events::WaitFor::Description
+{
+  WaitForChargeDescription()
+  : rmf_task_sequence::events::WaitFor::Description(rmf_traffic::Duration(0))
+  {
+    // Do nothing
+  }
+
+  static rmf_task_sequence::Event::StandbyPtr standby(
+    const rmf_task_sequence::Event::AssignIDPtr& id,
+    const std::function<rmf_task::State()>& get_state,
+    const rmf_task::ConstParametersPtr& parameters,
+    const WaitForChargeDescription&,
+    std::function<void()> update)
+  {
+    const auto state = get_state();
+    const auto context = state.get<agv::GetContext>()->value;
+
+    auto legacy = phases::WaitForCharge::make(
+      context,
+      parameters->battery_system(),
+      context->task_planner()->configuration().constraints().recharge_soc());
+
+    return events::LegacyPhaseShim::Standby::make(
+      std::move(legacy), context->worker(), context->clock(), id,
+      std::move(update));
+  }
+
+  static void add(rmf_task_sequence::Event::Initializer& initializer)
+  {
+    initializer.add<WaitForChargeDescription>(
+      [](
+        const rmf_task_sequence::Event::AssignIDPtr& id,
+        const std::function<rmf_task::State()>& get_state,
+        const rmf_task::ConstParametersPtr& parameters,
+        const WaitForChargeDescription& description,
+        std::function<void()> update) -> rmf_task_sequence::Event::StandbyPtr
+      {
+        return standby(
+          id, get_state, parameters, description, std::move(update));
+      },
+      [](
+        const rmf_task_sequence::Event::AssignIDPtr& id,
+        const std::function<rmf_task::State()>& get_state,
+        const rmf_task::ConstParametersPtr& parameters,
+        const WaitForChargeDescription& description,
+        const nlohmann::json&,
+        std::function<void()> update,
+        std::function<void()> checkpoint,
+        std::function<void()> finished) -> rmf_task_sequence::Event::ActivePtr
+      {
+        return standby(
+          id, get_state, parameters, description, std::move(update))
+          ->begin(std::move(checkpoint), std::move(finished));
+      });
+  }
+};
+
+//==============================================================================
+// TODO(MXG): Consider making the ChargeBatteryEvent description public so it
+// can be incorporated into other task types
+struct ChargeBatteryEventDescription
+  : public rmf_task_sequence::events::WaitFor::Description
+{
+  ChargeBatteryEventDescription()
+  : rmf_task_sequence::events::WaitFor::Description(rmf_traffic::Duration(0))
+  {
+    // Do nothing
+  }
+};
+
+//==============================================================================
+void add_charge_battery(
+  rmf_task::Activator& task_activator,
+  const rmf_task_sequence::Phase::ConstActivatorPtr& phase_activator,
+  rmf_task_sequence::Event::Initializer& event_initializer,
+  std::function<rmf_traffic::Time()> clock)
+{
+  using Bundle = rmf_task_sequence::events::Bundle;
+  using Phase = rmf_task_sequence::phases::SimplePhase;
+  using ChargeBattery = rmf_task::requests::ChargeBattery;
+
+  auto private_initializer =
+    std::make_shared<rmf_task_sequence::Event::Initializer>();
+
+  WaitForChargeDescription::add(*private_initializer);
+  GoToChargerDescription::add(*private_initializer);
+
+  auto charge_battery_event_unfolder =
+    [](const ChargeBatteryEventDescription&)
+  {
+    return Bundle::Description({
+       std::make_shared<GoToChargerDescription>(),
+       std::make_shared<WaitForChargeDescription>()
+     }, Bundle::Sequence, "Charge Battery");
+  };
+
+  Bundle::unfold<ChargeBatteryEventDescription>(
+    std::move(charge_battery_event_unfolder),
+    event_initializer, private_initializer);
+
+  auto charge_battery_task_unfolder =
+    [](const rmf_task::requests::ChargeBattery::Description&)
+    {
+      rmf_task_sequence::Task::Builder builder;
+      builder
+        .add_phase(
+          Phase::Description::make(
+            std::make_shared<ChargeBatteryEventDescription>(),
+            "Charge Battery", ""), {});
+
+      return *builder.build("Charge Battery", "");
+    };
+
+  rmf_task_sequence::Task::unfold<ChargeBattery::Description>(
+    std::move(charge_battery_task_unfolder),
+    task_activator,
+    phase_activator,
+    std::move(clock));
+}
+
 } // namespace task
 } // namespace rmf_fleet_adapter
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp
index 225b6877c..55657bbcf 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp
@@ -27,6 +27,9 @@
 #include <rmf_task/State.hpp>
 #include <rmf_task/requests/ChargeBattery.hpp>
 
+#include <rmf_task_sequence/Phase.hpp>
+#include <rmf_task_sequence/Event.hpp>
+
 namespace rmf_fleet_adapter {
 namespace tasks {
 
@@ -38,6 +41,13 @@ std::shared_ptr<LegacyTask> make_charge_battery(
   const rmf_traffic::Time deployment_time,
   const rmf_task::State finish_state);
 
+//==============================================================================
+void add_charge_battery(
+  rmf_task::Activator& task_activator,
+  const rmf_task_sequence::Phase::ConstActivatorPtr& phase_activator,
+  rmf_task_sequence::Event::Initializer& event_initializer,
+  std::function<rmf_traffic::Time()> clock);
+
 } // namespace tasks
 } // namespace rmf_fleet_adapter
 

From eea945cdeae78dfa6fdda11dcf88b8cbd778b200 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Wed, 22 Dec 2021 21:54:44 +0800
Subject: [PATCH 24/79] Fix battery charging and deliveries

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 .../src/rmf_fleet_adapter/TaskManager.cpp     | 49 ++++++++++++-------
 .../agv/FleetUpdateHandle.cpp                 |  7 +++
 .../rmf_fleet_adapter/agv/RobotContext.cpp    |  6 ++-
 3 files changed, 44 insertions(+), 18 deletions(-)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
index 5f11f4d07..2cf089c0c 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
@@ -354,6 +354,28 @@ void TaskManager::_begin_next_task()
 
     _queue.erase(_queue.begin());
 
+    if (!_active_task)
+    {
+      const auto info = assignment.request()->description()->generate_info(
+        _context->make_get_state()(), *_context->task_parameters());
+      RCLCPP_ERROR(
+        _context->node()->get_logger(),
+        "[rmf_fleet_adapter::TaskManager::_begin_next_task] Failed to "
+        "instantiate task type [%s] for ID [%s]. This indicates that the "
+        "fleet adapter is incorrectly configured.",
+        info.category.c_str(),
+        assignment.request()->booking()->id().c_str());
+
+      _context->worker().schedule(
+        [w = weak_from_this()](const auto&)
+        {
+          if (const auto self = w.lock())
+            self->_begin_next_task();
+        });
+
+      return;
+    }
+
     RCLCPP_INFO(
       _context->node()->get_logger(),
       "Beginning new task [%s] for [%s]. Remaining queue size: %ld",
@@ -805,12 +827,8 @@ nlohmann::json& copy_phase_data(
       *rmf_task::VersionedString::Reader().read(top->detail());
 
     std::vector<nlohmann::json> logs;
-    std::size_t log_count = 0;
     for (const auto& log : reader.read(top->log()))
-    {
-      std::cout << "LOG COUNT: " << log_count++ << std::endl;
       logs.push_back(log_to_json(log));
-    }
 
     if (!logs.empty())
       event_logs[std::to_string(top->id())] = std::move(logs);
@@ -1027,7 +1045,7 @@ TaskManager::_checkpoint_cb()
 std::function<void(rmf_task::Phase::ConstCompletedPtr)>
 TaskManager::_phase_finished_cb()
 {
-  return [w = weak_from_this()](rmf_task::Phase::ConstCompletedPtr phase)
+  return [w = weak_from_this()](rmf_task::Phase::ConstCompletedPtr)
     {
       const auto self = w.lock();
       if (!self)
@@ -1047,19 +1065,16 @@ std::function<void()> TaskManager::_task_finished(std::string id)
       if (!self)
         return;
 
-      // TODO
-
-      auto task_state_update = self->_task_state_update_json;
-      task_state_update["data"] = self->_active_task_state;
-      self->_validate_and_publish_json(
-        task_state_update, rmf_api_msgs::schemas::task_state_update);
-
-      auto task_log_update = self->_task_log_update_msg;
-      task_log_update["data"] = self->_task_logs[self->_active_task_state["booking"]["id"]];
-      self->_validate_and_publish_json(
-        task_log_update, rmf_api_msgs::schemas::task_log_update);
+      // Publish the final state of the task before destructing it
+      self->_publish_task_state();
+      self->_active_task = nullptr;
 
-      self->_active_task_state = {};
+      self->_context->worker().schedule(
+        [w = self->weak_from_this()](const auto&)
+        {
+          if (const auto self = w.lock())
+            self->_begin_next_task();
+        });
     };
 }
 
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
index 3b0875d80..fd9631145 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
@@ -28,6 +28,7 @@
 #include "../tasks/Delivery.hpp"
 #include "../tasks/Loop.hpp"
 #include "../tasks/Clean.hpp"
+#include "../tasks/ChargeBattery.hpp"
 #include "../events/GoToPlace.hpp"
 
 #include <rmf_task/Constraints.hpp>
@@ -961,6 +962,12 @@ void FleetUpdateHandle::Implementation::add_standard_tasks()
     activation.phase,
     activation.event,
     node->clock());
+
+  tasks::add_charge_battery(
+    *activation.task,
+    activation.phase,
+    *activation.event,
+    node->clock());
 }
 
 //==============================================================================
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp
index f68ff8429..94132e975 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp
@@ -271,7 +271,11 @@ std::function<rmf_task::State()> RobotContext::make_get_state()
   return [self = shared_from_this()]()
     {
       rmf_task::State state;
-      state.load(self->_location.front());
+      state.load_basic(
+        self->_location.front(),
+        self->_charger_wp,
+        self->_current_battery_soc);
+
       state.insert<GetContext>(GetContext{self->shared_from_this()});
       return state;
     };

From 2855b26baba88d22b7eea51402c3de7904bee51b Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Wed, 22 Dec 2021 22:34:44 +0800
Subject: [PATCH 25/79] Use Placeholder event instead of WaitFor event

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 .../rmf_fleet_adapter/tasks/ChargeBattery.cpp   | 17 ++++++++++-------
 .../src/rmf_fleet_adapter/tasks/Clean.cpp       |  6 +++---
 .../src/rmf_fleet_adapter/tasks/Delivery.cpp    | 10 ++++------
 3 files changed, 17 insertions(+), 16 deletions(-)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp
index be5cd3dbf..8f7f11393 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp
@@ -25,7 +25,7 @@
 
 #include <rmf_task_sequence/events/Bundle.hpp>
 #include <rmf_task_sequence/phases/SimplePhase.hpp>
-#include <rmf_task_sequence/events/WaitFor.hpp>
+#include <rmf_task_sequence/events/Placeholder.hpp>
 
 #include <rmf_task_sequence/Task.hpp>
 
@@ -70,10 +70,11 @@ std::shared_ptr<LegacyTask> make_charge_battery(
 
 //==============================================================================
 struct GoToChargerDescription
-  : public rmf_task_sequence::events::WaitFor::Description
+  : public rmf_task_sequence::events::Placeholder::Description
 {
   GoToChargerDescription()
-    : rmf_task_sequence::events::WaitFor::Description(rmf_traffic::Duration(0))
+    : rmf_task_sequence::events::Placeholder::Description(
+        "Go to charger", "")
   {
     // Do nothing
   }
@@ -127,10 +128,11 @@ struct GoToChargerDescription
 
 //==============================================================================
 struct WaitForChargeDescription
-  : public rmf_task_sequence::events::WaitFor::Description
+  : public rmf_task_sequence::events::Placeholder::Description
 {
   WaitForChargeDescription()
-  : rmf_task_sequence::events::WaitFor::Description(rmf_traffic::Duration(0))
+  : rmf_task_sequence::events::Placeholder::Description(
+      "Wait for charging", "")
   {
     // Do nothing
   }
@@ -189,10 +191,11 @@ struct WaitForChargeDescription
 // TODO(MXG): Consider making the ChargeBatteryEvent description public so it
 // can be incorporated into other task types
 struct ChargeBatteryEventDescription
-  : public rmf_task_sequence::events::WaitFor::Description
+  : public rmf_task_sequence::events::Placeholder::Description
 {
   ChargeBatteryEventDescription()
-  : rmf_task_sequence::events::WaitFor::Description(rmf_traffic::Duration(0))
+  : rmf_task_sequence::events::Placeholder::Description(
+      "Charge Battery", "")
   {
     // Do nothing
   }
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp
index 046aa4197..91b39163c 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp
@@ -20,7 +20,7 @@
 #include "Clean.hpp"
 
 #include <rmf_task_sequence/Task.hpp>
-#include <rmf_task_sequence/events/WaitFor.hpp>
+#include <rmf_task_sequence/events/Placeholder.hpp>
 #include <rmf_task_sequence/events/Bundle.hpp>
 #include <rmf_task_sequence/phases/SimplePhase.hpp>
 
@@ -86,10 +86,10 @@ std::shared_ptr<LegacyTask> make_clean(
 //==============================================================================
 // TODO(MXG): This implementation that uses the Dock command is crude and
 // should be replaced with an explicit cleaning activation command.
-struct CleanEvent : public rmf_task_sequence::events::WaitFor::Description
+struct CleanEvent : public rmf_task_sequence::events::Placeholder::Description
 {
   CleanEvent(const rmf_task::requests::Clean::Description& clean)
-  : rmf_task_sequence::events::WaitFor::Description(rmf_traffic::Duration(0)),
+  : rmf_task_sequence::events::Placeholder::Description("Clean", ""),
     start_waypoint(clean.start_waypoint()),
     end_waypoint(clean.end_waypoint())
   {
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp
index e008a6e6b..01332df75 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp
@@ -30,7 +30,7 @@
 
 #include <rmf_task_sequence/events/PickUp.hpp>
 #include <rmf_task_sequence/events/DropOff.hpp>
-#include <rmf_task_sequence/events/WaitFor.hpp>
+#include <rmf_task_sequence/events/Placeholder.hpp>
 #include <rmf_task_sequence/events/Bundle.hpp>
 #include <rmf_task_sequence/Task.hpp>
 #include <rmf_task_sequence/phases/SimplePhase.hpp>
@@ -129,7 +129,7 @@ std::shared_ptr<LegacyTask> make_delivery(
 }
 
 //==============================================================================
-struct TransferItems : public rmf_task_sequence::events::WaitFor::Description
+struct TransferItems : public rmf_task_sequence::events::Placeholder::Description
 {
   enum class Dir
   {
@@ -138,8 +138,7 @@ struct TransferItems : public rmf_task_sequence::events::WaitFor::Description
   };
 
   TransferItems(const rmf_task_sequence::events::PickUp::Description& pickup)
-  : rmf_task_sequence::events::WaitFor::Description(
-      pickup.loading_duration_estimate()),
+  : rmf_task_sequence::events::Placeholder::Description("Load items", ""),
     direction(Dir::Load),
     target(pickup.from_dispenser()),
     payload(pickup.payload())
@@ -148,8 +147,7 @@ struct TransferItems : public rmf_task_sequence::events::WaitFor::Description
   }
 
   TransferItems(const rmf_task_sequence::events::DropOff::Description& dropoff)
-  : rmf_task_sequence::events::WaitFor::Description(
-      dropoff.unloading_duration_estimate()),
+  : rmf_task_sequence::events::Placeholder::Description("Unload items", ""),
     direction(Dir::Unload),
     target(dropoff.into_ingestor()),
     payload(dropoff.payload())

From 96cc7255af488aecb7b5ebf71d289a255cd93979 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Fri, 24 Dec 2021 17:52:46 +0800
Subject: [PATCH 26/79] Implemented json message handling -- need to finish
 task change implementation

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 .../rmf_fleet_adapter/StandardNames.hpp       |   3 +
 .../src/rmf_fleet_adapter/TaskManager.cpp     | 596 +++++++++++++++---
 .../src/rmf_fleet_adapter/TaskManager.hpp     | 156 ++++-
 .../agv/EasyTrafficLight.cpp                  |   6 +-
 .../agv/FleetUpdateHandle.cpp                 |   3 -
 .../src/rmf_fleet_adapter/agv/Node.cpp        |  20 +
 .../src/rmf_fleet_adapter/agv/Node.hpp        |  13 +
 7 files changed, 700 insertions(+), 97 deletions(-)

diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp
index bc76055e8..ff916826f 100644
--- a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp
+++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp
@@ -60,6 +60,9 @@ const std::string DockSummaryTopicName = "dock_summary";
 const std::string LaneClosureRequestTopicName = "lane_closure_requests";
 const std::string ClosedLaneTopicName = "closed_lanes";
 
+const std::string TaskApiRequests = "task_api_requests";
+const std::string TaskApiResponses = "task_api_responses";
+
 } // namespace rmf_fleet_adapter
 
 #endif // RMF_FLEET_ADAPTER__STANDARDNAMES_HPP
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
index 2cf089c0c..4e2305bab 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
@@ -39,6 +39,22 @@
 #include <rmf_api_msgs/schemas/task_log_update.hpp>
 #include <rmf_api_msgs/schemas/task_log.hpp>
 #include <rmf_api_msgs/schemas/log_entry.hpp>
+#include <rmf_api_msgs/schemas/simple_response.hpp>
+#include <rmf_api_msgs/schemas/token_response.hpp>
+#include <rmf_api_msgs/schemas/cancel_task_request.hpp>
+#include <rmf_api_msgs/schemas/cancel_task_response.hpp>
+#include <rmf_api_msgs/schemas/kill_task_request.hpp>
+#include <rmf_api_msgs/schemas/kill_task_response.hpp>
+#include <rmf_api_msgs/schemas/interrupt_task_request.hpp>
+#include <rmf_api_msgs/schemas/interrupt_task_response.hpp>
+#include <rmf_api_msgs/schemas/resume_task_request.hpp>
+#include <rmf_api_msgs/schemas/resume_task_response.hpp>
+#include <rmf_api_msgs/schemas/rewind_task_request.hpp>
+#include <rmf_api_msgs/schemas/rewind_task_response.hpp>
+#include <rmf_api_msgs/schemas/skip_phase_request.hpp>
+#include <rmf_api_msgs/schemas/skip_phase_response.hpp>
+#include <rmf_api_msgs/schemas/undo_skip_phase_request.hpp>
+#include <rmf_api_msgs/schemas/undo_skip_phase_response.hpp>
 
 namespace rmf_fleet_adapter {
 
@@ -69,7 +85,7 @@ TaskManagerPtr TaskManager::make(
         // TODO(MXG): Consider subscribing to the emergency pullover update
         self->_emergency_pullover = events::EmergencyPullover::Standby::make(
           rmf_task_sequence::Event::AssignID::make(), self->_context, [](){})
-            ->begin([](){}, self->_make_resume());
+            ->begin([](){}, self->_make_resume_from_emergency());
       });
     };
 
@@ -86,13 +102,13 @@ TaskManagerPtr TaskManager::make(
         mgr->_emergency_active = msg->data;
         if (msg->data)
         {
-          if (auto task = mgr->_active_task)
+          if (mgr->_active_task)
           {
-            mgr->_resume_task = task->interrupt(begin_pullover);
+            mgr->_emergency_pullover_interrupt_token =
+              mgr->_active_task.add_interruption({"emergency pullover"});
           }
           else
           {
-            mgr->_resume_task = std::nullopt;
             begin_pullover();
           }
         }
@@ -137,21 +153,45 @@ TaskManagerPtr TaskManager::make(
       self->_consider_publishing_updates();
   });
 
-  auto schema = rmf_api_msgs::schemas::task_state;
-  nlohmann::json_uri json_uri = nlohmann::json_uri{schema["$id"]};
-  mgr->_schema_dictionary.insert({json_uri.url(), schema});
-  schema = rmf_api_msgs::schemas::task_log;
-  json_uri = nlohmann::json_uri{schema["$id"]};
-  mgr->_schema_dictionary.insert({json_uri.url(), schema});
-  schema = rmf_api_msgs::schemas::log_entry;
-  json_uri = nlohmann::json_uri{schema["$id"]};
-  mgr->_schema_dictionary.insert({json_uri.url(), schema});
-  schema = rmf_api_msgs::schemas::task_state_update;
-  json_uri = nlohmann::json_uri{schema["$id"]};
-  mgr->_schema_dictionary.insert({json_uri.url(), schema});
-  schema = rmf_api_msgs::schemas::task_log_update;
-  json_uri = nlohmann::json_uri{schema["$id"]};
-  mgr->_schema_dictionary.insert({json_uri.url(), schema});
+  mgr->_task_request_api_sub = mgr->_context->node()->task_api_request()
+    .observe_on(rxcpp::identity_same_worker(mgr->_context->worker()))
+    .subscribe(
+      [w = mgr->weak_from_this()](
+        const rmf_task_msgs::msg::ApiRequest::SharedPtr& request)
+      {
+        if (const auto self = w.lock())
+          self->_handle_request(request->json_msg, request->request_id);
+      });
+
+  const std::vector<nlohmann::json> schemas = {
+    rmf_api_msgs::schemas::task_state,
+    rmf_api_msgs::schemas::task_log,
+    rmf_api_msgs::schemas::log_entry,
+    rmf_api_msgs::schemas::task_state_update,
+    rmf_api_msgs::schemas::task_log_update,
+    rmf_api_msgs::schemas::simple_response,
+    rmf_api_msgs::schemas::token_response,
+    rmf_api_msgs::schemas::cancel_task_request,
+    rmf_api_msgs::schemas::cancel_task_response,
+    rmf_api_msgs::schemas::kill_task_request,
+    rmf_api_msgs::schemas::kill_task_response,
+    rmf_api_msgs::schemas::interrupt_task_request,
+    rmf_api_msgs::schemas::interrupt_task_response,
+    rmf_api_msgs::schemas::resume_task_request,
+    rmf_api_msgs::schemas::resume_task_response,
+    rmf_api_msgs::schemas::rewind_task_request,
+    rmf_api_msgs::schemas::rewind_task_response,
+    rmf_api_msgs::schemas::skip_phase_request,
+    rmf_api_msgs::schemas::skip_phase_response,
+    rmf_api_msgs::schemas::undo_skip_phase_request,
+    rmf_api_msgs::schemas::undo_skip_phase_response
+  };
+
+  for (const auto& schema : schemas)
+  {
+    const auto json_uri = nlohmann::json_uri{schema["$id"]};
+    mgr->_schema_dictionary.insert({json_uri.url(), schema});
+  }
 
   return mgr;
 }
@@ -180,7 +220,7 @@ auto TaskManager::expected_finish_location() const -> StartSet
 std::optional<std::string> TaskManager::current_task_id() const
 {
   if (_active_task)
-    return _active_task->tag()->booking()->id();
+    return _active_task.id();
 
   return std::nullopt;
 }
@@ -269,7 +309,7 @@ const std::vector<rmf_task::ConstRequestPtr> TaskManager::requests() const
 TaskManager::RobotModeMsg TaskManager::robot_mode() const
 {
   const auto mode = rmf_fleet_msgs::build<RobotModeMsg>()
-    .mode(_active_task == nullptr ?
+    .mode(_active_task ?
       RobotModeMsg::MODE_IDLE :
       _context->current_mode())
     .mode_request_id(0);
@@ -379,11 +419,11 @@ void TaskManager::_begin_next_task()
     RCLCPP_INFO(
       _context->node()->get_logger(),
       "Beginning new task [%s] for [%s]. Remaining queue size: %ld",
-      _active_task->tag()->booking()->id().c_str(),
+      _active_task.id().c_str(),
       _context->requester_id().c_str(),
       _queue.size());
 
-    _register_executed_task(_active_task->tag()->booking()->id());
+    _register_executed_task(_active_task.id());
   }
   else
   {
@@ -468,7 +508,7 @@ void TaskManager::_begin_waiting()
 }
 
 //==============================================================================
-std::function<void()> TaskManager::_make_resume()
+std::function<void()> TaskManager::_make_resume_from_emergency()
 {
   return [w = weak_from_this()]()
   {
@@ -476,12 +516,12 @@ std::function<void()> TaskManager::_make_resume()
     if (!self)
       return;
 
-    self->_resume();
+    self->_resume_from_emergency();
   };
 }
 
 //==============================================================================
-void TaskManager::_resume()
+void TaskManager::_resume_from_emergency()
 {
   _context->worker().schedule(
     [w = weak_from_this()](const auto&)
@@ -493,12 +533,16 @@ void TaskManager::_resume()
       if (self->_emergency_active)
         return;
 
+      if (!self->_emergency_pullover_interrupt_token.has_value())
+        return;
+
       self->_emergency_pullover = nullptr;
-      if (self->_resume_task.has_value())
+      if (self->_active_task)
       {
-        auto resume = *std::move(self->_resume_task);
-        self->_resume_task = std::nullopt;
-        resume();
+        self->_active_task.remove_interruption(
+          {*self->_emergency_pullover_interrupt_token},
+          {"emergency finished"});
+        self->_emergency_pullover_interrupt_token = std::nullopt;
       }
       else
       {
@@ -663,12 +707,12 @@ void TaskManager::_schema_loader(
 }
 
 //==============================================================================
-void TaskManager::_validate_and_publish_json(
+void TaskManager::_validate_and_publish_websocket(
   const nlohmann::json& msg,
-  const nlohmann::json& schema) const
+  const nlohmann::json_schema::json_validator& validator) const
 {
   std::string error = "";
-  if (!_validate_json(msg, schema, error))
+  if (!_validate_json(msg, validator, error))
   {
     RCLCPP_ERROR(
       _context->node()->get_logger(),
@@ -689,6 +733,29 @@ void TaskManager::_validate_and_publish_json(
   client->publish(msg);
 }
 
+//==============================================================================
+void TaskManager::_validate_and_publish_api_response(
+  const nlohmann::json& response,
+  const nlohmann::json_schema::json_validator& validator,
+  const std::string& request_id)
+{
+  std::string error;
+  if (!_validate_json(response, validator, error))
+  {
+    RCLCPP_ERROR(
+      _context->node()->get_logger(),
+      "Error in response to [%s]: %s",
+      request_id.c_str(),
+      error.c_str());
+    return;
+  }
+
+  _context->node()->task_api_response()->publish(
+    rmf_task_msgs::build<rmf_task_msgs::msg::ApiResponse>()
+      .type(rmf_task_msgs::msg::ApiResponse::TYPE_RESPONDING)
+      .json_msg(response.dump())
+      .request_id(request_id));
+}
 
 //==============================================================================
 rmf_task::State TaskManager::_get_state() const
@@ -883,6 +950,14 @@ void copy_assignment(
   assigned_to_json["name"] = context.name();
 }
 
+//==============================================================================
+nlohmann::json make_simple_success_response()
+{
+  nlohmann::json response;
+  response["success"] = true;
+  return response;
+}
+
 } // anonymous namespace
 
 //==============================================================================
@@ -891,34 +966,41 @@ void TaskManager::_publish_task_state()
   if (!_active_task)
     return;
 
-  auto task_state_update = _task_state_update_json;
+  _active_task.publish_task_state(*this);
+}
 
-  const auto& booking = *_active_task->tag()->booking();
-  copy_booking_data(_active_task_state["booking"], booking);
+//==============================================================================
+void TaskManager::ActiveTask::publish_task_state(TaskManager& mgr)
+{
+  auto task_state_update = mgr._task_state_update_json;
 
-  const auto& header = _active_task->tag()->header();
-  _active_task_state["category"] = header.category();
-  _active_task_state["detail"] = header.detail();
+  const auto& booking = *_task->tag()->booking();
+  copy_booking_data(_state_msg["booking"], booking);
+
+  const auto& header = _task->tag()->header();
+  _state_msg["category"] = header.category();
+  _state_msg["detail"] = header.detail();
   // TODO(MXG): Add unix_millis_start_time and unix_millis_finish_time
-  _active_task_state["original_estimate_millis"] =
+  _state_msg["original_estimate_millis"] =
     std::max(0l, to_millis(header.original_duration_estimate()).count());
-  _active_task_state["estimate_millis"] =
-    std::max(0l, to_millis(_active_task->estimate_remaining_time()).count());
-  copy_assignment(_active_task_state["assigned_to"], *_context);
-  _active_task_state["status"] =
-    status_to_string(_active_task->status_overview());
+  _state_msg["estimate_millis"] =
+    std::max(0l, to_millis(_task->estimate_remaining_time()).count());
+  copy_assignment(_state_msg["assigned_to"], *mgr._context);
+  _state_msg["status"] =
+    status_to_string(_task->status_overview());
 
-  auto& phases = _active_task_state["phases"];
+  auto& phases = _state_msg["phases"];
 
   nlohmann::json task_logs;
   auto& phase_logs = task_logs["phases"];
 
   std::vector<uint64_t> completed_ids;
-  completed_ids.reserve(_active_task->completed_phases().size());
-  for (const auto& completed : _active_task->completed_phases())
+  completed_ids.reserve(_task->completed_phases().size());
+  for (const auto& completed : _task->completed_phases())
   {
     const auto& snapshot = completed->snapshot();
-    auto& phase = copy_phase_data(phases, *snapshot, _log_reader, phase_logs);
+    auto& phase = copy_phase_data(
+      phases, *snapshot, mgr._log_reader, phase_logs);
 
     phase["unix_millis_start_time"] =
       completed->start_time().time_since_epoch().count();
@@ -928,25 +1010,27 @@ void TaskManager::_publish_task_state()
 
     completed_ids.push_back(snapshot->tag()->id());
   }
-  _active_task_state["completed"] = std::move(completed_ids);
+  _state_msg["completed"] = std::move(completed_ids);
 
-  const auto active_phase = _active_task->active_phase();
-  copy_phase_data(phases, *active_phase, _log_reader, phase_logs);
+  const auto active_phase = _task->active_phase();
+  copy_phase_data(phases, *active_phase, mgr._log_reader, phase_logs);
 
-  _active_task_state["active"] = active_phase->tag()->id();
+  _state_msg["active"] = active_phase->tag()->id();
 
   std::vector<uint64_t> pending_ids;
-  pending_ids.reserve(_active_task->pending_phases().size());
-  for (const auto& pending : _active_task->pending_phases())
+  pending_ids.reserve(_task->pending_phases().size());
+  for (const auto& pending : _task->pending_phases())
   {
     copy_phase_data(phases, pending);
     pending_ids.push_back(pending.tag()->id());
   }
-  _active_task_state["pending"] = std::move(pending_ids);
+  _state_msg["pending"] = std::move(pending_ids);
+
+  task_state_update["data"] = _state_msg;
 
-  task_state_update["data"] = _active_task_state;
-  _validate_and_publish_json(
-    task_state_update, rmf_api_msgs::schemas::task_state_update);
+  static const auto validator =
+    mgr._make_validator(rmf_api_msgs::schemas::task_state_update);
+  mgr._validate_and_publish_websocket(task_state_update, validator);
 }
 
 //==============================================================================
@@ -976,7 +1060,7 @@ void TaskManager::_publish_task_queue()
     auto task_state_update = _task_state_update_json;
     task_state_update["data"] = pending_json;
 
-    _validate_and_publish_json(
+    _validate_and_publish_websocket(
       task_state_update, rmf_api_msgs::schemas::task_state_update);
 
     expected_state = pending.finish_state();
@@ -986,21 +1070,11 @@ void TaskManager::_publish_task_queue()
 //==============================================================================
 bool TaskManager::_validate_json(
   const nlohmann::json& json,
-  const nlohmann::json& schema,
+  const nlohmann::json_schema::json_validator& validator,
   std::string& error) const
 {
   try
   {
-    nlohmann::json_schema::json_validator validator(
-      schema,
-      [w = weak_from_this()](const nlohmann::json_uri& id,
-      nlohmann::json& value)
-      {
-        const auto self = w.lock();
-        if (!self)
-          return;
-        self->_schema_loader(id, value);
-      });
     validator.validate(json);
   }
   catch (const std::exception& e)
@@ -1012,6 +1086,118 @@ bool TaskManager::_validate_json(
   return true;
 }
 
+//==============================================================================
+bool TaskManager::_validate_request_message(
+  const nlohmann::json& request_json,
+  const nlohmann::json_schema::json_validator& request_validator,
+  const std::string& request_id)
+{
+  std::string error;
+  if (_validate_json(request_json, request_validator, error))
+    return true;
+
+  _send_simple_error_response(
+    request_id, 5, "Invalid request format", std::move(error));
+  return false;
+}
+
+//==============================================================================
+void TaskManager::_send_simple_success_response(const std::string& request_id)
+{
+  static const auto response = make_simple_success_response();
+
+  static const auto simple_response_validator =
+    _make_validator(rmf_api_msgs::schemas::simple_response);
+
+  _validate_and_publish_api_response(
+    response, simple_response_validator, request_id);
+}
+
+//==============================================================================
+void TaskManager::_send_token_success_response(
+  std::string token,
+  const std::string& request_id)
+{
+  nlohmann::json response;
+  response["success"] = true;
+  response["token"] = std::move(token);
+
+  static const auto token_response_validator =
+    _make_validator(rmf_api_msgs::schemas::token_response);
+
+  _validate_and_publish_api_response(
+    response, token_response_validator, request_id);
+}
+
+//==============================================================================
+nlohmann::json_schema::json_validator TaskManager::_make_validator(
+  const nlohmann::json& schema) const
+{
+  return nlohmann::json_schema::json_validator(
+    schema,
+    [w = weak_from_this()](const nlohmann::json_uri& id,
+    nlohmann::json& value)
+    {
+      const auto self = w.lock();
+      if (!self)
+        return;
+      self->_schema_loader(id, value);
+    });
+}
+
+//==============================================================================
+void TaskManager::_send_simple_error_response(
+  const std::string& request_id,
+  uint64_t code,
+  std::string category,
+  std::string detail)
+{
+  static const auto error_validator =
+    _make_validator(rmf_api_msgs::schemas::simple_response);
+
+  _validate_and_publish_api_response(
+    _make_error_response(code, std::move(category), std::move(detail)),
+    error_validator,
+    request_id);
+}
+
+//==============================================================================
+void TaskManager::_send_simple_error_if_queued(
+  const std::string& task_id,
+  const std::string& request_id,
+  const std::string& type)
+{
+  for (const auto& a : _queue)
+  {
+    if (a.request()->booking()->id() == task_id)
+    {
+      return _send_simple_error_response(
+        request_id, 6, "Invalid Circumstances",
+        type + " a task that is queued (not yet active) "
+        "is not currently supported");
+    }
+  }
+}
+
+//==============================================================================
+nlohmann::json TaskManager::_make_error_response(
+  uint64_t code,
+  std::string category,
+  std::string detail)
+{
+  nlohmann::json response;
+  response["success"] = false;
+
+  nlohmann::json error;
+  error["code"] = code;
+  error["category"] = std::move(category);
+  error["detail"] = std::move(detail);
+
+  response["errors"] = std::vector<nlohmann::json>({std::move(error)});
+
+  return response;
+}
+
 //==============================================================================
 std::function<void(rmf_task::Phase::ConstSnapshotPtr)>
 TaskManager::_update_cb()
@@ -1067,7 +1253,7 @@ std::function<void()> TaskManager::_task_finished(std::string id)
 
       // Publish the final state of the task before destructing it
       self->_publish_task_state();
-      self->_active_task = nullptr;
+      self->_active_task = ActiveTask();
 
       self->_context->worker().schedule(
         [w = self->weak_from_this()](const auto&)
@@ -1078,4 +1264,270 @@ std::function<void()> TaskManager::_task_finished(std::string id)
     };
 }
 
+//==============================================================================
+void TaskManager::_handle_request(
+  const std::string& request_msg,
+  const std::string& request_id)
+{
+  const auto request_json = nlohmann::json::parse(request_msg);
+  const auto& type = request_json["type"];
+  if (!type)
+    return;
+
+  try
+  {
+    const auto& type_str = type.get<std::string>();
+    if (type_str == "cancel_task_request")
+      _handle_cancel_request(request_json, request_id);
+    else if (type_str == "kill_task_request")
+      _handle_kill_request(request_json, request_id);
+    else if (type_str == "interrupt_task_request")
+      _handle_interrupt_request(request_json, request_id);
+    else if (type_str == "resume_task_request")
+      _handle_resume_request(request_json, request_id);
+    else if (type_str == "rewind_task_request")
+      _handle_rewind_request(request_json, request_id);
+    else if (type_str == "skip_phase_request")
+      _handle_skip_phase_request(request_json, request_id);
+    else if (type_str == "undo_phase_skip_request")
+      _handle_undo_skip_phase_request(request_json, request_id);
+  }
+  catch (const std::exception& e)
+  {
+    RCLCPP_WARN(
+      _context->node()->get_logger(),
+      "Encountered exception while handling a request: %s", e.what());
+  }
+}
+
+namespace {
+//==============================================================================
+std::vector<std::string> get_labels(const nlohmann::json& request)
+{
+  if (const auto& labels = request["labels"])
+    return labels.get<std::vector<std::string>>();
+
+  return {};
+}
+
+//==============================================================================
+void remove_task_from_queue(
+  const std::string& task_id,
+  std::vector<TaskManager::Assignment>& queue)
+{
+  // If the task is queued, then we should make sure to remove it from the
+  // queue, just in case it reaches an active state before the dispatcher
+  // issues its cancellation request.
+  //
+  // TODO(MXG): We should do a much better of job of coordinating these
+  // different moving parts in the system. E.g. who is ultimately responsible
+  // for issuing the response to the request or updating the task state?
+  for (auto it = queue.begin(); it != queue.end(); ++it)
+  {
+    if (it->request()->booking()->id() == task_id)
+    {
+      queue.erase(it);
+      break;
+    }
+  }
+}
+}
+
+//==============================================================================
+void TaskManager::_handle_cancel_request(
+  const nlohmann::json& request_json,
+  const std::string& request_id)
+{
+  static const auto request_validator =
+    _make_validator(rmf_api_msgs::schemas::cancel_task_request);
+
+  if (!_validate_request_message(request_json, request_validator, request_id))
+    return;
+
+  const auto& task_id = request_json["task_id"].get<std::string>();
+
+  if (_active_task && _active_task.id() == task_id)
+  {
+    _active_task.cancel(get_labels(request_json));
+    _task_state_update_available = true;
+    return _send_simple_success_response(request_id);
+  }
+
+  remove_task_from_queue(task_id, _queue);
+}
+
+//==============================================================================
+void TaskManager::_handle_kill_request(
+  const nlohmann::json& request_json,
+  const std::string& request_id)
+{
+  static const auto request_validator =
+    _make_validator(rmf_api_msgs::schemas::kill_task_request);
+
+  if (!_validate_request_message(request_json, request_validator, request_id))
+    return;
+
+  const auto& task_id = request_json["task_id"].get<std::string>();
+
+  if (_active_task && _active_task.id() == task_id)
+  {
+    _task_state_update_available = true;
+    _active_task.kill(get_labels(request_json));
+    return _send_simple_success_response(request_id);
+  }
+
+  remove_task_from_queue(task_id, _queue);
+}
+
+//==============================================================================
+void TaskManager::_handle_interrupt_request(
+  const nlohmann::json& request_json,
+  const std::string& request_id)
+{
+  static const auto request_validator =
+    _make_validator(rmf_api_msgs::schemas::interrupt_task_request);
+
+  if (!_validate_request_message(request_json, request_validator, request_id))
+    return;
+
+  const auto& task_id = request_json["task_id"].get<std::string>();
+
+  if (_active_task && _active_task.id() == task_id)
+  {
+    _task_state_update_available = true;
+    return _send_token_success_response(
+      _active_task.add_interruption(get_labels(request_json)),
+      request_id);
+  }
+
+  _send_simple_error_if_queued(task_id, request_id, "Interrupting");
+}
+
+//==============================================================================
+void TaskManager::_handle_resume_request(
+  const nlohmann::json& request_json,
+  const std::string& request_id)
+{
+  static const auto request_validator =
+    _make_validator(rmf_api_msgs::schemas::resume_task_request);
+
+  if (!_validate_request_message(request_json, request_validator, request_id))
+    return;
+
+  const auto& task_id = request_json["for_tokens"].get<std::string>();
+
+  if (_active_task && _active_task.id() == task_id)
+  {
+    _task_state_update_available = true;
+    auto unknown_tokens = _active_task.remove_interruption(
+      request_json["for_tokens"].get<std::vector<std::string>>(),
+      get_labels(request_json));
+
+    if (unknown_tokens.empty())
+      return _send_simple_success_response(request_id);
+
+    std::string detail = "[";
+    for (std::size_t i=0; i < unknown_tokens.size(); ++i)
+    {
+      detail += unknown_tokens[i];
+      if (i < unknown_tokens.size()-1)
+        detail += ", ";
+    }
+    detail += "]";
+
+    return _send_simple_error_response(
+      request_id, 7, "Unknown Tokens", std::move(detail));
+  }
+
+  _send_simple_error_if_queued(task_id, request_id, "Resuming");
+}
+
+//==============================================================================
+void TaskManager::_handle_rewind_request(
+  const nlohmann::json& request_json,
+  const std::string& request_id)
+{
+  static const auto request_validator =
+    _make_validator(rmf_api_msgs::schemas::rewind_task_request);
+
+  if (!_validate_request_message(request_json, request_validator, request_id))
+    return;
+
+  const auto& task_id = request_json["task_id"].get<std::string>();
+
+  if (_active_task && _active_task.id() == task_id)
+  {
+    _task_state_update_available = true;
+    _active_task.rewind(request_json["phase_id"].get<uint64_t>());
+    return _send_simple_success_response(request_id);
+  }
+
+  _send_simple_error_if_queued(task_id, request_id, "Rewinding");
+}
+
+//==============================================================================
+void TaskManager::_handle_skip_phase_request(
+  const nlohmann::json& request_json,
+  const std::string& request_id)
+{
+  static const auto request_validator =
+    _make_validator(rmf_api_msgs::schemas::skip_phase_request);
+
+  if (!_validate_request_message(request_json, request_validator, request_id))
+    return;
+
+  const auto& task_id = request_json["task_id"].get<std::string>();
+
+  if (_active_task && _active_task.id() == task_id)
+  {
+    _task_state_update_available = true;
+    return _send_token_success_response(
+      _active_task.skip(
+        request_json["phase_id"].get<uint64_t>(),
+        get_labels(request_json)),
+      request_id);
+  }
+
+  _send_simple_error_if_queued(task_id, request_id, "Skipping a phase in ");
+}
+
+//==============================================================================
+void TaskManager::_handle_undo_skip_phase_request(
+  const nlohmann::json& request_json,
+  const std::string& request_id)
+{
+  static const auto request_validator =
+    _make_validator(rmf_api_msgs::schemas::undo_skip_phase_request);
+
+  if (!_validate_request_message(request_json, request_validator, request_id))
+    return;
+
+  const auto& task_id = request_json["for_task"];
+
+  if (_active_task && _active_task.id() == task_id)
+  {
+    _task_state_update_available = true;
+    auto unknown_tokens = _active_task.remove_skips(
+      request_json["for_tokens"].get<std::vector<std::string>>(),
+      get_labels(request_json));
+
+    if (unknown_tokens.empty())
+      return _send_simple_success_response(request_id);
+
+    std::string detail = "[";
+    for (std::size_t i=0; i < unknown_tokens.size(); ++i)
+    {
+      detail += unknown_tokens[i];
+      if (i < unknown_tokens.size()-1)
+        detail += ", ";
+    }
+    detail += "]";
+
+    return _send_simple_error_response(
+      request_id, 7, "Unknown Tokens", std::move(detail));
+  }
+
+  _send_simple_error_if_queued(task_id, request_id, "Undoing a phase skip in ");
+}
+
 } // namespace rmf_fleet_adapter
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
index 66314d547..3dfaa985f 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
@@ -105,12 +105,63 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
     agv::RobotContextPtr context,
     std::weak_ptr<BroadcastClient> broadcast_client);
 
+  class ActiveTask
+  {
+  public:
+    ActiveTask(rmf_task::Task::ActivePtr task = nullptr)
+    : _task(std::move(task))
+    {
+      // Do nothing
+    }
+
+    const std::string& id() const;
+
+    void publish_task_state(TaskManager& mgr);
+
+    operator bool() const
+    {
+      return static_cast<bool>(_task);
+    }
+
+    /// Adds an interruption
+    std::string add_interruption(std::vector<std::string> labels);
+
+    // Any unknown tokens that were included will be returned
+    std::vector<std::string> remove_interruption(
+      std::vector<std::string> for_tokens,
+      std::vector<std::string> labels);
+
+    void cancel(std::vector<std::string> labels);
+
+    void kill(std::vector<std::string> labels);
+
+    void rewind(uint64_t phase_id);
+
+    std::string skip(uint64_t phase_id, std::vector<std::string> labels);
+
+    std::vector<std::string> remove_skips(
+      std::vector<std::string> for_tokens,
+      std::vector<std::string> labels);
+
+  private:
+    std::unordered_map<std::string, nlohmann::json> active_interruptions;
+    std::unordered_map<std::string, nlohmann::json> resolved_interruptions;
+    std::optional<rmf_task::Task::Active::Resume> resume_task;
+    std::optional<nlohmann::json> cancellation;
+    std::optional<nlohmann::json> killed;
+
+    rmf_task::Task::ActivePtr _task;
+    nlohmann::json _state_msg;
+  };
+
+  friend class ActiveTask;
+
   agv::RobotContextPtr _context;
   std::weak_ptr<BroadcastClient> _broadcast_client;
   rmf_task::ConstActivatorPtr _task_activator;
-  rmf_task::Task::ActivePtr _active_task;
-  std::optional<rmf_task::Task::Active::Resume> _resume_task;
+  ActiveTask _active_task;
   bool _emergency_active = false;
+  std::optional<std::string> _emergency_pullover_interrupt_token;
   rmf_task_sequence::Event::ActivePtr _emergency_pullover;
   std::vector<Assignment> _queue;
   rmf_utils::optional<Start> _expected_finish_location;
@@ -144,6 +195,8 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
   // TODO: Get this and loader from FleetUpdateHandle
   std::unordered_map<std::string, nlohmann::json> _schema_dictionary = {};
 
+  rmf_rxcpp::subscription_guard _task_request_api_sub;
+
   // Constant jsons with validated schemas for internal use
   // TODO(YV): Replace these with codegen tools
   const nlohmann::json _task_log_update_msg =
@@ -159,11 +212,10 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
     {"pending", {}}, {"interruptions", {}}, {"cancellation", {}},
     {"killed", {}}};
 
-  // The task_state.json for the active task. This should be initialized when
-  // a request is activated.
-  nlohmann::json _active_task_state;
   rmf_task::Log::Reader _log_reader;
 
+  uint64_t _next_token = 0;
+
   // Map task_id to task_log.json for all tasks managed by this TaskManager
   std::unordered_map<std::string, nlohmann::json> _task_logs = {};
 
@@ -174,10 +226,10 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
   void _begin_waiting();
 
   /// Make the callback for resuming
-  std::function<void()> _make_resume();
+  std::function<void()> _make_resume_from_emergency();
 
   /// Resume whatever the task manager should be doing
-  void _resume();
+  void _resume_from_emergency();
 
   /// Get the current state of the robot
   rmf_task::State _get_state() const;
@@ -199,14 +251,57 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
   // TODO: Move this into a utils?
   bool _validate_json(
     const nlohmann::json& json,
-    const nlohmann::json& schema,
+    const nlohmann::json_schema::json_validator& validator,
     std::string& error) const;
 
+  /// If the request message is valid this will return true. If it is not valid,
+  /// this will publish an error message for this request and return false. The
+  /// caller should not attempt to process this request or respond to it any
+  /// further.
+  bool _validate_request_message(
+    const nlohmann::json& request,
+    const nlohmann::json_schema::json_validator& validator,
+    const std::string& request_id);
+
+  void _send_simple_success_response(
+    const std::string& request_id);
+
+  void _send_token_success_response(
+    std::string token,
+    const std::string& request_id);
+
+  /// Make a validator for the given schema
+  nlohmann::json_schema::json_validator _make_validator(
+    const nlohmann::json& schema) const;
+
+  void _send_simple_error_response(
+    const std::string& request_id,
+    uint64_t code,
+    std::string category,
+    std::string detail);
+
+  void _send_simple_error_if_queued(
+    const std::string& task_id,
+    const std::string& request_id,
+    const std::string& type);
+
+  /// Make an error message to return
+  static nlohmann::json _make_error_response(
+    uint64_t code,
+    std::string category,
+    std::string detail);
+
   /// Validate and publish a json. This can be used for task
   /// state and log updates
-  void _validate_and_publish_json(
+  void _validate_and_publish_websocket(
     const nlohmann::json& msg,
-    const nlohmann::json& schema) const;
+    const nlohmann::json_schema::json_validator& validator) const;
+
+  /// Validate and publish a response message over the ROS2 API response topic
+  void _validate_and_publish_api_response(
+    const nlohmann::json& msg,
+    const nlohmann::json_schema::json_validator& validator,
+    const std::string& request_id);
 
   /// Callback for when the task has a significat update
   std::function<void(rmf_task::Phase::ConstSnapshotPtr)> _update_cb();
@@ -220,15 +315,6 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
   /// Callback for when the task has finished
   std::function<void()> _task_finished(std::string id);
 
-  // TODO: Assuming each LegacyTask::Active instance stores a weak_ptr to this
-  // TaskManager, the implementations of the corresponding functions in
-  // LegacyTask::Active can call these methods to publish state/log updates
-  // void _interrupt_active_task();
-  // void _cancel_active_task();
-  // void _kill_active_task();
-  // void _skip_phase_in_active_task(uint64_t phase_id, bool value = true);
-  // void _rewind_active_task(uint64_t phase_id);
-
   /// Function to register the task id of a task that has begun execution
   /// The input task id will be inserted into the registry such that the max
   /// size of the registry is 100.
@@ -238,6 +324,38 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
     std::shared_ptr<LegacyTask> task,
     uint32_t task_summary_state,
     TaskSummaryMsg& msg);
+
+  void _handle_request(
+    const std::string& request_msg,
+    const std::string& request_id);
+
+  void _handle_cancel_request(
+    const nlohmann::json& request_json,
+    const std::string& request_id);
+
+  void _handle_kill_request(
+    const nlohmann::json& request_json,
+    const std::string& request_id);
+
+  void _handle_interrupt_request(
+    const nlohmann::json& request_json,
+    const std::string& request_id);
+
+  void _handle_resume_request(
+    const nlohmann::json& request_json,
+    const std::string& request_id);
+
+  void _handle_rewind_request(
+    const nlohmann::json& request_json,
+    const std::string& request_id);
+
+  void _handle_skip_phase_request(
+    const nlohmann::json& request_json,
+    const std::string& request_id);
+
+  void _handle_undo_skip_phase_request(
+    const nlohmann::json& request_json,
+    const std::string& request_id);
 };
 
 using TaskManagerPtr = std::shared_ptr<TaskManager>;
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyTrafficLight.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyTrafficLight.cpp
index e7ab8f5ea..e5520ae2b 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyTrafficLight.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyTrafficLight.cpp
@@ -321,7 +321,7 @@ auto EasyTrafficLight::Implementation::moving_from(
       RCLCPP_WARN(
         node->get_logger(),
         "[EasyTrafficLight::moving_from] [%s] owned by [%s] is moving from an "
-        "invalid checkpoint [%u]. This robot currently does not have a path.",
+        "invalid checkpoint [%lu]. This robot currently does not have a path.",
         name.c_str(), owner.c_str(), checkpoint);
     }
     else
@@ -329,8 +329,8 @@ auto EasyTrafficLight::Implementation::moving_from(
       RCLCPP_WARN(
         node->get_logger(),
         "[EasyTrafficLight::moving_from] [%s] owned by [%s] is moving from "
-        "an invalid checkpoint [%u]. The highest checkpoint value that you "
-        "can move from is [%u].",
+        "an invalid checkpoint [%lu]. The highest checkpoint value that you "
+        "can move from is [%lu].",
         name.c_str(), owner.c_str(),
         checkpoint, current_checkpoints.size()-1);
     }
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
index fd9631145..9487cbaed 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
@@ -648,9 +648,6 @@ void FleetUpdateHandle::Implementation::dispatch_request_cb(
 
   else if (msg->method == DispatchRequest::CANCEL)
   {
-    // We currently only support cancellation of a queued task.
-    // TODO: Support cancellation of an active task.
-
     // When a queued task is to be cancelled, we simply re-plan and re-allocate
     // task assignments for the request set containing all the queued tasks
     // excluding the task to be cancelled.
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp
index d1cd875bd..2e4629ac7 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp
@@ -93,6 +93,14 @@ std::shared_ptr<Node> Node::make(
     node->create_publisher<FleetState>(
     FleetStateTopicName, default_qos);
 
+  node->_task_api_request_obs =
+    node->create_observable<ApiRequest>(
+    TaskApiRequests, rclcpp::SystemDefaultsQoS().reliable().transient_local());
+
+  node->_task_api_response_pub =
+    node->create_publisher<ApiResponse>(
+    TaskApiResponses, rclcpp::SystemDefaultsQoS().reliable().transient_local());
+
   return node;
 }
 
@@ -198,5 +206,17 @@ auto Node::fleet_state() const -> const FleetStatePub&
   return _fleet_state_pub;
 }
 
+//==============================================================================
+auto Node::task_api_request() const -> const ApiRequestObs&
+{
+  return _task_api_request_obs->observe();
+}
+
+//==============================================================================
+auto Node::task_api_response() const -> const ApiResponsePub&
+{
+  return _task_api_response_pub;
+}
+
 } // namespace agv
 } // namespace rmf_fleet_adapter
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp
index 58b0a11d4..f395f4bc8 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp
@@ -36,6 +36,9 @@
 
 #include <rmf_fleet_msgs/msg/fleet_state.hpp>
 
+#include <rmf_task_msgs/msg/api_request.hpp>
+#include <rmf_task_msgs/msg/api_response.hpp>
+
 #include <rmf_traffic/Time.hpp>
 
 namespace rmf_fleet_adapter {
@@ -109,6 +112,14 @@ class Node : public rmf_rxcpp::Transport
   using FleetStatePub = rclcpp::Publisher<FleetState>::SharedPtr;
   const FleetStatePub& fleet_state() const;
 
+  using ApiRequest = rmf_task_msgs::msg::ApiRequest;
+  using ApiRequestObs = rxcpp::observable<ApiRequest::SharedPtr>;
+  const ApiRequestObs& task_api_request() const;
+
+  using ApiResponse = rmf_task_msgs::msg::ApiResponse;
+  using ApiResponsePub = rclcpp::Publisher<ApiResponse>::SharedPtr;
+  const ApiResponsePub& task_api_response() const;
+
   template<typename DurationRepT, typename DurationT, typename CallbackT>
   rclcpp::TimerBase::SharedPtr try_create_wall_timer(
     std::chrono::duration<DurationRepT, DurationT> period,
@@ -164,6 +175,8 @@ class Node : public rmf_rxcpp::Transport
   Bridge<IngestorResult> _ingestor_result_obs;
   Bridge<IngestorState> _ingestor_state_obs;
   FleetStatePub _fleet_state_pub;
+  Bridge<ApiRequest> _task_api_request_obs;
+  ApiResponsePub _task_api_response_pub;
 };
 
 } // namespace agv

From 1d37028d703e8e98ae4490854799c2ebb0ed6320 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Fri, 24 Dec 2021 20:01:44 +0800
Subject: [PATCH 27/79] Implemented interrupt and resume -- just a few more
 commands missing

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 .../src/rmf_fleet_adapter/TaskManager.cpp     | 632 +++++++++++-------
 .../src/rmf_fleet_adapter/TaskManager.hpp     |  41 +-
 2 files changed, 408 insertions(+), 265 deletions(-)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
index 4e2305bab..e7f5961f7 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
@@ -105,7 +105,10 @@ TaskManagerPtr TaskManager::make(
           if (mgr->_active_task)
           {
             mgr->_emergency_pullover_interrupt_token =
-              mgr->_active_task.add_interruption({"emergency pullover"});
+              mgr->_active_task.add_interruption(
+                {"emergency pullover"},
+                mgr->_context->now(),
+                [begin_pullover]() { begin_pullover(); });
           }
           else
           {
@@ -207,6 +210,378 @@ TaskManager::TaskManager(
   // Do nothing. The make() function does all further initialization.
 }
 
+//==============================================================================
+TaskManager::ActiveTask::ActiveTask(rmf_task::Task::ActivePtr task)
+: _task(std::move(task)),
+  _interruption_handler(std::make_shared<InterruptionHandler>())
+{
+  // Do nothing
+}
+
+//==============================================================================
+const std::string& TaskManager::ActiveTask::id() const
+{
+  if (!_task)
+  {
+    throw std::runtime_error(
+      "[TaskManager::ActiveTask::id] Called when there is no active task. "
+      "This is a serious bug, please report this to the developers of RMF ");
+  }
+
+  return _task->tag()->booking()->id();
+}
+
+namespace {
+//==============================================================================
+std::chrono::milliseconds to_millis(rmf_traffic::Duration duration)
+{
+  return std::chrono::duration_cast<std::chrono::milliseconds>(duration);
+}
+
+//==============================================================================
+std::string status_to_string(rmf_task::Event::Status status)
+{
+  using Status = rmf_task::Event::Status;
+  switch (status)
+  {
+  case Status::Uninitialized:
+    return "uninitialized";
+  case Status::Blocked:
+    return "blocked";
+  case Status::Error:
+    return "error";
+  case Status::Failed:
+    return "failed";
+  case Status::Standby:
+    return "standby";
+  case Status::Underway:
+    return "underway";
+  case Status::Delayed:
+    return "delayed";
+  case Status::Skipped:
+    return "skipped";
+  case Status::Canceled:
+    return "canceled";
+  case Status::Killed:
+    return "killed";
+  case Status::Completed:
+    return "completed";
+  default:
+    return "uninitialized";
+  }
+}
+
+//==============================================================================
+std::string tier_to_string(rmf_task::Log::Entry::Tier tier)
+{
+  using Tier = rmf_task::Log::Entry::Tier;
+  switch (tier)
+  {
+  case Tier::Info:
+    return "info";
+  case Tier::Warning:
+    return "warning";
+  case Tier::Error:
+    return "error";
+  default:
+    return "uninitialized";
+  }
+}
+
+//==============================================================================
+nlohmann::json log_to_json(const rmf_task::Log::Entry& entry)
+{
+  nlohmann::json output;
+  output["seq"] = entry.seq();
+  output["tier"] = tier_to_string(entry.tier());
+  output["unix_millis_time"] =
+    to_millis(entry.time().time_since_epoch()).count();
+  output["text"] = entry.text();
+
+  return output;
+}
+
+//==============================================================================
+nlohmann::json& copy_phase_data(
+  nlohmann::json& phases,
+  const rmf_task::Phase::Active& snapshot,
+  rmf_task::Log::Reader& reader,
+  nlohmann::json& all_phase_logs)
+{
+  const auto& tag = *snapshot.tag();
+  const auto& header = tag.header();
+  const auto id = tag.id();
+  auto& phase = phases[std::to_string(id)];
+  phase["id"] = id;
+  phase["category"] = header.category();
+  phase["detail"] = header.detail();
+  phase["original_estimate_millis"] =
+    std::max(0l, to_millis(header.original_duration_estimate()).count());
+  phase["estimate_millis"] =
+    std::max(0l, to_millis(snapshot.estimate_remaining_time()).count());
+  phase["final_event_id"] = snapshot.final_event()->id();
+
+  // TODO(MXG): Add in skip request information
+
+  std::vector<rmf_task::Event::ConstStatePtr> event_queue;
+  event_queue.push_back(snapshot.final_event());
+
+  auto& phase_logs = all_phase_logs[std::to_string(id)];
+  auto& event_logs = phase_logs["events"];
+
+  while (!event_queue.empty())
+  {
+    const auto top = event_queue.back();
+    event_queue.pop_back();
+
+    nlohmann::json event_state;
+    event_state["id"] = top->id();
+    event_state["status"] = status_to_string(top->status());
+
+    // TODO(MXG): Keep a VersionedString Reader to know when to actually update
+    // this string
+    event_state["name"] =
+      *rmf_task::VersionedString::Reader().read(top->name());
+
+    event_state["detail"] =
+      *rmf_task::VersionedString::Reader().read(top->detail());
+
+    std::vector<nlohmann::json> logs;
+    for (const auto& log : reader.read(top->log()))
+      logs.push_back(log_to_json(log));
+
+    if (!logs.empty())
+      event_logs[std::to_string(top->id())] = std::move(logs);
+
+    std::vector<uint32_t> deps;
+    deps.reserve(top->dependencies().size());
+    for (const auto& dep : top->dependencies())
+    {
+      event_queue.push_back(dep);
+      deps.push_back(dep->id());
+    }
+
+    event_state["deps"] = std::move(deps);
+  }
+
+  return phase;
+}
+
+//==============================================================================
+void copy_phase_data(
+  nlohmann::json& phases,
+  const rmf_task::Phase::Pending& pending)
+{
+  const auto id = pending.tag()->id();
+  auto& phase = phases[std::to_string(id)];
+  phase["id"] = id;
+
+  const auto& header = pending.tag()->header();
+  phase["category"] = header.category();
+  phase["detail"] = header.detail();
+  phase["estimate_millis"] =
+    std::max(0l, to_millis(header.original_duration_estimate()).count());
+}
+
+//==============================================================================
+void copy_booking_data(
+  nlohmann::json& booking_json,
+  const rmf_task::Task::Booking& booking)
+{
+  booking_json["id"] = booking.id();
+  booking_json["unix_millis_earliest_start_time"] =
+    to_millis(booking.earliest_start_time().time_since_epoch()).count();
+  // TODO(MXG): Add priority and labels
+}
+
+//==============================================================================
+void copy_assignment(
+  nlohmann::json& assigned_to_json,
+  const agv::RobotContext& context)
+{
+  assigned_to_json["group"] = context.group();
+  assigned_to_json["name"] = context.name();
+}
+
+//==============================================================================
+nlohmann::json make_simple_success_response()
+{
+  nlohmann::json response;
+  response["success"] = true;
+  return response;
+}
+
+} // anonymous namespace
+
+//==============================================================================
+void TaskManager::ActiveTask::publish_task_state(TaskManager& mgr)
+{
+  auto task_state_update = mgr._task_state_update_json;
+
+  const auto& booking = *_task->tag()->booking();
+  copy_booking_data(_state_msg["booking"], booking);
+
+  const auto& header = _task->tag()->header();
+  _state_msg["category"] = header.category();
+  _state_msg["detail"] = header.detail();
+  // TODO(MXG): Add unix_millis_start_time and unix_millis_finish_time
+  _state_msg["original_estimate_millis"] =
+    std::max(0l, to_millis(header.original_duration_estimate()).count());
+  _state_msg["estimate_millis"] =
+    std::max(0l, to_millis(_task->estimate_remaining_time()).count());
+  copy_assignment(_state_msg["assigned_to"], *mgr._context);
+  _state_msg["status"] =
+    status_to_string(_task->status_overview());
+
+  auto& phases = _state_msg["phases"];
+
+  nlohmann::json task_logs;
+  auto& phase_logs = task_logs["phases"];
+
+  std::vector<uint64_t> completed_ids;
+  completed_ids.reserve(_task->completed_phases().size());
+  for (const auto& completed : _task->completed_phases())
+  {
+    const auto& snapshot = completed->snapshot();
+    auto& phase = copy_phase_data(
+      phases, *snapshot, mgr._log_reader, phase_logs);
+
+    phase["unix_millis_start_time"] =
+      completed->start_time().time_since_epoch().count();
+
+    phase["unix_millis_finish_time"] =
+      completed->finish_time().time_since_epoch().count();
+
+    completed_ids.push_back(snapshot->tag()->id());
+  }
+  _state_msg["completed"] = std::move(completed_ids);
+
+  const auto active_phase = _task->active_phase();
+  copy_phase_data(phases, *active_phase, mgr._log_reader, phase_logs);
+
+  _state_msg["active"] = active_phase->tag()->id();
+
+  std::vector<uint64_t> pending_ids;
+  pending_ids.reserve(_task->pending_phases().size());
+  for (const auto& pending : _task->pending_phases())
+  {
+    copy_phase_data(phases, pending);
+    pending_ids.push_back(pending.tag()->id());
+  }
+  _state_msg["pending"] = std::move(pending_ids);
+
+  task_state_update["data"] = _state_msg;
+
+  // TODO(MXG): Add interruption, skip, cancel, and kill information
+
+  static const auto validator =
+    mgr._make_validator(rmf_api_msgs::schemas::task_state_update);
+
+  mgr._validate_and_publish_websocket(task_state_update, validator);
+}
+
+//==============================================================================
+std::string TaskManager::ActiveTask::add_interruption(
+  std::vector<std::string> labels,
+  rmf_traffic::Time time,
+  std::function<void()> task_is_interrupted)
+{
+  std::string token = std::to_string(_next_token++);
+
+  nlohmann::json interruption_json;
+  interruption_json["unix_millis_request_time"] =
+    to_millis(time.time_since_epoch()).count();
+
+  interruption_json["labels"] = std::move(labels);
+
+  _active_interruptions[token] = std::move(interruption_json);
+
+  if (_resume_task.has_value())
+  {
+    std::lock_guard<std::mutex> lock(_interruption_handler->mutex);
+    if (_interruption_handler->is_interrupted)
+    {
+      task_is_interrupted();
+    }
+    else
+    {
+      _interruption_handler->interruption_listeners
+        .push_back(std::move(task_is_interrupted));
+    }
+
+    return token;
+  }
+
+  _resume_task = _task->interrupt(
+    [w = _interruption_handler->weak_from_this()]()
+    {
+      const auto handler = w.lock();
+      if (!handler)
+        return;
+
+      std::lock_guard<std::mutex> lock(handler->mutex);
+      handler->is_interrupted = true;
+      for (const auto& listener : handler->interruption_listeners)
+        listener();
+    });
+
+  return token;
+}
+
+//==============================================================================
+std::vector<std::string> TaskManager::ActiveTask::remove_interruption(
+  std::vector<std::string> for_tokens,
+  std::vector<std::string> labels,
+  rmf_traffic::Time time)
+{
+  nlohmann::json resume_json;
+  resume_json["unix_millis_request_time"] =
+    to_millis(time.time_since_epoch()).count();
+
+  resume_json["labels"] = std::move(labels);
+
+  std::vector<std::string> missing_tokens;
+  for (const auto& token : for_tokens)
+  {
+    const auto it = _active_interruptions.find(token);
+    if (it == _active_interruptions.end())
+    {
+      if (_removed_interruptions.count(token) == 0)
+      {
+        missing_tokens.push_back(token);
+        continue;
+      }
+    }
+
+    auto interruption_json = it->second;
+    interruption_json["resumed_by"] = resume_json;
+    _removed_interruptions[token] = interruption_json;
+    _active_interruptions.erase(it);
+  }
+
+  if (_active_interruptions.empty())
+  {
+    if (_resume_task.has_value())
+    {
+      std::lock_guard<std::mutex> lock(_interruption_handler->mutex);
+      _interruption_handler->is_interrupted = false;
+      _interruption_handler->interruption_listeners.clear();
+
+      (*_resume_task)();
+      _resume_task = std::nullopt;
+    }
+  }
+
+  return missing_tokens;
+}
+
+//==============================================================================
+void TaskManager::ActiveTask::cancel(std::vector<std::string> labels)
+{
+  nlohmann::json cancellation;
+
+}
+
 //==============================================================================
 auto TaskManager::expected_finish_location() const -> StartSet
 {
@@ -541,7 +916,8 @@ void TaskManager::_resume_from_emergency()
       {
         self->_active_task.remove_interruption(
           {*self->_emergency_pullover_interrupt_token},
-          {"emergency finished"});
+          {"emergency finished"},
+          self->_context->now());
         self->_emergency_pullover_interrupt_token = std::nullopt;
       }
       else
@@ -778,188 +1154,6 @@ void TaskManager::_consider_publishing_updates()
   }
 }
 
-namespace {
-//==============================================================================
-std::chrono::milliseconds to_millis(rmf_traffic::Duration duration)
-{
-  return std::chrono::duration_cast<std::chrono::milliseconds>(duration);
-}
-
-//==============================================================================
-std::string status_to_string(rmf_task::Event::Status status)
-{
-  using Status = rmf_task::Event::Status;
-  switch (status)
-  {
-  case Status::Uninitialized:
-    return "uninitialized";
-  case Status::Blocked:
-    return "blocked";
-  case Status::Error:
-    return "error";
-  case Status::Failed:
-    return "failed";
-  case Status::Standby:
-    return "standby";
-  case Status::Underway:
-    return "underway";
-  case Status::Delayed:
-    return "delayed";
-  case Status::Skipped:
-    return "skipped";
-  case Status::Canceled:
-    return "canceled";
-  case Status::Killed:
-    return "killed";
-  case Status::Completed:
-    return "completed";
-  default:
-    return "uninitialized";
-  }
-}
-
-//==============================================================================
-std::string tier_to_string(rmf_task::Log::Entry::Tier tier)
-{
-  using Tier = rmf_task::Log::Entry::Tier;
-  switch (tier)
-  {
-  case Tier::Info:
-    return "info";
-  case Tier::Warning:
-    return "warning";
-  case Tier::Error:
-    return "error";
-  default:
-    return "uninitialized";
-  }
-}
-
-//==============================================================================
-nlohmann::json log_to_json(const rmf_task::Log::Entry& entry)
-{
-  nlohmann::json output;
-  output["seq"] = entry.seq();
-  output["tier"] = tier_to_string(entry.tier());
-  output["unix_millis_time"] =
-    to_millis(entry.time().time_since_epoch()).count();
-  output["text"] = entry.text();
-
-  return output;
-}
-
-//==============================================================================
-nlohmann::json& copy_phase_data(
-  nlohmann::json& phases,
-  const rmf_task::Phase::Active& snapshot,
-  rmf_task::Log::Reader& reader,
-  nlohmann::json& all_phase_logs)
-{
-  const auto& tag = *snapshot.tag();
-  const auto& header = tag.header();
-  const auto id = tag.id();
-  auto& phase = phases[std::to_string(id)];
-  phase["id"] = id;
-  phase["category"] = header.category();
-  phase["detail"] = header.detail();
-  phase["original_estimate_millis"] =
-    std::max(0l, to_millis(header.original_duration_estimate()).count());
-  phase["estimate_millis"] =
-    std::max(0l, to_millis(snapshot.estimate_remaining_time()).count());
-  phase["final_event_id"] = snapshot.final_event()->id();
-
-  // TODO(MXG): Add in skip request information
-
-  std::vector<rmf_task::Event::ConstStatePtr> event_queue;
-  event_queue.push_back(snapshot.final_event());
-
-  auto& phase_logs = all_phase_logs[std::to_string(id)];
-  auto& event_logs = phase_logs["events"];
-
-  while (!event_queue.empty())
-  {
-    const auto top = event_queue.back();
-    event_queue.pop_back();
-
-    nlohmann::json event_state;
-    event_state["id"] = top->id();
-    event_state["status"] = status_to_string(top->status());
-
-    // TODO(MXG): Keep a VersionedString Reader to know when to actually update
-    // this string
-    event_state["name"] =
-      *rmf_task::VersionedString::Reader().read(top->name());
-
-    event_state["detail"] =
-      *rmf_task::VersionedString::Reader().read(top->detail());
-
-    std::vector<nlohmann::json> logs;
-    for (const auto& log : reader.read(top->log()))
-      logs.push_back(log_to_json(log));
-
-    if (!logs.empty())
-      event_logs[std::to_string(top->id())] = std::move(logs);
-
-    std::vector<uint32_t> deps;
-    deps.reserve(top->dependencies().size());
-    for (const auto& dep : top->dependencies())
-    {
-      event_queue.push_back(dep);
-      deps.push_back(dep->id());
-    }
-
-    event_state["deps"] = std::move(deps);
-  }
-
-  return phase;
-}
-
-//==============================================================================
-void copy_phase_data(
-  nlohmann::json& phases,
-  const rmf_task::Phase::Pending& pending)
-{
-  const auto id = pending.tag()->id();
-  auto& phase = phases[std::to_string(id)];
-  phase["id"] = id;
-
-  const auto& header = pending.tag()->header();
-  phase["category"] = header.category();
-  phase["detail"] = header.detail();
-  phase["estimate_millis"] =
-    std::max(0l, to_millis(header.original_duration_estimate()).count());
-}
-
-//==============================================================================
-void copy_booking_data(
-  nlohmann::json& booking_json,
-  const rmf_task::Task::Booking& booking)
-{
-  booking_json["id"] = booking.id();
-  booking_json["unix_millis_earliest_start_time"] =
-    to_millis(booking.earliest_start_time().time_since_epoch()).count();
-  // TODO(MXG): Add priority and labels
-}
-
-//==============================================================================
-void copy_assignment(
-  nlohmann::json& assigned_to_json,
-  const agv::RobotContext& context)
-{
-  assigned_to_json["group"] = context.group();
-  assigned_to_json["name"] = context.name();
-}
-
-//==============================================================================
-nlohmann::json make_simple_success_response()
-{
-  nlohmann::json response;
-  response["success"] = true;
-  return response;
-}
-
-} // anonymous namespace
-
 //==============================================================================
 void TaskManager::_publish_task_state()
 {
@@ -969,70 +1163,6 @@ void TaskManager::_publish_task_state()
   _active_task.publish_task_state(*this);
 }
 
-//==============================================================================
-void TaskManager::ActiveTask::publish_task_state(TaskManager& mgr)
-{
-  auto task_state_update = mgr._task_state_update_json;
-
-  const auto& booking = *_task->tag()->booking();
-  copy_booking_data(_state_msg["booking"], booking);
-
-  const auto& header = _task->tag()->header();
-  _state_msg["category"] = header.category();
-  _state_msg["detail"] = header.detail();
-  // TODO(MXG): Add unix_millis_start_time and unix_millis_finish_time
-  _state_msg["original_estimate_millis"] =
-    std::max(0l, to_millis(header.original_duration_estimate()).count());
-  _state_msg["estimate_millis"] =
-    std::max(0l, to_millis(_task->estimate_remaining_time()).count());
-  copy_assignment(_state_msg["assigned_to"], *mgr._context);
-  _state_msg["status"] =
-    status_to_string(_task->status_overview());
-
-  auto& phases = _state_msg["phases"];
-
-  nlohmann::json task_logs;
-  auto& phase_logs = task_logs["phases"];
-
-  std::vector<uint64_t> completed_ids;
-  completed_ids.reserve(_task->completed_phases().size());
-  for (const auto& completed : _task->completed_phases())
-  {
-    const auto& snapshot = completed->snapshot();
-    auto& phase = copy_phase_data(
-      phases, *snapshot, mgr._log_reader, phase_logs);
-
-    phase["unix_millis_start_time"] =
-      completed->start_time().time_since_epoch().count();
-
-    phase["unix_millis_finish_time"] =
-      completed->finish_time().time_since_epoch().count();
-
-    completed_ids.push_back(snapshot->tag()->id());
-  }
-  _state_msg["completed"] = std::move(completed_ids);
-
-  const auto active_phase = _task->active_phase();
-  copy_phase_data(phases, *active_phase, mgr._log_reader, phase_logs);
-
-  _state_msg["active"] = active_phase->tag()->id();
-
-  std::vector<uint64_t> pending_ids;
-  pending_ids.reserve(_task->pending_phases().size());
-  for (const auto& pending : _task->pending_phases())
-  {
-    copy_phase_data(phases, pending);
-    pending_ids.push_back(pending.tag()->id());
-  }
-  _state_msg["pending"] = std::move(pending_ids);
-
-  task_state_update["data"] = _state_msg;
-
-  static const auto validator =
-    mgr._make_validator(rmf_api_msgs::schemas::task_state_update);
-  mgr._validate_and_publish_websocket(task_state_update, validator);
-}
-
 //==============================================================================
 void TaskManager::_publish_task_queue()
 {
@@ -1396,7 +1526,8 @@ void TaskManager::_handle_interrupt_request(
   {
     _task_state_update_available = true;
     return _send_token_success_response(
-      _active_task.add_interruption(get_labels(request_json)),
+      _active_task.add_interruption(
+        get_labels(request_json), _context->now(), [](){}),
       request_id);
   }
 
@@ -1421,7 +1552,8 @@ void TaskManager::_handle_resume_request(
     _task_state_update_available = true;
     auto unknown_tokens = _active_task.remove_interruption(
       request_json["for_tokens"].get<std::vector<std::string>>(),
-      get_labels(request_json));
+      get_labels(request_json),
+      _context->now());
 
     if (unknown_tokens.empty())
       return _send_simple_success_response(request_id);
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
index 3dfaa985f..2b7430f82 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
@@ -108,11 +108,7 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
   class ActiveTask
   {
   public:
-    ActiveTask(rmf_task::Task::ActivePtr task = nullptr)
-    : _task(std::move(task))
-    {
-      // Do nothing
-    }
+    ActiveTask(rmf_task::Task::ActivePtr task = nullptr);
 
     const std::string& id() const;
 
@@ -124,12 +120,16 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
     }
 
     /// Adds an interruption
-    std::string add_interruption(std::vector<std::string> labels);
+    std::string add_interruption(
+      std::vector<std::string> labels,
+      rmf_traffic::Time time,
+      std::function<void()> task_is_interrupted);
 
     // Any unknown tokens that were included will be returned
     std::vector<std::string> remove_interruption(
       std::vector<std::string> for_tokens,
-      std::vector<std::string> labels);
+      std::vector<std::string> labels,
+      rmf_traffic::Time time);
 
     void cancel(std::vector<std::string> labels);
 
@@ -144,14 +144,27 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
       std::vector<std::string> labels);
 
   private:
-    std::unordered_map<std::string, nlohmann::json> active_interruptions;
-    std::unordered_map<std::string, nlohmann::json> resolved_interruptions;
-    std::optional<rmf_task::Task::Active::Resume> resume_task;
-    std::optional<nlohmann::json> cancellation;
-    std::optional<nlohmann::json> killed;
-
     rmf_task::Task::ActivePtr _task;
     nlohmann::json _state_msg;
+
+    std::unordered_map<std::string, nlohmann::json> _active_interruptions;
+    std::unordered_map<std::string, nlohmann::json> _removed_interruptions;
+    std::optional<rmf_task::Task::Active::Resume> _resume_task;
+
+    struct InterruptionHandler
+      : public std::enable_shared_from_this<InterruptionHandler>
+    {
+      std::mutex mutex;
+      std::vector<std::function<void()>> interruption_listeners;
+      bool is_interrupted = false;
+    };
+
+    std::shared_ptr<InterruptionHandler> _interruption_handler;
+
+    std::optional<nlohmann::json> _cancellation;
+    std::optional<nlohmann::json> _killed;
+
+    uint64_t _next_token = 0;
   };
 
   friend class ActiveTask;
@@ -214,8 +227,6 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
 
   rmf_task::Log::Reader _log_reader;
 
-  uint64_t _next_token = 0;
-
   // Map task_id to task_log.json for all tasks managed by this TaskManager
   std::unordered_map<std::string, nlohmann::json> _task_logs = {};
 

From 15f548b6a28d28372751ddfce9cbfe127d2ad9ca Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Fri, 24 Dec 2021 21:05:27 +0800
Subject: [PATCH 28/79] Finished implementation of all task manipulation
 requests -- needs testing

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 .../src/rmf_fleet_adapter/TaskManager.cpp     | 139 +++++++++++++++++-
 .../src/rmf_fleet_adapter/TaskManager.hpp     |  26 +++-
 2 files changed, 153 insertions(+), 12 deletions(-)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
index e7f5961f7..ac2b7f9f6 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
@@ -470,9 +470,33 @@ void TaskManager::ActiveTask::publish_task_state(TaskManager& mgr)
   }
   _state_msg["pending"] = std::move(pending_ids);
 
-  task_state_update["data"] = _state_msg;
+  if (!_active_interruptions.empty() || !_removed_interruptions.empty())
+  {
+    auto& interruptions_json = _state_msg["interruptions"];
+    for (const auto& i : {&_active_interruptions, &_removed_interruptions})
+    {
+      for (const auto& [token, msg] : *i)
+        interruptions_json[token] = msg;
+    }
+  }
 
-  // TODO(MXG): Add interruption, skip, cancel, and kill information
+  if (_cancellation.has_value())
+    _state_msg["cancellation"] = *_cancellation;
+
+  if (_killed.has_value())
+    _state_msg["killed"] = *_killed;
+
+  for (const auto& [phase, skip_info] : _skip_info_map)
+  {
+    auto& skip_requests = phases[phase]["skip_requests"];
+    for (const auto& s : {&skip_info.active_skips, &skip_info.removed_skips})
+    {
+      for (const auto& [token, msg] : *s)
+        skip_requests[token] = msg;
+    }
+  }
+
+  task_state_update["data"] = _state_msg;
 
   static const auto validator =
     mgr._make_validator(rmf_api_msgs::schemas::task_state_update);
@@ -576,10 +600,109 @@ std::vector<std::string> TaskManager::ActiveTask::remove_interruption(
 }
 
 //==============================================================================
-void TaskManager::ActiveTask::cancel(std::vector<std::string> labels)
+void TaskManager::ActiveTask::cancel(
+  std::vector<std::string> labels,
+  rmf_traffic::Time time)
 {
+  if (_cancellation.has_value())
+    return;
+
   nlohmann::json cancellation;
+  cancellation["unix_millis_request_time"] =
+    to_millis(time.time_since_epoch()).count();
+
+  cancellation["labels"] = std::move(labels);
+
+  _cancellation = std::move(cancellation);
+  _task->cancel();
+}
+
+//==============================================================================
+void TaskManager::ActiveTask::kill(
+  std::vector<std::string> labels,
+  rmf_traffic::Time time)
+{
+  if (_killed.has_value())
+    return;
+
+  nlohmann::json killed;
+  killed["unix_millis_request_time"] =
+    to_millis(time.time_since_epoch()).count();
+
+  killed["labels"] = std::move(labels);
+
+  _killed = std::move(killed);
+  _task->kill();
+}
+
+//==============================================================================
+void TaskManager::ActiveTask::rewind(uint64_t phase_id)
+{
+  _task->rewind(phase_id);
+}
+
+//==============================================================================
+std::string TaskManager::ActiveTask::skip(
+  uint64_t phase_id,
+  std::vector<std::string> labels,
+  rmf_traffic::Time time)
+{
+  std::string token = std::to_string(_next_token++);
+
+  auto& skip_info = _skip_info_map[phase_id];
+
+  nlohmann::json skip_json;
+  skip_json["unix_millis_request_time"] =
+    to_millis(time.time_since_epoch()).count();
+
+  skip_json["labels"] = std::move(labels);
+
+  skip_info.active_skips[token] = std::move(skip_json);
 
+  _task->skip(phase_id, true);
+
+  return token;
+}
+
+//==============================================================================
+std::vector<std::string> TaskManager::ActiveTask::remove_skips(
+  const std::vector<std::string>& for_tokens,
+  std::vector<std::string> labels,
+  rmf_traffic::Time time)
+{
+  nlohmann::json undo_json;
+  undo_json["unix_millis_request_time"] =
+    to_millis(time.time_since_epoch()).count();
+
+  undo_json["labels"] = std::move(labels);
+
+  std::vector<std::string> missing_tokens;
+  for (const auto& token : for_tokens)
+  {
+    bool found_token = false;
+    for (auto& [phase, skip_info] : _skip_info_map)
+    {
+      const auto it = skip_info.active_skips.find(token);
+      if (it == skip_info.active_skips.end())
+        continue;
+
+      auto skip_json = it->second;
+      skip_json["undo"] = undo_json;
+      skip_info.removed_skips[token] = std::move(skip_json);
+      skip_info.active_skips.erase(it);
+
+      if (skip_info.active_skips.empty())
+        _task->skip(phase, false);
+
+      found_token = true;
+      break;
+    }
+
+    if (!found_token)
+      missing_tokens.push_back(token);
+  }
+
+  return missing_tokens;
 }
 
 //==============================================================================
@@ -1478,7 +1601,7 @@ void TaskManager::_handle_cancel_request(
 
   if (_active_task && _active_task.id() == task_id)
   {
-    _active_task.cancel(get_labels(request_json));
+    _active_task.cancel(get_labels(request_json), _context->now());
     _task_state_update_available = true;
     return _send_simple_success_response(request_id);
   }
@@ -1502,7 +1625,7 @@ void TaskManager::_handle_kill_request(
   if (_active_task && _active_task.id() == task_id)
   {
     _task_state_update_available = true;
-    _active_task.kill(get_labels(request_json));
+    _active_task.kill(get_labels(request_json), _context->now());
     return _send_simple_success_response(request_id);
   }
 
@@ -1616,7 +1739,8 @@ void TaskManager::_handle_skip_phase_request(
     return _send_token_success_response(
       _active_task.skip(
         request_json["phase_id"].get<uint64_t>(),
-        get_labels(request_json)),
+        get_labels(request_json),
+        _context->now()),
       request_id);
   }
 
@@ -1641,7 +1765,8 @@ void TaskManager::_handle_undo_skip_phase_request(
     _task_state_update_available = true;
     auto unknown_tokens = _active_task.remove_skips(
       request_json["for_tokens"].get<std::vector<std::string>>(),
-      get_labels(request_json));
+      get_labels(request_json),
+      _context->now());
 
     if (unknown_tokens.empty())
       return _send_simple_success_response(request_id);
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
index 2b7430f82..112986c06 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
@@ -131,17 +131,25 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
       std::vector<std::string> labels,
       rmf_traffic::Time time);
 
-    void cancel(std::vector<std::string> labels);
+    void cancel(
+      std::vector<std::string> labels,
+      rmf_traffic::Time time);
 
-    void kill(std::vector<std::string> labels);
+    void kill(
+      std::vector<std::string> labels,
+      rmf_traffic::Time time);
 
     void rewind(uint64_t phase_id);
 
-    std::string skip(uint64_t phase_id, std::vector<std::string> labels);
+    std::string skip(
+      uint64_t phase_id,
+      std::vector<std::string> labels,
+      rmf_traffic::Time time);
 
     std::vector<std::string> remove_skips(
-      std::vector<std::string> for_tokens,
-      std::vector<std::string> labels);
+      const std::vector<std::string>& for_tokens,
+      std::vector<std::string> labels,
+      rmf_traffic::Time time);
 
   private:
     rmf_task::Task::ActivePtr _task;
@@ -164,6 +172,14 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
     std::optional<nlohmann::json> _cancellation;
     std::optional<nlohmann::json> _killed;
 
+    struct SkipInfo
+    {
+      std::unordered_map<std::string, nlohmann::json> active_skips;
+      std::unordered_map<std::string, nlohmann::json> removed_skips;
+    };
+
+    std::unordered_map<uint64_t, SkipInfo> _skip_info_map;
+
     uint64_t _next_token = 0;
   };
 

From 8c51312ea4d50d85d75c85b7b94c1f816d35184a Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Wed, 29 Dec 2021 15:28:54 +0800
Subject: [PATCH 29/79] Beginning API for json deserialization

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 .../agv/FleetUpdateHandle.hpp                 | 37 +++++++++++++++++++
 1 file changed, 37 insertions(+)

diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp
index c13214fad..7b235f16c 100644
--- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp
@@ -30,6 +30,11 @@
 
 #include <rmf_task/RequestFactory.hpp>
 
+#include <nlohmann/json.hpp>
+#include <rmf_task/Activator.hpp>
+#include <rmf_task_sequence/Phase.hpp>
+#include <rmf_task_sequence/Event.hpp>
+
 namespace rmf_fleet_adapter {
 namespace agv {
 
@@ -70,6 +75,38 @@ class FleetUpdateHandle : public std::enable_shared_from_this<FleetUpdateHandle>
     rmf_traffic::agv::Plan::StartSet start,
     std::function<void(std::shared_ptr<RobotUpdateHandle> handle)> handle_cb);
 
+  using EventDescriptionDeserializer =
+    std::function<
+      rmf_task_sequence::Event::ConstDescriptionPtr(const nlohmann::json&)
+    >;
+
+  /// Add a type of activity for this fleet to support. Once added this activity
+  /// can be incorporated into event bundles for composed tasks.
+  ///
+  /// \param[in] category
+  ///   The category of event that will make use of the resources provided to
+  ///   this function. If this function is called multiple times with the same
+  ///   category, later calls will overwrite earlier calls.
+  ///
+  /// \param[in] schema
+  ///   The schema that will be applied to validate incoming event JSON messages
+  ///   of this category.
+  ///
+  /// \param[in] description_deserializer
+  ///   A function which takes in a JSON message and gives back an event
+  ///   description instance.
+  ///
+  /// \param[in] event_initializer
+  ///   The event initializer that will be used for initializing the event
+  ///   description. You should add an initializer for your event to this. Your
+  ///   own event may also use this initializer to initialize its own event
+  ///   dependencies.
+  void add_activity_type(
+    std::string category,
+    nlohmann::json schema,
+    EventDescriptionDeserializer description_deserializer,
+    const rmf_task_sequence::Event::InitializerPtr& event_initializer);
+
   /// Specify a set of lanes that should be closed.
   void close_lanes(std::vector<std::size_t> lane_indices);
 

From c34055822c299eec9571850cbf8027b59a5be2c6 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Wed, 29 Dec 2021 15:40:37 +0800
Subject: [PATCH 30/79] Remove redundant comparison function

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 .../schedule/ParticipantRegistry.cpp          |  8 -------
 .../test/unit/test_ParticipantRegistry.cpp    | 22 -------------------
 2 files changed, 30 deletions(-)

diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/ParticipantRegistry.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/ParticipantRegistry.cpp
index 71ebc1d8a..56bbd1453 100644
--- a/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/ParticipantRegistry.cpp
+++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/ParticipantRegistry.cpp
@@ -23,14 +23,6 @@
 namespace rmf_traffic_ros2 {
 namespace schedule {
 
-//==============================================================================
-bool operator!=(
-  const rmf_traffic::schedule::ParticipantDescription& p1,
-  const rmf_traffic::schedule::ParticipantDescription& p2)
-{
-  return rmf_traffic_ros2::convert(p1) != rmf_traffic_ros2::convert(p2);
-}
-
 //==============================================================================
 struct UniqueId
 {
diff --git a/rmf_traffic_ros2/test/unit/test_ParticipantRegistry.cpp b/rmf_traffic_ros2/test/unit/test_ParticipantRegistry.cpp
index 0ddeedd48..3d7ac7996 100644
--- a/rmf_traffic_ros2/test/unit/test_ParticipantRegistry.cpp
+++ b/rmf_traffic_ros2/test/unit/test_ParticipantRegistry.cpp
@@ -49,28 +49,6 @@ SCENARIO("Test idempotency of shape type")
   REQUIRE_THROWS(shape_type(node["type"]));
 }
 
-namespace rmf_traffic {
-
-bool operator==(const Profile p1, const Profile p2)
-{
-  return rmf_traffic_ros2::convert(p1) == rmf_traffic_ros2::convert(p2);
-}
-
-namespace schedule {
-
-bool operator==(
-  const ParticipantDescription desc1,
-  const ParticipantDescription desc2)
-{
-  return desc1.name() == desc2.name()
-    && desc1.owner() == desc2.owner()
-    && desc1.responsiveness() ==  desc2.responsiveness()
-    && desc1.profile() == desc2.profile();
-}
-
-}
-}
-
 namespace rmf_traffic_ros2 {
 namespace schedule {
 

From 19b93e1ddee08d390ba447324ead44ff97e891e7 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Thu, 30 Dec 2021 16:45:00 +0800
Subject: [PATCH 31/79] Adding the ability to deserialize json task requests

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 .../agv/FleetUpdateHandle.hpp                 | 82 +++++++++++++++++--
 .../src/rmf_fleet_adapter/DeserializeJSON.hpp | 55 +++++++++++++
 .../agv/internal_FleetUpdateHandle.hpp        | 11 +++
 3 files changed, 141 insertions(+), 7 deletions(-)
 create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/DeserializeJSON.hpp

diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp
index 7b235f16c..0ddfbe7ca 100644
--- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp
@@ -75,13 +75,80 @@ class FleetUpdateHandle : public std::enable_shared_from_this<FleetUpdateHandle>
     rmf_traffic::agv::Plan::StartSet start,
     std::function<void(std::shared_ptr<RobotUpdateHandle> handle)> handle_cb);
 
+  template<typename T>
+  struct DeserializedDescription
+  {
+    std::shared_ptr<const T> description;
+    std::vector<std::string> errors;
+  };
+
+  using DeserializedTask = DeserializedDescription<rmf_task::Task::Description>;
+
+  using TaskDescriptionDeserializer =
+    std::function<DeserializedTask(const nlohmann::json&)>;
+
+  /// Add a type of task for this fleet to support.
+  ///
+  /// \param[in] category
+  ///   The category of the task that will make use of the resources provided to
+  ///   this function. If this function is called multiple times with the same
+  ///   category, later calls will overwrite earlier calls.
+  ///
+  /// \param[in] schema
+  ///   The schema that will be applied to validate incoming task request JSON
+  ///   messages of this category.
+  ///
+  /// \param[in] description_deserializer
+  ///   A function which takes in a JSON message and gives back a task
+  ///   description instance.
+  ///
+  /// \param[in] task_activator
+  ///   The task activator that will be used for activating the task
+  ///   description. You should add an activator for your task to this.
+  void add_task_type(
+    const std::string& category,
+    nlohmann::json schema,
+    TaskDescriptionDeserializer description_deserializer,
+    rmf_task::Activator& task_activator);
+
+  using DeserializedPhase =
+    DeserializedDescription<rmf_task_sequence::Phase::Description>;
+
+  using PhaseDescriptionDeserializer =
+    std::function<DeserializedPhase(const nlohmann::json&)>;
+
+  /// Add a type of phase for this fleet to support.
+  ///
+  /// \param[in] category
+  ///   The category of the phase that will make use of the resources provided
+  ///   to this function. If this function is called multiple times with the
+  ///   same category, later calls will overwrite earlier calls.
+  ///
+  /// \param[in] schema
+  ///   The schema that will be applied to validate incoming phase JSON
+  ///   messages of this category.
+  ///
+  /// \param[in] description_deserializer
+  ///   A function which takes in a JSON message and gives back a phase
+  ///   description instance.
+  ///
+  /// \param[in] phase_activator
+  ///   The phase activator that will be used for activating the phase
+  ///   description. You should add an activator for phase to this.
+  void add_phase_type(
+    const std::string& category,
+    nlohmann::json schema,
+    PhaseDescriptionDeserializer description_deserializer,
+    rmf_task_sequence::Phase::Activator& phase_activator);
+
+  using DeserializedEvent =
+    DeserializedDescription<rmf_task_sequence::Event::Description>;
+
   using EventDescriptionDeserializer =
-    std::function<
-      rmf_task_sequence::Event::ConstDescriptionPtr(const nlohmann::json&)
-    >;
+    std::function<DeserializedEvent(const nlohmann::json&)>;
 
-  /// Add a type of activity for this fleet to support. Once added this activity
-  /// can be incorporated into event bundles for composed tasks.
+  /// Add a type of activity for this fleet to support. This activity can be
+  /// incorporated into event bundles for composed tasks.
   ///
   /// \param[in] category
   ///   The category of event that will make use of the resources provided to
@@ -100,9 +167,10 @@ class FleetUpdateHandle : public std::enable_shared_from_this<FleetUpdateHandle>
   ///   The event initializer that will be used for initializing the event
   ///   description. You should add an initializer for your event to this. Your
   ///   own event may also use this initializer to initialize its own event
-  ///   dependencies.
+  ///   dependencies, but it must hold a std::weak_ptr to avoid a circular
+  ///   reference.
   void add_activity_type(
-    std::string category,
+    const std::string& category,
     nlohmann::json schema,
     EventDescriptionDeserializer description_deserializer,
     const rmf_task_sequence::Event::InitializerPtr& event_initializer);
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/DeserializeJSON.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/DeserializeJSON.hpp
new file mode 100644
index 000000000..acd846144
--- /dev/null
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/DeserializeJSON.hpp
@@ -0,0 +1,55 @@
+/*
+ * 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 SRC__RMF_FLEET_ADAPTER__DESERIALIZEJSON_HPP
+#define SRC__RMF_FLEET_ADAPTER__DESERIALIZEJSON_HPP
+
+#include <unordered_map>
+#include <nlohmann/json.hpp>
+#include <nlohmann/json-schema.hpp>
+
+namespace rmf_fleet_adapter {
+
+//==============================================================================
+template<typename Deserializer>
+class DeserializeJSON
+{
+public:
+
+  struct Handlers
+  {
+    nlohmann::json_schema::json_validator validator;
+    Deserializer deserializer;
+  };
+
+  void add(
+    const std::string& category,
+    nlohmann::json_schema::json_validator validator,
+    Deserializer deserializer)
+  {
+    handlers.insert_or_assign(
+      category,
+      Handlers{std::move(validator), std::move(deserializer)});
+  }
+
+  std::unordered_map<std::string, Handlers> handlers;
+
+};
+
+} // namespace rmf_fleet_adapter
+
+#endif // SRC__RMF_FLEET_ADAPTER__DESERIALIZEJSON_HPP
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
index 6d7bad26c..bac21e80f 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
@@ -43,6 +43,7 @@
 #include "RobotContext.hpp"
 #include "../TaskManager.hpp"
 #include "../BroadcastClient.hpp"
+#include "../DeserializeJSON.hpp"
 
 #include <rmf_traffic/schedule/Snapshot.hpp>
 #include <rmf_traffic/agv/Interpolate.hpp>
@@ -68,6 +69,7 @@
 namespace rmf_fleet_adapter {
 namespace agv {
 
+//==============================================================================
 struct TaskActivation
 {
   rmf_task::ActivatorPtr task;
@@ -75,6 +77,14 @@ struct TaskActivation
   rmf_task_sequence::Event::InitializerPtr event;
 };
 
+//==============================================================================
+struct TaskDeserialization
+{
+  DeserializeJSON<FleetUpdateHandle::DeserializedTask> task;
+  DeserializeJSON<FleetUpdateHandle::DeserializedPhase> phase;
+  DeserializeJSON<FleetUpdateHandle::DeserializedEvent> event;
+};
+
 //==============================================================================
 /// This abstract interface class allows us to use the same implementation of
 /// FleetUpdateHandle whether we are running it in a distributed system or in a
@@ -160,6 +170,7 @@ class FleetUpdateHandle::Implementation
   std::optional<std::string> server_uri;
 
   TaskActivation activation = TaskActivation();
+  TaskDeserialization deserialization = TaskDeserialization();
 
   // LegacyTask planner params
   std::shared_ptr<rmf_task::CostCalculator> cost_calculator =

From 5ff66e38a03abbfd2b03f7b14819b26791b20943 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Fri, 31 Dec 2021 14:01:59 +0800
Subject: [PATCH 32/79] Implement deserialization of JSON for task descriptions

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 rmf_fleet_adapter/CMakeLists.txt              |   8 +
 .../agv/FleetUpdateHandle.hpp                 |  33 ++--
 .../schemas/event_description_Clean.json      |  12 ++
 .../schemas/event_description_DropOff.json    |   6 +
 .../schemas/event_description_GoToPlace.json  |  21 +++
 .../event_description_PayloadTransfer.json    |  47 +++++
 .../schemas/event_description_PickUp.json     |   6 +
 .../schemas/task_description_Delivery.json    |  11 ++
 .../src/rmf_fleet_adapter/DeserializeJSON.hpp |  22 ++-
 .../agv/FleetUpdateHandle.cpp                 |  11 +-
 .../agv/internal_FleetUpdateHandle.hpp        |  10 +-
 .../src/rmf_fleet_adapter/tasks/Delivery.cpp  | 160 +++++++++++++++++-
 .../src/rmf_fleet_adapter/tasks/Delivery.hpp  |   6 +
 13 files changed, 331 insertions(+), 22 deletions(-)
 create mode 100644 rmf_fleet_adapter/schemas/event_description_Clean.json
 create mode 100644 rmf_fleet_adapter/schemas/event_description_DropOff.json
 create mode 100644 rmf_fleet_adapter/schemas/event_description_GoToPlace.json
 create mode 100644 rmf_fleet_adapter/schemas/event_description_PayloadTransfer.json
 create mode 100644 rmf_fleet_adapter/schemas/event_description_PickUp.json
 create mode 100644 rmf_fleet_adapter/schemas/task_description_Delivery.json

diff --git a/rmf_fleet_adapter/CMakeLists.txt b/rmf_fleet_adapter/CMakeLists.txt
index 96a0dd67f..13a2483fd 100644
--- a/rmf_fleet_adapter/CMakeLists.txt
+++ b/rmf_fleet_adapter/CMakeLists.txt
@@ -95,6 +95,7 @@ target_link_libraries(rmf_fleet_adapter
 target_include_directories(rmf_fleet_adapter
   PUBLIC
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<BUILD_INTERFACE:${CMAKE_BINARY_DIR}/rmf_api_generate_schema_headers/include> # for auto-generated schema headers
     $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>
     ${rmf_traffic_ros2_INCLUDE_DIRS}
     ${rmf_fleet_msgs_INCLUDE_DIRS}
@@ -472,6 +473,13 @@ target_include_directories(close_lanes
 
 # -----------------------------------------------------------------------------
 
+rmf_api_generate_schema_headers(
+  PACKAGE rmf_fleet_adapter
+  SCHEMAS_DIR ${CMAKE_CURRENT_LIST_DIR}/schemas
+)
+
+# -----------------------------------------------------------------------------
+
 ament_export_targets(rmf_fleet_adapter HAS_LIBRARY_TARGET)
 ament_export_dependencies(
   rmf_task
diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp
index 0ddfbe7ca..91528687b 100644
--- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp
@@ -103,13 +103,14 @@ class FleetUpdateHandle : public std::enable_shared_from_this<FleetUpdateHandle>
   ///   description instance.
   ///
   /// \param[in] task_activator
-  ///   The task activator that will be used for activating the task
-  ///   description. You should add an activator for your task to this.
+  ///   This lambda will receive the task activator that will be used for
+  ///   activating the task description. The lambda should add your task to this
+  ///   activator. The lambda will only be called one time.
   void add_task_type(
     const std::string& category,
     nlohmann::json schema,
     TaskDescriptionDeserializer description_deserializer,
-    rmf_task::Activator& task_activator);
+    std::function<void(rmf_task::Activator&)> task_activator);
 
   using DeserializedPhase =
     DeserializedDescription<rmf_task_sequence::Phase::Description>;
@@ -133,13 +134,19 @@ class FleetUpdateHandle : public std::enable_shared_from_this<FleetUpdateHandle>
   ///   description instance.
   ///
   /// \param[in] phase_activator
-  ///   The phase activator that will be used for activating the phase
-  ///   description. You should add an activator for phase to this.
+  ///   This lambda will receive the phase activator that will be used for
+  ///   activating the phase description. The lambda should add your phase to
+  ///   this activator. The lambda will only be called one time.
   void add_phase_type(
     const std::string& category,
     nlohmann::json schema,
     PhaseDescriptionDeserializer description_deserializer,
-    rmf_task_sequence::Phase::Activator& phase_activator);
+    std::function<void(rmf_task_sequence::Phase::Activator&)> phase_activator);
+
+  /// Get the builtin phase activator that will be used by composed tasks to
+  /// activate phases. A custom task type may make use of this to activate
+  /// standard and custom phases.
+  rmf_task_sequence::Phase::ActivatorPtr builtin_phase_activator();
 
   using DeserializedEvent =
     DeserializedDescription<rmf_task_sequence::Event::Description>;
@@ -164,16 +171,18 @@ class FleetUpdateHandle : public std::enable_shared_from_this<FleetUpdateHandle>
   ///   description instance.
   ///
   /// \param[in] event_initializer
-  ///   The event initializer that will be used for initializing the event
-  ///   description. You should add an initializer for your event to this. Your
-  ///   own event may also use this initializer to initialize its own event
-  ///   dependencies, but it must hold a std::weak_ptr to avoid a circular
-  ///   reference.
+  ///   This lambda will receive the event initializer that will be used for
+  ///   initializing the event description. The lambda should add your event to
+  ///   this initializer. Your own event may also use this initializer to
+  ///   initialize its own event dependencies, but it must hold a std::weak_ptr
+  ///   to avoid a circular reference.
   void add_activity_type(
     const std::string& category,
     nlohmann::json schema,
     EventDescriptionDeserializer description_deserializer,
-    const rmf_task_sequence::Event::InitializerPtr& event_initializer);
+    std::function<void(const rmf_task_sequence::Event::InitializerPtr&)> event_initializer);
+
+  rmf_task_sequence::Event::InitializerPtr builtin_event_initializer();
 
   /// Specify a set of lanes that should be closed.
   void close_lanes(std::vector<std::size_t> lane_indices);
diff --git a/rmf_fleet_adapter/schemas/event_description_Clean.json b/rmf_fleet_adapter/schemas/event_description_Clean.json
new file mode 100644
index 000000000..bb32232d1
--- /dev/null
+++ b/rmf_fleet_adapter/schemas/event_description_Clean.json
@@ -0,0 +1,12 @@
+{
+  "$schema": "https://json-schema.org/draft/2020-12/schema",
+  "$id": "https://raw.githubusercontent.com/open-rmf/rmf_ros2/main/rmf_fleet_adapter/schemas/event_description_Clean.json",
+  "title": "Clean Event",
+  "description": "Clean a zone",
+  "type": "object",
+  "properties": {
+    "zone": { "type": "string" },
+    "type": { "type": "string" }
+  },
+  "required": ["zone"]
+}
diff --git a/rmf_fleet_adapter/schemas/event_description_DropOff.json b/rmf_fleet_adapter/schemas/event_description_DropOff.json
new file mode 100644
index 000000000..02685aa56
--- /dev/null
+++ b/rmf_fleet_adapter/schemas/event_description_DropOff.json
@@ -0,0 +1,6 @@
+{
+  "$schema": "https://json-schema.org/draft/2020-12/schema",
+  "$id": "https://raw.githubusercontent.com/open-rmf/rmf_ros2/main/rmf_fleet_adapter/schemas/event_description_DropOff.json",
+  "title": "Drop Off Event Description",
+  "$ref": "event_description_PayloadTransfer.json"
+}
diff --git a/rmf_fleet_adapter/schemas/event_description_GoToPlace.json b/rmf_fleet_adapter/schemas/event_description_GoToPlace.json
new file mode 100644
index 000000000..de0ab54d9
--- /dev/null
+++ b/rmf_fleet_adapter/schemas/event_description_GoToPlace.json
@@ -0,0 +1,21 @@
+{
+  "$schema": "https://json-schema.org/draft/2020-12/schema",
+  "$id": "https://raw.githubusercontent.com/open-rmf/rmf_ros2/main/rmf_fleet_adapter/schemas/event_description_GoToPlace.json",
+  "title": "Go To Place Event",
+  "description": "Have a robot go to a place",
+  "type": "object",
+  "properties": {
+    "place": {
+      "oneOf": [
+        {
+          "type": "string"
+        },
+        {
+          "type": "integer",
+          "minimum": 0
+        }
+      ]
+    }
+  },
+  "required": ["place"]
+}
diff --git a/rmf_fleet_adapter/schemas/event_description_PayloadTransfer.json b/rmf_fleet_adapter/schemas/event_description_PayloadTransfer.json
new file mode 100644
index 000000000..bb1ba24fa
--- /dev/null
+++ b/rmf_fleet_adapter/schemas/event_description_PayloadTransfer.json
@@ -0,0 +1,47 @@
+{
+  "$schema": "https://json-schema.org/draft/2020-12/schema",
+  "$id": "https://raw.githubusercontent.com/open-rmf/rmf_ros2/main/rmf_fleet_adapter/schemas/event_description_PayloadTransfer.json",
+  "title": "Item Transfer Event Description",
+  "description": "Description for a drop off or a pick up event category",
+  "type": "object",
+  "properties": {
+    "place": {
+      "oneOf": [
+        {
+          "type": "string"
+        },
+        {
+          "type": "integer",
+          "minimum": 0
+        }
+      ]
+    },
+    "handler": { "type": "string" },
+    "payload" : {
+      "oneOf": [
+        {
+          "$ref": "#/$defs/payload_component"
+        },
+        {
+          "type": "array",
+          "items": { "$ref": "#/$defs/payload_component" }
+        }
+      ]
+    }
+  },
+  "required": ["place", "payload"],
+  "$defs": {
+    "payload_component": {
+      "type": "object",
+      "properties": {
+        "sku": { "type": "string" },
+        "quantity": {
+          "type": "integer",
+          "minimum": 0
+        },
+        "compartment": { "type": "string" }
+      },
+      "required": ["sku", "quantity"]
+    }
+  }
+}
diff --git a/rmf_fleet_adapter/schemas/event_description_PickUp.json b/rmf_fleet_adapter/schemas/event_description_PickUp.json
new file mode 100644
index 000000000..654736429
--- /dev/null
+++ b/rmf_fleet_adapter/schemas/event_description_PickUp.json
@@ -0,0 +1,6 @@
+{
+  "$schema": "https://json-schema.org/draft/2020-12/schema",
+  "$id": "https://raw.githubusercontent.com/open-rmf/rmf_ros2/main/rmf_fleet_adapter/schemas/event_description_PickUp.json",
+  "title": "Pick Up Event Description",
+  "$ref": "event_description_PayloadTransfer.json"
+}
diff --git a/rmf_fleet_adapter/schemas/task_description_Delivery.json b/rmf_fleet_adapter/schemas/task_description_Delivery.json
new file mode 100644
index 000000000..3fc00a404
--- /dev/null
+++ b/rmf_fleet_adapter/schemas/task_description_Delivery.json
@@ -0,0 +1,11 @@
+{
+  "$schema": "https://json-schema.org/draft/2020-12/schema",
+  "$id": "https://raw.githubusercontent.com/open-rmf/rmf_ros2/main/rmf_fleet_adapter/schemas/task_description_Delivery.json",
+  "title": "Delivery Task Description",
+  "description": "Description for a simple delivery task category",
+  "properties": {
+    "pickup": { "$ref": "event_description_PickUp.json" },
+    "dropoff": { "$ref": "event_description_DropOff.json" }
+  },
+  "required": ["pickup", "dropoff"]
+}
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/DeserializeJSON.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/DeserializeJSON.hpp
index acd846144..5f43037fd 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/DeserializeJSON.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/DeserializeJSON.hpp
@@ -25,20 +25,36 @@
 namespace rmf_fleet_adapter {
 
 //==============================================================================
-template<typename Deserializer>
+template<typename Deserialized>
 class DeserializeJSON
 {
 public:
 
+  using Deserializer = std::function<Deserialized(const nlohmann::json&)>;
+  using Validator = nlohmann::json_schema::json_validator;
+
   struct Handlers
   {
-    nlohmann::json_schema::json_validator validator;
+    std::shared_ptr<const Validator> validator;
     Deserializer deserializer;
   };
 
   void add(
     const std::string& category,
-    nlohmann::json_schema::json_validator validator,
+    Validator validator,
+    Deserializer deserializer)
+  {
+    handlers.insert_or_assign(
+      category,
+      Handlers{
+        std::make_shared<Validator>(std::move(validator)),
+        std::move(deserializer)
+      });
+  }
+
+  void add(
+    const std::string& category,
+    std::shared_ptr<const Validator> validator,
     Deserializer deserializer)
   {
     handlers.insert_or_assign(
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
index 9487cbaed..81c090753 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
@@ -94,9 +94,9 @@ void FleetUpdateHandle::Implementation::dock_summary_cb(
   {
     if (dock.fleet_name == name)
     {
-      dock_param_map.clear();
+      dock_param_map->clear();
       for (const auto& param : dock.params)
-        dock_param_map.insert({param.start, param});
+        dock_param_map->insert({param.start, param});
       break;
     }
   }
@@ -211,8 +211,8 @@ void FleetUpdateHandle::Implementation::bid_notice_cb(
     }
 
     // Get dock parameters
-    const auto clean_param_it = dock_param_map.find(start_wp_name);
-    if (clean_param_it == dock_param_map.end())
+    const auto clean_param_it = dock_param_map->find(start_wp_name);
+    if (clean_param_it == dock_param_map->end())
     {
       RCLCPP_INFO(
         node->get_logger(),
@@ -944,6 +944,9 @@ void FleetUpdateHandle::Implementation::add_standard_tasks()
   events::GoToPlace::add(*activation.event);
 
   tasks::add_delivery(
+    deserialization.task,
+    deserialization.event,
+    planner,
     *activation.task,
     activation.phase,
     *activation.event,
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
index bac21e80f..359e74369 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
@@ -197,9 +197,15 @@ class FleetUpdateHandle::Implementation
   // Map task id to pair of <RequestPtr, Assignments>
   using Assignments = rmf_task::TaskPlanner::Assignments;
 
+  using DockParamMap =
+    std::unordered_map<
+      std::string,
+      rmf_fleet_msgs::msg::DockParameter
+    >;
+
   // Map of dock name to dock parameters
-  std::unordered_map<std::string,
-    rmf_fleet_msgs::msg::DockParameter> dock_param_map = {};
+  std::shared_ptr<DockParamMap> dock_param_map =
+    std::make_shared<DockParamMap>();
 
   // TODO Support for various charging configurations
   std::unordered_set<std::size_t> charging_waypoints = {};
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp
index 01332df75..118f61a6c 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp
@@ -35,6 +35,9 @@
 #include <rmf_task_sequence/Task.hpp>
 #include <rmf_task_sequence/phases/SimplePhase.hpp>
 
+#include <rmf_fleet_adapter/schemas/event_description_PayloadTransfer.hpp>
+#include <rmf_fleet_adapter/schemas/task_description_Delivery.hpp>
+
 namespace rmf_fleet_adapter {
 namespace tasks {
 
@@ -281,8 +284,123 @@ struct TransferItems : public rmf_task_sequence::events::Placeholder::Descriptio
   }
 };
 
+//==============================================================================
+template<typename T>
+std::function<agv::FleetUpdateHandle::DeserializedEvent(const nlohmann::json& msg)>
+make_deserializer(
+  std::shared_ptr<const std::shared_ptr<const rmf_traffic::agv::Planner>> planner)
+{
+  auto parse_payload_component = [](const nlohmann::json& msg)
+    -> rmf_task::Payload::Component
+    {
+      std::string compartment = "";
+
+      if (const auto& compartment_json = msg["compartment"])
+        compartment = compartment_json.get<std::string>();
+
+      return rmf_task::Payload::Component(
+        msg["sku"].get<std::string>(),
+        msg["quantity"].get<uint64_t>(),
+        std::move(compartment));
+    };
+
+  return
+    [
+      planner = std::move(planner),
+      parse_payload_component = std::move(parse_payload_component)
+    ](const nlohmann::json& msg)
+    -> agv::FleetUpdateHandle::DeserializedEvent
+    {
+      const auto& place_json = msg["place"];
+      std::optional<rmf_traffic::agv::Plan::Goal> place;
+      const auto& graph = (*planner)->get_configuration().graph();
+      if (place_json.is_number())
+      {
+        const auto wp_index = place_json.get<std::size_t>();
+        if (graph.num_waypoints() <= wp_index)
+        {
+          return {
+            nullptr,
+            {"waypoint index value for 'place' property ["
+              + std::to_string(wp_index) + "] exceeds the limits for the nav "
+              "graph size [" + std::to_string(graph.num_waypoints()) + "]"}
+          };
+        }
+
+        place = rmf_traffic::agv::Plan::Goal(wp_index);
+      }
+      else if (place_json.is_string())
+      {
+        const auto& wp_name = place_json.get<std::string>();
+        const auto* wp = graph.find_waypoint(wp_name);
+        if (!wp)
+        {
+          return {
+            nullptr,
+            {"waypoint name for 'place' property [" + wp_name + "] cannot be "
+             "found in the navigation graph"}
+          };
+        }
+
+        place = rmf_traffic::agv::Plan::Goal(wp->index());
+      }
+      else
+      {
+        return {
+          nullptr,
+          {"invalid data type provided for 'place' property: expected a number "
+           "or a string but got " + std::string(place_json.type_name())
+           + " type instead"}
+        };
+      }
+
+      std::vector<rmf_task::Payload::Component> payload_components;
+      const auto& payload_json = msg["payload"];
+      if (payload_json.is_object())
+      {
+        payload_components.push_back(parse_payload_component(payload_json));
+      }
+      else if (payload_json.is_array())
+      {
+        for (const auto& component_json : payload_json)
+        {
+          payload_components.push_back(parse_payload_component(component_json));
+        }
+      }
+      else
+      {
+        return {
+          nullptr,
+          {"invalid data type provided for 'payload' property: expected an "
+           "object or an array but got " + std::string(payload_json.type_name())
+           + " type instead"}
+        };
+      }
+
+      std::string handler;
+      if (const auto& handler_json = msg["handler"])
+      {
+        handler = handler_json.get<std::string>();
+      }
+
+      // TODO(MXG): Add a way for system integrators to specify a duration
+      // estimate for the payload transfer
+      return {
+        T::make(
+          std::move(*place),
+          std::move(handler),
+          rmf_task::Payload(std::move(payload_components)),
+          rmf_traffic::Duration(0)),
+        {}
+      };
+    };
+}
+
 //==============================================================================
 void add_delivery(
+  DeserializeJSON<agv::FleetUpdateHandle::DeserializedTask>& task_serde,
+  DeserializeJSON<agv::FleetUpdateHandle::DeserializedEvent>& event_serde,
+  std::shared_ptr<const std::shared_ptr<const rmf_traffic::agv::Planner>> planner,
   rmf_task::Activator& task_activator,
   const rmf_task_sequence::Phase::ConstActivatorPtr& phase_activator,
   rmf_task_sequence::Event::Initializer& event_initializer,
@@ -292,6 +410,46 @@ void add_delivery(
   using PickUp = rmf_task_sequence::events::PickUp;
   using DropOff = rmf_task_sequence::events::DropOff;
   using Phase = rmf_task_sequence::phases::SimplePhase;
+  using DeliveryDescription = rmf_task::requests::Delivery::Description;
+
+  auto validate_payload_transfer =
+    std::make_shared<nlohmann::json_schema::json_validator>(
+      schemas::event_description_PayloadTransfer);
+
+  auto validate_delivery =
+    std::make_shared<nlohmann::json_schema::json_validator>(
+      schemas::task_description_Delivery);
+
+  auto deserialize_pickup = make_deserializer<PickUp::Description>(planner);
+  auto deserialize_dropoff = make_deserializer<DropOff::Description>(planner);
+  auto deserialize_delivery =
+    [deserialize_pickup, deserialize_dropoff](
+      const nlohmann::json& msg) -> agv::FleetUpdateHandle::DeserializedTask
+    {
+      const auto pickup = deserialize_pickup(msg["pickup"]);
+      const auto dropoff = deserialize_dropoff(msg["dropoff"]);
+      std::vector<std::string> errors;
+      errors.reserve(pickup.errors.size() + dropoff.errors.size());
+      errors.insert(
+        errors.end(), pickup.errors.begin(), pickup.errors.end());
+      errors.insert(
+        errors.end(), dropoff.errors.begin(), dropoff.errors.end());
+
+      if (!pickup.description || !dropoff.description)
+      {
+        return {nullptr, std::move(errors)};
+      }
+
+      rmf_task_sequence::Task::Builder builder;
+      builder.add_phase(Phase::Description::make(pickup.description), {});
+      builder.add_phase(Phase::Description::make(dropoff.description), {});
+      // TODO(MXG): Consider making the category and detail more detailed
+      return {builder.build("Delivery", ""), std::move(errors)};
+    };
+
+  task_serde.add("delivery", validate_delivery, deserialize_delivery);
+  event_serde.add("pickup", validate_payload_transfer, deserialize_pickup);
+  event_serde.add("dropoff", validate_payload_transfer, deserialize_dropoff);
 
   auto private_initializer =
     std::make_shared<rmf_task_sequence::Event::Initializer>();
@@ -324,7 +482,7 @@ void add_delivery(
     std::move(dropoff_unfolder), event_initializer, private_initializer);
 
   auto delivery_unfolder =
-    [](const rmf_task::requests::Delivery::Description& delivery)
+    [](const DeliveryDescription& delivery)
     {
       rmf_task_sequence::Task::Builder builder;
       builder
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp
index d1dd477fe..ed66ad455 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp
@@ -28,6 +28,9 @@
 #include <rmf_task_sequence/Phase.hpp>
 #include <rmf_task_sequence/Event.hpp>
 
+#include "../DeserializeJSON.hpp"
+#include <rmf_fleet_adapter/agv/FleetUpdateHandle.hpp>
+
 namespace rmf_fleet_adapter {
 namespace tasks {
 
@@ -41,6 +44,9 @@ std::shared_ptr<LegacyTask> make_delivery(
   const rmf_task_msgs::msg::Delivery delivery_profile);
 
 void add_delivery(
+  DeserializeJSON<agv::FleetUpdateHandle::DeserializedTask>& task_serde,
+  DeserializeJSON<agv::FleetUpdateHandle::DeserializedEvent>& event_serde,
+  std::shared_ptr<const std::shared_ptr<const rmf_traffic::agv::Planner>> planner,
   rmf_task::Activator& task_activator,
   const rmf_task_sequence::Phase::ConstActivatorPtr& phase_activator,
   rmf_task_sequence::Event::Initializer& event_initializer,

From 42a826efac7f27d8f0ade47760a690c26a533c6a Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Fri, 31 Dec 2021 17:46:52 +0800
Subject: [PATCH 33/79] Finish implementations for delivery and loop

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 .../agv/FleetUpdateHandle.hpp                 |  24 +++-
 rmf_fleet_adapter/schemas/Place.json          |  32 +++++
 .../schemas/event_description_GoToPlace.json  |  18 +--
 .../event_description_PayloadTransfer.json    |  12 +-
 .../schemas/task_description_Patrol.json      |  19 +++
 .../agv/FleetUpdateHandle.cpp                 | 121 ++++++++++++++++--
 .../agv/internal_FleetUpdateHandle.hpp        |  15 +++
 .../src/rmf_fleet_adapter/tasks/Clean.cpp     |  13 +-
 .../src/rmf_fleet_adapter/tasks/Clean.hpp     |   7 +-
 .../src/rmf_fleet_adapter/tasks/Delivery.cpp  |  94 +++++---------
 .../src/rmf_fleet_adapter/tasks/Delivery.hpp  |  14 +-
 .../src/rmf_fleet_adapter/tasks/Loop.cpp      |  77 ++++++++++-
 .../src/rmf_fleet_adapter/tasks/Loop.hpp      |   6 +-
 13 files changed, 325 insertions(+), 127 deletions(-)
 create mode 100644 rmf_fleet_adapter/schemas/Place.json
 create mode 100644 rmf_fleet_adapter/schemas/task_description_Patrol.json

diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp
index 91528687b..be6273ef7 100644
--- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp
@@ -78,11 +78,12 @@ class FleetUpdateHandle : public std::enable_shared_from_this<FleetUpdateHandle>
   template<typename T>
   struct DeserializedDescription
   {
-    std::shared_ptr<const T> description;
+    T description;
     std::vector<std::string> errors;
   };
 
-  using DeserializedTask = DeserializedDescription<rmf_task::Task::Description>;
+  using DeserializedTask =
+    DeserializedDescription<std::shared_ptr<const rmf_task::Task::Description>>;
 
   using TaskDescriptionDeserializer =
     std::function<DeserializedTask(const nlohmann::json&)>;
@@ -113,7 +114,9 @@ class FleetUpdateHandle : public std::enable_shared_from_this<FleetUpdateHandle>
     std::function<void(rmf_task::Activator&)> task_activator);
 
   using DeserializedPhase =
-    DeserializedDescription<rmf_task_sequence::Phase::Description>;
+    DeserializedDescription<
+      std::shared_ptr<const rmf_task_sequence::Phase::Description>
+    >;
 
   using PhaseDescriptionDeserializer =
     std::function<DeserializedPhase(const nlohmann::json&)>;
@@ -149,7 +152,9 @@ class FleetUpdateHandle : public std::enable_shared_from_this<FleetUpdateHandle>
   rmf_task_sequence::Phase::ActivatorPtr builtin_phase_activator();
 
   using DeserializedEvent =
-    DeserializedDescription<rmf_task_sequence::Event::Description>;
+    DeserializedDescription<
+      std::shared_ptr<const rmf_task_sequence::Event::Description>
+    >;
 
   using EventDescriptionDeserializer =
     std::function<DeserializedEvent(const nlohmann::json&)>;
@@ -184,6 +189,17 @@ class FleetUpdateHandle : public std::enable_shared_from_this<FleetUpdateHandle>
 
   rmf_task_sequence::Event::InitializerPtr builtin_event_initializer();
 
+  using DeserializedPlace =
+    agv::FleetUpdateHandle::DeserializedDescription<
+      std::optional<rmf_traffic::agv::Plan::Goal>
+    >;
+
+  using PlaceDeserializer =
+    std::function<DeserializedPlace(const nlohmann::json&)>;
+
+  /// Get the "place" deserializer for the nav graph of this fleet.
+  PlaceDeserializer builtin_place_deserializer() const;
+
   /// Specify a set of lanes that should be closed.
   void close_lanes(std::vector<std::size_t> lane_indices);
 
diff --git a/rmf_fleet_adapter/schemas/Place.json b/rmf_fleet_adapter/schemas/Place.json
new file mode 100644
index 000000000..94c2f8a1e
--- /dev/null
+++ b/rmf_fleet_adapter/schemas/Place.json
@@ -0,0 +1,32 @@
+{
+  "$schema": "https://json-schema.org/draft/2020-12/schema",
+  "$id": "https://raw.githubusercontent.com/open-rmf/rmf_ros2/main/rmf_fleet_adapter/schemas/Place.json",
+  "title": "Place Description",
+  "description": "Description of a place that the robot can go to",
+  "oneOf": [
+    {
+      "$ref": "#/$defs/waypoint"
+    },
+    {
+      "type": "object",
+      "properties": {
+        "waypoint": { "$ref": "#/$defs/waypoint" },
+        "orientation": { "type": "number" }
+      },
+      "required": ["waypoint"]
+    }
+  ],
+  "$defs": {
+    "waypoint": {
+      "oneOf": [
+        {
+          "type": "string"
+        },
+        {
+          "type": "integer",
+          "minimum": 0
+        }
+      ]
+    }
+  }
+}
diff --git a/rmf_fleet_adapter/schemas/event_description_GoToPlace.json b/rmf_fleet_adapter/schemas/event_description_GoToPlace.json
index de0ab54d9..c9a57826d 100644
--- a/rmf_fleet_adapter/schemas/event_description_GoToPlace.json
+++ b/rmf_fleet_adapter/schemas/event_description_GoToPlace.json
@@ -1,21 +1,7 @@
 {
   "$schema": "https://json-schema.org/draft/2020-12/schema",
   "$id": "https://raw.githubusercontent.com/open-rmf/rmf_ros2/main/rmf_fleet_adapter/schemas/event_description_GoToPlace.json",
-  "title": "Go To Place Event",
+  "title": "Go To Place Event Description",
   "description": "Have a robot go to a place",
-  "type": "object",
-  "properties": {
-    "place": {
-      "oneOf": [
-        {
-          "type": "string"
-        },
-        {
-          "type": "integer",
-          "minimum": 0
-        }
-      ]
-    }
-  },
-  "required": ["place"]
+  "$ref": "Place.json"
 }
diff --git a/rmf_fleet_adapter/schemas/event_description_PayloadTransfer.json b/rmf_fleet_adapter/schemas/event_description_PayloadTransfer.json
index bb1ba24fa..0155859f5 100644
--- a/rmf_fleet_adapter/schemas/event_description_PayloadTransfer.json
+++ b/rmf_fleet_adapter/schemas/event_description_PayloadTransfer.json
@@ -5,17 +5,7 @@
   "description": "Description for a drop off or a pick up event category",
   "type": "object",
   "properties": {
-    "place": {
-      "oneOf": [
-        {
-          "type": "string"
-        },
-        {
-          "type": "integer",
-          "minimum": 0
-        }
-      ]
-    },
+    "place": { "$ref": "Place.json" },
     "handler": { "type": "string" },
     "payload" : {
       "oneOf": [
diff --git a/rmf_fleet_adapter/schemas/task_description_Patrol.json b/rmf_fleet_adapter/schemas/task_description_Patrol.json
new file mode 100644
index 000000000..dab5f2acc
--- /dev/null
+++ b/rmf_fleet_adapter/schemas/task_description_Patrol.json
@@ -0,0 +1,19 @@
+{
+  "$schema": "https://json-schema.org/draft/2020-12/schema",
+  "$id": "https://raw.githubusercontent.com/open-rmf/rmf_ros2/main/rmf_fleet_adapter/schemas/task_description_Patrol.json",
+  "title": "Patrol Task Description",
+  "description": "Description of a patrolling task",
+  "properties": {
+    "places": {
+      "description": "A list of which places to patrol between",
+      "type": "array",
+      "items": { "$ref": "Place.json" }
+    },
+    "rounds": {
+      "description": "How many times the patrol should be performed. By default this is 1.",
+      "type": "integer",
+      "minimum": 1
+    }
+  },
+  "required": ["places"]
+}
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
index 81c090753..14d25b89b 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
@@ -48,6 +48,8 @@
 #include <unordered_set>
 #include <stdexcept>
 
+#include <rmf_fleet_adapter/schemas/Place.hpp>
+
 namespace rmf_fleet_adapter {
 namespace agv {
 
@@ -86,6 +88,45 @@ class LiaisonNegotiator : public rmf_traffic::schedule::Negotiator
 };
 } // anonymous namespace
 
+//==============================================================================
+void TaskDeserialization::add_schema(const nlohmann::json& schema)
+{
+  _schema_dictionary->insert_or_assign(
+    nlohmann::json_uri(schema["$id"]).url(), schema);
+}
+
+//==============================================================================
+nlohmann::json_schema::json_validator TaskDeserialization::make_validator(
+  nlohmann::json schema) const
+{
+  return nlohmann::json_schema::json_validator(std::move(schema), _loader);
+}
+
+//==============================================================================
+std::shared_ptr<nlohmann::json_schema::json_validator>
+TaskDeserialization::make_validator_shared(nlohmann::json schema) const
+{
+  return std::make_shared<nlohmann::json_schema::json_validator>(
+    make_validator(std::move(schema)));
+}
+
+//==============================================================================
+TaskDeserialization::TaskDeserialization()
+{
+  _schema_dictionary = std::make_shared<SchemaDictionary>();
+  _loader = [dict = _schema_dictionary](
+      const nlohmann::json_uri& id,
+      nlohmann::json& value)
+    {
+      const auto it = dict->find(id.url());
+      if (it == dict->end())
+        return;
+
+      value = it->second;
+    };
+}
+
+
 //==============================================================================
 void FleetUpdateHandle::Implementation::dock_summary_cb(
   const DockSummary::SharedPtr& msg)
@@ -931,6 +972,72 @@ void FleetUpdateHandle::Implementation::update_fleet_state() const
   }
 }
 
+namespace {
+FleetUpdateHandle::PlaceDeserializer make_place_deserializer(
+  std::shared_ptr<const std::shared_ptr<const rmf_traffic::agv::Planner>>
+    planner)
+{
+  return [planner = std::move(planner)](const nlohmann::json& msg)
+      -> agv::FleetUpdateHandle::DeserializedPlace
+    {
+      std::optional<rmf_traffic::agv::Plan::Goal> place;
+      const auto& graph = (*planner)->get_configuration().graph();
+      if (msg.is_number() || (msg.is_object() && msg["waypoint"].is_number()))
+      {
+        const auto wp_index = msg.is_number()?
+          msg.get<std::size_t>() : msg["waypoint"].get<std::size_t>();
+
+        if (graph.num_waypoints() <= wp_index)
+        {
+          return {
+            std::nullopt,
+            {"waypoint index value for Place ["
+              + std::to_string(wp_index) + "] exceeds the limits for the nav "
+              "graph size [" + std::to_string(graph.num_waypoints()) + "]"}
+          };
+        }
+
+        place = rmf_traffic::agv::Plan::Goal(wp_index);
+      }
+      else if (msg.is_string() || (msg.is_object() && msg["waypoint"].is_string()))
+      {
+        const auto& wp_name = msg.is_string()?
+          msg.get<std::string>() : msg["waypoint"].get<std::string>();
+
+        const auto* wp = graph.find_waypoint(wp_name);
+        if (!wp)
+        {
+          return {
+            std::nullopt,
+            {"waypoint name for Place [" + wp_name + "] cannot be "
+             "found in the navigation graph"}
+          };
+        }
+
+        place = rmf_traffic::agv::Plan::Goal(wp->index());
+      }
+      else
+      {
+        return {
+          std::nullopt,
+          {"invalid data type provided for Place: expected a number "
+           "or a string or an object but got " + std::string(msg.type_name())
+           + " type instead"}
+        };
+      }
+
+      if (msg.is_object())
+      {
+        const auto& ori_json = msg["orientation"];
+        if (ori_json)
+          place->orientation(ori_json.get<double>());
+      }
+
+      return {place, {}};
+    };
+}
+} // anonymous namespace
+
 //==============================================================================
 void FleetUpdateHandle::Implementation::add_standard_tasks()
 {
@@ -942,19 +1049,17 @@ void FleetUpdateHandle::Implementation::add_standard_tasks()
     *activation.phase, activation.event);
 
   events::GoToPlace::add(*activation.event);
+  deserialization.place = make_place_deserializer(planner);
+  deserialization.add_schema(schemas::Place);
 
   tasks::add_delivery(
-    deserialization.task,
-    deserialization.event,
-    planner,
-    *activation.task,
-    activation.phase,
-    *activation.event,
+    deserialization,
+    activation,
     node->clock());
 
   tasks::add_loop(
-    *activation.task,
-    activation.phase,
+    deserialization,
+    activation,
     node->clock());
 
   tasks::add_clean(
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
index 359e74369..d4df5eaa9 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
@@ -83,6 +83,21 @@ struct TaskDeserialization
   DeserializeJSON<FleetUpdateHandle::DeserializedTask> task;
   DeserializeJSON<FleetUpdateHandle::DeserializedPhase> phase;
   DeserializeJSON<FleetUpdateHandle::DeserializedEvent> event;
+  FleetUpdateHandle::PlaceDeserializer place;
+
+  void add_schema(const nlohmann::json& schema);
+
+  nlohmann::json_schema::json_validator make_validator(
+    nlohmann::json schema) const;
+
+  std::shared_ptr<nlohmann::json_schema::json_validator> make_validator_shared(
+    nlohmann::json schema) const;
+
+  TaskDeserialization();
+private:
+  using SchemaDictionary = std::unordered_map<std::string, nlohmann::json>;
+  std::shared_ptr<SchemaDictionary> _schema_dictionary;
+  std::function<void(const nlohmann::json_uri&, nlohmann::json&)> _loader;
 };
 
 //==============================================================================
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp
index 91b39163c..0d8b3906f 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp
@@ -252,15 +252,16 @@ struct CleanEvent : public rmf_task_sequence::events::Placeholder::Description
 
 //==============================================================================
 void add_clean(
-  rmf_task::Activator& task_activator,
-  const rmf_task_sequence::Phase::ConstActivatorPtr& phase_activator,
-  const rmf_task_sequence::Event::InitializerPtr& event_initializer,
+  agv::TaskDeserialization& deserialization,
+  agv::TaskActivation& activation,
   std::function<rmf_traffic::Time()> clock)
 {
   using Clean = rmf_task::requests::Clean;
   using Phase = rmf_task_sequence::phases::SimplePhase;
 
-  CleanEvent::add(*event_initializer);
+
+
+  CleanEvent::add(*activation.event);
 
   auto clean_unfolder =
     [](const Clean::Description& clean)
@@ -274,8 +275,8 @@ void add_clean(
   };
 
   rmf_task_sequence::Task::unfold<rmf_task::requests::Clean::Description>(
-    std::move(clean_unfolder), task_activator,
-    phase_activator, std::move(clock));
+    std::move(clean_unfolder), *activation.task,
+    activation.phase, std::move(clock));
 }
 
 } // namespace task
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp
index ea18f0e24..20da081fa 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp
@@ -30,6 +30,8 @@
 #include <rmf_task_sequence/Phase.hpp>
 #include <rmf_task_sequence/Event.hpp>
 
+#include "../agv/internal_FleetUpdateHandle.hpp"
+
 namespace rmf_fleet_adapter {
 namespace tasks {
 
@@ -42,9 +44,8 @@ std::shared_ptr<LegacyTask> make_clean(
   const rmf_task::State finish_state);
 
 void add_clean(
-  rmf_task::Activator& task_activator,
-  const rmf_task_sequence::Phase::ConstActivatorPtr& phase_activator,
-  const rmf_task_sequence::Event::InitializerPtr& event_initializer,
+  agv::TaskDeserialization& deserialization,
+  agv::TaskActivation& activation,
   std::function<rmf_traffic::Time()> clock);
 
 } // namespace tasks
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp
index 118f61a6c..668e76679 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp
@@ -36,6 +36,8 @@
 #include <rmf_task_sequence/phases/SimplePhase.hpp>
 
 #include <rmf_fleet_adapter/schemas/event_description_PayloadTransfer.hpp>
+#include <rmf_fleet_adapter/schemas/event_description_PickUp.hpp>
+#include <rmf_fleet_adapter/schemas/event_description_DropOff.hpp>
 #include <rmf_fleet_adapter/schemas/task_description_Delivery.hpp>
 
 namespace rmf_fleet_adapter {
@@ -288,7 +290,7 @@ struct TransferItems : public rmf_task_sequence::events::Placeholder::Descriptio
 template<typename T>
 std::function<agv::FleetUpdateHandle::DeserializedEvent(const nlohmann::json& msg)>
 make_deserializer(
-  std::shared_ptr<const std::shared_ptr<const rmf_traffic::agv::Planner>> planner)
+  const agv::FleetUpdateHandle::PlaceDeserializer& place_deser)
 {
   auto parse_payload_component = [](const nlohmann::json& msg)
     -> rmf_task::Payload::Component
@@ -306,52 +308,15 @@ make_deserializer(
 
   return
     [
-      planner = std::move(planner),
+      place_deser,
       parse_payload_component = std::move(parse_payload_component)
     ](const nlohmann::json& msg)
-    -> agv::FleetUpdateHandle::DeserializedEvent
+      -> agv::FleetUpdateHandle::DeserializedEvent
     {
-      const auto& place_json = msg["place"];
-      std::optional<rmf_traffic::agv::Plan::Goal> place;
-      const auto& graph = (*planner)->get_configuration().graph();
-      if (place_json.is_number())
+      auto place = place_deser(msg["place"]);
+      if (!place.description.has_value())
       {
-        const auto wp_index = place_json.get<std::size_t>();
-        if (graph.num_waypoints() <= wp_index)
-        {
-          return {
-            nullptr,
-            {"waypoint index value for 'place' property ["
-              + std::to_string(wp_index) + "] exceeds the limits for the nav "
-              "graph size [" + std::to_string(graph.num_waypoints()) + "]"}
-          };
-        }
-
-        place = rmf_traffic::agv::Plan::Goal(wp_index);
-      }
-      else if (place_json.is_string())
-      {
-        const auto& wp_name = place_json.get<std::string>();
-        const auto* wp = graph.find_waypoint(wp_name);
-        if (!wp)
-        {
-          return {
-            nullptr,
-            {"waypoint name for 'place' property [" + wp_name + "] cannot be "
-             "found in the navigation graph"}
-          };
-        }
-
-        place = rmf_traffic::agv::Plan::Goal(wp->index());
-      }
-      else
-      {
-        return {
-          nullptr,
-          {"invalid data type provided for 'place' property: expected a number "
-           "or a string but got " + std::string(place_json.type_name())
-           + " type instead"}
-        };
+        return {nullptr, std::move(place.errors)};
       }
 
       std::vector<rmf_task::Payload::Component> payload_components;
@@ -387,7 +352,7 @@ make_deserializer(
       // estimate for the payload transfer
       return {
         T::make(
-          std::move(*place),
+          std::move(*place.description),
           std::move(handler),
           rmf_task::Payload(std::move(payload_components)),
           rmf_traffic::Duration(0)),
@@ -398,12 +363,8 @@ make_deserializer(
 
 //==============================================================================
 void add_delivery(
-  DeserializeJSON<agv::FleetUpdateHandle::DeserializedTask>& task_serde,
-  DeserializeJSON<agv::FleetUpdateHandle::DeserializedEvent>& event_serde,
-  std::shared_ptr<const std::shared_ptr<const rmf_traffic::agv::Planner>> planner,
-  rmf_task::Activator& task_activator,
-  const rmf_task_sequence::Phase::ConstActivatorPtr& phase_activator,
-  rmf_task_sequence::Event::Initializer& event_initializer,
+  agv::TaskDeserialization& deserialization,
+  agv::TaskActivation& activation,
   std::function<rmf_traffic::Time()> clock)
 {
   using Bundle = rmf_task_sequence::events::Bundle;
@@ -413,15 +374,26 @@ void add_delivery(
   using DeliveryDescription = rmf_task::requests::Delivery::Description;
 
   auto validate_payload_transfer =
-    std::make_shared<nlohmann::json_schema::json_validator>(
+    deserialization.make_validator_shared(
       schemas::event_description_PayloadTransfer);
+  deserialization.add_schema(schemas::event_description_PayloadTransfer);
+
+  auto deserialize_pickup =
+    make_deserializer<PickUp::Description>(deserialization.place);
+  deserialization.event.add(
+    "pickup", validate_payload_transfer, deserialize_pickup);
+
+  auto deserialize_dropoff =
+    make_deserializer<DropOff::Description>(deserialization.place);
+  deserialization.event.add(
+    "dropoff", validate_payload_transfer, deserialize_dropoff);
 
   auto validate_delivery =
-    std::make_shared<nlohmann::json_schema::json_validator>(
-      schemas::task_description_Delivery);
+    deserialization.make_validator_shared(schemas::task_description_Delivery);
+  deserialization.add_schema(schemas::task_description_Delivery);
+  deserialization.add_schema(schemas::event_description_PickUp);
+  deserialization.add_schema(schemas::event_description_DropOff);
 
-  auto deserialize_pickup = make_deserializer<PickUp::Description>(planner);
-  auto deserialize_dropoff = make_deserializer<DropOff::Description>(planner);
   auto deserialize_delivery =
     [deserialize_pickup, deserialize_dropoff](
       const nlohmann::json& msg) -> agv::FleetUpdateHandle::DeserializedTask
@@ -447,9 +419,7 @@ void add_delivery(
       return {builder.build("Delivery", ""), std::move(errors)};
     };
 
-  task_serde.add("delivery", validate_delivery, deserialize_delivery);
-  event_serde.add("pickup", validate_payload_transfer, deserialize_pickup);
-  event_serde.add("dropoff", validate_payload_transfer, deserialize_dropoff);
+  deserialization.task.add("delivery", validate_delivery, deserialize_delivery);
 
   auto private_initializer =
     std::make_shared<rmf_task_sequence::Event::Initializer>();
@@ -467,7 +437,7 @@ void add_delivery(
     };
 
   Bundle::unfold<PickUp::Description>(
-    std::move(pickup_unfolder), event_initializer, private_initializer);
+    std::move(pickup_unfolder), *activation.event, private_initializer);
 
   auto dropoff_unfolder =
     [](const DropOff::Description& dropoff)
@@ -479,7 +449,7 @@ void add_delivery(
     };
 
   Bundle::unfold<DropOff::Description>(
-    std::move(dropoff_unfolder), event_initializer, private_initializer);
+    std::move(dropoff_unfolder), *activation.event, private_initializer);
 
   auto delivery_unfolder =
     [](const DeliveryDescription& delivery)
@@ -506,8 +476,8 @@ void add_delivery(
     };
 
   rmf_task_sequence::Task::unfold<rmf_task::requests::Delivery::Description>(
-    std::move(delivery_unfolder), task_activator,
-    phase_activator, std::move(clock));
+    std::move(delivery_unfolder), *activation.task,
+    activation.phase, std::move(clock));
 }
 
 } // namespace task
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp
index ed66ad455..41a0ed124 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp
@@ -25,11 +25,7 @@
 
 #include <rmf_task_msgs/msg/delivery.hpp>
 
-#include <rmf_task_sequence/Phase.hpp>
-#include <rmf_task_sequence/Event.hpp>
-
-#include "../DeserializeJSON.hpp"
-#include <rmf_fleet_adapter/agv/FleetUpdateHandle.hpp>
+#include "../agv/internal_FleetUpdateHandle.hpp"
 
 namespace rmf_fleet_adapter {
 namespace tasks {
@@ -44,12 +40,8 @@ std::shared_ptr<LegacyTask> make_delivery(
   const rmf_task_msgs::msg::Delivery delivery_profile);
 
 void add_delivery(
-  DeserializeJSON<agv::FleetUpdateHandle::DeserializedTask>& task_serde,
-  DeserializeJSON<agv::FleetUpdateHandle::DeserializedEvent>& event_serde,
-  std::shared_ptr<const std::shared_ptr<const rmf_traffic::agv::Planner>> planner,
-  rmf_task::Activator& task_activator,
-  const rmf_task_sequence::Phase::ConstActivatorPtr& phase_activator,
-  rmf_task_sequence::Event::Initializer& event_initializer,
+  agv::TaskDeserialization& deserialization,
+  agv::TaskActivation& activation,
   std::function<rmf_traffic::Time()> clock);
 
 } // namespace tasks
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp
index 39b24ead2..6a23ad5c5 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp
@@ -23,6 +23,9 @@
 #include <rmf_task_sequence/phases/SimplePhase.hpp>
 #include <rmf_task_sequence/events/GoToPlace.hpp>
 
+#include <rmf_fleet_adapter/schemas/event_description_GoToPlace.hpp>
+#include <rmf_fleet_adapter/schemas/task_description_Patrol.hpp>
+
 namespace rmf_fleet_adapter {
 namespace tasks {
 
@@ -115,14 +118,80 @@ std::shared_ptr<LegacyTask> make_loop(
 
 //==============================================================================
 void add_loop(
-  rmf_task::Activator& task_activator,
-  const rmf_task_sequence::Phase::ConstActivatorPtr& phase_activator,
+  agv::TaskDeserialization& deserialization,
+  agv::TaskActivation& activation,
   std::function<rmf_traffic::Time()> clock)
 {
   using Loop = rmf_task::requests::Loop;
   using Phase = rmf_task_sequence::phases::SimplePhase;
   using GoToPlace = rmf_task_sequence::events::GoToPlace;
 
+  auto validate_go_to_place =
+    deserialization.make_validator_shared(schemas::event_description_GoToPlace);
+  deserialization.add_schema(schemas::event_description_GoToPlace);
+
+  auto deserialize_go_to_place =
+    [place_deser = deserialization.place](const nlohmann::json& msg)
+      -> agv::FleetUpdateHandle::DeserializedEvent
+    {
+      auto place = place_deser(msg);
+      if (!place.description.has_value())
+        return {nullptr, std::move(place.errors)};
+
+      return {
+        GoToPlace::Description::make(std::move(*place.description)),
+        std::move(place.errors)
+      };
+    };
+
+  deserialization.event.add(
+    "go_to_place", validate_go_to_place, deserialize_go_to_place);
+
+  auto validate_patrol =
+    deserialization.make_validator_shared(schemas::task_description_Patrol);
+  deserialization.add_schema(schemas::task_description_Patrol);
+
+  auto deserialize_patrol =
+    [place_deser = deserialization.place](const nlohmann::json& msg)
+      -> agv::FleetUpdateHandle::DeserializedTask
+    {
+      const auto& places_json = msg["places"];
+      std::vector<rmf_traffic::agv::Plan::Goal> places;
+      std::vector<std::string> errors;
+      bool any_failure = false;
+      for (const auto& place_json : places_json)
+      {
+        const auto place = place_deser(place_json);
+        if (!place.description.has_value())
+          any_failure = true;
+        else
+          places.push_back(*place.description);
+
+        errors.insert(errors.begin(), place.errors.begin(), place.errors.end());
+      }
+
+      if (any_failure)
+        return {nullptr, std::move(errors)};
+
+      std::size_t rounds = 1;
+      if (const auto& rounds_json = msg["rounds"])
+        rounds = rounds_json.get<std::size_t>();
+
+      rmf_task_sequence::Task::Builder builder;
+      for (std::size_t i=0; i < rounds; ++i)
+      {
+        for (const auto& place : places)
+        {
+          builder.add_phase(
+            Phase::Description::make(GoToPlace::Description::make(place)), {});
+        }
+      }
+
+      return {builder.build("Patrol", ""), std::move(errors)};
+    };
+
+  deserialization.task.add("patrol", validate_patrol, deserialize_patrol);
+
   auto loop_unfolder =
     [](const Loop::Description& loop)
     {
@@ -143,8 +212,8 @@ void add_loop(
     };
 
   rmf_task_sequence::Task::unfold<rmf_task::requests::Loop::Description>(
-    std::move(loop_unfolder), task_activator,
-    phase_activator, std::move(clock));
+    std::move(loop_unfolder), *activation.task,
+    activation.phase, std::move(clock));
 }
 
 } // namespace tasks
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.hpp
index b02d5b1e7..388bbaf4b 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.hpp
@@ -25,6 +25,8 @@
 
 #include <rmf_task_sequence/Phase.hpp>
 
+#include "../agv/internal_FleetUpdateHandle.hpp"
+
 namespace rmf_fleet_adapter {
 namespace tasks {
 
@@ -38,8 +40,8 @@ std::shared_ptr<LegacyTask> make_loop(
 
 //==============================================================================
 void add_loop(
-  rmf_task::Activator& task_activator,
-  const rmf_task_sequence::Phase::ConstActivatorPtr& phase_activator,
+  agv::TaskDeserialization& deserialization,
+  agv::TaskActivation& activation,
   std::function<rmf_traffic::Time()> clock);
 
 } // namespace tasks

From e6e75b4f07b5a41340db4fe78c0ec6773ebfec29 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Fri, 31 Dec 2021 18:30:10 +0800
Subject: [PATCH 34/79] Fix validator instantiation

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
index ac2b7f9f6..fddda5946 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
@@ -1291,6 +1291,9 @@ void TaskManager::_publish_task_queue()
 {
   rmf_task::State expected_state = _context->current_task_end_state();
   const auto& parameters = *_context->task_parameters();
+  static const auto validator =
+    _make_validator(rmf_api_msgs::schemas::task_state_update);
+
   for (const auto& pending : _queue)
   {
     const auto info = pending.request()->description()->generate_info(
@@ -1313,8 +1316,7 @@ void TaskManager::_publish_task_queue()
     auto task_state_update = _task_state_update_json;
     task_state_update["data"] = pending_json;
 
-    _validate_and_publish_websocket(
-      task_state_update, rmf_api_msgs::schemas::task_state_update);
+    _validate_and_publish_websocket(task_state_update, validator);
 
     expected_state = pending.finish_state();
   }

From 18078c4902232778d223f31d9ff451b33a52cc1c Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Mon, 3 Jan 2022 16:07:15 +0800
Subject: [PATCH 35/79] Finish implementation of task description
 deserialization

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 .../agv/FleetUpdateHandle.cpp                 |  7 +-
 .../agv/internal_FleetUpdateHandle.hpp        |  2 +
 .../src/rmf_fleet_adapter/tasks/Clean.cpp     | 88 +++++++++++++++++++
 .../src/rmf_fleet_adapter/tasks/Clean.hpp     |  2 +
 .../src/rmf_task_ros2/Dispatcher.cpp          |  2 +-
 5 files changed, 97 insertions(+), 4 deletions(-)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
index 14d25b89b..d80b866c4 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
@@ -1063,9 +1063,10 @@ void FleetUpdateHandle::Implementation::add_standard_tasks()
     node->clock());
 
   tasks::add_clean(
-    *activation.task,
-    activation.phase,
-    activation.event,
+    dock_param_map,
+    (*planner)->get_configuration().vehicle_traits(),
+    deserialization,
+    activation,
     node->clock());
 
   tasks::add_charge_battery(
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
index d4df5eaa9..d2fea7d07 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
@@ -218,6 +218,8 @@ class FleetUpdateHandle::Implementation
       rmf_fleet_msgs::msg::DockParameter
     >;
 
+  using ConstDockParamsPtr = std::shared_ptr<const DockParamMap>;
+
   // Map of dock name to dock parameters
   std::shared_ptr<DockParamMap> dock_param_map =
     std::make_shared<DockParamMap>();
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp
index 0d8b3906f..231210c5a 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp
@@ -27,6 +27,9 @@
 #include "../events/Error.hpp"
 #include "../events/GoToPlace.hpp"
 
+#include <rmf_fleet_adapter/schemas/event_description_Clean.hpp>
+#include <rmf_fleet_adapter/schemas/task_description_Clean.hpp>
+
 namespace rmf_fleet_adapter {
 namespace tasks {
 
@@ -252,6 +255,8 @@ struct CleanEvent : public rmf_task_sequence::events::Placeholder::Description
 
 //==============================================================================
 void add_clean(
+  const agv::FleetUpdateHandle::Implementation::ConstDockParamsPtr& dock_params,
+  const rmf_traffic::agv::VehicleTraits& traits,
   agv::TaskDeserialization& deserialization,
   agv::TaskActivation& activation,
   std::function<rmf_traffic::Time()> clock)
@@ -259,7 +264,90 @@ void add_clean(
   using Clean = rmf_task::requests::Clean;
   using Phase = rmf_task_sequence::phases::SimplePhase;
 
+  auto validate_clean_event =
+    deserialization.make_validator_shared(
+      schemas::event_description_Clean);
+  deserialization.add_schema(schemas::event_description_Clean);
+
+  auto validate_clean_task =
+    deserialization.make_validator_shared(
+      schemas::task_description_Clean);
+  deserialization.add_schema(schemas::task_description_Clean);
+
+  auto deserialize_clean =
+    [dock_params, traits, place_deser = deserialization.place](
+      const nlohmann::json& msg)
+      -> agv::FleetUpdateHandle::DeserializedTask
+    {
+      const auto zone = msg["zone"].get<std::string>();
+      const auto clean_it = dock_params->find(zone);
+      if (clean_it == dock_params->end())
+      {
+        return {
+          nullptr,
+          {"No cleaning zone named [" + zone + "] for this fleet adapter"}
+        };
+      }
+
+      const auto& clean_info = clean_it->second;
+      auto start_place = place_deser(clean_info.start);
+      auto exit_place = place_deser(clean_info.finish);
+      if (!start_place.description.has_value()
+          || !exit_place.description.has_value())
+      {
+        auto errors = std::move(start_place.errors);
+        errors.insert(
+          errors.end(), exit_place.errors.begin(), exit_place.errors.end());
+        return {nullptr, std::move(errors)};
+      }
+
+      std::vector<Eigen::Vector3d> positions;
+      for (const auto& p : clean_info.path)
+        positions.push_back({p.x, p.y, p.yaw});
 
+      rmf_traffic::Trajectory clean_path =
+        rmf_traffic::agv::Interpolate::positions(
+          traits,
+          rmf_traffic::Time(rmf_traffic::Duration(0)),
+          positions);
+
+      if (clean_path.size() < 2)
+      {
+        return {
+          nullptr,
+          {"Invalid cleaning path for zone named [" + zone
+            + "]: Too few waypoints [" + std::to_string(clean_path.size())
+            + "]"}
+        };
+      }
+
+      // TODO(MXG): Validate the type of cleaning (vacuum, mopping, etc)
+      return {
+        rmf_task::requests::Clean::Description::make(
+          start_place.description->waypoint(),
+          exit_place.description->waypoint(),
+          std::move(clean_path)),
+        {}
+      };
+    };
+  deserialization.task.add("clean", validate_clean_task, deserialize_clean);
+
+  auto deserialize_clean_event =
+    [deserialize_clean](const nlohmann::json& msg)
+      -> agv::FleetUpdateHandle::DeserializedEvent
+    {
+      auto clean_task = deserialize_clean(msg);
+      if (!clean_task.description)
+        return {nullptr, std::move(clean_task.errors)};
+
+      return {
+        std::make_shared<CleanEvent>(
+          static_cast<const Clean::Description&>(*clean_task.description)),
+          std::move(clean_task.errors)
+      };
+    };
+  deserialization.event.add(
+    "clean", validate_clean_event, deserialize_clean_event);
 
   CleanEvent::add(*activation.event);
 
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp
index 20da081fa..80a47e0bf 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp
@@ -44,6 +44,8 @@ std::shared_ptr<LegacyTask> make_clean(
   const rmf_task::State finish_state);
 
 void add_clean(
+  const agv::FleetUpdateHandle::Implementation::ConstDockParamsPtr& dock_params,
+  const rmf_traffic::agv::VehicleTraits& traits,
   agv::TaskDeserialization& deserialization,
   agv::TaskActivation& activation,
   std::function<rmf_traffic::Time()> clock);
diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
index 1932bbed2..7beb57e61 100644
--- a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
+++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
@@ -93,7 +93,7 @@ class Dispatcher::Implementation
     publish_active_tasks_period =
       node->declare_parameter<int>("publish_active_tasks_period", 2);
     RCLCPP_INFO(node->get_logger(),
-      " Declared publish_active_tasks_period as: %f secs",
+      " Declared publish_active_tasks_period as: %d secs",
       publish_active_tasks_period);
 
     const auto qos = rclcpp::ServicesQoS().reliable();

From 12ba59816a580c74b376af590a257f2b2a4e4a68 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Mon, 3 Jan 2022 16:07:31 +0800
Subject: [PATCH 36/79] Add a task description schema for Clean

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 rmf_fleet_adapter/schemas/task_description_Clean.json | 7 +++++++
 1 file changed, 7 insertions(+)
 create mode 100644 rmf_fleet_adapter/schemas/task_description_Clean.json

diff --git a/rmf_fleet_adapter/schemas/task_description_Clean.json b/rmf_fleet_adapter/schemas/task_description_Clean.json
new file mode 100644
index 000000000..d885eaa19
--- /dev/null
+++ b/rmf_fleet_adapter/schemas/task_description_Clean.json
@@ -0,0 +1,7 @@
+{
+  "$schema": "https://json-schema.org/draft/2020-12/schema",
+  "$id": "https://raw.githubusercontent.com/open-rmf/rmf_ros2/main/rmf_fleet_adapter/schemas/task_description_Clean.json",
+  "title": "Clean Task",
+  "description": "Clean a zone",
+  "$ref": "event_description_Clean.json"
+}

From 6eb7c29f443908612b72e37f7261dd2946f12b04 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Mon, 3 Jan 2022 16:18:23 +0800
Subject: [PATCH 37/79] Remember to publish task log updates

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 .../src/rmf_fleet_adapter/TaskManager.cpp             | 11 +++++++++--
 1 file changed, 9 insertions(+), 2 deletions(-)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
index fddda5946..d8c18ed53 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
@@ -498,10 +498,17 @@ void TaskManager::ActiveTask::publish_task_state(TaskManager& mgr)
 
   task_state_update["data"] = _state_msg;
 
-  static const auto validator =
+  static const auto task_update_validator =
     mgr._make_validator(rmf_api_msgs::schemas::task_state_update);
+  mgr._validate_and_publish_websocket(task_state_update, task_update_validator);
+
+  auto task_log_update = nlohmann::json();
+  task_log_update["type"] = "task_log_update";
+  task_log_update["data"] = task_logs;
 
-  mgr._validate_and_publish_websocket(task_state_update, validator);
+  static const auto log_update_validator =
+    mgr._make_validator(rmf_api_msgs::schemas::task_log_update);
+  mgr._validate_and_publish_websocket(task_log_update, log_update_validator);
 }
 
 //==============================================================================

From 943f62fa31dd0f36525f056de80164ba5aed559c Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Mon, 3 Jan 2022 16:26:18 +0800
Subject: [PATCH 38/79] Fix websocket fleet state updates

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 .../src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp    | 10 ++++++----
 .../agv/internal_FleetUpdateHandle.hpp                 |  1 +
 2 files changed, 7 insertions(+), 4 deletions(-)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
index 9487cbaed..95d28b89d 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
@@ -869,12 +869,13 @@ void FleetUpdateHandle::Implementation::update_fleet_state() const
   // Publish to API server
   if (broadcast_client)
   {
-    nlohmann::json fleet_state_msg = {{"name", {}}, {"robots", {}}};
+    nlohmann::json fleet_state_msg;
     fleet_state_msg["name"] = name;
+    auto& robots = fleet_state_msg["robots"];
     for (const auto& [context, mgr] : task_managers)
     {
       const auto& name = context->name();
-      nlohmann::json& json = fleet_state_msg["robots"][name];
+      nlohmann::json& json = robots[name];
       json["name"] = name;
       json["status"] = mgr->robot_status();
       json["task_id"] = mgr->current_task_id().value_or("");
@@ -910,8 +911,9 @@ void FleetUpdateHandle::Implementation::update_fleet_state() const
         value = it->second;
       };
 
-    nlohmann::json fleet_state_update_msg =
-    {{"type", "fleet_state_update"}, {"data", fleet_state_msg}};
+    nlohmann::json fleet_state_update_msg;
+    fleet_state_update_msg["type"] = "fleet_state_update";
+    fleet_state_update_msg["data"] = fleet_state_msg;
     try
     {
       // TODO(MXG): We should make this validator static
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
index 6d7bad26c..60dbf2eb7 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
@@ -251,6 +251,7 @@ class FleetUpdateHandle::Implementation
 
     handle->_pimpl->fleet_state_pub = handle->_pimpl->node->fleet_state();
     handle->fleet_state_topic_publish_period(std::chrono::seconds(1));
+    handle->fleet_state_update_period(std::chrono::seconds(1));
 
     // Create subs and pubs for bidding
     auto default_qos = rclcpp::SystemDefaultsQoS();

From 7cfff092e42e46c4ac2de0ae73870efe282677e3 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Wed, 5 Jan 2022 00:25:46 +0800
Subject: [PATCH 39/79] Reworking the dispatcher to reduce responsibilities and
 eliminate cruft

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 rmf_task_ros2/CMakeLists.txt                  |   3 +-
 .../include/rmf_task_ros2/DispatchState.hpp   |  60 ++++++
 .../include/rmf_task_ros2/Dispatcher.hpp      |  12 +-
 .../include/rmf_task_ros2/TaskStatus.hpp      |  68 -------
 rmf_task_ros2/package.xml                     |   1 +
 rmf_task_ros2/src/mock_bidder/main.cpp        |   6 +-
 .../src/rmf_task_ros2/DispatchState.cpp       |  38 ++++
 .../src/rmf_task_ros2/Dispatcher.cpp          | 184 ++++++++++++++----
 .../src/rmf_task_ros2/TaskStatus.cpp          |  60 ------
 .../src/rmf_task_ros2/action/Client.cpp       |   8 +-
 .../src/rmf_task_ros2/action/Client.hpp       |   2 +-
 .../src/rmf_task_ros2/action/Server.cpp       |   2 +-
 .../src/rmf_task_ros2/action/Server.hpp       |   2 +-
 .../test/action/test_ActionInterface.cpp      |  14 +-
 .../test/dispatcher/test_Dispatcher.cpp       |  22 +--
 15 files changed, 277 insertions(+), 205 deletions(-)
 create mode 100644 rmf_task_ros2/include/rmf_task_ros2/DispatchState.hpp
 delete mode 100644 rmf_task_ros2/include/rmf_task_ros2/TaskStatus.hpp
 create mode 100644 rmf_task_ros2/src/rmf_task_ros2/DispatchState.cpp
 delete mode 100644 rmf_task_ros2/src/rmf_task_ros2/TaskStatus.cpp

diff --git a/rmf_task_ros2/CMakeLists.txt b/rmf_task_ros2/CMakeLists.txt
index 9b04c0641..dd947d557 100644
--- a/rmf_task_ros2/CMakeLists.txt
+++ b/rmf_task_ros2/CMakeLists.txt
@@ -19,6 +19,7 @@ find_package(rmf_traffic_ros2 REQUIRED)
 find_package(rmf_task_msgs REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(nlohmann_json REQUIRED)
 
 file(GLOB_RECURSE core_lib_srcs "src/rmf_task_ros2/*.cpp")
 add_library(rmf_task_ros2 SHARED ${core_lib_srcs})
@@ -41,7 +42,7 @@ target_include_directories(rmf_task_ros2
 )
 
 ament_export_targets(rmf_task_ros2 HAS_LIBRARY_TARGET)
-ament_export_dependencies(rmf_traffic rmf_task_msgs rclcpp)
+ament_export_dependencies(rmf_traffic rmf_task_msgs rclcpp nlohmann_json)
 
 #===============================================================================
 
diff --git a/rmf_task_ros2/include/rmf_task_ros2/DispatchState.hpp b/rmf_task_ros2/include/rmf_task_ros2/DispatchState.hpp
new file mode 100644
index 000000000..ddc04ea6e
--- /dev/null
+++ b/rmf_task_ros2/include/rmf_task_ros2/DispatchState.hpp
@@ -0,0 +1,60 @@
+/*
+ * 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_TASK_ROS2__DISPATCH_STATE_HPP
+#define RMF_TASK_ROS2__DISPATCH_STATE_HPP
+
+#include <unordered_map>
+#include <memory>
+
+namespace rmf_task_ros2 {
+
+//==============================================================================
+using TaskID = std::string;
+
+//==============================================================================
+/// \note TaskStatus struct is based on TaskSummary.msg
+struct DispatchState
+{
+  enum class Status : uint8_t
+  {
+    Queued,
+    Dispatched,
+    FailedToAssign,
+    CanceledInFlight
+  };
+
+  struct Assignment
+  {
+    std::string fleet_name;
+    std::string robot_name;
+  };
+
+  std::string task_id;
+  Status status = Status::Queued;
+  std::optional<Assignment> assignment;
+
+  DispatchState(std::string task_id);
+
+  bool is_terminated() const;
+};
+
+using DispatchStatePtr = std::shared_ptr<DispatchState>;
+
+} // namespace rmf_task_ros2
+
+#endif // RMF_TASK_ROS2__DISPATCH_STATE_HPP
diff --git a/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp b/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp
index f01ebf656..574078913 100644
--- a/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp
+++ b/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp
@@ -24,8 +24,7 @@
 #include <rmf_utils/optional.hpp>
 
 #include <rmf_task_ros2/bidding/Auctioneer.hpp>
-#include <rmf_task_ros2/TaskStatus.hpp>
-
+#include <rmf_task_ros2/DispatchState.hpp>
 
 namespace rmf_task_ros2 {
 
@@ -35,8 +34,7 @@ namespace rmf_task_ros2 {
 class Dispatcher : public std::enable_shared_from_this<Dispatcher>
 {
 public:
-  using DispatchTasks = std::unordered_map<TaskID, TaskStatusPtr>;
-  using TaskDescription = rmf_task_msgs::msg::TaskDescription;
+  using DispatchTasks = std::unordered_map<TaskID, DispatchStatePtr>;
 
   /// Initialize an rclcpp context and make an dispatcher instance. This will
   /// instantiate an rclcpp::Node, a task dispatcher node. Dispatcher node will
@@ -99,7 +97,7 @@ class Dispatcher : public std::enable_shared_from_this<Dispatcher>
   ///   task_id obtained from `submit_task()`
   ///
   /// \return State of the task, nullopt if task is not available
-  const rmf_utils::optional<TaskStatus::State> get_task_state(
+  const rmf_utils::optional<std::string> get_task_state(
     const TaskID& task_id) const;
 
   /// Get a mutable ref of active tasks map list handled by dispatcher
@@ -108,13 +106,13 @@ class Dispatcher : public std::enable_shared_from_this<Dispatcher>
   /// Get a mutable ref of terminated tasks map list
   const DispatchTasks& terminated_tasks() const;
 
-  using StatusCallback = std::function<void(const TaskStatusPtr status)>;
+  using DispatchStateCallback = std::function<void(const DispatchState& status)>;
 
   /// Trigger this callback when a task status is changed. This will return the
   /// Changed task status.
   ///
   /// \param [in] callback function
-  void on_change(StatusCallback on_change_fn);
+  void on_change(DispatchStateCallback on_change_fn);
 
   /// Change the default evaluator to a custom evaluator, which is used by
   /// bidding auctioneer. Default evaluator is: `LeastFleetDiffCostEvaluator`
diff --git a/rmf_task_ros2/include/rmf_task_ros2/TaskStatus.hpp b/rmf_task_ros2/include/rmf_task_ros2/TaskStatus.hpp
deleted file mode 100644
index a8f96fcd6..000000000
--- a/rmf_task_ros2/include/rmf_task_ros2/TaskStatus.hpp
+++ /dev/null
@@ -1,68 +0,0 @@
-/*
- * Copyright (C) 2020 Open Source Robotics Foundation
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *
-*/
-
-#ifndef RMF_TASK_ROS2__TASK_STATUS_HPP
-#define RMF_TASK_ROS2__TASK_STATUS_HPP
-
-#include <unordered_map>
-#include <rmf_traffic/Time.hpp>
-#include <rmf_task_msgs/msg/task_profile.hpp>
-#include <rmf_task_msgs/msg/task_summary.hpp>
-
-namespace rmf_task_ros2 {
-
-//==============================================================================
-using TaskProfile = rmf_task_msgs::msg::TaskProfile;
-using StatusMsg = rmf_task_msgs::msg::TaskSummary;
-using TaskID = std::string;
-
-//==============================================================================
-/// \note TaskStatus struct is based on TaskSummary.msg
-struct TaskStatus
-{
-  enum class State : uint8_t
-  {
-    Queued    = StatusMsg::STATE_QUEUED,
-    Executing = StatusMsg::STATE_ACTIVE,
-    Completed = StatusMsg::STATE_COMPLETED,
-    Failed    = StatusMsg::STATE_FAILED,
-    Canceled  = StatusMsg::STATE_CANCELED,
-    Pending   = StatusMsg::STATE_PENDING
-  };
-
-  std::string fleet_name;
-  TaskProfile task_profile;
-  rmf_traffic::Time start_time;
-  rmf_traffic::Time end_time;
-  std::string robot_name;
-  std::string status; // verbose msg
-  State state = State::Pending; // default
-
-  bool is_terminated() const;
-};
-
-using TaskStatusPtr = std::shared_ptr<TaskStatus>;
-
-// ==============================================================================
-TaskStatus convert_status(const StatusMsg& from);
-
-// ==============================================================================
-StatusMsg convert_status(const TaskStatus& from);
-
-} // namespace rmf_task_ros2
-
-#endif // RMF_TASK_ROS2__TASK_STATUS_HPP
diff --git a/rmf_task_ros2/package.xml b/rmf_task_ros2/package.xml
index 155a2d6c6..e2e69a44f 100644
--- a/rmf_task_ros2/package.xml
+++ b/rmf_task_ros2/package.xml
@@ -15,6 +15,7 @@
   <depend>rmf_traffic_ros2</depend>
   <depend>rmf_task_msgs</depend>
   <depend>rclcpp</depend>
+  <depend>nlohmann-json3-dev</depend>
 
   <build_depend>eigen</build_depend>
 
diff --git a/rmf_task_ros2/src/mock_bidder/main.cpp b/rmf_task_ros2/src/mock_bidder/main.cpp
index 7340c6b30..f13bc87eb 100644
--- a/rmf_task_ros2/src/mock_bidder/main.cpp
+++ b/rmf_task_ros2/src/mock_bidder/main.cpp
@@ -75,7 +75,7 @@ int main(int argc, char* argv[])
       auto t = std::thread(
         [&action_server, &node](auto profile)
         {
-          TaskStatus status;
+          DispatchState status;
           status.task_profile = profile;
           status.robot_name = "dumbot";
           status.start_time = rmf_traffic_ros2::convert(node->now());
@@ -88,12 +88,12 @@ int main(int argc, char* argv[])
 
           std::this_thread::sleep_for(std::chrono::seconds(2));
           std::cout << " [MockBidder] Executing, TaskID: " << id << std::endl;
-          status.state = TaskStatus::State::Executing;
+          status.state = DispatchState::State::Executing;
           action_server->update_status(status);
 
           std::this_thread::sleep_for(std::chrono::seconds(5));
           std::cout << " [MockBidder] Completed, TaskID: " << id << std::endl;
-          status.state = TaskStatus::State::Completed;
+          status.state = DispatchState::State::Completed;
           action_server->update_status(status);
         }, task_profile
       );
diff --git a/rmf_task_ros2/src/rmf_task_ros2/DispatchState.cpp b/rmf_task_ros2/src/rmf_task_ros2/DispatchState.cpp
new file mode 100644
index 000000000..a6849cec3
--- /dev/null
+++ b/rmf_task_ros2/src/rmf_task_ros2/DispatchState.cpp
@@ -0,0 +1,38 @@
+/*
+ * 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_task_ros2/DispatchState.hpp>
+#include <rmf_traffic_ros2/Time.hpp>
+
+namespace rmf_task_ros2 {
+
+// ==============================================================================
+DispatchState::DispatchState(std::string task_id_)
+: task_id(std::move(task_id_))
+{
+  // Do nothing
+}
+
+// ==============================================================================
+bool DispatchState::is_terminated() const
+{
+  return (status == Status::Failed) ||
+    (status == Status::Completed) ||
+    (status == Status::Canceled);
+}
+
+} // namespace rmf_task_ros2
diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
index 7beb57e61..e5d4c386d 100644
--- a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
+++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
@@ -22,13 +22,19 @@
 
 #include "action/Client.hpp"
 
+#include <rmf_task_msgs/msg/task_description.hpp>
+#include <rmf_task_msgs/msg/task_profile.hpp>
 #include <rmf_task_msgs/srv/submit_task.hpp>
 #include <rmf_task_msgs/srv/cancel_task.hpp>
 #include <rmf_task_msgs/srv/get_task_list.hpp>
 #include <rmf_task_msgs/msg/tasks.hpp>
+#include <rmf_task_msgs/msg/api_request.hpp>
+#include <rmf_task_msgs/msg/api_response.hpp>
 
 #include <rmf_traffic_ros2/Time.hpp>
 
+#include <nlohmann/json.hpp>
+
 namespace rmf_task_ros2 {
 
 //==============================================================================
@@ -43,17 +49,23 @@ class Dispatcher::Implementation
   using CancelTaskSrv = rmf_task_msgs::srv::CancelTask;
   using GetTaskListSrv = rmf_task_msgs::srv::GetTaskList;
   using TasksMsg = rmf_task_msgs::msg::Tasks;
+  using TaskDescription = rmf_task_msgs::msg::TaskDescription;
 
   rclcpp::Service<SubmitTaskSrv>::SharedPtr submit_task_srv;
   rclcpp::Service<CancelTaskSrv>::SharedPtr cancel_task_srv;
   rclcpp::Service<GetTaskListSrv>::SharedPtr get_task_list_srv;
 
+  using ApiRequest = rmf_task_msgs::msg::ApiRequest;
+  using ApiResponse = rmf_task_msgs::msg::ApiResponse;
+  rclcpp::Subscription<ApiRequest>::SharedPtr api_request;
+  rclcpp::Publisher<ApiResponse>::SharedPtr api_response;
+
   using ActiveTasksPub = rclcpp::Publisher<TasksMsg>;
   ActiveTasksPub::SharedPtr ongoing_tasks_pub;
 
   rclcpp::TimerBase::SharedPtr timer;
 
-  StatusCallback on_change_fn;
+  DispatchStateCallback on_change_fn;
 
   std::queue<bidding::BidNotice> queue_bidding_tasks;
 
@@ -67,16 +79,17 @@ class Dispatcher::Implementation
   int terminated_tasks_max_size;
   int publish_active_tasks_period;
 
-  std::unordered_map<std::size_t, std::string> task_type_name =
+  std::unordered_map<std::size_t, std::string> legacy_task_type_names =
   {
-    {0, "Station"},
-    {1, "Loop"},
-    {2, "Delivery"},
-    {3, "ChargeBattery"},
-    {4, "Clean"},
-    {5, "Patrol"}
+    {1, "patrol"},
+    {2, "delivery"},
+    {4, "patrol"}
   };
 
+  using LegacyConversion = std::function<std::string(const TaskDescription&)>;
+  using LegacyConversionMap = std::unordered_map<std::string, LegacyConversion>;
+  LegacyConversionMap legacy_task_types;
+
   Implementation(std::shared_ptr<rclcpp::Node> node_)
   : node{std::move(node_)}
   {
@@ -157,6 +170,65 @@ class Dispatcher::Implementation
         response->success = true;
       }
     );
+
+    // Loop
+    legacy_task_types["patrol"] =
+      [](const TaskDescription& task_description) -> std::string
+      {
+        const auto& loop = task_description.loop;
+        nlohmann::json description;
+        std::vector<std::string> places;
+        places.push_back(loop.start_name);
+        places.push_back(loop.finish_name);
+        description["places"] = std::move(places);
+        description["rounds"] = loop.num_loops;
+
+        return description.dump();
+      };
+
+    // Delivery
+    legacy_task_types["delivery"] =
+      [](const TaskDescription& task_description) -> std::string
+      {
+        const auto& delivery = task_description.delivery;
+        std::vector<nlohmann::json> payload;
+        payload.reserve(delivery.items.size());
+        for (const auto& item : delivery.items)
+        {
+          nlohmann::json item_json;
+          item_json["sku"] = item.type_guid;
+          item_json["quantity"] = item.quantity;
+          item_json["compartment"] = item.compartment_name;
+          payload.push_back(item_json);
+        }
+
+        nlohmann::json pickup;
+        pickup["place"] = delivery.pickup_place_name;
+        pickup["handler"] = delivery.pickup_dispenser;
+        pickup["payload"] = payload;
+
+        nlohmann::json dropoff;
+        dropoff["place"] = delivery.dropoff_place_name;
+        dropoff["handler"] = delivery.dropoff_ingestor;
+        dropoff["payload"] = payload;
+
+        nlohmann::json description;
+        description["pickup"] = pickup;
+        description["dropoff"] = dropoff;
+
+        return description.dump();
+      };
+
+    // Clean
+    legacy_task_types["clean"] =
+      [](const TaskDescription& task_description) -> std::string
+      {
+        const auto& clean = task_description.clean;
+        nlohmann::json description;
+        description["zone"] = clean.start_waypoint;
+
+        return description.dump();
+      };
   }
 
   void start()
@@ -170,47 +242,77 @@ class Dispatcher::Implementation
       std::bind(&Implementation::task_status_cb, this, _1));
   }
 
-  std::optional<TaskID> submit_task(const TaskDescription& description)
+  std::optional<TaskID> submit_task(const TaskDescription& submission)
   {
-    TaskProfile submitted_task;
-    submitted_task.submission_time = node->now();
-    submitted_task.description = description;
-
-    const auto task_type = static_cast<std::size_t>(description.task_type.type);
-
-    if (!task_type_name.count(task_type))
+    const auto task_type_index = submission.task_type.type;
+    const auto desc_it = legacy_task_type_names.find(task_type_index);
+    if (desc_it == legacy_task_type_names.end())
     {
-      RCLCPP_ERROR(node->get_logger(), "TaskType: %ld is invalid", task_type);
+      RCLCPP_ERROR(
+        node->get_logger(), "TaskType: %u is invalid", task_type_index);
       return std::nullopt;
     }
 
+    const std::string category = desc_it->second;
+
     // auto generate a task_id for a given submitted task
-    submitted_task.task_id =
-      task_type_name[task_type] + std::to_string(task_counter++);
+    const auto task_id = "dispatch#" + std::to_string(task_counter++);
 
     RCLCPP_INFO(node->get_logger(),
-      "Received Task Submission [%s]", submitted_task.task_id.c_str());
+      "Received Task Submission [%s]", task_id.c_str());
 
-    // add task to internal cache
-    TaskStatus status;
-    status.task_profile = submitted_task;
-    auto new_task_status = std::make_shared<TaskStatus>(status);
-    active_dispatch_tasks[submitted_task.task_id] = new_task_status;
-    user_submitted_tasks.insert(submitted_task.task_id);
+    nlohmann::json task_request;
+    task_request["unix_millis_earliest_start_time"] =
+      std::chrono::duration_cast<std::chrono::milliseconds>(
+        rmf_traffic_ros2::convert(submission.start_time).time_since_epoch())
+        .count();
 
-    if (on_change_fn)
-      on_change_fn(new_task_status);
+    auto& priority = task_request["priority"];
+    priority["type"] = "binary";
+    priority["value"] = submission.priority.value;
+    task_request["category"] = category;
+    task_request["description"] = legacy_task_types.at(category)(submission);
+    task_request["labels"] = std::vector<std::string>({"legacy_request"});
 
     bidding::BidNotice bid_notice;
-    bid_notice.task_profile = submitted_task;
+    bid_notice.request = task_request.dump();
     bid_notice.time_window = rmf_traffic_ros2::convert(
       rmf_traffic::time::from_seconds(bidding_time_window));
+    push_bid_notice(std::move(bid_notice));
+
+    return task_id;
+  }
+
+  void push_bid_notice(bidding::BidNotice bid_notice)
+  {
+    nlohmann::json state;
+    auto& booking = state["booking"];
+    booking["id"] = bid_notice.task_id;
+
+    const auto request = nlohmann::json::parse(bid_notice.request);
+    static const std::vector<std::string> copy_fields = {
+        "unix_millis_earliest_start_time",
+        "priority",
+        "labels"
+      };
+
+    for (const auto& field : copy_fields)
+      booking[field] = request.at(field);
+
+    state["category"] = request["category"];
+    state["detail"] = request["description"];
+
+    if (on_change_fn)
+      on_change_fn(state);
+
+    // add task to internal cache
+    active_dispatch_tasks[bid_notice.task_id] = new_task_status;
+    user_submitted_tasks.insert(submitted_task.task_id);
+
     queue_bidding_tasks.push(bid_notice);
 
     if (queue_bidding_tasks.size() == 1)
       auctioneer->start_bidding(queue_bidding_tasks.front());
-
-    return submitted_task.task_id;
   }
 
   bool cancel_task(const TaskID& task_id)
@@ -228,9 +330,9 @@ class Dispatcher::Implementation
 
     // Cancel bidding. This will remove the bidding process
     const auto& cancel_task_status = it->second;
-    if (cancel_task_status->state == TaskStatus::State::Pending)
+    if (cancel_task_status->state == DispatchState::State::Pending)
     {
-      cancel_task_status->state = TaskStatus::State::Canceled;
+      cancel_task_status->state = DispatchState::State::Canceled;
       terminate_task(cancel_task_status);
 
       if (on_change_fn)
@@ -248,7 +350,7 @@ class Dispatcher::Implementation
     }
 
     // Curently cancel can only work on Queued Task in Fleet Adapter
-    if (cancel_task_status->state != TaskStatus::State::Queued)
+    if (cancel_task_status->state != DispatchState::State::Queued)
     {
       RCLCPP_ERROR(node->get_logger(),
         "Unable to cancel task [%s] as it is not a Queued Task",
@@ -269,7 +371,7 @@ class Dispatcher::Implementation
 
       if (is_self_gererated && is_fleet_name)
       {
-        it->second->state = TaskStatus::State::Canceled;
+        it->second->state = DispatchState::State::Canceled;
         terminate_task((it++)->second);
       }
       else
@@ -282,7 +384,7 @@ class Dispatcher::Implementation
     return action_client->cancel_task(cancel_task_status->task_profile);
   }
 
-  const std::optional<TaskStatus::State> get_task_state(
+  const std::optional<DispatchState::State> get_task_state(
     const TaskID& task_id) const
   {
     // check if taskid exists in active tasks
@@ -311,7 +413,7 @@ class Dispatcher::Implementation
     {
       RCLCPP_WARN(node->get_logger(), "Dispatcher Bidding Result: task [%s]"
         " has no submissions during bidding, Task Failed", task_id.c_str());
-      pending_task_status->state = TaskStatus::State::Failed;
+      pending_task_status->state = DispatchState::State::Failed;
       terminate_task(pending_task_status);
 
       if (on_change_fn)
@@ -342,7 +444,7 @@ class Dispatcher::Implementation
 
       if (is_self_gererated && is_fleet_name)
       {
-        it->second->state = TaskStatus::State::Canceled;
+        it->second->state = DispatchState::State::Canceled;
         terminate_task((it++)->second);
       }
       else
@@ -381,7 +483,7 @@ class Dispatcher::Implementation
     const auto id = terminate_status->task_profile.task_id;
 
     // destroy prev status ptr and recreate one
-    auto status = std::make_shared<TaskStatus>(*terminate_status);
+    auto status = std::make_shared<DispatchState>(*terminate_status);
     (terminal_dispatch_tasks)[id] = status;
     user_submitted_tasks.erase(id);
     active_dispatch_tasks.erase(id);
@@ -469,7 +571,7 @@ bool Dispatcher::cancel_task(const TaskID& task_id)
 }
 
 //==============================================================================
-const std::optional<TaskStatus::State> Dispatcher::get_task_state(
+const rmf_utils::optional<std::string> Dispatcher::get_task_state(
   const TaskID& task_id) const
 {
   return _pimpl->get_task_state(task_id);
@@ -488,7 +590,7 @@ const Dispatcher::DispatchTasks& Dispatcher::terminated_tasks() const
 }
 
 //==============================================================================
-void Dispatcher::on_change(StatusCallback on_change_fn)
+void Dispatcher::on_change(TaskStateCallback on_change_fn)
 {
   _pimpl->on_change_fn = on_change_fn;
 }
diff --git a/rmf_task_ros2/src/rmf_task_ros2/TaskStatus.cpp b/rmf_task_ros2/src/rmf_task_ros2/TaskStatus.cpp
deleted file mode 100644
index cd06e3c94..000000000
--- a/rmf_task_ros2/src/rmf_task_ros2/TaskStatus.cpp
+++ /dev/null
@@ -1,60 +0,0 @@
-/*
- * Copyright (C) 2020 Open Source Robotics Foundation
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *
-*/
-
-#include <rmf_task_ros2/TaskStatus.hpp>
-#include <rmf_traffic_ros2/Time.hpp>
-
-namespace rmf_task_ros2 {
-
-// ==============================================================================
-bool TaskStatus::is_terminated() const
-{
-  return (state == State::Failed) ||
-    (state == State::Completed) ||
-    (state == State::Canceled);
-}
-
-// ==============================================================================
-TaskStatus convert_status(const StatusMsg& from)
-{
-  TaskStatus status;
-  status.fleet_name = from.fleet_name;
-  status.task_profile = from.task_profile;
-  status.start_time = rmf_traffic_ros2::convert(from.start_time);
-  status.end_time = rmf_traffic_ros2::convert(from.end_time);
-  status.robot_name = from.robot_name;
-  status.status = from.status;
-  status.state = static_cast<TaskStatus::State>(from.state);
-  return status;
-}
-
-// ==============================================================================
-StatusMsg convert_status(const TaskStatus& from)
-{
-  StatusMsg status;
-  status.fleet_name = from.fleet_name;
-  status.task_id = from.task_profile.task_id;  // duplication
-  status.task_profile = from.task_profile;
-  status.start_time = rmf_traffic_ros2::convert(from.start_time);
-  status.end_time = rmf_traffic_ros2::convert(from.end_time);
-  status.robot_name = from.robot_name;
-  status.status = from.status;
-  status.state = static_cast<uint32_t>(from.state);
-  return status;
-}
-
-} // namespace rmf_task_ros2
diff --git a/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp b/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp
index 1c2069d2b..e2b376426 100644
--- a/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp
+++ b/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp
@@ -73,7 +73,7 @@ Client::Client(std::shared_ptr<rclcpp::Node> node)
         // will still provide onchange even if the task_id is unknown.
         RCLCPP_DEBUG(_node->get_logger(),
         "[action] Unknown task: [%s]", task_id.c_str());
-        auto task_status = std::make_shared<TaskStatus>(convert_status(*msg));
+        auto task_status = std::make_shared<DispatchState>(convert_status(*msg));
         _active_task_status[task_id] = task_status;
         update_task_status(task_status);
       }
@@ -95,7 +95,7 @@ Client::Client(std::shared_ptr<rclcpp::Node> node)
             RCLCPP_INFO(_node->get_logger(),
             "Received dispatch ack from fleet [%s] that task [%s] is queued",
             msg->dispatch_request.fleet_name.c_str(), task_id.c_str());
-            weak_status->state = TaskStatus::State::Queued;
+            weak_status->state = DispatchState::State::Queued;
           }
           else
           {
@@ -103,7 +103,7 @@ Client::Client(std::shared_ptr<rclcpp::Node> node)
             RCLCPP_ERROR(_node->get_logger(),
             "Received dispatch ack from fleet [%s] that task [%s] Add Failed",
             msg->dispatch_request.fleet_name.c_str(), task_id.c_str());
-            weak_status->state = TaskStatus::State::Failed;
+            weak_status->state = DispatchState::State::Failed;
           }
           break;
         case RequestMsg::CANCEL:
@@ -113,7 +113,7 @@ Client::Client(std::shared_ptr<rclcpp::Node> node)
             RCLCPP_INFO(_node->get_logger(),
             "Received dispatch ack from fleet [%s] that task [%s] is canceled",
             msg->dispatch_request.fleet_name.c_str(), task_id.c_str());
-            weak_status->state = TaskStatus::State::Canceled;
+            weak_status->state = DispatchState::State::Canceled;
           }
           break;
         default:
diff --git a/rmf_task_ros2/src/rmf_task_ros2/action/Client.hpp b/rmf_task_ros2/src/rmf_task_ros2/action/Client.hpp
index d39f1fbba..4d4a633a6 100644
--- a/rmf_task_ros2/src/rmf_task_ros2/action/Client.hpp
+++ b/rmf_task_ros2/src/rmf_task_ros2/action/Client.hpp
@@ -101,7 +101,7 @@ class Client
   std::shared_ptr<rclcpp::Node> _node;
   StatusCallback _on_change_callback;
   StatusCallback _on_terminate_callback;
-  std::unordered_map<TaskID, std::weak_ptr<TaskStatus>> _active_task_status;
+  std::unordered_map<TaskID, std::weak_ptr<DispatchState>> _active_task_status;
   rclcpp::Publisher<RequestMsg>::SharedPtr _request_msg_pub;
   rclcpp::Subscription<StatusMsg>::SharedPtr _status_msg_sub;
   rclcpp::Subscription<AckMsg>::SharedPtr _ack_msg_sub;
diff --git a/rmf_task_ros2/src/rmf_task_ros2/action/Server.cpp b/rmf_task_ros2/src/rmf_task_ros2/action/Server.cpp
index 87130837a..4540d56fc 100644
--- a/rmf_task_ros2/src/rmf_task_ros2/action/Server.cpp
+++ b/rmf_task_ros2/src/rmf_task_ros2/action/Server.cpp
@@ -38,7 +38,7 @@ void Server::register_callbacks(
 
 //==============================================================================
 void Server::update_status(
-  const TaskStatus& task_status)
+  const DispatchState& task_status)
 {
   auto msg = convert_status(task_status);
   msg.fleet_name = _fleet_name;
diff --git a/rmf_task_ros2/src/rmf_task_ros2/action/Server.hpp b/rmf_task_ros2/src/rmf_task_ros2/action/Server.hpp
index d8c0150a6..63e191383 100644
--- a/rmf_task_ros2/src/rmf_task_ros2/action/Server.hpp
+++ b/rmf_task_ros2/src/rmf_task_ros2/action/Server.hpp
@@ -69,7 +69,7 @@ class Server
   ///
   /// \param[in] task_status
   ///   latest status of the task
-  void update_status(const TaskStatus& task_status);
+  void update_status(const DispatchState& task_status);
 
 private:
   Server(
diff --git a/rmf_task_ros2/test/action/test_ActionInterface.cpp b/rmf_task_ros2/test/action/test_ActionInterface.cpp
index 297d40aff..d4c77374c 100644
--- a/rmf_task_ros2/test/action/test_ActionInterface.cpp
+++ b/rmf_task_ros2/test/action/test_ActionInterface.cpp
@@ -95,14 +95,14 @@ SCENARIO("Action communication with client and server", "[ActionInterface]")
       rmf_traffic::time::from_seconds(0.5));
 
     // should not receive cuz incorrect serverid
-    REQUIRE(status_ptr->state == TaskStatus::State::Pending);
+    REQUIRE(status_ptr->state == DispatchState::State::Pending);
 
     action_client->add_task("test_server", task_profile1, status_ptr);
     executor.spin_until_future_complete(ready_future,
       rmf_traffic::time::from_seconds(0.5));
 
     // check status
-    REQUIRE(status_ptr->state == TaskStatus::State::Queued);
+    REQUIRE(status_ptr->state == DispatchState::State::Queued);
 
     // status ptr is destroyed, should not have anymore tracking
     status_ptr.reset();
@@ -169,22 +169,22 @@ SCENARIO("Action communication with client and server", "[ActionInterface]")
 
     REQUIRE(test_task_onchange.get());
     REQUIRE(test_task_onchange->has_value());
-    CHECK((*test_task_onchange)->state == TaskStatus::State::Queued);
+    CHECK((*test_task_onchange)->state == DispatchState::State::Queued);
 
     TaskStatus server_task;
     server_task.task_profile = task_profile1;
-    server_task.state = TaskStatus::State::Executing;
+    server_task.state = DispatchState::State::Executing;
 
     // Update it as executing
     action_server->update_status(server_task);
     executor.spin_until_future_complete(ready_future,
       rmf_traffic::time::from_seconds(1.5));
 
-    CHECK((*test_task_onchange)->state == TaskStatus::State::Executing);
+    CHECK((*test_task_onchange)->state == DispatchState::State::Executing);
     CHECK(!test_task_onterminate->has_value());
 
     // completion
-    server_task.state = TaskStatus::State::Completed;
+    server_task.state = DispatchState::State::Completed;
     // Update it as executing
     action_server->update_status(server_task);
     executor.spin_until_future_complete(ready_future,
@@ -192,7 +192,7 @@ SCENARIO("Action communication with client and server", "[ActionInterface]")
 
     REQUIRE(test_task_onterminate.get());
     REQUIRE(test_task_onterminate->has_value());
-    CHECK((*test_task_onterminate)->state == TaskStatus::State::Completed);
+    CHECK((*test_task_onterminate)->state == DispatchState::State::Completed);
   }
 
   rclcpp::shutdown(rcl_context);
diff --git a/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp b/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp
index 2fbb532b8..66649e483 100644
--- a/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp
+++ b/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp
@@ -78,7 +78,7 @@ SCENARIO("Dispatcher API Test", "[Dispatcher][.flaky]")
     REQUIRE(id.has_value());
     REQUIRE(dispatcher->active_tasks().size() == 1);
     REQUIRE(dispatcher->terminated_tasks().size() == 0);
-    REQUIRE(dispatcher->get_task_state(*id) == TaskStatus::State::Pending);
+    REQUIRE(dispatcher->get_task_state(*id) == DispatchState::State::Pending);
 
     // cancel task
     REQUIRE(dispatcher->cancel_task(*id));
@@ -116,11 +116,11 @@ SCENARIO("Dispatcher API Test", "[Dispatcher][.flaky]")
     // Submit first task and wait for bidding
     auto id = dispatcher->submit_task(task_desc1);
     REQUIRE(dispatcher->active_tasks().size() == 1);
-    REQUIRE(dispatcher->get_task_state(*id) == TaskStatus::State::Pending);
+    REQUIRE(dispatcher->get_task_state(*id) == DispatchState::State::Pending);
 
     // Default 2s timeout, wait 3s for timetout, should fail here
     std::this_thread::sleep_for(std::chrono::milliseconds(3500));
-    CHECK(dispatcher->get_task_state(*id) == TaskStatus::State::Failed);
+    CHECK(dispatcher->get_task_state(*id) == DispatchState::State::Failed);
     REQUIRE(dispatcher->terminated_tasks().size() == 1);
     // TODO(MXG): Flake out after previous line: SIGABRT
     REQUIRE(test_taskprofile->task_id == id);
@@ -189,12 +189,12 @@ SCENARIO("Dispatcher API Test", "[Dispatcher][.flaky]")
           }
 
           // Executing
-          status.state = TaskStatus::State::Executing;
+          status.state = DispatchState::State::Executing;
           action_server->update_status(status);
           std::this_thread::sleep_for(std::chrono::seconds(1));
 
           // Completed
-          status.state = TaskStatus::State::Completed;
+          status.state = DispatchState::State::Completed;
           action_server->update_status(status);
         }, task_profile
       );
@@ -213,16 +213,16 @@ SCENARIO("Dispatcher API Test", "[Dispatcher][.flaky]")
   WHEN("Full Dispatch cycle")
   {
     const auto id = dispatcher->submit_task(task_desc1);
-    CHECK(dispatcher->get_task_state(*id) == TaskStatus::State::Pending);
+    CHECK(dispatcher->get_task_state(*id) == DispatchState::State::Pending);
     std::this_thread::sleep_for(std::chrono::milliseconds(3500));
 
     // now should queue the task
-    CHECK(dispatcher->get_task_state(*id) == TaskStatus::State::Queued);
+    CHECK(dispatcher->get_task_state(*id) == DispatchState::State::Queued);
     REQUIRE(dispatcher->terminated_tasks().size() == 0);
     CHECK(*change_times == 2); // Pending and Queued
 
     std::this_thread::sleep_for(std::chrono::seconds(3));
-    CHECK(dispatcher->get_task_state(*id) == TaskStatus::State::Completed);
+    CHECK(dispatcher->get_task_state(*id) == DispatchState::State::Completed);
     REQUIRE(dispatcher->active_tasks().size() == 0);
     REQUIRE(dispatcher->terminated_tasks().size() == 1);
     CHECK(*change_times == 4); // Pending > Queued > Executing > Completed
@@ -230,7 +230,7 @@ SCENARIO("Dispatcher API Test", "[Dispatcher][.flaky]")
     // Add auto generated ChargeBattery Task from fleet adapter
     TaskStatus status;
     status.task_profile.task_id = "ChargeBattery10";
-    status.state = TaskStatus::State::Queued;
+    status.state = DispatchState::State::Queued;
     status.task_profile.description.task_type.type =
       rmf_task_msgs::msg::TaskType::TYPE_CHARGE_BATTERY;
     action_server->update_status(status);
@@ -243,7 +243,7 @@ SCENARIO("Dispatcher API Test", "[Dispatcher][.flaky]")
   WHEN("Half way cancel Dispatch cycle")
   {
     const auto id = dispatcher->submit_task(task_desc2);
-    CHECK(dispatcher->get_task_state(*id) == TaskStatus::State::Pending);
+    CHECK(dispatcher->get_task_state(*id) == DispatchState::State::Pending);
     REQUIRE(dispatcher->active_tasks().size() == 1);
     std::this_thread::sleep_for(std::chrono::milliseconds(3000));
 
@@ -255,7 +255,7 @@ SCENARIO("Dispatcher API Test", "[Dispatcher][.flaky]")
     REQUIRE(dispatcher->terminated_tasks().size() == 1);
     REQUIRE(dispatcher->terminated_tasks().begin()->first == *id);
     auto status = dispatcher->terminated_tasks().begin()->second;
-    CHECK(status->state == TaskStatus::State::Canceled);
+    CHECK(status->state == DispatchState::State::Canceled);
     CHECK(*change_times == 3); // Pending -> Queued -> Canceled
   }
 

From 4f430461778236d0eb2e5b5203d2078481e40ba7 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Wed, 5 Jan 2022 00:28:01 +0800
Subject: [PATCH 40/79] Remember to put task_id into task log update

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
index d8c18ed53..53e9a9be6 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
@@ -436,6 +436,7 @@ void TaskManager::ActiveTask::publish_task_state(TaskManager& mgr)
   auto& phases = _state_msg["phases"];
 
   nlohmann::json task_logs;
+  task_logs["task_id"] = booking.id();
   auto& phase_logs = task_logs["phases"];
 
   std::vector<uint64_t> completed_ids;

From 750699e67ce851db37b05629e1e22e5e395f6455 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Thu, 6 Jan 2022 15:55:19 +0800
Subject: [PATCH 41/79] Refactoring dispatch node -- finished Dispatcher class

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 .../src/rmf_fleet_adapter/TaskManager.cpp     |   2 +-
 .../include/rmf_task_ros2/DispatchState.hpp   |  48 +-
 .../include/rmf_task_ros2/Dispatcher.hpp      |  17 +-
 .../include/rmf_task_ros2/StandardNames.hpp   |   6 +-
 .../rmf_task_ros2/bidding/Auctioneer.hpp      |   7 +-
 .../src/rmf_task_ros2/DispatchState.cpp       |  52 +-
 .../src/rmf_task_ros2/Dispatcher.cpp          | 618 ++++++++++++------
 .../src/rmf_task_ros2/action/Client.cpp       |   4 +-
 .../src/rmf_task_ros2/action/Client.hpp       |   2 +-
 .../src/rmf_task_ros2/action/Server.cpp       |   4 +-
 .../src/rmf_task_ros2/action/Server.hpp       |   2 +-
 .../src/rmf_task_ros2/bidding/Auctioneer.cpp  |  26 +-
 .../bidding/internal_Auctioneer.hpp           |   6 +-
 13 files changed, 528 insertions(+), 266 deletions(-)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
index 53e9a9be6..f4c62c5e1 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
@@ -1319,7 +1319,7 @@ void TaskManager::_publish_task_queue()
     pending_json["original_estimate_millis"] =
       std::max(0l, to_millis(estimate).count());
     copy_assignment(pending_json["assigned_to"], *_context);
-    pending_json["status"] = "standby";
+    pending_json["status"] = "queued";
 
     auto task_state_update = _task_state_update_json;
     task_state_update["data"] = pending_json;
diff --git a/rmf_task_ros2/include/rmf_task_ros2/DispatchState.hpp b/rmf_task_ros2/include/rmf_task_ros2/DispatchState.hpp
index ddc04ea6e..151129478 100644
--- a/rmf_task_ros2/include/rmf_task_ros2/DispatchState.hpp
+++ b/rmf_task_ros2/include/rmf_task_ros2/DispatchState.hpp
@@ -21,6 +21,11 @@
 #include <unordered_map>
 #include <memory>
 
+#include <rmf_traffic_ros2/Time.hpp>
+#include <rmf_task_msgs/msg/dispatch_state.hpp>
+
+#include <nlohmann/json.hpp>
+
 namespace rmf_task_ros2 {
 
 //==============================================================================
@@ -30,31 +35,60 @@ using TaskID = std::string;
 /// \note TaskStatus struct is based on TaskSummary.msg
 struct DispatchState
 {
+  using Msg = rmf_task_msgs::msg::DispatchState;
+
   enum class Status : uint8_t
   {
-    Queued,
-    Dispatched,
-    FailedToAssign,
-    CanceledInFlight
+    /// This task has not been assigned yet
+    Queued = Msg::STATUS_QUEUED,
+
+    /// An assignment has been selected, but we have not received acknowledgment
+    /// from the assignee
+    Selected = Msg::STATUS_SELECTED,
+
+    /// The task is dispatched and no longer being managed by the dispatcher
+    Dispatched = Msg::STATUS_DISPATCHED,
+
+    /// There was a failure to assign the task to anyone
+    FailedToAssign = Msg::STATUS_FAILED_TO_ASSIGN,
+
+    /// The task was canceled before it managed to get dispatched
+    CanceledInFlight = Msg::STATUS_CANCELED_IN_FLIGHT
   };
 
   struct Assignment
   {
     std::string fleet_name;
-    std::string robot_name;
+    std::string expected_robot_name;
   };
 
+  /// The Task ID for that this dispatching refers to
   std::string task_id;
+
+  /// Submission arrival time
+  rmf_traffic::Time submission_time;
+
+  /// The status of this dispatching
   Status status = Status::Queued;
+
+  /// The assignment that was made for this dispatching
   std::optional<Assignment> assignment;
 
-  DispatchState(std::string task_id);
+  /// Any errors that have occurred for this dispatching
+  std::vector<nlohmann::json> errors;
 
-  bool is_terminated() const;
+  DispatchState(std::string task_id, rmf_traffic::Time submission_time);
 };
 
 using DispatchStatePtr = std::shared_ptr<DispatchState>;
 
+//==============================================================================
+rmf_task_msgs::msg::Assignment convert(
+  const std::optional<DispatchState::Assignment>& assignment);
+
+//==============================================================================
+rmf_task_msgs::msg::DispatchState convert(const DispatchState& state);
+
 } // namespace rmf_task_ros2
 
 #endif // RMF_TASK_ROS2__DISPATCH_STATE_HPP
diff --git a/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp b/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp
index 574078913..53d27dfe4 100644
--- a/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp
+++ b/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp
@@ -26,6 +26,9 @@
 #include <rmf_task_ros2/bidding/Auctioneer.hpp>
 #include <rmf_task_ros2/DispatchState.hpp>
 
+// Deprecated message definition
+#include <rmf_task_msgs/msg/task_description.hpp>
+
 namespace rmf_task_ros2 {
 
 //==============================================================================
@@ -34,7 +37,7 @@ namespace rmf_task_ros2 {
 class Dispatcher : public std::enable_shared_from_this<Dispatcher>
 {
 public:
-  using DispatchTasks = std::unordered_map<TaskID, DispatchStatePtr>;
+  using DispatchStates = std::unordered_map<TaskID, DispatchStatePtr>;
 
   /// Initialize an rclcpp context and make an dispatcher instance. This will
   /// instantiate an rclcpp::Node, a task dispatcher node. Dispatcher node will
@@ -76,8 +79,9 @@ class Dispatcher : public std::enable_shared_from_this<Dispatcher>
   ///
   /// \return task_id
   ///   self-generated task_id, nullopt is submit task failed
+  [[deprecated]]
   std::optional<TaskID> submit_task(
-    const TaskDescription& task_description);
+    const rmf_task_msgs::msg::TaskDescription& task_description);
 
   /// Cancel an active task which was previously submitted to Dispatcher. This
   /// will terminate the task with a State of: `Canceled`. If a task is
@@ -97,16 +101,17 @@ class Dispatcher : public std::enable_shared_from_this<Dispatcher>
   ///   task_id obtained from `submit_task()`
   ///
   /// \return State of the task, nullopt if task is not available
-  const rmf_utils::optional<std::string> get_task_state(
+  std::optional<DispatchState> get_dispatch_state(
     const TaskID& task_id) const;
 
   /// Get a mutable ref of active tasks map list handled by dispatcher
-  const DispatchTasks& active_tasks() const;
+  const DispatchStates& active_dispatches() const;
 
   /// Get a mutable ref of terminated tasks map list
-  const DispatchTasks& terminated_tasks() const;
+  const DispatchStates& finished_dispatches() const;
 
-  using DispatchStateCallback = std::function<void(const DispatchState& status)>;
+  using DispatchStateCallback =
+    std::function<void(const DispatchState& status)>;
 
   /// Trigger this callback when a task status is changed. This will return the
   /// Changed task status.
diff --git a/rmf_task_ros2/include/rmf_task_ros2/StandardNames.hpp b/rmf_task_ros2/include/rmf_task_ros2/StandardNames.hpp
index 6d1ac8338..c8aa6f9a5 100644
--- a/rmf_task_ros2/include/rmf_task_ros2/StandardNames.hpp
+++ b/rmf_task_ros2/include/rmf_task_ros2/StandardNames.hpp
@@ -30,10 +30,10 @@ const std::string SubmitTaskSrvName = "submit_task";
 const std::string CancelTaskSrvName = "cancel_task";
 const std::string GetTaskListSrvName = "get_tasks";
 
-const std::string TaskRequestTopicName = Prefix + "dispatch_request";
-const std::string TaskAckTopicName = Prefix + "dispatch_ack";
+const std::string DispatchCommandTopicName = Prefix + "dispatch_request";
+const std::string DispatchAckTopicName = Prefix + "dispatch_ack";
+const std::string DispatchStatesTopicName = "dispatch_states";
 const std::string TaskStatusTopicName = "task_summaries";
-const std::string ActiveTasksTopicName = "dispatcher_ongoing_tasks";
 
 } // namespace rmf_task_ros2
 
diff --git a/rmf_task_ros2/include/rmf_task_ros2/bidding/Auctioneer.hpp b/rmf_task_ros2/include/rmf_task_ros2/bidding/Auctioneer.hpp
index 4b9b5a87d..47ef29c9d 100644
--- a/rmf_task_ros2/include/rmf_task_ros2/bidding/Auctioneer.hpp
+++ b/rmf_task_ros2/include/rmf_task_ros2/bidding/Auctioneer.hpp
@@ -42,8 +42,11 @@ class Auctioneer : public std::enable_shared_from_this<Auctioneer>
   /// \param[in] winner
   ///   single winner from all submissions. nullopt if non
   using BiddingResultCallback =
-    std::function<void( const std::string& task_id,
-      const std::optional<Submission> winner)>;
+    std::function<
+      void(
+        const std::string& task_id,
+        const std::optional<Submission> winner,
+        const std::vector<std::string>& errors)>;
 
   /// Create an instance of the Auctioneer. This instance will handle all
   /// the task dispatching bidding mechanism. A default evaluator is used.
diff --git a/rmf_task_ros2/src/rmf_task_ros2/DispatchState.cpp b/rmf_task_ros2/src/rmf_task_ros2/DispatchState.cpp
index a6849cec3..b2f2ba2b2 100644
--- a/rmf_task_ros2/src/rmf_task_ros2/DispatchState.cpp
+++ b/rmf_task_ros2/src/rmf_task_ros2/DispatchState.cpp
@@ -18,21 +18,57 @@
 #include <rmf_task_ros2/DispatchState.hpp>
 #include <rmf_traffic_ros2/Time.hpp>
 
+#include <rmf_task_msgs/msg/assignment.hpp>
+
 namespace rmf_task_ros2 {
 
-// ==============================================================================
-DispatchState::DispatchState(std::string task_id_)
-: task_id(std::move(task_id_))
+//==============================================================================
+DispatchState::DispatchState(
+  std::string task_id_,
+  rmf_traffic::Time submission_time_)
+: task_id(std::move(task_id_)),
+  submission_time(submission_time_)
 {
   // Do nothing
 }
 
-// ==============================================================================
-bool DispatchState::is_terminated() const
+//=============================================================================
+rmf_task_msgs::msg::Assignment convert(
+  const std::optional<DispatchState::Assignment>& assignment)
+{
+  if (assignment.has_value())
+  {
+    return rmf_task_msgs::build<rmf_task_msgs::msg::Assignment>()
+        .is_assigned(true)
+        .fleet_name(assignment->fleet_name)
+        .expected_robot_name(assignment->expected_robot_name);
+  }
+
+  return rmf_task_msgs::build<rmf_task_msgs::msg::Assignment>()
+      .is_assigned(false)
+      .fleet_name("")
+      .expected_robot_name("");
+}
+
+//=============================================================================
+std::vector<std::string> convert(const std::vector<nlohmann::json>& values)
+{
+  std::vector<std::string> output;
+  output.reserve(values.size());
+  for (const auto& v : values)
+    output.push_back(v.dump());
+
+  return output;
+}
+
+//=============================================================================
+rmf_task_msgs::msg::DispatchState convert(const DispatchState& state)
 {
-  return (status == Status::Failed) ||
-    (status == Status::Completed) ||
-    (status == Status::Canceled);
+  return rmf_task_msgs::build<rmf_task_msgs::msg::DispatchState>()
+      .task_id(state.task_id)
+      .status(static_cast<uint8_t>(state.status))
+      .assignment(convert(state.assignment))
+      .errors(convert(state.errors));
 }
 
 } // namespace rmf_task_ros2
diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
index e5d4c386d..db676c144 100644
--- a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
+++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
@@ -16,6 +16,7 @@
 */
 
 #include <rmf_task_ros2/Dispatcher.hpp>
+#include <rmf_task_ros2/StandardNames.hpp>
 
 #include <rclcpp/node.hpp>
 #include <rclcpp/executors/single_threaded_executor.hpp>
@@ -26,7 +27,10 @@
 #include <rmf_task_msgs/msg/task_profile.hpp>
 #include <rmf_task_msgs/srv/submit_task.hpp>
 #include <rmf_task_msgs/srv/cancel_task.hpp>
-#include <rmf_task_msgs/srv/get_task_list.hpp>
+#include <rmf_task_msgs/msg/dispatch_states.hpp>
+#include <rmf_task_msgs/msg/dispatch_command.hpp>
+#include <rmf_task_msgs/msg/dispatch_ack.hpp>
+#include <rmf_task_msgs/srv/get_dispatch_states.hpp>
 #include <rmf_task_msgs/msg/tasks.hpp>
 #include <rmf_task_msgs/msg/api_request.hpp>
 #include <rmf_task_msgs/msg/api_response.hpp>
@@ -43,48 +47,53 @@ class Dispatcher::Implementation
 public:
   std::shared_ptr<rclcpp::Node> node;
   std::shared_ptr<bidding::Auctioneer> auctioneer;
-  std::shared_ptr<action::Client> action_client;
 
   using SubmitTaskSrv = rmf_task_msgs::srv::SubmitTask;
   using CancelTaskSrv = rmf_task_msgs::srv::CancelTask;
-  using GetTaskListSrv = rmf_task_msgs::srv::GetTaskList;
-  using TasksMsg = rmf_task_msgs::msg::Tasks;
+  using GetDispatchStatesSrv = rmf_task_msgs::srv::GetDispatchStates;
+  using DispatchStateMsg = rmf_task_msgs::msg::DispatchState;
+  using DispatchStatesMsg = rmf_task_msgs::msg::DispatchStates;
   using TaskDescription = rmf_task_msgs::msg::TaskDescription;
+  using DispatchCommandMsg = rmf_task_msgs::msg::DispatchCommand;
+  using DispatchAckMsg = rmf_task_msgs::msg::DispatchAck;
 
   rclcpp::Service<SubmitTaskSrv>::SharedPtr submit_task_srv;
   rclcpp::Service<CancelTaskSrv>::SharedPtr cancel_task_srv;
-  rclcpp::Service<GetTaskListSrv>::SharedPtr get_task_list_srv;
+  rclcpp::Service<GetDispatchStatesSrv>::SharedPtr get_dispatch_states_srv;
 
   using ApiRequest = rmf_task_msgs::msg::ApiRequest;
   using ApiResponse = rmf_task_msgs::msg::ApiResponse;
   rclcpp::Subscription<ApiRequest>::SharedPtr api_request;
   rclcpp::Publisher<ApiResponse>::SharedPtr api_response;
 
-  using ActiveTasksPub = rclcpp::Publisher<TasksMsg>;
-  ActiveTasksPub::SharedPtr ongoing_tasks_pub;
+  using DispatchStatesPub =rclcpp::Publisher<DispatchStatesMsg>;
+  DispatchStatesPub::SharedPtr dispatch_states_pub;
+  rclcpp::TimerBase::SharedPtr dispatch_states_pub_timer;
 
-  rclcpp::TimerBase::SharedPtr timer;
+  uint64_t next_dispatch_command_id = 0;
+  std::unordered_map<uint64_t, DispatchCommandMsg> lingering_commands;
+  rclcpp::TimerBase::SharedPtr dispatch_command_timer;
+  rclcpp::Publisher<DispatchCommandMsg>::SharedPtr dispatch_command_pub;
+
+  rclcpp::Subscription<DispatchAckMsg>::SharedPtr dispatch_ack_sub;
 
   DispatchStateCallback on_change_fn;
 
-  std::queue<bidding::BidNotice> queue_bidding_tasks;
+  std::queue<bidding::BidNotice> task_bid_queue;
 
-  /// TODO: should rename "active" to "ongoing" to prevent confusion
-  /// of with task STATE_ACTIVE
-  DispatchTasks active_dispatch_tasks;
-  DispatchTasks terminal_dispatch_tasks;
-  std::set<std::string> user_submitted_tasks;  // ongoing submitted task_ids
+  DispatchStates active_dispatch_states;
+  DispatchStates finished_dispatch_states;
   std::size_t task_counter = 0; // index for generating task_id
   double bidding_time_window;
-  int terminated_tasks_max_size;
+  std::size_t terminated_tasks_max_size;
   int publish_active_tasks_period;
 
   std::unordered_map<std::size_t, std::string> legacy_task_type_names =
-  {
-    {1, "patrol"},
-    {2, "delivery"},
-    {4, "patrol"}
-  };
+    {
+      {1, "patrol"},
+      {2, "delivery"},
+      {4, "patrol"}
+    };
 
   using LegacyConversion = std::function<std::string(const TaskDescription&)>;
   using LegacyConversionMap = std::unordered_map<std::string, LegacyConversion>;
@@ -101,7 +110,7 @@ class Dispatcher::Implementation
     terminated_tasks_max_size =
       node->declare_parameter<int>("terminated_tasks_max_size", 100);
     RCLCPP_INFO(node->get_logger(),
-      " Declared Terminated Tasks Max Size Param as: %d",
+      " Declared Terminated Tasks Max Size Param as: %lu",
       terminated_tasks_max_size);
     publish_active_tasks_period =
       node->declare_parameter<int>("publish_active_tasks_period", 2);
@@ -110,13 +119,41 @@ class Dispatcher::Implementation
       publish_active_tasks_period);
 
     const auto qos = rclcpp::ServicesQoS().reliable();
-    ongoing_tasks_pub = node->create_publisher<TasksMsg>(
-      rmf_task_ros2::ActiveTasksTopicName, qos);
+    dispatch_states_pub = node->create_publisher<DispatchStatesMsg>(
+      rmf_task_ros2::DispatchStatesTopicName, qos);
 
-    timer = node->create_wall_timer(
+    // TODO(MXG): The smallest resolution this supports is 1 second. That
+    // doesn't seem great.
+    dispatch_states_pub_timer = node->create_wall_timer(
       std::chrono::seconds(publish_active_tasks_period),
-      std::bind(
-        &Dispatcher::Implementation::publish_ongoing_tasks, this));
+      [this]() { this->publish_dispatch_states(); });
+
+    dispatch_command_pub = node->create_publisher<DispatchCommandMsg>(
+      rmf_task_ros2::DispatchCommandTopicName,
+      rclcpp::ServicesQoS().keep_last(20).reliable().transient_local());
+
+    // TODO(MXG): Make this publishing period configurable
+    dispatch_command_timer = node->create_wall_timer(
+      std::chrono::seconds(1),
+      [this](){ this->publish_lingering_commands(); });
+
+    dispatch_ack_sub = node->create_subscription<DispatchAckMsg>(
+      rmf_task_ros2::DispatchAckTopicName,
+      rclcpp::ServicesQoS().keep_last(20).transient_local(),
+      [this](const DispatchAckMsg& msg)
+      {
+        this->handle_dispatch_ack(msg);
+      });
+
+    auctioneer = bidding::Auctioneer::make(
+        node,
+        [this](
+          const TaskID& task_id,
+          const std::optional<bidding::Submission> winner,
+          const std::vector<std::string>& errors)
+        {
+          this->conclude_bid(task_id, std::move(winner), errors);
+        });
 
     // Setup up stream srv interfaces
     submit_task_srv = node->create_service<SubmitTaskSrv>(
@@ -149,24 +186,30 @@ class Dispatcher::Implementation
       }
     );
 
-    get_task_list_srv = node->create_service<GetTaskListSrv>(
+    get_dispatch_states_srv = node->create_service<GetDispatchStatesSrv>(
       rmf_task_ros2::GetTaskListSrvName,
       [this](
-        const std::shared_ptr<GetTaskListSrv::Request> request,
-        std::shared_ptr<GetTaskListSrv::Response> response)
+        const std::shared_ptr<GetDispatchStatesSrv::Request> request,
+        std::shared_ptr<GetDispatchStatesSrv::Response> response)
       {
-        for (auto task : (this->active_dispatch_tasks))
-        {
-          response->active_tasks.push_back(
-            rmf_task_ros2::convert_status(*(task.second)));
-        }
+        std::unordered_set<std::string> relevant_tasks;
+        relevant_tasks.insert(
+          request->task_ids.begin(), request->task_ids.end());
+
+        const auto fill_states = [&relevant_tasks](auto& into, const auto& from)
+          {
+            for (const auto& [id, state] : from)
+            {
+              if (relevant_tasks.empty())
+                into.push_back(convert(*state));
+              else if (relevant_tasks.count(id))
+                into.push_back(convert(*state));
+            }
+          };
+
+        fill_states(response->states.active, this->active_dispatch_states);
+        fill_states(response->states.finished, this->finished_dispatch_states);
 
-        // Terminated Tasks
-        for (auto task : (this->terminal_dispatch_tasks))
-        {
-          response->terminated_tasks.push_back(
-            rmf_task_ros2::convert_status(*(task.second)));
-        }
         response->success = true;
       }
     );
@@ -231,17 +274,6 @@ class Dispatcher::Implementation
       };
   }
 
-  void start()
-  {
-    using namespace std::placeholders;
-    auctioneer = bidding::Auctioneer::make(node,
-        std::bind(&Implementation::receive_bidding_winner_cb, this, _1, _2));
-    action_client->on_terminate(
-      std::bind(&Implementation::terminate_task, this, _1));
-    action_client->on_change(
-      std::bind(&Implementation::task_status_cb, this, _1));
-  }
-
   std::optional<TaskID> submit_task(const TaskDescription& submission)
   {
     const auto task_type_index = submission.task_type.type;
@@ -302,230 +334,378 @@ class Dispatcher::Implementation
     state["category"] = request["category"];
     state["detail"] = request["description"];
 
-    if (on_change_fn)
-      on_change_fn(state);
+    // TODO(MXG): Publish this initial task state message to the websocket!
+
+    auto new_dispatch_state =
+      std::make_shared<DispatchState>(
+        bid_notice.task_id, std::chrono::steady_clock::now());
 
-    // add task to internal cache
-    active_dispatch_tasks[bid_notice.task_id] = new_task_status;
-    user_submitted_tasks.insert(submitted_task.task_id);
+    active_dispatch_states[bid_notice.task_id] = new_dispatch_state;
+
+    if (on_change_fn)
+      on_change_fn(*new_dispatch_state);
 
-    queue_bidding_tasks.push(bid_notice);
+    task_bid_queue.push(bid_notice);
 
-    if (queue_bidding_tasks.size() == 1)
-      auctioneer->start_bidding(queue_bidding_tasks.front());
+    if (task_bid_queue.size() == 1)
+      auctioneer->start_bidding(task_bid_queue.front());
   }
 
   bool cancel_task(const TaskID& task_id)
   {
-    // check if key exists
-    const auto it = active_dispatch_tasks.find(task_id);
-    if (it == active_dispatch_tasks.end())
-    {
-      RCLCPP_ERROR(node->get_logger(),
-        "Task [%s] is not found in active_tasks", task_id.c_str());
-      return false;
-    }
+    using Status = DispatchState::Status;
 
-    RCLCPP_WARN(node->get_logger(), "Cancel task: [%s]", task_id.c_str());
-
-    // Cancel bidding. This will remove the bidding process
-    const auto& cancel_task_status = it->second;
-    if (cancel_task_status->state == DispatchState::State::Pending)
+    // Check if the task has already terminated for some reason
+    const auto finished_it = finished_dispatch_states.find(task_id);
+    if (finished_it != finished_dispatch_states.end())
     {
-      cancel_task_status->state = DispatchState::State::Canceled;
-      terminate_task(cancel_task_status);
+      if (finished_it->second->status == Status::FailedToAssign
+          || finished_it->second->status == Status::CanceledInFlight)
+      {
+        // This task was never assigned to a fleet adapter so we will respond
+        // positively that it is cancelled.
+        return true;
+      }
 
-      if (on_change_fn)
-        on_change_fn(cancel_task_status);
+      if (finished_it->second->status == Status::Dispatched)
+      {
+        // This task is now the responsibility of a fleet adapter
+        return false;
+      }
 
-      return true;
-    }
+      // If the dispatch status is Queued or Selected then this task is in the
+      // wrong dispatch state set.
+      RCLCPP_ERROR(
+        node->get_logger(),
+        "Canceled task [%s] is in the set of finished dispatches but has an "
+        "invalid status for that set: %d",
+        task_id.c_str(),
+        static_cast<uint8_t>(finished_it->second->status));
 
-    // only user submitted task is cancelable
-    if (user_submitted_tasks.find(task_id) == user_submitted_tasks.end())
-    {
-      RCLCPP_ERROR(node->get_logger(),
-        "only user submitted task is cancelable");
       return false;
     }
 
-    // Curently cancel can only work on Queued Task in Fleet Adapter
-    if (cancel_task_status->state != DispatchState::State::Queued)
+    const auto it = active_dispatch_states.find(task_id);
+    if (it == active_dispatch_states.end())
     {
-      RCLCPP_ERROR(node->get_logger(),
-        "Unable to cancel task [%s] as it is not a Queued Task",
-        task_id.c_str());
+      // This must mean that some other system besides the dispatch system
+      // created the task ..?
+      RCLCPP_INFO(node->get_logger(),
+        "Canceled task [%s] was not dispatched", task_id.c_str());
       return false;
     }
 
-    // Remove non-user submitted task from "active_dispatch_tasks"
-    // this is to prevent duplicated task during reassignation
-    // TODO: a better way to impl this
-    for (auto it = active_dispatch_tasks.begin();
-      it != active_dispatch_tasks.end(); )
+    // Cancel bidding. This will remove the bidding process
+    const auto& canceled_dispatch = it->second;
+    if (canceled_dispatch->status == DispatchState::Status::Queued)
     {
-      const bool is_fleet_name =
-        (cancel_task_status->fleet_name == it->second->fleet_name);
-      const bool is_self_gererated =
-        (user_submitted_tasks.find(it->first) == user_submitted_tasks.end());
+      canceled_dispatch->status = DispatchState::Status::CanceledInFlight;
+      move_to_finished(task_id);
+
+      if (on_change_fn)
+        on_change_fn(*canceled_dispatch);
+
+      return true;
+    }
 
-      if (is_self_gererated && is_fleet_name)
+    // Cancel the assignment. We will need to make sure the cancelation is seen
+    // by the fleet adapter that the task was assigned to.
+    if (canceled_dispatch->status == DispatchState::Status::Selected)
+    {
+      canceled_dispatch->status = DispatchState::Status::CanceledInFlight;
+      if (!canceled_dispatch->assignment.has_value())
       {
-        it->second->state = DispatchState::State::Canceled;
-        terminate_task((it++)->second);
+        RCLCPP_ERROR(
+          node->get_logger(),
+          "Canceled task [%s] has Selected status but its assignment is empty. "
+          "This indicates a serious bug. Please report this to the RMF "
+          "developers.",
+          task_id.c_str());
       }
       else
-        ++it;
-    }
+      {
+        auto cancel_command = rmf_task_msgs::build<DispatchCommandMsg>()
+            .fleet_name(canceled_dispatch->assignment->fleet_name)
+            .task_id(task_id)
+            .dispatch_id(next_dispatch_command_id++)
+            .timestamp(node->get_clock()->now())
+            .type(DispatchCommandMsg::TYPE_REMOVE);
+
+        lingering_commands[cancel_command.dispatch_id] = cancel_command;
+        dispatch_command_pub->publish(cancel_command);
+      }
 
-    // Cancel action task, this will only send a cancel to FA. up to
-    // the FA whether to cancel the task. On change is implemented
-    // internally in action client
-    return action_client->cancel_task(cancel_task_status->task_profile);
-  }
+      move_to_finished(task_id);
 
-  const std::optional<DispatchState::State> get_task_state(
-    const TaskID& task_id) const
-  {
-    // check if taskid exists in active tasks
-    auto it = active_dispatch_tasks.find(task_id);
-    if (it != active_dispatch_tasks.end())
-      return it->second->state;
+      if (on_change_fn)
+        on_change_fn(*canceled_dispatch);
+
+      return true;
+    }
 
-    // check if taskid exists in terminated tasks
-    it = terminal_dispatch_tasks.find(task_id);
-    if (it != terminal_dispatch_tasks.end())
-      return it->second->state;
+    RCLCPP_ERROR(
+      node->get_logger(),
+      "Canceled task [%s] is in the set of active dispatches but has an "
+      "invalid status for that set: %d",
+      task_id.c_str(),
+      static_cast<uint8_t>(canceled_dispatch->status));
 
-    return std::nullopt;
+    return false;
   }
 
-  void receive_bidding_winner_cb(
+  void conclude_bid(
     const TaskID& task_id,
-    const rmf_utils::optional<bidding::Submission> winner)
+    const std::optional<bidding::Submission> winner,
+    const std::vector<std::string>& errors)
   {
-    const auto it = active_dispatch_tasks.find(task_id);
-    if (it == active_dispatch_tasks.end())
+    const auto it = active_dispatch_states.find(task_id);
+    if (it == active_dispatch_states.end())
+    {
+      RCLCPP_ERROR(
+        node->get_logger(),
+        "Received a winning bid for a task request [%s] which is no longer "
+        "being dispatched. This may indicate a bug and should be reported to "
+        "the developers of RMF.",
+        task_id.c_str());
       return;
-    const auto& pending_task_status = it->second;
+    }
+
+    auto& dispatch_state = it->second;
+    for (const auto& error : errors)
+    {
+      try
+      {
+        dispatch_state->errors.push_back(nlohmann::json::parse(error));
+      }
+      catch (const std::exception&)
+      {
+        // If the message cannot be converted directly into an error message,
+        // then we will create an error message for it
+        nlohmann::json error_msg;
+        error_msg["code"] = 255;
+        error_msg["category"] = "unknown";
+        error_msg["detail"] = error;
+
+        dispatch_state->errors.push_back(std::move(error_msg));
+      }
+    }
 
     if (!winner)
     {
-      RCLCPP_WARN(node->get_logger(), "Dispatcher Bidding Result: task [%s]"
-        " has no submissions during bidding, Task Failed", task_id.c_str());
-      pending_task_status->state = DispatchState::State::Failed;
-      terminate_task(pending_task_status);
+      RCLCPP_WARN(node->get_logger(),
+        "Dispatcher Bidding Result: task [%s] has no submissions during "
+        "bidding. Dispatching failed, and the task will not be performed.",
+        task_id.c_str());
+
+      dispatch_state->status = DispatchState::Status::FailedToAssign;
+      nlohmann::json error;
+      // TODO(MXG): Standardize the codes
+      error["code"] = 10;
+      error["category"] = "rejection";
+      error["detail"] =
+        "No fleet adapters offered a bid for task [" + task_id + "]";
+
+      dispatch_state->errors.push_back(std::move(error));
 
       if (on_change_fn)
-        on_change_fn(pending_task_status);
+        on_change_fn(*dispatch_state);
+
+      if (task_bid_queue.empty())
+      {
+        RCLCPP_ERROR(
+          node->get_logger(),
+          "task_bid_queue is empty while a bid is concluding. This indicates a "
+          "serious bug. Please report this to the RMF developers.");
+        return;
+      }
+
+      task_bid_queue.pop();
+      if (!task_bid_queue.empty())
+        auctioneer->start_bidding(task_bid_queue.front());
 
-      queue_bidding_tasks.pop();
-      if (!queue_bidding_tasks.empty())
-        auctioneer->start_bidding(queue_bidding_tasks.front());
       return;
     }
 
     // now we know which fleet will execute the task
-    pending_task_status->fleet_name = winner->fleet_name;
+    dispatch_state->assignment =
+      DispatchState::Assignment{
+        winner->fleet_name,
+        winner->robot_name
+      };
 
-    RCLCPP_INFO(node->get_logger(), "Dispatcher Bidding Result: task [%s]"
-      " is accepted by fleet adapter [%s]",
-      task_id.c_str(), winner->fleet_name.c_str());
+    RCLCPP_INFO(
+      node->get_logger(),
+      "Dispatcher Bidding Result: task [%s] is awarded to fleet adapter [%s], "
+      "with expected robot [%s].",
+      task_id.c_str(),
+      winner->fleet_name.c_str(),
+      winner->robot_name.c_str());
+
+    auto award_command = rmf_task_msgs::build<DispatchCommandMsg>()
+        .fleet_name(winner->fleet_name)
+        .task_id(task_id)
+        .dispatch_id(next_dispatch_command_id++)
+        .timestamp(node->get_clock()->now())
+        .type(DispatchCommandMsg::TYPE_AWARD);
+
+    lingering_commands[award_command.dispatch_id] = award_command;
+    dispatch_command_pub->publish(award_command);
+  }
 
-    // Remove non-user submitted charging task from "active_dispatch_tasks"
-    // this is to prevent duplicated task during reassignation.
-    // TODO: a better way to impl this
-    for (auto it = active_dispatch_tasks.begin();
-      it != active_dispatch_tasks.end(); )
-    {
-      const bool is_fleet_name = (winner->fleet_name == it->second->fleet_name);
-      const bool is_self_gererated =
-        (user_submitted_tasks.find(it->first) == user_submitted_tasks.end());
+  void move_to_finished(const std::string& task_id)
+  {
+    const auto active_it = active_dispatch_states.find(task_id);
 
-      if (is_self_gererated && is_fleet_name)
+    // TODO(MXG): We can make this more efficient by having a queue of
+    // finished dispatch state IDs in chronological order
+    if (finished_dispatch_states.size() >= terminated_tasks_max_size)
+    {
+      auto check_it = finished_dispatch_states.begin();
+      auto oldest_it = check_it++;
+      for (; check_it != finished_dispatch_states.end(); ++check_it)
       {
-        it->second->state = DispatchState::State::Canceled;
-        terminate_task((it++)->second);
+        const auto& check = check_it->second->submission_time;
+        const auto& oldest = oldest_it->second->submission_time;
+        if (check < oldest)
+          oldest_it = check_it;
       }
-      else
-        ++it;
+
+      finished_dispatch_states.erase(oldest_it);
     }
 
-    // add task to action server
-    action_client->add_task(
-      winner->fleet_name,
-      pending_task_status->task_profile,
-      pending_task_status);
+    finished_dispatch_states[active_it->first] = active_it->second;
   }
 
-  void terminate_task(const TaskStatusPtr terminate_status)
+  void publish_dispatch_states()
   {
-    assert(terminate_status->is_terminated());
-    publish_ongoing_tasks();
+    const auto fill_states = [](auto& into, const auto& from)
+      {
+        for (const auto& [id, state] : from)
+          into.push_back(convert(*state));
+      };
 
-    // prevent terminal_dispatch_tasks from piling up meaning
-    if (terminal_dispatch_tasks.size() >= terminated_tasks_max_size)
-    {
-      RCLCPP_WARN(node->get_logger(),
-        "Terminated tasks reached max size, remove earliest submited task");
+    std::vector<DispatchStateMsg> active;
+    std::vector<DispatchStateMsg> finished;
+    fill_states(active, active_dispatch_states);
+    fill_states(finished, finished_dispatch_states);
+
+    dispatch_states_pub->publish(
+      rmf_task_msgs::build<DispatchStatesMsg>()
+          .active(std::move(active))
+          .finished(std::move(finished)));
+  }
 
-      auto rm_task = terminal_dispatch_tasks.begin();
-      for (auto it = rm_task++; it != terminal_dispatch_tasks.end(); it++)
+  void publish_lingering_commands()
+  {
+    std::vector<uint64_t> expired_commands;
+    const auto now = node->get_clock()->now();
+
+    // TODO(MXG): Make this timeout period configurable
+    const auto timeout = std::chrono::seconds(10);
+
+    for (const auto& [id, r] : lingering_commands)
+    {
+      const auto timestamp = rclcpp::Time(r.timestamp);
+      if (timestamp + timeout < now)
       {
-        const auto t1 = it->second->task_profile.submission_time;
-        const auto t2 = rm_task->second->task_profile.submission_time;
-        if (rmf_traffic_ros2::convert(t1) < rmf_traffic_ros2::convert(t2))
-          rm_task = it;
+        // This request has expired.
+        expired_commands.push_back(id);
+        continue;
       }
-      terminal_dispatch_tasks.erase(rm_task);
+
+      dispatch_command_pub->publish(r);
     }
 
-    const auto id = terminate_status->task_profile.task_id;
+    for (const auto& id : expired_commands)
+    {
+      const auto it = lingering_commands.find(id);
+      if (it == lingering_commands.end())
+      {
+        RCLCPP_ERROR(
+          node->get_logger(),
+          "Weird bug, [%lu] is no longer in the lingering requests even though "
+          "it was just detected as being expired. Please report this to the "
+          "RMF developers.", id);
+        continue;
+      }
 
-    // destroy prev status ptr and recreate one
-    auto status = std::make_shared<DispatchState>(*terminate_status);
-    (terminal_dispatch_tasks)[id] = status;
-    user_submitted_tasks.erase(id);
-    active_dispatch_tasks.erase(id);
+      const auto& request = it->second;
+      RCLCPP_ERROR(
+        node->get_logger(),
+        "Dispatch request [%lu] type [%u] for task [%s] directed at fleet [%s] "
+        "has expired. This likely means something is wrong with the fleet "
+        "adapter for [%s] preventing it from responding.",
+        id,
+        request.type,
+        request.task_id.c_str(),
+        request.fleet_name.c_str(),
+        request.fleet_name.c_str());
+
+      lingering_commands.erase(it);
+    }
   }
 
-  void task_status_cb(const TaskStatusPtr status)
+  void handle_dispatch_ack(const DispatchAckMsg& ack)
   {
-    // This is to solve the issue that the dispatcher is not aware of those
-    // "stray" tasks that are not dispatched by the dispatcher. This will add
-    // the stray tasks when an unknown TaskSummary is heard.
-    const std::string id = status->task_profile.task_id;
-    const auto it = active_dispatch_tasks.find(id);
-    if (it == active_dispatch_tasks.end())
+    const auto command_it = lingering_commands.find(ack.request_id);
+    if (command_it == lingering_commands.end())
     {
-      active_dispatch_tasks[id] = status;
-      RCLCPP_WARN(node->get_logger(),
-        "Add previously unheard task: [%s]", id.c_str());
+      // Already processed this acknowledgment
+      return;
     }
 
-    // check if there's a change in state for the previous completed bidding task
-    // TODO, better way to impl this
-    if (!queue_bidding_tasks.empty()
-      && id == queue_bidding_tasks.front().task_profile.task_id)
+    const auto command = std::move(command_it->second);
+    lingering_commands.erase(command_it);
+
+    if (command.type == DispatchCommandMsg::TYPE_AWARD)
     {
-      queue_bidding_tasks.pop();
-      if (!queue_bidding_tasks.empty())
-        auctioneer->start_bidding(queue_bidding_tasks.front());
-    }
+      const auto state_it = active_dispatch_states.find(command.task_id);
+      if (state_it == active_dispatch_states.end())
+      {
+        RCLCPP_ERROR(
+          node->get_logger(),
+          "Could not find active dispatch state for [%s] despite receiving an "
+          "acknowledgment for its bid award. This indicates a bug. Please "
+          "report this to the RMF developers.",
+          command.task_id.c_str());
+        return;
+      }
 
-    if (on_change_fn)
-      on_change_fn(status);
-  }
+      const auto state = state_it->second;
+      if (state->status == DispatchState::Status::Selected)
+      {
+        state->status = DispatchState::Status::Dispatched;
+        move_to_finished(state->task_id);
+      }
+      else if (state->status == DispatchState::Status::CanceledInFlight)
+      {
+        // If the task was canceled in flight then we will simply ignore this
+        // acknowledgment. There should be another lingering command telling the
+        // fleet adapter to cancel the task.
+      }
+      else
+      {
+        RCLCPP_ERROR(
+          node->get_logger(),
+          "Dispatch status for [%s] is [%u], but we have received an award "
+          "acknowledgment for it. This indicates a bug. Please report this to "
+          "the RMF developers.",
+          state->task_id.c_str(),
+          static_cast<uint8_t>(state->status));
+      }
 
-  void publish_ongoing_tasks()
-  {
-    TasksMsg task_msgs;
-    for (auto task : (this->active_dispatch_tasks))
+      return;
+    }
+    else if (command.type == DispatchCommandMsg::TYPE_REMOVE)
     {
-      task_msgs.tasks.push_back(
-        rmf_task_ros2::convert_status(*(task.second)));
+      // No further action is needed. We simply remove the lingering command.
     }
-    ongoing_tasks_pub->publish(task_msgs);
+
+    RCLCPP_ERROR(
+      node->get_logger(),
+      "Unrecognized command type [%u] in lingering dispatch command queue. "
+      "This indicates a bug. Please report this to the RMF developers.",
+      command.type);
   }
 };
 
@@ -548,18 +728,14 @@ std::shared_ptr<Dispatcher> Dispatcher::make_node(
 std::shared_ptr<Dispatcher> Dispatcher::make(
   const std::shared_ptr<rclcpp::Node>& node)
 {
-  auto pimpl = rmf_utils::make_impl<Implementation>(node);
-  pimpl->action_client = action::Client::make(node);
-
   auto dispatcher = std::shared_ptr<Dispatcher>(new Dispatcher());
-  dispatcher->_pimpl = std::move(pimpl);
-  dispatcher->_pimpl->start();
+  dispatcher->_pimpl = rmf_utils::make_impl<Implementation>(node);
   return dispatcher;
 }
 
 //==============================================================================
 std::optional<TaskID> Dispatcher::submit_task(
-  const TaskDescription& task_description)
+  const rmf_task_msgs::msg::TaskDescription& task_description)
 {
   return _pimpl->submit_task(task_description);
 }
@@ -571,26 +747,34 @@ bool Dispatcher::cancel_task(const TaskID& task_id)
 }
 
 //==============================================================================
-const rmf_utils::optional<std::string> Dispatcher::get_task_state(
+std::optional<DispatchState> Dispatcher::get_dispatch_state(
   const TaskID& task_id) const
 {
-  return _pimpl->get_task_state(task_id);
+  const auto active_it = _pimpl->active_dispatch_states.find(task_id);
+  if (active_it != _pimpl->active_dispatch_states.end())
+    return *active_it->second;
+
+  const auto finished_it = _pimpl->finished_dispatch_states.find(task_id);
+  if (finished_it != _pimpl->finished_dispatch_states.end())
+    return *finished_it->second;
+
+  return std::nullopt;
 }
 
 //==============================================================================
-const Dispatcher::DispatchTasks& Dispatcher::active_tasks() const
+const Dispatcher::DispatchStates& Dispatcher::active_dispatches() const
 {
-  return _pimpl->active_dispatch_tasks;
+  return _pimpl->active_dispatch_states;
 }
 
 //==============================================================================
-const Dispatcher::DispatchTasks& Dispatcher::terminated_tasks() const
+const Dispatcher::DispatchStates& Dispatcher::finished_dispatches() const
 {
-  return _pimpl->terminal_dispatch_tasks;
+  return _pimpl->finished_dispatch_states;
 }
 
 //==============================================================================
-void Dispatcher::on_change(TaskStateCallback on_change_fn)
+void Dispatcher::on_change(DispatchStateCallback on_change_fn)
 {
   _pimpl->on_change_fn = on_change_fn;
 }
diff --git a/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp b/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp
index e2b376426..5c8b84581 100644
--- a/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp
+++ b/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp
@@ -33,7 +33,7 @@ Client::Client(std::shared_ptr<rclcpp::Node> node)
   const auto dispatch_qos = rclcpp::ServicesQoS().reliable();
 
   _request_msg_pub = _node->create_publisher<RequestMsg>(
-    TaskRequestTopicName, dispatch_qos);
+    DispatchCommandTopicName, dispatch_qos);
 
   _status_msg_sub = _node->create_subscription<StatusMsg>(
     TaskStatusTopicName, dispatch_qos,
@@ -80,7 +80,7 @@ Client::Client(std::shared_ptr<rclcpp::Node> node)
     });
 
   _ack_msg_sub = _node->create_subscription<AckMsg>(
-    TaskAckTopicName, dispatch_qos,
+    DispatchAckTopicName, dispatch_qos,
     [&](const std::unique_ptr<AckMsg> msg)
     {
       const auto task_id = msg->dispatch_request.task_profile.task_id;
diff --git a/rmf_task_ros2/src/rmf_task_ros2/action/Client.hpp b/rmf_task_ros2/src/rmf_task_ros2/action/Client.hpp
index 4d4a633a6..40c8b67cb 100644
--- a/rmf_task_ros2/src/rmf_task_ros2/action/Client.hpp
+++ b/rmf_task_ros2/src/rmf_task_ros2/action/Client.hpp
@@ -95,7 +95,7 @@ class Client
 
   void update_task_status(const TaskStatusPtr status);
 
-  using RequestMsg = rmf_task_msgs::msg::DispatchRequest;
+  using RequestMsg = rmf_task_msgs::msg::DispatchCommand;
   using AckMsg = rmf_task_msgs::msg::DispatchAck;
 
   std::shared_ptr<rclcpp::Node> _node;
diff --git a/rmf_task_ros2/src/rmf_task_ros2/action/Server.cpp b/rmf_task_ros2/src/rmf_task_ros2/action/Server.cpp
index 4540d56fc..0eb8d13be 100644
--- a/rmf_task_ros2/src/rmf_task_ros2/action/Server.cpp
+++ b/rmf_task_ros2/src/rmf_task_ros2/action/Server.cpp
@@ -54,7 +54,7 @@ Server::Server(
   const auto dispatch_qos = rclcpp::ServicesQoS().reliable();
 
   _request_msg_sub = _node->create_subscription<RequestMsg>(
-    TaskRequestTopicName, dispatch_qos,
+    DispatchCommandTopicName, dispatch_qos,
     [&](const std::unique_ptr<RequestMsg> msg)
     {
       if (msg->fleet_name != _fleet_name)
@@ -89,7 +89,7 @@ Server::Server(
     });
 
   _ack_msg_pub = _node->create_publisher<AckMsg>(
-    TaskAckTopicName, dispatch_qos);
+    DispatchAckTopicName, dispatch_qos);
 
   _status_msg_pub = _node->create_publisher<StatusMsg>(
     TaskStatusTopicName, dispatch_qos);
diff --git a/rmf_task_ros2/src/rmf_task_ros2/action/Server.hpp b/rmf_task_ros2/src/rmf_task_ros2/action/Server.hpp
index 63e191383..e2872ec33 100644
--- a/rmf_task_ros2/src/rmf_task_ros2/action/Server.hpp
+++ b/rmf_task_ros2/src/rmf_task_ros2/action/Server.hpp
@@ -76,7 +76,7 @@ class Server
     std::shared_ptr<rclcpp::Node> node,
     const std::string& fleet_name);
 
-  using RequestMsg = rmf_task_msgs::msg::DispatchRequest;
+  using RequestMsg = rmf_task_msgs::msg::DispatchCommand;
   using AckMsg = rmf_task_msgs::msg::DispatchAck;
 
   std::shared_ptr<rclcpp::Node> _node;
diff --git a/rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.cpp b/rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.cpp
index 98bad0b79..6f1f27030 100644
--- a/rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.cpp
+++ b/rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.cpp
@@ -67,12 +67,12 @@ void Auctioneer::Implementation::start_bidding(
   const BidNotice& bid_notice)
 {
   RCLCPP_INFO(node->get_logger(), "Add Task [%s] to a bidding queue",
-    bid_notice.task_profile.task_id.c_str());
+    bid_notice.task_id.c_str());
 
-  BiddingTask bidding_task;
+  OpenBid bidding_task;
   bidding_task.bid_notice = bid_notice;
   bidding_task.start_time = node->now();
-  queue_bidding_tasks.push(bidding_task);
+  open_bid_queue.push(bidding_task);
 }
 
 //==============================================================================
@@ -86,25 +86,25 @@ void Auctioneer::Implementation::receive_proposal(
 
   // check if bidding task is initiated by the auctioneer previously
   // add submited proposal to the current bidding tasks list
-  if (queue_bidding_tasks.front().bid_notice.task_profile.task_id == id)
-    queue_bidding_tasks.front().submissions.push_back(convert(msg));
+  if (open_bid_queue.front().bid_notice.task_profile.task_id == id)
+    open_bid_queue.front().submissions.push_back(convert(msg));
 }
 
 //==============================================================================
 // determine the winner within a bidding task instance
 void Auctioneer::Implementation::check_bidding_process()
 {
-  if (queue_bidding_tasks.size() == 0)
+  if (open_bid_queue.size() == 0)
     return;
 
   // Executing the task at the front queue
-  auto front_task = queue_bidding_tasks.front();
+  auto front_task = open_bid_queue.front();
 
   if (bidding_in_proccess)
   {
     if (determine_winner(front_task))
     {
-      queue_bidding_tasks.pop();
+      open_bid_queue.pop();
       bidding_in_proccess = false;
     }
   }
@@ -112,7 +112,7 @@ void Auctioneer::Implementation::check_bidding_process()
   {
     RCLCPP_DEBUG(node->get_logger(), " - Start new bidding task: %s",
       front_task.bid_notice.task_profile.task_id.c_str());
-    queue_bidding_tasks.front().start_time = node->now();
+    open_bid_queue.front().start_time = node->now();
     bid_notice_pub->publish(front_task.bid_notice);
     bidding_in_proccess = true;
   }
@@ -120,7 +120,7 @@ void Auctioneer::Implementation::check_bidding_process()
 
 //==============================================================================
 bool Auctioneer::Implementation::determine_winner(
-  const BiddingTask& bidding_task)
+  const OpenBid& bidding_task)
 {
   const auto duration = node->now() - bidding_task.start_time;
 
@@ -181,10 +181,10 @@ std::shared_ptr<Auctioneer> Auctioneer::make(
   const std::shared_ptr<rclcpp::Node>& node,
   BiddingResultCallback result_callback)
 {
-  auto pimpl = rmf_utils::make_unique_impl<Implementation>(
-    node, result_callback);
   auto auctioneer = std::shared_ptr<Auctioneer>(new Auctioneer());
-  auctioneer->_pimpl = std::move(pimpl);
+  auctioneer->_pimpl = rmf_utils::make_unique_impl<Implementation>(
+    node, std::move(result_callback));
+
   return auctioneer;
 }
 
diff --git a/rmf_task_ros2/src/rmf_task_ros2/bidding/internal_Auctioneer.hpp b/rmf_task_ros2/src/rmf_task_ros2/bidding/internal_Auctioneer.hpp
index 57cf1a0eb..bafba2dd7 100644
--- a/rmf_task_ros2/src/rmf_task_ros2/bidding/internal_Auctioneer.hpp
+++ b/rmf_task_ros2/src/rmf_task_ros2/bidding/internal_Auctioneer.hpp
@@ -40,7 +40,7 @@ class Auctioneer::Implementation
   BiddingResultCallback bidding_result_callback;
   std::shared_ptr<Evaluator> evaluator;
 
-  struct BiddingTask
+  struct OpenBid
   {
     BidNotice bid_notice;
     builtin_interfaces::msg::Time start_time;
@@ -48,7 +48,7 @@ class Auctioneer::Implementation
   };
 
   bool bidding_in_proccess = false;
-  std::queue<BiddingTask> queue_bidding_tasks;
+  std::queue<OpenBid> open_bid_queue;
 
   using BidNoticePub = rclcpp::Publisher<BidNotice>;
   BidNoticePub::SharedPtr bid_notice_pub;
@@ -69,7 +69,7 @@ class Auctioneer::Implementation
   // determine the winner within a bidding task instance
   void check_bidding_process();
 
-  bool determine_winner(const BiddingTask& bidding_task);
+  bool determine_winner(const OpenBid& bidding_task);
 
   std::optional<Submission> evaluate(const Submissions& submissions);
 

From 8eba8cc0a8cae162507451b0e7447f907d943733 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Fri, 7 Jan 2022 15:53:03 +0800
Subject: [PATCH 42/79] Finished migrating the dispatch system -- needs testing

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 rmf_task_ros2/CMakeLists.txt                  |   5 +
 .../include/rmf_task_ros2/Dispatcher.hpp      |   2 +-
 .../include/rmf_task_ros2/StandardNames.hpp   |   4 +-
 .../{MinimalBidder.hpp => AsyncBidder.hpp}    |  36 +--
 .../rmf_task_ros2/bidding/Auctioneer.hpp      |  47 ++--
 .../rmf_task_ros2/bidding/Response.hpp        |  67 +++++
 .../rmf_task_ros2/bidding/Submission.hpp      |  44 ----
 rmf_task_ros2/package.xml                     |   2 +
 rmf_task_ros2/src/mock_bidder/main.cpp        |  92 ++-----
 .../src/rmf_task_ros2/Dispatcher.cpp          | 234 +++++++++++++++--
 .../src/rmf_task_ros2/action/Client.cpp       | 236 ------------------
 .../src/rmf_task_ros2/action/Client.hpp       | 113 ---------
 .../src/rmf_task_ros2/action/Server.cpp       |  99 --------
 .../src/rmf_task_ros2/action/Server.hpp       |  94 -------
 .../src/rmf_task_ros2/bidding/AsyncBidder.cpp | 107 ++++++++
 .../src/rmf_task_ros2/bidding/Auctioneer.cpp  | 221 ++++++++--------
 .../rmf_task_ros2/bidding/MinimalBidder.cpp   | 132 ----------
 .../src/rmf_task_ros2/bidding/Response.cpp    |  81 ++++++
 .../bidding/internal_Auctioneer.hpp           |  34 +--
 .../test/action/test_ActionInterface.cpp      | 202 ---------------
 rmf_task_ros2/test/bidding/test_Evaluator.cpp |  53 ++--
 rmf_task_ros2/test/bidding/test_SelfBid.cpp   |  94 ++++---
 .../test/dispatcher/test_Dispatcher.cpp       | 183 ++++----------
 23 files changed, 802 insertions(+), 1380 deletions(-)
 rename rmf_task_ros2/include/rmf_task_ros2/bidding/{MinimalBidder.hpp => AsyncBidder.hpp} (65%)
 create mode 100644 rmf_task_ros2/include/rmf_task_ros2/bidding/Response.hpp
 delete mode 100644 rmf_task_ros2/include/rmf_task_ros2/bidding/Submission.hpp
 delete mode 100644 rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp
 delete mode 100644 rmf_task_ros2/src/rmf_task_ros2/action/Client.hpp
 delete mode 100644 rmf_task_ros2/src/rmf_task_ros2/action/Server.cpp
 delete mode 100644 rmf_task_ros2/src/rmf_task_ros2/action/Server.hpp
 create mode 100644 rmf_task_ros2/src/rmf_task_ros2/bidding/AsyncBidder.cpp
 delete mode 100644 rmf_task_ros2/src/rmf_task_ros2/bidding/MinimalBidder.cpp
 create mode 100644 rmf_task_ros2/src/rmf_task_ros2/bidding/Response.cpp
 delete mode 100644 rmf_task_ros2/test/action/test_ActionInterface.cpp

diff --git a/rmf_task_ros2/CMakeLists.txt b/rmf_task_ros2/CMakeLists.txt
index dd947d557..0cd778244 100644
--- a/rmf_task_ros2/CMakeLists.txt
+++ b/rmf_task_ros2/CMakeLists.txt
@@ -14,22 +14,27 @@ endif()
 include(GNUInstallDirs)
 
 find_package(ament_cmake REQUIRED)
+find_package(rmf_api_msgs REQUIRED)
 find_package(rmf_traffic REQUIRED)
 find_package(rmf_traffic_ros2 REQUIRED)
 find_package(rmf_task_msgs REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(nlohmann_json REQUIRED)
+find_package(nlohmann_json_schema_validator_vendor REQUIRED)
 
 file(GLOB_RECURSE core_lib_srcs "src/rmf_task_ros2/*.cpp")
 add_library(rmf_task_ros2 SHARED ${core_lib_srcs})
 
 target_link_libraries(rmf_task_ros2
   PUBLIC
+    rmf_api_msgs::rmf_api_msgs
     rmf_traffic::rmf_traffic
     rmf_traffic_ros2::rmf_traffic_ros2
     ${rmf_task_msgs_LIBRARIES}
     ${rclcpp_LIBRARIES}
+    nlohmann_json::nlohmann_json
+    nlohmann_json_schema_validator
 )
 
 target_include_directories(rmf_task_ros2
diff --git a/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp b/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp
index 53d27dfe4..d13985dd4 100644
--- a/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp
+++ b/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp
@@ -124,7 +124,7 @@ class Dispatcher : public std::enable_shared_from_this<Dispatcher>
   ///
   /// \param [in] evaluator
   ///   evaluator used to select the best bid from fleets
-  void evaluator(std::shared_ptr<bidding::Auctioneer::Evaluator> evaluator);
+  void evaluator(bidding::Auctioneer::ConstEvaluatorPtr evaluator);
 
   /// Get the rclcpp::Node that this dispatcher will be using for communication.
   std::shared_ptr<rclcpp::Node> node();
diff --git a/rmf_task_ros2/include/rmf_task_ros2/StandardNames.hpp b/rmf_task_ros2/include/rmf_task_ros2/StandardNames.hpp
index c8aa6f9a5..fea8c49eb 100644
--- a/rmf_task_ros2/include/rmf_task_ros2/StandardNames.hpp
+++ b/rmf_task_ros2/include/rmf_task_ros2/StandardNames.hpp
@@ -24,11 +24,11 @@ namespace rmf_task_ros2 {
 
 const std::string Prefix = "rmf_task/";
 const std::string BidNoticeTopicName = Prefix + "bid_notice";
-const std::string BidProposalTopicName = Prefix + "bid_proposal";
+const std::string BidResponseTopicName = Prefix + "bid_response";
 
 const std::string SubmitTaskSrvName = "submit_task";
 const std::string CancelTaskSrvName = "cancel_task";
-const std::string GetTaskListSrvName = "get_tasks";
+const std::string GetDispatchStatesSrvName = "get_dispatches";
 
 const std::string DispatchCommandTopicName = Prefix + "dispatch_request";
 const std::string DispatchAckTopicName = Prefix + "dispatch_ack";
diff --git a/rmf_task_ros2/include/rmf_task_ros2/bidding/MinimalBidder.hpp b/rmf_task_ros2/include/rmf_task_ros2/bidding/AsyncBidder.hpp
similarity index 65%
rename from rmf_task_ros2/include/rmf_task_ros2/bidding/MinimalBidder.hpp
rename to rmf_task_ros2/include/rmf_task_ros2/bidding/AsyncBidder.hpp
index ef53db6fc..80bfd12e5 100644
--- a/rmf_task_ros2/include/rmf_task_ros2/bidding/MinimalBidder.hpp
+++ b/rmf_task_ros2/include/rmf_task_ros2/bidding/AsyncBidder.hpp
@@ -15,34 +15,25 @@
  *
 */
 
-#ifndef RMF_TASK_ROS2__BIDDING__MINIMALBIDDER_HPP
-#define RMF_TASK_ROS2__BIDDING__MINIMALBIDDER_HPP
+#ifndef RMF_TASK_ROS2__BIDDING__ASYNCBIDDER_HPP
+#define RMF_TASK_ROS2__BIDDING__ASYNCBIDDER_HPP
 
 #include <unordered_set>
 
 #include <rclcpp/node.hpp>
 #include <rmf_utils/impl_ptr.hpp>
 
-#include <rmf_task_ros2/bidding/Submission.hpp>
+#include <rmf_task_ros2/bidding/Response.hpp>
 
 namespace rmf_task_ros2 {
 namespace bidding {
 
 //==============================================================================
-class MinimalBidder
+class AsyncBidder
 {
 public:
-  /// Type of Task in RMF
-  using TaskTypeMsg = rmf_task_msgs::msg::TaskType;
-  enum class TaskType
-  {
-    Station       = TaskTypeMsg::TYPE_STATION,
-    Loop          = TaskTypeMsg::TYPE_LOOP,
-    Delivery      = TaskTypeMsg::TYPE_DELIVERY,
-    ChargeBattery = TaskTypeMsg::TYPE_CHARGE_BATTERY,
-    Clean         = TaskTypeMsg::TYPE_CLEAN,
-    Patrol        = TaskTypeMsg::TYPE_PATROL
-  };
+
+  using Respond = std::function<void(const Response&)>;
 
   /// Callback function when a bid notice is received from the autioneer
   ///
@@ -51,9 +42,8 @@ class MinimalBidder
   ///
   /// \return submission
   ///   Estimates of a task. This submission is used by dispatcher for eval
-  using ParseSubmissionCallback =
-    std::function<Submission(const BidNotice& notice)>;
-
+  using ReceiveNotice =
+    std::function<void(const BidNoticeMsg& notice, Respond respond)>;
 
   /// Create a bidder to bid for incoming task requests from Task Dispatcher
   ///
@@ -68,20 +58,18 @@ class MinimalBidder
   ///
   /// \param[in] submission_cb
   ///   fn which is used to provide a bid submission during a call for bid
-  static std::shared_ptr<MinimalBidder> make(
+  static std::shared_ptr<AsyncBidder> make(
     const std::shared_ptr<rclcpp::Node>& node,
-    const std::string& fleet_name,
-    const std::unordered_set<TaskType>& valid_task_types,
-    ParseSubmissionCallback submission_cb);
+    ReceiveNotice notice_cb);
 
   class Implementation;
 
 private:
-  MinimalBidder();
+  AsyncBidder();
   rmf_utils::unique_impl_ptr<Implementation> _pimpl;
 };
 
 } // namespace bidding
 } // namespace rmf_task_ros2
 
-#endif // RMF_TASK_ROS2__BIDDING__MINIMALBIDDER_HPP
+#endif // RMF_TASK_ROS2__BIDDING__ASYNCBIDDER_HPP
diff --git a/rmf_task_ros2/include/rmf_task_ros2/bidding/Auctioneer.hpp b/rmf_task_ros2/include/rmf_task_ros2/bidding/Auctioneer.hpp
index 47ef29c9d..b6fd1a96b 100644
--- a/rmf_task_ros2/include/rmf_task_ros2/bidding/Auctioneer.hpp
+++ b/rmf_task_ros2/include/rmf_task_ros2/bidding/Auctioneer.hpp
@@ -22,7 +22,7 @@
 #include <rclcpp/node.hpp>
 #include <rmf_utils/impl_ptr.hpp>
 
-#include <rmf_task_ros2/bidding/Submission.hpp>
+#include <rmf_task_ros2/bidding/Response.hpp>
 
 namespace rmf_task_ros2 {
 namespace bidding {
@@ -34,6 +34,7 @@ namespace bidding {
 class Auctioneer : public std::enable_shared_from_this<Auctioneer>
 {
 public:
+
   /// Callback which will provide the winner when a bid is concluded
   ///
   /// \param[in] task_id
@@ -45,9 +46,25 @@ class Auctioneer : public std::enable_shared_from_this<Auctioneer>
     std::function<
       void(
         const std::string& task_id,
-        const std::optional<Submission> winner,
+        const std::optional<Response::Proposal> winner,
         const std::vector<std::string>& errors)>;
 
+  /// A pure abstract interface class for the auctioneer to choose the best
+  /// choosing the best submissions.
+  class Evaluator
+  {
+  public:
+
+    /// Given a list of submissions, choose the one that is the "best". It is
+    /// up to the implementation of the Evaluator to decide how to rank.
+    virtual std::optional<std::size_t> choose(
+      const Responses& responses) const = 0;
+
+    virtual ~Evaluator() = default;
+  };
+
+  using ConstEvaluatorPtr = std::shared_ptr<const Evaluator>;
+
   /// Create an instance of the Auctioneer. This instance will handle all
   /// the task dispatching bidding mechanism. A default evaluator is used.
   ///
@@ -60,33 +77,21 @@ class Auctioneer : public std::enable_shared_from_this<Auctioneer>
   /// \sa make()
   static std::shared_ptr<Auctioneer> make(
     const std::shared_ptr<rclcpp::Node>& node,
-    BiddingResultCallback result_callback);
+    BiddingResultCallback result_callback,
+    ConstEvaluatorPtr evaluator);
 
   /// Start a bidding process by provide a bidding task. Note the each
   /// bidding process is conducted sequentially
   ///
   /// \param[in] bid_notice
   ///   bidding task, task which will call for bid
-  void start_bidding(const BidNotice& bid_notice);
-
-  /// A pure abstract interface class for the auctioneer to choose the best
-  /// choosing the best submissions.
-  class Evaluator
-  {
-  public:
-
-    /// Given a list of submissions, choose the one that is the "best". It is
-    /// up to the implementation of the Evaluator to decide how to rank.
-    virtual std::size_t choose(const Submissions& submissions) const = 0;
-
-    virtual ~Evaluator() = default;
-  };
+  void start_bidding(const BidNoticeMsg& bid_notice);
 
   /// Provide a custom evaluator which will be used to choose the best bid
   /// If no selection is given, Default is: LeastFleetDiffCostEvaluator
   ///
   /// \param[in] evaluator
-  void select_evaluator(std::shared_ptr<Evaluator> evaluator);
+  void set_evaluator(ConstEvaluatorPtr evaluator);
 
   class Implementation;
 
@@ -99,21 +104,21 @@ class Auctioneer : public std::enable_shared_from_this<Auctioneer>
 class LeastFleetDiffCostEvaluator : public Auctioneer::Evaluator
 {
 public:
-  std::size_t choose(const Submissions& submissions) const final;
+  std::optional<std::size_t> choose(const Responses& responses) const final;
 };
 
 //==============================================================================
 class LeastFleetCostEvaluator : public Auctioneer::Evaluator
 {
 public:
-  std::size_t choose(const Submissions& submissions) const final;
+  std::optional<std::size_t> choose(const Responses& submissions) const final;
 };
 
 //==============================================================================
 class QuickestFinishEvaluator : public Auctioneer::Evaluator
 {
 public:
-  std::size_t choose(const Submissions& submissions) const final;
+  std::optional<std::size_t> choose(const Responses& submissions) const final;
 };
 
 } // namespace bidding
diff --git a/rmf_task_ros2/include/rmf_task_ros2/bidding/Response.hpp b/rmf_task_ros2/include/rmf_task_ros2/bidding/Response.hpp
new file mode 100644
index 000000000..3bf78f407
--- /dev/null
+++ b/rmf_task_ros2/include/rmf_task_ros2/bidding/Response.hpp
@@ -0,0 +1,67 @@
+/*
+ * 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_TASK_ROS2__BIDDING__RESPONSE_HPP
+#define RMF_TASK_ROS2__BIDDING__RESPONSE_HPP
+
+#include <rmf_traffic/Time.hpp>
+#include <rmf_task_msgs/msg/bid_notice.hpp>
+#include <rmf_task_msgs/msg/bid_proposal.hpp>
+#include <rmf_task_msgs/msg/bid_response.hpp>
+
+namespace rmf_task_ros2 {
+namespace bidding {
+
+//==============================================================================
+struct Response
+{
+  struct Proposal
+  {
+    std::string fleet_name;
+    std::string expected_robot_name;
+    double prev_cost;
+    double new_cost;
+    rmf_traffic::Time finish_time;
+  };
+
+  std::optional<Proposal> proposal;
+  std::vector<std::string> errors;
+};
+
+//==============================================================================
+using Responses = std::vector<Response>;
+
+//==============================================================================
+using BidResponseMsg = rmf_task_msgs::msg::BidResponse;
+using BidProposalMsg = rmf_task_msgs::msg::BidProposal;
+using BidNoticeMsg = rmf_task_msgs::msg::BidNotice;
+
+//==============================================================================
+BidProposalMsg convert(const Response::Proposal& proposal);
+
+//==============================================================================
+BidResponseMsg convert(
+  const Response& response,
+  const std::string& task_id);
+
+//==============================================================================
+Response convert(const BidResponseMsg& msg);
+
+} // namespace bidding
+} // namespace rmf_task_ros2
+
+#endif // RMF_TASK_ROS2__BIDDING__RESPONSE_HPP
diff --git a/rmf_task_ros2/include/rmf_task_ros2/bidding/Submission.hpp b/rmf_task_ros2/include/rmf_task_ros2/bidding/Submission.hpp
deleted file mode 100644
index b9b8cc813..000000000
--- a/rmf_task_ros2/include/rmf_task_ros2/bidding/Submission.hpp
+++ /dev/null
@@ -1,44 +0,0 @@
-/*
- * Copyright (C) 2020 Open Source Robotics Foundation
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *
-*/
-
-#ifndef RMF_TASK_ROS2__BIDDING__SUBMISSION_HPP
-#define RMF_TASK_ROS2__BIDDING__SUBMISSION_HPP
-
-#include <rmf_traffic/Time.hpp>
-#include <rmf_task_msgs/msg/bid_notice.hpp>
-
-namespace rmf_task_ros2 {
-namespace bidding {
-
-//==============================================================================
-struct Submission
-{
-  std::string fleet_name;
-  std::string robot_name;
-  double prev_cost = 0.0;
-  double new_cost = std::numeric_limits<double>::max();
-  rmf_traffic::Time finish_time;
-};
-
-//==============================================================================
-using Submissions = std::vector<Submission>;
-using BidNotice = rmf_task_msgs::msg::BidNotice;
-
-} // namespace bidding
-} // namespace rmf_task_ros2
-
-#endif // RMF_TASK_ROS2__BIDDING__SUBMISSION_HPP
diff --git a/rmf_task_ros2/package.xml b/rmf_task_ros2/package.xml
index e2e69a44f..3e4e87a61 100644
--- a/rmf_task_ros2/package.xml
+++ b/rmf_task_ros2/package.xml
@@ -11,11 +11,13 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>rmf_utils</depend>
+  <depend>rmf_api_msgs</depend>
   <depend>rmf_traffic</depend>
   <depend>rmf_traffic_ros2</depend>
   <depend>rmf_task_msgs</depend>
   <depend>rclcpp</depend>
   <depend>nlohmann-json3-dev</depend>
+  <depend>nlohmann_json_schema_validator_vendor</depend>
 
   <build_depend>eigen</build_depend>
 
diff --git a/rmf_task_ros2/src/mock_bidder/main.cpp b/rmf_task_ros2/src/mock_bidder/main.cpp
index f13bc87eb..96022a686 100644
--- a/rmf_task_ros2/src/mock_bidder/main.cpp
+++ b/rmf_task_ros2/src/mock_bidder/main.cpp
@@ -17,14 +17,14 @@
 
 /// Note: This is a testing bidder node script
 
-#include <rmf_task_ros2/bidding/MinimalBidder.hpp>
-#include "../rmf_task_ros2/action/Server.hpp"
+#include <rmf_task_ros2/bidding/AsyncBidder.hpp>
 
 #include <rclcpp/rclcpp.hpp>
 #include <rmf_traffic_ros2/Time.hpp>
 
+#include <nlohmann/json.hpp>
+
 using namespace rmf_task_ros2;
-using TaskType = bidding::MinimalBidder::TaskType;
 
 int main(int argc, char* argv[])
 {
@@ -39,73 +39,31 @@ int main(int argc, char* argv[])
 
   //============================================================================
   // Create Bidder instance
-  std::shared_ptr<bidding::MinimalBidder> bidder = bidding::MinimalBidder::make(
+  std::shared_ptr<bidding::AsyncBidder> bidder = bidding::AsyncBidder::make(
     node,
-    fleet_name,
-    { TaskType::Clean, TaskType::Delivery },
-    [](const bidding::BidNotice& notice)
+    [node](
+      const bidding::BidNoticeMsg& notice,
+      bidding::AsyncBidder::Respond resp)
     {
-      // Here user will provice the best robot as a bid submission
+      // Here user will provide the best robot as a bid submission
       std::cout << "[MockBidder] Providing best estimates" << std::endl;
-      auto req_start_time =
-      rmf_traffic_ros2::convert(notice.task_profile.description.start_time);
-
-      bidding::Submission best_robot_estimate;
-      best_robot_estimate.robot_name = "dumbot";
-      best_robot_estimate.prev_cost = 10.2;
-      best_robot_estimate.new_cost = 13.5;
-      best_robot_estimate.finish_time =
-      rmf_traffic::time::apply_offset(req_start_time, 7);
-      return best_robot_estimate;
-    }
-  );
-
-  //============================================================================
-  // Create sample RMF task action server
-  auto action_server = action::Server::make(node, fleet_name);
-
-  action_server->register_callbacks(
-    [&action_server, &node](const TaskProfile& task_profile)
-    {
-      std::cout << "[MockBidder] ~Start Queue Task: "
-                << task_profile.task_id<<std::endl;
-
-      // async on executing task
-      // auto _ = std::async(std::launch::async,
-      auto t = std::thread(
-        [&action_server, &node](auto profile)
-        {
-          DispatchState status;
-          status.task_profile = profile;
-          status.robot_name = "dumbot";
-          status.start_time = rmf_traffic_ros2::convert(node->now());
-          status.end_time =
-          rmf_traffic::time::apply_offset(status.start_time, 7);
-
-          const auto id = profile.task_id;
-          std::cout << " [MockBidder] Queued, TaskID: "  << id << std::endl;
-          action_server->update_status(status);
-
-          std::this_thread::sleep_for(std::chrono::seconds(2));
-          std::cout << " [MockBidder] Executing, TaskID: " << id << std::endl;
-          status.state = DispatchState::State::Executing;
-          action_server->update_status(status);
-
-          std::this_thread::sleep_for(std::chrono::seconds(5));
-          std::cout << " [MockBidder] Completed, TaskID: " << id << std::endl;
-          status.state = DispatchState::State::Completed;
-          action_server->update_status(status);
-        }, task_profile
-      );
-      t.detach();
-
-      return true; //successs (send State::Queued)
-    },
-    [&action_server](const TaskProfile& task_profile)
-    {
-      std::cout << "[MockBidder] ~Cancel Executing Task: "
-                << task_profile.task_id<<std::endl;
-      return true; //success , send State::Canceled
+      const auto req = nlohmann::json::parse(notice.request);
+      const auto& start_time_json = req["unix_millis_earliest_start_time"];
+      const auto req_start_time = start_time_json?
+        rmf_traffic::Time(
+          std::chrono::milliseconds(start_time_json.get<int64_t>()))
+        : rmf_traffic_ros2::convert(node->now());
+
+      resp(bidding::Response{
+        bidding::Response::Proposal{
+          "mockfleet",
+          "mockbot",
+          10.2,
+          13.5,
+          rmf_traffic::time::apply_offset(req_start_time, 7)
+        },
+        {}
+      });
     }
   );
 
diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
index db676c144..d07f8b9dc 100644
--- a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
+++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
@@ -21,8 +21,6 @@
 #include <rclcpp/node.hpp>
 #include <rclcpp/executors/single_threaded_executor.hpp>
 
-#include "action/Client.hpp"
-
 #include <rmf_task_msgs/msg/task_description.hpp>
 #include <rmf_task_msgs/msg/task_profile.hpp>
 #include <rmf_task_msgs/srv/submit_task.hpp>
@@ -38,13 +36,63 @@
 #include <rmf_traffic_ros2/Time.hpp>
 
 #include <nlohmann/json.hpp>
+#include <nlohmann/json-schema.hpp>
+
+#include <rmf_api_msgs/schemas/dispatch_task_request.hpp>
+#include <rmf_api_msgs/schemas/dispatch_task_response.hpp>
+#include <rmf_api_msgs/schemas/task_request.hpp>
+#include <rmf_api_msgs/schemas/task_state.hpp>
+#include <rmf_api_msgs/schemas/error.hpp>
 
 namespace rmf_task_ros2 {
 
+namespace {
+//==============================================================================
+std::function<void(const nlohmann::json_uri& id, nlohmann::json& value)>
+make_schema_loader()
+{
+  const std::vector<nlohmann::json> schemas = {
+    rmf_api_msgs::schemas::dispatch_task_request,
+    rmf_api_msgs::schemas::dispatch_task_response,
+    rmf_api_msgs::schemas::task_request,
+    rmf_api_msgs::schemas::task_state,
+    rmf_api_msgs::schemas::error
+  };
+
+  std::unordered_map<std::string, nlohmann::json> dictionary;
+  for (const auto& schema : schemas)
+  {
+    const auto json_uri = nlohmann::json_uri(schema["$id"]);
+    dictionary[json_uri.url()] = schema;
+  }
+
+  return [dictionary = std::move(dictionary)](
+    const nlohmann::json_uri& id, nlohmann::json& value)
+    {
+      const auto it = dictionary.find(id.url());
+      if (it != dictionary.end())
+        value = it->second;
+    };
+}
+
+//==============================================================================
+static const
+std::function<void(const nlohmann::json_uri& id, nlohmann::json& value)>
+schema_loader = make_schema_loader();
+
+//==============================================================================
+nlohmann::json_schema::json_validator make_validator(nlohmann::json schema)
+{
+  return nlohmann::json_schema::json_validator(
+    std::move(schema), schema_loader);
+}
+} // anonymous namespace
+
 //==============================================================================
 class Dispatcher::Implementation
 {
 public:
+
   std::shared_ptr<rclcpp::Node> node;
   std::shared_ptr<bidding::Auctioneer> auctioneer;
 
@@ -61,10 +109,44 @@ class Dispatcher::Implementation
   rclcpp::Service<CancelTaskSrv>::SharedPtr cancel_task_srv;
   rclcpp::Service<GetDispatchStatesSrv>::SharedPtr get_dispatch_states_srv;
 
-  using ApiRequest = rmf_task_msgs::msg::ApiRequest;
-  using ApiResponse = rmf_task_msgs::msg::ApiResponse;
-  rclcpp::Subscription<ApiRequest>::SharedPtr api_request;
-  rclcpp::Publisher<ApiResponse>::SharedPtr api_response;
+  using ApiRequestMsg = rmf_task_msgs::msg::ApiRequest;
+  using ApiResponseMsg = rmf_task_msgs::msg::ApiResponse;
+  rclcpp::Subscription<ApiRequestMsg>::SharedPtr api_request;
+  rclcpp::Publisher<ApiResponseMsg>::SharedPtr api_response;
+
+  class ApiMemory
+  {
+  public:
+
+    std::optional<ApiResponseMsg> lookup(const std::string& api_id) const
+    {
+      const auto it = _cached_responses.find(api_id);
+      if (it == _cached_responses.end())
+        return std::nullopt;
+
+      return it->second;
+    }
+
+    void add(ApiResponseMsg msg)
+    {
+      if (_tracker.size() > _max_size)
+      {
+        _cached_responses.erase(_tracker.front());
+        _tracker.pop_front();
+      }
+
+      _tracker.push_back(msg.request_id);
+      _cached_responses[msg.request_id] = std::move(msg);
+    }
+
+  private:
+
+    std::unordered_map<std::string, ApiResponseMsg> _cached_responses;
+    std::list<std::string> _tracker;
+    std::size_t _max_size = 50;
+  };
+
+  ApiMemory api_memory;
 
   using DispatchStatesPub =rclcpp::Publisher<DispatchStatesMsg>;
   DispatchStatesPub::SharedPtr dispatch_states_pub;
@@ -79,12 +161,12 @@ class Dispatcher::Implementation
 
   DispatchStateCallback on_change_fn;
 
-  std::queue<bidding::BidNotice> task_bid_queue;
+  std::queue<bidding::BidNoticeMsg> task_bid_queue;
 
   DispatchStates active_dispatch_states;
   DispatchStates finished_dispatch_states;
   std::size_t task_counter = 0; // index for generating task_id
-  double bidding_time_window;
+  builtin_interfaces::msg::Duration bidding_time_window;
   std::size_t terminated_tasks_max_size;
   int publish_active_tasks_period;
 
@@ -103,10 +185,13 @@ class Dispatcher::Implementation
   : node{std::move(node_)}
   {
     // ros2 param
-    bidding_time_window =
+    double bidding_time_window_param =
       node->declare_parameter<double>("bidding_time_window", 2.0);
     RCLCPP_INFO(node->get_logger(),
-      " Declared Time Window Param as: %f secs", bidding_time_window);
+      " Declared Time Window Param as: %f secs", bidding_time_window_param);
+    bidding_time_window = rmf_traffic_ros2::convert(
+          rmf_traffic::time::from_seconds(bidding_time_window_param));
+
     terminated_tasks_max_size =
       node->declare_parameter<int>("terminated_tasks_max_size", 100);
     RCLCPP_INFO(node->get_logger(),
@@ -122,6 +207,17 @@ class Dispatcher::Implementation
     dispatch_states_pub = node->create_publisher<DispatchStatesMsg>(
       rmf_task_ros2::DispatchStatesTopicName, qos);
 
+    // TODO(MXG): Sync up with rmf_fleet_adapter/StandardNames on these topic
+    // names
+    api_request = node->create_subscription<ApiRequestMsg>(
+      "task_api_requests",
+      rclcpp::SystemDefaultsQoS().reliable().transient_local(),
+      [this](const ApiRequestMsg& msg) { this->handle_api_request(msg); });
+
+    api_response = node->create_publisher<ApiResponseMsg>(
+      "task_api_responses",
+      rclcpp::SystemDefaultsQoS().reliable().transient_local());
+
     // TODO(MXG): The smallest resolution this supports is 1 second. That
     // doesn't seem great.
     dispatch_states_pub_timer = node->create_wall_timer(
@@ -149,11 +245,12 @@ class Dispatcher::Implementation
         node,
         [this](
           const TaskID& task_id,
-          const std::optional<bidding::Submission> winner,
+          const std::optional<bidding::Response::Proposal> winner,
           const std::vector<std::string>& errors)
         {
           this->conclude_bid(task_id, std::move(winner), errors);
-        });
+        },
+        std::make_shared<bidding::LeastFleetDiffCostEvaluator>());
 
     // Setup up stream srv interfaces
     submit_task_srv = node->create_service<SubmitTaskSrv>(
@@ -187,7 +284,7 @@ class Dispatcher::Implementation
     );
 
     get_dispatch_states_srv = node->create_service<GetDispatchStatesSrv>(
-      rmf_task_ros2::GetTaskListSrvName,
+      rmf_task_ros2::GetDispatchStatesSrvName,
       [this](
         const std::shared_ptr<GetDispatchStatesSrv::Request> request,
         std::shared_ptr<GetDispatchStatesSrv::Response> response)
@@ -274,6 +371,94 @@ class Dispatcher::Implementation
       };
   }
 
+  void handle_api_request(const ApiRequestMsg& msg)
+  {
+    const auto check = api_memory.lookup(msg.request_id);
+    if (check.has_value())
+    {
+      api_response->publish(*check);
+      return;
+    }
+
+    const auto msg_json = nlohmann::json::parse(msg.json_msg);
+    const auto& type = msg_json["type"];
+    if (!type)
+      return;
+
+    try
+    {
+      const auto& type_str = type.get<std::string>();
+      if (type_str != "dispatch_task_request")
+        return;
+
+      static const auto request_validator =
+        make_validator(rmf_api_msgs::schemas::dispatch_task_request);
+
+      try
+      {
+        request_validator.validate(msg_json);
+      }
+      catch (const std::exception& e)
+      {
+        nlohmann::json error;
+        error["code"] = 5;
+        error["category"] = "Invalid request format";
+        error["detail"] = e.what();
+
+        nlohmann::json response_json;
+        response_json["success"] = false;
+        response_json["errors"] =
+          std::vector<nlohmann::json>({std::move(error)});
+
+        auto response = rmf_task_msgs::build<ApiResponseMsg>()
+            .type(ApiResponseMsg::TYPE_RESPONDING)
+            .json_msg(std::move(response_json))
+            .request_id(msg.request_id);
+
+        api_memory.add(response);
+        api_response->publish(response);
+      }
+
+      const std::string task_id = "dispatch#" + std::to_string(task_counter++);
+      const auto task_request_json = msg_json["request"];
+      push_bid_notice(
+        rmf_task_msgs::build<bidding::BidNoticeMsg>()
+          .request(task_request_json.dump())
+          .task_id(task_id)
+          .time_window(bidding_time_window));
+
+      nlohmann::json response_json;
+      response_json["success"] = true;
+
+      auto& task_state = response_json["state"];
+      auto& booking = task_state["booking"];
+      booking["id"] = task_id;
+      booking["unix_millis_earliest_start_time"] =
+          task_request_json["unix_millis_earliest_start_time"];
+      booking["priority"] = task_request_json["priority"];
+      booking["labels"] = task_request_json["labels"];
+
+      auto& dispatch = task_state["dispatch"];
+      dispatch["status"] = "queued";
+
+      auto response = rmf_task_msgs::build<ApiResponseMsg>()
+        .type(ApiResponseMsg::TYPE_RESPONDING)
+        .json_msg(task_state.dump())
+        .request_id(msg.request_id);
+
+      api_memory.add(response);
+      api_response->publish(response);
+
+      // TODO(MXG): Make some way to keep pushing task state updates to the
+      // api-server as the bidding process progresses. We could do a websocket
+      // connection or maybe just a simple ROS2 publisher.
+    }
+    catch (const std::exception&)
+    {
+      // Do nothing. The message is not meant for us.
+    }
+  }
+
   std::optional<TaskID> submit_task(const TaskDescription& submission)
   {
     const auto task_type_index = submission.task_type.type;
@@ -306,16 +491,16 @@ class Dispatcher::Implementation
     task_request["description"] = legacy_task_types.at(category)(submission);
     task_request["labels"] = std::vector<std::string>({"legacy_request"});
 
-    bidding::BidNotice bid_notice;
-    bid_notice.request = task_request.dump();
-    bid_notice.time_window = rmf_traffic_ros2::convert(
-      rmf_traffic::time::from_seconds(bidding_time_window));
-    push_bid_notice(std::move(bid_notice));
+    push_bid_notice(
+      rmf_task_msgs::build<bidding::BidNoticeMsg>()
+        .request(task_request.dump())
+        .task_id(task_id)
+        .time_window(bidding_time_window));
 
     return task_id;
   }
 
-  void push_bid_notice(bidding::BidNotice bid_notice)
+  void push_bid_notice(bidding::BidNoticeMsg bid_notice)
   {
     nlohmann::json state;
     auto& booking = state["booking"];
@@ -455,7 +640,7 @@ class Dispatcher::Implementation
 
   void conclude_bid(
     const TaskID& task_id,
-    const std::optional<bidding::Submission> winner,
+    const std::optional<bidding::Response::Proposal> winner,
     const std::vector<std::string>& errors)
   {
     const auto it = active_dispatch_states.find(task_id);
@@ -530,7 +715,7 @@ class Dispatcher::Implementation
     dispatch_state->assignment =
       DispatchState::Assignment{
         winner->fleet_name,
-        winner->robot_name
+        winner->expected_robot_name
       };
 
     RCLCPP_INFO(
@@ -539,7 +724,7 @@ class Dispatcher::Implementation
       "with expected robot [%s].",
       task_id.c_str(),
       winner->fleet_name.c_str(),
-      winner->robot_name.c_str());
+      winner->expected_robot_name.c_str());
 
     auto award_command = rmf_task_msgs::build<DispatchCommandMsg>()
         .fleet_name(winner->fleet_name)
@@ -780,10 +965,9 @@ void Dispatcher::on_change(DispatchStateCallback on_change_fn)
 }
 
 //==============================================================================
-void Dispatcher::evaluator(
-  std::shared_ptr<bidding::Auctioneer::Evaluator> evaluator)
+void Dispatcher::evaluator(bidding::Auctioneer::ConstEvaluatorPtr evaluator)
 {
-  _pimpl->auctioneer->select_evaluator(evaluator);
+  _pimpl->auctioneer->set_evaluator(std::move(evaluator));
 }
 
 //==============================================================================
diff --git a/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp b/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp
deleted file mode 100644
index 5c8b84581..000000000
--- a/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp
+++ /dev/null
@@ -1,236 +0,0 @@
-/*
- * Copyright (C) 2020 Open Source Robotics Foundation
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *
-*/
-
-#include "Client.hpp"
-
-namespace rmf_task_ros2 {
-namespace action {
-
-std::shared_ptr<Client>
-Client::make(std::shared_ptr<rclcpp::Node> node)
-{
-  return std::shared_ptr<Client>(new Client(node));
-}
-
-//==============================================================================
-Client::Client(std::shared_ptr<rclcpp::Node> node)
-: _node(node)
-{
-  const auto dispatch_qos = rclcpp::ServicesQoS().reliable();
-
-  _request_msg_pub = _node->create_publisher<RequestMsg>(
-    DispatchCommandTopicName, dispatch_qos);
-
-  _status_msg_sub = _node->create_subscription<StatusMsg>(
-    TaskStatusTopicName, dispatch_qos,
-    [&](const std::unique_ptr<StatusMsg> msg)
-    {
-      const auto task_id = msg->task_profile.task_id;
-      if (task_id.empty())
-        return;
-
-      // status update, check if task_id is previously known
-      if (_active_task_status.count(task_id))
-      {
-        auto weak_status = _active_task_status[task_id].lock();
-
-        if (!weak_status)
-        {
-          RCLCPP_INFO(_node->get_logger(), "Task was previously terminated");
-          _active_task_status.erase(task_id);
-          return;
-        }
-
-        // TODO: hack to retain task profile and fleet name (to remove)
-        auto cache_profile = weak_status->task_profile;
-        // update status to ptr
-        *weak_status = convert_status(*msg);
-        weak_status->task_profile = cache_profile;
-
-        if (weak_status->is_terminated())
-          RCLCPP_INFO(_node->get_logger(),
-          "Received status from fleet [%s], task [%s] is now terminated",
-          msg->fleet_name.c_str(), task_id.c_str());
-
-        update_task_status(weak_status);
-      }
-      else
-      {
-        // will still provide onchange even if the task_id is unknown.
-        RCLCPP_DEBUG(_node->get_logger(),
-        "[action] Unknown task: [%s]", task_id.c_str());
-        auto task_status = std::make_shared<DispatchState>(convert_status(*msg));
-        _active_task_status[task_id] = task_status;
-        update_task_status(task_status);
-      }
-    });
-
-  _ack_msg_sub = _node->create_subscription<AckMsg>(
-    DispatchAckTopicName, dispatch_qos,
-    [&](const std::unique_ptr<AckMsg> msg)
-    {
-      const auto task_id = msg->dispatch_request.task_profile.task_id;
-      const auto weak_status = _active_task_status[task_id].lock();
-
-      switch (msg->dispatch_request.method)
-      {
-        case RequestMsg::ADD:
-          if (msg->success)
-          {
-            // update this as pending
-            RCLCPP_INFO(_node->get_logger(),
-            "Received dispatch ack from fleet [%s] that task [%s] is queued",
-            msg->dispatch_request.fleet_name.c_str(), task_id.c_str());
-            weak_status->state = DispatchState::State::Queued;
-          }
-          else
-          {
-            // update this as failed
-            RCLCPP_ERROR(_node->get_logger(),
-            "Received dispatch ack from fleet [%s] that task [%s] Add Failed",
-            msg->dispatch_request.fleet_name.c_str(), task_id.c_str());
-            weak_status->state = DispatchState::State::Failed;
-          }
-          break;
-        case RequestMsg::CANCEL:
-          // update this as Canceled
-          if (msg->success)
-          {
-            RCLCPP_INFO(_node->get_logger(),
-            "Received dispatch ack from fleet [%s] that task [%s] is canceled",
-            msg->dispatch_request.fleet_name.c_str(), task_id.c_str());
-            weak_status->state = DispatchState::State::Canceled;
-          }
-          break;
-        default:
-          RCLCPP_ERROR(_node->get_logger(), "Invalid Dispatch ack method");
-          return;
-      }
-
-      update_task_status(weak_status);
-    });
-}
-
-//==============================================================================
-void Client::update_task_status(const TaskStatusPtr status)
-{
-  // call on_change callback
-  if (_on_change_callback)
-    _on_change_callback(status);
-
-  // erase terminated task and call on_terminate callback
-  if (status->is_terminated())
-  {
-    _active_task_status.erase(status->task_profile.task_id);
-    if (_on_terminate_callback)
-      _on_terminate_callback(status);
-  }
-}
-
-//==============================================================================
-// check if task is updated TODO
-
-//==============================================================================
-void Client::add_task(
-  const std::string& fleet_name,
-  const TaskProfile& task_profile,
-  TaskStatusPtr status_ptr)
-{
-  // send request and wait for acknowledgement
-  RequestMsg request_msg;
-  request_msg.fleet_name = fleet_name;
-  request_msg.task_profile = task_profile;
-  request_msg.method = RequestMsg::ADD;
-  _request_msg_pub->publish(request_msg);
-
-  // save status ptr
-  status_ptr->fleet_name = fleet_name;
-  status_ptr->task_profile = task_profile;
-  _active_task_status[task_profile.task_id] = status_ptr;
-  RCLCPP_DEBUG(_node->get_logger(), "Assign task: [%s] to fleet [%s]",
-    task_profile.task_id.c_str(), fleet_name.c_str());
-  return;
-}
-
-//==============================================================================
-bool Client::cancel_task(
-  const TaskProfile& task_profile)
-{
-  const auto task_id = task_profile.task_id;
-  RCLCPP_DEBUG(_node->get_logger(),
-    "[action] Cancel Task: [%s]", task_id.c_str());
-
-  // check if task is previously added
-  if (!_active_task_status.count(task_id))
-  {
-    RCLCPP_WARN(_node->get_logger(),
-      "Canceling an unknown task [%s]", task_id.c_str());
-    return false;
-  }
-
-  auto weak_status = _active_task_status[task_id].lock();
-
-  if (!weak_status)
-  {
-    RCLCPP_WARN(_node->get_logger(), "Task was previously terminated");
-    _active_task_status.erase(task_id);
-    return false;
-  }
-
-  // send cancel
-  RequestMsg request_msg;
-  request_msg.fleet_name = weak_status->fleet_name;
-  request_msg.task_profile = task_profile;
-  request_msg.method = RequestMsg::CANCEL;
-  _request_msg_pub->publish(request_msg);
-  return true;
-}
-
-//==============================================================================
-int Client::size()
-{
-  for (auto it = _active_task_status.begin(); it != _active_task_status.end(); )
-  {
-    if (auto weak_status = it->second.lock() )
-    {
-      if (weak_status->is_terminated())
-        it = _active_task_status.erase(it);
-      else
-        ++it;
-    }
-    else
-      it = _active_task_status.erase(it);
-  }
-  return _active_task_status.size();
-}
-
-//==============================================================================
-void Client::on_change(
-  StatusCallback status_cb_fn)
-{
-  _on_change_callback = std::move(status_cb_fn);
-}
-
-//==============================================================================
-void Client::on_terminate(
-  StatusCallback status_cb_fn)
-{
-  _on_terminate_callback = std::move(status_cb_fn);
-}
-
-} // namespace action
-} // namespace rmf_task_ros2
diff --git a/rmf_task_ros2/src/rmf_task_ros2/action/Client.hpp b/rmf_task_ros2/src/rmf_task_ros2/action/Client.hpp
deleted file mode 100644
index 40c8b67cb..000000000
--- a/rmf_task_ros2/src/rmf_task_ros2/action/Client.hpp
+++ /dev/null
@@ -1,113 +0,0 @@
-/*
- * Copyright (C) 2020 Open Source Robotics Foundation
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *
-*/
-
-#ifndef SRC__RMF_TASK_ROS2__ACTION__CLIENT_HPP
-#define SRC__RMF_TASK_ROS2__ACTION__CLIENT_HPP
-
-#include <rclcpp/node.hpp>
-
-#include <rmf_task_ros2/StandardNames.hpp>
-#include <rmf_task_ros2/TaskStatus.hpp>
-#include <rmf_task_msgs/msg/dispatch_request.hpp>
-#include <rmf_task_msgs/msg/dispatch_ack.hpp>
-
-namespace rmf_task_ros2 {
-namespace action {
-
-//==============================================================================
-// Task Action Client -- responsible for initiating a rmf task to a target
-// fleet. The fleet will work on the requested task and provides a status to
-// the client when the task progresses. Termination will be triggered when the
-// task ends.
-
-class Client
-{
-public:
-  /// make an action client
-  ///
-  /// \param[in] node
-  ///   ros2 node instance
-  static std::shared_ptr<Client> make(
-    std::shared_ptr<rclcpp::Node> node);
-
-  /// Add a task to a targeted fleet
-  ///
-  /// \param[in] fleet_name
-  ///   Target fleet which will execute this task
-  ///
-  /// \param[in] task_profile
-  ///   Task Description which will be executed
-  ///
-  /// \param[out] status_ptr
-  ///   Will update the status of the task here
-  void add_task(
-    const std::string& fleet_name,
-    const TaskProfile& task_profile,
-    TaskStatusPtr status_ptr);
-
-  /// Cancel an added task
-  ///
-  /// \param[in] task_profile
-  ///   Task which to cancel
-  ///
-  /// \return bool which indicate if cancel task is success
-  bool cancel_task(const TaskProfile& task_profile);
-
-  /// Get the number of active task being track by client
-  ///
-  /// \return number of active task
-  int size();
-
-  /// Callback Function which will trigger during an event
-  ///
-  /// \param[in] status
-  ///   Status of a task which the event is triggered
-  using StatusCallback = std::function<void(const TaskStatusPtr status)>;
-
-  /// Callback when a task status has changed
-  ///
-  /// \param[in] status_cb_fn
-  ///   Status callback function
-  void on_change(StatusCallback status_cb_fn);
-
-  /// Callback when a task is terminated
-  ///
-  /// \param[in] status_cb_fn
-  ///   Status callback function
-  void on_terminate(StatusCallback status_cb_fn);
-
-private:
-  Client(std::shared_ptr<rclcpp::Node> node);
-
-  void update_task_status(const TaskStatusPtr status);
-
-  using RequestMsg = rmf_task_msgs::msg::DispatchCommand;
-  using AckMsg = rmf_task_msgs::msg::DispatchAck;
-
-  std::shared_ptr<rclcpp::Node> _node;
-  StatusCallback _on_change_callback;
-  StatusCallback _on_terminate_callback;
-  std::unordered_map<TaskID, std::weak_ptr<DispatchState>> _active_task_status;
-  rclcpp::Publisher<RequestMsg>::SharedPtr _request_msg_pub;
-  rclcpp::Subscription<StatusMsg>::SharedPtr _status_msg_sub;
-  rclcpp::Subscription<AckMsg>::SharedPtr _ack_msg_sub;
-};
-
-} // namespace action
-} // namespace rmf_task_ros2
-
-#endif // SRC__RMF_TASK_ROS2__ACTION__CLIENT_HPP
diff --git a/rmf_task_ros2/src/rmf_task_ros2/action/Server.cpp b/rmf_task_ros2/src/rmf_task_ros2/action/Server.cpp
deleted file mode 100644
index 0eb8d13be..000000000
--- a/rmf_task_ros2/src/rmf_task_ros2/action/Server.cpp
+++ /dev/null
@@ -1,99 +0,0 @@
-/*
- * Copyright (C) 2020 Open Source Robotics Foundation
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *
-*/
-
-#include "Server.hpp"
-
-namespace rmf_task_ros2 {
-namespace action {
-
-std::shared_ptr<Server> Server::make(
-  std::shared_ptr<rclcpp::Node> node,
-  const std::string& fleet_name)
-{
-  return std::shared_ptr<Server>(new Server(node, fleet_name));
-}
-
-//==============================================================================
-void Server::register_callbacks(
-  AddTaskCallback add_task_cb_fn,
-  CancelTaskCallback cancel_task_cb_fn)
-{
-  _add_task_cb_fn = std::move(add_task_cb_fn);
-  _cancel_task_cb_fn = std::move(cancel_task_cb_fn);
-}
-
-//==============================================================================
-void Server::update_status(
-  const DispatchState& task_status)
-{
-  auto msg = convert_status(task_status);
-  msg.fleet_name = _fleet_name;
-  _status_msg_pub->publish(msg);
-}
-
-//==============================================================================
-Server::Server(
-  std::shared_ptr<rclcpp::Node> node,
-  const std::string& fleet_name)
-: _node(node), _fleet_name(fleet_name)
-{
-  const auto dispatch_qos = rclcpp::ServicesQoS().reliable();
-
-  _request_msg_sub = _node->create_subscription<RequestMsg>(
-    DispatchCommandTopicName, dispatch_qos,
-    [&](const std::unique_ptr<RequestMsg> msg)
-    {
-      if (msg->fleet_name != _fleet_name)
-        return;// not me
-
-      RCLCPP_INFO(_node->get_logger(),
-      "[Action server] Received task request!");
-      AckMsg ack_msg;
-      ack_msg.dispatch_request = *msg;
-      ack_msg.success = false;
-
-      switch (msg->method)
-      {
-        // Add Request
-        case RequestMsg::ADD:
-          {
-            if (_add_task_cb_fn && _add_task_cb_fn(msg->task_profile))
-              ack_msg.success = true;
-            break;
-          }
-        // Cancel Request
-        case RequestMsg::CANCEL:
-          {
-            if (_cancel_task_cb_fn && _cancel_task_cb_fn(msg->task_profile))
-              ack_msg.success = true;
-            break;
-          }
-        default:
-          RCLCPP_ERROR(_node->get_logger(), "Request Method is not supported!");
-      }
-      _ack_msg_pub->publish(ack_msg);
-    });
-
-  _ack_msg_pub = _node->create_publisher<AckMsg>(
-    DispatchAckTopicName, dispatch_qos);
-
-  _status_msg_pub = _node->create_publisher<StatusMsg>(
-    TaskStatusTopicName, dispatch_qos);
-}
-
-} // namespace action
-} // namespace rmf_task_ros2
diff --git a/rmf_task_ros2/src/rmf_task_ros2/action/Server.hpp b/rmf_task_ros2/src/rmf_task_ros2/action/Server.hpp
deleted file mode 100644
index e2872ec33..000000000
--- a/rmf_task_ros2/src/rmf_task_ros2/action/Server.hpp
+++ /dev/null
@@ -1,94 +0,0 @@
-/*
- * Copyright (C) 2020 Open Source Robotics Foundation
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *
-*/
-
-#ifndef SRC__RMF_TASK_ROS2__ACTION__SERVER_HPP
-#define SRC__RMF_TASK_ROS2__ACTION__SERVER_HPP
-
-#include <rclcpp/node.hpp>
-
-#include <rmf_task_ros2/StandardNames.hpp>
-#include <rmf_task_ros2/TaskStatus.hpp>
-#include <rmf_task_msgs/msg/dispatch_request.hpp>
-#include <rmf_task_msgs/msg/dispatch_ack.hpp>
-#include <rmf_traffic/Time.hpp>
-
-using TaskProfile = rmf_task_msgs::msg::TaskProfile;
-
-namespace rmf_task_ros2 {
-namespace action {
-
-//==============================================================================
-/// Task Action Server - This is used within the fleet adapter with the role of
-/// receiving incoming dispatch requests (from a action_client/Dispatcher),
-/// then execute the task accordingly.
-class Server
-{
-public:
-  /// initialize action server
-  ///
-  /// \param[in] node
-  ///   ros2 node instance
-  ///
-  /// \param[in] fleet_name
-  ///   action server id (e.g. fleet adapter name)
-  static std::shared_ptr<Server> make(
-    std::shared_ptr<rclcpp::Node> node,
-    const std::string& fleet_name);
-
-  using AddTaskCallback = std::function<bool(const TaskProfile& task)>;
-  using CancelTaskCallback = std::function<bool(const TaskProfile& task)>;
-
-  /// Add event callback functions. These functions will be triggered when a
-  /// relevant task requests msg is received.
-  ///
-  /// \param[in] add_task_cb
-  ///   When a new task request is called.
-  ///
-  /// \param[in] cancel_task_cb
-  ///   when a cancel task request is called
-  void register_callbacks(
-    AddTaskCallback add_task_cb,
-    CancelTaskCallback cancel_task_cb);
-
-  /// Use this to send a status update to action client
-  /// A On Change update is recommended to inform the task progress
-  ///
-  /// \param[in] task_status
-  ///   latest status of the task
-  void update_status(const DispatchState& task_status);
-
-private:
-  Server(
-    std::shared_ptr<rclcpp::Node> node,
-    const std::string& fleet_name);
-
-  using RequestMsg = rmf_task_msgs::msg::DispatchCommand;
-  using AckMsg = rmf_task_msgs::msg::DispatchAck;
-
-  std::shared_ptr<rclcpp::Node> _node;
-  std::string _fleet_name;
-  AddTaskCallback _add_task_cb_fn;
-  CancelTaskCallback _cancel_task_cb_fn;
-  rclcpp::Subscription<RequestMsg>::SharedPtr _request_msg_sub;
-  rclcpp::Publisher<StatusMsg>::SharedPtr _status_msg_pub;
-  rclcpp::Publisher<AckMsg>::SharedPtr _ack_msg_pub;
-};
-
-} // namespace action
-} // namespace rmf_task_ros2
-
-#endif // SRC__RMF_TASK_ROS2__ACTION__SERVER_HPP
diff --git a/rmf_task_ros2/src/rmf_task_ros2/bidding/AsyncBidder.cpp b/rmf_task_ros2/src/rmf_task_ros2/bidding/AsyncBidder.cpp
new file mode 100644
index 000000000..c2fa0a535
--- /dev/null
+++ b/rmf_task_ros2/src/rmf_task_ros2/bidding/AsyncBidder.cpp
@@ -0,0 +1,107 @@
+/*
+ * 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_task_ros2/bidding/Response.hpp>
+#include <rmf_task_ros2/bidding/AsyncBidder.hpp>
+
+#include <rmf_traffic_ros2/Time.hpp>
+#include <rmf_task_msgs/msg/bid_proposal.hpp>
+#include <rmf_task_ros2/StandardNames.hpp>
+
+namespace rmf_task_ros2 {
+namespace bidding {
+
+//==============================================================================
+class AsyncBidder::Implementation
+{
+public:
+
+  std::weak_ptr<rclcpp::Node> w_node;
+  ReceiveNotice receive_notice;
+
+  using BidNoticeSub = rclcpp::Subscription<BidNoticeMsg>;
+  BidNoticeSub::SharedPtr bid_notice_sub;
+
+  using BidResponsePub = rclcpp::Publisher<BidResponseMsg>;
+  BidResponsePub::SharedPtr bid_response_pub;
+
+  Implementation(
+    std::shared_ptr<rclcpp::Node> node_,
+    ReceiveNotice receive_notice)
+  : w_node{std::move(node_)},
+    receive_notice{std::move(receive_notice)}
+  {
+    const auto bid_qos = rclcpp::ServicesQoS().reliable();
+    const auto node = w_node.lock();
+
+    bid_notice_sub = node->create_subscription<BidNoticeMsg>(
+      rmf_task_ros2::BidNoticeTopicName, bid_qos,
+      [&](const BidNoticeMsg::UniquePtr msg)
+      {
+        this->handle_notice(*msg);
+      });
+
+    bid_response_pub = node->create_publisher<BidResponseMsg>(
+      rmf_task_ros2::BidResponseTopicName, bid_qos);
+  }
+
+  // Callback fn when a dispatch notice is received
+  void handle_notice(const BidNoticeMsg& msg)
+  {
+    const auto node = w_node.lock();
+    if (!node)
+      return;
+
+    RCLCPP_INFO(node->get_logger(),
+      "[Bidder] Received Bidding notice for task_id [%s]",
+      msg.task_id.c_str());
+
+    // check if the user did not supply a receive notice callback
+    if (!receive_notice)
+      return;
+
+    // Send the notice
+    receive_notice(
+      msg,
+      [task_id = msg.task_id, pub = bid_response_pub](
+        const Response& response)
+      {
+        pub->publish(convert(response, task_id));
+      });
+  }
+};
+
+//==============================================================================
+std::shared_ptr<AsyncBidder> AsyncBidder::make(
+  const std::shared_ptr<rclcpp::Node>& node,
+  ReceiveNotice receive_notice)
+{
+  auto bidder = std::shared_ptr<AsyncBidder>(new AsyncBidder());
+  bidder->_pimpl =
+    rmf_utils::make_unique_impl<Implementation>(node, receive_notice);
+
+  return bidder;
+}
+
+//==============================================================================
+AsyncBidder::AsyncBidder()
+{
+  // do nothing
+}
+
+} // namespace bidding
+} // namespace rmf_task_ros2
diff --git a/rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.cpp b/rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.cpp
index 6f1f27030..d7daef9d5 100644
--- a/rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.cpp
+++ b/rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.cpp
@@ -21,21 +21,8 @@ namespace rmf_task_ros2 {
 namespace bidding {
 
 //==============================================================================
-Submission convert(const BidProposal& from)
-{
-  Submission submission;
-  submission.fleet_name = from.fleet_name;
-  submission.robot_name = from.robot_name;
-  submission.prev_cost = from.prev_cost;
-  submission.new_cost = from.new_cost;
-  submission.finish_time = rmf_traffic_ros2::convert(from.finish_time);
-  return submission;
-}
-
-//==============================================================================
-Auctioneer::Implementation::Implementation(
-  const std::shared_ptr<rclcpp::Node>& node_,
-  BiddingResultCallback result_callback)
+Auctioneer::Implementation::Implementation(const std::shared_ptr<rclcpp::Node>& node_,
+  BiddingResultCallback result_callback, ConstEvaluatorPtr evaluator)
 : node{std::move(node_)},
   bidding_result_callback{std::move(result_callback)}
 {
@@ -46,53 +33,61 @@ Auctioneer::Implementation::Implementation(
     "Dispatcher evaluator set to QuickestFinishEvaluator");
   const auto dispatch_qos = rclcpp::ServicesQoS().reliable();
 
-  bid_notice_pub = node->create_publisher<BidNotice>(
+  bid_notice_pub = node->create_publisher<BidNoticeMsg>(
     rmf_task_ros2::BidNoticeTopicName, dispatch_qos);
 
-  bid_proposal_sub = node->create_subscription<BidProposal>(
-    rmf_task_ros2::BidProposalTopicName, dispatch_qos,
-    [&](const BidProposal::UniquePtr msg)
+  bid_proposal_sub = node->create_subscription<BidResponseMsg>(
+    rmf_task_ros2::BidResponseTopicName, dispatch_qos,
+    [&](const BidResponseMsg::UniquePtr msg)
     {
-      this->receive_proposal(*msg);
+      this->receive_response(*msg);
     });
 
   timer = node->create_wall_timer(std::chrono::milliseconds(200), [&]()
       {
-        this->check_bidding_process();
+        this->finish_bidding_process();
       });
 }
 
 //==============================================================================
 void Auctioneer::Implementation::start_bidding(
-  const BidNotice& bid_notice)
+  const BidNoticeMsg& bid_notice)
 {
   RCLCPP_INFO(node->get_logger(), "Add Task [%s] to a bidding queue",
     bid_notice.task_id.c_str());
 
-  OpenBid bidding_task;
-  bidding_task.bid_notice = bid_notice;
-  bidding_task.start_time = node->now();
-  open_bid_queue.push(bidding_task);
+  open_bid_queue.push(OpenBid{bid_notice, node->now(), {}});
 }
 
 //==============================================================================
-void Auctioneer::Implementation::receive_proposal(
-  const BidProposal& msg)
+void Auctioneer::Implementation::receive_response(const BidResponseMsg& msg)
 {
-  const auto id = msg.task_profile.task_id;
-  RCLCPP_DEBUG(node->get_logger(),
-    "[Auctioneer] Receive proposal from task_id: %s | from: %s",
-    id.c_str(), msg.fleet_name.c_str());
+  const auto id = msg.task_id;
+
+  const auto response = convert(msg);
+  if (response.proposal.has_value())
+  {
+    RCLCPP_DEBUG(node->get_logger(),
+      "[Auctioneer] Receive proposal from task_id: %s | from: %s",
+      id.c_str(), response.proposal->fleet_name.c_str());
+  }
+  else if (!response.errors.empty())
+  {
+    RCLCPP_DEBUG(
+      node->get_logger(),
+      "[Auctioneer] Received %lu errors from a bidder",
+      response.errors.size());
+  }
 
   // check if bidding task is initiated by the auctioneer previously
   // add submited proposal to the current bidding tasks list
-  if (open_bid_queue.front().bid_notice.task_profile.task_id == id)
-    open_bid_queue.front().submissions.push_back(convert(msg));
+  if (open_bid_queue.front().bid_notice.task_id == id)
+    open_bid_queue.front().responses.push_back(response);
 }
 
 //==============================================================================
 // determine the winner within a bidding task instance
-void Auctioneer::Implementation::check_bidding_process()
+void Auctioneer::Implementation::finish_bidding_process()
 {
   if (open_bid_queue.size() == 0)
     return;
@@ -111,7 +106,7 @@ void Auctioneer::Implementation::check_bidding_process()
   else
   {
     RCLCPP_DEBUG(node->get_logger(), " - Start new bidding task: %s",
-      front_task.bid_notice.task_profile.task_id.c_str());
+      front_task.bid_notice.task_id.c_str());
     open_bid_queue.front().start_time = node->now();
     bid_notice_pub->publish(front_task.bid_notice);
     bidding_in_proccess = true;
@@ -123,43 +118,54 @@ bool Auctioneer::Implementation::determine_winner(
   const OpenBid& bidding_task)
 {
   const auto duration = node->now() - bidding_task.start_time;
+  if (duration < bidding_task.bid_notice.time_window)
+    return false;
 
-  if (duration > bidding_task.bid_notice.time_window)
-  {
-    auto id = bidding_task.bid_notice.task_profile.task_id;
-    RCLCPP_DEBUG(node->get_logger(), "Bidding Deadline reached: %s",
-      id.c_str());
-    std::optional<Submission> winner = std::nullopt;
+  if (!bidding_result_callback)
+    return true;
 
-    if (bidding_task.submissions.size() == 0)
-    {
-      RCLCPP_DEBUG(node->get_logger(),
-        "Bidding task has not received any bids");
-    }
-    else
-    {
-      winner = evaluate(bidding_task.submissions);
-      RCLCPP_INFO(
-        node->get_logger(),
-        "Determined winning Fleet Adapter: [%s], from %ld submissions",
-        winner->fleet_name.c_str(),
-        bidding_task.submissions.size());
-    }
+  auto task_id = bidding_task.bid_notice.task_id;
+  RCLCPP_DEBUG(
+    node->get_logger(),
+    "Bidding Deadline reached: %s",
+    task_id.c_str());
+
+  std::vector<std::string> errors;
+  for (const auto& r : bidding_task.responses)
+  {
+    errors.insert(errors.end(), r.errors.begin(), r.errors.end());
+  }
 
-    // Call the user defined callback function
-    if (bidding_result_callback)
-      bidding_result_callback(id, winner);
+  if (bidding_task.responses.empty())
+  {
+    RCLCPP_DEBUG(node->get_logger(),
+      "Bidding task has not received any bids");
 
+    bidding_result_callback(task_id, std::nullopt, errors);
     return true;
   }
-  return false;
+
+  auto winner = evaluate(bidding_task.responses);
+  if (winner.has_value())
+  {
+    RCLCPP_INFO(
+      node->get_logger(),
+      "Determined winning Fleet Adapter: [%s], from %ld responses",
+      winner->fleet_name.c_str(),
+      bidding_task.responses.size());
+  }
+
+  // Call the user defined callback function
+  bidding_result_callback(task_id, winner, errors);
+
+  return true;
 }
 
 //==============================================================================
-std::optional<Submission> Auctioneer::Implementation::evaluate(
-  const Submissions& submissions)
+std::optional<Response::Proposal> Auctioneer::Implementation::evaluate(
+  const Responses& responses)
 {
-  if (submissions.size() == 0)
+  if (responses.size() == 0)
     return std::nullopt;
 
   if (!evaluator)
@@ -168,35 +174,37 @@ std::optional<Submission> Auctioneer::Implementation::evaluate(
     return std::nullopt;
   }
 
-  const std::size_t choice = evaluator->choose(submissions);
+  const auto choice = evaluator->choose(responses);
+  if (!choice.has_value())
+    return std::nullopt;
 
-  if (choice >= submissions.size())
+  if (*choice >= responses.size())
     return std::nullopt;
 
-  return submissions[choice];
+  return responses[*choice].proposal;
 }
 
 //==============================================================================
 std::shared_ptr<Auctioneer> Auctioneer::make(
   const std::shared_ptr<rclcpp::Node>& node,
-  BiddingResultCallback result_callback)
+  BiddingResultCallback result_callback,
+  ConstEvaluatorPtr evaluator)
 {
   auto auctioneer = std::shared_ptr<Auctioneer>(new Auctioneer());
   auctioneer->_pimpl = rmf_utils::make_unique_impl<Implementation>(
-    node, std::move(result_callback));
+    node, std::move(result_callback), std::move(evaluator));
 
   return auctioneer;
 }
 
 //==============================================================================
-void Auctioneer::start_bidding(const BidNotice& bid_notice)
+void Auctioneer::start_bidding(const BidNoticeMsg& bid_notice)
 {
   _pimpl->start_bidding(bid_notice);
 }
 
 //==============================================================================
-void Auctioneer::select_evaluator(
-  std::shared_ptr<Auctioneer::Evaluator> evaluator)
+void Auctioneer::set_evaluator(ConstEvaluatorPtr evaluator)
 {
   _pimpl->evaluator = std::move(evaluator);
 }
@@ -207,51 +215,60 @@ Auctioneer::Auctioneer()
   // do nothing
 }
 
+namespace {
 //==============================================================================
-std::size_t LeastFleetDiffCostEvaluator::choose(
-  const Submissions& submissions) const
+std::optional<std::size_t> select_best(
+  const Responses& responses,
+  const std::function<double(const Response::Proposal&)> eval)
 {
-  auto winner_it = submissions.begin();
-  float winner_cost_diff = winner_it->new_cost - winner_it->prev_cost;
-  for (auto nominee_it = ++submissions.begin();
-    nominee_it != submissions.end(); ++nominee_it)
+  std::optional<std::size_t> best_index;
+  std::optional<double> best_cost;
+  for (std::size_t i=0; i < responses.size(); ++i)
   {
-    float nominee_cost_diff = nominee_it->new_cost - nominee_it->prev_cost;
-    if (nominee_cost_diff < winner_cost_diff)
+    if (responses[i].proposal.has_value())
     {
-      winner_it = nominee_it;
-      winner_cost_diff = nominee_cost_diff;
+      const auto cost = eval(*responses[i].proposal);
+      if (!best_cost.has_value() || cost < *best_cost)
+      {
+        best_index = i;
+        best_cost = cost;
+      }
     }
   }
-  return std::distance(submissions.begin(), winner_it);
+
+  return best_index;
 }
+} // anonymous namespace
 
 //==============================================================================
-std::size_t LeastFleetCostEvaluator::choose(
-  const Submissions& submissions) const
+std::optional<std::size_t> LeastFleetDiffCostEvaluator::choose(
+  const Responses& responses) const
 {
-  auto winner_it = submissions.begin();
-  for (auto nominee_it = ++submissions.begin();
-    nominee_it != submissions.end(); ++nominee_it)
-  {
-    if (nominee_it->new_cost < winner_it->new_cost)
-      winner_it = nominee_it;
-  }
-  return std::distance(submissions.begin(), winner_it);
+  return select_best(
+    responses,
+    [](const auto& nominee) { return nominee.new_cost - nominee.prev_cost; });
 }
 
 //==============================================================================
-std::size_t QuickestFinishEvaluator::choose(
-  const Submissions& submissions) const
+std::optional<std::size_t> LeastFleetCostEvaluator::choose(
+  const Responses& responses) const
 {
-  auto winner_it = submissions.begin();
-  for (auto nominee_it = ++submissions.begin();
-    nominee_it != submissions.end(); ++nominee_it)
-  {
-    if (nominee_it->finish_time < winner_it->finish_time)
-      winner_it = nominee_it;
-  }
-  return std::distance(submissions.begin(), winner_it);
+  return select_best(
+    responses,
+    [](const auto& nominee) { return nominee.new_cost; });
+}
+
+//==============================================================================
+std::optional<std::size_t> QuickestFinishEvaluator::choose(
+  const Responses& responses) const
+{
+  return select_best(
+    responses,
+    [](const auto& nominee)
+    {
+      return rmf_traffic::time::to_seconds(
+        nominee.finish_time.time_since_epoch());
+    });
 }
 
 } // namespace bidding
diff --git a/rmf_task_ros2/src/rmf_task_ros2/bidding/MinimalBidder.cpp b/rmf_task_ros2/src/rmf_task_ros2/bidding/MinimalBidder.cpp
deleted file mode 100644
index 524097a4a..000000000
--- a/rmf_task_ros2/src/rmf_task_ros2/bidding/MinimalBidder.cpp
+++ /dev/null
@@ -1,132 +0,0 @@
-/*
- * Copyright (C) 2020 Open Source Robotics Foundation
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *
-*/
-
-#include <rmf_task_ros2/bidding/Submission.hpp>
-#include <rmf_task_ros2/bidding/MinimalBidder.hpp>
-
-#include <rmf_traffic_ros2/Time.hpp>
-#include <rmf_task_msgs/msg/bid_proposal.hpp>
-#include <rmf_task_ros2/StandardNames.hpp>
-
-namespace rmf_task_ros2 {
-namespace bidding {
-
-using BidProposal = rmf_task_msgs::msg::BidProposal;
-
-//==============================================================================
-BidProposal convert(const Submission& from)
-{
-  BidProposal proposal_msg;
-  proposal_msg.fleet_name = from.fleet_name;
-  proposal_msg.robot_name = from.robot_name;
-  proposal_msg.prev_cost = from.prev_cost;
-  proposal_msg.new_cost = from.new_cost;
-  proposal_msg.finish_time = rmf_traffic_ros2::convert(from.finish_time);
-  return proposal_msg;
-}
-
-//==============================================================================
-class MinimalBidder::Implementation
-{
-public:
-
-  std::shared_ptr<rclcpp::Node> node;
-  std::string fleet_name;
-  std::unordered_set<TaskType> valid_task_types;
-  ParseSubmissionCallback get_submission_fn;
-
-  using BidNoticeSub = rclcpp::Subscription<BidNotice>;
-  BidNoticeSub::SharedPtr dispatch_notice_sub;
-
-  using BidProposalPub = rclcpp::Publisher<BidProposal>;
-  BidProposalPub::SharedPtr dispatch_proposal_pub;
-
-  Implementation(
-    std::shared_ptr<rclcpp::Node> node_,
-    const std::string& fleet_name_,
-    const std::unordered_set<TaskType>& valid_task_types_,
-    ParseSubmissionCallback submission_cb)
-  : node{std::move(node_)},
-    fleet_name{std::move(fleet_name_)},
-    valid_task_types{std::move(valid_task_types_)},
-    get_submission_fn{std::move(submission_cb)}
-  {
-    const auto dispatch_qos = rclcpp::ServicesQoS().reliable();
-
-    dispatch_notice_sub = node->create_subscription<BidNotice>(
-      rmf_task_ros2::BidNoticeTopicName, dispatch_qos,
-      [&](const BidNotice::UniquePtr msg)
-      {
-        this->receive_notice(*msg);
-      });
-
-    dispatch_proposal_pub = node->create_publisher<BidProposal>(
-      rmf_task_ros2::BidProposalTopicName, dispatch_qos);
-  }
-
-  // Callback fn when a dispatch notice is received
-  void receive_notice(const BidNotice& msg)
-  {
-    RCLCPP_INFO(node->get_logger(),
-      "[Bidder] Received Bidding notice for task_id [%s]",
-      msg.task_profile.task_id.c_str());
-
-    const auto task_type = (msg.task_profile.description.task_type.type);
-
-    // check if task type is valid
-    if (!valid_task_types.count(static_cast<TaskType>(task_type)))
-    {
-      RCLCPP_WARN(node->get_logger(), "[%s]: task type %d is not supported",
-        fleet_name.c_str(), task_type);
-      return;
-    }
-
-    // check if get submission function is declared
-    if (!get_submission_fn)
-      return;
-
-    // Submit proposal
-    const auto bid_submission = get_submission_fn(msg);
-    auto best_proposal = convert(bid_submission);
-    best_proposal.fleet_name = fleet_name;
-    best_proposal.task_profile = msg.task_profile;
-    dispatch_proposal_pub->publish(best_proposal);
-  }
-};
-
-//==============================================================================
-std::shared_ptr<MinimalBidder> MinimalBidder::make(
-  const std::shared_ptr<rclcpp::Node>& node,
-  const std::string& fleet_name,
-  const std::unordered_set<TaskType>& valid_task_types,
-  ParseSubmissionCallback submission_cb)
-{
-  auto pimpl = rmf_utils::make_unique_impl<Implementation>(
-    node, fleet_name, valid_task_types, submission_cb);
-  auto bidder = std::shared_ptr<MinimalBidder>(new MinimalBidder());
-  bidder->_pimpl = std::move(pimpl);
-  return bidder;
-}
-
-//==============================================================================
-MinimalBidder::MinimalBidder()
-{
-  // do nothing
-}
-
-} // namespace bidding
-} // namespace rmf_task_ros2
diff --git a/rmf_task_ros2/src/rmf_task_ros2/bidding/Response.cpp b/rmf_task_ros2/src/rmf_task_ros2/bidding/Response.cpp
new file mode 100644
index 000000000..d0bde5cec
--- /dev/null
+++ b/rmf_task_ros2/src/rmf_task_ros2/bidding/Response.cpp
@@ -0,0 +1,81 @@
+/*
+ * Copyright (C) 2022 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_task_ros2/bidding/Response.hpp>
+
+#include <rmf_traffic_ros2/Time.hpp>
+
+namespace rmf_task_ros2 {
+namespace bidding {
+
+//==============================================================================
+BidProposalMsg convert(const Response::Proposal& proposal)
+{
+  return rmf_task_msgs::build<BidProposalMsg>()
+      .fleet_name(proposal.fleet_name)
+      .expected_robot_name(proposal.expected_robot_name)
+      .prev_cost(proposal.prev_cost)
+      .new_cost(proposal.new_cost)
+      .finish_time(rmf_traffic_ros2::convert(proposal.finish_time));
+}
+
+//==============================================================================
+BidResponseMsg convert(
+  const Response& response,
+  const std::string& task_id)
+{
+  if (response.proposal.has_value())
+  {
+    return rmf_task_msgs::build<BidResponseMsg>()
+    .task_id(task_id)
+    .has_proposal(true)
+    .proposal(convert(*response.proposal))
+    .errors(response.errors);
+  }
+
+  return rmf_task_msgs::build<BidResponseMsg>()
+    .task_id(task_id)
+    .has_proposal(false)
+    .proposal(rmf_task_msgs::msg::BidProposal())
+    .errors(response.errors);
+}
+
+//==============================================================================
+Response convert(const BidResponseMsg& msg)
+{
+  if (msg.has_proposal)
+  {
+    return Response{
+      Response::Proposal{
+        msg.proposal.fleet_name,
+        msg.proposal.expected_robot_name,
+        msg.proposal.prev_cost,
+        msg.proposal.new_cost,
+        rmf_traffic_ros2::convert(msg.proposal.finish_time)
+      },
+      msg.errors
+    };
+  }
+
+  return Response{
+    std::nullopt,
+    msg.errors
+  };
+}
+
+} // namespace bidding
+} // namespace rmf_task_ros2
diff --git a/rmf_task_ros2/src/rmf_task_ros2/bidding/internal_Auctioneer.hpp b/rmf_task_ros2/src/rmf_task_ros2/bidding/internal_Auctioneer.hpp
index bafba2dd7..c84cd9d02 100644
--- a/rmf_task_ros2/src/rmf_task_ros2/bidding/internal_Auctioneer.hpp
+++ b/rmf_task_ros2/src/rmf_task_ros2/bidding/internal_Auctioneer.hpp
@@ -19,7 +19,7 @@
 #ifndef SRC__RMF_TASK_ROS2__BIDDING__INTERNAL_AUCTIONEER_HPP
 #define SRC__RMF_TASK_ROS2__BIDDING__INTERNAL_AUCTIONEER_HPP
 
-#include <rmf_task_ros2/bidding/Submission.hpp>
+#include <rmf_task_ros2/bidding/Response.hpp>
 #include <rmf_task_ros2/bidding/Auctioneer.hpp>
 #include <rmf_task_msgs/msg/bid_proposal.hpp>
 
@@ -29,49 +29,49 @@
 namespace rmf_task_ros2 {
 namespace bidding {
 
-using BidProposal = rmf_task_msgs::msg::BidProposal;
-
 //==============================================================================
 class Auctioneer::Implementation
 {
 public:
+
   std::shared_ptr<rclcpp::Node> node;
   rclcpp::TimerBase::SharedPtr timer;
   BiddingResultCallback bidding_result_callback;
-  std::shared_ptr<Evaluator> evaluator;
+  ConstEvaluatorPtr evaluator;
 
   struct OpenBid
   {
-    BidNotice bid_notice;
+    BidNoticeMsg bid_notice;
     builtin_interfaces::msg::Time start_time;
-    std::vector<bidding::Submission> submissions;
+    std::vector<bidding::Response> responses;
   };
 
   bool bidding_in_proccess = false;
   std::queue<OpenBid> open_bid_queue;
 
-  using BidNoticePub = rclcpp::Publisher<BidNotice>;
+  using BidNoticePub = rclcpp::Publisher<BidNoticeMsg>;
   BidNoticePub::SharedPtr bid_notice_pub;
 
-  using BidProposalSub = rclcpp::Subscription<BidProposal>;
-  BidProposalSub::SharedPtr bid_proposal_sub;
+  using BidResponseSub = rclcpp::Subscription<BidResponseMsg>;
+  BidResponseSub::SharedPtr bid_proposal_sub;
 
   Implementation(
     const std::shared_ptr<rclcpp::Node>& node_,
-    BiddingResultCallback result_callback);
+    BiddingResultCallback result_callback,
+    ConstEvaluatorPtr evaluator);
 
   /// Start a bidding process
-  void start_bidding(const BidNotice& bid_notice);
+  void start_bidding(const BidNoticeMsg& bid_notice);
 
   // Receive proposal and evaluate
-  void receive_proposal(const BidProposal& msg);
+  void receive_response(const BidResponseMsg& msg);
 
   // determine the winner within a bidding task instance
-  void check_bidding_process();
+  void finish_bidding_process();
 
   bool determine_winner(const OpenBid& bidding_task);
 
-  std::optional<Submission> evaluate(const Submissions& submissions);
+  std::optional<Response::Proposal> evaluate(const Responses& responses);
 
   static const Implementation& get(const Auctioneer& auctioneer)
   {
@@ -80,12 +80,12 @@ class Auctioneer::Implementation
 };
 
 //==============================================================================
-std::optional<Submission> evaluate(
+std::optional<Response::Proposal> evaluate(
   const Auctioneer& auctioneer,
-  const Submissions& submissions)
+  const Responses& responses)
 {
   auto fimpl = Auctioneer::Implementation::get(auctioneer);
-  return fimpl.evaluate(submissions);
+  return fimpl.evaluate(responses);
 }
 
 } // namespace bidding
diff --git a/rmf_task_ros2/test/action/test_ActionInterface.cpp b/rmf_task_ros2/test/action/test_ActionInterface.cpp
deleted file mode 100644
index d4c77374c..000000000
--- a/rmf_task_ros2/test/action/test_ActionInterface.cpp
+++ /dev/null
@@ -1,202 +0,0 @@
-/*
- * Copyright (C) 2020 Open Source Robotics Foundation
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *
-*/
-
-#include <rmf_utils/optional.hpp>
-#include "../../src/rmf_task_ros2/action/Client.hpp"
-#include "../../src/rmf_task_ros2/action/Server.hpp"
-#include <rclcpp/rclcpp.hpp>
-
-#include <chrono>
-#include <rmf_utils/catch.hpp>
-
-namespace rmf_task_ros2 {
-namespace action {
-
-//==============================================================================
-SCENARIO("Action communication with client and server", "[ActionInterface]")
-{
-  const auto rcl_context = std::make_shared<rclcpp::Context>();
-  rcl_context->init(0, nullptr);
-
-  TaskProfile task_profile1;
-  task_profile1.task_id = "task1";
-  task_profile1.description.task_type.type =
-    rmf_task_msgs::msg::TaskType::TYPE_STATION;
-
-  TaskProfile task_profile2;
-  task_profile2.task_id = "task2";
-  task_profile2.description.task_type.type =
-    rmf_task_msgs::msg::TaskType::TYPE_STATION;
-
-  //============================================================================
-
-  // received task to test
-  // NOTE(MXG): We do not appear to be placing any test expectations on
-  // the value of test_add_task. That might mean we're not testing as thoroughly
-  // as we could.
-  const auto test_add_task = std::make_shared<std::optional<TaskProfile>>();
-  const auto test_cancel_task = std::make_shared<std::optional<TaskProfile>>();
-
-  // Creating 1 auctioneer and 1 bidder
-  auto node = rclcpp::Node::make_shared(
-    "test_ActionInferface",
-    rclcpp::NodeOptions().context(rcl_context));
-
-  auto action_server = Server::make(node, "test_server");
-  auto action_client = Client::make(node);
-
-  rclcpp::ExecutorOptions exec_options;
-  exec_options.context = rcl_context;
-  rclcpp::executors::SingleThreadedExecutor executor(exec_options);
-  executor.add_node(node);
-
-  // received test request msg from client
-  action_server->register_callbacks(
-    // add task
-    [test_add_task](const TaskProfile& task_profile)
-    {
-      *test_add_task = task_profile;
-      return true;
-    },
-    // cancel task
-    [test_cancel_task](const TaskProfile& task_profile)
-    {
-      *test_cancel_task = task_profile;
-      return true;
-    }
-  );
-
-  // ROS Spin: forever incompleted future
-  std::promise<void> ready_promise;
-  std::shared_future<void> ready_future(ready_promise.get_future());
-
-  WHEN("Add Task")
-  {
-    // Add invalid Task!
-    TaskStatusPtr status_ptr(new TaskStatus);
-
-    action_client->add_task("wrong_server", task_profile1, status_ptr);
-
-    executor.spin_until_future_complete(ready_future,
-      rmf_traffic::time::from_seconds(0.5));
-
-    // should not receive cuz incorrect serverid
-    REQUIRE(status_ptr->state == DispatchState::State::Pending);
-
-    action_client->add_task("test_server", task_profile1, status_ptr);
-    executor.spin_until_future_complete(ready_future,
-      rmf_traffic::time::from_seconds(0.5));
-
-    // check status
-    REQUIRE(status_ptr->state == DispatchState::State::Queued);
-
-    // status ptr is destroyed, should not have anymore tracking
-    status_ptr.reset();
-    REQUIRE(action_client->size() == 0);
-  }
-
-  WHEN("Cancel Task")
-  {
-    // send valid task
-    TaskStatusPtr status_ptr(new TaskStatus);
-    action_client->add_task("test_server", task_profile2, status_ptr);
-
-    executor.spin_until_future_complete(ready_future,
-      rmf_traffic::time::from_seconds(0.5));
-
-    REQUIRE(test_cancel_task.get());
-
-    // Invalid Cancel Task!
-    bool cancel_success = action_client->cancel_task(task_profile1);
-    executor.spin_until_future_complete(ready_future,
-      rmf_traffic::time::from_seconds(0.5));
-    REQUIRE(!test_cancel_task->has_value());
-    REQUIRE(cancel_success == false); // cancel failed
-    REQUIRE(action_client->size() == 1);
-
-    // Valid Cancel task
-    action_client->cancel_task(task_profile2);
-    executor.spin_until_future_complete(ready_future,
-      rmf_traffic::time::from_seconds(0.5));
-
-    REQUIRE(test_cancel_task->has_value());
-    REQUIRE(*test_cancel_task == task_profile2);
-    REQUIRE(status_ptr->is_terminated());
-    REQUIRE(action_client->size() == 0);
-  }
-
-  //============================================================================
-
-  const auto test_task_onchange = std::make_shared<std::optional<TaskStatus>>();
-  const auto test_task_onterminate =
-    std::make_shared<std::optional<TaskStatus>>();
-
-  // received status update from server
-  action_client->on_change(
-    [test_task_onchange](const TaskStatusPtr status)
-    {
-      *test_task_onchange = *status;
-    }
-  );
-  action_client->on_terminate(
-    [test_task_onterminate](const TaskStatusPtr status)
-    {
-      *test_task_onterminate = *status;
-    }
-  );
-
-  WHEN("On Change and On Terminate Task")
-  {
-    TaskStatusPtr status_ptr(new TaskStatus);
-    action_client->add_task("test_server", task_profile1, status_ptr);
-
-    executor.spin_until_future_complete(ready_future,
-      rmf_traffic::time::from_seconds(0.5));
-
-    REQUIRE(test_task_onchange.get());
-    REQUIRE(test_task_onchange->has_value());
-    CHECK((*test_task_onchange)->state == DispatchState::State::Queued);
-
-    TaskStatus server_task;
-    server_task.task_profile = task_profile1;
-    server_task.state = DispatchState::State::Executing;
-
-    // Update it as executing
-    action_server->update_status(server_task);
-    executor.spin_until_future_complete(ready_future,
-      rmf_traffic::time::from_seconds(1.5));
-
-    CHECK((*test_task_onchange)->state == DispatchState::State::Executing);
-    CHECK(!test_task_onterminate->has_value());
-
-    // completion
-    server_task.state = DispatchState::State::Completed;
-    // Update it as executing
-    action_server->update_status(server_task);
-    executor.spin_until_future_complete(ready_future,
-      rmf_traffic::time::from_seconds(0.5));
-
-    REQUIRE(test_task_onterminate.get());
-    REQUIRE(test_task_onterminate->has_value());
-    CHECK((*test_task_onterminate)->state == DispatchState::State::Completed);
-  }
-
-  rclcpp::shutdown(rcl_context);
-}
-
-} // namespace action
-} // namespace rmf_task_ros2
diff --git a/rmf_task_ros2/test/bidding/test_Evaluator.cpp b/rmf_task_ros2/test/bidding/test_Evaluator.cpp
index 3da0b9ad6..f3615ee6b 100644
--- a/rmf_task_ros2/test/bidding/test_Evaluator.cpp
+++ b/rmf_task_ros2/test/bidding/test_Evaluator.cpp
@@ -25,19 +25,19 @@ namespace bidding {
 //==============================================================================
 auto now = std::chrono::steady_clock::now();
 
-Submission submission1{
+Response::Proposal submission1{
   "fleet1", "", 2.3, 3.4, rmf_traffic::time::apply_offset(now, 5)
 };
-Submission submission2{
+Response::Proposal submission2{
   "fleet2", "", 3.5, 3.6, rmf_traffic::time::apply_offset(now, 5.5)
 };
-Submission submission3{
+Response::Proposal submission3{
   "fleet3", "", 0.0, 1.4, rmf_traffic::time::apply_offset(now, 3)
 };
-Submission submission4{
+Response::Proposal submission4{
   "fleet4", "", 5.0, 5.4, rmf_traffic::time::apply_offset(now, 4)
 };
-Submission submission5{
+Response::Proposal submission5{
   "fleet5", "", 0.5, 0.8, rmf_traffic::time::apply_offset(now, 3.5)
 };
 
@@ -50,24 +50,29 @@ SCENARIO("Auctioneer Winner Evaluator", "[Evaluator]")
     "test_selfbidding", rclcpp::NodeOptions().context(rcl_context));
 
   auto auctioneer = Auctioneer::make(node,
-      [](const std::string&, const std::optional<Submission>) {});
+      [](const auto&, const auto, const auto&) {},
+      nullptr);
 
   WHEN("Least Diff Cost Evaluator")
   {
     auto eval = std::make_shared<LeastFleetDiffCostEvaluator>();
-    auctioneer->select_evaluator(eval);
+    auctioneer->set_evaluator(eval);
 
     AND_WHEN("0 submissions")
     {
-      std::vector<Submission> submissions{};
-      auto winner = evaluate(*auctioneer, submissions);
+      std::vector<Response> responses{};
+      auto winner = evaluate(*auctioneer, responses);
       REQUIRE(!winner); // no winner
     }
     AND_WHEN("5 submissions")
     {
-      std::vector<Submission> submissions{
-        submission1, submission2, submission3, submission4, submission5 };
-      auto winner = evaluate(*auctioneer, submissions);
+      std::vector<Response> responses{
+        Response{submission1, {}},
+        Response{submission2, {}},
+        Response{submission3, {}},
+        Response{submission4, {}},
+        Response{submission5, {}} };
+      auto winner = evaluate(*auctioneer, responses);
       REQUIRE(winner->fleet_name == "fleet2"); // least diff cost agent
     }
   }
@@ -75,18 +80,22 @@ SCENARIO("Auctioneer Winner Evaluator", "[Evaluator]")
   WHEN("Least Fleet Cost Evaluator")
   {
     auto eval = std::make_shared<LeastFleetCostEvaluator>();
-    auctioneer->select_evaluator(eval);
+    auctioneer->set_evaluator(eval);
 
     AND_WHEN("0 submissions")
     {
-      std::vector<Submission> submissions{};
+      std::vector<Response> submissions{};
       auto winner = evaluate(*auctioneer, submissions);
       REQUIRE(!winner); // no winner
     }
     AND_WHEN("5 submissions")
     {
-      std::vector<Submission> submissions{
-        submission1, submission2, submission3, submission4, submission5 };
+      std::vector<Response> submissions{
+        Response{submission1, {}},
+        Response{submission2, {}},
+        Response{submission3, {}},
+        Response{submission4, {}},
+        Response{submission5, {}} };
       auto winner = evaluate(*auctioneer, submissions);
       REQUIRE(winner->fleet_name == "fleet5"); // least diff cost agent
     }
@@ -95,18 +104,22 @@ SCENARIO("Auctioneer Winner Evaluator", "[Evaluator]")
   WHEN("Quickest Finish Time Evaluator")
   {
     auto eval = std::make_shared<QuickestFinishEvaluator>();
-    auctioneer->select_evaluator(eval);
+    auctioneer->set_evaluator(eval);
 
     AND_WHEN("0 submissions")
     {
-      std::vector<Submission> submissions{};
+      std::vector<Response> submissions{};
       auto winner = evaluate(*auctioneer, submissions);
       REQUIRE(!winner); // no winner
     }
     AND_WHEN("5 submissions")
     {
-      std::vector<Submission> submissions{
-        submission1, submission2, submission3, submission4, submission5 };
+      std::vector<Response> submissions{
+        Response{submission1, {}},
+        Response{submission2, {}},
+        Response{submission3, {}},
+        Response{submission4, {}},
+        Response{submission5, {}} };
       auto winner = evaluate(*auctioneer, submissions);
       REQUIRE(winner->fleet_name == "fleet3"); // least diff cost agent
     }
diff --git a/rmf_task_ros2/test/bidding/test_SelfBid.cpp b/rmf_task_ros2/test/bidding/test_SelfBid.cpp
index 766ed3c78..a295c2f0f 100644
--- a/rmf_task_ros2/test/bidding/test_SelfBid.cpp
+++ b/rmf_task_ros2/test/bidding/test_SelfBid.cpp
@@ -15,11 +15,13 @@
  *
 */
 
-#include <rmf_task_ros2/bidding/MinimalBidder.hpp>
+#include <rmf_task_ros2/bidding/AsyncBidder.hpp>
 #include <rmf_task_ros2/bidding/Auctioneer.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <rmf_traffic_ros2/Time.hpp>
 
+#include <nlohmann/json.hpp>
+
 #include <chrono>
 #include <thread>
 #include <rmf_utils/catch.hpp>
@@ -27,12 +29,9 @@
 namespace rmf_task_ros2 {
 namespace bidding {
 
-using TaskProfile = rmf_task_msgs::msg::TaskProfile;
-using TaskType = bidding::MinimalBidder::TaskType;
-
 //==============================================================================
-BidNotice bidding_task1;
-BidNotice bidding_task2;
+BidNoticeMsg bidding_task1;
+BidNoticeMsg bidding_task2;
 
 // set time window to 2s
 auto timeout = rmf_traffic_ros2::convert(rmf_traffic::time::from_seconds(2.0));
@@ -41,20 +40,26 @@ auto timeout = rmf_traffic_ros2::convert(rmf_traffic::time::from_seconds(2.0));
 SCENARIO("Auction with 2 Bids", "[TwoBids]")
 {
   // Initializing bidding task
-  bidding_task1.task_profile.task_id = "bid1";
+  nlohmann::json req_1;
+  req_1["category"] = "patrol";
+  req_1["description"] = "mocking a patrol";
+
+  nlohmann::json req_2;
+  req_2["category"] = "delivery";
+  req_2["description"] = "mocking a delivery";
+
+  bidding_task1.task_id = "bid1";
   bidding_task1.time_window = timeout;
-  bidding_task1.task_profile.description.task_type.type =
-    rmf_task_msgs::msg::TaskType::TYPE_STATION;
+  bidding_task1.request = req_1.dump();
 
-  bidding_task2.task_profile.task_id = "bid2";
+  bidding_task2.task_id = "bid2";
   bidding_task2.time_window = timeout;
-  bidding_task2.task_profile.description.task_type.type =
-    rmf_task_msgs::msg::TaskType::TYPE_DELIVERY;
+  bidding_task2.request = req_2.dump();
 
   //============================================================================
   // test received msg
-  std::optional<TaskProfile> test_notice_bidder1;
-  std::optional<TaskProfile> test_notice_bidder2;
+  std::optional<std::string> test_notice_bidder1;
+  std::optional<std::string> test_notice_bidder2;
   std::string r_result_id = "";
   std::string r_result_winner = "";
 
@@ -68,14 +73,17 @@ SCENARIO("Auction with 2 Bids", "[TwoBids]")
     node,
     /// Bidding Result Callback Function
     [&r_result_id, &r_result_winner](
-      const std::string& task_id, const std::optional<Submission> winner)
+      const auto& task_id,
+      const auto winner,
+      const auto&)
     {
       if (!winner)
         return;
       r_result_id = task_id;
       r_result_winner = winner->fleet_name;
       return;
-    }
+    },
+    nullptr
   );
 
   rclcpp::ExecutorOptions exec_options;
@@ -83,29 +91,37 @@ SCENARIO("Auction with 2 Bids", "[TwoBids]")
   rclcpp::executors::SingleThreadedExecutor executor(exec_options);
   executor.add_node(node);
 
-  auto bidder1 = MinimalBidder::make(
-    node, "bidder1", { TaskType::Station, TaskType::Delivery },
-    [&test_notice_bidder1](const BidNotice& notice)
+  auto bidder1 = AsyncBidder::make(
+    node,
+    [&test_notice_bidder1](const auto& notice, auto respond)
     {
-      Submission best_robot_estimate;
-      test_notice_bidder1 = notice.task_profile;
+      Response::Proposal best_robot_estimate;
+      test_notice_bidder1 = notice.request;
+      best_robot_estimate.fleet_name = "bidder1";
       best_robot_estimate.finish_time =
-      std::chrono::steady_clock::time_point::max();
-      return best_robot_estimate;
+        std::chrono::steady_clock::time_point::max();
+
+      respond(Response{best_robot_estimate, {}});
     }
   );
 
-  auto bidder2 = MinimalBidder::make(
-    node, "bidder2", { TaskType::Delivery, TaskType::Clean },
-    [&test_notice_bidder2](const BidNotice& notice)
+  auto bidder2 = AsyncBidder::make(
+    node,
+    [&test_notice_bidder2](const auto& notice, auto respond)
     {
+      auto request = nlohmann::json::parse(notice.request);
+      if (request["category"] == "patrol")
+        return respond(Response{std::nullopt, {"Cannot patrol"}});
+
       // TaskType should not be supported
-      Submission best_robot_estimate;
+      Response::Proposal best_robot_estimate;
       best_robot_estimate.new_cost = 2.3; // lower cost than bidder1
+      best_robot_estimate.fleet_name = "bidder2";
       best_robot_estimate.finish_time =
       std::chrono::steady_clock::time_point::min();
-      test_notice_bidder2 = notice.task_profile;
-      return best_robot_estimate;
+      test_notice_bidder2 = notice.request;
+
+      respond(Response{best_robot_estimate, {}});
     }
   );
 
@@ -113,19 +129,17 @@ SCENARIO("Auction with 2 Bids", "[TwoBids]")
   std::promise<void> ready_promise;
   std::shared_future<void> ready_future(ready_promise.get_future());
 
-  WHEN("First 'Station' Task Bid")
+  WHEN("First 'patrol' Task Bid")
   {
-    // start bidding
-    bidding_task1.task_profile.submission_time = node->now();
     auctioneer->start_bidding(bidding_task1);
 
     executor.spin_until_future_complete(ready_future,
       rmf_traffic::time::from_seconds(1.0));
 
     // Check if bidder 1 & 2 receive BidNotice1
-    REQUIRE(test_notice_bidder1);
-    REQUIRE(test_notice_bidder1->task_id == bidding_task1.task_profile.task_id);
-    REQUIRE(!test_notice_bidder2); // bidder2 doesnt support tasktype
+    REQUIRE(test_notice_bidder1.has_value());
+    CHECK(*test_notice_bidder1 == bidding_task1.request);
+    REQUIRE(!test_notice_bidder2.has_value()); // bidder2 doesn't support patrol
 
     executor.spin_until_future_complete(ready_future,
       rmf_traffic::time::from_seconds(2.5));
@@ -135,19 +149,19 @@ SCENARIO("Auction with 2 Bids", "[TwoBids]")
     REQUIRE(r_result_id == "bid1");
   }
 
-  WHEN("Second 'Delivery' Task bid")
+  WHEN("Second 'delivery' Task bid")
   {
     // start bidding
-    bidding_task2.task_profile.submission_time = node->now();
     auctioneer->start_bidding(bidding_task2);
 
     executor.spin_until_future_complete(ready_future,
       rmf_traffic::time::from_seconds(1.0));
 
     // Check if bidder 1 & 2 receive BidNotice2
-    auto task2_profile = bidding_task2.task_profile;
-    REQUIRE(test_notice_bidder1->task_id == task2_profile.task_id);
-    REQUIRE(test_notice_bidder2->task_id == task2_profile.task_id);
+    REQUIRE(test_notice_bidder1.has_value());
+    CHECK(*test_notice_bidder1 == bidding_task2.request);
+    REQUIRE(test_notice_bidder2.has_value());
+    REQUIRE(*test_notice_bidder2 == bidding_task2.request);
 
     executor.spin_until_future_complete(ready_future,
       rmf_traffic::time::from_seconds(2.5));
diff --git a/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp b/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp
index 66649e483..291adb649 100644
--- a/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp
+++ b/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp
@@ -19,12 +19,14 @@
 #include <rmf_task_ros2/Dispatcher.hpp>
 
 // mock Fleet Adapter to test dispatcher
-#include <rmf_task_ros2/bidding/MinimalBidder.hpp>
-#include "../../src/rmf_task_ros2/action/Server.hpp"
+#include <rmf_task_ros2/bidding/AsyncBidder.hpp>
+#include <rmf_task_ros2/StandardNames.hpp>
 
 #include <rmf_task_msgs/srv/submit_task.hpp>
 #include <rmf_task_msgs/srv/cancel_task.hpp>
-#include <rmf_task_msgs/srv/get_task_list.hpp>
+#include <rmf_task_msgs/srv/get_dispatch_states.hpp>
+
+#include <rmf_task_msgs/srv/submit_task.hpp>
 
 #include <chrono>
 #include <thread>
@@ -35,8 +37,8 @@ namespace rmf_task_ros2 {
 //==============================================================================
 SCENARIO("Dispatcher API Test", "[Dispatcher][.flaky]")
 {
-  Dispatcher::TaskDescription task_desc1;
-  Dispatcher::TaskDescription task_desc2;
+  rmf_task_msgs::msg::TaskDescription task_desc1;
+  rmf_task_msgs::msg::TaskDescription task_desc2;
   task_desc1.task_type.type = rmf_task_msgs::msg::TaskType::TYPE_STATION;
   task_desc2.task_type.type = rmf_task_msgs::msg::TaskType::TYPE_CLEAN;
 
@@ -58,7 +60,7 @@ SCENARIO("Dispatcher API Test", "[Dispatcher][.flaky]")
   {
     using SubmitTaskSrv = rmf_task_msgs::srv::SubmitTask;
     using CancelTaskSrv = rmf_task_msgs::srv::CancelTask;
-    using GetTaskListSrv = rmf_task_msgs::srv::GetTaskList;
+    using GetDispatchStatesSrv = rmf_task_msgs::srv::GetDispatchStates;
 
     auto submit_client = dispatcher->node()->create_client<SubmitTaskSrv>(
       rmf_task_ros2::SubmitTaskSrvName);
@@ -66,8 +68,8 @@ SCENARIO("Dispatcher API Test", "[Dispatcher][.flaky]")
     auto cancel_client = dispatcher->node()->create_client<CancelTaskSrv>(
       rmf_task_ros2::CancelTaskSrvName);
     REQUIRE(cancel_client->wait_for_service(std::chrono::milliseconds(0)));
-    auto get_tasks_client = dispatcher->node()->create_client<GetTaskListSrv>(
-      rmf_task_ros2::GetTaskListSrvName);
+    auto get_tasks_client = dispatcher->node()->create_client<GetDispatchStatesSrv>(
+      rmf_task_ros2::GetDispatchStatesSrvName);
     REQUIRE(get_tasks_client->wait_for_service(std::chrono::milliseconds(0)));
   }
 
@@ -76,17 +78,19 @@ SCENARIO("Dispatcher API Test", "[Dispatcher][.flaky]")
     // add task
     const auto id = dispatcher->submit_task(task_desc1);
     REQUIRE(id.has_value());
-    REQUIRE(dispatcher->active_tasks().size() == 1);
-    REQUIRE(dispatcher->terminated_tasks().size() == 0);
-    REQUIRE(dispatcher->get_task_state(*id) == DispatchState::State::Pending);
+    CHECK(dispatcher->active_dispatches().size() == 1);
+    CHECK(dispatcher->finished_dispatches().size() == 0);
+    const auto state = dispatcher->get_dispatch_state(*id);
+    REQUIRE(state.has_value());
+    REQUIRE(state->status == DispatchState::Status::Queued);
 
     // cancel task
     REQUIRE(dispatcher->cancel_task(*id));
-    REQUIRE(dispatcher->active_tasks().size() == 0);
-    REQUIRE(dispatcher->terminated_tasks().size() == 1);
+    REQUIRE(dispatcher->active_dispatches().size() == 0);
+    REQUIRE(dispatcher->finished_dispatches().size() == 1);
 
     // check nonsense id
-    REQUIRE(!(dispatcher->get_task_state("non_existent_id")));
+    REQUIRE(!(dispatcher->get_dispatch_state("non_existent_id")));
 
     // Try sleeping for a moment here to mitigate race conditions.
     // TODO(MXG): We should rework these tests so that we don't need to put in
@@ -102,11 +106,11 @@ SCENARIO("Dispatcher API Test", "[Dispatcher][.flaky]")
   //============================================================================
   // test on change fn callback
   const auto change_times = std::make_shared<int>(0);
-  const auto test_taskprofile = std::make_shared<TaskProfile>();
+  const auto test_id = std::make_shared<std::string>();
   dispatcher->on_change(
-    [change_times, test_taskprofile](const TaskStatusPtr status)
+    [change_times, test_id](const auto& state)
     {
-      *test_taskprofile = status->task_profile;
+      *test_id = state.task_id;
       (*change_times)++;
     }
   );
@@ -115,151 +119,48 @@ SCENARIO("Dispatcher API Test", "[Dispatcher][.flaky]")
   {
     // Submit first task and wait for bidding
     auto id = dispatcher->submit_task(task_desc1);
-    REQUIRE(dispatcher->active_tasks().size() == 1);
-    REQUIRE(dispatcher->get_task_state(*id) == DispatchState::State::Pending);
+    REQUIRE(dispatcher->active_dispatches().size() == 1);
+    const auto state = dispatcher->get_dispatch_state(*id);
+    REQUIRE(state.has_value());
+    REQUIRE(state->status == DispatchState::Status::Queued);
 
     // Default 2s timeout, wait 3s for timetout, should fail here
     std::this_thread::sleep_for(std::chrono::milliseconds(3500));
-    CHECK(dispatcher->get_task_state(*id) == DispatchState::State::Failed);
-    REQUIRE(dispatcher->terminated_tasks().size() == 1);
+    CHECK(dispatcher->get_dispatch_state(*id)->status ==
+        DispatchState::Status::FailedToAssign);
+
+    CHECK(dispatcher->finished_dispatches().size() == 1);
     // TODO(MXG): Flake out after previous line: SIGABRT
-    REQUIRE(test_taskprofile->task_id == id);
+    CHECK(*test_id == id);
     CHECK(*change_times == 2); // add and failed
 
     // Submit another task
     id = dispatcher->submit_task(task_desc2);
+    REQUIRE(id.has_value());
     std::this_thread::sleep_for(std::chrono::milliseconds(3000));
-    REQUIRE(dispatcher->terminated_tasks().size() == 2);
-    REQUIRE(test_taskprofile->task_id == *id);
+    REQUIRE(dispatcher->finished_dispatches().size() == 2);
+    REQUIRE(*test_id == *id);
     CHECK(*change_times == 4); // add and failed x2
   }
 
   //============================================================================
   // Setup Mock Fleetadapter: mock bidder to test
-  using TaskType = bidding::MinimalBidder::TaskType;
-  auto bidder = bidding::MinimalBidder::make(
+  auto bidder = bidding::AsyncBidder::make(
     node,
-    "dummy_fleet",
-    { TaskType::Station, TaskType::Clean },
-    [](const bidding::BidNotice&)
+    [](const bidding::BidNoticeMsg& notice, auto respond)
     {
+      const auto request = nlohmann::json::parse(notice.request);
+      if (request["category"] != "patrol")
+        return respond(bidding::Response{std::nullopt, {}});
+
       // Provide a best estimate
-      bidding::Submission best_robot_estimate;
+      bidding::Response::Proposal best_robot_estimate;
+      best_robot_estimate.fleet_name = "dummy_fleet";
       best_robot_estimate.new_cost = 13.5;
-      return best_robot_estimate;
-    }
-  );
-
-  //============================================================================
-  // Setup Mock Fleetadapter: action server to test
-  auto action_server = action::Server::make(node, "dummy_fleet");
-
-  const auto task_canceled_flag = std::make_shared<bool>(false);
-
-  // We use the action_mutex to make sure that the main thread does not stop
-  // until the action response callback has had enough time to do its job.
-  const auto action_mutex = std::make_shared<std::mutex>();
-
-  action_server->register_callbacks(
-    // Add Task callback
-    [
-      a = std::weak_ptr<action::Server>(action_server),
-      action_mutex,
-      task_canceled_flag
-    ](const TaskProfile& task_profile)
-    {
-      // Start action task
-      auto t = std::thread(
-        [a, action_mutex, task_canceled_flag](auto profile)
-        {
-          std::lock_guard<std::mutex> lock(*action_mutex);
-          const auto action_server = a.lock();
-          if (!action_server)
-            return;
-
-          TaskStatus status;
-          status.task_profile = profile;
-          status.robot_name = "dumbot";
-          std::this_thread::sleep_for(std::chrono::seconds(2));
-
-          if (*task_canceled_flag)
-          {
-            // std::cout << "[task impl] Cancelled!" << std::endl;
-            return;
-          }
-
-          // Executing
-          status.state = DispatchState::State::Executing;
-          action_server->update_status(status);
-          std::this_thread::sleep_for(std::chrono::seconds(1));
-
-          // Completed
-          status.state = DispatchState::State::Completed;
-          action_server->update_status(status);
-        }, task_profile
-      );
-      t.detach();
-      return true; //successs (send State::Queued)
-    },
-    // Cancel Task callback
-    [task_canceled_flag](const TaskProfile&)
-    {
-      *task_canceled_flag = true;
-      return true; //success ,send State::Canceled when dispatcher->cancel_task
+      respond(bidding::Response{best_robot_estimate, {}});
     }
   );
 
-  //============================================================================
-  WHEN("Full Dispatch cycle")
-  {
-    const auto id = dispatcher->submit_task(task_desc1);
-    CHECK(dispatcher->get_task_state(*id) == DispatchState::State::Pending);
-    std::this_thread::sleep_for(std::chrono::milliseconds(3500));
-
-    // now should queue the task
-    CHECK(dispatcher->get_task_state(*id) == DispatchState::State::Queued);
-    REQUIRE(dispatcher->terminated_tasks().size() == 0);
-    CHECK(*change_times == 2); // Pending and Queued
-
-    std::this_thread::sleep_for(std::chrono::seconds(3));
-    CHECK(dispatcher->get_task_state(*id) == DispatchState::State::Completed);
-    REQUIRE(dispatcher->active_tasks().size() == 0);
-    REQUIRE(dispatcher->terminated_tasks().size() == 1);
-    CHECK(*change_times == 4); // Pending > Queued > Executing > Completed
-
-    // Add auto generated ChargeBattery Task from fleet adapter
-    TaskStatus status;
-    status.task_profile.task_id = "ChargeBattery10";
-    status.state = DispatchState::State::Queued;
-    status.task_profile.description.task_type.type =
-      rmf_task_msgs::msg::TaskType::TYPE_CHARGE_BATTERY;
-    action_server->update_status(status);
-    std::this_thread::sleep_for(std::chrono::seconds(1));
-
-    CHECK(*change_times == 5); // new stray charge task
-    REQUIRE(dispatcher->active_tasks().size() == 1);
-  }
-
-  WHEN("Half way cancel Dispatch cycle")
-  {
-    const auto id = dispatcher->submit_task(task_desc2);
-    CHECK(dispatcher->get_task_state(*id) == DispatchState::State::Pending);
-    REQUIRE(dispatcher->active_tasks().size() == 1);
-    std::this_thread::sleep_for(std::chrono::milliseconds(3000));
-
-    // cancel the task after QUEUED State
-    REQUIRE(dispatcher->cancel_task(*id));
-    std::this_thread::sleep_for(std::chrono::seconds(1));
-
-    REQUIRE(dispatcher->active_tasks().size() == 0);
-    REQUIRE(dispatcher->terminated_tasks().size() == 1);
-    REQUIRE(dispatcher->terminated_tasks().begin()->first == *id);
-    auto status = dispatcher->terminated_tasks().begin()->second;
-    CHECK(status->state == DispatchState::State::Canceled);
-    CHECK(*change_times == 3); // Pending -> Queued -> Canceled
-  }
-
-  std::lock_guard<std::mutex> lock(*action_mutex);
   rclcpp::shutdown(rcl_context);
   dispatcher_spin_thread.join();
 }

From bab1e34fb77c689af28941a40058b1b0d7532ede Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Sun, 9 Jan 2022 01:26:34 +0800
Subject: [PATCH 43/79] Creating new API for letting users accept tasks

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 rmf_fleet_adapter/CMakeLists.txt              |   2 +
 .../agv/FleetUpdateHandle.hpp                 |  99 ++++++-
 rmf_fleet_adapter/package.xml                 |   1 +
 .../agv/FleetUpdateHandle.cpp                 | 274 +++++++++++++++---
 .../agv/internal_FleetUpdateHandle.hpp        |  43 ++-
 .../agv/test/MockAdapter.cpp                  |   2 +-
 .../src/rmf_fleet_adapter/tasks/Clean.cpp     |  25 +-
 .../src/rmf_fleet_adapter/tasks/Delivery.cpp  |  27 +-
 .../src/rmf_fleet_adapter/tasks/Loop.cpp      |  23 +-
 .../test/dispatcher/test_Dispatcher.cpp       |   5 +-
 10 files changed, 432 insertions(+), 69 deletions(-)

diff --git a/rmf_fleet_adapter/CMakeLists.txt b/rmf_fleet_adapter/CMakeLists.txt
index 13a2483fd..626b49d04 100644
--- a/rmf_fleet_adapter/CMakeLists.txt
+++ b/rmf_fleet_adapter/CMakeLists.txt
@@ -33,6 +33,7 @@ set(dep_pkgs
   rmf_task_msgs
   rmf_traffic
   rmf_traffic_ros2
+  rmf_task_ros2
   rmf_battery
   rmf_task
   rmf_task_sequence
@@ -76,6 +77,7 @@ target_link_libraries(rmf_fleet_adapter
     rmf_battery::rmf_battery
     rmf_task::rmf_task
     rmf_task_sequence::rmf_task_sequence
+    rmf_task_ros2::rmf_task_ros2
     yaml-cpp
     ${rmf_fleet_msgs_LIBRARIES}
     ${rclcpp_LIBRARIES}
diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp
index be6273ef7..55d221695 100644
--- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp
@@ -75,6 +75,102 @@ class FleetUpdateHandle : public std::enable_shared_from_this<FleetUpdateHandle>
     rmf_traffic::agv::Plan::StartSet start,
     std::function<void(std::shared_ptr<RobotUpdateHandle> handle)> handle_cb);
 
+  /// Confirmation is a class used by the task acceptance callbacks to decide if
+  /// a task description should be accepted.
+  class Confirmation
+  {
+  public:
+
+    /// Constructor
+    Confirmation();
+
+    /// Call this function to decide that you want to accept the task request.
+    /// If this function is never called, it will be assumed that the task is
+    /// rejected.
+    Confirmation& accept();
+
+    /// Check whether
+    bool is_accepted() const;
+
+    /// Call this function to bring attention to errors related to the task
+    /// request. Each call to this function will overwrite any past calls, so
+    /// it is recommended to only call it once.
+    Confirmation& errors(std::vector<std::string> error_messages);
+
+    /// Call this function to add errors instead of overwriting the ones that
+    /// were already there.
+    Confirmation& add_errors(std::vector<std::string> error_messages);
+
+    /// Check the errors that have been given to this confirmation.
+    const std::vector<std::string>& errors() const;
+
+    class Implementation;
+  private:
+    rmf_utils::impl_ptr<Implementation> _pimpl;
+  };
+
+  /// Signature for a callback that decides whether to accept a specific
+  /// category of task request.
+  ///
+  /// \param[in] description
+  ///   A description of the task that is being considered
+  ///
+  /// \param[in] confirm
+  ///   Use this object to decide if you want to accept the task
+  using ConsiderRequest =
+    std::function<void(
+      const nlohmann::json& description,
+      Confirmation& confirm)
+    >;
+
+  /// Allow this fleet adapter to consider delivery requests.
+  ///
+  /// Pass in nullptrs to disable delivery requests.
+  ///
+  /// By default, delivery requests are not accepted until you provide these
+  /// callbacks.
+  ///
+  /// The FleetUpdateHandle will ensure that the requests are feasible for the
+  /// robots before triggering these callbacks.
+  ///
+  /// \param[in] consider_pickup
+  ///   Decide whether to accept a pickup request. The description will satisfy
+  ///   the event_description_PickUp.json schema of rmf_fleet_adapter.
+  ///
+  /// \param[in] consider_dropoff
+  ///   Decide whether to accept a dropoff request. The description will satisfy
+  ///   the event_description_DropOff.json schema of rmf_fleet_adapter.
+  FleetUpdateHandle& consider_delivery_requests(
+    ConsiderRequest consider_pickup,
+    ConsiderRequest consider_dropoff);
+
+  /// Allow this fleet adapter to consider cleaning requests.
+  ///
+  /// Pass in a nullptr to disable cleaning requests.
+  ///
+  /// By default, cleaning requests are not accepted until you provide this
+  /// callback.
+  ///
+  /// \param[in] consider
+  ///   Decide whether to accept a cleaning request. The description will
+  ///   satisfy the event_description_Clean.json schema of rmf_fleet_adapter.
+  ///   The FleetUpdateHandle will ensure that the request is feasible for the
+  ///   robots before triggering this callback.
+  FleetUpdateHandle& consider_cleaning_requests(ConsiderRequest consider);
+
+  /// Allow this fleet adapter to consider patrol requests.
+  ///
+  /// Pass in a nullptr to disable patrol requests.
+  ///
+  /// By default, patrol requests are always accepted.
+  ///
+  /// \param[in] consider
+  ///   Decide whether to accept a patrol request. The description will satisfy
+  ///   the task_description_Patrol.json schema of rmf_fleet_adapter. The
+  ///   FleetUpdateHandle will ensure that the request is feasible for the
+  ///   robots before triggering this callback.
+  FleetUpdateHandle& consider_patrol_requests(ConsiderRequest consider);
+
   template<typename T>
   struct DeserializedDescription
   {
@@ -272,6 +368,7 @@ class FleetUpdateHandle : public std::enable_shared_from_this<FleetUpdateHandle>
   /// compatible with the requested payload, pickup, and dropoff behavior
   /// settings. The path planning feasibility will be taken care of by the
   /// adapter internally.
+  [[deprecated("Use the consider_..._requests functions instead")]]
   FleetUpdateHandle& accept_task_requests(AcceptTaskRequest check);
 
   /// A callback function that evaluates whether a fleet will accept a delivery
@@ -294,7 +391,7 @@ class FleetUpdateHandle : public std::enable_shared_from_this<FleetUpdateHandle>
   /// compatible with the requested payload, pickup, and dropoff behavior
   /// settings. The path planning feasibility will be taken care of by the
   /// adapter internally.
-  [[deprecated("Use accept_task_requests() instead")]]
+  [[deprecated("Use consider_delivery_requests() instead")]]
   FleetUpdateHandle& accept_delivery_requests(AcceptDeliveryRequest check);
 
   /// Specify the default value for how high the delay of the current itinerary
diff --git a/rmf_fleet_adapter/package.xml b/rmf_fleet_adapter/package.xml
index 2870fbfcd..119d6463a 100644
--- a/rmf_fleet_adapter/package.xml
+++ b/rmf_fleet_adapter/package.xml
@@ -22,6 +22,7 @@
   <depend>rmf_task_msgs</depend>
   <depend>rmf_traffic</depend>
   <depend>rmf_traffic_ros2</depend>
+  <depend>rmf_task_ros2</depend>
   <depend>rmf_battery</depend>
   <depend>rmf_task</depend>
   <depend>rmf_task_sequence</depend>
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
index 835fda4f3..52a977fde 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
@@ -39,10 +39,6 @@
 
 #include <rmf_task_sequence/phases/SimplePhase.hpp>
 
-#include <rmf_task_msgs/msg/clean.hpp>
-#include <rmf_task_msgs/msg/delivery.hpp>
-#include <rmf_task_msgs/msg/loop.hpp>
-
 #include <sstream>
 #include <unordered_map>
 #include <unordered_set>
@@ -147,7 +143,8 @@ void FleetUpdateHandle::Implementation::dock_summary_cb(
 
 //==============================================================================
 void FleetUpdateHandle::Implementation::bid_notice_cb(
-  const BidNotice::SharedPtr msg)
+  const BidNoticeMsg& msg,
+  rmf_task_ros2::bidding::AsyncBidder::Respond respond)
 {
   if (task_managers.empty())
   {
@@ -155,45 +152,22 @@ void FleetUpdateHandle::Implementation::bid_notice_cb(
       node->get_logger(),
       "Fleet [%s] does not have any robots to accept task [%s]. Use "
       "FleetUpdateHadndle::add_robot(~) to add robots to this fleet. ",
-      name.c_str(), msg->task_profile.task_id.c_str());
+      name.c_str(), msg.task_id.c_str());
     return;
   }
 
-  if (msg->task_profile.task_id.empty())
+  if (msg.task_id.empty())
   {
     RCLCPP_WARN(
       node->get_logger(),
-      "Received BidNotice for a task with invalid task_id. Request will be "
+      "Received BidNotice for a task with empty task_id. Request will be "
       "ignored.");
     return;
   }
 
   // TODO remove this block when we support task revival
-  if (bid_notice_assignments.find(msg->task_profile.task_id)
-    != bid_notice_assignments.end())
-    return;
-
-  if (!accept_task)
-  {
-    RCLCPP_WARN(
-      node->get_logger(),
-      "Fleet [%s] is not configured to accept any task requests. Use "
-      "FleetUpdateHadndle::accept_task_requests(~) to define a callback "
-      "for accepting requests", name.c_str());
-
+  if (bid_notice_assignments.find(msg.task_id) != bid_notice_assignments.end())
     return;
-  }
-
-  if (!accept_task(msg->task_profile))
-  {
-    RCLCPP_INFO(
-      node->get_logger(),
-      "Fleet [%s] is configured to not accept task [%s]",
-      name.c_str(),
-      msg->task_profile.task_id.c_str());
-
-    return;
-  }
 
   if (!task_planner)
   {
@@ -1358,6 +1332,86 @@ void FleetUpdateHandle::add_robot(
     });
 }
 
+//==============================================================================
+class FleetUpdateHandle::Confirmation::Implementation
+{
+public:
+  bool is_accepted = false;
+  std::vector<std::string> errors;
+};
+
+//==============================================================================
+FleetUpdateHandle::Confirmation::Confirmation()
+: _pimpl(rmf_utils::make_impl<Implementation>())
+{
+  // Do nothing
+}
+
+//==============================================================================
+auto FleetUpdateHandle::Confirmation::accept() -> Confirmation&
+{
+  _pimpl->is_accepted = true;
+  return *this;
+}
+
+//==============================================================================
+bool FleetUpdateHandle::Confirmation::is_accepted() const
+{
+  return _pimpl->is_accepted;
+}
+
+//==============================================================================
+auto FleetUpdateHandle::Confirmation::errors(
+  std::vector<std::string> error_messages) -> Confirmation&
+{
+  _pimpl->errors = std::move(error_messages);
+  return *this;
+}
+
+//==============================================================================
+auto FleetUpdateHandle::Confirmation::add_errors(
+  std::vector<std::string> error_messages) -> Confirmation&
+{
+  _pimpl->errors.insert(
+    _pimpl->errors.end(),
+    std::make_move_iterator(error_messages.begin()),
+    std::make_move_iterator(error_messages.end()));
+
+  return *this;
+}
+
+//==============================================================================
+const std::vector<std::string>& FleetUpdateHandle::Confirmation::errors() const
+{
+  return _pimpl->errors;
+}
+
+//==============================================================================
+FleetUpdateHandle& FleetUpdateHandle::consider_delivery_requests(
+  ConsiderRequest consider_pickup,
+  ConsiderRequest consider_dropoff)
+{
+  *_pimpl->deserialization.consider_pickup = std::move(consider_pickup);
+  *_pimpl->deserialization.consider_dropoff = std::move(consider_dropoff);
+  return *this;
+}
+
+//==============================================================================
+FleetUpdateHandle& FleetUpdateHandle::consider_cleaning_requests(
+  ConsiderRequest consider)
+{
+  *_pimpl->deserialization.consider_clean = std::move(consider);
+  return *this;
+}
+
+//==============================================================================
+FleetUpdateHandle& FleetUpdateHandle::consider_patrol_requests(
+  ConsiderRequest consider)
+{
+  *_pimpl->deserialization.consider_patrol = std::move(consider);
+  return *this;
+}
+
 //==============================================================================
 void FleetUpdateHandle::close_lanes(std::vector<std::size_t> lane_indices)
 {
@@ -1446,7 +1500,161 @@ void FleetUpdateHandle::open_lanes(std::vector<std::size_t> lane_indices)
 FleetUpdateHandle& FleetUpdateHandle::accept_task_requests(
   AcceptTaskRequest check)
 {
-  _pimpl->accept_task = std::move(check);
+  const auto legacy_adapter =
+    [check = std::move(check)](
+      const rmf_task_msgs::msg::TaskProfile& profile,
+      Confirmation& confirm)
+    {
+      if (check(profile))
+      {
+        confirm.accept();
+        return;
+      }
+
+      confirm.errors({"Task rejected by legacy AcceptTaskRequest callback"});
+    };
+
+  const auto convert_item = [](const nlohmann::json& item)
+    -> rmf_dispenser_msgs::msg::DispenserRequestItem
+    {
+      rmf_dispenser_msgs::msg::DispenserRequestItem output;
+      output.type_guid = item["sku"];
+      output.quantity = item["quantity"];
+      if (const auto& compartment = item["compartment"])
+        output.compartment_name = compartment.get<std::string>();
+
+      return output;
+    };
+
+  const auto convert_items = [convert_item](const nlohmann::json& payload)
+    -> std::vector<rmf_dispenser_msgs::msg::DispenserRequestItem>
+    {
+      std::vector<rmf_dispenser_msgs::msg::DispenserRequestItem> items;
+      if (payload.is_object())
+      {
+        items.push_back(convert_item(payload));
+      }
+      else if (payload.is_array())
+      {
+        for (const auto& p : payload)
+          items.push_back(convert_item(p));
+      }
+      else
+      {
+        throw std::runtime_error(
+          "Invalid payload schema for delivery request:" + payload.dump());
+      }
+
+      return items;
+    };
+
+  auto consider_pickup = [legacy_adapter, convert_items](
+    const nlohmann::json& msg, Confirmation& confirm)
+    {
+      rmf_task_msgs::msg::TaskProfile profile;
+      profile.description.task_type.type =
+        rmf_task_msgs::msg::TaskType::TYPE_DELIVERY;
+
+      profile.description.delivery =
+        rmf_task_msgs::build<rmf_task_msgs::msg::Delivery>()
+          .task_id("")
+          .items(convert_items(msg["payload"]))
+          .pickup_place_name(msg["place"].get<std::string>())
+          .pickup_dispenser(msg["hanlder"].get<std::string>())
+          .pickup_behavior(rmf_task_msgs::msg::Behavior{})
+          .dropoff_place_name("")
+          .dropoff_ingestor("")
+          .dropoff_behavior(rmf_task_msgs::msg::Behavior{});
+
+      legacy_adapter(profile, confirm);
+    };
+
+  auto consider_dropoff = [legacy_adapter, convert_items](
+    const nlohmann::json& msg, Confirmation& confirm)
+    {
+    rmf_task_msgs::msg::TaskProfile profile;
+    profile.description.task_type.type =
+      rmf_task_msgs::msg::TaskType::TYPE_DELIVERY;
+
+    profile.description.delivery =
+      rmf_task_msgs::build<rmf_task_msgs::msg::Delivery>()
+        .task_id("")
+        .items(convert_items(msg["payload"]))
+        .pickup_place_name("")
+        .pickup_dispenser("")
+        .pickup_behavior(rmf_task_msgs::msg::Behavior{})
+        .dropoff_place_name(msg["place"].get<std::string>())
+        .dropoff_ingestor(msg["hanlder"].get<std::string>())
+        .dropoff_behavior(rmf_task_msgs::msg::Behavior{});
+
+      legacy_adapter(profile, confirm);
+    };
+
+  consider_delivery_requests(
+    std::move(consider_pickup),
+    std::move(consider_dropoff));
+
+  consider_patrol_requests(
+    [legacy_adapter](const nlohmann::json& msg, Confirmation& confirm)
+    {
+      rmf_task_msgs::msg::TaskProfile profile;
+
+      // We use loop here because patrol wasn't supported during the legacy
+      // versions anyway.
+      profile.description.task_type.type =
+        rmf_task_msgs::msg::TaskType::TYPE_LOOP;
+
+      rmf_task_msgs::msg::Loop loop;
+      const auto& places = msg["places"];
+      if (places.size() == 1)
+      {
+        const auto& place = places[0];
+        if (!place.is_string())
+        {
+          confirm.errors(
+            {"Legacy AcceptTaskRequest only accepts destination names "
+             "for patrol requests"});
+          return;
+        }
+
+        loop.start_name = places[0].get<std::string>();
+        loop.finish_name = loop.start_name;
+      }
+      else
+      {
+        const auto& start_place = places[0];
+        const auto& finish_place = places[1];
+        if (!start_place.is_string() || !finish_place.is_string())
+        {
+          confirm.errors(
+            {"Legacy AcceptTaskRequest only accepts destination names "
+             "for patrol requests"});
+          return;
+        }
+
+        loop.start_name = start_place.get<std::string>();
+        loop.finish_name = finish_place.get<std::string>();
+      }
+
+      loop.num_loops = msg["rounds"].get<uint32_t>();
+      profile.description.loop = std::move(loop);
+
+      legacy_adapter(profile, confirm);
+    });
+
+  consider_cleaning_requests(
+    [legacy_adapter](const nlohmann::json& msg, Confirmation& confirm)
+    {
+      rmf_task_msgs::msg::TaskProfile profile;
+
+      profile.description.task_type.type =
+        rmf_task_msgs::msg::TaskType::TYPE_CLEAN;
+
+      profile.description.clean.start_waypoint = msg["zone"].get<std::string>();
+
+      legacy_adapter(profile, confirm);
+    });
+
   return *this;
 }
 
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
index bac6e2975..36db42640 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
@@ -20,8 +20,8 @@
 
 #include <rmf_task_msgs/msg/loop.hpp>
 
-#include <rmf_task_msgs/msg/bid_proposal.hpp>
-#include <rmf_task_msgs/msg/bid_notice.hpp>
+#include <rmf_task_ros2/bidding/AsyncBidder.hpp>
+
 #include <rmf_task_msgs/msg/dispatch_request.hpp>
 #include <rmf_task_msgs/msg/dispatch_ack.hpp>
 
@@ -85,6 +85,11 @@ struct TaskDeserialization
   DeserializeJSON<FleetUpdateHandle::DeserializedEvent> event;
   FleetUpdateHandle::PlaceDeserializer place;
 
+  std::shared_ptr<FleetUpdateHandle::ConsiderRequest> consider_pickup;
+  std::shared_ptr<FleetUpdateHandle::ConsiderRequest> consider_dropoff;
+  std::shared_ptr<FleetUpdateHandle::ConsiderRequest> consider_clean;
+  std::shared_ptr<FleetUpdateHandle::ConsiderRequest> consider_patrol;
+
   void add_schema(const nlohmann::json& schema);
 
   nlohmann::json_schema::json_validator make_validator(
@@ -227,6 +232,8 @@ class FleetUpdateHandle::Implementation
   // TODO Support for various charging configurations
   std::unordered_set<std::size_t> charging_waypoints = {};
 
+  std::shared_ptr<rmf_task_ros2::bidding::AsyncBidder> bidder;
+
   double current_assignment_cost = 0.0;
   // Map to store task id with assignments for BidNotice
   std::unordered_map<std::string, Assignments> bid_notice_assignments = {};
@@ -239,15 +246,7 @@ class FleetUpdateHandle::Implementation
   using TaskProfileMsg = rmf_task_msgs::msg::TaskProfile;
   std::unordered_map<std::string, TaskProfileMsg> task_profile_map = {};
 
-  AcceptTaskRequest accept_task = nullptr;
-
-  using BidNotice = rmf_task_msgs::msg::BidNotice;
-  using BidNoticeSub = rclcpp::Subscription<BidNotice>::SharedPtr;
-  BidNoticeSub bid_notice_sub = nullptr;
-
-  using BidProposal = rmf_task_msgs::msg::BidProposal;
-  using BidProposalPub = rclcpp::Publisher<BidProposal>::SharedPtr;
-  BidProposalPub bid_proposal_pub = nullptr;
+  using BidNoticeMsg = rmf_task_msgs::msg::BidNotice;
 
   using DispatchRequest = rmf_task_msgs::msg::DispatchRequest;
   using DispatchRequestSub = rclcpp::Subscription<DispatchRequest>::SharedPtr;
@@ -291,25 +290,19 @@ class FleetUpdateHandle::Implementation
     auto default_qos = rclcpp::SystemDefaultsQoS();
     auto transient_qos = rclcpp::QoS(10);  transient_qos.transient_local();
 
-    // Publish BidProposal
-    handle->_pimpl->bid_proposal_pub =
-      handle->_pimpl->node->create_publisher<BidProposal>(
-      BidProposalTopicName, default_qos);
-
     // Publish DispatchAck
     handle->_pimpl->dispatch_ack_pub =
       handle->_pimpl->node->create_publisher<DispatchAck>(
       DispatchAckTopicName, default_qos);
 
-    // Subscribe BidNotice
-    handle->_pimpl->bid_notice_sub =
-      handle->_pimpl->node->create_subscription<BidNotice>(
-      BidNoticeTopicName,
-      default_qos,
-      [w = handle->weak_from_this()](const BidNotice::SharedPtr msg)
+    //
+    handle->_pimpl->bidder = rmf_task_ros2::bidding::AsyncBidder::make(
+      handle->_pimpl->node,
+      [w = handle->weak_from_this()](
+        const auto& msg, auto respond)
       {
         if (const auto self = w.lock())
-          self->_pimpl->bid_notice_cb(msg);
+          self->_pimpl->bid_notice_cb(msg, std::move(respond));
       });
 
     // Subscribe DispatchRequest
@@ -369,7 +362,9 @@ class FleetUpdateHandle::Implementation
 
   void dock_summary_cb(const DockSummary::SharedPtr& msg);
 
-  void bid_notice_cb(const BidNotice::SharedPtr msg);
+  void bid_notice_cb(
+    const BidNoticeMsg& msg,
+    rmf_task_ros2::bidding::AsyncBidder::Respond respond);
 
   void dispatch_request_cb(const DispatchRequest::SharedPtr msg);
 
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp
index 05284e5dd..330bdc16d 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp
@@ -314,7 +314,7 @@ void MockAdapter::dispatch_task(const rmf_task_msgs::msg::TaskProfile& profile)
       for (auto& fleet : fleets)
       {
         auto& fimpl = FleetUpdateHandle::Implementation::get(*fleet);
-        if (!fimpl.accept_task)
+        if (!fimpl.legacy_accept_task)
           continue;
 
         // NOTE: althought the current adapter supports multiple fleets. The test
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp
index 231210c5a..5189ffb76 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp
@@ -274,11 +274,27 @@ void add_clean(
       schemas::task_description_Clean);
   deserialization.add_schema(schemas::task_description_Clean);
 
+  deserialization.consider_clean =
+    std::make_shared<agv::FleetUpdateHandle::ConsiderRequest>();
+
   auto deserialize_clean =
-    [dock_params, traits, place_deser = deserialization.place](
+    [
+      dock_params,
+      traits,
+      place_deser = deserialization.place,
+      consider = deserialization.consider_clean
+    ](
       const nlohmann::json& msg)
       -> agv::FleetUpdateHandle::DeserializedTask
     {
+      if (!consider || !(*consider))
+      {
+        return {
+          nullptr,
+          {"Not accepting cleaning tasks"}
+        };
+      }
+
       const auto zone = msg["zone"].get<std::string>();
       const auto clean_it = dock_params->find(zone);
       if (clean_it == dock_params->end())
@@ -321,13 +337,18 @@ void add_clean(
         };
       }
 
+      agv::FleetUpdateHandle::Confirmation confirm;
+      (*consider)(msg, confirm);
+      if (!confirm.is_accepted())
+        return {nullptr, confirm.errors()};
+
       // TODO(MXG): Validate the type of cleaning (vacuum, mopping, etc)
       return {
         rmf_task::requests::Clean::Description::make(
           start_place.description->waypoint(),
           exit_place.description->waypoint(),
           std::move(clean_path)),
-        {}
+        confirm.errors()
       };
     };
   deserialization.task.add("clean", validate_clean_task, deserialize_clean);
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp
index 668e76679..2dfe157b8 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp
@@ -287,10 +287,12 @@ struct TransferItems : public rmf_task_sequence::events::Placeholder::Descriptio
 };
 
 //==============================================================================
+using DeserializedItemTransfer = agv::FleetUpdateHandle::DeserializedEvent;
 template<typename T>
-std::function<agv::FleetUpdateHandle::DeserializedEvent(const nlohmann::json& msg)>
+std::function<DeserializedItemTransfer(const nlohmann::json& msg)>
 make_deserializer(
-  const agv::FleetUpdateHandle::PlaceDeserializer& place_deser)
+  const agv::FleetUpdateHandle::PlaceDeserializer& place_deser,
+  const std::shared_ptr<agv::FleetUpdateHandle::ConsiderRequest>& consider)
 {
   auto parse_payload_component = [](const nlohmann::json& msg)
     -> rmf_task::Payload::Component
@@ -309,10 +311,14 @@ make_deserializer(
   return
     [
       place_deser,
+      consider,
       parse_payload_component = std::move(parse_payload_component)
     ](const nlohmann::json& msg)
       -> agv::FleetUpdateHandle::DeserializedEvent
     {
+      if (!consider || !(*consider))
+        return {nullptr, {"Not accepting delivery requests"}};
+
       auto place = place_deser(msg["place"]);
       if (!place.description.has_value())
       {
@@ -348,6 +354,11 @@ make_deserializer(
         handler = handler_json.get<std::string>();
       }
 
+      agv::FleetUpdateHandle::Confirmation confirm;
+      (*consider)(msg, confirm);
+      if (!confirm.is_accepted())
+        return {nullptr, confirm.errors()};
+
       // TODO(MXG): Add a way for system integrators to specify a duration
       // estimate for the payload transfer
       return {
@@ -356,7 +367,7 @@ make_deserializer(
           std::move(handler),
           rmf_task::Payload(std::move(payload_components)),
           rmf_traffic::Duration(0)),
-        {}
+        confirm.errors()
       };
     };
 }
@@ -378,13 +389,19 @@ void add_delivery(
       schemas::event_description_PayloadTransfer);
   deserialization.add_schema(schemas::event_description_PayloadTransfer);
 
+  deserialization.consider_pickup =
+    std::make_shared<agv::FleetUpdateHandle::ConsiderRequest>();
   auto deserialize_pickup =
-    make_deserializer<PickUp::Description>(deserialization.place);
+    make_deserializer<PickUp::Description>(
+      deserialization.place, deserialization.consider_pickup);
   deserialization.event.add(
     "pickup", validate_payload_transfer, deserialize_pickup);
 
+  deserialization.consider_dropoff =
+    std::make_shared<agv::FleetUpdateHandle::ConsiderRequest>();
   auto deserialize_dropoff =
-    make_deserializer<DropOff::Description>(deserialization.place);
+    make_deserializer<DropOff::Description>(
+      deserialization.place, deserialization.consider_dropoff);
   deserialization.event.add(
     "dropoff", validate_payload_transfer, deserialize_dropoff);
 
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp
index 6a23ad5c5..de73e67d0 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp
@@ -151,10 +151,23 @@ void add_loop(
     deserialization.make_validator_shared(schemas::task_description_Patrol);
   deserialization.add_schema(schemas::task_description_Patrol);
 
+  deserialization.consider_patrol =
+    std::make_shared<agv::FleetUpdateHandle::ConsiderRequest>();
+  *deserialization.consider_patrol = [](const auto&, auto confirm)
+    {
+      confirm.accept();
+    };
+
   auto deserialize_patrol =
-    [place_deser = deserialization.place](const nlohmann::json& msg)
+    [
+      place_deser = deserialization.place,
+      consider = deserialization.consider_patrol
+    ](const nlohmann::json& msg)
       -> agv::FleetUpdateHandle::DeserializedTask
     {
+      if (!(*consider))
+        return {nullptr, {"Not accepting patrol requests"}};
+
       const auto& places_json = msg["places"];
       std::vector<rmf_traffic::agv::Plan::Goal> places;
       std::vector<std::string> errors;
@@ -173,6 +186,14 @@ void add_loop(
       if (any_failure)
         return {nullptr, std::move(errors)};
 
+      agv::FleetUpdateHandle::Confirmation confirm;
+      (*consider)(msg, confirm);
+      errors.insert(
+        errors.end(), confirm.errors().begin(), confirm.errors().end());
+
+      if (!confirm.is_accepted())
+        return {nullptr, errors};
+
       std::size_t rounds = 1;
       if (const auto& rounds_json = msg["rounds"])
         rounds = rounds_json.get<std::size_t>();
diff --git a/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp b/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp
index 291adb649..0374946a2 100644
--- a/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp
+++ b/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp
@@ -68,8 +68,9 @@ SCENARIO("Dispatcher API Test", "[Dispatcher][.flaky]")
     auto cancel_client = dispatcher->node()->create_client<CancelTaskSrv>(
       rmf_task_ros2::CancelTaskSrvName);
     REQUIRE(cancel_client->wait_for_service(std::chrono::milliseconds(0)));
-    auto get_tasks_client = dispatcher->node()->create_client<GetDispatchStatesSrv>(
-      rmf_task_ros2::GetDispatchStatesSrvName);
+    auto get_tasks_client =
+      dispatcher->node()->create_client<GetDispatchStatesSrv>(
+        rmf_task_ros2::GetDispatchStatesSrvName);
     REQUIRE(get_tasks_client->wait_for_service(std::chrono::milliseconds(0)));
   }
 

From abf79bfbe8033670ef4c75dff6b12ec7bcd56fdf Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Sun, 9 Jan 2022 12:50:37 +0800
Subject: [PATCH 44/79] Migrating to new bid notice system

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 .../agv/FleetUpdateHandle.hpp                 |   4 +
 .../agv/FleetUpdateHandle.cpp                 | 631 +++++++-----------
 .../agv/internal_FleetUpdateHandle.hpp        |  16 +-
 3 files changed, 272 insertions(+), 379 deletions(-)

diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp
index 55d221695..a36a23289 100644
--- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp
@@ -424,6 +424,10 @@ class FleetUpdateHandle : public std::enable_shared_from_this<FleetUpdateHandle>
   FleetUpdateHandle& fleet_state_update_period(
     std::optional<rmf_traffic::Duration> value);
 
+  // Do not allow moving
+  FleetUpdateHandle(FleetUpdateHandle&&) = delete;
+  FleetUpdateHandle& operator=(FleetUpdateHandle&&) = delete;
+
   class Implementation;
 private:
   FleetUpdateHandle();
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
index 52a977fde..d71e4c519 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
@@ -45,6 +45,7 @@
 #include <stdexcept>
 
 #include <rmf_fleet_adapter/schemas/Place.hpp>
+#include <rmf_api_msgs/schemas/task_request.hpp>
 
 namespace rmf_fleet_adapter {
 namespace agv {
@@ -141,22 +142,37 @@ void FleetUpdateHandle::Implementation::dock_summary_cb(
   return;
 }
 
+namespace {
+//==============================================================================
+std::string make_error_str(
+  uint64_t code, std::string category, std::string detail)
+{
+  nlohmann::json error;
+  error["code"] = code;
+  error["category"] = std::move(category);
+  error["detail"] = std::move(detail);
+
+  return error.dump();
+}
+} // anonymous namespace
+
 //==============================================================================
 void FleetUpdateHandle::Implementation::bid_notice_cb(
-  const BidNoticeMsg& msg,
+  const BidNoticeMsg& bid_notice,
   rmf_task_ros2::bidding::AsyncBidder::Respond respond)
 {
+  const auto& task_id = bid_notice.task_id;
   if (task_managers.empty())
   {
     RCLCPP_INFO(
       node->get_logger(),
       "Fleet [%s] does not have any robots to accept task [%s]. Use "
       "FleetUpdateHadndle::add_robot(~) to add robots to this fleet. ",
-      name.c_str(), msg.task_id.c_str());
+      name.c_str(), task_id.c_str());
     return;
   }
 
-  if (msg.task_id.empty())
+  if (task_id.empty())
   {
     RCLCPP_WARN(
       node->get_logger(),
@@ -166,7 +182,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb(
   }
 
   // TODO remove this block when we support task revival
-  if (bid_notice_assignments.find(msg.task_id) != bid_notice_assignments.end())
+  if (bid_notice_assignments.find(task_id) != bid_notice_assignments.end())
     return;
 
   if (!task_planner)
@@ -180,310 +196,122 @@ void FleetUpdateHandle::Implementation::bid_notice_cb(
     return;
   }
 
-  // Determine task type and convert to request pointer
-  rmf_task::ConstRequestPtr new_request = nullptr;
-  const auto& task_profile = msg->task_profile;
-  const auto& task_type = task_profile.description.task_type;
-  const rmf_traffic::Time start_time =
-    rmf_traffic_ros2::convert(task_profile.description.start_time);
-  // TODO (YV) get rid of ID field in RequestPtr
-  std::string id = msg->task_profile.task_id;
-  const auto& graph = (*planner)->get_configuration().graph();
-
-  // Generate the priority of the request. The current implementation supports
-  // binary priority
-  auto priority =
-    task_profile.description.priority.value > 0 ?
-    rmf_task::BinaryPriorityScheme::make_high_priority() :
-    rmf_task::BinaryPriorityScheme::make_low_priority();
-
-  // Process Cleaning task
-  if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_CLEAN)
-  {
-    if (task_profile.description.clean.start_waypoint.empty())
-    {
-      RCLCPP_ERROR(
-        node->get_logger(),
-        "Required param [clean.start_waypoint] missing in TaskProfile."
-        "Rejecting BidNotice with task_id:[%s]", id.c_str());
-
-      return;
-    }
-
-    // Check for valid start waypoint
-    const std::string start_wp_name =
-      task_profile.description.clean.start_waypoint;
-    const auto start_wp = graph.find_waypoint(start_wp_name);
-    if (!start_wp)
-    {
-      RCLCPP_INFO(
-        node->get_logger(),
-        "Fleet [%s] does not have a named waypoint [%s] configured in its "
-        "nav graph. Rejecting BidNotice with task_id:[%s]",
-        name.c_str(), start_wp_name.c_str(), id.c_str());
-
-      return;
-    }
+  const auto request_msg = nlohmann::json::parse(bid_notice.request);
+  static const auto request_validator =
+    nlohmann::json_schema::json_validator(rmf_api_msgs::schemas::task_request);
 
-    // Get dock parameters
-    const auto clean_param_it = dock_param_map->find(start_wp_name);
-    if (clean_param_it == dock_param_map->end())
-    {
-      RCLCPP_INFO(
-        node->get_logger(),
-        "Dock param for dock_name:[%s] unavailable. Rejecting BidNotice with "
-        "task_id:[%s]", start_wp_name.c_str(), id.c_str());
-
-      return;
-    }
-    const auto& clean_param = clean_param_it->second;
-
-    // Check for valid finish waypoint
-    // This is the waypoint on the map where the robot will end up at the end
-    // of its cleaning process. RMF will not move the robot to this waypoint.
-    // This information is used to estimate the state of the robot at the end
-    // of its cleaning process which is relevant for task allocation planning.
-    const std::string& finish_wp_name = clean_param.finish;
-    const auto finish_wp = graph.find_waypoint(finish_wp_name);
-    if (!finish_wp)
-    {
-      RCLCPP_INFO(
-        node->get_logger(),
-        "Fleet [%s] does not have a named waypoint [%s] configured in its "
-        "nav graph. Rejecting BidNotice with task_id:[%s]",
-        name.c_str(), finish_wp_name.c_str(), id.c_str());
-
-      return;
-    }
-
-    // Interpolate docking waypoint into trajectory
-    std::vector<Eigen::Vector3d> positions;
-    for (const auto& location: clean_param.path)
-      positions.push_back({location.x, location.y, location.yaw});
-    rmf_traffic::Trajectory cleaning_trajectory =
-      rmf_traffic::agv::Interpolate::positions(
-      (*planner)->get_configuration().vehicle_traits(),
-      start_time,
-      positions);
-
-    if (cleaning_trajectory.size() == 0)
-    {
-      RCLCPP_INFO(
-        node->get_logger(),
-        "Unable to generate cleaning trajectory from positions specified "
-        " in DockSummary msg for [%s]", start_wp_name.c_str());
-
-      return;
-    }
-
-    new_request = rmf_task::requests::Clean::make(
-      start_wp->index(),
-      finish_wp->index(),
-      cleaning_trajectory,
-      id,
-      start_time,
-      priority);
-
-    RCLCPP_INFO(
-      node->get_logger(),
-      "Generated Clean request for task_id:[%s]", id.c_str());
+  try
+  {
+    request_validator.validate(request_msg);
   }
-
-  else if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_DELIVERY)
+  catch (const std::exception& e)
   {
-    const auto& delivery = task_profile.description.delivery;
-    if (delivery.pickup_place_name.empty())
-    {
-      RCLCPP_ERROR(
-        node->get_logger(),
-        "Required param [delivery.pickup_place_name] missing in TaskProfile."
-        "Rejecting BidNotice with task_id:[%s]", id.c_str());
-
-      return;
-    }
-
-    if (delivery.pickup_dispenser.empty())
-    {
-      RCLCPP_ERROR(
-        node->get_logger(),
-        "Required param [delivery.pickup_dispenser] missing in TaskProfile."
-        "Rejecting BidNotice with task_id:[%s]", id.c_str());
-
-      return;
-    }
-
-    if (delivery.dropoff_place_name.empty())
-    {
-      RCLCPP_ERROR(
-        node->get_logger(),
-        "Required param [delivery.dropoff_place_name] missing in TaskProfile."
-        "Rejecting BidNotice with task_id:[%s]", id.c_str());
-
-      return;
-    }
-
-    if (delivery.dropoff_place_name.empty())
-    {
-      RCLCPP_ERROR(
-        node->get_logger(),
-        "Required param [delivery.dropoff_place_name] missing in TaskProfile."
-        "Rejecting BidNotice with task_id:[%s]", id.c_str());
-
-      return;
-    }
-
-    if (delivery.dropoff_ingestor.empty())
-    {
-      RCLCPP_ERROR(
-        node->get_logger(),
-        "Required param [delivery.dropoff_ingestor] missing in TaskProfile."
-        "Rejecting BidNotice with task_id:[%s]", id.c_str());
-
-      return;
-    }
-
-    const auto pickup_wp = graph.find_waypoint(delivery.pickup_place_name);
-    if (!pickup_wp)
-    {
-      RCLCPP_INFO(
-        node->get_logger(),
-        "Fleet [%s] does not have a named waypoint [%s] configured in its "
-        "nav graph. Rejecting BidNotice with task_id:[%s]",
-        name.c_str(), delivery.pickup_place_name.c_str(), id.c_str());
-
-      return;
-    }
-
-    const auto dropoff_wp = graph.find_waypoint(delivery.dropoff_place_name);
-    if (!dropoff_wp)
-    {
-      RCLCPP_INFO(
-        node->get_logger(),
-        "Fleet [%s] does not have a named waypoint [%s] configured in its "
-        "nav graph. Rejecting BidNotice with task_id:[%s]",
-        name.c_str(), delivery.dropoff_place_name.c_str(), id.c_str());
-
-      return;
-    }
-
-    std::vector<rmf_task::Payload::Component> components;
-    components.reserve(delivery.items.size());
-    for (const auto& item : delivery.items)
-    {
-      components.push_back(
-        {item.type_guid, (uint32_t)item.quantity, item.compartment_name});
-    }
+    return respond(
+      {
+        std::nullopt,
+        {make_error_str(5, "Invalid request format", e.what())}
+      });
+  }
 
-    // TODO: We set the waiting duration at the pickup and dropoff locations to
-    // 0s as the cycle time of the dispensers and ingestors are not available.
-    // We should implement a means to lookup these values for each system.
-    new_request = rmf_task::requests::Delivery::make(
-      pickup_wp->index(),
-      rmf_traffic::time::from_seconds(0),
-      dropoff_wp->index(),
-      rmf_traffic::time::from_seconds(0),
-      rmf_task::Payload(std::move(components)),
-      id,
-      start_time,
-      priority,
-      false,
-      delivery.pickup_dispenser,
-      delivery.dropoff_ingestor);
+  const auto& category = request_msg["category"].get<std::string>();
+  const auto task_deser_it = deserialization.task.handlers.find(category);
+  if (task_deser_it == deserialization.task.handlers.end())
+  {
+    return respond(
+      {
+        std::nullopt,
+        {make_error_str(
+          4, "Unsupported type",
+          "Fleet [" + name + "] does not support task category ["
+          + category + "]")}
+      });
+  }
 
-    RCLCPP_INFO(
-      node->get_logger(),
-      "Generated Delivery request for task_id:[%s]", id.c_str());
+  const auto& description_msg = request_msg["description"];
+  const auto& task_deser_handler = task_deser_it->second;
 
+  try
+  {
+    task_deser_handler.validator->validate(description_msg);
   }
-  else if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_LOOP)
+  catch (const std::exception& e)
   {
-    const auto& loop = task_profile.description.loop;
-    if (loop.start_name.empty())
-    {
-      RCLCPP_ERROR(
-        node->get_logger(),
-        "Required param [loop.start_name] missing in TaskProfile."
-        "Rejecting BidNotice with task_id:[%s]", id.c_str());
+    return respond(
+      rmf_task_ros2::bidding::Response{
+        std::nullopt,
+        {make_error_str(5, "Invalid request format", e.what())}
+      });
+  }
 
-      return;
-    }
+  const auto deserialized_task =
+    task_deser_handler.deserializer(description_msg);
 
-    if (loop.finish_name.empty())
-    {
-      RCLCPP_ERROR(
-        node->get_logger(),
-        "Required param [loop.finish_name] missing in TaskProfile."
-        "Rejecting BidNotice with task_id:[%s]", id.c_str());
-
-      return;
-    }
+  if (!deserialized_task.description)
+  {
+    return respond(
+      {
+        std::nullopt,
+        deserialized_task.errors
+      });
+  }
 
-    if (loop.num_loops < 1)
-    {
-      RCLCPP_ERROR(
-        node->get_logger(),
-        "Required param [loop.num_loops: %d] in TaskProfile is invalid."
-        "Rejecting BidNotice with task_id:[%s]", loop.num_loops, id.c_str());
+  std::vector<std::string> errors;
 
-      return;
-    }
+  rmf_traffic::Time earliest_start_time =
+    rmf_traffic::Time(rmf_traffic::Duration::min());
+  if (const auto& t = request_msg["unix_millis_earliest_start_time"])
+  {
+    earliest_start_time =
+      rmf_traffic::Time(std::chrono::milliseconds(t.get<uint64_t>()));
+  }
 
-    const auto start_wp = graph.find_waypoint(loop.start_name);
-    if (!start_wp)
+  rmf_task::ConstPriorityPtr priority;
+  if (const auto& p = request_msg["priority"])
+  {
+    if (const auto& p_type = p["type"])
     {
-      RCLCPP_INFO(
-        node->get_logger(),
-        "Fleet [%s] does not have a named waypoint [%s] configured in its "
-        "nav graph. Rejecting BidNotice with task_id:[%s]",
-        name.c_str(), loop.start_name.c_str(), id.c_str());
+      if (p_type.is_string() && p_type.get<std::string>() == "binary")
+      {
+        if (const auto& p_value = p["value"])
+        {
+          if (p_value.is_number_integer() && p_value.get<uint64_t>() > 0)
+            priority = rmf_task::BinaryPriorityScheme::make_high_priority();
+        }
 
-      return;
+        priority = rmf_task::BinaryPriorityScheme::make_low_priority();
+      }
     }
 
-    const auto finish_wp = graph.find_waypoint(loop.finish_name);
-    if (!finish_wp)
+    if (!priority)
     {
-      RCLCPP_INFO(
-        node->get_logger(),
-        "Fleet [%s] does not have a named waypoint [%s] configured in its "
-        "nav graph. Rejecting BidNotice with task_id:[%s]",
-        name.c_str(), loop.finish_name.c_str(), id.c_str());
-
-      return;
+      errors.push_back(
+        make_error_str(
+          4, "Unsupported type",
+          "Fleet [" + name + "] does not support priority request: " + p.dump()
+          + "\nDefaulting to low binary priority."));
     }
-
-    new_request = rmf_task::requests::Loop::make(
-      start_wp->index(),
-      finish_wp->index(),
-      loop.num_loops,
-      id,
-      start_time,
-      priority);
-
-    RCLCPP_INFO(
-      node->get_logger(),
-      "Generated Loop request for task_id:[%s]", id.c_str());
   }
-  else
-  {
-    RCLCPP_ERROR(
-      node->get_logger(),
-      "Invalid TaskType [%d] in TaskProfile. Rejecting BidNotice with "
-      "task_id:[%s]",
-      task_type.type, id.c_str());
 
-    return;
-  }
+  if (!priority)
+    priority = rmf_task::BinaryPriorityScheme::make_low_priority();
 
-  if (!new_request)
-    return;
-  generated_requests.insert({id, new_request});
-  task_profile_map.insert({id, task_profile});
+  const auto new_request =
+    std::make_shared<rmf_task::Request>(
+      task_id,
+      earliest_start_time,
+      priority,
+      deserialized_task.description);
 
-  const auto allocation_result = allocate_tasks(new_request);
+  generated_requests.insert({task_id, new_request});
 
+  // TODO(MXG): Make the task planning asynchronous. The worker should schedule
+  // a job to perform the planning which should then spawn a job to save the
+  // plan result and respond. I started to refactor allocate_tasks(~) to make it
+  // async, but I will save the remaining effort for later, when there is more
+  // time to spare.
+  auto allocation_result = allocate_tasks(new_request, nullptr, &errors);
   if (!allocation_result.has_value())
-    return;
+    return respond({std::nullopt, std::move(errors)});
 
   const auto& assignments = allocation_result.value();
 
@@ -516,13 +344,6 @@ void FleetUpdateHandle::Implementation::bid_notice_cb(
 
   RCLCPP_DEBUG(node->get_logger(), "%s", debug_stream.str().c_str());
 
-  // Publish BidProposal
-  rmf_task_msgs::msg::BidProposal bid_proposal;
-  bid_proposal.fleet_name = name;
-  bid_proposal.task_profile = task_profile;
-  bid_proposal.prev_cost = current_assignment_cost;
-  bid_proposal.new_cost = cost;
-
   // Map robot index to name to populate robot_name in BidProposal
   std::unordered_map<std::size_t, std::string> robot_name_map;
   std::size_t index = 0;
@@ -532,32 +353,55 @@ void FleetUpdateHandle::Implementation::bid_notice_cb(
     ++index;
   }
 
+  std::optional<std::string> robot_name;
+  std::optional<rmf_traffic::Time> finish_time;
   index = 0;
   for (const auto& agent : assignments)
   {
     for (const auto& assignment : agent)
     {
-      if (assignment.request()->booking()->id() == id)
+      if (assignment.request()->booking()->id() == task_id)
       {
-        bid_proposal.finish_time = rmf_traffic_ros2::convert(
-          assignment.finish_state().time().value());
+        finish_time = assignment.finish_state().time().value();
         if (robot_name_map.find(index) != robot_name_map.end())
-          bid_proposal.robot_name = robot_name_map[index];
+          robot_name = robot_name_map[index];
         break;
       }
     }
     ++index;
   }
 
-  bid_proposal_pub->publish(bid_proposal);
+  if (!robot_name.has_value() || !finish_time.has_value())
+  {
+    errors.push_back(
+      make_error_str(
+        13, "Internal bug",
+        "Failed to find robot_name or finish_time after allocating task. "
+        "Please report this bug to the RMF developers."));
+
+    return respond({std::nullopt, std::move(errors)});
+  }
+
+  // Publish BidProposal
+  respond(
+    {
+      rmf_task_ros2::bidding::Response::Proposal{
+        name,
+        *robot_name,
+        current_assignment_cost,
+        cost,
+        *finish_time
+      },
+      std::move(errors)
+    });
+
   RCLCPP_INFO(
     node->get_logger(),
     "Submitted BidProposal to accommodate task [%s] by robot [%s] with new cost [%f]",
-    id.c_str(), bid_proposal.robot_name.c_str(), cost);
+    task_id.c_str(), robot_name->c_str(), cost);
 
   // Store assignments in internal map
-  bid_notice_assignments.insert({id, assignments});
-
+  bid_notice_assignments.insert({task_id, assignments});
 }
 
 //==============================================================================
@@ -567,21 +411,23 @@ void FleetUpdateHandle::Implementation::dispatch_request_cb(
   if (msg->fleet_name != name)
     return;
 
-  const std::string id = msg->task_profile.task_id;
+  const auto& task_id = msg->task_id;
   DispatchAck dispatch_ack;
-  dispatch_ack.dispatch_request = *msg;
   dispatch_ack.success = false;
-
-  if (msg->method == DispatchRequest::ADD)
+  dispatch_ack.dispatch_id = msg->dispatch_id;
+  if (msg->type == DispatchRequest::TYPE_AWARD)
   {
-    const auto task_it = bid_notice_assignments.find(id);
+    const auto task_it = bid_notice_assignments.find(task_id);
     if (task_it == bid_notice_assignments.end())
     {
-      RCLCPP_WARN(
-        node->get_logger(),
-        "Received DispatchRequest for task_id:[%s] before receiving BidNotice. "
-        "This request will be ignored.",
-        id.c_str());
+      std::string error_str =
+        "Received DispatchRequest for task_id [" + task_id
+        + "] before receiving BidNotice. This request will be ignored.";
+
+      RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str());
+
+      dispatch_ack.errors.push_back(
+        make_error_str(14, "Invalid sequence", std::move(error_str)));
       dispatch_ack_pub->publish(dispatch_ack);
       return;
     }
@@ -589,58 +435,71 @@ void FleetUpdateHandle::Implementation::dispatch_request_cb(
     RCLCPP_INFO(
       node->get_logger(),
       "Bid for task_id:[%s] awarded to fleet [%s]. Processing request...",
-      id.c_str(),
+      task_id.c_str(),
       name.c_str());
 
     auto& assignments = task_it->second;
 
     if (assignments.size() != task_managers.size())
     {
-      RCLCPP_ERROR(
-        node->get_logger(),
+      // FIXME(MXG): This error mode seems like a problem with our
+      // implementation. If a robot is added during a bid process, we could
+      // trigger this error even though it shouldn't actually be a problem.
+
+      std::string error_str =
         "The number of available robots does not match that in the assignments "
-        "for task_id:[%s]. This request will be ignored.",
-        id.c_str());
+        "for task_id [" + task_id + "]. This request will be ignored.";
+      RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str());
+      dispatch_ack.errors.push_back(
+        make_error_str(13, "Internal bug", std::move(error_str)));
+
       dispatch_ack_pub->publish(dispatch_ack);
       return;
     }
 
-    // Here we make sure none of the tasks in the assignments has already begun
-    // execution. If so, we replan assignments until a valid set is obtained
-    // and only then update the task manager queues
-    const auto request_it = generated_requests.find(id);
+    const auto request_it = generated_requests.find(task_id);
     if (request_it == generated_requests.end())
     {
-      RCLCPP_ERROR(
-        node->get_logger(),
-        "Unable to find generated request for task_id:[%s]. This request will "
-        "be ignored.",
-        id.c_str());
+      std::string error_str =
+        "Unable to find generated request for task_id [" + task_id
+        + "]. This request will be ignored.";
+
+      RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str());
+      dispatch_ack.errors.push_back(
+        make_error_str(14, "Invalid sequence", std::move(error_str)));
+
       dispatch_ack_pub->publish(dispatch_ack);
       return;
     }
 
+    // Here we make sure none of the tasks in the assignments has already begun
+    // execution. If so, we replan assignments until a valid set is obtained
+    // and only then update the task manager queues
     bool valid_assignments = is_valid_assignments(assignments);
     if (!valid_assignments)
     {
       // TODO: This replanning is blocking the main thread. Instead, the
       // replanning should run on a separate worker and then deliver the
       // result back to the main worker.
-      const auto replan_results = allocate_tasks(request_it->second);
+      const auto replan_results = allocate_tasks(
+        request_it->second, nullptr, &dispatch_ack.errors);
+
       if (!replan_results)
       {
-        RCLCPP_WARN(
-          node->get_logger(),
-          "Unable to replan assignments when accommodating task_id:[%s]. This "
-          "request will be ignored.",
-          id.c_str());
+        std::string error_str =
+          "Unable to replan assignments when accommodating task_id [" + task_id
+          + "]. This request will be ignored.";
+
+        RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str());
         dispatch_ack_pub->publish(dispatch_ack);
         return;
       }
+
       assignments = replan_results.value();
       // We do not need to re-check if assignments are valid as this function
       // is being called by the ROS2 executor and is running on the main
-      // rxcpp worker. Hence, no new tasks would have started during this replanning.
+      // rxcpp worker. Hence, no new tasks would have started during this
+      // replanning.
     }
 
     std::size_t index = 0;
@@ -651,17 +510,16 @@ void FleetUpdateHandle::Implementation::dispatch_request_cb(
     }
 
     current_assignment_cost = task_planner->compute_cost(assignments);
-    assigned_requests.insert({id, request_it->second});
+    assigned_requests.insert({task_id, request_it->second});
     dispatch_ack.success = true;
     dispatch_ack_pub->publish(dispatch_ack);
 
     RCLCPP_INFO(
       node->get_logger(),
       "Assignments updated for robots in fleet [%s] to accommodate task_id:[%s]",
-      name.c_str(), id.c_str());
+      name.c_str(), task_id.c_str());
   }
-
-  else if (msg->method == DispatchRequest::CANCEL)
+  else if (msg->type == DispatchRequest::TYPE_REMOVE)
   {
     // When a queued task is to be cancelled, we simply re-plan and re-allocate
     // task assignments for the request set containing all the queued tasks
@@ -746,17 +604,14 @@ void FleetUpdateHandle::Implementation::dispatch_request_cb(
       "updated for robots in fleet [%s].",
       id.c_str(), name.c_str());
   }
-
   else
   {
-    RCLCPP_WARN(
-      node->get_logger(),
-      "Received DispatchRequest for task_id:[%s] with invalid method. Only "
-      "ADD and CANCEL methods are supported. This request will be ignored.",
-      id.c_str());
-    return;
+    std::string error_str =
+      "Received unknown dispatch request type: " + std::to_string(msg->type);
+    RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str());
+    dispatch_ack.errors.push_back(
+      make_error_str(4, "Unsupported type", std::move(error_str)));
   }
-
 }
 
 //==============================================================================
@@ -1052,44 +907,53 @@ void FleetUpdateHandle::Implementation::add_standard_tasks()
     node->clock());
 }
 
+//==============================================================================
+auto FleetUpdateHandle::Implementation::aggregate_expectations() const
+-> Expectations
+{
+  Expectations expect;
+  for (const auto& t : task_managers)
+  {
+    expect.states.push_back(t.second->expected_finish_state());
+    const auto requests = t.second->requests();
+    expect.pending_requests.insert(
+      expect.pending_requests.end(), requests.begin(), requests.end());
+  }
+
+  return expect;
+}
+
 //==============================================================================
 auto FleetUpdateHandle::Implementation::allocate_tasks(
   rmf_task::ConstRequestPtr new_request,
-  rmf_task::ConstRequestPtr ignore_request) const -> std::optional<Assignments>
+  rmf_task::ConstRequestPtr ignore_request,
+  std::vector<std::string>* errors) const -> std::optional<Assignments>
 {
   // Collate robot states, constraints and combine new requestptr with
   // requestptr of non-charging tasks in task manager queues
-  std::vector<rmf_task::State> states;
-  std::vector<rmf_task::ConstRequestPtr> pending_requests;
+  auto expect = aggregate_expectations();
   std::string id = "";
 
   if (new_request)
   {
-    pending_requests.push_back(new_request);
+    expect.pending_requests.push_back(new_request);
     id = new_request->booking()->id();
   }
 
-  for (const auto& t : task_managers)
-  {
-    states.push_back(t.second->expected_finish_state());
-    const auto requests = t.second->requests();
-    pending_requests.insert(
-      pending_requests.end(), requests.begin(), requests.end());
-  }
-
   // Remove the request to be ignored if present
   if (ignore_request)
   {
-    auto ignore_request_it = pending_requests.end();
-    for (auto it = pending_requests.begin(); it != pending_requests.end(); ++it)
+    auto ignore_request_it = expect.pending_requests.end();
+    for (auto it = expect.pending_requests.begin();
+         it != expect.pending_requests.end(); ++it)
     {
       auto pending_request = *it;
       if (pending_request->booking()->id() == ignore_request->booking()->id())
         ignore_request_it = it;
     }
-    if (ignore_request_it != pending_requests.end())
+    if (ignore_request_it != expect.pending_requests.end())
     {
-      pending_requests.erase(ignore_request_it);
+      expect.pending_requests.erase(ignore_request_it);
       RCLCPP_INFO(
         node->get_logger(),
         "Request with task_id:[%s] will be ignored during task allocation.",
@@ -1107,14 +971,14 @@ auto FleetUpdateHandle::Implementation::allocate_tasks(
   RCLCPP_INFO(
     node->get_logger(),
     "Planning for [%ld] robot(s) and [%ld] request(s)",
-    states.size(),
-    pending_requests.size());
+    expect.states.size(),
+    expect.pending_requests.size());
 
   // Generate new task assignments
   const auto result = task_planner->plan(
     rmf_traffic_ros2::convert(node->now()),
-    states,
-    pending_requests);
+    expect.states,
+    expect.pending_requests);
 
   auto assignments_ptr = std::get_if<
     rmf_task::TaskPlanner::Assignments>(&result);
@@ -1126,29 +990,46 @@ auto FleetUpdateHandle::Implementation::allocate_tasks(
 
     if (*error == rmf_task::TaskPlanner::TaskPlannerError::low_battery)
     {
-      RCLCPP_ERROR(
-        node->get_logger(),
-        "[TaskPlanner] Failed to compute assignments for task_id:[%s] due to"
-        " insufficient initial battery charge for all robots in this fleet.",
-        id.c_str());
+      std::string error_str =
+        "[TaskPlanner] Failed to compute assignments for task_id [" + id
+        + "] due to insufficient initial battery charge for all robots in this "
+        "fleet.";
+
+      RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str());
+      if (errors)
+      {
+        errors->push_back(
+          make_error_str(9, "Not feasible", std::move(error_str)));
+      }
     }
 
     else if (*error ==
       rmf_task::TaskPlanner::TaskPlannerError::limited_capacity)
     {
-      RCLCPP_ERROR(
-        node->get_logger(),
-        "[TaskPlanner] Failed to compute assignments for task_id:[%s] due to"
-        " insufficient battery capacity to accommodate one or more requests by"
-        " any of the robots in this fleet.", id.c_str());
+      std::string error_str =
+        "[TaskPlanner] Failed to compute assignments for task_id [" + id
+        + "] due to insufficient battery capacity to accommodate one or more "
+        "requests by any of the robots in this fleet.";
+
+      RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str());
+      if (errors)
+      {
+        errors->push_back(
+          make_error_str(9, "Not feasible", std::move(error_str)));
+      }
     }
 
     else
     {
-      RCLCPP_ERROR(
-        node->get_logger(),
-        "[TaskPlanner] Failed to compute assignments for task_id:[%s]",
-        id.c_str());
+      std::string error_str =
+        "[TaskPlanner] Failed to compute assignments for task_id [" + id + "]";
+
+      RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str());
+      if (errors)
+      {
+        errors->push_back(
+          make_error_str(9, "Not feasible", std::move(error_str)));
+      }
     }
 
     return std::nullopt;
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
index 36db42640..df90f5670 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
@@ -180,6 +180,7 @@ class FleetUpdateHandle::Implementation
 {
 public:
 
+  std::weak_ptr<FleetUpdateHandle> weak_self;
   std::string name;
   std::shared_ptr<std::shared_ptr<const rmf_traffic::agv::Planner>> planner;
   std::shared_ptr<Node> node;
@@ -243,8 +244,6 @@ class FleetUpdateHandle::Implementation
   std::unordered_map<
     std::string, rmf_task::ConstRequestPtr> assigned_requests = {};
   std::unordered_set<std::string> cancelled_task_ids = {};
-  using TaskProfileMsg = rmf_task_msgs::msg::TaskProfile;
-  std::unordered_map<std::string, TaskProfileMsg> task_profile_map = {};
 
   using BidNoticeMsg = rmf_task_msgs::msg::BidNotice;
 
@@ -265,7 +264,7 @@ class FleetUpdateHandle::Implementation
   {
     auto handle = std::shared_ptr<FleetUpdateHandle>(new FleetUpdateHandle);
     handle->_pimpl = rmf_utils::make_unique_impl<Implementation>(
-      Implementation{std::forward<Args>(args)...});
+      Implementation{handle, std::forward<Args>(args)...});
 
     handle->_pimpl->add_standard_tasks();
 
@@ -371,12 +370,21 @@ class FleetUpdateHandle::Implementation
   std::optional<std::size_t> get_nearest_charger(
     const rmf_traffic::agv::Planner::Start& start);
 
+  struct Expectations
+  {
+    std::vector<rmf_task::State> states;
+    std::vector<rmf_task::ConstRequestPtr> pending_requests;
+  };
+
+  Expectations aggregate_expectations() const;
+
   /// Generate task assignments for a collection of task requests comprising of
   /// task requests currently in TaskManager queues while optionally including a
   /// new request and while optionally ignoring a specific request.
   std::optional<Assignments> allocate_tasks(
     rmf_task::ConstRequestPtr new_request = nullptr,
-    rmf_task::ConstRequestPtr ignore_request = nullptr) const;
+    rmf_task::ConstRequestPtr ignore_request = nullptr,
+    std::vector<std::string>* errors = nullptr) const;
 
   /// Helper function to check if assignments are valid. An assignment set is
   /// invalid if one of the assignments has already begun execution.

From 36a2cc23b6a885c4fb46b8f58c2b11ec72dfe6a5 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Sun, 9 Jan 2022 19:56:06 +0800
Subject: [PATCH 45/79] Finished critical parts of migration -- needs testing

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 rmf_fleet_adapter/CMakeLists.txt              |  12 +-
 .../agv/test/MockAdapter.hpp                  |   2 +-
 .../src/rmf_fleet_adapter/TaskManager.cpp     |  27 ++
 .../src/rmf_fleet_adapter/TaskManager.hpp     |   4 +
 .../agv/FleetUpdateHandle.cpp                 | 288 +++++++++---------
 .../agv/internal_FleetUpdateHandle.hpp        |  23 +-
 .../agv/test/MockAdapter.cpp                  |  51 +++-
 .../test/tasks/test_Delivery.cpp              |  29 +-
 rmf_fleet_adapter/test/tasks/test_Loop.cpp    |  28 +-
 .../src/rmf_task_ros2/Dispatcher.cpp          |   2 +-
 10 files changed, 255 insertions(+), 211 deletions(-)

diff --git a/rmf_fleet_adapter/CMakeLists.txt b/rmf_fleet_adapter/CMakeLists.txt
index 626b49d04..843c9fb4d 100644
--- a/rmf_fleet_adapter/CMakeLists.txt
+++ b/rmf_fleet_adapter/CMakeLists.txt
@@ -483,17 +483,7 @@ rmf_api_generate_schema_headers(
 # -----------------------------------------------------------------------------
 
 ament_export_targets(rmf_fleet_adapter HAS_LIBRARY_TARGET)
-ament_export_dependencies(
-  rmf_task
-  rmf_task_sequence
-  rmf_traffic_ros2
-  rmf_fleet_msgs
-  rmf_door_msgs
-  rmf_lift_msgs
-  rmf_ingestor_msgs
-  rmf_dispenser_msgs
-  websocketpp
-)
+ament_export_dependencies(${dep_pkgs})
 
 install(
   TARGETS
diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/test/MockAdapter.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/test/MockAdapter.hpp
index 225730b29..bf2ecbf83 100644
--- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/test/MockAdapter.hpp
+++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/test/MockAdapter.hpp
@@ -72,7 +72,7 @@ class MockAdapter : public std::enable_shared_from_this<MockAdapter>
   void stop();
 
   /// Submit a task request
-  void dispatch_task(const rmf_task_msgs::msg::TaskProfile& profile);
+  void dispatch_task(std::string task_id, const nlohmann::json& request);
 
   ~MockAdapter();
 
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
index f4c62c5e1..0124c9730 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
@@ -731,6 +731,33 @@ std::optional<std::string> TaskManager::current_task_id() const
   return std::nullopt;
 }
 
+//==============================================================================
+auto TaskManager::get_queue() const -> const std::vector<Assignment>&
+{
+  return _queue;
+}
+
+//==============================================================================
+bool TaskManager::cancel_task_if_present(const std::string& task_id)
+{
+  if (_active_task && _active_task.id() == task_id)
+  {
+    _active_task.cancel({"DispatchRequest"}, _context->now());
+    return true;
+  }
+
+  for (auto it = _queue.begin(); it != _queue.end(); ++it)
+  {
+    if (it->request()->booking()->id() == task_id)
+    {
+      _queue.erase(it);
+      return true;
+    }
+  }
+
+  return false;
+}
+
 //==============================================================================
 std::string TaskManager::robot_status() const
 {
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
index 112986c06..fb62e4469 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
@@ -81,6 +81,10 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
 
   std::optional<std::string> current_task_id() const;
 
+  const std::vector<Assignment>& get_queue() const;
+
+  bool cancel_task_if_present(const std::string& task_id);
+
   std::string robot_status() const;
 
   /// The state of the robot.
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
index d71e4c519..a8229f956 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
@@ -302,14 +302,12 @@ void FleetUpdateHandle::Implementation::bid_notice_cb(
       priority,
       deserialized_task.description);
 
-  generated_requests.insert({task_id, new_request});
-
   // TODO(MXG): Make the task planning asynchronous. The worker should schedule
   // a job to perform the planning which should then spawn a job to save the
   // plan result and respond. I started to refactor allocate_tasks(~) to make it
   // async, but I will save the remaining effort for later, when there is more
   // time to spare.
-  auto allocation_result = allocate_tasks(new_request, nullptr, &errors);
+  auto allocation_result = allocate_tasks(new_request, &errors);
   if (!allocation_result.has_value())
     return respond({std::nullopt, std::move(errors)});
 
@@ -406,26 +404,62 @@ void FleetUpdateHandle::Implementation::bid_notice_cb(
 
 //==============================================================================
 void FleetUpdateHandle::Implementation::dispatch_request_cb(
-  const DispatchRequest::SharedPtr msg)
+  const DispatchCmdMsg::SharedPtr msg)
 {
+  const auto& task_id = msg->task_id;
   if (msg->fleet_name != name)
+  {
+    // This task is either being awarded or canceled for another fleet. Either
+    // way, we will delete it from our record of bid notice assignments.
+    bid_notice_assignments.erase(task_id);
     return;
+  }
 
-  const auto& task_id = msg->task_id;
   DispatchAck dispatch_ack;
   dispatch_ack.success = false;
   dispatch_ack.dispatch_id = msg->dispatch_id;
-  if (msg->type == DispatchRequest::TYPE_AWARD)
+  if (msg->type == DispatchCmdMsg::TYPE_AWARD)
   {
     const auto task_it = bid_notice_assignments.find(task_id);
     if (task_it == bid_notice_assignments.end())
     {
+      // We don't have a record of this bid_notice, so let's check if we already
+      // received the task assignment.
+      bool already_assigned = false;
+      for (const auto& tm : task_managers)
+      {
+        const auto current = tm.second->current_task_id();
+        if (current == task_id)
+        {
+          already_assigned = true;
+          break;
+        }
+
+        for (const auto& p : tm.second->get_queue())
+        {
+          if (p.request()->booking()->id() == task_id)
+          {
+            already_assigned = true;
+            break;
+          }
+        }
+
+        if (already_assigned)
+          break;
+      }
+
+      if (already_assigned)
+      {
+        dispatch_ack.success = true;
+        dispatch_ack_pub->publish(dispatch_ack);
+        return;
+      }
+
       std::string error_str =
-        "Received DispatchRequest for task_id [" + task_id
+        "Received DispatchRequest award for task_id [" + task_id
         + "] before receiving BidNotice. This request will be ignored.";
 
       RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str());
-
       dispatch_ack.errors.push_back(
         make_error_str(14, "Invalid sequence", std::move(error_str)));
       dispatch_ack_pub->publish(dispatch_ack);
@@ -438,7 +472,8 @@ void FleetUpdateHandle::Implementation::dispatch_request_cb(
       task_id.c_str(),
       name.c_str());
 
-    auto& assignments = task_it->second;
+    auto assignments = std::move(task_it->second);
+    bid_notice_assignments.erase(task_it);
 
     if (assignments.size() != task_managers.size())
     {
@@ -457,33 +492,46 @@ void FleetUpdateHandle::Implementation::dispatch_request_cb(
       return;
     }
 
-    const auto request_it = generated_requests.find(task_id);
-    if (request_it == generated_requests.end())
-    {
-      std::string error_str =
-        "Unable to find generated request for task_id [" + task_id
-        + "]. This request will be ignored.";
-
-      RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str());
-      dispatch_ack.errors.push_back(
-        make_error_str(14, "Invalid sequence", std::move(error_str)));
-
-      dispatch_ack_pub->publish(dispatch_ack);
-      return;
-    }
-
     // Here we make sure none of the tasks in the assignments has already begun
     // execution. If so, we replan assignments until a valid set is obtained
     // and only then update the task manager queues
     bool valid_assignments = is_valid_assignments(assignments);
     if (!valid_assignments)
     {
+      rmf_task::ConstRequestPtr request;
+      for (const auto& a : assignments)
+      {
+        for (const auto& r : a)
+        {
+          if (r.request()->booking()->id() == task_id)
+          {
+            request = r.request();
+            break;
+          }
+        }
+
+        if (request)
+          break;
+      }
+
+      if (!request)
+      {
+        std::string error_str =
+          "Could not find task_id [" + task_id + "] in the set of assignments "
+          "associated with it. This is a critical bug and should be reported "
+          "to the RMF developers";
+
+        RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str());
+        dispatch_ack.errors.push_back(
+          make_error_str(13, "Internal bug", std::move(error_str)));
+        dispatch_ack_pub->publish(dispatch_ack);
+        return;
+      }
+
       // TODO: This replanning is blocking the main thread. Instead, the
       // replanning should run on a separate worker and then deliver the
       // result back to the main worker.
-      const auto replan_results = allocate_tasks(
-        request_it->second, nullptr, &dispatch_ack.errors);
-
+      const auto replan_results = allocate_tasks(request, &dispatch_ack.errors);
       if (!replan_results)
       {
         std::string error_str =
@@ -491,6 +539,8 @@ void FleetUpdateHandle::Implementation::dispatch_request_cb(
           + "]. This request will be ignored.";
 
         RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str());
+        dispatch_ack.errors.push_back(
+          make_error_str(9, "Not feasible", std::move(error_str)));
         dispatch_ack_pub->publish(dispatch_ack);
         return;
       }
@@ -510,7 +560,6 @@ void FleetUpdateHandle::Implementation::dispatch_request_cb(
     }
 
     current_assignment_cost = task_planner->compute_cost(assignments);
-    assigned_requests.insert({task_id, request_it->second});
     dispatch_ack.success = true;
     dispatch_ack_pub->publish(dispatch_ack);
 
@@ -519,90 +568,77 @@ void FleetUpdateHandle::Implementation::dispatch_request_cb(
       "Assignments updated for robots in fleet [%s] to accommodate task_id:[%s]",
       name.c_str(), task_id.c_str());
   }
-  else if (msg->type == DispatchRequest::TYPE_REMOVE)
+  else if (msg->type == DispatchCmdMsg::TYPE_REMOVE)
   {
-    // When a queued task is to be cancelled, we simply re-plan and re-allocate
-    // task assignments for the request set containing all the queued tasks
-    // excluding the task to be cancelled.
-    if (cancelled_task_ids.find(id) != cancelled_task_ids.end())
-    {
-      RCLCPP_WARN(
-        node->get_logger(),
-        "Request with task_id:[%s] has already been cancelled.",
-        id.c_str());
-
-      dispatch_ack.success = true;
-      dispatch_ack_pub->publish(dispatch_ack);
-      return;
-    }
-
-    auto request_to_cancel_it = assigned_requests.find(id);
-    if (request_to_cancel_it == assigned_requests.end())
+    const auto bid_it = bid_notice_assignments.find(task_id);
+    if (bid_it != bid_notice_assignments.end())
     {
-      RCLCPP_WARN(
-        node->get_logger(),
-        "Unable to cancel task with task_id:[%s] as it is not assigned to "
-        "fleet:[%s].",
-        id.c_str(), name.c_str());
-
-      dispatch_ack_pub->publish(dispatch_ack);
-      return;
-    }
-
-    std::unordered_set<std::string> executed_tasks;
-    for (const auto& [context, mgr] : task_managers)
-    {
-      const auto& tasks = mgr->get_executed_tasks();
-      executed_tasks.insert(tasks.begin(), tasks.end());
+      // The task was still in the bid notice assignments, so it was never
+      // actually assigned to a robot. We will just delete it from this map
+      // and we're done.
+      bid_notice_assignments.erase(bid_it);
     }
-
-    // Check if received request is to cancel an active task
-    if (executed_tasks.find(id) != executed_tasks.end())
+    else
     {
-      RCLCPP_WARN(
-        node->get_logger(),
-        "Unable to cancel active task with task_id:[%s]. Only queued tasks may "
-        "be cancelled.",
-        id.c_str());
+      bool task_was_found = false;
+      // Make sure the task isn't running in any of the task managers
+      for (const auto& [_, tm] : task_managers)
+      {
+        task_was_found = tm->cancel_task_if_present(task_id);
+        if (task_was_found)
+          break;
+      }
 
-      dispatch_ack_pub->publish(dispatch_ack);
-      return;
-    }
+      if (task_was_found)
+      {
+        // Re-plan assignments while ignoring request for task to be cancelled
+        std::vector<std::string> errors;
+        const auto replan_results = allocate_tasks(nullptr, &errors);
+        if (!replan_results.has_value())
+        {
+          std::stringstream ss;
+          ss << "Unabled to replan assignments when cancelling task ["
+             << task_id << "]. ";
+          if (errors.empty())
+          {
+            ss << "No planner error messages were provided.";
+          }
+          else
+          {
+            ss << "The following planner errors occurred:";
+            for (const auto& e : errors)
+            {
+              const auto err = nlohmann::json::parse(e);
+              ss << "\n -- " << err["detail"].get<std::string>();
+            }
+          }
+          ss << "\n";
 
-    // Re-plan assignments while ignoring request for task to be cancelled
-    const auto replan_results = allocate_tasks(
-      nullptr, request_to_cancel_it->second);
+          RCLCPP_WARN(node->get_logger(), "%s", ss.str().c_str());
+        }
+        else
+        {
+          const auto& assignments = replan_results.value();
+          std::size_t index = 0;
+          for (auto& t : task_managers)
+          {
+            t.second->set_queue(assignments[index]);
+            ++index;
+          }
 
-    if (!replan_results.has_value())
-    {
-      RCLCPP_WARN(
-        node->get_logger(),
-        "Unable to re-plan assignments when cancelling task with task_id:[%s]",
-        id.c_str());
+          current_assignment_cost = task_planner->compute_cost(assignments);
 
-      dispatch_ack_pub->publish(dispatch_ack);
-      return;
-    }
-
-    const auto& assignments = replan_results.value();
-    std::size_t index = 0;
-    for (auto& t : task_managers)
-    {
-      t.second->set_queue(assignments[index]);
-      ++index;
+          RCLCPP_INFO(
+            node->get_logger(),
+            "Task with task_id [%s] has successfully been cancelled. Assignments "
+            "updated for robots in fleet [%s].",
+            task_id.c_str(), name.c_str());
+        }
+      }
     }
 
-    current_assignment_cost = task_planner->compute_cost(assignments);
-
     dispatch_ack.success = true;
     dispatch_ack_pub->publish(dispatch_ack);
-    cancelled_task_ids.insert(id);
-
-    RCLCPP_INFO(
-      node->get_logger(),
-      "LegacyTask with task_id:[%s] has successfully been cancelled. Assignments "
-      "updated for robots in fleet [%s].",
-      id.c_str(), name.c_str());
   }
   else
   {
@@ -611,6 +647,7 @@ void FleetUpdateHandle::Implementation::dispatch_request_cb(
     RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str());
     dispatch_ack.errors.push_back(
       make_error_str(4, "Unsupported type", std::move(error_str)));
+    dispatch_ack_pub->publish(dispatch_ack);
   }
 }
 
@@ -926,7 +963,6 @@ auto FleetUpdateHandle::Implementation::aggregate_expectations() const
 //==============================================================================
 auto FleetUpdateHandle::Implementation::allocate_tasks(
   rmf_task::ConstRequestPtr new_request,
-  rmf_task::ConstRequestPtr ignore_request,
   std::vector<std::string>* errors) const -> std::optional<Assignments>
 {
   // Collate robot states, constraints and combine new requestptr with
@@ -940,34 +976,6 @@ auto FleetUpdateHandle::Implementation::allocate_tasks(
     id = new_request->booking()->id();
   }
 
-  // Remove the request to be ignored if present
-  if (ignore_request)
-  {
-    auto ignore_request_it = expect.pending_requests.end();
-    for (auto it = expect.pending_requests.begin();
-         it != expect.pending_requests.end(); ++it)
-    {
-      auto pending_request = *it;
-      if (pending_request->booking()->id() == ignore_request->booking()->id())
-        ignore_request_it = it;
-    }
-    if (ignore_request_it != expect.pending_requests.end())
-    {
-      expect.pending_requests.erase(ignore_request_it);
-      RCLCPP_INFO(
-        node->get_logger(),
-        "Request with task_id:[%s] will be ignored during task allocation.",
-        ignore_request->booking()->id().c_str());
-    }
-    else
-    {
-      RCLCPP_WARN(
-        node->get_logger(),
-        "Request with task_id:[%s] is not present in any of the task queues.",
-        ignore_request->booking()->id().c_str());
-    }
-  }
-
   RCLCPP_INFO(
     node->get_logger(),
     "Planning for [%ld] robot(s) and [%ld] request(s)",
@@ -1381,7 +1389,7 @@ void FleetUpdateHandle::open_lanes(std::vector<std::size_t> lane_indices)
 FleetUpdateHandle& FleetUpdateHandle::accept_task_requests(
   AcceptTaskRequest check)
 {
-  const auto legacy_adapter =
+  const auto legacy_converter =
     [check = std::move(check)](
       const rmf_task_msgs::msg::TaskProfile& profile,
       Confirmation& confirm)
@@ -1429,7 +1437,7 @@ FleetUpdateHandle& FleetUpdateHandle::accept_task_requests(
       return items;
     };
 
-  auto consider_pickup = [legacy_adapter, convert_items](
+  auto consider_pickup = [legacy_converter, convert_items](
     const nlohmann::json& msg, Confirmation& confirm)
     {
       rmf_task_msgs::msg::TaskProfile profile;
@@ -1441,16 +1449,16 @@ FleetUpdateHandle& FleetUpdateHandle::accept_task_requests(
           .task_id("")
           .items(convert_items(msg["payload"]))
           .pickup_place_name(msg["place"].get<std::string>())
-          .pickup_dispenser(msg["hanlder"].get<std::string>())
+          .pickup_dispenser(msg["handler"].get<std::string>())
           .pickup_behavior(rmf_task_msgs::msg::Behavior{})
           .dropoff_place_name("")
           .dropoff_ingestor("")
           .dropoff_behavior(rmf_task_msgs::msg::Behavior{});
 
-      legacy_adapter(profile, confirm);
+      legacy_converter(profile, confirm);
     };
 
-  auto consider_dropoff = [legacy_adapter, convert_items](
+  auto consider_dropoff = [legacy_converter, convert_items](
     const nlohmann::json& msg, Confirmation& confirm)
     {
     rmf_task_msgs::msg::TaskProfile profile;
@@ -1465,10 +1473,10 @@ FleetUpdateHandle& FleetUpdateHandle::accept_task_requests(
         .pickup_dispenser("")
         .pickup_behavior(rmf_task_msgs::msg::Behavior{})
         .dropoff_place_name(msg["place"].get<std::string>())
-        .dropoff_ingestor(msg["hanlder"].get<std::string>())
+        .dropoff_ingestor(msg["handler"].get<std::string>())
         .dropoff_behavior(rmf_task_msgs::msg::Behavior{});
 
-      legacy_adapter(profile, confirm);
+      legacy_converter(profile, confirm);
     };
 
   consider_delivery_requests(
@@ -1476,7 +1484,7 @@ FleetUpdateHandle& FleetUpdateHandle::accept_task_requests(
     std::move(consider_dropoff));
 
   consider_patrol_requests(
-    [legacy_adapter](const nlohmann::json& msg, Confirmation& confirm)
+    [legacy_converter](const nlohmann::json& msg, Confirmation& confirm)
     {
       rmf_task_msgs::msg::TaskProfile profile;
 
@@ -1520,11 +1528,11 @@ FleetUpdateHandle& FleetUpdateHandle::accept_task_requests(
       loop.num_loops = msg["rounds"].get<uint32_t>();
       profile.description.loop = std::move(loop);
 
-      legacy_adapter(profile, confirm);
+      legacy_converter(profile, confirm);
     });
 
   consider_cleaning_requests(
-    [legacy_adapter](const nlohmann::json& msg, Confirmation& confirm)
+    [legacy_converter](const nlohmann::json& msg, Confirmation& confirm)
     {
       rmf_task_msgs::msg::TaskProfile profile;
 
@@ -1533,7 +1541,7 @@ FleetUpdateHandle& FleetUpdateHandle::accept_task_requests(
 
       profile.description.clean.start_waypoint = msg["zone"].get<std::string>();
 
-      legacy_adapter(profile, confirm);
+      legacy_converter(profile, confirm);
     });
 
   return *this;
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
index df90f5670..a3d9b65a0 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
@@ -22,7 +22,7 @@
 
 #include <rmf_task_ros2/bidding/AsyncBidder.hpp>
 
-#include <rmf_task_msgs/msg/dispatch_request.hpp>
+#include <rmf_task_msgs/msg/dispatch_command.hpp>
 #include <rmf_task_msgs/msg/dispatch_ack.hpp>
 
 #include <rmf_task/TaskPlanner.hpp>
@@ -233,23 +233,17 @@ class FleetUpdateHandle::Implementation
   // TODO Support for various charging configurations
   std::unordered_set<std::size_t> charging_waypoints = {};
 
-  std::shared_ptr<rmf_task_ros2::bidding::AsyncBidder> bidder;
+  std::shared_ptr<rmf_task_ros2::bidding::AsyncBidder> bidder = nullptr;
 
   double current_assignment_cost = 0.0;
   // Map to store task id with assignments for BidNotice
   std::unordered_map<std::string, Assignments> bid_notice_assignments = {};
 
-  std::unordered_map<
-    std::string, rmf_task::ConstRequestPtr> generated_requests = {};
-  std::unordered_map<
-    std::string, rmf_task::ConstRequestPtr> assigned_requests = {};
-  std::unordered_set<std::string> cancelled_task_ids = {};
-
   using BidNoticeMsg = rmf_task_msgs::msg::BidNotice;
 
-  using DispatchRequest = rmf_task_msgs::msg::DispatchRequest;
-  using DispatchRequestSub = rclcpp::Subscription<DispatchRequest>::SharedPtr;
-  DispatchRequestSub dispatch_request_sub = nullptr;
+  using DispatchCmdMsg = rmf_task_msgs::msg::DispatchCommand;
+  using DispatchCommandSub = rclcpp::Subscription<DispatchCmdMsg>::SharedPtr;
+  DispatchCommandSub dispatch_request_sub = nullptr;
 
   using DispatchAck = rmf_task_msgs::msg::DispatchAck;
   using DispatchAckPub = rclcpp::Publisher<DispatchAck>::SharedPtr;
@@ -306,10 +300,10 @@ class FleetUpdateHandle::Implementation
 
     // Subscribe DispatchRequest
     handle->_pimpl->dispatch_request_sub =
-      handle->_pimpl->node->create_subscription<DispatchRequest>(
+      handle->_pimpl->node->create_subscription<DispatchCmdMsg>(
       DispatchRequestTopicName,
       default_qos,
-      [w = handle->weak_from_this()](const DispatchRequest::SharedPtr msg)
+      [w = handle->weak_from_this()](const DispatchCmdMsg::SharedPtr msg)
       {
         if (const auto self = w.lock())
           self->_pimpl->dispatch_request_cb(msg);
@@ -365,7 +359,7 @@ class FleetUpdateHandle::Implementation
     const BidNoticeMsg& msg,
     rmf_task_ros2::bidding::AsyncBidder::Respond respond);
 
-  void dispatch_request_cb(const DispatchRequest::SharedPtr msg);
+  void dispatch_request_cb(const DispatchCmdMsg::SharedPtr msg);
 
   std::optional<std::size_t> get_nearest_charger(
     const rmf_traffic::agv::Planner::Start& start);
@@ -383,7 +377,6 @@ class FleetUpdateHandle::Implementation
   /// new request and while optionally ignoring a specific request.
   std::optional<Assignments> allocate_tasks(
     rmf_task::ConstRequestPtr new_request = nullptr,
-    rmf_task::ConstRequestPtr ignore_request = nullptr,
     std::vector<std::string>* errors = nullptr) const;
 
   /// Helper function to check if assignments are valid. An assignment set is
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp
index 330bdc16d..d4b13808f 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp
@@ -307,29 +307,50 @@ void MockAdapter::stop()
 }
 
 //==============================================================================
-void MockAdapter::dispatch_task(const rmf_task_msgs::msg::TaskProfile& profile)
+void MockAdapter::dispatch_task(
+  std::string task_id,
+  const nlohmann::json& request)
 {
-  _pimpl->worker.schedule([profile, fleets = _pimpl->fleets](const auto&)
+  _pimpl->worker.schedule(
+    [
+      request,
+      task_id = std::move(task_id),
+      fleets = _pimpl->fleets
+    ](const auto&)
     {
       for (auto& fleet : fleets)
       {
         auto& fimpl = FleetUpdateHandle::Implementation::get(*fleet);
-        if (!fimpl.legacy_accept_task)
-          continue;
 
-        // NOTE: althought the current adapter supports multiple fleets. The test
+        // NOTE: although the current adapter supports multiple fleets. The test
         // here assumses using a single fleet for each adapter
-        rmf_task_msgs::msg::BidNotice bid;
-        bid.task_profile = profile;
+        bool accepted = false;
+        auto bid = rmf_task_msgs::build<rmf_task_msgs::msg::BidNotice>()
+            .request(request)
+            .task_id(task_id)
+            .time_window(rclcpp::Duration(2, 0));
+
         fimpl.bid_notice_cb(
-          std::make_shared<rmf_task_msgs::msg::BidNotice>(bid));
-
-        rmf_task_msgs::msg::DispatchRequest req;
-        req.task_profile = profile;
-        req.fleet_name = fimpl.name;
-        req.method = req.ADD;
-        fimpl.dispatch_request_cb(
-          std::make_shared<rmf_task_msgs::msg::DispatchRequest>(req));
+          bid,
+          [&accepted](const rmf_task_ros2::bidding::Response& response)
+          {
+            accepted = response.proposal.has_value();
+          });
+
+        if (accepted)
+        {
+          rmf_task_msgs::msg::DispatchCommand req;
+          req.task_id = task_id;
+          req.fleet_name = fimpl.name;
+          req.type = req.TYPE_AWARD;
+          fimpl.dispatch_request_cb(
+            std::make_shared<rmf_task_msgs::msg::DispatchCommand>(req));
+        }
+        else
+        {
+          std::cout << "Fleet [" << fimpl.name << "] rejected the task request"
+                    << std::endl;
+        }
       }
     });
 }
diff --git a/rmf_fleet_adapter/test/tasks/test_Delivery.cpp b/rmf_fleet_adapter/test/tasks/test_Delivery.cpp
index 480aeb858..f459f1a54 100644
--- a/rmf_fleet_adapter/test/tasks/test_Delivery.cpp
+++ b/rmf_fleet_adapter/test/tasks/test_Delivery.cpp
@@ -446,21 +446,20 @@ SCENARIO("Test Delivery")
   // Note: wait for task_manager to start, else TM will be suspicously "empty"
   std::this_thread::sleep_for(1s);
 
-  // Dispatch Delivery LegacyTask
-  rmf_task_msgs::msg::TaskProfile task_profile;
-  task_profile.task_id = delivery_id;
-  task_profile.description.start_time = adapter.node()->now();
-  task_profile.description.task_type.type =
-    rmf_task_msgs::msg::TaskType::TYPE_DELIVERY;
-
-  rmf_task_msgs::msg::Delivery delivery;
-  delivery.pickup_place_name = pickup_name;
-  delivery.pickup_dispenser = quiet_dispenser_name;
-  delivery.dropoff_place_name = dropoff_name;
-  delivery.dropoff_ingestor = flaky_ingestor_name;
-
-  task_profile.description.delivery = delivery;
-  adapter.dispatch_task(task_profile);
+  // Dispatch Delivery Task
+  nlohmann::json request;
+  request["category"] = "delivery";
+  auto& desc = request["description"];
+  auto& pickup = desc["pickup"];
+  pickup["place"] = pickup_name;
+  pickup["handler"] = quiet_dispenser_name;
+  pickup["payload"] = std::vector<nlohmann::json>();
+  auto& dropoff = desc["dropoff"];
+  dropoff["place"] = dropoff_name;
+  dropoff["handler"] = flaky_ingestor_name;
+  dropoff["payload"] = std::vector<nlohmann::json>();
+
+  adapter.dispatch_task(delivery_id, request);
 
   const auto quiet_status = quiet_future.wait_for(15s);
   REQUIRE(quiet_status == std::future_status::ready);
diff --git a/rmf_fleet_adapter/test/tasks/test_Loop.cpp b/rmf_fleet_adapter/test/tasks/test_Loop.cpp
index 5c4d6198b..f7df03fa4 100644
--- a/rmf_fleet_adapter/test/tasks/test_Loop.cpp
+++ b/rmf_fleet_adapter/test/tasks/test_Loop.cpp
@@ -301,19 +301,21 @@ SCENARIO("Test loop requests", "[.flaky]")
   task_profile.description.task_type.type =
     rmf_task_msgs::msg::TaskType::TYPE_LOOP;
 
-  // Dsipatch Loop 0 LegacyTask
-  task_profile.task_id = loop_0;
-  task_profile.description.loop.num_loops = n_loops;
-  task_profile.description.loop.robot_type = fleet_type;
-  task_profile.description.loop.start_name = south;
-  task_profile.description.loop.finish_name = east;
-  adapter.dispatch_task(task_profile);
-
-  // Dispatch Loop 1 LegacyTask
-  task_profile.task_id = loop_1;
-  task_profile.description.loop.start_name = north;
-  task_profile.description.loop.finish_name = east;
-  adapter.dispatch_task(task_profile);
+  // Dsipatch Loop 0 Task
+  nlohmann::json request;
+  request["category"] = "patrol";
+  auto& desc = request["description"];
+  auto& places = desc["places"];
+  places.push_back(south);
+  places.push_back(east);
+  desc["rounds"] = n_loops;
+  adapter.dispatch_task(loop_0, request);
+
+  // Dispatch Loop 1 Task
+  places.clear();
+  places.push_back(north);
+  places.push_back(east);
+  adapter.dispatch_task(loop_1, request);
 
   const auto task_0_completed_status = task_0_completed_future.wait_for(20s);
   CHECK(task_0_completed_status == std::future_status::ready);
diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
index d07f8b9dc..30c738537 100644
--- a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
+++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
@@ -832,7 +832,7 @@ class Dispatcher::Implementation
 
   void handle_dispatch_ack(const DispatchAckMsg& ack)
   {
-    const auto command_it = lingering_commands.find(ack.request_id);
+    const auto command_it = lingering_commands.find(ack.dispatch_id);
     if (command_it == lingering_commands.end())
     {
       // Already processed this acknowledgment

From 765ed0ddc1f0d5d62563761fbe359ca9b1a47bfb Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Mon, 10 Jan 2022 01:22:14 +0800
Subject: [PATCH 46/79] Fixing subtle bugs

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 .../schemas/task_description_Delivery.json    |  1 +
 .../schemas/task_description_Patrol.json      |  1 +
 .../src/rmf_fleet_adapter/TaskManager.cpp     | 11 +++-
 .../agv/FleetUpdateHandle.cpp                 | 57 +++++++++++++------
 .../agv/internal_FleetUpdateHandle.hpp        | 35 ++++++------
 .../agv/test/MockAdapter.cpp                  |  2 +-
 .../src/rmf_fleet_adapter/tasks/Delivery.cpp  | 14 +++--
 .../src/rmf_fleet_adapter/tasks/Loop.cpp      |  7 ++-
 .../rmf_task_ros2/bidding/Auctioneer.hpp      |  5 +-
 .../src/rmf_task_ros2/Dispatcher.cpp          | 50 +++++++---------
 .../src/rmf_task_ros2/bidding/Auctioneer.cpp  | 39 ++++++++-----
 .../bidding/internal_Auctioneer.hpp           |  4 +-
 rmf_task_ros2/test/bidding/test_SelfBid.cpp   |  4 +-
 .../schedule/MirrorManager.cpp                | 12 ++--
 14 files changed, 140 insertions(+), 102 deletions(-)

diff --git a/rmf_fleet_adapter/schemas/task_description_Delivery.json b/rmf_fleet_adapter/schemas/task_description_Delivery.json
index 3fc00a404..c2710af12 100644
--- a/rmf_fleet_adapter/schemas/task_description_Delivery.json
+++ b/rmf_fleet_adapter/schemas/task_description_Delivery.json
@@ -3,6 +3,7 @@
   "$id": "https://raw.githubusercontent.com/open-rmf/rmf_ros2/main/rmf_fleet_adapter/schemas/task_description_Delivery.json",
   "title": "Delivery Task Description",
   "description": "Description for a simple delivery task category",
+  "type": "object",
   "properties": {
     "pickup": { "$ref": "event_description_PickUp.json" },
     "dropoff": { "$ref": "event_description_DropOff.json" }
diff --git a/rmf_fleet_adapter/schemas/task_description_Patrol.json b/rmf_fleet_adapter/schemas/task_description_Patrol.json
index dab5f2acc..6bf738c68 100644
--- a/rmf_fleet_adapter/schemas/task_description_Patrol.json
+++ b/rmf_fleet_adapter/schemas/task_description_Patrol.json
@@ -3,6 +3,7 @@
   "$id": "https://raw.githubusercontent.com/open-rmf/rmf_ros2/main/rmf_fleet_adapter/schemas/task_description_Patrol.json",
   "title": "Patrol Task Description",
   "description": "Description of a patrolling task",
+  "type": "object",
   "properties": {
     "places": {
       "description": "A list of which places to patrol between",
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
index 0124c9730..cbd423677 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
@@ -55,6 +55,7 @@
 #include <rmf_api_msgs/schemas/skip_phase_response.hpp>
 #include <rmf_api_msgs/schemas/undo_skip_phase_request.hpp>
 #include <rmf_api_msgs/schemas/undo_skip_phase_response.hpp>
+#include <rmf_api_msgs/schemas/error.hpp>
 
 namespace rmf_fleet_adapter {
 
@@ -187,7 +188,8 @@ TaskManagerPtr TaskManager::make(
     rmf_api_msgs::schemas::skip_phase_request,
     rmf_api_msgs::schemas::skip_phase_response,
     rmf_api_msgs::schemas::undo_skip_phase_request,
-    rmf_api_msgs::schemas::undo_skip_phase_response
+    rmf_api_msgs::schemas::undo_skip_phase_response,
+    rmf_api_msgs::schemas::error
   };
 
   for (const auto& schema : schemas)
@@ -328,6 +330,7 @@ nlohmann::json& copy_phase_data(
 
   auto& phase_logs = all_phase_logs[std::to_string(id)];
   auto& event_logs = phase_logs["events"];
+  event_logs = std::unordered_map<std::string, std::vector<nlohmann::json>>();
 
   while (!event_queue.empty())
   {
@@ -1250,7 +1253,8 @@ void TaskManager::_validate_and_publish_websocket(
   {
     RCLCPP_ERROR(
       _context->node()->get_logger(),
-      "[%s]",
+      "Failed to validate message [%s]: [%s]",
+      msg.dump().c_str(),
       error.c_str());
     return;
   }
@@ -1594,7 +1598,8 @@ namespace {
 //==============================================================================
 std::vector<std::string> get_labels(const nlohmann::json& request)
 {
-  if (const auto& labels = request["labels"])
+  const auto& labels = request["labels"];
+  if (!labels.is_null())
     return labels.get<std::vector<std::string>>();
 
   return {};
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
index a8229f956..586fa26f1 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
@@ -206,6 +206,12 @@ void FleetUpdateHandle::Implementation::bid_notice_cb(
   }
   catch (const std::exception& e)
   {
+    RCLCPP_ERROR(
+      node->get_logger(),
+      "Received a request with an invalid format. Error: %s\nRequest:\n%s",
+      e.what(),
+      request_msg.dump(2, ' ').c_str());
+
     return respond(
       {
         std::nullopt,
@@ -214,6 +220,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb(
   }
 
   const auto& category = request_msg["category"].get<std::string>();
+
   const auto task_deser_it = deserialization.task.handlers.find(category);
   if (task_deser_it == deserialization.task.handlers.end())
   {
@@ -236,6 +243,14 @@ void FleetUpdateHandle::Implementation::bid_notice_cb(
   }
   catch (const std::exception& e)
   {
+    RCLCPP_ERROR(
+      node->get_logger(),
+      "Received a request description for [%s] with an invalid format. "
+      "Error: %s\nRequest:\n%s",
+      category.c_str(),
+      e.what(),
+      description_msg.dump(2, ' ').c_str());
+
     return respond(
       rmf_task_ros2::bidding::Response{
         std::nullopt,
@@ -259,27 +274,28 @@ void FleetUpdateHandle::Implementation::bid_notice_cb(
 
   rmf_traffic::Time earliest_start_time =
     rmf_traffic::Time(rmf_traffic::Duration::min());
-  if (const auto& t = request_msg["unix_millis_earliest_start_time"])
+  const auto& t = request_msg["unix_millis_earliest_start_time"];
+  if (!t.is_null())
   {
     earliest_start_time =
       rmf_traffic::Time(std::chrono::milliseconds(t.get<uint64_t>()));
   }
 
   rmf_task::ConstPriorityPtr priority;
-  if (const auto& p = request_msg["priority"])
+  const auto& p = request_msg["priority"];
+  if (!p.is_null())
   {
-    if (const auto& p_type = p["type"])
+    const auto& p_type = p["type"];
+    if (p_type.is_string() && p_type.get<std::string>() == "binary")
     {
-      if (p_type.is_string() && p_type.get<std::string>() == "binary")
+      const auto& p_value = p["value"];
+      if (p_value.is_number_integer())
       {
-        if (const auto& p_value = p["value"])
-        {
-          if (p_value.is_number_integer() && p_value.get<uint64_t>() > 0)
-            priority = rmf_task::BinaryPriorityScheme::make_high_priority();
-        }
-
-        priority = rmf_task::BinaryPriorityScheme::make_low_priority();
+        if (p_value.is_number_integer() && p_value.get<uint64_t>() > 0)
+          priority = rmf_task::BinaryPriorityScheme::make_high_priority();
       }
+
+      priority = rmf_task::BinaryPriorityScheme::make_low_priority();
     }
 
     if (!priority)
@@ -403,7 +419,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb(
 }
 
 //==============================================================================
-void FleetUpdateHandle::Implementation::dispatch_request_cb(
+void FleetUpdateHandle::Implementation::dispatch_command_cb(
   const DispatchCmdMsg::SharedPtr msg)
 {
   const auto& task_id = msg->task_id;
@@ -468,7 +484,7 @@ void FleetUpdateHandle::Implementation::dispatch_request_cb(
 
     RCLCPP_INFO(
       node->get_logger(),
-      "Bid for task_id:[%s] awarded to fleet [%s]. Processing request...",
+      "Bid for task_id [%s] awarded to fleet [%s]. Processing request...",
       task_id.c_str(),
       name.c_str());
 
@@ -565,7 +581,7 @@ void FleetUpdateHandle::Implementation::dispatch_request_cb(
 
     RCLCPP_INFO(
       node->get_logger(),
-      "Assignments updated for robots in fleet [%s] to accommodate task_id:[%s]",
+      "Assignments updated for robots in fleet [%s] to accommodate task_id [%s]",
       name.c_str(), task_id.c_str());
   }
   else if (msg->type == DispatchCmdMsg::TYPE_REMOVE)
@@ -823,8 +839,9 @@ void FleetUpdateHandle::Implementation::update_fleet_state() const
     fleet_state_update_msg["data"] = fleet_state_msg;
     try
     {
-      // TODO(MXG): We should make this validator static
-      nlohmann::json_schema::json_validator validator(fleet_schema, loader);
+      static const nlohmann::json_schema::json_validator
+        validator(fleet_schema, loader);
+
       validator.validate(fleet_state_update_msg);
     }
     catch (const std::exception& e)
@@ -913,6 +930,9 @@ void FleetUpdateHandle::Implementation::add_standard_tasks()
   activation.phase = std::make_shared<rmf_task_sequence::Phase::Activator>();
   activation.event = std::make_shared<rmf_task_sequence::Event::Initializer>();
 
+  rmf_task_sequence::Task::add(
+    *activation.task, activation.phase, node->clock());
+
   rmf_task_sequence::phases::SimplePhase::add(
     *activation.phase, activation.event);
 
@@ -1049,7 +1069,7 @@ auto FleetUpdateHandle::Implementation::allocate_tasks(
   {
     RCLCPP_ERROR(
       node->get_logger(),
-      "[TaskPlanner] Failed to compute assignments for task_id:[%s]",
+      "[TaskPlanner] Failed to compute assignments for task_id [%s]",
       id.c_str());
 
     return std::nullopt;
@@ -1409,7 +1429,8 @@ FleetUpdateHandle& FleetUpdateHandle::accept_task_requests(
       rmf_dispenser_msgs::msg::DispenserRequestItem output;
       output.type_guid = item["sku"];
       output.quantity = item["quantity"];
-      if (const auto& compartment = item["compartment"])
+      const auto& compartment = item["compartment"];
+      if (!compartment.is_null())
         output.compartment_name = compartment.get<std::string>();
 
       return output;
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
index a3d9b65a0..4107bfac2 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
@@ -243,7 +243,7 @@ class FleetUpdateHandle::Implementation
 
   using DispatchCmdMsg = rmf_task_msgs::msg::DispatchCommand;
   using DispatchCommandSub = rclcpp::Subscription<DispatchCmdMsg>::SharedPtr;
-  DispatchCommandSub dispatch_request_sub = nullptr;
+  DispatchCommandSub dispatch_command_sub = nullptr;
 
   using DispatchAck = rmf_task_msgs::msg::DispatchAck;
   using DispatchAckPub = rclcpp::Publisher<DispatchAck>::SharedPtr;
@@ -280,15 +280,27 @@ class FleetUpdateHandle::Implementation
     handle->fleet_state_update_period(std::chrono::seconds(1));
 
     // Create subs and pubs for bidding
-    auto default_qos = rclcpp::SystemDefaultsQoS();
-    auto transient_qos = rclcpp::QoS(10);  transient_qos.transient_local();
+    auto transient_qos = rclcpp::QoS(10).transient_local();
+    auto reliable_transient_qos =
+      rclcpp::ServicesQoS().keep_last(20).transient_local();
+
+    // Subscribe DispatchCommand
+    handle->_pimpl->dispatch_command_sub =
+      handle->_pimpl->node->create_subscription<DispatchCmdMsg>(
+      DispatchRequestTopicName,
+      reliable_transient_qos,
+      [w = handle->weak_from_this()](const DispatchCmdMsg::SharedPtr msg)
+      {
+        if (const auto self = w.lock())
+          self->_pimpl->dispatch_command_cb(msg);
+      });
 
     // Publish DispatchAck
     handle->_pimpl->dispatch_ack_pub =
       handle->_pimpl->node->create_publisher<DispatchAck>(
-      DispatchAckTopicName, default_qos);
+      DispatchAckTopicName, reliable_transient_qos);
 
-    //
+    // Make a dispatch bidder
     handle->_pimpl->bidder = rmf_task_ros2::bidding::AsyncBidder::make(
       handle->_pimpl->node,
       [w = handle->weak_from_this()](
@@ -298,17 +310,6 @@ class FleetUpdateHandle::Implementation
           self->_pimpl->bid_notice_cb(msg, std::move(respond));
       });
 
-    // Subscribe DispatchRequest
-    handle->_pimpl->dispatch_request_sub =
-      handle->_pimpl->node->create_subscription<DispatchCmdMsg>(
-      DispatchRequestTopicName,
-      default_qos,
-      [w = handle->weak_from_this()](const DispatchCmdMsg::SharedPtr msg)
-      {
-        if (const auto self = w.lock())
-          self->_pimpl->dispatch_request_cb(msg);
-      });
-
     // Subscribe DockSummary
     handle->_pimpl->dock_summary_sub =
       handle->_pimpl->node->create_subscription<DockSummary>(
@@ -359,7 +360,7 @@ class FleetUpdateHandle::Implementation
     const BidNoticeMsg& msg,
     rmf_task_ros2::bidding::AsyncBidder::Respond respond);
 
-  void dispatch_request_cb(const DispatchCmdMsg::SharedPtr msg);
+  void dispatch_command_cb(const DispatchCmdMsg::SharedPtr msg);
 
   std::optional<std::size_t> get_nearest_charger(
     const rmf_traffic::agv::Planner::Start& start);
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp
index d4b13808f..1950b817e 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp
@@ -343,7 +343,7 @@ void MockAdapter::dispatch_task(
           req.task_id = task_id;
           req.fleet_name = fimpl.name;
           req.type = req.TYPE_AWARD;
-          fimpl.dispatch_request_cb(
+          fimpl.dispatch_command_cb(
             std::make_shared<rmf_task_msgs::msg::DispatchCommand>(req));
         }
         else
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp
index 2dfe157b8..e3697f65f 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp
@@ -299,7 +299,8 @@ make_deserializer(
     {
       std::string compartment = "";
 
-      if (const auto& compartment_json = msg["compartment"])
+      const auto& compartment_json = msg["compartment"];
+      if (!compartment_json.is_null())
         compartment = compartment_json.get<std::string>();
 
       return rmf_task::Payload::Component(
@@ -349,7 +350,8 @@ make_deserializer(
       }
 
       std::string handler;
-      if (const auto& handler_json = msg["handler"])
+      const auto& handler_json = msg["handler"];
+      if (!handler_json.is_null())
       {
         handler = handler_json.get<std::string>();
       }
@@ -384,10 +386,13 @@ void add_delivery(
   using Phase = rmf_task_sequence::phases::SimplePhase;
   using DeliveryDescription = rmf_task::requests::Delivery::Description;
 
+  deserialization.add_schema(schemas::event_description_PayloadTransfer);
+  deserialization.add_schema(schemas::task_description_Delivery);
+  deserialization.add_schema(schemas::event_description_PickUp);
+  deserialization.add_schema(schemas::event_description_DropOff);
   auto validate_payload_transfer =
     deserialization.make_validator_shared(
       schemas::event_description_PayloadTransfer);
-  deserialization.add_schema(schemas::event_description_PayloadTransfer);
 
   deserialization.consider_pickup =
     std::make_shared<agv::FleetUpdateHandle::ConsiderRequest>();
@@ -407,9 +412,6 @@ void add_delivery(
 
   auto validate_delivery =
     deserialization.make_validator_shared(schemas::task_description_Delivery);
-  deserialization.add_schema(schemas::task_description_Delivery);
-  deserialization.add_schema(schemas::event_description_PickUp);
-  deserialization.add_schema(schemas::event_description_DropOff);
 
   auto deserialize_delivery =
     [deserialize_pickup, deserialize_dropoff](
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp
index de73e67d0..536467b7c 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp
@@ -126,9 +126,10 @@ void add_loop(
   using Phase = rmf_task_sequence::phases::SimplePhase;
   using GoToPlace = rmf_task_sequence::events::GoToPlace;
 
+  deserialization.add_schema(schemas::task_description_Patrol);
+  deserialization.add_schema(schemas::event_description_GoToPlace);
   auto validate_go_to_place =
     deserialization.make_validator_shared(schemas::event_description_GoToPlace);
-  deserialization.add_schema(schemas::event_description_GoToPlace);
 
   auto deserialize_go_to_place =
     [place_deser = deserialization.place](const nlohmann::json& msg)
@@ -149,7 +150,6 @@ void add_loop(
 
   auto validate_patrol =
     deserialization.make_validator_shared(schemas::task_description_Patrol);
-  deserialization.add_schema(schemas::task_description_Patrol);
 
   deserialization.consider_patrol =
     std::make_shared<agv::FleetUpdateHandle::ConsiderRequest>();
@@ -195,7 +195,8 @@ void add_loop(
         return {nullptr, errors};
 
       std::size_t rounds = 1;
-      if (const auto& rounds_json = msg["rounds"])
+      const auto& rounds_json = msg["rounds"];
+      if (!rounds_json.is_null())
         rounds = rounds_json.get<std::size_t>();
 
       rmf_task_sequence::Task::Builder builder;
diff --git a/rmf_task_ros2/include/rmf_task_ros2/bidding/Auctioneer.hpp b/rmf_task_ros2/include/rmf_task_ros2/bidding/Auctioneer.hpp
index b6fd1a96b..fb21adf69 100644
--- a/rmf_task_ros2/include/rmf_task_ros2/bidding/Auctioneer.hpp
+++ b/rmf_task_ros2/include/rmf_task_ros2/bidding/Auctioneer.hpp
@@ -85,7 +85,10 @@ class Auctioneer : public std::enable_shared_from_this<Auctioneer>
   ///
   /// \param[in] bid_notice
   ///   bidding task, task which will call for bid
-  void start_bidding(const BidNoticeMsg& bid_notice);
+  void request_bid(const BidNoticeMsg& bid_notice);
+
+  /// Call this to tell the auctioneer that it may begin to perform the next bid
+  void ready_for_next_bid();
 
   /// Provide a custom evaluator which will be used to choose the best bid
   /// If no selection is given, Default is: LeastFleetDiffCostEvaluator
diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
index 30c738537..b203f9b37 100644
--- a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
+++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
@@ -161,8 +161,6 @@ class Dispatcher::Implementation
 
   DispatchStateCallback on_change_fn;
 
-  std::queue<bidding::BidNoticeMsg> task_bid_queue;
-
   DispatchStates active_dispatch_states;
   DispatchStates finished_dispatch_states;
   std::size_t task_counter = 0; // index for generating task_id
@@ -177,7 +175,8 @@ class Dispatcher::Implementation
       {4, "patrol"}
     };
 
-  using LegacyConversion = std::function<std::string(const TaskDescription&)>;
+  using LegacyConversion =
+    std::function<nlohmann::json(const TaskDescription&)>;
   using LegacyConversionMap = std::unordered_map<std::string, LegacyConversion>;
   LegacyConversionMap legacy_task_types;
 
@@ -313,7 +312,7 @@ class Dispatcher::Implementation
 
     // Loop
     legacy_task_types["patrol"] =
-      [](const TaskDescription& task_description) -> std::string
+      [](const TaskDescription& task_description) -> nlohmann::json
       {
         const auto& loop = task_description.loop;
         nlohmann::json description;
@@ -323,12 +322,12 @@ class Dispatcher::Implementation
         description["places"] = std::move(places);
         description["rounds"] = loop.num_loops;
 
-        return description.dump();
+        return description;
       };
 
     // Delivery
     legacy_task_types["delivery"] =
-      [](const TaskDescription& task_description) -> std::string
+      [](const TaskDescription& task_description) -> nlohmann::json
       {
         const auto& delivery = task_description.delivery;
         std::vector<nlohmann::json> payload;
@@ -356,18 +355,18 @@ class Dispatcher::Implementation
         description["pickup"] = pickup;
         description["dropoff"] = dropoff;
 
-        return description.dump();
+        return description;
       };
 
     // Clean
     legacy_task_types["clean"] =
-      [](const TaskDescription& task_description) -> std::string
+      [](const TaskDescription& task_description) -> nlohmann::json
       {
         const auto& clean = task_description.clean;
         nlohmann::json description;
         description["zone"] = clean.start_waypoint;
 
-        return description.dump();
+        return description;
       };
   }
 
@@ -419,8 +418,11 @@ class Dispatcher::Implementation
         api_response->publish(response);
       }
 
-      const std::string task_id = "dispatch#" + std::to_string(task_counter++);
       const auto task_request_json = msg_json["request"];
+      const std::string task_id =
+        task_request_json["category"].get<std::string>()
+        + ":dispatch#" + std::to_string(task_counter++);
+
       push_bid_notice(
         rmf_task_msgs::build<bidding::BidNoticeMsg>()
           .request(task_request_json.dump())
@@ -473,7 +475,8 @@ class Dispatcher::Implementation
     const std::string category = desc_it->second;
 
     // auto generate a task_id for a given submitted task
-    const auto task_id = "dispatch#" + std::to_string(task_counter++);
+    const auto task_id =
+      category + ":dispatch#" + std::to_string(task_counter++);
 
     RCLCPP_INFO(node->get_logger(),
       "Received Task Submission [%s]", task_id.c_str());
@@ -530,10 +533,7 @@ class Dispatcher::Implementation
     if (on_change_fn)
       on_change_fn(*new_dispatch_state);
 
-    task_bid_queue.push(bid_notice);
-
-    if (task_bid_queue.size() == 1)
-      auctioneer->start_bidding(task_bid_queue.front());
+    auctioneer->request_bid(bid_notice);
   }
 
   bool cancel_task(const TaskID& task_id)
@@ -695,19 +695,6 @@ class Dispatcher::Implementation
       if (on_change_fn)
         on_change_fn(*dispatch_state);
 
-      if (task_bid_queue.empty())
-      {
-        RCLCPP_ERROR(
-          node->get_logger(),
-          "task_bid_queue is empty while a bid is concluding. This indicates a "
-          "serious bug. Please report this to the RMF developers.");
-        return;
-      }
-
-      task_bid_queue.pop();
-      if (!task_bid_queue.empty())
-        auctioneer->start_bidding(task_bid_queue.front());
-
       return;
     }
 
@@ -717,6 +704,7 @@ class Dispatcher::Implementation
         winner->fleet_name,
         winner->expected_robot_name
       };
+    dispatch_state->status = DispatchState::Status::Selected;
 
     RCLCPP_INFO(
       node->get_logger(),
@@ -817,7 +805,7 @@ class Dispatcher::Implementation
       const auto& request = it->second;
       RCLCPP_ERROR(
         node->get_logger(),
-        "Dispatch request [%lu] type [%u] for task [%s] directed at fleet [%s] "
+        "Dispatch command [%lu] type [%u] for task [%s] directed at fleet [%s] "
         "has expired. This likely means something is wrong with the fleet "
         "adapter for [%s] preventing it from responding.",
         id,
@@ -826,6 +814,9 @@ class Dispatcher::Implementation
         request.fleet_name.c_str(),
         request.fleet_name.c_str());
 
+      if (request.type == request.TYPE_AWARD)
+        auctioneer->ready_for_next_bid();
+
       lingering_commands.erase(it);
     }
   }
@@ -879,6 +870,7 @@ class Dispatcher::Implementation
           static_cast<uint8_t>(state->status));
       }
 
+      auctioneer->ready_for_next_bid();
       return;
     }
     else if (command.type == DispatchCommandMsg::TYPE_REMOVE)
diff --git a/rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.cpp b/rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.cpp
index d7daef9d5..21f7860ce 100644
--- a/rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.cpp
+++ b/rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.cpp
@@ -21,16 +21,22 @@ namespace rmf_task_ros2 {
 namespace bidding {
 
 //==============================================================================
-Auctioneer::Implementation::Implementation(const std::shared_ptr<rclcpp::Node>& node_,
-  BiddingResultCallback result_callback, ConstEvaluatorPtr evaluator)
+Auctioneer::Implementation::Implementation(
+  const std::shared_ptr<rclcpp::Node>& node_,
+  BiddingResultCallback result_callback,
+  ConstEvaluatorPtr evaluator_)
 : node{std::move(node_)},
-  bidding_result_callback{std::move(result_callback)}
+  bidding_result_callback{std::move(result_callback)},
+  evaluator(std::move(evaluator_))
 {
   // default evaluator
-  evaluator = std::make_shared<QuickestFinishEvaluator>();
-  RCLCPP_INFO(
-    node->get_logger(),
-    "Dispatcher evaluator set to QuickestFinishEvaluator");
+  if (!evaluator)
+  {
+    evaluator = std::make_shared<QuickestFinishEvaluator>();
+    RCLCPP_INFO(
+      node->get_logger(),
+      "Dispatcher evaluator set to QuickestFinishEvaluator by default");
+  }
   const auto dispatch_qos = rclcpp::ServicesQoS().reliable();
 
   bid_notice_pub = node->create_publisher<BidNoticeMsg>(
@@ -50,7 +56,7 @@ Auctioneer::Implementation::Implementation(const std::shared_ptr<rclcpp::Node>&
 }
 
 //==============================================================================
-void Auctioneer::Implementation::start_bidding(
+void Auctioneer::Implementation::request_bid(
   const BidNoticeMsg& bid_notice)
 {
   RCLCPP_INFO(node->get_logger(), "Add Task [%s] to a bidding queue",
@@ -95,21 +101,20 @@ void Auctioneer::Implementation::finish_bidding_process()
   // Executing the task at the front queue
   auto front_task = open_bid_queue.front();
 
-  if (bidding_in_proccess)
+  if (bidding_in_process)
   {
     if (determine_winner(front_task))
     {
       open_bid_queue.pop();
-      bidding_in_proccess = false;
     }
   }
   else
   {
-    RCLCPP_DEBUG(node->get_logger(), " - Start new bidding task: %s",
+    RCLCPP_INFO(node->get_logger(), " - Start new bidding task: %s",
       front_task.bid_notice.task_id.c_str());
     open_bid_queue.front().start_time = node->now();
     bid_notice_pub->publish(front_task.bid_notice);
-    bidding_in_proccess = true;
+    bidding_in_process = true;
   }
 }
 
@@ -198,9 +203,15 @@ std::shared_ptr<Auctioneer> Auctioneer::make(
 }
 
 //==============================================================================
-void Auctioneer::start_bidding(const BidNoticeMsg& bid_notice)
+void Auctioneer::request_bid(const BidNoticeMsg& bid_notice)
+{
+  _pimpl->request_bid(bid_notice);
+}
+
+//==============================================================================
+void Auctioneer::ready_for_next_bid()
 {
-  _pimpl->start_bidding(bid_notice);
+  _pimpl->bidding_in_process = false;
 }
 
 //==============================================================================
diff --git a/rmf_task_ros2/src/rmf_task_ros2/bidding/internal_Auctioneer.hpp b/rmf_task_ros2/src/rmf_task_ros2/bidding/internal_Auctioneer.hpp
index c84cd9d02..a30848f6d 100644
--- a/rmf_task_ros2/src/rmf_task_ros2/bidding/internal_Auctioneer.hpp
+++ b/rmf_task_ros2/src/rmf_task_ros2/bidding/internal_Auctioneer.hpp
@@ -46,7 +46,7 @@ class Auctioneer::Implementation
     std::vector<bidding::Response> responses;
   };
 
-  bool bidding_in_proccess = false;
+  bool bidding_in_process = false;
   std::queue<OpenBid> open_bid_queue;
 
   using BidNoticePub = rclcpp::Publisher<BidNoticeMsg>;
@@ -61,7 +61,7 @@ class Auctioneer::Implementation
     ConstEvaluatorPtr evaluator);
 
   /// Start a bidding process
-  void start_bidding(const BidNoticeMsg& bid_notice);
+  void request_bid(const BidNoticeMsg& bid_notice);
 
   // Receive proposal and evaluate
   void receive_response(const BidResponseMsg& msg);
diff --git a/rmf_task_ros2/test/bidding/test_SelfBid.cpp b/rmf_task_ros2/test/bidding/test_SelfBid.cpp
index a295c2f0f..0eed54f06 100644
--- a/rmf_task_ros2/test/bidding/test_SelfBid.cpp
+++ b/rmf_task_ros2/test/bidding/test_SelfBid.cpp
@@ -131,7 +131,7 @@ SCENARIO("Auction with 2 Bids", "[TwoBids]")
 
   WHEN("First 'patrol' Task Bid")
   {
-    auctioneer->start_bidding(bidding_task1);
+    auctioneer->request_bid(bidding_task1);
 
     executor.spin_until_future_complete(ready_future,
       rmf_traffic::time::from_seconds(1.0));
@@ -152,7 +152,7 @@ SCENARIO("Auction with 2 Bids", "[TwoBids]")
   WHEN("Second 'delivery' Task bid")
   {
     // start bidding
-    auctioneer->start_bidding(bidding_task2);
+    auctioneer->request_bid(bidding_task2);
 
     executor.spin_until_future_complete(ready_future,
       rmf_traffic::time::from_seconds(1.0));
diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/MirrorManager.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/MirrorManager.cpp
index c953d0239..e52454890 100644
--- a/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/MirrorManager.cpp
+++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/MirrorManager.cpp
@@ -408,12 +408,12 @@ class MirrorManager::Implementation
     {
       request.version = minimum_version.value();
       request.full_update = false;
-      RCLCPP_INFO(
-        node->get_logger(),
-        "[rmf_traffic_ros2::MirrorManager::request_update] Requesting changes "
-        "for query ID [%ld] since version [%ld]",
-        request.query_id,
-        request.version);
+//      RCLCPP_INFO(
+//        node->get_logger(),
+//        "[rmf_traffic_ros2::MirrorManager::request_update] Requesting changes "
+//        "for query ID [%ld] since version [%ld]",
+//        request.query_id,
+//        request.version);
     }
     else
     {

From 1bb3bed7c0c84c989ccf1634672482c727129be8 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Mon, 10 Jan 2022 14:15:15 +0800
Subject: [PATCH 47/79] Implement support for composed tasks

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 .../agv/FleetUpdateHandle.hpp                 |  14 +
 .../schemas/event_description_Sequence.json   |  43 +++
 .../schemas/task_description_Compose.json     |  51 ++++
 .../src/rmf_fleet_adapter/DeserializeJSON.hpp |   3 +
 .../agv/FleetUpdateHandle.cpp                 |  19 +-
 .../agv/internal_FleetUpdateHandle.hpp        |   7 +-
 .../src/rmf_fleet_adapter/tasks/Clean.cpp     |   4 +-
 .../src/rmf_fleet_adapter/tasks/Compose.cpp   | 251 ++++++++++++++++++
 .../src/rmf_fleet_adapter/tasks/Compose.hpp   |  35 +++
 .../src/rmf_fleet_adapter/tasks/Delivery.cpp  |   6 +-
 .../src/rmf_fleet_adapter/tasks/Loop.cpp      |   8 +-
 11 files changed, 428 insertions(+), 13 deletions(-)
 create mode 100644 rmf_fleet_adapter/schemas/event_description_Sequence.json
 create mode 100644 rmf_fleet_adapter/schemas/task_description_Compose.json
 create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Compose.cpp
 create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Compose.hpp

diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp
index a36a23289..f4577aa04 100644
--- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp
@@ -171,6 +171,20 @@ class FleetUpdateHandle : public std::enable_shared_from_this<FleetUpdateHandle>
   ///   robots before triggering this callback.
   FleetUpdateHandle& consider_patrol_requests(ConsiderRequest consider);
 
+  /// Allow this fleet adapter to consider composed requests.
+  ///
+  /// Pass in a nullptr to disable composed requests.
+  ///
+  /// By default, composed requests are always accepted, as long as the events
+  /// that they are composed of are accepted.
+  ///
+  /// \param[in] consider
+  ///   Decide whether to accept a composed request. The description will
+  ///   satisfy the task_description_Compose.json schema of rmf_fleet_adapter.
+  ///   The FleetUpdateHandle will ensure that the request is feasible for the
+  ///   robots before triggering this callback.
+  FleetUpdateHandle& consider_composed_requests(ConsiderRequest consider);
+
   template<typename T>
   struct DeserializedDescription
   {
diff --git a/rmf_fleet_adapter/schemas/event_description_Sequence.json b/rmf_fleet_adapter/schemas/event_description_Sequence.json
new file mode 100644
index 000000000..496383468
--- /dev/null
+++ b/rmf_fleet_adapter/schemas/event_description_Sequence.json
@@ -0,0 +1,43 @@
+{
+  "$schema": "https://json-schema.org/draft/2020-12/schema",
+  "$id": "https://raw.githubusercontent.com/open-rmf/rmf_ros2/main/rmf_fleet_adapter/schemas/event_description_Sequence.json",
+  "title": "Activity Sequence",
+  "description": "A sequence of activities",
+  "oneOf": [
+    {
+      "$ref": "#/$defs/activity_array"
+    },
+    {
+      "type": "object",
+      "properties": {
+        "activities": { "$ref": "#/$defs/activity_array" },
+        "category": {
+          "description": "Customize the category display for this sequence",
+          "type": "string"
+        },
+        "detail": {
+          "description": "Customize the detail display for this sequence"
+        }
+      },
+      "required": ["activities"]
+    }
+  ],
+  "$defs": {
+    "activity_array": {
+      "type": "array",
+      "items": {
+        "type": "object",
+        "properties": {
+          "category": {
+            "description": "The category of the activity",
+            "type": "string"
+          },
+          "description": {
+            "description": "A description of the activity. This must match a schema supported by a fleet for the activity category."
+          }
+        }
+      },
+      "minimum": 1,
+    }
+  }
+}
diff --git a/rmf_fleet_adapter/schemas/task_description_Compose.json b/rmf_fleet_adapter/schemas/task_description_Compose.json
new file mode 100644
index 000000000..94a30d643
--- /dev/null
+++ b/rmf_fleet_adapter/schemas/task_description_Compose.json
@@ -0,0 +1,51 @@
+{
+  "$schema": "https://json-schema.org/draft/2020-12/schema",
+  "$id": "https://raw.githubusercontent.com/open-rmf/rmf_api_msgs/main/rmf_api_msgs/schemas/compose_task_description.json",
+  "title": "Compose Task Description",
+  "description": "Compose a task description based on activities that are supported by fleets. When supported by fleets, this can be used as a task description for task_request.",
+  "type": "object",
+  "properties": {
+    "category": {
+      "description": "Specify the category for this composed task, as the operators should see it.",
+      "type": "string"
+    },
+    "detail": {
+      "description": "Specify the detail for this composed task, as the operators should see it.",
+      "type": "string"
+    },
+    "phases": {
+      "description": "List the phases of the task in the order that they should be performed.",
+      "type": "array",
+      "items": {
+        "type": "object",
+        "properties": {
+          "activity": { "$ref": "#/$defs/activity" },
+          "on_cancel": {
+            "description": "A list of activities to perform if the task is canceled during this phase (each activity is given its own phase which can be skipped but not canceled).",
+            "type": "array",
+            "items": { "$ref": "#/$defs/activity" }
+          }
+        },
+        "required": ["activity"]
+      },
+      "minimum": 1
+    }
+  },
+  "required": ["phases"],
+  "$defs": {
+    "activity": {
+      "description": "Describe the activity of the phase. This must match an activity description supported by one of the fleets. If there should be multiple activities within a phase, then use a Sequence activity.",
+      "type": "object",
+      "properties": {
+        "category": {
+          "description": "The category of this phase's activity.",
+          "type": "string"
+        },
+        "description": {
+          "description": "A description of the activity. This must match a schema supported by a fleet for the category of this activity."
+        }
+      },
+      "required": ["category", "description"]
+    }
+  }
+}
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/DeserializeJSON.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/DeserializeJSON.hpp
index 5f43037fd..9874d0aed 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/DeserializeJSON.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/DeserializeJSON.hpp
@@ -66,6 +66,9 @@ class DeserializeJSON
 
 };
 
+template<typename T>
+using DeserializeJSONPtr = std::shared_ptr<DeserializeJSON<T>>;
+
 } // namespace rmf_fleet_adapter
 
 #endif // SRC__RMF_FLEET_ADAPTER__DESERIALIZEJSON_HPP
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
index 586fa26f1..2b5aab0c5 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
@@ -29,6 +29,7 @@
 #include "../tasks/Loop.hpp"
 #include "../tasks/Clean.hpp"
 #include "../tasks/ChargeBattery.hpp"
+#include "../tasks/Compose.hpp"
 #include "../events/GoToPlace.hpp"
 
 #include <rmf_task/Constraints.hpp>
@@ -110,6 +111,15 @@ TaskDeserialization::make_validator_shared(nlohmann::json schema) const
 //==============================================================================
 TaskDeserialization::TaskDeserialization()
 {
+  task =
+    std::make_shared<DeserializeJSON<FleetUpdateHandle::DeserializedTask>>();
+
+  phase =
+    std::make_shared<DeserializeJSON<FleetUpdateHandle::DeserializedPhase>>();
+
+  event =
+    std::make_shared<DeserializeJSON<FleetUpdateHandle::DeserializedEvent>>();
+
   _schema_dictionary = std::make_shared<SchemaDictionary>();
   _loader = [dict = _schema_dictionary](
       const nlohmann::json_uri& id,
@@ -221,8 +231,8 @@ void FleetUpdateHandle::Implementation::bid_notice_cb(
 
   const auto& category = request_msg["category"].get<std::string>();
 
-  const auto task_deser_it = deserialization.task.handlers.find(category);
-  if (task_deser_it == deserialization.task.handlers.end())
+  const auto task_deser_it = deserialization.task->handlers.find(category);
+  if (task_deser_it == deserialization.task->handlers.end())
   {
     return respond(
       {
@@ -962,6 +972,11 @@ void FleetUpdateHandle::Implementation::add_standard_tasks()
     activation.phase,
     *activation.event,
     node->clock());
+
+  tasks::add_compose(
+    deserialization,
+    activation,
+    node->clock());
 }
 
 //==============================================================================
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
index 4107bfac2..6403cb84a 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
@@ -80,15 +80,16 @@ struct TaskActivation
 //==============================================================================
 struct TaskDeserialization
 {
-  DeserializeJSON<FleetUpdateHandle::DeserializedTask> task;
-  DeserializeJSON<FleetUpdateHandle::DeserializedPhase> phase;
-  DeserializeJSON<FleetUpdateHandle::DeserializedEvent> event;
+  DeserializeJSONPtr<FleetUpdateHandle::DeserializedTask> task;
+  DeserializeJSONPtr<FleetUpdateHandle::DeserializedPhase> phase;
+  DeserializeJSONPtr<FleetUpdateHandle::DeserializedEvent> event;
   FleetUpdateHandle::PlaceDeserializer place;
 
   std::shared_ptr<FleetUpdateHandle::ConsiderRequest> consider_pickup;
   std::shared_ptr<FleetUpdateHandle::ConsiderRequest> consider_dropoff;
   std::shared_ptr<FleetUpdateHandle::ConsiderRequest> consider_clean;
   std::shared_ptr<FleetUpdateHandle::ConsiderRequest> consider_patrol;
+  std::shared_ptr<FleetUpdateHandle::ConsiderRequest> consider_composed;
 
   void add_schema(const nlohmann::json& schema);
 
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp
index 5189ffb76..1e7819ea3 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp
@@ -351,7 +351,7 @@ void add_clean(
         confirm.errors()
       };
     };
-  deserialization.task.add("clean", validate_clean_task, deserialize_clean);
+  deserialization.task->add("clean", validate_clean_task, deserialize_clean);
 
   auto deserialize_clean_event =
     [deserialize_clean](const nlohmann::json& msg)
@@ -367,7 +367,7 @@ void add_clean(
           std::move(clean_task.errors)
       };
     };
-  deserialization.event.add(
+  deserialization.event->add(
     "clean", validate_clean_event, deserialize_clean_event);
 
   CleanEvent::add(*activation.event);
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Compose.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Compose.cpp
new file mode 100644
index 000000000..43365f568
--- /dev/null
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Compose.cpp
@@ -0,0 +1,251 @@
+/*
+ * Copyright (C) 2022 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 "Compose.hpp"
+
+#include <rmf_task_sequence/Task.hpp>
+#include <rmf_task_sequence/phases/SimplePhase.hpp>
+#include <rmf_task_sequence/events/Bundle.hpp>
+
+#include <rmf_fleet_adapter/schemas/task_description_Compose.hpp>
+#include <rmf_fleet_adapter/schemas/event_description_Sequence.hpp>
+
+namespace rmf_fleet_adapter {
+namespace tasks {
+
+//==============================================================================
+void add_compose(
+  agv::TaskDeserialization& deserialization,
+  agv::TaskActivation& activation,
+  std::function<rmf_traffic::Time()> clock)
+{
+  deserialization.add_schema(
+    rmf_fleet_adapter::schemas::task_description_Compose);
+  deserialization.add_schema(
+    rmf_fleet_adapter::schemas::event_description_Sequence);
+
+  auto validate_compose_task =
+    deserialization.make_validator_shared(schemas::task_description_Compose);
+
+  auto deserialize_activity =
+    [
+      deser_phase = deserialization.phase,
+      deser_event = deserialization.event
+    ](const nlohmann::json& msg) -> agv::FleetUpdateHandle::DeserializedPhase
+    {
+      const auto& category = msg["category"].get<std::string>();
+      const auto& description_json = msg["description"];
+
+      const auto p_it = deser_phase->handlers.find(category);
+      if (p_it != deser_phase->handlers.end())
+      {
+        // There is a specialized phase for this category, so we will
+        // deserialize and use that.
+        auto phase = p_it->second.deserializer(description_json);
+        if (!phase.description)
+          return {nullptr, std::move(phase.errors)};
+
+        return phase;
+      }
+
+      // There is no specialized phase for this category, so we will check for
+      // an event that has the same category name.
+      const auto e_it = deser_event->handlers.find(category);
+      if (e_it == deser_event->handlers.end())
+      {
+        // There is no specialized phase or event for this category, so we
+        // cannot support this task.
+        return {nullptr, {"No support for [" + category + "] activities"}};
+      }
+
+      auto event = e_it->second.deserializer(description_json);
+      if (!event.description)
+        return {nullptr, std::move(event.errors)};
+
+      using rmf_task_sequence::phases::SimplePhase;
+      return {
+        SimplePhase::Description::make(event.description),
+        std::move(event.errors)
+      };
+    };
+
+  deserialization.consider_composed =
+    std::make_shared<agv::FleetUpdateHandle::ConsiderRequest>();
+
+  // Accept composed tasks by default
+  *deserialization.consider_composed = [](const auto&, auto& confirm)
+    {
+      confirm.accept();
+    };
+
+  auto deserialize_compose_task =
+    [
+      deser_activity = deserialize_activity,
+      consider = deserialization.consider_composed
+    ](const nlohmann::json& msg)
+    -> agv::FleetUpdateHandle::DeserializedTask
+    {
+      if (!(*consider))
+        return {nullptr, {"Not accepting composed requests"}};
+
+      rmf_task_sequence::Task::Builder builder;
+      std::string category = "Composed Task";
+      const auto& category_json = msg["category"];
+      if (category_json.is_string())
+        category = category_json.get<std::string>();
+
+      std::string detail = "";
+      const auto& detail_json = msg["detail"];
+      if (detail_json.is_string())
+        detail = detail_json.get<std::string>();
+
+      std::vector<std::string> errors;
+      const auto& phases_json = msg["phases"];
+      for (const auto& phase_json : phases_json)
+      {
+        auto phase_activity = deser_activity(phase_json["activity"]);
+        if (!phase_activity.description)
+          return {nullptr, std::move(phase_activity.errors)};
+
+        errors.insert(
+          errors.end(),
+          phase_activity.errors.begin(), phase_activity.errors.end());
+
+        std::vector<rmf_task_sequence::Phase::ConstDescriptionPtr> cancel;
+        const auto& on_cancel_json = phase_json["on_cancel"];
+        if (on_cancel_json.is_array())
+        {
+          cancel.reserve(on_cancel_json.size());
+          for (const auto& cancellation_activity_json : on_cancel_json)
+          {
+            auto cancellation_activity =
+              deser_activity(cancellation_activity_json);
+
+            if (!cancellation_activity.description)
+              return {nullptr, std::move(cancellation_activity.errors)};
+
+            cancel.push_back(cancellation_activity.description);
+            errors.insert(
+              errors.end(),
+              cancellation_activity.errors.begin(),
+              cancellation_activity.errors.end());
+          }
+        }
+      }
+
+      agv::FleetUpdateHandle::Confirmation confirm;
+      (*consider)(msg, confirm);
+      if (!confirm.is_accepted())
+        return {nullptr, confirm.errors()};
+
+      return {
+        builder.build(category, detail),
+        std::move(errors)
+      };
+    };
+
+  using rmf_task_sequence::events::Bundle;
+  using DeserializedDependencies =
+    agv::FleetUpdateHandle::DeserializedDescription<
+      std::optional<Bundle::Description::Dependencies>>;
+
+  // Deserialize composed tasks
+  deserialization.task->add(
+    "compose", validate_compose_task, deserialize_compose_task);
+
+  // Activate composed tasks
+  rmf_task_sequence::Task::add(*activation.task, activation.phase, clock);
+
+  auto validate_event_sequence =
+    deserialization.make_validator_shared(schemas::event_description_Sequence);
+
+  auto deserialize_event_dependencies =
+    [deser_event = deserialization.event](const nlohmann::json& msg)
+      -> DeserializedDependencies
+    {
+      std::vector<std::string> errors;
+      Bundle::Description::Dependencies deps;
+      deps.reserve(msg.size());
+      for (const auto& activity_json : msg)
+      {
+        const auto& category = activity_json["category"].get<std::string>();
+        const auto& description_json = activity_json["description"];
+        const auto e_it = deser_event->handlers.find(category);
+        if (e_it == deser_event->handlers.end())
+        {
+          errors.push_back("No support for [" + category + "] activities");
+          return {std::nullopt, std::move(errors)};
+        }
+
+        auto event = e_it->second.deserializer(description_json);
+        errors.insert(errors.end(), event.errors.begin(), event.errors.end());
+        if (!event.description)
+          return {std::nullopt, std::move(errors)};
+
+        deps.push_back(event.description);
+      }
+
+      return {std::move(deps), std::move(errors)};
+    };
+
+  auto deserialize_event_sequence =
+    [deserialize_event_dependencies](const nlohmann::json& msg)
+      -> agv::FleetUpdateHandle::DeserializedEvent
+    {
+      DeserializedDependencies dependencies;
+      std::optional<std::string> category;
+      std::optional<std::string> detail;
+      if (msg.is_array())
+      {
+        dependencies = deserialize_event_dependencies(msg);
+      }
+      else
+      {
+        dependencies = deserialize_event_dependencies(msg["activities"]);
+
+        const auto& category_json = msg["category"];
+        if (category_json.is_string())
+          category = category_json.get<std::string>();
+
+        const auto& detail_json = msg["detail"];
+        if (detail_json.is_string())
+          detail = detail_json.get<std::string>();
+      }
+
+      if (!dependencies.description.has_value())
+        return {nullptr, std::move(dependencies.errors)};
+
+      return {
+        std::make_shared<Bundle::Description>(
+          *dependencies.description,
+          Bundle::Type::Sequence,
+          std::move(category),
+          std::move(detail)),
+        std::move(dependencies.errors)
+      };
+    };
+
+  // Deserialize activity sequences
+  deserialization.event->add(
+    "sequence", validate_event_sequence, deserialize_event_sequence);
+
+  // Activate activity sequences
+  rmf_task_sequence::events::Bundle::add(activation.event);
+}
+
+} // namespace tasks
+} // namespace rmf_fleet_adapter
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Compose.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Compose.hpp
new file mode 100644
index 000000000..068c8f58c
--- /dev/null
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Compose.hpp
@@ -0,0 +1,35 @@
+/*
+ * Copyright (C) 2022 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 SRC__RMF_FLEET_ADAPTER__TASKS__COMPOSE_HPP
+#define SRC__RMF_FLEET_ADAPTER__TASKS__COMPOSE_HPP
+
+#include "../agv/internal_FleetUpdateHandle.hpp"
+
+namespace rmf_fleet_adapter {
+namespace tasks {
+
+//==============================================================================
+void add_compose(
+  agv::TaskDeserialization& deserialization,
+  agv::TaskActivation& activation,
+  std::function<rmf_traffic::Time()> clock);
+
+} // namespace tasks
+} // namespace rmf_fleet_adapter
+
+#endif // SRC__RMF_FLEET_ADAPTER__TASKS__COMPOSE_HPP
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp
index e3697f65f..c02375f02 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp
@@ -399,7 +399,7 @@ void add_delivery(
   auto deserialize_pickup =
     make_deserializer<PickUp::Description>(
       deserialization.place, deserialization.consider_pickup);
-  deserialization.event.add(
+  deserialization.event->add(
     "pickup", validate_payload_transfer, deserialize_pickup);
 
   deserialization.consider_dropoff =
@@ -407,7 +407,7 @@ void add_delivery(
   auto deserialize_dropoff =
     make_deserializer<DropOff::Description>(
       deserialization.place, deserialization.consider_dropoff);
-  deserialization.event.add(
+  deserialization.event->add(
     "dropoff", validate_payload_transfer, deserialize_dropoff);
 
   auto validate_delivery =
@@ -438,7 +438,7 @@ void add_delivery(
       return {builder.build("Delivery", ""), std::move(errors)};
     };
 
-  deserialization.task.add("delivery", validate_delivery, deserialize_delivery);
+  deserialization.task->add("delivery", validate_delivery, deserialize_delivery);
 
   auto private_initializer =
     std::make_shared<rmf_task_sequence::Event::Initializer>();
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp
index 536467b7c..2de9629d2 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp
@@ -145,7 +145,7 @@ void add_loop(
       };
     };
 
-  deserialization.event.add(
+  deserialization.event->add(
     "go_to_place", validate_go_to_place, deserialize_go_to_place);
 
   auto validate_patrol =
@@ -153,7 +153,9 @@ void add_loop(
 
   deserialization.consider_patrol =
     std::make_shared<agv::FleetUpdateHandle::ConsiderRequest>();
-  *deserialization.consider_patrol = [](const auto&, auto confirm)
+
+  // Accept patrol tasks by default
+  *deserialization.consider_patrol = [](const auto&, auto& confirm)
     {
       confirm.accept();
     };
@@ -212,7 +214,7 @@ void add_loop(
       return {builder.build("Patrol", ""), std::move(errors)};
     };
 
-  deserialization.task.add("patrol", validate_patrol, deserialize_patrol);
+  deserialization.task->add("patrol", validate_patrol, deserialize_patrol);
 
   auto loop_unfolder =
     [](const Loop::Description& loop)

From acf6affc76370b4f2f2fbc66c3354ee4492dd231 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Mon, 10 Jan 2022 14:28:47 +0800
Subject: [PATCH 48/79] Removing support for adding custom tasks -- wait until
 we can work more on refining the API

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 .../agv/FleetUpdateHandle.hpp                 | 125 ------------------
 .../agv/FleetUpdateHandle.cpp                 |  22 +--
 .../agv/internal_FleetUpdateHandle.hpp        |  52 +++++++-
 .../src/rmf_fleet_adapter/tasks/Clean.cpp     |   7 +-
 .../src/rmf_fleet_adapter/tasks/Compose.cpp   |   8 +-
 .../src/rmf_fleet_adapter/tasks/Delivery.cpp  |   9 +-
 .../src/rmf_fleet_adapter/tasks/Loop.cpp      |   5 +-
 7 files changed, 72 insertions(+), 156 deletions(-)

diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp
index f4577aa04..e4cd19b28 100644
--- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp
@@ -185,131 +185,6 @@ class FleetUpdateHandle : public std::enable_shared_from_this<FleetUpdateHandle>
   ///   robots before triggering this callback.
   FleetUpdateHandle& consider_composed_requests(ConsiderRequest consider);
 
-  template<typename T>
-  struct DeserializedDescription
-  {
-    T description;
-    std::vector<std::string> errors;
-  };
-
-  using DeserializedTask =
-    DeserializedDescription<std::shared_ptr<const rmf_task::Task::Description>>;
-
-  using TaskDescriptionDeserializer =
-    std::function<DeserializedTask(const nlohmann::json&)>;
-
-  /// Add a type of task for this fleet to support.
-  ///
-  /// \param[in] category
-  ///   The category of the task that will make use of the resources provided to
-  ///   this function. If this function is called multiple times with the same
-  ///   category, later calls will overwrite earlier calls.
-  ///
-  /// \param[in] schema
-  ///   The schema that will be applied to validate incoming task request JSON
-  ///   messages of this category.
-  ///
-  /// \param[in] description_deserializer
-  ///   A function which takes in a JSON message and gives back a task
-  ///   description instance.
-  ///
-  /// \param[in] task_activator
-  ///   This lambda will receive the task activator that will be used for
-  ///   activating the task description. The lambda should add your task to this
-  ///   activator. The lambda will only be called one time.
-  void add_task_type(
-    const std::string& category,
-    nlohmann::json schema,
-    TaskDescriptionDeserializer description_deserializer,
-    std::function<void(rmf_task::Activator&)> task_activator);
-
-  using DeserializedPhase =
-    DeserializedDescription<
-      std::shared_ptr<const rmf_task_sequence::Phase::Description>
-    >;
-
-  using PhaseDescriptionDeserializer =
-    std::function<DeserializedPhase(const nlohmann::json&)>;
-
-  /// Add a type of phase for this fleet to support.
-  ///
-  /// \param[in] category
-  ///   The category of the phase that will make use of the resources provided
-  ///   to this function. If this function is called multiple times with the
-  ///   same category, later calls will overwrite earlier calls.
-  ///
-  /// \param[in] schema
-  ///   The schema that will be applied to validate incoming phase JSON
-  ///   messages of this category.
-  ///
-  /// \param[in] description_deserializer
-  ///   A function which takes in a JSON message and gives back a phase
-  ///   description instance.
-  ///
-  /// \param[in] phase_activator
-  ///   This lambda will receive the phase activator that will be used for
-  ///   activating the phase description. The lambda should add your phase to
-  ///   this activator. The lambda will only be called one time.
-  void add_phase_type(
-    const std::string& category,
-    nlohmann::json schema,
-    PhaseDescriptionDeserializer description_deserializer,
-    std::function<void(rmf_task_sequence::Phase::Activator&)> phase_activator);
-
-  /// Get the builtin phase activator that will be used by composed tasks to
-  /// activate phases. A custom task type may make use of this to activate
-  /// standard and custom phases.
-  rmf_task_sequence::Phase::ActivatorPtr builtin_phase_activator();
-
-  using DeserializedEvent =
-    DeserializedDescription<
-      std::shared_ptr<const rmf_task_sequence::Event::Description>
-    >;
-
-  using EventDescriptionDeserializer =
-    std::function<DeserializedEvent(const nlohmann::json&)>;
-
-  /// Add a type of activity for this fleet to support. This activity can be
-  /// incorporated into event bundles for composed tasks.
-  ///
-  /// \param[in] category
-  ///   The category of event that will make use of the resources provided to
-  ///   this function. If this function is called multiple times with the same
-  ///   category, later calls will overwrite earlier calls.
-  ///
-  /// \param[in] schema
-  ///   The schema that will be applied to validate incoming event JSON messages
-  ///   of this category.
-  ///
-  /// \param[in] description_deserializer
-  ///   A function which takes in a JSON message and gives back an event
-  ///   description instance.
-  ///
-  /// \param[in] event_initializer
-  ///   This lambda will receive the event initializer that will be used for
-  ///   initializing the event description. The lambda should add your event to
-  ///   this initializer. Your own event may also use this initializer to
-  ///   initialize its own event dependencies, but it must hold a std::weak_ptr
-  ///   to avoid a circular reference.
-  void add_activity_type(
-    const std::string& category,
-    nlohmann::json schema,
-    EventDescriptionDeserializer description_deserializer,
-    std::function<void(const rmf_task_sequence::Event::InitializerPtr&)> event_initializer);
-
-  rmf_task_sequence::Event::InitializerPtr builtin_event_initializer();
-
-  using DeserializedPlace =
-    agv::FleetUpdateHandle::DeserializedDescription<
-      std::optional<rmf_traffic::agv::Plan::Goal>
-    >;
-
-  using PlaceDeserializer =
-    std::function<DeserializedPlace(const nlohmann::json&)>;
-
-  /// Get the "place" deserializer for the nav graph of this fleet.
-  PlaceDeserializer builtin_place_deserializer() const;
-
   /// Specify a set of lanes that should be closed.
   void close_lanes(std::vector<std::size_t> lane_indices);
 
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
index 2b5aab0c5..7e45078c1 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
@@ -111,14 +111,9 @@ TaskDeserialization::make_validator_shared(nlohmann::json schema) const
 //==============================================================================
 TaskDeserialization::TaskDeserialization()
 {
-  task =
-    std::make_shared<DeserializeJSON<FleetUpdateHandle::DeserializedTask>>();
-
-  phase =
-    std::make_shared<DeserializeJSON<FleetUpdateHandle::DeserializedPhase>>();
-
-  event =
-    std::make_shared<DeserializeJSON<FleetUpdateHandle::DeserializedEvent>>();
+  task = std::make_shared<DeserializeJSON<DeserializedTask>>();
+  phase = std::make_shared<DeserializeJSON<DeserializedPhase>>();
+  event = std::make_shared<DeserializeJSON<DeserializedEvent>>();
 
   _schema_dictionary = std::make_shared<SchemaDictionary>();
   _loader = [dict = _schema_dictionary](
@@ -868,12 +863,12 @@ void FleetUpdateHandle::Implementation::update_fleet_state() const
 }
 
 namespace {
-FleetUpdateHandle::PlaceDeserializer make_place_deserializer(
+PlaceDeserializer make_place_deserializer(
   std::shared_ptr<const std::shared_ptr<const rmf_traffic::agv::Planner>>
     planner)
 {
   return [planner = std::move(planner)](const nlohmann::json& msg)
-      -> agv::FleetUpdateHandle::DeserializedPlace
+      -> agv::DeserializedPlace
     {
       std::optional<rmf_traffic::agv::Plan::Goal> place;
       const auto& graph = (*planner)->get_configuration().graph();
@@ -1336,6 +1331,13 @@ FleetUpdateHandle& FleetUpdateHandle::consider_patrol_requests(
   return *this;
 }
 
+//==============================================================================
+FleetUpdateHandle& FleetUpdateHandle::consider_composed_requests(
+  ConsiderRequest consider)
+{
+  *_pimpl->deserialization.consider_composed = std::move(consider);
+}
+
 //==============================================================================
 void FleetUpdateHandle::close_lanes(std::vector<std::size_t> lane_indices)
 {
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
index 6403cb84a..74be3dc03 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
@@ -77,13 +77,57 @@ struct TaskActivation
   rmf_task_sequence::Event::InitializerPtr event;
 };
 
+//==============================================================================
+template<typename T>
+struct DeserializedDescription
+{
+  T description;
+  std::vector<std::string> errors;
+};
+
+//==============================================================================
+using DeserializedTask =
+  DeserializedDescription<std::shared_ptr<const rmf_task::Task::Description>>;
+
+//==============================================================================
+using TaskDescriptionDeserializer =
+  std::function<DeserializedTask(const nlohmann::json&)>;
+
+//==============================================================================
+using DeserializedPhase =
+  DeserializedDescription<
+    std::shared_ptr<const rmf_task_sequence::Phase::Description>
+  >;
+
+//==============================================================================
+using PhaseDescriptionDeserializer =
+  std::function<DeserializedPhase(const nlohmann::json&)>;
+
+//==============================================================================
+using DeserializedEvent =
+  DeserializedDescription<
+    std::shared_ptr<const rmf_task_sequence::Event::Description>
+  >;
+
+//==============================================================================
+using EventDescriptionDeserializer =
+  std::function<DeserializedEvent(const nlohmann::json&)>;
+
+//==============================================================================
+using DeserializedPlace =
+  DeserializedDescription<std::optional<rmf_traffic::agv::Plan::Goal>>;
+
+//==============================================================================
+using PlaceDeserializer =
+  std::function<DeserializedPlace(const nlohmann::json&)>;
+
 //==============================================================================
 struct TaskDeserialization
 {
-  DeserializeJSONPtr<FleetUpdateHandle::DeserializedTask> task;
-  DeserializeJSONPtr<FleetUpdateHandle::DeserializedPhase> phase;
-  DeserializeJSONPtr<FleetUpdateHandle::DeserializedEvent> event;
-  FleetUpdateHandle::PlaceDeserializer place;
+  DeserializeJSONPtr<DeserializedTask> task;
+  DeserializeJSONPtr<DeserializedPhase> phase;
+  DeserializeJSONPtr<DeserializedEvent> event;
+  PlaceDeserializer place;
 
   std::shared_ptr<FleetUpdateHandle::ConsiderRequest> consider_pickup;
   std::shared_ptr<FleetUpdateHandle::ConsiderRequest> consider_dropoff;
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp
index 1e7819ea3..93a0367b1 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp
@@ -283,9 +283,7 @@ void add_clean(
       traits,
       place_deser = deserialization.place,
       consider = deserialization.consider_clean
-    ](
-      const nlohmann::json& msg)
-      -> agv::FleetUpdateHandle::DeserializedTask
+    ](const nlohmann::json& msg) -> agv::DeserializedTask
     {
       if (!consider || !(*consider))
       {
@@ -354,8 +352,7 @@ void add_clean(
   deserialization.task->add("clean", validate_clean_task, deserialize_clean);
 
   auto deserialize_clean_event =
-    [deserialize_clean](const nlohmann::json& msg)
-      -> agv::FleetUpdateHandle::DeserializedEvent
+    [deserialize_clean](const nlohmann::json& msg) -> agv::DeserializedEvent
     {
       auto clean_task = deserialize_clean(msg);
       if (!clean_task.description)
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Compose.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Compose.cpp
index 43365f568..8f5b09932 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Compose.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Compose.cpp
@@ -45,7 +45,7 @@ void add_compose(
     [
       deser_phase = deserialization.phase,
       deser_event = deserialization.event
-    ](const nlohmann::json& msg) -> agv::FleetUpdateHandle::DeserializedPhase
+    ](const nlohmann::json& msg) -> agv::DeserializedPhase
     {
       const auto& category = msg["category"].get<std::string>();
       const auto& description_json = msg["description"];
@@ -97,7 +97,7 @@ void add_compose(
       deser_activity = deserialize_activity,
       consider = deserialization.consider_composed
     ](const nlohmann::json& msg)
-    -> agv::FleetUpdateHandle::DeserializedTask
+    -> agv::DeserializedTask
     {
       if (!(*consider))
         return {nullptr, {"Not accepting composed requests"}};
@@ -160,7 +160,7 @@ void add_compose(
 
   using rmf_task_sequence::events::Bundle;
   using DeserializedDependencies =
-    agv::FleetUpdateHandle::DeserializedDescription<
+    agv::DeserializedDescription<
       std::optional<Bundle::Description::Dependencies>>;
 
   // Deserialize composed tasks
@@ -204,7 +204,7 @@ void add_compose(
 
   auto deserialize_event_sequence =
     [deserialize_event_dependencies](const nlohmann::json& msg)
-      -> agv::FleetUpdateHandle::DeserializedEvent
+      -> agv::DeserializedEvent
     {
       DeserializedDependencies dependencies;
       std::optional<std::string> category;
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp
index c02375f02..a499b257c 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp
@@ -287,11 +287,11 @@ struct TransferItems : public rmf_task_sequence::events::Placeholder::Descriptio
 };
 
 //==============================================================================
-using DeserializedItemTransfer = agv::FleetUpdateHandle::DeserializedEvent;
+using DeserializedItemTransfer = agv::DeserializedEvent;
 template<typename T>
 std::function<DeserializedItemTransfer(const nlohmann::json& msg)>
 make_deserializer(
-  const agv::FleetUpdateHandle::PlaceDeserializer& place_deser,
+  const agv::PlaceDeserializer& place_deser,
   const std::shared_ptr<agv::FleetUpdateHandle::ConsiderRequest>& consider)
 {
   auto parse_payload_component = [](const nlohmann::json& msg)
@@ -314,8 +314,7 @@ make_deserializer(
       place_deser,
       consider,
       parse_payload_component = std::move(parse_payload_component)
-    ](const nlohmann::json& msg)
-      -> agv::FleetUpdateHandle::DeserializedEvent
+    ](const nlohmann::json& msg) -> agv::DeserializedEvent
     {
       if (!consider || !(*consider))
         return {nullptr, {"Not accepting delivery requests"}};
@@ -415,7 +414,7 @@ void add_delivery(
 
   auto deserialize_delivery =
     [deserialize_pickup, deserialize_dropoff](
-      const nlohmann::json& msg) -> agv::FleetUpdateHandle::DeserializedTask
+      const nlohmann::json& msg) -> agv::DeserializedTask
     {
       const auto pickup = deserialize_pickup(msg["pickup"]);
       const auto dropoff = deserialize_dropoff(msg["dropoff"]);
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp
index 2de9629d2..476570954 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp
@@ -133,7 +133,7 @@ void add_loop(
 
   auto deserialize_go_to_place =
     [place_deser = deserialization.place](const nlohmann::json& msg)
-      -> agv::FleetUpdateHandle::DeserializedEvent
+      -> agv::DeserializedEvent
     {
       auto place = place_deser(msg);
       if (!place.description.has_value())
@@ -164,8 +164,7 @@ void add_loop(
     [
       place_deser = deserialization.place,
       consider = deserialization.consider_patrol
-    ](const nlohmann::json& msg)
-      -> agv::FleetUpdateHandle::DeserializedTask
+    ](const nlohmann::json& msg) -> agv::DeserializedTask
     {
       if (!(*consider))
         return {nullptr, {"Not accepting patrol requests"}};

From 3ca614b34f67143e3e3683bb8f808360efe7c9af Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Mon, 10 Jan 2022 15:44:29 +0800
Subject: [PATCH 49/79] Fix stray comma

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 rmf_fleet_adapter/schemas/event_description_Sequence.json       | 2 +-
 rmf_fleet_adapter/schemas/task_description_Compose.json         | 2 +-
 .../src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp             | 1 +
 rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Compose.cpp       | 1 +
 4 files changed, 4 insertions(+), 2 deletions(-)

diff --git a/rmf_fleet_adapter/schemas/event_description_Sequence.json b/rmf_fleet_adapter/schemas/event_description_Sequence.json
index 496383468..2f2554191 100644
--- a/rmf_fleet_adapter/schemas/event_description_Sequence.json
+++ b/rmf_fleet_adapter/schemas/event_description_Sequence.json
@@ -37,7 +37,7 @@
           }
         }
       },
-      "minimum": 1,
+      "minimum": 1
     }
   }
 }
diff --git a/rmf_fleet_adapter/schemas/task_description_Compose.json b/rmf_fleet_adapter/schemas/task_description_Compose.json
index 94a30d643..db656347d 100644
--- a/rmf_fleet_adapter/schemas/task_description_Compose.json
+++ b/rmf_fleet_adapter/schemas/task_description_Compose.json
@@ -21,7 +21,7 @@
         "properties": {
           "activity": { "$ref": "#/$defs/activity" },
           "on_cancel": {
-            "description": "A list of activities to perform if the task is canceled during this phase (each activity is given its own phase which can be skipped but not canceled).",
+            "description": "A list of activities to perform if the task is canceled during this phase. Each activity is given its own phase which can be skipped but not canceled.",
             "type": "array",
             "items": { "$ref": "#/$defs/activity" }
           }
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
index 7e45078c1..493980f0e 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
@@ -1336,6 +1336,7 @@ FleetUpdateHandle& FleetUpdateHandle::consider_composed_requests(
   ConsiderRequest consider)
 {
   *_pimpl->deserialization.consider_composed = std::move(consider);
+  return *this;
 }
 
 //==============================================================================
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Compose.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Compose.cpp
index 8f5b09932..6982afc9e 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Compose.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Compose.cpp
@@ -35,6 +35,7 @@ void add_compose(
 {
   deserialization.add_schema(
     rmf_fleet_adapter::schemas::task_description_Compose);
+
   deserialization.add_schema(
     rmf_fleet_adapter::schemas::event_description_Sequence);
 

From 8ca867a8b3eacecdb474bda93351f356e980684d Mon Sep 17 00:00:00 2001
From: youliang <tan_you_liang@hotmail.com>
Date: Tue, 11 Jan 2022 17:42:25 +0800
Subject: [PATCH 50/79] fix breaking add_fleet() py api change

Signed-off-by: youliang <tan_you_liang@hotmail.com>
---
 rmf_fleet_adapter_python/src/adapter.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/rmf_fleet_adapter_python/src/adapter.cpp b/rmf_fleet_adapter_python/src/adapter.cpp
index ea35b1906..2b99b12f4 100644
--- a/rmf_fleet_adapter_python/src/adapter.cpp
+++ b/rmf_fleet_adapter_python/src/adapter.cpp
@@ -354,7 +354,7 @@ PYBIND11_MODULE(rmf_adapter, m) {
     py::arg("fleet_name"),
     py::arg("traits"),
     py::arg("navigation_graph"),
-    py::arg("server_uri"))
+    py::arg("server_uri") = std::nullopt)
   .def("add_easy_traffic_light", &agv::Adapter::add_easy_traffic_light,
     py::arg("handle_callback"),
     py::arg("fleet_name"),
@@ -385,7 +385,7 @@ PYBIND11_MODULE(rmf_adapter, m) {
     py::arg("fleet_name"),
     py::arg("traits"),
     py::arg("navigation_graph"),
-    py::arg("server_uri"))
+    py::arg("server_uri") = std::nullopt)
   .def_property_readonly("node",
     py::overload_cast<>(
       &agv::test::MockAdapter::node))

From 06a34477642e046e731df22d131f00f04516d650 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Wed, 12 Jan 2022 13:30:58 +0800
Subject: [PATCH 51/79] Remove url reserved characters from task_id fields

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
index b203f9b37..80a0aa11c 100644
--- a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
+++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
@@ -421,7 +421,7 @@ class Dispatcher::Implementation
       const auto task_request_json = msg_json["request"];
       const std::string task_id =
         task_request_json["category"].get<std::string>()
-        + ":dispatch#" + std::to_string(task_counter++);
+        + "-dispatch_" + std::to_string(task_counter++);
 
       push_bid_notice(
         rmf_task_msgs::build<bidding::BidNoticeMsg>()
@@ -476,7 +476,7 @@ class Dispatcher::Implementation
 
     // auto generate a task_id for a given submitted task
     const auto task_id =
-      category + ":dispatch#" + std::to_string(task_counter++);
+      category + "-dispatch_" + std::to_string(task_counter++);
 
     RCLCPP_INFO(node->get_logger(),
       "Received Task Submission [%s]", task_id.c_str());

From 4baf4f8e12b7347b3c12cf83710892d4cf33731b Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Wed, 12 Jan 2022 17:17:05 +0800
Subject: [PATCH 52/79] Change the task_id format to something a bit nicer

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
index 80a0aa11c..69175818e 100644
--- a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
+++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
@@ -421,7 +421,7 @@ class Dispatcher::Implementation
       const auto task_request_json = msg_json["request"];
       const std::string task_id =
         task_request_json["category"].get<std::string>()
-        + "-dispatch_" + std::to_string(task_counter++);
+        + ".dispatch-" + std::to_string(task_counter++);
 
       push_bid_notice(
         rmf_task_msgs::build<bidding::BidNoticeMsg>()
@@ -476,7 +476,7 @@ class Dispatcher::Implementation
 
     // auto generate a task_id for a given submitted task
     const auto task_id =
-      category + "-dispatch_" + std::to_string(task_counter++);
+      category + ".dispatch-" + std::to_string(task_counter++);
 
     RCLCPP_INFO(node->get_logger(),
       "Received Task Submission [%s]", task_id.c_str());

From ce8bb9afcb5107cdaa435c14dc9bd216fdb9fc3c Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Wed, 12 Jan 2022 17:32:53 +0800
Subject: [PATCH 53/79] Remember to fill in event states

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 .../src/rmf_fleet_adapter/TaskManager.cpp     | 19 ++++++++++---------
 1 file changed, 10 insertions(+), 9 deletions(-)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
index cbd423677..104ed0141 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
@@ -313,15 +313,16 @@ nlohmann::json& copy_phase_data(
   const auto& tag = *snapshot.tag();
   const auto& header = tag.header();
   const auto id = tag.id();
-  auto& phase = phases[std::to_string(id)];
-  phase["id"] = id;
-  phase["category"] = header.category();
-  phase["detail"] = header.detail();
-  phase["original_estimate_millis"] =
+  auto& phase_state = phases[std::to_string(id)];
+  phase_state["id"] = id;
+  phase_state["category"] = header.category();
+  phase_state["detail"] = header.detail();
+  phase_state["original_estimate_millis"] =
     std::max(0l, to_millis(header.original_duration_estimate()).count());
-  phase["estimate_millis"] =
+  phase_state["estimate_millis"] =
     std::max(0l, to_millis(snapshot.estimate_remaining_time()).count());
-  phase["final_event_id"] = snapshot.final_event()->id();
+  phase_state["final_event_id"] = snapshot.final_event()->id();
+  auto& event_states = phase_state["events"];
 
   // TODO(MXG): Add in skip request information
 
@@ -337,7 +338,7 @@ nlohmann::json& copy_phase_data(
     const auto top = event_queue.back();
     event_queue.pop_back();
 
-    nlohmann::json event_state;
+    auto& event_state = event_states[std::to_string(top->id())];
     event_state["id"] = top->id();
     event_state["status"] = status_to_string(top->status());
 
@@ -367,7 +368,7 @@ nlohmann::json& copy_phase_data(
     event_state["deps"] = std::move(deps);
   }
 
-  return phase;
+  return phase_state;
 }
 
 //==============================================================================

From a4fab05a8b5a616aaaa81dd9b3a1634e5de67cc8 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Thu, 13 Jan 2022 14:33:49 +0800
Subject: [PATCH 54/79] Fix the usage of nlohmann::json

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
index 104ed0141..4fd169593 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
@@ -1565,8 +1565,12 @@ void TaskManager::_handle_request(
   const std::string& request_id)
 {
   const auto request_json = nlohmann::json::parse(request_msg);
-  const auto& type = request_json["type"];
-  if (!type)
+  const auto type_it = request_json.find("type");
+  if (type_it == request_json.end())
+    return;
+
+  const auto& type = type_it.value();
+  if (!type.is_string())
     return;
 
   try

From 067d8e2ffb89243c9ea393f9d7410c0853466940 Mon Sep 17 00:00:00 2001
From: youliang <tan_you_liang@hotmail.com>
Date: Thu, 13 Jan 2022 13:44:00 +0800
Subject: [PATCH 55/79] fix json key check error

Signed-off-by: youliang <tan_you_liang@hotmail.com>
---
 .../src/rmf_fleet_adapter/TaskManager.cpp     |  6 ++---
 .../agv/FleetUpdateHandle.cpp                 | 22 +++++++++----------
 .../src/rmf_fleet_adapter/tasks/Delivery.cpp  | 12 +++++-----
 .../src/rmf_fleet_adapter/tasks/Loop.cpp      |  6 ++---
 4 files changed, 23 insertions(+), 23 deletions(-)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
index 4fd169593..e39123c2b 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
@@ -1603,9 +1603,9 @@ namespace {
 //==============================================================================
 std::vector<std::string> get_labels(const nlohmann::json& request)
 {
-  const auto& labels = request["labels"];
-  if (!labels.is_null())
-    return labels.get<std::vector<std::string>>();
+  const auto labels_it = request.find("labels");
+  if (labels_it != request.end())
+    return labels_it->get<std::vector<std::string>>();
 
   return {};
 }
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
index 493980f0e..c312e77fb 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
@@ -279,21 +279,21 @@ void FleetUpdateHandle::Implementation::bid_notice_cb(
 
   rmf_traffic::Time earliest_start_time =
     rmf_traffic::Time(rmf_traffic::Duration::min());
-  const auto& t = request_msg["unix_millis_earliest_start_time"];
-  if (!t.is_null())
+  const auto t_it = request_msg.find("unix_millis_earliest_start_time");
+  if (t_it != request_msg.end())
   {
     earliest_start_time =
-      rmf_traffic::Time(std::chrono::milliseconds(t.get<uint64_t>()));
+      rmf_traffic::Time(std::chrono::milliseconds(t_it->get<uint64_t>()));
   }
 
   rmf_task::ConstPriorityPtr priority;
-  const auto& p = request_msg["priority"];
-  if (!p.is_null())
+  const auto p_it = request_msg.find("priority");
+  if (p_it != request_msg.end())
   {
-    const auto& p_type = p["type"];
+    const auto& p_type = (*p_it)["type"];
     if (p_type.is_string() && p_type.get<std::string>() == "binary")
     {
-      const auto& p_value = p["value"];
+      const auto& p_value = (*p_it)["value"];
       if (p_value.is_number_integer())
       {
         if (p_value.is_number_integer() && p_value.get<uint64_t>() > 0)
@@ -308,7 +308,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb(
       errors.push_back(
         make_error_str(
           4, "Unsupported type",
-          "Fleet [" + name + "] does not support priority request: " + p.dump()
+          "Fleet [" + name + "] does not support priority request: " + p_it->dump()
           + "\nDefaulting to low binary priority."));
     }
   }
@@ -1447,9 +1447,9 @@ FleetUpdateHandle& FleetUpdateHandle::accept_task_requests(
       rmf_dispenser_msgs::msg::DispenserRequestItem output;
       output.type_guid = item["sku"];
       output.quantity = item["quantity"];
-      const auto& compartment = item["compartment"];
-      if (!compartment.is_null())
-        output.compartment_name = compartment.get<std::string>();
+      const auto compartment_it = item.find("compartment");
+      if (compartment_it != item.end())
+        output.compartment_name = compartment_it->get<std::string>();
 
       return output;
     };
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp
index a499b257c..4afc4915c 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp
@@ -299,9 +299,9 @@ make_deserializer(
     {
       std::string compartment = "";
 
-      const auto& compartment_json = msg["compartment"];
-      if (!compartment_json.is_null())
-        compartment = compartment_json.get<std::string>();
+      const auto compartment_json_it = msg.find("compartment");
+      if (compartment_json_it != msg.end())
+        compartment = compartment_json_it->get<std::string>();
 
       return rmf_task::Payload::Component(
         msg["sku"].get<std::string>(),
@@ -349,10 +349,10 @@ make_deserializer(
       }
 
       std::string handler;
-      const auto& handler_json = msg["handler"];
-      if (!handler_json.is_null())
+      const auto& handler_json_it = msg.find("handler");
+      if (handler_json_it != msg.end())
       {
-        handler = handler_json.get<std::string>();
+        handler = handler_json_it->get<std::string>();
       }
 
       agv::FleetUpdateHandle::Confirmation confirm;
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp
index 476570954..06a5b916f 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp
@@ -196,9 +196,9 @@ void add_loop(
         return {nullptr, errors};
 
       std::size_t rounds = 1;
-      const auto& rounds_json = msg["rounds"];
-      if (!rounds_json.is_null())
-        rounds = rounds_json.get<std::size_t>();
+      const auto& rounds_json_it = msg.find("rounds");
+      if (rounds_json_it != msg.end())
+        rounds = rounds_json_it->get<std::size_t>();
 
       rmf_task_sequence::Task::Builder builder;
       for (std::size_t i=0; i < rounds; ++i)

From 26f4b39aae8c8c4d16c90bfe1e91e46773f56e63 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Thu, 13 Jan 2022 16:09:38 +0800
Subject: [PATCH 56/79] Fix the use of nlohmann::json

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp | 16 +++++++++++++---
 1 file changed, 13 insertions(+), 3 deletions(-)

diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
index 69175818e..b7b3d33e6 100644
--- a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
+++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
@@ -380,13 +380,23 @@ class Dispatcher::Implementation
     }
 
     const auto msg_json = nlohmann::json::parse(msg.json_msg);
-    const auto& type = msg_json["type"];
-    if (!type)
+//    const auto& type = msg_json["type"];
+    const auto type_it = msg_json.find("type");
+    if (type_it == msg_json.end())
+    {
+      // Whatever type of message this is, we don't support it
+      return;
+    }
+
+    if (!type_it.value().is_string())
+    {
+      // We expect the type field to contain a string
       return;
+    }
 
     try
     {
-      const auto& type_str = type.get<std::string>();
+      const auto& type_str = type_it.value().get<std::string>();
       if (type_str != "dispatch_task_request")
         return;
 

From cac195942ece633873a6ed764af54c3330ad278b Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Thu, 13 Jan 2022 16:27:47 +0800
Subject: [PATCH 57/79] Adding debug printouts to dispatcher

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 .../src/rmf_task_ros2/Dispatcher.cpp          | 19 ++++++++++++++++++-
 1 file changed, 18 insertions(+), 1 deletion(-)

diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
index b7b3d33e6..a61d11619 100644
--- a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
+++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
@@ -372,22 +372,32 @@ class Dispatcher::Implementation
 
   void handle_api_request(const ApiRequestMsg& msg)
   {
+    RCLCPP_INFO(node->get_logger(), "Received an API request message");
+
     const auto check = api_memory.lookup(msg.request_id);
     if (check.has_value())
     {
+      RCLCPP_INFO(
+        node->get_logger(),
+        "Receive duplicate request, republishing the previous message");
       api_response->publish(*check);
       return;
     }
 
     const auto msg_json = nlohmann::json::parse(msg.json_msg);
-//    const auto& type = msg_json["type"];
     const auto type_it = msg_json.find("type");
     if (type_it == msg_json.end())
     {
+      RCLCPP_INFO(
+        node->get_logger(),
+        "Received an unknown type of request:\n%s", msg.json_msg.c_str());
       // Whatever type of message this is, we don't support it
       return;
     }
 
+    RCLCPP_INFO(
+      node->get_logger(),
+      "Received a request of type [%s]", msg.json_msg.c_str());
     if (!type_it.value().is_string())
     {
       // We expect the type field to contain a string
@@ -398,7 +408,12 @@ class Dispatcher::Implementation
     {
       const auto& type_str = type_it.value().get<std::string>();
       if (type_str != "dispatch_task_request")
+      {
+        RCLCPP_INFO(
+          node->get_logger(),
+          "Rejecting because we only accept dispatch_task_request type");
         return;
+      }
 
       static const auto request_validator =
         make_validator(rmf_api_msgs::schemas::dispatch_task_request);
@@ -406,6 +421,7 @@ class Dispatcher::Implementation
       try
       {
         request_validator.validate(msg_json);
+        RCLCPP_INFO(node->get_logger(), "Message was succesfully validated");
       }
       catch (const std::exception& e)
       {
@@ -459,6 +475,7 @@ class Dispatcher::Implementation
         .request_id(msg.request_id);
 
       api_memory.add(response);
+      RCLCPP_INFO(node->get_logger(), "Publishing API response");
       api_response->publish(response);
 
       // TODO(MXG): Make some way to keep pushing task state updates to the

From ede8b53114bdf8a2e435b1874e75ca9ba7aaaa42 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Thu, 13 Jan 2022 16:34:56 +0800
Subject: [PATCH 58/79] Desperate debug

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
index a61d11619..d7f2ce94b 100644
--- a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
+++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
@@ -442,19 +442,23 @@ class Dispatcher::Implementation
 
         api_memory.add(response);
         api_response->publish(response);
+        return;
       }
 
+      RCLCPP_INFO(node->get_logger(), "Made it to line %d", __LINE__);
       const auto task_request_json = msg_json["request"];
       const std::string task_id =
         task_request_json["category"].get<std::string>()
         + ".dispatch-" + std::to_string(task_counter++);
 
+      RCLCPP_INFO(node->get_logger(), "Made it to line %d", __LINE__);
       push_bid_notice(
         rmf_task_msgs::build<bidding::BidNoticeMsg>()
           .request(task_request_json.dump())
           .task_id(task_id)
           .time_window(bidding_time_window));
 
+      RCLCPP_INFO(node->get_logger(), "Made it to line %d", __LINE__);
       nlohmann::json response_json;
       response_json["success"] = true;
 
@@ -466,9 +470,11 @@ class Dispatcher::Implementation
       booking["priority"] = task_request_json["priority"];
       booking["labels"] = task_request_json["labels"];
 
+      RCLCPP_INFO(node->get_logger(), "Made it to line %d", __LINE__);
       auto& dispatch = task_state["dispatch"];
       dispatch["status"] = "queued";
 
+      RCLCPP_INFO(node->get_logger(), "Made it to line %d", __LINE__);
       auto response = rmf_task_msgs::build<ApiResponseMsg>()
         .type(ApiResponseMsg::TYPE_RESPONDING)
         .json_msg(task_state.dump())
@@ -478,6 +484,7 @@ class Dispatcher::Implementation
       RCLCPP_INFO(node->get_logger(), "Publishing API response");
       api_response->publish(response);
 
+      RCLCPP_INFO(node->get_logger(), "Made it to line %d", __LINE__);
       // TODO(MXG): Make some way to keep pushing task state updates to the
       // api-server as the bidding process progresses. We could do a websocket
       // connection or maybe just a simple ROS2 publisher.

From acca4ae64c3f8cc44f5b6c993aea46183f0524e0 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Thu, 13 Jan 2022 16:49:45 +0800
Subject: [PATCH 59/79] Fix usage of json

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 .../src/rmf_task_ros2/Dispatcher.cpp          | 35 +++++++++----------
 1 file changed, 17 insertions(+), 18 deletions(-)

diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
index d7f2ce94b..e69687867 100644
--- a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
+++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
@@ -446,13 +446,13 @@ class Dispatcher::Implementation
       }
 
       RCLCPP_INFO(node->get_logger(), "Made it to line %d", __LINE__);
-      const auto task_request_json = msg_json["request"];
+      const auto& task_request_json = msg_json["request"];
       const std::string task_id =
         task_request_json["category"].get<std::string>()
         + ".dispatch-" + std::to_string(task_counter++);
 
       RCLCPP_INFO(node->get_logger(), "Made it to line %d", __LINE__);
-      push_bid_notice(
+      const auto task_state = push_bid_notice(
         rmf_task_msgs::build<bidding::BidNoticeMsg>()
           .request(task_request_json.dump())
           .task_id(task_id)
@@ -461,18 +461,7 @@ class Dispatcher::Implementation
       RCLCPP_INFO(node->get_logger(), "Made it to line %d", __LINE__);
       nlohmann::json response_json;
       response_json["success"] = true;
-
-      auto& task_state = response_json["state"];
-      auto& booking = task_state["booking"];
-      booking["id"] = task_id;
-      booking["unix_millis_earliest_start_time"] =
-          task_request_json["unix_millis_earliest_start_time"];
-      booking["priority"] = task_request_json["priority"];
-      booking["labels"] = task_request_json["labels"];
-
-      RCLCPP_INFO(node->get_logger(), "Made it to line %d", __LINE__);
-      auto& dispatch = task_state["dispatch"];
-      dispatch["status"] = "queued";
+      response_json["state"] = task_state;
 
       RCLCPP_INFO(node->get_logger(), "Made it to line %d", __LINE__);
       auto response = rmf_task_msgs::build<ApiResponseMsg>()
@@ -489,9 +478,11 @@ class Dispatcher::Implementation
       // api-server as the bidding process progresses. We could do a websocket
       // connection or maybe just a simple ROS2 publisher.
     }
-    catch (const std::exception&)
+    catch (const std::exception& e)
     {
-      // Do nothing. The message is not meant for us.
+      RCLCPP_ERROR(
+        node->get_logger(),
+        "Failed to handle API request message: %s", e.what());
     }
   }
 
@@ -537,7 +528,7 @@ class Dispatcher::Implementation
     return task_id;
   }
 
-  void push_bid_notice(bidding::BidNoticeMsg bid_notice)
+  nlohmann::json push_bid_notice(bidding::BidNoticeMsg bid_notice)
   {
     nlohmann::json state;
     auto& booking = state["booking"];
@@ -551,11 +542,18 @@ class Dispatcher::Implementation
       };
 
     for (const auto& field : copy_fields)
-      booking[field] = request.at(field);
+    {
+      const auto f_it = request.find(field);
+      if (f_it != request.end())
+        booking[field] = f_it.value();
+    }
 
     state["category"] = request["category"];
     state["detail"] = request["description"];
 
+    auto& dispatch = state["dispatch"];
+    dispatch["status"] = "queued";
+
     // TODO(MXG): Publish this initial task state message to the websocket!
 
     auto new_dispatch_state =
@@ -568,6 +566,7 @@ class Dispatcher::Implementation
       on_change_fn(*new_dispatch_state);
 
     auctioneer->request_bid(bid_notice);
+    return state;
   }
 
   bool cancel_task(const TaskID& task_id)

From 707aba106b5264bc5d78f465a16da4fab7c309d5 Mon Sep 17 00:00:00 2001
From: Yadunund <yadunund@openrobotics.org>
Date: Fri, 14 Jan 2022 00:27:46 +0800
Subject: [PATCH 60/79] Fix category for legacy clean

Signed-off-by: Yadunund <yadunund@openrobotics.org>
---
 rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
index b7b3d33e6..212fbb454 100644
--- a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
+++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
@@ -172,7 +172,7 @@ class Dispatcher::Implementation
     {
       {1, "patrol"},
       {2, "delivery"},
-      {4, "patrol"}
+      {4, "clean"}
     };
 
   using LegacyConversion =

From 50c5982b137ea033c380669233331add48fb6ab4 Mon Sep 17 00:00:00 2001
From: Yadunund <yadunund@openrobotics.org>
Date: Fri, 14 Jan 2022 06:15:11 +0800
Subject: [PATCH 61/79] Wrap json parse in try-catch blocks

Signed-off-by: Yadunund <yadunund@openrobotics.org>
---
 .../src/rmf_fleet_adapter/TaskManager.cpp       | 15 ++++++++++++++-
 rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp  | 17 +++++++++++++++--
 2 files changed, 29 insertions(+), 3 deletions(-)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
index e39123c2b..bae68e1b1 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
@@ -1564,7 +1564,20 @@ void TaskManager::_handle_request(
   const std::string& request_msg,
   const std::string& request_id)
 {
-  const auto request_json = nlohmann::json::parse(request_msg);
+  nlohmann::json request_json;
+  try
+  {
+    request_json = nlohmann::json::parse(request_msg);
+  }
+  catch(const std::exception& e)
+  {
+    RCLCPP_ERROR(
+      _context->node()->get_logger(),
+      "Error parsing json_msg: %s",
+      e.what());
+    return;
+  }
+
   const auto type_it = request_json.find("type");
   if (type_it == request_json.end())
     return;
diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
index 212fbb454..6c7566e75 100644
--- a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
+++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
@@ -379,8 +379,21 @@ class Dispatcher::Implementation
       return;
     }
 
-    const auto msg_json = nlohmann::json::parse(msg.json_msg);
-//    const auto& type = msg_json["type"];
+    nlohmann::json msg_json;
+    try
+    {
+      msg_json = nlohmann::json::parse(msg.json_msg);
+    }
+    catch(const std::exception& e)
+    {
+      RCLCPP_ERROR(
+        node->get_logger(),
+        "Error parsing json_msg: %s",
+        e.what());
+      return;
+    }
+
+    // const auto& type = msg_json["type"];
     const auto type_it = msg_json.find("type");
     if (type_it == msg_json.end())
     {

From d3c49386e87297536af24cde541a37dae143a38d Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Fri, 14 Jan 2022 13:59:03 +0800
Subject: [PATCH 62/79] Remove debugging cruft

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 .../src/rmf_task_ros2/Dispatcher.cpp          | 21 -------------------
 1 file changed, 21 deletions(-)

diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
index e69687867..5a3b72448 100644
--- a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
+++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
@@ -372,14 +372,9 @@ class Dispatcher::Implementation
 
   void handle_api_request(const ApiRequestMsg& msg)
   {
-    RCLCPP_INFO(node->get_logger(), "Received an API request message");
-
     const auto check = api_memory.lookup(msg.request_id);
     if (check.has_value())
     {
-      RCLCPP_INFO(
-        node->get_logger(),
-        "Receive duplicate request, republishing the previous message");
       api_response->publish(*check);
       return;
     }
@@ -388,16 +383,10 @@ class Dispatcher::Implementation
     const auto type_it = msg_json.find("type");
     if (type_it == msg_json.end())
     {
-      RCLCPP_INFO(
-        node->get_logger(),
-        "Received an unknown type of request:\n%s", msg.json_msg.c_str());
       // Whatever type of message this is, we don't support it
       return;
     }
 
-    RCLCPP_INFO(
-      node->get_logger(),
-      "Received a request of type [%s]", msg.json_msg.c_str());
     if (!type_it.value().is_string())
     {
       // We expect the type field to contain a string
@@ -409,9 +398,6 @@ class Dispatcher::Implementation
       const auto& type_str = type_it.value().get<std::string>();
       if (type_str != "dispatch_task_request")
       {
-        RCLCPP_INFO(
-          node->get_logger(),
-          "Rejecting because we only accept dispatch_task_request type");
         return;
       }
 
@@ -421,7 +407,6 @@ class Dispatcher::Implementation
       try
       {
         request_validator.validate(msg_json);
-        RCLCPP_INFO(node->get_logger(), "Message was succesfully validated");
       }
       catch (const std::exception& e)
       {
@@ -445,35 +430,29 @@ class Dispatcher::Implementation
         return;
       }
 
-      RCLCPP_INFO(node->get_logger(), "Made it to line %d", __LINE__);
       const auto& task_request_json = msg_json["request"];
       const std::string task_id =
         task_request_json["category"].get<std::string>()
         + ".dispatch-" + std::to_string(task_counter++);
 
-      RCLCPP_INFO(node->get_logger(), "Made it to line %d", __LINE__);
       const auto task_state = push_bid_notice(
         rmf_task_msgs::build<bidding::BidNoticeMsg>()
           .request(task_request_json.dump())
           .task_id(task_id)
           .time_window(bidding_time_window));
 
-      RCLCPP_INFO(node->get_logger(), "Made it to line %d", __LINE__);
       nlohmann::json response_json;
       response_json["success"] = true;
       response_json["state"] = task_state;
 
-      RCLCPP_INFO(node->get_logger(), "Made it to line %d", __LINE__);
       auto response = rmf_task_msgs::build<ApiResponseMsg>()
         .type(ApiResponseMsg::TYPE_RESPONDING)
         .json_msg(task_state.dump())
         .request_id(msg.request_id);
 
       api_memory.add(response);
-      RCLCPP_INFO(node->get_logger(), "Publishing API response");
       api_response->publish(response);
 
-      RCLCPP_INFO(node->get_logger(), "Made it to line %d", __LINE__);
       // TODO(MXG): Make some way to keep pushing task state updates to the
       // api-server as the bidding process progresses. We could do a websocket
       // connection or maybe just a simple ROS2 publisher.

From 6d174d2da0b23bceeedc15ed553ac01a06412042 Mon Sep 17 00:00:00 2001
From: youliang <tan_you_liang@hotmail.com>
Date: Thu, 20 Jan 2022 12:26:04 +0800
Subject: [PATCH 63/79] fix empty itinerary gotoplace error

Signed-off-by: youliang <tan_you_liang@hotmail.com>
---
 .../src/rmf_fleet_adapter/events/GoToPlace.cpp              | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp
index 7938ab2ce..b1e56e9f6 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp
@@ -377,6 +377,12 @@ void GoToPlace::Active::_execute_plan(rmf_traffic::agv::Plan plan)
   if (_is_interrupted)
     return;
 
+  if (plan.get_itinerary().empty())
+  {
+    _finished();
+    return;
+  }
+
   _execution = ExecutePlan::make(
     _context, std::move(plan), _assign_id, _state,
     _update, _finished, _tail_period);

From d054c3246a735f51e5e97f87b8ecc22b42fdfc1f Mon Sep 17 00:00:00 2001
From: youliang <tan_you_liang@hotmail.com>
Date: Fri, 21 Jan 2022 15:29:27 +0800
Subject: [PATCH 64/79] fix compilation on foxy

Signed-off-by: youliang <tan_you_liang@hotmail.com>
---
 rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp | 11 ++++++++---
 1 file changed, 8 insertions(+), 3 deletions(-)

diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
index afe7d1f5c..edaab78e9 100644
--- a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
+++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
@@ -44,6 +44,8 @@
 #include <rmf_api_msgs/schemas/task_state.hpp>
 #include <rmf_api_msgs/schemas/error.hpp>
 
+#include <unordered_set>
+
 namespace rmf_task_ros2 {
 
 namespace {
@@ -211,7 +213,10 @@ class Dispatcher::Implementation
     api_request = node->create_subscription<ApiRequestMsg>(
       "task_api_requests",
       rclcpp::SystemDefaultsQoS().reliable().transient_local(),
-      [this](const ApiRequestMsg& msg) { this->handle_api_request(msg); });
+      [this](const ApiRequestMsg::UniquePtr msg)
+      {
+        this->handle_api_request(*msg);
+      });
 
     api_response = node->create_publisher<ApiResponseMsg>(
       "task_api_responses",
@@ -235,9 +240,9 @@ class Dispatcher::Implementation
     dispatch_ack_sub = node->create_subscription<DispatchAckMsg>(
       rmf_task_ros2::DispatchAckTopicName,
       rclcpp::ServicesQoS().keep_last(20).transient_local(),
-      [this](const DispatchAckMsg& msg)
+      [this](const DispatchAckMsg::UniquePtr msg)
       {
-        this->handle_dispatch_ack(msg);
+        this->handle_dispatch_ack(*msg);
       });
 
     auctioneer = bidding::Auctioneer::make(

From 10c9a5110c0cc5496ad913b480048246764d1d57 Mon Sep 17 00:00:00 2001
From: Yadu <yadunund@openrobotics.org>
Date: Tue, 25 Jan 2022 01:29:51 +0900
Subject: [PATCH 65/79] Support PerformAction event in composed tasks (#155)

* Implemented PerformAction event and schema

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Added ActionExecutor to RobotUpdateHandle

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Add category to PerformAction schema

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Support teleop in full_control

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Print errors when auctioneer does not receive any bids

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Fix bug in ActiveTask::publish_task_state()

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Add PerformAction event inside make()

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Implement ActionExecution

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Update ActionExecution implementation

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Update ActionExecution API

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Schema and other fixes

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Call finished only once

Signed-off-by: Yadunund <yadunund@openrobotics.org>
---
 .../agv/FleetUpdateHandle.hpp                 |  15 +
 .../agv/RobotUpdateHandle.hpp                 |  46 +++
 .../event_description_PerformAction.json      |  29 ++
 rmf_fleet_adapter/src/full_control/main.cpp   |  91 +++++-
 .../src/rmf_fleet_adapter/TaskManager.cpp     |   6 +-
 .../agv/FleetUpdateHandle.cpp                 |  35 ++-
 .../rmf_fleet_adapter/agv/RobotContext.cpp    |  24 ++
 .../rmf_fleet_adapter/agv/RobotContext.hpp    |  10 +
 .../agv/RobotUpdateHandle.cpp                 |  52 ++++
 .../agv/internal_FleetUpdateHandle.hpp        |  81 ++++++
 .../agv/internal_RobotUpdateHandle.hpp        |  37 ++-
 .../rmf_fleet_adapter/events/GoToPlace.cpp    |   2 +-
 .../events/PerformAction.cpp                  | 269 ++++++++++++++++++
 .../events/PerformAction.hpp                  | 134 +++++++++
 .../src/rmf_task_ros2/Dispatcher.cpp          |  12 +
 15 files changed, 830 insertions(+), 13 deletions(-)
 create mode 100644 rmf_fleet_adapter/schemas/event_description_PerformAction.json
 create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/events/PerformAction.cpp
 create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/events/PerformAction.hpp

diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp
index e4cd19b28..8f2716194 100644
--- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp
@@ -185,6 +185,21 @@ class FleetUpdateHandle : public std::enable_shared_from_this<FleetUpdateHandle>
   ///   robots before triggering this callback.
   FleetUpdateHandle& consider_composed_requests(ConsiderRequest consider);
 
+  /// Allow this fleet adapter to execute a PerformAction activity of specified
+  /// category which may be present in sequence event.
+  ///
+  /// \param[in] category
+  ///   A string that categorizes the action. This value should be used when
+  ///   filling out the category field in event_description_PerformAction.json
+  ///   schema.
+  ///
+  /// \param[in] consider
+  ///   Decide whether to accept the action based on the description field in
+  ///   event_description_PerformAction.json schema.
+  FleetUpdateHandle& add_performable_action(
+    const std::string& category,
+    ConsiderRequest consider);
+
   /// Specify a set of lanes that should be closed.
   void close_lanes(std::vector<std::size_t> lane_indices);
 
diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp
index 35b16518f..ee9839344 100644
--- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp
+++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp
@@ -25,6 +25,7 @@
 #include <rmf_traffic/schedule/Participant.hpp>
 
 #include <Eigen/Geometry>
+#include <nlohmann/json.hpp>
 
 #include <vector>
 #include <memory>
@@ -110,6 +111,51 @@ class RobotUpdateHandle
   /// value that was given to the setter.
   rmf_utils::optional<rmf_traffic::Duration> maximum_delay() const;
 
+  /// The ActionExecution class should be used to manage the execution of and
+  /// provide updates on ongoing actions.
+  class ActionExecution
+  {
+  public:
+    // Update the amount of time remaining for this action
+    void update_remaining_time(rmf_traffic::Duration remaining_time_estimate);
+
+    // Trigger this when the action is finished
+    void finished();
+
+    // Returns false if the Action has been killed or cancelled
+    bool okay() const;
+
+    // TODO: Consider giving access to the participant schedule and
+    // traffic negotiation
+
+    // The desctructor will trigger finished() if it has not already been called
+    ~ActionExecution();
+
+    class Implementation;
+  private:
+    ActionExecution();
+    rmf_utils::impl_ptr<Implementation> _pimpl;
+  };
+
+  /// Signature for a callback to request the robot to perform an action
+  ///
+  /// \param[in] category
+  ///   A category of the action to be performed
+  ///
+  /// \param[in] description
+  ///   A description of the action to be performed
+  ///
+  /// \param[in] execution
+  ///   An ActionExecution object that will be provided to the user for
+  ///   updating the state of the action.
+  using ActionExecutor = std::function<void(
+    const std::string& category,
+    const nlohmann::json& description,
+    ActionExecution execution)>;
+
+  /// Set the ActionExecutor for this robot
+  void set_action_executor(ActionExecutor action_executor);
+
   class Implementation;
 
   /// This API is experimental and will not be supported in the future. Users
diff --git a/rmf_fleet_adapter/schemas/event_description_PerformAction.json b/rmf_fleet_adapter/schemas/event_description_PerformAction.json
new file mode 100644
index 000000000..2422aa0ea
--- /dev/null
+++ b/rmf_fleet_adapter/schemas/event_description_PerformAction.json
@@ -0,0 +1,29 @@
+{
+  "$schema": "https://json-schema.org/draft/2020-12/schema",
+  "$id": "https://raw.githubusercontent.com/open-rmf/rmf_ros2/main/rmf_fleet_adapter/schemas/event_description_PerformAction.json",
+  "title": "PerformAction event Description",
+  "description": "Description schema for the perform_action category",
+  "type": "object",
+  "properties": {
+    "category": {
+      "description": "A string that describes the nature of this action. It should match that passed into FleetUpdateHandle::add_performable_action()",
+      "type": "string"
+    },
+    "description": {
+      "description": "Additional information that will be passed along to the action executor."
+    },
+    "unix_millis_action_duration_estimate": {
+      "description": "(Recommended) The estimated duration for this action",
+      "type": "integer"
+    },
+    "use_tool_sink": {
+      "description": "(Optional) Whether the tool on the robot will be powered during the action. Default to false",
+      "type": "boolean"
+    },
+    "expected_finish_location": {
+      "description": "(Optional) A place that the robot will end up after the action. Default to last known location",
+      "$ref": "Place.json"
+    }
+  },
+  "required": ["category", "description"]
+}
diff --git a/rmf_fleet_adapter/src/full_control/main.cpp b/rmf_fleet_adapter/src/full_control/main.cpp
index daab9ab7d..010b8726f 100644
--- a/rmf_fleet_adapter/src/full_control/main.cpp
+++ b/rmf_fleet_adapter/src/full_control/main.cpp
@@ -58,6 +58,8 @@
 #include <rmf_battery/agv/SimpleDevicePowerSink.hpp>
 
 #include <Eigen/Geometry>
+#include <nlohmann/json.hpp>
+
 #include <unordered_set>
 #include <optional>
 
@@ -150,7 +152,8 @@ std::optional<DistanceFromGraph> distance_from_graph(
 
 //==============================================================================
 class FleetDriverRobotCommandHandle
-  : public rmf_fleet_adapter::agv::RobotCommandHandle
+  : public rmf_fleet_adapter::agv::RobotCommandHandle,
+    public std::enable_shared_from_this<FleetDriverRobotCommandHandle>
 {
 public:
 
@@ -160,6 +163,9 @@ class FleetDriverRobotCommandHandle
   using ModeRequestPub =
     rclcpp::Publisher<rmf_fleet_msgs::msg::ModeRequest>::SharedPtr;
 
+  using ActionExecution =
+    rmf_fleet_adapter::agv::RobotUpdateHandle::ActionExecution;
+
   FleetDriverRobotCommandHandle(
     rclcpp::Node& node,
     std::string fleet_name,
@@ -481,6 +487,21 @@ class FleetDriverRobotCommandHandle
   void set_updater(rmf_fleet_adapter::agv::RobotUpdateHandlePtr updater)
   {
     _travel_info.updater = std::move(updater);
+    // Set the action_executor for the robot
+    const auto teleop_executioner =
+      [w = weak_from_this()](const std::string& category,
+        const nlohmann::json& description,
+        ActionExecution execution)
+      {
+        // We do not do anything here. The user can can move the robot by
+        // sending PathRequest msgs. Instead we simply store the completed
+        // callback which will be called when we receive a RobotModeRequest.
+        const auto self = w.lock();
+        if (!self)
+          return;
+        self->set_action_execution(execution);
+      };
+    _travel_info.updater->set_action_executor(teleop_executioner);
   }
 
   void newly_closed_lanes(const std::unordered_set<std::size_t>& closed_lanes)
@@ -567,6 +588,25 @@ class FleetDriverRobotCommandHandle
       _travel_info.updater->interrupted();
   }
 
+void set_action_execution(ActionExecution action_execution)
+{
+  _action_execution = action_execution;
+}
+
+void complete_robot_action()
+{
+  if (!_action_execution.has_value())
+    return;
+
+  _action_execution->finished();
+  _action_execution = std::nullopt;
+
+  RCLCPP_INFO(
+  _node->get_logger(),
+  "Robot [%s] has completed the action it was performing",
+  _travel_info.robot_name.c_str());
+}
+
 private:
 
   rclcpp::Node* _node;
@@ -590,6 +630,9 @@ class FleetDriverRobotCommandHandle
 
   std::mutex _mutex;
 
+  // ActionExecution for managing teleop action
+  std::optional<ActionExecution> _action_execution = std::nullopt;
+
   std::unique_lock<std::mutex> _lock()
   {
     std::unique_lock<std::mutex> lock(_mutex, std::defer_lock);
@@ -640,6 +683,10 @@ struct Connections : public std::enable_shared_from_this<Connections>
   rclcpp::Publisher<rmf_fleet_msgs::msg::ModeRequest>::SharedPtr
     mode_request_pub;
 
+  /// The topic subscription for ending teleop actions
+  rclcpp::Subscription<rmf_fleet_msgs::msg::ModeRequest>::SharedPtr
+    mode_request_sub;
+
   /// The client for listening to whether there is clearance in a lift
   rclcpp::Client<rmf_fleet_msgs::srv::LiftClearance>::SharedPtr
     lift_watchdog_client;
@@ -907,6 +954,34 @@ std::shared_ptr<Connections> make_fleet(
       connections->closed_lanes_pub->publish(state_msg);
     });
 
+    connections->mode_request_sub =
+      adapter->node()->create_subscription<rmf_fleet_msgs::msg::ModeRequest>(
+      "/action_execution_notice",
+      rclcpp::SystemDefaultsQoS(),
+      [w = connections->weak_from_this(), fleet_name](
+      rmf_fleet_msgs::msg::ModeRequest::UniquePtr msg)
+      {
+        if (msg->fleet_name.empty() ||
+          msg->fleet_name != fleet_name ||
+          msg->robot_name.empty())
+        {
+          return;
+        }
+
+        if (msg->mode.mode == msg->mode.MODE_IDLE)
+        {
+          const auto self = w.lock();
+          if (!self)
+            return;
+
+          const auto command_it = self->robots.find(msg->robot_name);
+          if (command_it == self->robots.end())
+            return;
+
+          command_it->second->complete_robot_action();
+        }
+      });
+
   // Parameters required for task planner
   // Battery system
   auto battery_system_optional = rmf_fleet_adapter::get_battery_system(
@@ -1065,6 +1140,20 @@ std::shared_ptr<Connections> make_fleet(
       return false;
     });
 
+  const auto consider =
+    [](const nlohmann::json& /*description*/,
+      rmf_fleet_adapter::agv::FleetUpdateHandle::Confirmation& confirm)
+    {
+      // We accept all actions since full_control may be used for different
+      // types of robots.
+      confirm.accept();
+    };
+
+  // Configure this fleet to perform any kind of teleop action
+  connections->fleet->add_performable_action(
+    "teleop",
+    consider);
+
   if (node->declare_parameter<bool>("disable_delay_threshold", false))
   {
     connections->fleet->default_maximum_delay(rmf_utils::nullopt);
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
index bae68e1b1..80a6b5078 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
@@ -424,7 +424,6 @@ void TaskManager::ActiveTask::publish_task_state(TaskManager& mgr)
 
   const auto& booking = *_task->tag()->booking();
   copy_booking_data(_state_msg["booking"], booking);
-
   const auto& header = _task->tag()->header();
   _state_msg["category"] = header.category();
   _state_msg["detail"] = header.detail();
@@ -436,7 +435,6 @@ void TaskManager::ActiveTask::publish_task_state(TaskManager& mgr)
   copy_assignment(_state_msg["assigned_to"], *mgr._context);
   _state_msg["status"] =
     status_to_string(_task->status_overview());
-
   auto& phases = _state_msg["phases"];
 
   nlohmann::json task_logs;
@@ -450,7 +448,6 @@ void TaskManager::ActiveTask::publish_task_state(TaskManager& mgr)
     const auto& snapshot = completed->snapshot();
     auto& phase = copy_phase_data(
       phases, *snapshot, mgr._log_reader, phase_logs);
-
     phase["unix_millis_start_time"] =
       completed->start_time().time_since_epoch().count();
 
@@ -462,8 +459,9 @@ void TaskManager::ActiveTask::publish_task_state(TaskManager& mgr)
   _state_msg["completed"] = std::move(completed_ids);
 
   const auto active_phase = _task->active_phase();
+  if (active_phase == nullptr)
+    return;
   copy_phase_data(phases, *active_phase, mgr._log_reader, phase_logs);
-
   _state_msg["active"] = active_phase->tag()->id();
 
   std::vector<uint64_t> pending_ids;
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
index c312e77fb..8dba67e16 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
@@ -31,6 +31,7 @@
 #include "../tasks/ChargeBattery.hpp"
 #include "../tasks/Compose.hpp"
 #include "../events/GoToPlace.hpp"
+#include "../events/PerformAction.hpp"
 
 #include <rmf_task/Constraints.hpp>
 #include <rmf_task/Parameters.hpp>
@@ -114,6 +115,9 @@ TaskDeserialization::TaskDeserialization()
   task = std::make_shared<DeserializeJSON<DeserializedTask>>();
   phase = std::make_shared<DeserializeJSON<DeserializedPhase>>();
   event = std::make_shared<DeserializeJSON<DeserializedEvent>>();
+  consider_actions =
+    std::make_shared<std::unordered_map<
+      std::string, FleetUpdateHandle::ConsiderRequest>>();
 
   _schema_dictionary = std::make_shared<SchemaDictionary>();
   _loader = [dict = _schema_dictionary](
@@ -277,8 +281,8 @@ void FleetUpdateHandle::Implementation::bid_notice_cb(
 
   std::vector<std::string> errors;
 
-  rmf_traffic::Time earliest_start_time =
-    rmf_traffic::Time(rmf_traffic::Duration::min());
+  rmf_traffic::Time earliest_start_time = rmf_traffic_ros2::convert(
+    node->get_clock()->now());
   const auto t_it = request_msg.find("unix_millis_earliest_start_time");
   if (t_it != request_msg.end())
   {
@@ -918,9 +922,9 @@ PlaceDeserializer make_place_deserializer(
 
       if (msg.is_object())
       {
-        const auto& ori_json = msg["orientation"];
-        if (ori_json)
-          place->orientation(ori_json.get<double>());
+        const auto& ori_it = msg.find("orientation");
+        if (ori_it != msg.end())
+          place->orientation(ori_it->get<double>());
       }
 
       return {place, {}};
@@ -942,6 +946,7 @@ void FleetUpdateHandle::Implementation::add_standard_tasks()
     *activation.phase, activation.event);
 
   events::GoToPlace::add(*activation.event);
+  events::PerformAction::add(*activation.event);
   deserialization.place = make_place_deserializer(planner);
   deserialization.add_schema(schemas::Place);
 
@@ -974,6 +979,26 @@ void FleetUpdateHandle::Implementation::add_standard_tasks()
     node->clock());
 }
 
+//==============================================================================
+FleetUpdateHandle& FleetUpdateHandle::add_performable_action(
+  const std::string& category,
+  ConsiderRequest consider)
+{
+  if (category.empty())
+  {
+    RCLCPP_ERROR(
+      _pimpl->node->get_logger(),
+      "FleetUpdateHandle::add_performable_action(~) called with empty category"
+    );
+    return *this;
+  }
+
+  _pimpl->deserialization.consider_actions->insert_or_assign(
+    category, consider);
+
+  return *this;
+}
+
 //==============================================================================
 auto FleetUpdateHandle::Implementation::aggregate_expectations() const
 -> Expectations
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp
index 94132e975..93b3a045d 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp
@@ -393,6 +393,28 @@ uint32_t RobotContext::current_mode() const
   return _current_mode;
 }
 
+//==============================================================================
+void RobotContext::action_executor(
+  RobotUpdateHandle::ActionExecutor action_executor)
+{
+  if (action_executor == nullptr)
+  {
+    RCLCPP_WARN(
+      _node->get_logger(),
+      "ActionExecutor set to nullptr for robot [%s]. If this robot needs to "
+      "perform an action as part of a task, a critical task error will be "
+      "thrown.",
+      this->name().c_str());
+  }
+  _action_executor = action_executor;
+}
+
+//==============================================================================
+RobotUpdateHandle::ActionExecutor RobotContext::action_executor() const
+{
+  return _action_executor;
+}
+
 //==============================================================================
 RobotContext::RobotContext(
   std::shared_ptr<RobotCommandHandle> command_handle,
@@ -433,6 +455,8 @@ RobotContext::RobotContext(
   _battery_soc_obs = _battery_soc_publisher.get_observable();
 
   _current_mode = rmf_fleet_msgs::msg::RobotMode::MODE_IDLE;
+
+  _action_executor = nullptr;
 }
 
 } // namespace agv
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp
index 075fb8878..112ed9f2a 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp
@@ -187,6 +187,14 @@ class RobotContext
   /// Return the current mode of the robot
   uint32_t current_mode() const;
 
+  /// Set the action executor for requesting this robot to execute a
+  /// PerformAction activity
+  void action_executor(RobotUpdateHandle::ActionExecutor action_executor);
+
+  /// Get the action executor for requesting this robot to execute a
+  /// PerformAction activity
+  RobotUpdateHandle::ActionExecutor action_executor() const;
+
 private:
   friend class FleetUpdateHandle;
   friend class RobotUpdateHandle;
@@ -241,6 +249,8 @@ class RobotContext
 
   // Mode value for RobotMode message
   uint32_t _current_mode;
+
+  RobotUpdateHandle::ActionExecutor _action_executor;
 };
 
 using RobotContextPtr = std::shared_ptr<RobotContext>;
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp
index 3b3caf5f0..324194719 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp
@@ -201,6 +201,20 @@ void RobotUpdateHandle::update_battery_soc(const double battery_soc)
   }
 }
 
+//==============================================================================
+void RobotUpdateHandle::set_action_executor(
+  RobotUpdateHandle::ActionExecutor action_executor)
+{
+  if (const auto context = _pimpl->get_context())
+  {
+    context->worker().schedule(
+      [context, action_executor](const auto&)
+      {
+        context->action_executor(action_executor);
+      });
+  }
+}
+
 //==============================================================================
 RobotUpdateHandle& RobotUpdateHandle::maximum_delay(
   rmf_utils::optional<rmf_traffic::Duration> value)
@@ -272,5 +286,43 @@ void RobotUpdateHandle::Unstable::set_lift_entry_watchdog(
   }
 }
 
+//==============================================================================
+void RobotUpdateHandle::ActionExecution::update_remaining_time(
+    rmf_traffic::Duration remaining_time_estimate)
+{
+  _pimpl->data->remaining_time = remaining_time_estimate;
+}
+
+//==============================================================================
+void RobotUpdateHandle::ActionExecution::finished()
+{
+  if (!_pimpl->data->finished)
+    return;
+
+  _pimpl->data->finished();
+  _pimpl->data->finished = nullptr;
+}
+
+//==============================================================================
+bool RobotUpdateHandle::ActionExecution::okay() const
+{
+  return _pimpl->data->okay;
+}
+
+//==============================================================================
+RobotUpdateHandle::ActionExecution::ActionExecution()
+{
+  // Do nothing
+}
+
+//==============================================================================
+RobotUpdateHandle::ActionExecution::~ActionExecution()
+{
+  // Automatically trigger finished when this object dies
+  if (_pimpl->data->finished)
+    _pimpl->data->finished();
+}
+
+
 } // namespace agv
 } // namespace rmf_fleet_adapter
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
index 74be3dc03..19c539750 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
@@ -33,6 +33,7 @@
 #include <rmf_task_sequence/Task.hpp>
 #include <rmf_task_sequence/Phase.hpp>
 #include <rmf_task_sequence/Event.hpp>
+#include <rmf_task_sequence/events/PerformAction.hpp>
 
 #include <rmf_fleet_msgs/msg/dock_summary.hpp>
 
@@ -61,6 +62,7 @@
 #include <rmf_api_msgs/schemas/robot_state.hpp>
 #include <rmf_api_msgs/schemas/location_2D.hpp>
 
+#include <rmf_fleet_adapter/schemas/event_description_PerformAction.hpp>
 
 #include <iostream>
 #include <unordered_set>
@@ -134,6 +136,9 @@ struct TaskDeserialization
   std::shared_ptr<FleetUpdateHandle::ConsiderRequest> consider_clean;
   std::shared_ptr<FleetUpdateHandle::ConsiderRequest> consider_patrol;
   std::shared_ptr<FleetUpdateHandle::ConsiderRequest> consider_composed;
+  // Map category string to its ConsiderRequest for PerformAction events
+  std::shared_ptr<std::unordered_map<
+    std::string, FleetUpdateHandle::ConsiderRequest>> consider_actions;
 
   void add_schema(const nlohmann::json& schema);
 
@@ -396,6 +401,82 @@ class FleetUpdateHandle::Implementation
         handle->weak_from_this());
     }
 
+    // Add PerformAction event to deserialization
+    auto validator = handle->_pimpl->deserialization.make_validator_shared(
+      schemas::event_description_PerformAction);
+
+    const auto deserializer =
+      [validator,
+      place = handle->_pimpl->deserialization.place,
+      consider_actions = handle->_pimpl->deserialization.consider_actions](
+        const nlohmann::json& msg) -> DeserializedEvent
+      {
+        validator->validate(msg);
+        try
+        {
+          const std::string& category = msg["category"].get<std::string>();
+          const auto consider_action_it = consider_actions->find(category);
+          if (consider_action_it == consider_actions->end())
+          {
+            return {nullptr, {"Fleet not configured to perform this action"}};
+          }
+          Confirmation confirm;
+          const auto& consider = consider_action_it->second;
+          consider(msg, confirm);
+          if (!confirm.is_accepted())
+          {
+            return {nullptr, confirm.errors()};
+          }
+
+          const nlohmann::json desc = msg["description"];
+          rmf_traffic::Duration duration_estimate = rmf_traffic::Duration(0);
+          bool use_tool_sink = false;
+          std::optional<rmf_traffic::agv::Planner::Goal> finish_location =
+            std::nullopt;
+          auto it = msg.find("unix_millis_action_duration_estimate");
+          if (it != msg.end())
+          {
+            duration_estimate =
+              rmf_traffic::Duration(
+              std::chrono::milliseconds(it->get<uint64_t>()));
+          }
+          it = msg.find("use_tool_sink");
+          if (it != msg.end())
+          {
+            use_tool_sink = it->get<bool>();
+          }
+          it = msg.find("expected_finish_location");
+          if (it != msg.end())
+          {
+            auto deser_place =
+              place(msg["expected_finish_location"]);
+            if (!deser_place.description.has_value())
+            {
+              return {nullptr, deser_place.errors};
+            }
+            finish_location = deser_place.description.value();
+          }
+
+          const auto description =
+            rmf_task_sequence::events::PerformAction::Description::make(
+              category,
+              desc,
+              duration_estimate,
+              use_tool_sink,
+              finish_location);
+
+          std::vector<std::string> errors = {};
+          return {description, errors};
+        }
+        catch(const std::exception& e)
+        {
+          return {nullptr, {e.what()}};
+        }
+      };
+
+    handle->_pimpl->deserialization.event->add(
+      "perform_action", validator, deserializer);
+
     return handle;
   }
 
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp
index 2793c27f0..e3a86f5d0 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp
@@ -25,10 +25,44 @@ namespace rmf_fleet_adapter {
 namespace agv {
 
 //==============================================================================
-class RobotUpdateHandle::Implementation
+class RobotUpdateHandle::ActionExecution::Implementation
 {
 public:
 
+  struct Data
+  {
+
+    std::function<void()> finished;
+    std::optional<rmf_traffic::Duration> remaining_time;
+    bool okay;
+    // TODO: Consider adding a mutex to lock read/write
+
+    Data(
+      std::function<void()> finished_,
+      std::optional<rmf_traffic::Duration> remaining_time_ = std::nullopt)
+    {
+      finished = std::move(finished_);
+      remaining_time = remaining_time_;
+      okay = true;
+    }
+  };
+
+  static ActionExecution make(std::shared_ptr<Data> data)
+  {
+    ActionExecution execution;
+    execution._pimpl = rmf_utils::make_impl<Implementation>(
+      Implementation{std::move(data)});
+
+    return execution;
+  }
+
+  std::shared_ptr<Data> data;
+};
+
+//==============================================================================
+class RobotUpdateHandle::Implementation
+{
+public:
   std::weak_ptr<RobotContext> context;
   std::string name;
   RobotUpdateHandle::Unstable unstable = RobotUpdateHandle::Unstable();
@@ -59,7 +93,6 @@ class RobotUpdateHandle::Implementation
   std::shared_ptr<RobotContext> get_context();
 
   std::shared_ptr<const RobotContext> get_context() const;
-
 };
 
 } // namespace agv
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp
index b1e56e9f6..354eee4db 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp
@@ -23,7 +23,7 @@ namespace rmf_fleet_adapter {
 namespace events {
 
 //==============================================================================
-void GoToPlace::add(rmf_task_sequence::Event::Initializer &initializer)
+void GoToPlace::add(rmf_task_sequence::Event::Initializer& initializer)
 {
   initializer.add<Description>(
     [](
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/PerformAction.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/PerformAction.cpp
new file mode 100644
index 000000000..3081b60ae
--- /dev/null
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/PerformAction.cpp
@@ -0,0 +1,269 @@
+/*
+ * Copyright (C) 2022 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 "PerformAction.hpp"
+
+#include <rmf_traffic_ros2/Time.hpp>
+
+namespace rmf_fleet_adapter {
+namespace events {
+
+//==============================================================================
+void PerformAction::add(rmf_task_sequence::Event::Initializer& initializer)
+{
+  initializer.add<Description>(
+    [](
+      const AssignIDPtr& id,
+      const std::function<rmf_task::State()>& get_state,
+      const rmf_task::ConstParametersPtr& parameters,
+      const Description& description,
+      std::function<void()> update) -> StandbyPtr
+    {
+      return Standby::make(
+        id, get_state, parameters, description, std::move(update));
+    },
+    [](
+      const AssignIDPtr& id,
+      const std::function<rmf_task::State()>& get_state,
+      const rmf_task::ConstParametersPtr& parameters,
+      const Description& description,
+      const nlohmann::json&,
+      std::function<void()> update,
+      std::function<void()> checkpoint,
+      std::function<void()> finished) -> ActivePtr
+    {
+      return Standby::make(
+        id, get_state, parameters, description, std::move(update))
+        ->begin(std::move(checkpoint), std::move(finished));
+    });
+}
+
+//==============================================================================
+auto PerformAction::Standby::make(
+  const AssignIDPtr& id,
+  const std::function<rmf_task::State()>& get_state,
+  const rmf_task::ConstParametersPtr& parameters,
+  const rmf_task_sequence::events::PerformAction::Description& description,
+  std::function<void()> update) -> std::shared_ptr<Standby>
+{
+  const auto state = get_state();
+  const auto context = state.get<agv::GetContext>()->value;
+  const auto header = description.generate_header(state, *parameters);
+
+  auto standby = std::make_shared<Standby>(Standby{description});
+  standby->_assign_id = id;
+  standby->_context = context;
+  standby->_time_estimate = header.original_duration_estimate();
+  standby->_update = std::move(update);
+  standby->_state = rmf_task::events::SimpleEventState::make(
+    id->assign(),
+    header.category(),
+    header.detail(),
+    rmf_task::Event::Status::Standby,
+    {},
+    context->clock());
+
+  return standby;
+}
+
+//==============================================================================
+PerformAction::Standby::Standby(
+  rmf_task_sequence::events::PerformAction::Description description)
+: _description(std::move(description))
+{
+  // Do nothing
+}
+
+//==============================================================================
+auto PerformAction::Standby::state() const -> ConstStatePtr
+{
+  return _state;
+}
+
+//==============================================================================
+rmf_traffic::Duration PerformAction::Standby::duration_estimate() const
+{
+  return _time_estimate;
+}
+
+//==============================================================================
+auto PerformAction::Standby::begin(
+  std::function<void()>,
+  std::function<void()> finished) -> ActivePtr
+{
+  if (!_active)
+  {
+    _active = Active::make(
+      _assign_id,
+      _context,
+      _description.category(),
+      _description.description(),
+      _time_estimate,
+      _state,
+      _update,
+      std::move(finished));
+  }
+
+  return _active;
+}
+
+//==============================================================================
+auto PerformAction::Active::make(
+  const AssignIDPtr& id,
+  agv::RobotContextPtr context,
+  const std::string& category,
+  nlohmann::json desc,
+  rmf_traffic::Duration time_estimate,
+  rmf_task::events::SimpleEventStatePtr state,
+  std::function<void()> update,
+  std::function<void()> finished) -> std::shared_ptr<Active>
+{
+  auto active = std::make_shared<Active>(
+    Active(std::move(category), std::move(desc), time_estimate));
+  active->_assign_id = id;
+  active->_context = std::move(context);
+  active->_update = std::move(update);
+  active->_finished = std::move(finished);
+  active->_state = std::move(state);
+  active->_execute_action();
+  active->_expected_finish_time =
+    active->_context->now() + time_estimate;
+  return active;
+}
+
+//==============================================================================
+PerformAction::Active::Active(
+  const std::string& category,
+  nlohmann::json desc,
+  rmf_traffic::Duration time_estimate)
+: _action_category(std::move(category)),
+  _action_description(std::move(desc)),
+  _time_estimate(std::move(time_estimate))
+{
+  // Do nothing
+}
+
+//==============================================================================
+auto PerformAction::Active::state() const -> ConstStatePtr
+{
+  return _state;
+}
+
+//==============================================================================
+rmf_traffic::Duration PerformAction::Active::remaining_time_estimate() const
+{
+  auto execution = _execution_data.lock();
+  if (!execution)
+    return rmf_traffic::Duration(0);
+
+  if (execution->remaining_time.has_value())
+    return execution->remaining_time.value();
+
+  // If an estimate is not provided we compute one based on the expected finish
+  // time
+  const auto estimate =
+    std::max(rmf_traffic::Duration(0),
+      _expected_finish_time - _context->now());
+  return estimate;
+}
+
+//==============================================================================
+auto PerformAction::Active::backup() const -> Backup
+{
+  // TODO: Consider adding a function to ActionExecution that allows the
+  // system integrator to periodically send in backup messages. For now, we
+  // do not generate backup messages.
+  return Backup::make(0, nlohmann::json());
+}
+
+//==============================================================================
+auto PerformAction::Active::interrupt(
+  std::function<void()> task_is_interrupted) -> Resume
+{
+  _state->update_status(Status::Standby);
+  _state->update_log().info("Going into standby for an interruption");
+  _state->update_dependencies({});
+
+  _context->worker().schedule(
+    [task_is_interrupted](const auto&)
+    {
+      task_is_interrupted();
+    });
+
+  // TODO(YV): Currently we will ask the robot to perform the action again when
+  // resumed. Future work can be to receive a callback from RobotUpdateHandle
+  // which can be used to ask the robot to resume from its interrupted state.
+  // This callback can be accepted by RobotUpdateHandle::ActionExecution
+  return Resume::make(
+    [w = weak_from_this()]()
+    {
+      if (const auto self = w.lock())
+      {
+        self->_execute_action();
+      }
+    });
+}
+
+//==============================================================================
+void PerformAction::Active::cancel()
+{
+  _state->update_status(Status::Canceled);
+  _state->update_log().info("Received signal to cancel");
+  _finished();
+  if (auto data = _execution_data.lock())
+    data->okay = false;
+}
+
+//==============================================================================
+void PerformAction::Active::kill()
+{
+  _state->update_status(Status::Killed);
+  _state->update_log().info("Received signal to kill");
+  _finished();
+  if (auto data = _execution_data.lock())
+    data->okay = false;
+}
+
+//==============================================================================
+void PerformAction::Active::_execute_action()
+{
+  auto action_executor = _context->action_executor();
+  if (action_executor == nullptr)
+  {
+    // The action_executor has not been set by the user
+    _state->update_status(Status::Error);
+    const std::string msg = "ActionExecutor not set via RobotUpdateHandle. "
+      "Unable to perform the requested action.";
+    _state->update_log().error(msg);
+    _finished();
+    return;
+  }
+
+  auto data = std::make_shared<ExecutionData>(_finished, std::nullopt);
+  _execution_data = data;
+
+  auto action_execution =
+    agv::RobotUpdateHandle::ActionExecution::Implementation::make(data);
+
+  action_executor(
+    _action_category,
+    _action_description,
+    std::move(action_execution));
+}
+
+} // namespace events
+} // namespace rmf_fleet_adapter
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/PerformAction.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/PerformAction.hpp
new file mode 100644
index 000000000..b8f275c2e
--- /dev/null
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/PerformAction.hpp
@@ -0,0 +1,134 @@
+/*
+ * Copyright (C) 2022 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 SRC__RMF_FLEET_ADAPTER__EVENTS__PERFORMACTION_HPP
+#define SRC__RMF_FLEET_ADAPTER__EVENTS__PERFORMACTION_HPP
+
+#include "../agv/internal_RobotUpdateHandle.hpp"
+#include "../agv/RobotContext.hpp"
+
+#include <rmf_task_sequence/Event.hpp>
+#include <rmf_task_sequence/events/PerformAction.hpp>
+#include <rmf_task/events/SimpleEventState.hpp>
+
+#include <nlohmann/json.hpp>
+
+namespace rmf_fleet_adapter {
+namespace events {
+
+//==============================================================================
+class PerformAction : public rmf_task_sequence::Event
+{
+public:
+
+  using Description = rmf_task_sequence::events::PerformAction::Description;
+
+  static void add(rmf_task_sequence::Event::Initializer& initializer);
+
+  class Standby : public rmf_task_sequence::Event::Standby
+  {
+  public:
+
+    static std::shared_ptr<Standby> make(
+      const AssignIDPtr& id,
+      const std::function<rmf_task::State()>& get_state,
+      const rmf_task::ConstParametersPtr& parameters,
+      const rmf_task_sequence::events::PerformAction::Description& description,
+      std::function<void()> update);
+
+    /// Documentation inherited
+    ConstStatePtr state() const final;
+
+    /// Documentation inherited
+    rmf_traffic::Duration duration_estimate() const final;
+
+    /// Documentation inherited
+    ActivePtr begin(
+      std::function<void()> checkpoint,
+      std::function<void()> finished) final;
+
+  private:
+
+    Standby(rmf_task_sequence::events::PerformAction::Description description);
+
+    rmf_task_sequence::events::PerformAction::Description _description;
+    AssignIDPtr _assign_id;
+    agv::RobotContextPtr _context;
+    rmf_traffic::Duration _time_estimate;
+    std::function<void()> _update;
+    rmf_task::events::SimpleEventStatePtr _state;
+    ActivePtr _active = nullptr;
+  };
+
+  class Active
+    : public rmf_task_sequence::Event::Active,
+      public std::enable_shared_from_this<Active>
+  {
+  public:
+
+    using ExecutionData =
+      agv::RobotUpdateHandle::ActionExecution::Implementation::Data;
+
+    static std::shared_ptr<Active> make(
+      const AssignIDPtr& id,
+      agv::RobotContextPtr context,
+      const std::string& action_category,
+      nlohmann::json action_description,
+      rmf_traffic::Duration _time_estimate,
+      rmf_task::events::SimpleEventStatePtr state,
+      std::function<void()> update,
+      std::function<void()> finished);
+
+    ConstStatePtr state() const final;
+
+    rmf_traffic::Duration remaining_time_estimate() const final;
+
+    Backup backup() const final;
+
+    Resume interrupt(std::function<void()> task_is_interrupted) final;
+
+    void cancel() final;
+
+    void kill() final;
+
+  private:
+
+    Active(
+      const std::string& action_category,
+      nlohmann::json action_description,
+      rmf_traffic::Duration time_estimate);
+
+    void _execute_action();
+
+    AssignIDPtr _assign_id;
+    agv::RobotContextPtr _context;
+    std::string _action_category;
+    nlohmann::json _action_description;
+    rmf_traffic::Duration _time_estimate;
+    std::function<void()> _update;
+    std::function<void()> _finished;
+    rmf_task::events::SimpleEventStatePtr _state;
+    rmf_traffic::Time _expected_finish_time;
+    std::shared_ptr<void> _be_stubborn;
+    std::weak_ptr<ExecutionData> _execution_data;
+  };
+};
+
+} // namespace events
+} // namespace rmf_fleet_adapter
+
+#endif // SRC__RMF_FLEET_ADAPTER__EVENTS__PERFORMACTION_HPP
diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
index edaab78e9..5e95d2227 100644
--- a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
+++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
@@ -725,6 +725,18 @@ class Dispatcher::Implementation
       if (on_change_fn)
         on_change_fn(*dispatch_state);
 
+      // Print the errors
+      std::size_t error_count = 1;
+      for (const auto& error : errors)
+      {
+        RCLCPP_ERROR(
+          node->get_logger(),
+          "No submission error[%d]: %s",
+          error_count,
+          error.c_str());
+        ++error_count;
+      }
+
       return;
     }
 

From 2f41a93c968dda65a6942f68fa8f094fdc275fda Mon Sep 17 00:00:00 2001
From: Yadu <yadunund@openrobotics.org>
Date: Thu, 27 Jan 2022 21:27:44 +0900
Subject: [PATCH 66/79] Fix Compose Tasks (#159)

* Validate perform_action description inside try block

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Add deserialized activity to builder

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Cleanup

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Add error when on_cancel is not an array

Signed-off-by: Yadunund <yadunund@openrobotics.org>
---
 .../agv/internal_FleetUpdateHandle.hpp        |  2 +-
 .../src/rmf_fleet_adapter/tasks/Compose.cpp   | 43 ++++++++++++-------
 2 files changed, 28 insertions(+), 17 deletions(-)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
index 19c539750..147c6f7c1 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
@@ -411,9 +411,9 @@ class FleetUpdateHandle::Implementation
       consider_actions = handle->_pimpl->deserialization.consider_actions](
         const nlohmann::json& msg) -> DeserializedEvent
       {
-        validator->validate(msg);
         try
         {
+          validator->validate(msg);
           const std::string& category = msg["category"].get<std::string>();
           const auto consider_action_it = consider_actions->find(category);
           if (consider_action_it == consider_actions->end())
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Compose.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Compose.cpp
index 6982afc9e..737d69557 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Compose.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Compose.cpp
@@ -126,26 +126,37 @@ void add_compose(
           errors.end(),
           phase_activity.errors.begin(), phase_activity.errors.end());
 
-        std::vector<rmf_task_sequence::Phase::ConstDescriptionPtr> cancel;
-        const auto& on_cancel_json = phase_json["on_cancel"];
-        if (on_cancel_json.is_array())
+        std::vector<rmf_task_sequence::Phase::ConstDescriptionPtr> cancel = {};
+        const auto cancel_it = phase_json.find("on_cancel");
+        if (cancel_it != phase_json.end())
         {
-          cancel.reserve(on_cancel_json.size());
-          for (const auto& cancellation_activity_json : on_cancel_json)
+          if (cancel_it.value().is_array())
           {
-            auto cancellation_activity =
-              deser_activity(cancellation_activity_json);
-
-            if (!cancellation_activity.description)
-              return {nullptr, std::move(cancellation_activity.errors)};
-
-            cancel.push_back(cancellation_activity.description);
-            errors.insert(
-              errors.end(),
-              cancellation_activity.errors.begin(),
-              cancellation_activity.errors.end());
+            const auto& on_cancel_json = cancel_it.value();
+            cancel.reserve(on_cancel_json.size());
+            for (const auto& cancellation_activity_json : on_cancel_json)
+            {
+              auto cancellation_activity =
+                deser_activity(cancellation_activity_json);
+
+              if (!cancellation_activity.description)
+                return {nullptr, std::move(cancellation_activity.errors)};
+
+              cancel.push_back(cancellation_activity.description);
+              errors.insert(
+                errors.end(),
+                cancellation_activity.errors.begin(),
+                cancellation_activity.errors.end());
+            }
+          }
+          else
+          {
+            errors.push_back("The on_cancel property for one of the phases is "
+            "not an array. The intended activities will not be performed.");
           }
         }
+
+        builder.add_phase(phase_activity.description, cancel);
       }
 
       agv::FleetUpdateHandle::Confirmation confirm;

From 1c33ab9f722732917bb7467395f5a929154180d5 Mon Sep 17 00:00:00 2001
From: Yadunund <yadunund@openrobotics.org>
Date: Fri, 28 Jan 2022 11:04:33 +0800
Subject: [PATCH 67/79] Add sqlite3 dependency

Signed-off-by: Yadunund <yadunund@openrobotics.org>
---
 rmf_fleet_adapter/CMakeLists.txt                               | 3 +++
 rmf_fleet_adapter/package.xml                                  | 3 ++-
 .../src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp   | 2 ++
 rmf_task_ros2/package.xml                                      | 2 +-
 4 files changed, 8 insertions(+), 2 deletions(-)

diff --git a/rmf_fleet_adapter/CMakeLists.txt b/rmf_fleet_adapter/CMakeLists.txt
index 843c9fb4d..7753acdfa 100644
--- a/rmf_fleet_adapter/CMakeLists.txt
+++ b/rmf_fleet_adapter/CMakeLists.txt
@@ -42,6 +42,7 @@ set(dep_pkgs
   websocketpp
   nlohmann_json
   nlohmann_json_schema_validator_vendor
+  SQLite3
 )
 foreach(pkg ${dep_pkgs})
   find_package(${pkg} REQUIRED)
@@ -92,6 +93,7 @@ target_link_libraries(rmf_fleet_adapter
     ${rmf_ingestor_msgs_LIBRARIES}
     ${websocketpp_LIBRARIES}
     nlohmann_json_schema_validator
+    ${SQLite3_LIBRARIES}
 )
 
 target_include_directories(rmf_fleet_adapter
@@ -112,6 +114,7 @@ target_include_directories(rmf_fleet_adapter
     ${rmf_api_msgs_INCLUDE_DIRS}
     ${WEBSOCKETPP_INCLUDE_DIR}
     ${nlohmann_json_schema_validator_INCLUDE_DIRS}
+    ${SQLite3_INCLUDE_DIRS}
 )
 
 if (BUILD_TESTING)
diff --git a/rmf_fleet_adapter/package.xml b/rmf_fleet_adapter/package.xml
index 119d6463a..8ac7f0aa7 100644
--- a/rmf_fleet_adapter/package.xml
+++ b/rmf_fleet_adapter/package.xml
@@ -32,8 +32,9 @@
   <depend condition="$RMF_ENABLE_FAILOVER == 1">stubborn_buddies_msgs</depend>
 
   <depend>libwebsocketpp-dev</depend>
-  <depend>nlohmann-json3-dev</depend>
+  <depend>nlohmann-json-dev</depend>
   <depend>nlohmann_json_schema_validator_vendor</depend>
+  <depend>libsqlite3-dev</depend>
 
   <build_depend>eigen</build_depend>
   <build_depend>yaml-cpp</build_depend>
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
index 147c6f7c1..4861430a2 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
@@ -64,6 +64,8 @@
 
 #include <rmf_fleet_adapter/schemas/event_description_PerformAction.hpp>
 
+#include <sqlite3.h>
+
 #include <iostream>
 #include <unordered_set>
 #include <optional>
diff --git a/rmf_task_ros2/package.xml b/rmf_task_ros2/package.xml
index 3e4e87a61..1945557a9 100644
--- a/rmf_task_ros2/package.xml
+++ b/rmf_task_ros2/package.xml
@@ -16,7 +16,7 @@
   <depend>rmf_traffic_ros2</depend>
   <depend>rmf_task_msgs</depend>
   <depend>rclcpp</depend>
-  <depend>nlohmann-json3-dev</depend>
+  <depend>nlohmann-json-dev</depend>
   <depend>nlohmann_json_schema_validator_vendor</depend>
 
   <build_depend>eigen</build_depend>

From 0a6193c156bb07b67411485ccea8420c4b536936 Mon Sep 17 00:00:00 2001
From: Yadunund <yadunund@openrobotics.org>
Date: Fri, 28 Jan 2022 11:38:54 +0800
Subject: [PATCH 68/79] Fix dispatch response

Signed-off-by: Yadunund <yadunund@openrobotics.org>
---
 rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
index 5e95d2227..9a5fb8d39 100644
--- a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
+++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp
@@ -465,7 +465,7 @@ class Dispatcher::Implementation
 
       auto response = rmf_task_msgs::build<ApiResponseMsg>()
         .type(ApiResponseMsg::TYPE_RESPONDING)
-        .json_msg(task_state.dump())
+        .json_msg(response_json.dump())
         .request_id(msg.request_id);
 
       api_memory.add(response);

From 53b257ef7c375eb0505d306ce26b1d0216aef5b0 Mon Sep 17 00:00:00 2001
From: youliang <tan_you_liang@hotmail.com>
Date: Fri, 28 Jan 2022 11:41:52 +0800
Subject: [PATCH 69/79] check priority json keys

Signed-off-by: youliang <tan_you_liang@hotmail.com>
---
 .../agv/FleetUpdateHandle.cpp                 | 19 +++++++++++--------
 1 file changed, 11 insertions(+), 8 deletions(-)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
index 8dba67e16..d7164ebc9 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
@@ -294,17 +294,20 @@ void FleetUpdateHandle::Implementation::bid_notice_cb(
   const auto p_it = request_msg.find("priority");
   if (p_it != request_msg.end())
   {
-    const auto& p_type = (*p_it)["type"];
-    if (p_type.is_string() && p_type.get<std::string>() == "binary")
+    if (p_it->contains("type") && p_it->contains("value"))
     {
-      const auto& p_value = (*p_it)["value"];
-      if (p_value.is_number_integer())
+      const auto& p_type = (*p_it)["type"];
+      if (p_type.is_string() && p_type.get<std::string>() == "binary")
       {
-        if (p_value.is_number_integer() && p_value.get<uint64_t>() > 0)
-          priority = rmf_task::BinaryPriorityScheme::make_high_priority();
-      }
+        const auto& p_value = (*p_it)["value"];
+        if (p_value.is_number_integer())
+        {
+          if (p_value.is_number_integer() && p_value.get<uint64_t>() > 0)
+            priority = rmf_task::BinaryPriorityScheme::make_high_priority();
+        }
 
-      priority = rmf_task::BinaryPriorityScheme::make_low_priority();
+        priority = rmf_task::BinaryPriorityScheme::make_low_priority();
+      }
     }
 
     if (!priority)

From 012d51f3f0e3cb6e62cbea06568027a3503d521c Mon Sep 17 00:00:00 2001
From: Yadunund <yadunund@openrobotics.org>
Date: Fri, 28 Jan 2022 15:49:27 +0800
Subject: [PATCH 70/79] WIP

Signed-off-by: Yadunund <yadunund@openrobotics.org>
---
 .../include/rmf_fleet_adapter/agv/Adapter.hpp |  9 ++-
 .../src/rmf_fleet_adapter/DatabaseLogger.hpp  | 61 +++++++++++++++++++
 .../src/rmf_fleet_adapter/agv/Adapter.cpp     |  7 ++-
 .../agv/internal_FleetUpdateHandle.hpp        |  4 +-
 4 files changed, 77 insertions(+), 4 deletions(-)
 create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.hpp

diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp
index ca39142f4..46185df82 100644
--- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp
+++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp
@@ -97,11 +97,18 @@ class Adapter : public std::enable_shared_from_this<Adapter>
   /// \param[in] server_uri
   ///   Specify the URI for the websocket server that receives updates on tasks
   ///   and states. If nullopt, data will not be published.
+  ///
+  /// \param[in] backup_filename
+  ///   Loads and logs backup data to the specified file. If the file already,
+  ///   exists the fleet handle will be restored to the last backup state. If
+  ///   the file does not exist, a new one will be created.
+  ///   If nullopt, the fleet adapter will not perform any backup/restore.
   std::shared_ptr<FleetUpdateHandle> add_fleet(
     const std::string& fleet_name,
     rmf_traffic::agv::VehicleTraits traits,
     rmf_traffic::agv::Graph navigation_graph,
-    std::optional<std::string> server_uri = std::nullopt);
+    std::optional<std::string> server_uri = std::nullopt,
+    std::optional<std::string> backup_filename = std::nullopt);
 
   /// Create a traffic light to help manage robots that can only support pause
   /// and resume commands.
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.hpp
new file mode 100644
index 000000000..648b1a379
--- /dev/null
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.hpp
@@ -0,0 +1,61 @@
+/*
+ * Copyright (C) 2022 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 SRC__RMF_FLEET_ADAPTER__DATABASELOGGER_HPP
+#define SRC__RMF_FLEET_ADAPTER__DATABASELOGGER_HPP
+
+#include <rmf_task/TaskPlanner.hpp>
+
+#include "TaskManager.hpp"
+
+#include <sqlite3.h>
+
+#include <memory>
+
+namespace rmf_fleet_adapter {
+//==============================================================================
+// A wrapper around a sqlite3 database.
+// TODO(YV): The Logger can inherit from an abstract class so that users can
+// use whatever DBS they want.
+class DatabaseLogger
+{
+public:
+  using Assignments = rmf_task::TaskPlanner::Assignments;
+  // Bundle up the restored state of the fleet adapter
+  // TODO(YV): Ensure index of managers correspond to index of Assignments
+  struct Restored
+  {
+    std::vector<TaskManager> managers;
+    std::unordered_map<std::string, Assignments> bid_notice_assignments
+  };
+
+  static std::shared_ptr<DatabaseLogger> make(
+    const std::string& filename);
+
+
+
+  ~DatabaseLogger();
+
+private:
+  DatabaseLogger();
+  std::shared_ptr<sqlite3> db;
+
+};
+
+} // namespace rmf_fleet_adapter
+
+#endif // SRC__RMF_FLEET_ADAPTER__DATABASELOGGER_HPP
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp
index 0d375c167..ab3fcbc22 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp
@@ -31,6 +31,8 @@
 
 #include "../load_param.hpp"
 
+#include "../DatabaseLogger.hpp"
+
 namespace rmf_fleet_adapter {
 namespace agv {
 
@@ -209,7 +211,8 @@ std::shared_ptr<FleetUpdateHandle> Adapter::add_fleet(
   const std::string& fleet_name,
   rmf_traffic::agv::VehicleTraits traits,
   rmf_traffic::agv::Graph navigation_graph,
-  std::optional<std::string> server_uri)
+  std::optional<std::string> server_uri,
+  )
 {
   auto planner =
     std::make_shared<std::shared_ptr<const rmf_traffic::agv::Planner>>(
@@ -222,7 +225,7 @@ std::shared_ptr<FleetUpdateHandle> Adapter::add_fleet(
   auto fleet = FleetUpdateHandle::Implementation::make(
     fleet_name, std::move(planner), _pimpl->node, _pimpl->worker,
     _pimpl->schedule_writer, _pimpl->mirror_manager.snapshot_handle(),
-    _pimpl->negotiation, server_uri);
+    _pimpl->negotiation, server_uri, std::move(db));
 
   _pimpl->fleets.push_back(fleet);
   return fleet;
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
index 4861430a2..0c37ea917 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
@@ -64,7 +64,7 @@
 
 #include <rmf_fleet_adapter/schemas/event_description_PerformAction.hpp>
 
-#include <sqlite3.h>
+#include "../DatabaseLogger.hpp"
 
 #include <iostream>
 #include <unordered_set>
@@ -241,6 +241,8 @@ class FleetUpdateHandle::Implementation
   std::shared_ptr<rmf_traffic::schedule::Snappable> snappable;
   std::shared_ptr<rmf_traffic_ros2::schedule::Negotiation> negotiation;
   std::optional<std::string> server_uri;
+  // Database for backup and restore
+  std::shared_ptr<DatabaseLogger> db;
 
   TaskActivation activation = TaskActivation();
   TaskDeserialization deserialization = TaskDeserialization();

From c94b0adace3be63b6cc403307d6367f4c3b0e221 Mon Sep 17 00:00:00 2001
From: Yadu <yadunund@openrobotics.org>
Date: Fri, 28 Jan 2022 20:37:43 +0900
Subject: [PATCH 71/79] Update json dependency (#160)

Signed-off-by: Yadunund <yadunund@openrobotics.org>
---
 rmf_fleet_adapter/package.xml | 2 +-
 rmf_task_ros2/package.xml     | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/rmf_fleet_adapter/package.xml b/rmf_fleet_adapter/package.xml
index 119d6463a..03d675d3f 100644
--- a/rmf_fleet_adapter/package.xml
+++ b/rmf_fleet_adapter/package.xml
@@ -32,7 +32,7 @@
   <depend condition="$RMF_ENABLE_FAILOVER == 1">stubborn_buddies_msgs</depend>
 
   <depend>libwebsocketpp-dev</depend>
-  <depend>nlohmann-json3-dev</depend>
+  <depend>nlohmann-json-dev</depend>
   <depend>nlohmann_json_schema_validator_vendor</depend>
 
   <build_depend>eigen</build_depend>
diff --git a/rmf_task_ros2/package.xml b/rmf_task_ros2/package.xml
index 3e4e87a61..1945557a9 100644
--- a/rmf_task_ros2/package.xml
+++ b/rmf_task_ros2/package.xml
@@ -16,7 +16,7 @@
   <depend>rmf_traffic_ros2</depend>
   <depend>rmf_task_msgs</depend>
   <depend>rclcpp</depend>
-  <depend>nlohmann-json3-dev</depend>
+  <depend>nlohmann-json-dev</depend>
   <depend>nlohmann_json_schema_validator_vendor</depend>
 
   <build_depend>eigen</build_depend>

From b9e4a708cea83ec25f99388ac98c658b74fba053 Mon Sep 17 00:00:00 2001
From: Yadunund <yadunund@openrobotics.org>
Date: Fri, 28 Jan 2022 23:01:33 +0800
Subject: [PATCH 72/79] Added skeleton for DatabaseLogger

Signed-off-by: Yadunund <yadunund@openrobotics.org>
---
 .../include/rmf_fleet_adapter/agv/Adapter.hpp |  4 +-
 .../src/rmf_fleet_adapter/DatabaseLogger.cpp  | 92 +++++++++++++++++++
 .../src/rmf_fleet_adapter/DatabaseLogger.hpp  | 41 ++++++++-
 .../src/rmf_fleet_adapter/agv/Adapter.cpp     |  7 +-
 .../agv/internal_FleetUpdateHandle.hpp        | 10 +-
 5 files changed, 141 insertions(+), 13 deletions(-)
 create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.cpp

diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp
index 46185df82..2fecb815a 100644
--- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp
+++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp
@@ -98,7 +98,7 @@ class Adapter : public std::enable_shared_from_this<Adapter>
   ///   Specify the URI for the websocket server that receives updates on tasks
   ///   and states. If nullopt, data will not be published.
   ///
-  /// \param[in] backup_filename
+  /// \param[in] backup_file_path
   ///   Loads and logs backup data to the specified file. If the file already,
   ///   exists the fleet handle will be restored to the last backup state. If
   ///   the file does not exist, a new one will be created.
@@ -108,7 +108,7 @@ class Adapter : public std::enable_shared_from_this<Adapter>
     rmf_traffic::agv::VehicleTraits traits,
     rmf_traffic::agv::Graph navigation_graph,
     std::optional<std::string> server_uri = std::nullopt,
-    std::optional<std::string> backup_filename = std::nullopt);
+    std::optional<std::string> backup_file_path = std::nullopt);
 
   /// Create a traffic light to help manage robots that can only support pause
   /// and resume commands.
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.cpp
new file mode 100644
index 000000000..6aab6283a
--- /dev/null
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.cpp
@@ -0,0 +1,92 @@
+/*
+ * Copyright (C) 2022 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 "DatabaseLogger.hpp"
+
+#include <filesystem>
+#include <fstream>
+
+namespace rmf_fleet_adapter {
+//==============================================================================
+auto DatabaseLogger::make(const std::string& file_path) ->
+std::shared_ptr<DatabaseLogger>
+{
+  std::shared_ptr<DatabaseLogger> logger(new DatabaseLogger());
+  logger->_file_path = file_path;
+
+  if (!std::filesystem::exists(file_path))
+  {
+    std::filesystem::create_directories(
+      std::filesystem::absolute(file_path).parent_path());
+  }
+
+  auto error = sqlite3_open(file_path, *logger->_db)
+  if (error)
+    return nullptr;
+
+  // Currently we only store the state of the fleet adapter
+  // for restoration.
+  // Table TaskManagers: Robot name as key. Columns for active state json & queues
+
+
+
+  // TODO(YV): Also store history of task & state logs along with latest state.
+  // Proposed key (robot, task, phase, event, seq)
+
+  return logger;
+}
+
+//==============================================================================
+DatabaseLogger::DatabaseLogger()
+{
+  // Do nothing
+}
+
+//==============================================================================
+DatabaseLogger::DatabaseLogger()
+{
+  // Safely destruct the sqlite3 object
+  sqlite3_close(_db);
+}
+//==============================================================================
+void DatabaseLogger::backup(const BidNoticeAssignments& assignments)
+{
+
+}
+
+//==============================================================================
+void DatabaseLogger::backup(const TaskManager& mgr)
+{
+
+}
+
+//==============================================================================
+nlohmann::json DatabaseLogger::_convert(const Assignment& assignment)
+{
+  nlohmann::json msg;
+  return msg;
+}
+
+//==============================================================================
+auto DatabaseLogger::_convert(const nlohmann::json& msg) -> Assignment
+{
+  rmf_task::State state;
+  Assignment assignment{nullptr, state, rmf_traffic::Time(0)};
+  return assignment;
+}
+
+} // namespace rmf_fleet_adapter
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.hpp
index 648b1a379..3fc548a6d 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.hpp
@@ -21,10 +21,13 @@
 #include <rmf_task/TaskPlanner.hpp>
 
 #include "TaskManager.hpp"
+#include "agv/internal_FleetUpdateHandle.hpp"
 
 #include <sqlite3.h>
+#include <nlohmann/json.hpp>
 
 #include <memory>
+#include <mutex>
 
 namespace rmf_fleet_adapter {
 //==============================================================================
@@ -34,26 +37,58 @@ namespace rmf_fleet_adapter {
 class DatabaseLogger
 {
 public:
+  using Assignment = rmf_task::TaskPlanner::Assignment;
   using Assignments = rmf_task::TaskPlanner::Assignments;
+  using BidNoticeAssignments =
+    std::unordered_map<std::string, Assignments>;
   // Bundle up the restored state of the fleet adapter
   // TODO(YV): Ensure index of managers correspond to index of Assignments
+  // We should return some sort of map<robot_name, manager> which
+  // FleetUpdateHandle::add_robot() can lookup for restored task managers.
+  // But if a new robot is being added, what do we do with the
+  // previous bid_notice_assignments? The index order will be incorrect.
+  // We would need to modify the implementation of dispatch_command_cb to not
+  // return an error when the number of robots do not match the number in
+  // Assignments.
   struct Restored
   {
     std::vector<TaskManager> managers;
-    std::unordered_map<std::string, Assignments> bid_notice_assignments
+    BidNoticeAssignments bid_notice_assignments;
   };
 
   static std::shared_ptr<DatabaseLogger> make(
-    const std::string& filename);
+    const std::string& file_path);
 
+  // Returns nullopt if file_path did not exist previously
+  std::optional<Restored> restore() const;
 
+  // TODO(YV): Consider having internal_FleetUpdateHandle and TaskManager
+  // receive these functions as callbacks to trigger on updates
+  // Backup bid_notice_assignments
+  void backup(const BidNoticeAssignments& assignments);
+
+  // Backup active task state along with task queues
+  void backup(const TaskManager& mgr);
+
+  // Do not allow copying or moving
+  // DatabaseLogger(const DatabaseLogger&) = delete;
+  // DatabaseLogger& operator=(const DatabaseLogger&) = delete;
+  // DatabaseLogger(DatabaseLogger&&) = delete;
+  // DatabaseLogger& operator=(DatabaseLogger&&) = delete;
 
   ~DatabaseLogger();
 
 private:
   DatabaseLogger();
-  std::shared_ptr<sqlite3> db;
 
+  // Helper functions to serialize/deserialize assignments
+  // TODO(YV): Consider formalizing the schema in rmf_fleet_adapter/schemas
+  nlohmann::json _convert(const Assignment& assignment);
+  Assignment _convert(const nlohmann::json& msg);
+
+  std::string _file_path;
+  sqlite3* _db;
+  std::mutex _mutex;
 };
 
 } // namespace rmf_fleet_adapter
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp
index ab3fcbc22..140d4d9e3 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp
@@ -212,7 +212,7 @@ std::shared_ptr<FleetUpdateHandle> Adapter::add_fleet(
   rmf_traffic::agv::VehicleTraits traits,
   rmf_traffic::agv::Graph navigation_graph,
   std::optional<std::string> server_uri,
-  )
+  std::optional<std::string> backup_file_path)
 {
   auto planner =
     std::make_shared<std::shared_ptr<const rmf_traffic::agv::Planner>>(
@@ -222,10 +222,13 @@ std::shared_ptr<FleetUpdateHandle> Adapter::add_fleet(
         std::move(traits)),
       rmf_traffic::agv::Planner::Options(nullptr)));
 
+  auto backup_db = backup_file_path.has_value() ?
+    DatabaseLogger::make(backup_file_path.value()) : nullptr;
+
   auto fleet = FleetUpdateHandle::Implementation::make(
     fleet_name, std::move(planner), _pimpl->node, _pimpl->worker,
     _pimpl->schedule_writer, _pimpl->mirror_manager.snapshot_handle(),
-    _pimpl->negotiation, server_uri, std::move(db));
+    _pimpl->negotiation, server_uri, std::move(backup_db));
 
   _pimpl->fleets.push_back(fleet);
   return fleet;
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
index 0c37ea917..ee7e9c929 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
@@ -45,6 +45,7 @@
 #include "../TaskManager.hpp"
 #include "../BroadcastClient.hpp"
 #include "../DeserializeJSON.hpp"
+#include "../DatabaseLogger.hpp"
 
 #include <rmf_traffic/schedule/Snapshot.hpp>
 #include <rmf_traffic/agv/Interpolate.hpp>
@@ -64,8 +65,6 @@
 
 #include <rmf_fleet_adapter/schemas/event_description_PerformAction.hpp>
 
-#include "../DatabaseLogger.hpp"
-
 #include <iostream>
 #include <unordered_set>
 #include <optional>
@@ -269,9 +268,6 @@ class FleetUpdateHandle::Implementation
   rclcpp::TimerBase::SharedPtr fleet_state_topic_publish_timer = nullptr;
   rclcpp::TimerBase::SharedPtr fleet_state_update_timer = nullptr;
 
-  // Map task id to pair of <RequestPtr, Assignments>
-  using Assignments = rmf_task::TaskPlanner::Assignments;
-
   using DockParamMap =
     std::unordered_map<
       std::string,
@@ -291,7 +287,9 @@ class FleetUpdateHandle::Implementation
 
   double current_assignment_cost = 0.0;
   // Map to store task id with assignments for BidNotice
-  std::unordered_map<std::string, Assignments> bid_notice_assignments = {};
+  using Assignments = rmf_task::TaskPlanner::Assignments;
+  using BidNoticeAssignments = std::unordered_map<std::string, Assignments>;
+  BidNoticeAssignments bid_notice_assignments = {};
 
   using BidNoticeMsg = rmf_task_msgs::msg::BidNotice;
 

From 1efd68c9a04cbb6dff30b7532f60d9cd7a26d6e9 Mon Sep 17 00:00:00 2001
From: Yadunund <yadunund@gmail.com>
Date: Mon, 31 Jan 2022 13:26:58 +0800
Subject: [PATCH 73/79] Cleanup

Signed-off-by: Yadunund <yadunund@gmail.com>
---
 rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.cpp | 6 +++---
 rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.hpp | 1 -
 2 files changed, 3 insertions(+), 4 deletions(-)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.cpp
index 6aab6283a..e438f99a7 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.cpp
@@ -34,7 +34,7 @@ std::shared_ptr<DatabaseLogger>
       std::filesystem::absolute(file_path).parent_path());
   }
 
-  auto error = sqlite3_open(file_path, *logger->_db)
+  auto error = sqlite3_open(file_path.c_str(), &logger->_db);
   if (error)
     return nullptr;
 
@@ -57,7 +57,7 @@ DatabaseLogger::DatabaseLogger()
 }
 
 //==============================================================================
-DatabaseLogger::DatabaseLogger()
+DatabaseLogger::~DatabaseLogger()
 {
   // Safely destruct the sqlite3 object
   sqlite3_close(_db);
@@ -85,7 +85,7 @@ nlohmann::json DatabaseLogger::_convert(const Assignment& assignment)
 auto DatabaseLogger::_convert(const nlohmann::json& msg) -> Assignment
 {
   rmf_task::State state;
-  Assignment assignment{nullptr, state, rmf_traffic::Time(0)};
+  Assignment assignment{nullptr, state, std::chrono::steady_clock::now()};
   return assignment;
 }
 
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.hpp
index 3fc548a6d..2159a6f81 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.hpp
@@ -21,7 +21,6 @@
 #include <rmf_task/TaskPlanner.hpp>
 
 #include "TaskManager.hpp"
-#include "agv/internal_FleetUpdateHandle.hpp"
 
 #include <sqlite3.h>
 #include <nlohmann/json.hpp>

From 7efc506d41a858a265cd4a4ad8bdf3cd2a1779fe Mon Sep 17 00:00:00 2001
From: Yadu <yadunund@openrobotics.org>
Date: Mon, 31 Jan 2022 13:54:23 +0800
Subject: [PATCH 74/79] Feature: Direct Task Assignment (#158)

* WIP

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Fix regression

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Refactoring impl functions

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Compute direct assignments

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Implement priority for direct assignment queue

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Use set for direct queue

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Publish api response

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Add missing schemas for response validation

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Populate success response

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Lock mutex when accessing queues

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Add automatic charger retreat to direct queue

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Store broadcast_client as optional within TaskManager

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Cleanup

Signed-off-by: Yadunund <yadunund@openrobotics.org>
---
 .../src/rmf_fleet_adapter/TaskManager.cpp     | 308 +++++++++++++++---
 .../src/rmf_fleet_adapter/TaskManager.hpp     |  70 +++-
 .../agv/FleetUpdateHandle.cpp                 | 195 ++++++-----
 .../agv/internal_FleetUpdateHandle.hpp        |  11 +-
 4 files changed, 442 insertions(+), 142 deletions(-)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
index 80a6b5078..41e123946 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
@@ -26,6 +26,7 @@
 
 #include <rmf_traffic/agv/Planner.hpp>
 
+#include "agv/internal_FleetUpdateHandle.hpp"
 #include "tasks/Clean.hpp"
 #include "tasks/ChargeBattery.hpp"
 #include "tasks/Delivery.hpp"
@@ -51,8 +52,14 @@
 #include <rmf_api_msgs/schemas/resume_task_response.hpp>
 #include <rmf_api_msgs/schemas/rewind_task_request.hpp>
 #include <rmf_api_msgs/schemas/rewind_task_response.hpp>
+#include <rmf_api_msgs/schemas/robot_task_request.hpp>
+#include <rmf_api_msgs/schemas/dispatch_task_response.hpp>
+#include <rmf_api_msgs/schemas/task_state.hpp>
+#include <rmf_api_msgs/schemas/error.hpp>
+#include <rmf_api_msgs/schemas/robot_task_response.hpp>
 #include <rmf_api_msgs/schemas/skip_phase_request.hpp>
 #include <rmf_api_msgs/schemas/skip_phase_response.hpp>
+#include <rmf_api_msgs/schemas/task_request.hpp>
 #include <rmf_api_msgs/schemas/undo_skip_phase_request.hpp>
 #include <rmf_api_msgs/schemas/undo_skip_phase_response.hpp>
 #include <rmf_api_msgs/schemas/error.hpp>
@@ -62,12 +69,14 @@ namespace rmf_fleet_adapter {
 //==============================================================================
 TaskManagerPtr TaskManager::make(
   agv::RobotContextPtr context,
-  std::weak_ptr<BroadcastClient> broadcast_client)
+  std::optional<std::weak_ptr<BroadcastClient>> broadcast_client,
+  std::weak_ptr<agv::FleetUpdateHandle> fleet_handle)
 {
   auto mgr = TaskManagerPtr(
     new TaskManager(
       std::move(context),
-      std::move(broadcast_client)));
+      std::move(broadcast_client),
+      std::move(fleet_handle)));
 
   auto begin_pullover = [w = mgr->weak_from_this()]()
     {
@@ -185,8 +194,14 @@ TaskManagerPtr TaskManager::make(
     rmf_api_msgs::schemas::resume_task_response,
     rmf_api_msgs::schemas::rewind_task_request,
     rmf_api_msgs::schemas::rewind_task_response,
+    rmf_api_msgs::schemas::robot_task_request,
+    rmf_api_msgs::schemas::dispatch_task_response,
+    rmf_api_msgs::schemas::task_state,
+    rmf_api_msgs::schemas::error,
+    rmf_api_msgs::schemas::robot_task_response,
     rmf_api_msgs::schemas::skip_phase_request,
     rmf_api_msgs::schemas::skip_phase_response,
+    rmf_api_msgs::schemas::task_request,
     rmf_api_msgs::schemas::undo_skip_phase_request,
     rmf_api_msgs::schemas::undo_skip_phase_response,
     rmf_api_msgs::schemas::error
@@ -204,9 +219,12 @@ TaskManagerPtr TaskManager::make(
 //==============================================================================
 TaskManager::TaskManager(
   agv::RobotContextPtr context,
-  std::weak_ptr<BroadcastClient> broadcast_client)
+  std::optional<std::weak_ptr<BroadcastClient>> broadcast_client,
+  std::weak_ptr<agv::FleetUpdateHandle> fleet_handle)
 : _context(std::move(context)),
   _broadcast_client(std::move(broadcast_client)),
+  _fleet_handle(std::move(fleet_handle)),
+  _next_sequence_number(0),
   _last_update_time(std::chrono::steady_clock::now() - std::chrono::seconds(1))
 {
   // Do nothing. The make() function does all further initialization.
@@ -715,15 +733,6 @@ std::vector<std::string> TaskManager::ActiveTask::remove_skips(
   return missing_tokens;
 }
 
-//==============================================================================
-auto TaskManager::expected_finish_location() const -> StartSet
-{
-  if (_expected_finish_location)
-    return {*_expected_finish_location};
-
-  return _context->location();
-}
-
 //==============================================================================
 std::optional<std::string> TaskManager::current_task_id() const
 {
@@ -748,6 +757,7 @@ bool TaskManager::cancel_task_if_present(const std::string& task_id)
     return true;
   }
 
+  std::lock_guard<std::mutex> lock(_mutex);
   for (auto it = _queue.begin(); it != _queue.end(); ++it)
   {
     if (it->request()->booking()->id() == task_id)
@@ -773,20 +783,20 @@ std::string TaskManager::robot_status() const
 //==============================================================================
 auto TaskManager::expected_finish_state() const -> State
 {
-  // If an active task exists, return the estimated finish state of that task
-  /// else update the current time and battery level for the state and return
-  if (_active_task)
-    return _context->current_task_end_state();
+  rmf_task::State current_state =
+    _context->make_get_state()()
+    .time(rmf_traffic_ros2::convert(_context->node()->now()));
 
-  // Update battery soc and finish time in the current state
-  auto finish_state = _context->current_task_end_state();
-  auto location = finish_state.extract_plan_start().value();
-  finish_state.time(rmf_traffic_ros2::convert(_context->node()->now()));
+  std::lock_guard<std::mutex> lock(_mutex);
+  if (!_direct_queue.empty())
+  {
+    return _direct_queue.rbegin()->assignment.finish_state();
+  }
 
-  const double current_battery_soc = _context->current_battery_soc();
-  finish_state.battery_soc(current_battery_soc);
+  if (_active_task)
+    return _context->current_task_end_state();
 
-  return finish_state;
+  return current_state;
 }
 
 //==============================================================================
@@ -802,7 +812,8 @@ agv::ConstRobotContextPtr TaskManager::context() const
 }
 
 //==============================================================================
-std::weak_ptr<BroadcastClient> TaskManager::broadcast_client() const
+std::optional<std::weak_ptr<BroadcastClient>> TaskManager::broadcast_client()
+const
 {
   return _broadcast_client;
 }
@@ -815,6 +826,13 @@ void TaskManager::set_queue(
   // function that is called at the end of this function.
   {
     std::lock_guard<std::mutex> guard(_mutex);
+    // Do not remove automatic task if assignments is empty. See Issue #138
+    if (assignments.empty() &&
+      _queue.size() == 1 &&
+      _queue.front().request()->booking()->automatic())
+    {
+      return;
+    }
     _queue = assignments;
     _publish_task_queue();
   }
@@ -828,6 +846,7 @@ const std::vector<rmf_task::ConstRequestPtr> TaskManager::requests() const
   using namespace rmf_task::requests;
   std::vector<rmf_task::ConstRequestPtr> requests;
   requests.reserve(_queue.size());
+  std::lock_guard<std::mutex> lock(_mutex);
   for (const auto& task : _queue)
   {
     if (task.request()->booking()->automatic())
@@ -884,7 +903,7 @@ void TaskManager::_begin_next_task()
 
   std::lock_guard<std::mutex> guard(_mutex);
 
-  if (_queue.empty())
+  if (_queue.empty() && _direct_queue.empty())
   {
     if (!_waiting)
       _begin_waiting();
@@ -898,9 +917,12 @@ void TaskManager::_begin_next_task()
     return;
   }
 
-  const rmf_traffic::Time now = rmf_traffic_ros2::convert(
-    _context->node()->now());
-  const auto next_task = _queue.front();
+  // The next task should one in the direct assignment queue if present
+  const bool is_next_task_direct = !_direct_queue.empty();
+  const auto assignment = is_next_task_direct ?
+    _direct_queue.begin()->assignment :
+    _queue.front();
+
   // We take the minimum of the two to deal with cases where the deployment_time
   // as computed by the task planner is greater than the earliest_start_time
   // which is greater than now. This can happen for example if the previous task
@@ -908,16 +930,18 @@ void TaskManager::_begin_next_task()
   // TODO: Reactively replan task assignments across agents in a fleet every
   // time as task is completed.
   const auto deployment_time = std::min(
-    next_task.deployment_time(),
-    next_task.request()->booking()->earliest_start_time());
+    assignment.request()->booking()->earliest_start_time(),
+    assignment.deployment_time());
+
+  const rmf_traffic::Time now = rmf_traffic_ros2::convert(
+    _context->node()->now());
 
   if (now >= deployment_time)
   {
     // Update state in RobotContext and Assign active task
-    const auto id = _queue.front().request()->booking()->id();
-    _context->current_task_end_state(_queue.front().finish_state());
+    const auto& id = assignment.request()->booking()->id();
+    _context->current_task_end_state(assignment.finish_state());
     _context->current_task_id(id);
-    const auto assignment = _queue.front();
     _active_task = _context->task_activator()->activate(
       _context->make_get_state(),
       _context->task_parameters(),
@@ -927,7 +951,10 @@ void TaskManager::_begin_next_task()
       _phase_finished_cb(),
       _task_finished(id));
 
-    _queue.erase(_queue.begin());
+    if (is_next_task_direct)
+      _direct_queue.erase(_direct_queue.begin());
+   else
+      _queue.erase(_queue.begin());
 
     if (!_active_task)
     {
@@ -1091,9 +1118,9 @@ void TaskManager::_resume_from_emergency()
 void TaskManager::retreat_to_charger()
 {
   {
-    std::lock_guard<std::mutex> guard(_mutex);
-    if (_active_task || !_queue.empty())
-      return;
+  std::lock_guard<std::mutex> guard(_mutex);
+  if (_active_task || !_queue.empty())
+    return;
   }
 
   const auto task_planner = _context->task_planner();
@@ -1155,7 +1182,14 @@ void TaskManager::retreat_to_charger()
       finish.value().finish_state(),
       current_state.time().value());
 
-    set_queue({charging_assignment});
+    const DirectAssignment assignment = DirectAssignment{
+      _next_sequence_number,
+      charging_assignment};
+    ++_next_sequence_number;
+    {
+    std::lock_guard<std::mutex> lock(_mutex);
+    _direct_queue.insert(assignment);
+    }
 
     RCLCPP_INFO(
       _context->node()->get_logger(),
@@ -1258,7 +1292,10 @@ void TaskManager::_validate_and_publish_websocket(
     return;
   }
 
-  const auto client = _broadcast_client.lock();
+  if (!_broadcast_client.has_value())
+    return;
+
+  const auto client = _broadcast_client->lock();
   if (!client)
   {
     RCLCPP_ERROR(
@@ -1460,6 +1497,9 @@ void TaskManager::_send_simple_error_if_queued(
   const std::string& request_id,
   const std::string& type)
 {
+  // TODO(YV): We could cache the task_ids of direct and dispatched tasks in
+  // unordered_sets and perform a lookup to see which queue to iterate.
+  std::lock_guard<std::mutex> lock(_mutex);
   for (const auto& a : _queue)
   {
     if (a.request()->booking()->id() == task_id)
@@ -1470,6 +1510,17 @@ void TaskManager::_send_simple_error_if_queued(
         "is not currently supported");
     }
   }
+
+  for (const auto& a : _direct_queue)
+  {
+    if (a.assignment.request()->booking()->id() == task_id)
+    {
+      return _send_simple_error_response(
+        request_id, 6, "Invalid Circumstances",
+        type + " a task that is queued (not yet active) "
+        "is not currently supported");
+    }
+  }
 }
 
 //==============================================================================
@@ -1601,6 +1652,10 @@ void TaskManager::_handle_request(
       _handle_skip_phase_request(request_json, request_id);
     else if (type_str == "undo_phase_skip_request")
       _handle_undo_skip_phase_request(request_json, request_id);
+    else if (type_str == "robot_task_request")
+      _handle_direct_request(request_json, request_id);
+    else
+      return;
   }
   catch (const std::exception& e)
   {
@@ -1622,7 +1677,7 @@ std::vector<std::string> get_labels(const nlohmann::json& request)
 }
 
 //==============================================================================
-void remove_task_from_queue(
+bool remove_task_from_queue(
   const std::string& task_id,
   std::vector<TaskManager::Assignment>& queue)
 {
@@ -1638,10 +1693,168 @@ void remove_task_from_queue(
     if (it->request()->booking()->id() == task_id)
     {
       queue.erase(it);
-      break;
+      return true;
     }
   }
+  return false;
 }
+
+//==============================================================================
+bool remove_task_from_queue(
+  const std::string& task_id,
+  TaskManager::DirectQueue& queue)
+{
+  // If the task is queued, then we should make sure to remove it from the
+  // queue, just in case it reaches an active state before the dispatcher
+  // issues its cancellation request.
+  //
+  // TODO(MXG): We should do a much better of job of coordinating these
+  // different moving parts in the system. E.g. who is ultimately responsible
+  // for issuing the response to the request or updating the task state?
+  for (auto it = queue.begin(); it != queue.end(); ++it)
+  {
+    if (it->assignment.request()->booking()->id() == task_id)
+    {
+      queue.erase(it);
+      return true;
+    }
+  }
+  return false;
+}
+} // namespace anonymous
+
+//==============================================================================
+void TaskManager::_handle_direct_request(
+  const  nlohmann::json& request_json,
+  const std::string& request_id)
+{
+  static const auto request_validator =
+    _make_validator(rmf_api_msgs::schemas::robot_task_request);
+
+  static const auto response_validator =
+    _make_validator(rmf_api_msgs::schemas::robot_task_response);
+
+  if (!_validate_request_message(request_json, request_validator, request_id))
+    return;
+
+  const auto& robot = request_json["robot"].get<std::string>();
+  if (robot.empty() || robot != _context->name())
+    return;
+
+  const auto& fleet = request_json["fleet"].get<std::string>();
+  if (fleet.empty() || fleet != _context->group())
+    return;
+
+  const nlohmann::json& request = request_json["request"];
+  auto fleet_handle = _fleet_handle.lock();
+  if (!fleet_handle)
+    return;
+  const auto& impl =
+    agv::FleetUpdateHandle::Implementation::get(*fleet_handle);
+  std::vector<std::string> errors;
+  const auto new_request = impl.convert(request_id, request, errors);
+  if (!new_request)
+  {
+    RCLCPP_ERROR(
+      _context->node()->get_logger(),
+      "Unable to generate a valid request for direct task [%s]",
+      request_id.c_str());
+
+    nlohmann::json response_json;
+    response_json["success"] = false;
+    std::vector<nlohmann::json> json_errors = {};
+    for (const auto& e : errors)
+      json_errors.push_back(nlohmann::json::parse(e));
+    response_json["errors"] = std::move(json_errors);
+
+    _validate_and_publish_api_response(
+      response_json,
+      response_validator,
+      request_id);
+
+    return;
+  }
+  // Generate Assignment for the request
+  const auto task_planner = _context->task_planner();
+  if (!task_planner)
+  {
+    RCLCPP_ERROR(
+      _context->node()->get_logger(),
+      "Fleet [%s] is not configured with parameters for task planning."
+      "Use FleetUpdateHandle::set_task_planner_params(~) to set the "
+      "parameters required.", fleet.c_str());
+    return;
+  }
+
+  const auto current_state = expected_finish_state();
+  const auto& constraints = task_planner->configuration().constraints();
+  const auto& parameters = task_planner->configuration().parameters();
+  const auto model = new_request->description()->make_model(
+    new_request->booking()->earliest_start_time(),
+    parameters);
+  const auto estimate = model->estimate_finish(
+    current_state,
+    constraints,
+    *_travel_estimator);
+
+  rmf_task::State finish_state;
+  rmf_traffic::Time deployment_time;
+
+  if (!estimate.has_value())
+  {
+    RCLCPP_WARN(
+      _context->node()->get_logger(),
+      "Unable to estimate final state for direct task request [%s]. This may "
+      "be due to insufficient resources to perform the task. The task will be "
+      "still be added to the queue.",
+      request_id.c_str());
+    finish_state = current_state;
+    deployment_time = new_request->booking()->earliest_start_time();
+  }
+  else
+  {
+    finish_state = estimate.value().finish_state();
+    deployment_time = estimate.value().wait_until();
+  }
+
+  const DirectAssignment assignment = DirectAssignment{
+    _next_sequence_number,
+    Assignment(
+        new_request,
+        finish_state,
+        deployment_time)
+    };
+  ++_next_sequence_number;
+  {
+  std::lock_guard<std::mutex> lock(_mutex);
+  _direct_queue.insert(assignment);
+  }
+
+  RCLCPP_INFO(
+    _context->node()->get_logger(),
+    "Direct request [%s] successfully queued for robot [%s]",
+    request_id.c_str(),
+    robot.c_str());
+
+  // Publish api response
+  nlohmann::json response_json;
+  response_json["success"] = true;
+  nlohmann::json task_state;;
+  copy_booking_data(task_state["booking"], *new_request->booking());
+  // task_state["category"] = request["category"].get<std::string>();
+  task_state["detail"] = request["description"];
+  task_state["status"] = "queued";
+  auto& dispatch = task_state["dispatch"];
+  dispatch["status"] = "queued";
+  auto& assign = task_state["assigned_to"];
+  assign["group"] = fleet;
+  assign["name"] = robot;
+  response_json["state"] = task_state;
+
+  _validate_and_publish_api_response(
+    response_json,
+    response_validator,
+    request_id);
 }
 
 //==============================================================================
@@ -1663,8 +1876,11 @@ void TaskManager::_handle_cancel_request(
     _task_state_update_available = true;
     return _send_simple_success_response(request_id);
   }
-
-  remove_task_from_queue(task_id, _queue);
+  // TODO(YV): We could cache the task_ids of direct and dispatched tasks in
+  // unordered_sets and perform a lookup to see which function to call.
+  std::lock_guard<std::mutex> lock(_mutex);
+  if (!remove_task_from_queue(task_id, _queue))
+    remove_task_from_queue(task_id, _direct_queue);
 }
 
 //==============================================================================
@@ -1687,7 +1903,11 @@ void TaskManager::_handle_kill_request(
     return _send_simple_success_response(request_id);
   }
 
-  remove_task_from_queue(task_id, _queue);
+  // TODO(YV): We could cache the task_ids of direct and dispatched tasks in
+  // unordered_sets and perform a lookup to see which function to call.
+  std::lock_guard<std::mutex> lock(_mutex);
+  if (!remove_task_from_queue(task_id, _queue))
+    remove_task_from_queue(task_id, _direct_queue);
 }
 
 //==============================================================================
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
index fb62e4469..edf05f809 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
@@ -36,6 +36,7 @@
 #include <nlohmann/json-schema.hpp>
 
 #include <mutex>
+#include <set>
 
 namespace rmf_fleet_adapter {
 
@@ -51,7 +52,8 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
 
   static std::shared_ptr<TaskManager> make(
     agv::RobotContextPtr context,
-    std::weak_ptr<BroadcastClient> broadcast_client);
+    std::optional<std::weak_ptr<BroadcastClient>> broadcast_client,
+    std::weak_ptr<agv::FleetUpdateHandle> fleet_handle);
 
   using Start = rmf_traffic::agv::Plan::Start;
   using StartSet = rmf_traffic::agv::Plan::StartSet;
@@ -62,15 +64,50 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
   using TaskProfiles = std::unordered_map<std::string, TaskProfileMsg>;
   using TaskSummaryMsg = rmf_task_msgs::msg::TaskSummary;
 
-  /// The location where we expect this robot to be at the end of its current
-  /// task queue.
-  StartSet expected_finish_location() const;
+  struct DirectAssignment
+  {
+    std::size_t sequence_number;
+    Assignment assignment;
+
+    DirectAssignment(
+      std::size_t sequence_number_,
+      Assignment assignment_)
+    : sequence_number(sequence_number_),
+      assignment(std::move(assignment_))
+    {
+      // Do nothing
+    }
+
+  };
+
+  struct DirectQueuePriority
+  {
+    bool operator()(const DirectAssignment& a, const DirectAssignment& b) const
+    {
+      // Sort by start time and then sequence_number if tie
+      const auto start_time_b =
+        b.assignment.request()->booking()->earliest_start_time();
+      const auto start_time_a =
+        a.assignment.request()->booking()->earliest_start_time();
+
+      if (start_time_b == start_time_a)
+      {
+        return b.sequence_number < a.sequence_number;
+      }
+
+      return start_time_b < start_time_a;
+    }
+  };
+
+  using DirectQueue = std::set<
+    DirectAssignment,
+    DirectQueuePriority>;
 
   const agv::RobotContextPtr& context();
 
   agv::ConstRobotContextPtr context() const;
 
-  std::weak_ptr<BroadcastClient> broadcast_client() const;
+  std::optional<std::weak_ptr<BroadcastClient>> broadcast_client() const;
 
   /// Set the queue for this task manager with assignments generated from the
   /// task planner
@@ -87,7 +124,10 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
 
   std::string robot_status() const;
 
-  /// The state of the robot.
+  /// The state of the robot. If the direct assignment queue is not empty,
+  /// This will always return the state of the robot after completing all
+  /// the tasks in the direct assignment queue. Otherwise, the current state
+  /// is returned
   State expected_finish_state() const;
 
   /// Callback for the retreat timer. Appends a charging task to the task queue
@@ -107,7 +147,8 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
 
   TaskManager(
     agv::RobotContextPtr context,
-    std::weak_ptr<BroadcastClient> broadcast_client);
+    std::optional<std::weak_ptr<BroadcastClient>> broadcast_client,
+    std::weak_ptr<agv::FleetUpdateHandle>);
 
   class ActiveTask
   {
@@ -190,13 +231,19 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
   friend class ActiveTask;
 
   agv::RobotContextPtr _context;
-  std::weak_ptr<BroadcastClient> _broadcast_client;
+  std::optional<std::weak_ptr<BroadcastClient>> _broadcast_client;
+  std::weak_ptr<agv::FleetUpdateHandle> _fleet_handle;
   rmf_task::ConstActivatorPtr _task_activator;
   ActiveTask _active_task;
   bool _emergency_active = false;
   std::optional<std::string> _emergency_pullover_interrupt_token;
   rmf_task_sequence::Event::ActivePtr _emergency_pullover;
+  // Queue for dispatched tasks
   std::vector<Assignment> _queue;
+  // An ID to keep track of the FIFO order of direct tasks
+  std::size_t _next_sequence_number;
+  // Queue for directly assigned tasks
+  DirectQueue _direct_queue;
   rmf_utils::optional<Start> _expected_finish_location;
   rxcpp::subscription _task_sub;
   rxcpp::subscription _emergency_sub;
@@ -209,7 +256,7 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
   // TODO: Eliminate the need for a mutex by redesigning the use of the task
   // manager so that modifications of shared data only happen on designated
   // rxcpp worker
-  std::mutex _mutex;
+  mutable std::mutex _mutex;
   rclcpp::TimerBase::SharedPtr _task_timer;
   rclcpp::TimerBase::SharedPtr _retreat_timer;
   rclcpp::TimerBase::SharedPtr _update_timer;
@@ -360,6 +407,10 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
     const std::string& request_msg,
     const std::string& request_id);
 
+  void _handle_direct_request(
+    const nlohmann::json& request_json,
+    const std::string& request_id);
+
   void _handle_cancel_request(
     const nlohmann::json& request_json,
     const std::string& request_id);
@@ -387,6 +438,7 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
   void _handle_undo_skip_phase_request(
     const nlohmann::json& request_json,
     const std::string& request_id);
+
 };
 
 using TaskManagerPtr = std::shared_ptr<TaskManager>;
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
index d7164ebc9..d84d5ba6e 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
@@ -151,10 +151,9 @@ void FleetUpdateHandle::Implementation::dock_summary_cb(
   return;
 }
 
-namespace {
 //==============================================================================
-std::string make_error_str(
-  uint64_t code, std::string category, std::string detail)
+std::string FleetUpdateHandle::Implementation::make_error_str(
+  uint64_t code, std::string category, std::string detail) const
 {
   nlohmann::json error;
   error["code"] = code;
@@ -163,84 +162,23 @@ std::string make_error_str(
 
   return error.dump();
 }
-} // anonymous namespace
 
 //==============================================================================
-void FleetUpdateHandle::Implementation::bid_notice_cb(
-  const BidNoticeMsg& bid_notice,
-  rmf_task_ros2::bidding::AsyncBidder::Respond respond)
+std::shared_ptr<rmf_task::Request> FleetUpdateHandle::Implementation::convert(
+  const std::string& task_id,
+  const nlohmann::json& request_msg,
+  std::vector<std::string>& errors) const
 {
-  const auto& task_id = bid_notice.task_id;
-  if (task_managers.empty())
-  {
-    RCLCPP_INFO(
-      node->get_logger(),
-      "Fleet [%s] does not have any robots to accept task [%s]. Use "
-      "FleetUpdateHadndle::add_robot(~) to add robots to this fleet. ",
-      name.c_str(), task_id.c_str());
-    return;
-  }
-
-  if (task_id.empty())
-  {
-    RCLCPP_WARN(
-      node->get_logger(),
-      "Received BidNotice for a task with empty task_id. Request will be "
-      "ignored.");
-    return;
-  }
-
-  // TODO remove this block when we support task revival
-  if (bid_notice_assignments.find(task_id) != bid_notice_assignments.end())
-    return;
-
-  if (!task_planner)
-  {
-    RCLCPP_WARN(
-      node->get_logger(),
-      "Fleet [%s] is not configured with parameters for task planning."
-      "Use FleetUpdateHandle::set_task_planner_params(~) to set the "
-      "parameters required.", name.c_str());
-
-    return;
-  }
-
-  const auto request_msg = nlohmann::json::parse(bid_notice.request);
-  static const auto request_validator =
-    nlohmann::json_schema::json_validator(rmf_api_msgs::schemas::task_request);
-
-  try
-  {
-    request_validator.validate(request_msg);
-  }
-  catch (const std::exception& e)
-  {
-    RCLCPP_ERROR(
-      node->get_logger(),
-      "Received a request with an invalid format. Error: %s\nRequest:\n%s",
-      e.what(),
-      request_msg.dump(2, ' ').c_str());
-
-    return respond(
-      {
-        std::nullopt,
-        {make_error_str(5, "Invalid request format", e.what())}
-      });
-  }
-
   const auto& category = request_msg["category"].get<std::string>();
 
   const auto task_deser_it = deserialization.task->handlers.find(category);
   if (task_deser_it == deserialization.task->handlers.end())
   {
-    return respond(
-      {
-        std::nullopt,
-        {make_error_str(
-          4, "Unsupported type",
-          "Fleet [" + name + "] does not support task category ["
-          + category + "]")}
-      });
+    errors.push_back(make_error_str(
+      4, "Unsupported type",
+      "Fleet [" + name + "] does not support task category ["
+      + category + "]"));
+    return nullptr;
   }
 
   const auto& description_msg = request_msg["description"];
@@ -260,11 +198,8 @@ void FleetUpdateHandle::Implementation::bid_notice_cb(
       e.what(),
       description_msg.dump(2, ' ').c_str());
 
-    return respond(
-      rmf_task_ros2::bidding::Response{
-        std::nullopt,
-        {make_error_str(5, "Invalid request format", e.what())}
-      });
+    errors.push_back(make_error_str(5, "Invalid request format", e.what()));
+    return nullptr;
   }
 
   const auto deserialized_task =
@@ -272,15 +207,10 @@ void FleetUpdateHandle::Implementation::bid_notice_cb(
 
   if (!deserialized_task.description)
   {
-    return respond(
-      {
-        std::nullopt,
-        deserialized_task.errors
-      });
+    errors = deserialized_task.errors;
+    return nullptr;
   }
 
-  std::vector<std::string> errors;
-
   rmf_traffic::Time earliest_start_time = rmf_traffic_ros2::convert(
     node->get_clock()->now());
   const auto t_it = request_msg.find("unix_millis_earliest_start_time");
@@ -294,6 +224,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb(
   const auto p_it = request_msg.find("priority");
   if (p_it != request_msg.end())
   {
+    // TODO(YV): Validate with priority_description_Binary.json
     if (p_it->contains("type") && p_it->contains("value"))
     {
       const auto& p_type = (*p_it)["type"];
@@ -330,6 +261,84 @@ void FleetUpdateHandle::Implementation::bid_notice_cb(
       priority,
       deserialized_task.description);
 
+  return new_request;
+}
+
+//==============================================================================
+void FleetUpdateHandle::Implementation::bid_notice_cb(
+  const BidNoticeMsg& bid_notice,
+  rmf_task_ros2::bidding::AsyncBidder::Respond respond)
+{
+  // TODO(YV): Consider moving these checks into convert()
+  const auto& task_id = bid_notice.task_id;
+  if (task_managers.empty())
+  {
+    RCLCPP_INFO(
+      node->get_logger(),
+      "Fleet [%s] does not have any robots to accept task [%s]. Use "
+      "FleetUpdateHadndle::add_robot(~) to add robots to this fleet. ",
+      name.c_str(), task_id.c_str());
+    return;
+  }
+
+  if (task_id.empty())
+  {
+    RCLCPP_WARN(
+      node->get_logger(),
+      "Received BidNotice for a task with empty task_id. Request will be "
+      "ignored.");
+    return;
+  }
+
+  // TODO remove this block when we support task revival
+  if (bid_notice_assignments.find(task_id) != bid_notice_assignments.end())
+    return;
+
+  if (!task_planner)
+  {
+    RCLCPP_WARN(
+      node->get_logger(),
+      "Fleet [%s] is not configured with parameters for task planning."
+      "Use FleetUpdateHandle::set_task_planner_params(~) to set the "
+      "parameters required.", name.c_str());
+
+    return;
+  }
+
+  const auto request_msg = nlohmann::json::parse(bid_notice.request);
+  static const auto request_validator =
+    nlohmann::json_schema::json_validator(rmf_api_msgs::schemas::task_request);
+
+  try
+  {
+    request_validator.validate(request_msg);
+  }
+  catch (const std::exception& e)
+  {
+    RCLCPP_ERROR(
+      node->get_logger(),
+      "Received a request with an invalid format. Error: %s\nRequest:\n%s",
+      e.what(),
+      request_msg.dump(2, ' ').c_str());
+
+    return respond(
+      {
+        std::nullopt,
+        {make_error_str(5, "Invalid request format", e.what())}
+      });
+  }
+
+  std::vector<std::string> errors = {};
+  const auto new_request = convert(task_id, request_msg, errors);
+  if (!new_request)
+  {
+    return respond(
+      {
+        std::nullopt,
+        errors
+      });
+  }
+
   // TODO(MXG): Make the task planning asynchronous. The worker should schedule
   // a job to perform the planning which should then spawn a job to save the
   // plan result and respond. I started to refactor allocate_tasks(~) to make it
@@ -1021,11 +1030,13 @@ auto FleetUpdateHandle::Implementation::aggregate_expectations() const
 //==============================================================================
 auto FleetUpdateHandle::Implementation::allocate_tasks(
   rmf_task::ConstRequestPtr new_request,
-  std::vector<std::string>* errors) const -> std::optional<Assignments>
+  std::vector<std::string>* errors,
+  std::optional<Expectations> expectations) const -> std::optional<Assignments>
 {
   // Collate robot states, constraints and combine new requestptr with
   // requestptr of non-charging tasks in task manager queues
-  auto expect = aggregate_expectations();
+  auto expect = expectations.has_value() ? expectations.value() :
+    aggregate_expectations();
   std::string id = "";
 
   if (new_request)
@@ -1273,8 +1284,16 @@ void FleetUpdateHandle::add_robot(
             return;
           }
 
+          std::optional<std::weak_ptr<BroadcastClient>> broadcast_client =
+            std::nullopt;
+          if (fleet->_pimpl->broadcast_client)
+            broadcast_client = fleet->_pimpl->broadcast_client;
+
           fleet->_pimpl->task_managers.insert({context,
-            TaskManager::make(context, fleet->_pimpl->broadcast_client)});
+            TaskManager::make(
+              context,
+              broadcast_client,
+              std::weak_ptr<FleetUpdateHandle>(fleet))});
         });
     });
 }
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
index 147c6f7c1..a64729c1a 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
@@ -504,7 +504,8 @@ class FleetUpdateHandle::Implementation
   /// new request and while optionally ignoring a specific request.
   std::optional<Assignments> allocate_tasks(
     rmf_task::ConstRequestPtr new_request = nullptr,
-    std::vector<std::string>* errors = nullptr) const;
+    std::vector<std::string>* errors = nullptr,
+    std::optional<Expectations> expectations = std::nullopt) const;
 
   /// Helper function to check if assignments are valid. An assignment set is
   /// invalid if one of the assignments has already begun execution.
@@ -525,6 +526,14 @@ class FleetUpdateHandle::Implementation
   void update_fleet_state() const;
 
   void add_standard_tasks();
+
+  std::string make_error_str(
+    uint64_t code, std::string category, std::string detail) const;
+
+  std::shared_ptr<rmf_task::Request> convert(
+    const std::string& task_id,
+    const nlohmann::json& request_msg,
+    std::vector<std::string>& errors) const;
 };
 
 } // namespace agv

From 02475b7a9b7cba2757ecf2cdbfb74a1cb83a25e0 Mon Sep 17 00:00:00 2001
From: Yadunund <yadunund@openrobotics.org>
Date: Mon, 31 Jan 2022 19:23:04 +0800
Subject: [PATCH 75/79] Backup active task

Signed-off-by: Yadunund <yadunund@openrobotics.org>
---
 .../src/rmf_fleet_adapter/DatabaseLogger.cpp  | 95 +++++++++++++++++--
 .../src/rmf_fleet_adapter/DatabaseLogger.hpp  | 40 +++++---
 .../src/rmf_fleet_adapter/TaskManager.cpp     | 10 ++
 .../src/rmf_fleet_adapter/TaskManager.hpp     |  1 +
 4 files changed, 124 insertions(+), 22 deletions(-)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.cpp
index e438f99a7..b45f1dc4d 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.cpp
@@ -20,6 +20,8 @@
 #include <filesystem>
 #include <fstream>
 
+#include <iostream>
+
 namespace rmf_fleet_adapter {
 //==============================================================================
 auto DatabaseLogger::make(const std::string& file_path) ->
@@ -34,19 +36,48 @@ std::shared_ptr<DatabaseLogger>
       std::filesystem::absolute(file_path).parent_path());
   }
 
-  auto error = sqlite3_open(file_path.c_str(), &logger->_db);
+  // Variables that will be reassigned
+  int error;
+  std::string sql;
+  char* error_msg;
+
+
+  error = sqlite3_open(file_path.c_str(), &logger->_db);
   if (error)
     return nullptr;
 
-  // Currently we only store the state of the fleet adapter
-  // for restoration.
-  // Table TaskManagers: Robot name as key. Columns for active state json & queues
-
+  auto table_exists =
+    [logger](const std::string& name) -> bool
+    {
+      char* error_msg;
+      const std::string query = "SELECT name FROM sqlite_master "
+      "WHERE type='table' AND name='" + name +"';";
+      auto error = sqlite3_exec(logger->_db, query.c_str(), NULL, 0, &error_msg);
+      // TOOD(YV): Distinguish between error code from bad statement and table
+      // not existing.
+      return !error;
+    };
+
+  // Create tables if they do not already exist
+  // Active Task Table
+  if (!table_exists("ACTIVE_TASK"))
+  {
+    sql = "CREATE TABLE ACTIVE_TASK("
+          "ROBOT TEXT PRIMARY KEY NOT NULL, "
+          "STATE TEXT NOT NULL);";
+    error = sqlite3_exec(logger->_db, sql.c_str(), NULL, 0, &error_msg);
+    assert(!error);
+  }
 
+  // TODO(YV): Tables for task queues, task logs and bid notice assignments
 
   // TODO(YV): Also store history of task & state logs along with latest state.
   // Proposed key (robot, task, phase, event, seq)
 
+
+  // TODO(YV): If db is not empty, generate Restore. For now we will set it to
+  // nullopt.
+
   return logger;
 }
 
@@ -56,33 +87,79 @@ DatabaseLogger::DatabaseLogger()
   // Do nothing
 }
 
+//==============================================================================
+auto DatabaseLogger::restore() const -> std::optional<Restored>
+{
+  return std::nullopt;
+}
+
 //==============================================================================
 DatabaseLogger::~DatabaseLogger()
 {
   // Safely destruct the sqlite3 object
   sqlite3_close(_db);
 }
+
 //==============================================================================
-void DatabaseLogger::backup(const BidNoticeAssignments& assignments)
+void DatabaseLogger::backup_bid_notice_assignments(
+  const BidNoticeAssignments& assignments)
 {
+  // TODO
+}
 
+//==============================================================================
+void DatabaseLogger::backup_task_queues(TaskManager& mgr)
+{
+  // TODO
 }
 
 //==============================================================================
-void DatabaseLogger::backup(const TaskManager& mgr)
+void DatabaseLogger::backup_active_task(
+  const std::string& robot,
+  const nlohmann::json& task_state)
 {
+  std::lock_guard<std::mutex> lock(_mutex);
+  const std::string state = task_state.dump();
+
+  const std::string sql = "IF EXISTS(SELECT * FROM ACTIVE_TASK WHERE ROBOT=" +
+  robot +")" + "UPDATE ACTIVE_TASK SET STATE=" + state + "WHERE ROBOT=" + robot +
+  "ELSE INSERT INTO ACTIVE_TASK(" + robot +") values(" + state + ");";
+  char* error_msg;
+
+  auto print_table =
+    [](void* data, int num_col, char** col_values, char** col_names)
+    {
+      int i;
+      fprintf(stderr, "%s: ", (const char*)data);
 
+      for (i = 0; i < num_col; i++) {
+          printf("%s = %s\n", col_names[i], col_values[i] ? col_values[i] : "NULL");
+      }
+
+      printf("\n");
+      return 0;
+    };
+  auto error = sqlite3_exec(_db, sql.c_str(), print_table, 0, &error_msg);
+
+}
+
+//==============================================================================
+void DatabaseLogger::backup_task_logs(
+    const std::string& robot,
+    const nlohmann::json& task_logs)
+{
+  // TODO
 }
 
 //==============================================================================
-nlohmann::json DatabaseLogger::_convert(const Assignment& assignment)
+nlohmann::json DatabaseLogger::convert(const Assignment& assignment)
 {
   nlohmann::json msg;
   return msg;
 }
 
 //==============================================================================
-auto DatabaseLogger::_convert(const nlohmann::json& msg) -> Assignment
+auto DatabaseLogger::convert(const nlohmann::json& msg) -> Assignment
 {
   rmf_task::State state;
   Assignment assignment{nullptr, state, std::chrono::steady_clock::now()};
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.hpp
index 2159a6f81..b10efbbad 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.hpp
@@ -41,33 +41,44 @@ class DatabaseLogger
   using BidNoticeAssignments =
     std::unordered_map<std::string, Assignments>;
   // Bundle up the restored state of the fleet adapter
-  // TODO(YV): Ensure index of managers correspond to index of Assignments
-  // We should return some sort of map<robot_name, manager> which
-  // FleetUpdateHandle::add_robot() can lookup for restored task managers.
-  // But if a new robot is being added, what do we do with the
+  // TODO(YV): FleetUpdateHandle::add_robot() can lookup for restored task
+  // managers. But if a new robot is being added, what do we do with the
   // previous bid_notice_assignments? The index order will be incorrect.
   // We would need to modify the implementation of dispatch_command_cb to not
   // return an error when the number of robots do not match the number in
   // Assignments.
   struct Restored
   {
-    std::vector<TaskManager> managers;
+    std::unordered_map<std::string, TaskManager> managers;
     BidNoticeAssignments bid_notice_assignments;
   };
 
+  // TODO(YV): Should we accept a bool to delete/drop all tables?
   static std::shared_ptr<DatabaseLogger> make(
     const std::string& file_path);
 
   // Returns nullopt if file_path did not exist previously
   std::optional<Restored> restore() const;
 
-  // TODO(YV): Consider having internal_FleetUpdateHandle and TaskManager
-  // receive these functions as callbacks to trigger on updates
-  // Backup bid_notice_assignments
-  void backup(const BidNoticeAssignments& assignments);
+  void backup_bid_notice_assignments(const BidNoticeAssignments& assignments);
 
-  // Backup active task state along with task queues
-  void backup(const TaskManager& mgr);
+  void backup_task_queues(TaskManager& mgr);
+
+  void backup_active_task(
+    const std::string& robot,
+    const nlohmann::json& task_state);
+
+  void backup_task_logs(
+    const std::string& robot,
+    const nlohmann::json& task_logs);
+
+  // Backup the TaskManager. By default this will only backup the direct and
+  // dispatch queues. The active_task and task_logs will be backed up if
+  // provided.
+  // void backup(
+  //   const TaskManager& mgr,
+  //   std::optional<nlohmann::json> active_task = std::nullopt,
+  //   std::optional<nlohmann::json> task_logs = std::nullopt);
 
   // Do not allow copying or moving
   // DatabaseLogger(const DatabaseLogger&) = delete;
@@ -82,12 +93,15 @@ class DatabaseLogger
 
   // Helper functions to serialize/deserialize assignments
   // TODO(YV): Consider formalizing the schema in rmf_fleet_adapter/schemas
-  nlohmann::json _convert(const Assignment& assignment);
-  Assignment _convert(const nlohmann::json& msg);
+  nlohmann::json convert(const Assignment& assignment);
+  Assignment convert(const nlohmann::json& msg);
+
+  // TODO(YV): Explore using an open-source library to build sql statements
 
   std::string _file_path;
   sqlite3* _db;
   std::mutex _mutex;
+  // std::optional<Restored> _restored = std::nullopt;
 };
 
 } // namespace rmf_fleet_adapter
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
index 41e123946..64eed214e 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
@@ -530,6 +530,16 @@ void TaskManager::ActiveTask::publish_task_state(TaskManager& mgr)
   static const auto log_update_validator =
     mgr._make_validator(rmf_api_msgs::schemas::task_log_update);
   mgr._validate_and_publish_websocket(task_log_update, log_update_validator);
+
+  // Backup task state and logs
+  // TOOO(YV): Validate first
+  auto fleet_handle = mgr._fleet_handle.lock();
+  if (!fleet_handle)
+    return;
+  const auto impl = agv::FleetUpdateHandle::Implementation::get(*fleet_handle);
+  const std::string& robot = mgr._context->name();
+  impl.db->backup_active_task(robot, _state_msg);
+  impl.db->backup_task_logs(robot, task_logs);
 }
 
 //==============================================================================
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
index edf05f809..6b92af73e 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
@@ -229,6 +229,7 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
   };
 
   friend class ActiveTask;
+  friend class DatabaseLogger;
 
   agv::RobotContextPtr _context;
   std::optional<std::weak_ptr<BroadcastClient>> _broadcast_client;

From 681e416c411f731f22d99de55845178db618c09d Mon Sep 17 00:00:00 2001
From: Yadunund <yadunund@openrobotics.org>
Date: Tue, 1 Feb 2022 01:31:38 +0800
Subject: [PATCH 76/79] Tested logging active task and task logs

Signed-off-by: Yadunund <yadunund@openrobotics.org>
---
 rmf_fleet_adapter/src/full_control/main.cpp   |  4 +-
 .../src/rmf_fleet_adapter/DatabaseLogger.cpp  | 84 +++++++++----------
 .../src/rmf_fleet_adapter/DatabaseLogger.hpp  | 16 +---
 .../src/rmf_fleet_adapter/TaskManager.cpp     |  8 +-
 4 files changed, 53 insertions(+), 59 deletions(-)

diff --git a/rmf_fleet_adapter/src/full_control/main.cpp b/rmf_fleet_adapter/src/full_control/main.cpp
index 010b8726f..fe3a77a9d 100644
--- a/rmf_fleet_adapter/src/full_control/main.cpp
+++ b/rmf_fleet_adapter/src/full_control/main.cpp
@@ -899,8 +899,10 @@ std::shared_ptr<Connections> make_fleet(
     server_uri = uri;
   }
 
+  // TODO(YV): Remove this before merging. Replace with declare_param
+  const std::string& db_path = fleet_name + ".db";
   connections->fleet = adapter->add_fleet(
-    fleet_name, *connections->traits, *connections->graph, server_uri);
+    fleet_name, *connections->traits, *connections->graph, server_uri, db_path);
 
   // We disable fleet state publishing for this fleet adapter because we expect
   // the fleet drivers to publish these messages.
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.cpp
index b45f1dc4d..8fa7a1e15 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.cpp
@@ -20,6 +20,7 @@
 #include <filesystem>
 #include <fstream>
 
+// TODO(YV) Remove
 #include <iostream>
 
 namespace rmf_fleet_adapter {
@@ -46,28 +47,21 @@ std::shared_ptr<DatabaseLogger>
   if (error)
     return nullptr;
 
-  auto table_exists =
-    [logger](const std::string& name) -> bool
-    {
-      char* error_msg;
-      const std::string query = "SELECT name FROM sqlite_master "
-      "WHERE type='table' AND name='" + name +"';";
-      auto error = sqlite3_exec(logger->_db, query.c_str(), NULL, 0, &error_msg);
-      // TOOD(YV): Distinguish between error code from bad statement and table
-      // not existing.
-      return !error;
-    };
-
-  // Create tables if they do not already exist
-  // Active Task Table
-  if (!table_exists("ACTIVE_TASK"))
-  {
-    sql = "CREATE TABLE ACTIVE_TASK("
-          "ROBOT TEXT PRIMARY KEY NOT NULL, "
-          "STATE TEXT NOT NULL);";
-    error = sqlite3_exec(logger->_db, sql.c_str(), NULL, 0, &error_msg);
-    assert(!error);
-  }
+  // Create table for active task states
+  sql = "CREATE TABLE IF NOT EXISTS ACTIVE_TASK("
+        "ROBOT TEXT PRIMARY KEY NOT NULL, "
+        "STATE TEXT NOT NULL);";
+  error = sqlite3_exec(logger->_db, sql.c_str(), NULL, NULL, &error_msg);
+  if (error)
+    std::cout << "Error from creating ACTIVE_TASK table: " << error_msg << std::endl;
+
+  // Create table for task logs
+  sql = "CREATE TABLE IF NOT EXISTS TASK_LOGS("
+        "ID TEXT PRIMARY KEY NOT NULL, "
+        "LOGS TEXT NOT NULL);";
+  error = sqlite3_exec(logger->_db, sql.c_str(), NULL, NULL, &error_msg);
+  if (error)
+    std::cout << "Error from creating TASK_LOGS table: " << error_msg << std::endl;
 
   // TODO(YV): Tables for task queues, task logs and bid notice assignments
 
@@ -78,6 +72,8 @@ std::shared_ptr<DatabaseLogger>
   // TODO(YV): If db is not empty, generate Restore. For now we will set it to
   // nullopt.
 
+  std::cout << "Created database" << std::endl;
+
   return logger;
 }
 
@@ -121,26 +117,13 @@ void DatabaseLogger::backup_active_task(
   std::lock_guard<std::mutex> lock(_mutex);
   const std::string state = task_state.dump();
 
-  const std::string sql = "IF EXISTS(SELECT * FROM ACTIVE_TASK WHERE ROBOT=" +
-  robot +")" + "UPDATE ACTIVE_TASK SET STATE=" + state + "WHERE ROBOT=" + robot +
-  "ELSE INSERT INTO ACTIVE_TASK(" + robot +") values(" + state + ");";
-  char* error_msg;
-
-  auto print_table =
-    [](void* data, int num_col, char** col_values, char** col_names)
-    {
-      int i;
-      fprintf(stderr, "%s: ", (const char*)data);
-
-      for (i = 0; i < num_col; i++) {
-          printf("%s = %s\n", col_names[i], col_values[i] ? col_values[i] : "NULL");
-      }
-
-      printf("\n");
-      return 0;
-    };
-  auto error = sqlite3_exec(_db, sql.c_str(), print_table, 0, &error_msg);
+  const std::string sql = "REPLACE INTO ACTIVE_TASK (ROBOT,STATE) "
+    "VALUES('" + robot + "', '" + state + "');";
 
+  char* error_msg;
+  auto error = sqlite3_exec(_db, sql.c_str(), NULL, NULL, &error_msg);
+  if (error)
+    std::cout << "Error updating active task: " << error_msg << std::endl;
 }
 
 //==============================================================================
@@ -148,7 +131,24 @@ void DatabaseLogger::backup_task_logs(
     const std::string& robot,
     const nlohmann::json& task_logs)
 {
-  // TODO
+  std::lock_guard<std::mutex> lock(_mutex);
+  const auto it = task_logs.find("task_id");
+  if (it == task_logs.end())
+    return;
+  const auto& task_id = it.value();
+  if (!task_id.is_string())
+    return;
+
+  const std::string id = "(" + robot + "," + task_id.get<std::string>() + ")";
+  const std::string logs = task_logs.dump();
+
+  const std::string sql = "REPLACE INTO TASK_LOGS (ID,LOGS) "
+    "VALUES('" + id + "', '" + logs + "');";
+
+  char* error_msg;
+  auto error = sqlite3_exec(_db, sql.c_str(), NULL, NULL, &error_msg);
+  if (error)
+    std::cout << "Error updating active task: " << error_msg << std::endl;
 }
 
 //==============================================================================
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.hpp
index b10efbbad..2508d318d 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.hpp
@@ -64,28 +64,16 @@ class DatabaseLogger
 
   void backup_task_queues(TaskManager& mgr);
 
+  // Key used: robot
   void backup_active_task(
     const std::string& robot,
     const nlohmann::json& task_state);
 
+  // Key used: (robot,task_id)
   void backup_task_logs(
     const std::string& robot,
     const nlohmann::json& task_logs);
 
-  // Backup the TaskManager. By default this will only backup the direct and
-  // dispatch queues. The active_task and task_logs will be backed up if
-  // provided.
-  // void backup(
-  //   const TaskManager& mgr,
-  //   std::optional<nlohmann::json> active_task = std::nullopt,
-  //   std::optional<nlohmann::json> task_logs = std::nullopt);
-
-  // Do not allow copying or moving
-  // DatabaseLogger(const DatabaseLogger&) = delete;
-  // DatabaseLogger& operator=(const DatabaseLogger&) = delete;
-  // DatabaseLogger(DatabaseLogger&&) = delete;
-  // DatabaseLogger& operator=(DatabaseLogger&&) = delete;
-
   ~DatabaseLogger();
 
 private:
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
index 64eed214e..fc1898e30 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
@@ -538,8 +538,12 @@ void TaskManager::ActiveTask::publish_task_state(TaskManager& mgr)
     return;
   const auto impl = agv::FleetUpdateHandle::Implementation::get(*fleet_handle);
   const std::string& robot = mgr._context->name();
-  impl.db->backup_active_task(robot, _state_msg);
-  impl.db->backup_task_logs(robot, task_logs);
+  if (impl.db)
+  {
+    impl.db->backup_active_task(robot, _state_msg);
+    impl.db->backup_task_logs(robot, task_logs);
+  }
+
 }
 
 //==============================================================================

From 0947106fb2dd717e67f93fd6a3a7abcdb0bed75b Mon Sep 17 00:00:00 2001
From: Yadunund <yadunund@openrobotics.org>
Date: Tue, 1 Feb 2022 02:59:36 +0800
Subject: [PATCH 77/79] Backup dispatch and direct queues

Signed-off-by: Yadunund <yadunund@openrobotics.org>
---
 .../include/rmf_fleet_adapter/agv/Adapter.hpp |   5 +
 .../src/rmf_fleet_adapter/DatabaseLogger.cpp  | 166 +++++++++++++++++-
 .../src/rmf_fleet_adapter/DatabaseLogger.hpp  |  19 +-
 .../src/rmf_fleet_adapter/TaskManager.cpp     |   6 +-
 .../agv/FleetUpdateHandle.cpp                 |   3 +
 .../agv/internal_FleetUpdateHandle.hpp        |   5 +
 6 files changed, 188 insertions(+), 16 deletions(-)

diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp
index 2fecb815a..fa813fe40 100644
--- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp
+++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp
@@ -103,6 +103,11 @@ class Adapter : public std::enable_shared_from_this<Adapter>
   ///   exists the fleet handle will be restored to the last backup state. If
   ///   the file does not exist, a new one will be created.
   ///   If nullopt, the fleet adapter will not perform any backup/restore.
+  ///
+  /// TODO(YV): Add this param
+  /// \param[in] restore_from_backup
+  ///   If true, fleet adapter will be restored to last known backup state.
+  ///   [Not implemented yet]
   std::shared_ptr<FleetUpdateHandle> add_fleet(
     const std::string& fleet_name,
     rmf_traffic::agv::VehicleTraits traits,
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.cpp
index 8fa7a1e15..ffdf69557 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.cpp
@@ -16,6 +16,7 @@
 */
 
 #include "DatabaseLogger.hpp"
+#include "agv/internal_FleetUpdateHandle.hpp"
 
 #include <filesystem>
 #include <fstream>
@@ -23,6 +24,16 @@
 // TODO(YV) Remove
 #include <iostream>
 
+//==============================================================================
+// int sqlite3_exec(
+//   sqlite3*,                                  /* An open database */
+//   const char *sql,                           /* SQL to be evaluated */
+//   int (*callback)(void*,int,char**,char**),  /* Callback function */
+//   void *,                                    /* 1st argument to callback */
+//   char **errmsg                              /* Error msg written here */
+// );
+//==============================================================================
+
 namespace rmf_fleet_adapter {
 //==============================================================================
 auto DatabaseLogger::make(const std::string& file_path) ->
@@ -42,7 +53,6 @@ std::shared_ptr<DatabaseLogger>
   std::string sql;
   char* error_msg;
 
-
   error = sqlite3_open(file_path.c_str(), &logger->_db);
   if (error)
     return nullptr;
@@ -63,7 +73,18 @@ std::shared_ptr<DatabaseLogger>
   if (error)
     std::cout << "Error from creating TASK_LOGS table: " << error_msg << std::endl;
 
-  // TODO(YV): Tables for task queues, task logs and bid notice assignments
+  // Create table for task logs
+  // DISPATCH and DIRECT will be jsons containing deserialized assignment jsons
+  sql = "CREATE TABLE IF NOT EXISTS TASK_QUEUES("
+        "ROBOT TEXT PRIMARY KEY NOT NULL, "
+        "DISPATCH TEXT NOT NULL, "
+        "DIRECT TEXT NOT NULL);";
+  error = sqlite3_exec(logger->_db, sql.c_str(), NULL, NULL, &error_msg);
+  if (error)
+    std::cout << "Error from creating TASK_QUEUES table: " << error_msg << std::endl;
+
+
+  // TODO(YV): Table for bid notice assignments
 
   // TODO(YV): Also store history of task & state logs along with latest state.
   // Proposed key (robot, task, phase, event, seq)
@@ -86,7 +107,7 @@ DatabaseLogger::DatabaseLogger()
 //==============================================================================
 auto DatabaseLogger::restore() const -> std::optional<Restored>
 {
-  return std::nullopt;
+  return _restored;
 }
 
 //==============================================================================
@@ -104,9 +125,62 @@ void DatabaseLogger::backup_bid_notice_assignments(
 }
 
 //==============================================================================
-void DatabaseLogger::backup_task_queues(TaskManager& mgr)
+void DatabaseLogger::backup_task_queues(const TaskManager& mgr)
 {
-  // TODO
+  // We can access private members of TaskManager as DatabaseLogger is a friend
+
+  auto fleet_handle = mgr._fleet_handle.lock();
+  if (!fleet_handle)
+  {
+    std::cout << "Unable to lock fleet handle inside backup_task_queues"
+              << std::endl;
+    return;
+  }
+  auto& impl =
+    agv::FleetUpdateHandle::Implementation::get(*fleet_handle);
+  const std::string& robot = mgr._context->name();
+  const auto& request_jsons = impl.task_request_jsons;
+  std::vector<std::string> errors;
+
+  nlohmann::json queue;
+  std::vector<nlohmann::json> assignments;
+  for (const auto& assignment : mgr._queue)
+  {
+    assignments.push_back(convert(assignment, request_jsons));
+  }
+  queue = assignments;
+  // Write to db
+  {
+  std::lock_guard<std::mutex> lock(_mutex);
+  std::string data = queue.dump();
+  std::string sql = "REPLACE INTO TASK_QUEUES (ROBOT,DISPATCH) "
+    "VALUES('" + robot + "', '" + data + "');";
+  char* error_msg;
+  auto error = sqlite3_exec(_db, sql.c_str(), NULL, NULL, &error_msg);
+  if (error)
+    std::cout << "Error updating dispatch queue: " << error_msg << std::endl;
+  }
+
+  // Direct assignments
+  queue = {};
+  assignments.clear();
+  for (const auto& assignment : mgr._direct_queue)
+  {
+    assignments.push_back(convert(assignment.assignment, request_jsons));
+  }
+  queue = assignments;
+  // Write to db
+  {
+  std::lock_guard<std::mutex> lock(_mutex);
+  std::string data = queue.dump();
+  std::string sql = "REPLACE INTO TASK_QUEUES (ROBOT,DIRECT) "
+    "VALUES('" + robot + "', '" + data + "');";
+  char* error_msg;
+  auto error = sqlite3_exec(_db, sql.c_str(), NULL, NULL, &error_msg);
+  if (error)
+    std::cout << "Error updating dispatch queue: " << error_msg << std::endl;
+  }
+
 }
 
 //==============================================================================
@@ -119,7 +193,6 @@ void DatabaseLogger::backup_active_task(
 
   const std::string sql = "REPLACE INTO ACTIVE_TASK (ROBOT,STATE) "
     "VALUES('" + robot + "', '" + state + "');";
-
   char* error_msg;
   auto error = sqlite3_exec(_db, sql.c_str(), NULL, NULL, &error_msg);
   if (error)
@@ -144,24 +217,99 @@ void DatabaseLogger::backup_task_logs(
 
   const std::string sql = "REPLACE INTO TASK_LOGS (ID,LOGS) "
     "VALUES('" + id + "', '" + logs + "');";
-
   char* error_msg;
   auto error = sqlite3_exec(_db, sql.c_str(), NULL, NULL, &error_msg);
   if (error)
     std::cout << "Error updating active task: " << error_msg << std::endl;
 }
 
+namespace {
+//==============================================================================
+std::chrono::milliseconds to_millis(rmf_traffic::Duration duration)
+{
+  return std::chrono::duration_cast<std::chrono::milliseconds>(duration);
+}
+
+} // namespace anonymous
+
+//==============================================================================
+nlohmann::json DatabaseLogger::convert(const rmf_task::State& state) const
+{
+  // State schmea
+  // {
+  //   waypoint: integer,
+  //   orientation: double,[optional]
+  //   time: integer
+  //   charger: integer
+  //   battery_soc
+  // }
+
+  nlohmann::json msg;
+
+  // To be safe we check if the optionals have value but they definitely should
+  auto waypoint_opt = state.waypoint();
+  if (waypoint_opt.has_value())
+    msg["waypoint"] = waypoint_opt.value();
+  auto orientation_opt = state.orientation();
+  if (orientation_opt.has_value())
+    msg["orientation"] = orientation_opt.value();
+  auto time_opt = state.time();
+  if (time_opt.has_value())
+    msg["time"] = to_millis(time_opt.value().time_since_epoch()).count();
+  auto charger_opt = state.dedicated_charging_waypoint();
+  if (charger_opt.has_value())
+    msg["charger"] = charger_opt.value();
+  auto soc_opt = state.battery_soc();
+  if (soc_opt.has_value())
+    msg["battery_soc"] = soc_opt.value();
+
+  return msg;
+}
+
 //==============================================================================
-nlohmann::json DatabaseLogger::convert(const Assignment& assignment)
+nlohmann::json DatabaseLogger::convert(
+  const Assignment& assignment,
+  const std::unordered_map<std::string, nlohmann::json> request_jsons) const
 {
+
+  // Assignment schema
+  // {
+  //   request: ref rmf_api_msgs::schemas::task_request,
+  //   state: ref State schema,
+  //   unix_millis_deployment_time: integer,
+  // }
+
   nlohmann::json msg;
+  const auto request = assignment.request();
+  const auto& state = assignment.finish_state();
+  const auto deployment_time = assignment.deployment_time();
+
+  msg["state"] = convert(state);
+  msg["unix_millis_deployment_time"] =
+    to_millis(deployment_time.time_since_epoch()).count();
+
+  const auto it = request_jsons.find(request->booking()->id());
+  if (it != request_jsons.end())
+  {
+    msg["request"] = it->second;
+  }
+  else
+  {
+    // TODO(YV): Deserialize this automatic task into schemas::task_request
+  }
+
   return msg;
 }
 
 //==============================================================================
-auto DatabaseLogger::convert(const nlohmann::json& msg) -> Assignment
+auto DatabaseLogger::convert(const nlohmann::json& msg) const -> Assignment
 {
+  // msg follows the Assignment schema defined above
+
+  // TODO(YV): Fix
   rmf_task::State state;
+  // We can use FleetUpdateHandle::Implementation::convert(json_requst) to get
+  // ConstRequestPtr
   Assignment assignment{nullptr, state, std::chrono::steady_clock::now()};
   return assignment;
 }
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.hpp
index 2508d318d..2405e193c 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.hpp
@@ -49,7 +49,7 @@ class DatabaseLogger
   // Assignments.
   struct Restored
   {
-    std::unordered_map<std::string, TaskManager> managers;
+    std::unordered_map<std::string, std::shared_ptr<TaskManager>> managers;
     BidNoticeAssignments bid_notice_assignments;
   };
 
@@ -57,12 +57,14 @@ class DatabaseLogger
   static std::shared_ptr<DatabaseLogger> make(
     const std::string& file_path);
 
-  // Returns nullopt if file_path did not exist previously
+  // Returns nullopt if file_path did not exist previously.
+  // Does this function require a weak_ptr<FleetUpdateHandle> to generating
+  // requests/deserialization/validation?
   std::optional<Restored> restore() const;
 
   void backup_bid_notice_assignments(const BidNoticeAssignments& assignments);
 
-  void backup_task_queues(TaskManager& mgr);
+  void backup_task_queues(const TaskManager& mgr);
 
   // Key used: robot
   void backup_active_task(
@@ -81,15 +83,20 @@ class DatabaseLogger
 
   // Helper functions to serialize/deserialize assignments
   // TODO(YV): Consider formalizing the schema in rmf_fleet_adapter/schemas
-  nlohmann::json convert(const Assignment& assignment);
-  Assignment convert(const nlohmann::json& msg);
+  nlohmann::json convert(const rmf_task::State& state) const;
+
+  nlohmann::json convert(
+    const Assignment& assignment,
+    const std::unordered_map<std::string, nlohmann::json> request_jsons) const;
+
+  Assignment convert(const nlohmann::json& msg) const;
 
   // TODO(YV): Explore using an open-source library to build sql statements
 
   std::string _file_path;
   sqlite3* _db;
   std::mutex _mutex;
-  // std::optional<Restored> _restored = std::nullopt;
+  std::optional<Restored> _restored = std::nullopt;
 };
 
 } // namespace rmf_fleet_adapter
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
index fc1898e30..51832a8f4 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
@@ -1763,7 +1763,7 @@ void TaskManager::_handle_direct_request(
   auto fleet_handle = _fleet_handle.lock();
   if (!fleet_handle)
     return;
-  const auto& impl =
+  auto& impl =
     agv::FleetUpdateHandle::Implementation::get(*fleet_handle);
   std::vector<std::string> errors;
   const auto new_request = impl.convert(request_id, request, errors);
@@ -1788,6 +1788,10 @@ void TaskManager::_handle_direct_request(
 
     return;
   }
+
+  // Cache for backups
+  impl.task_request_jsons.insert({request_id, request});
+
   // Generate Assignment for the request
   const auto task_planner = _context->task_planner();
   if (!task_planner)
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
index d84d5ba6e..95d5df67d 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
@@ -339,6 +339,9 @@ void FleetUpdateHandle::Implementation::bid_notice_cb(
       });
   }
 
+  // Cache request_msg
+  task_request_jsons.insert({task_id, request_msg});
+
   // TODO(MXG): Make the task planning asynchronous. The worker should schedule
   // a job to perform the planning which should then spawn a job to save the
   // plan result and respond. I started to refactor allocate_tasks(~) to make it
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
index a4bc43dec..e10da1a7d 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp
@@ -283,6 +283,11 @@ class FleetUpdateHandle::Implementation
   // TODO Support for various charging configurations
   std::unordered_set<std::size_t> charging_waypoints = {};
 
+  // Map task_id to schemas::task_reqest.json
+  // TODO(YV): How would we insert request jsons for automatic tasks?
+  // Perhaps we should just deserialize in DatabaseLogger
+  std::unordered_map<std::string, nlohmann::json> task_request_jsons = {};
+
   std::shared_ptr<rmf_task_ros2::bidding::AsyncBidder> bidder = nullptr;
 
   double current_assignment_cost = 0.0;

From 060e88fc22ed412eea6c0f0b78d0b31d8180a666 Mon Sep 17 00:00:00 2001
From: Yadunund <yadunund@openrobotics.org>
Date: Tue, 1 Feb 2022 03:20:43 +0800
Subject: [PATCH 78/79] Backup task queues

Signed-off-by: Yadunund <yadunund@openrobotics.org>
---
 .../src/rmf_fleet_adapter/DatabaseLogger.cpp  |  4 +--
 .../src/rmf_fleet_adapter/TaskManager.cpp     | 29 +++++++++++++++++++
 2 files changed, 31 insertions(+), 2 deletions(-)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.cpp
index ffdf69557..671ffefbd 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.cpp
@@ -77,8 +77,8 @@ std::shared_ptr<DatabaseLogger>
   // DISPATCH and DIRECT will be jsons containing deserialized assignment jsons
   sql = "CREATE TABLE IF NOT EXISTS TASK_QUEUES("
         "ROBOT TEXT PRIMARY KEY NOT NULL, "
-        "DISPATCH TEXT NOT NULL, "
-        "DIRECT TEXT NOT NULL);";
+        "DISPATCH TEXT, "
+        "DIRECT TEXT);";
   error = sqlite3_exec(logger->_db, sql.c_str(), NULL, NULL, &error_msg);
   if (error)
     std::cout << "Error from creating TASK_QUEUES table: " << error_msg << std::endl;
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
index 51832a8f4..718ccfc55 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
@@ -851,6 +851,15 @@ void TaskManager::set_queue(
     _publish_task_queue();
   }
 
+  // Backup queue
+  auto fleet_handle = _fleet_handle.lock();
+  if (fleet_handle)
+  {
+    const auto& impl =
+      agv::FleetUpdateHandle::Implementation::get(*fleet_handle);
+    impl.db->backup_task_queues(*this);
+  }
+
   _begin_next_task();
 }
 
@@ -970,6 +979,15 @@ void TaskManager::_begin_next_task()
    else
       _queue.erase(_queue.begin());
 
+    // Backup queue
+    auto fleet_handle = _fleet_handle.lock();
+    if (fleet_handle)
+    {
+      const auto& impl =
+        agv::FleetUpdateHandle::Implementation::get(*fleet_handle);
+      impl.db->backup_task_queues(*this);
+    }
+
     if (!_active_task)
     {
       const auto info = assignment.request()->description()->generate_info(
@@ -1205,6 +1223,14 @@ void TaskManager::retreat_to_charger()
     _direct_queue.insert(assignment);
     }
 
+    auto fleet_handle = _fleet_handle.lock();
+    if (fleet_handle)
+    {
+      const auto& impl =
+        agv::FleetUpdateHandle::Implementation::get(*fleet_handle);
+      impl.db->backup_task_queues(*this);
+    }
+
     RCLCPP_INFO(
       _context->node()->get_logger(),
       "Initiating automatic retreat to charger for robot [%s]",
@@ -1848,6 +1874,9 @@ void TaskManager::_handle_direct_request(
   _direct_queue.insert(assignment);
   }
 
+  // Backup direct queue
+  impl.db->backup_task_queues(*this);
+
   RCLCPP_INFO(
     _context->node()->get_logger(),
     "Direct request [%s] successfully queued for robot [%s]",

From 4f0e58cd30a605f314c358619831e3dea949409a Mon Sep 17 00:00:00 2001
From: Yadunund <yadunund@openrobotics.org>
Date: Tue, 1 Feb 2022 03:48:54 +0800
Subject: [PATCH 79/79] Update task queue table

Signed-off-by: Yadunund <yadunund@openrobotics.org>
---
 .../src/rmf_fleet_adapter/DatabaseLogger.cpp  | 45 +++++--------------
 .../src/rmf_fleet_adapter/DatabaseLogger.hpp  |  4 +-
 .../src/rmf_fleet_adapter/TaskManager.cpp     |  2 +-
 3 files changed, 13 insertions(+), 38 deletions(-)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.cpp
index 671ffefbd..33a1e3495 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.cpp
@@ -77,8 +77,7 @@ std::shared_ptr<DatabaseLogger>
   // DISPATCH and DIRECT will be jsons containing deserialized assignment jsons
   sql = "CREATE TABLE IF NOT EXISTS TASK_QUEUES("
         "ROBOT TEXT PRIMARY KEY NOT NULL, "
-        "DISPATCH TEXT, "
-        "DIRECT TEXT);";
+        "QUEUES TEXT NOT NULL);";
   error = sqlite3_exec(logger->_db, sql.c_str(), NULL, NULL, &error_msg);
   if (error)
     std::cout << "Error from creating TASK_QUEUES table: " << error_msg << std::endl;
@@ -142,43 +141,28 @@ void DatabaseLogger::backup_task_queues(const TaskManager& mgr)
   const auto& request_jsons = impl.task_request_jsons;
   std::vector<std::string> errors;
 
-  nlohmann::json queue;
-  std::vector<nlohmann::json> assignments;
+  nlohmann::json queues = {{"dispatch",{}}, {"direct",{}}};
   for (const auto& assignment : mgr._queue)
   {
-    assignments.push_back(convert(assignment, request_jsons));
-  }
-  queue = assignments;
-  // Write to db
-  {
-  std::lock_guard<std::mutex> lock(_mutex);
-  std::string data = queue.dump();
-  std::string sql = "REPLACE INTO TASK_QUEUES (ROBOT,DISPATCH) "
-    "VALUES('" + robot + "', '" + data + "');";
-  char* error_msg;
-  auto error = sqlite3_exec(_db, sql.c_str(), NULL, NULL, &error_msg);
-  if (error)
-    std::cout << "Error updating dispatch queue: " << error_msg << std::endl;
+    queues["dispatch"].push_back(convert(assignment, request_jsons));
   }
 
   // Direct assignments
-  queue = {};
-  assignments.clear();
+
   for (const auto& assignment : mgr._direct_queue)
   {
-    assignments.push_back(convert(assignment.assignment, request_jsons));
+    queues["direct"].push_back(convert(assignment.assignment, request_jsons));
   }
-  queue = assignments;
   // Write to db
   {
   std::lock_guard<std::mutex> lock(_mutex);
-  std::string data = queue.dump();
-  std::string sql = "REPLACE INTO TASK_QUEUES (ROBOT,DIRECT) "
+  std::string data = queues.dump();
+  std::string sql = "REPLACE INTO TASK_QUEUES (ROBOT,QUEUES) "
     "VALUES('" + robot + "', '" + data + "');";
   char* error_msg;
   auto error = sqlite3_exec(_db, sql.c_str(), NULL, NULL, &error_msg);
   if (error)
-    std::cout << "Error updating dispatch queue: " << error_msg << std::endl;
+    std::cout << "Error updating task queue: " << error_msg << std::endl;
   }
 
 }
@@ -201,22 +185,13 @@ void DatabaseLogger::backup_active_task(
 
 //==============================================================================
 void DatabaseLogger::backup_task_logs(
-    const std::string& robot,
+    const std::string& task_id,
     const nlohmann::json& task_logs)
 {
   std::lock_guard<std::mutex> lock(_mutex);
-  const auto it = task_logs.find("task_id");
-  if (it == task_logs.end())
-    return;
-  const auto& task_id = it.value();
-  if (!task_id.is_string())
-    return;
-
-  const std::string id = "(" + robot + "," + task_id.get<std::string>() + ")";
   const std::string logs = task_logs.dump();
-
   const std::string sql = "REPLACE INTO TASK_LOGS (ID,LOGS) "
-    "VALUES('" + id + "', '" + logs + "');";
+    "VALUES('" + task_id + "', '" + logs + "');";
   char* error_msg;
   auto error = sqlite3_exec(_db, sql.c_str(), NULL, NULL, &error_msg);
   if (error)
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.hpp
index 2405e193c..93d9b267e 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.hpp
@@ -71,9 +71,9 @@ class DatabaseLogger
     const std::string& robot,
     const nlohmann::json& task_state);
 
-  // Key used: (robot,task_id)
+  // Key used: (task_id)
   void backup_task_logs(
-    const std::string& robot,
+    const std::string& task_id,
     const nlohmann::json& task_logs);
 
   ~DatabaseLogger();
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
index 718ccfc55..39565e224 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
@@ -541,7 +541,7 @@ void TaskManager::ActiveTask::publish_task_state(TaskManager& mgr)
   if (impl.db)
   {
     impl.db->backup_active_task(robot, _state_msg);
-    impl.db->backup_task_logs(robot, task_logs);
+    impl.db->backup_task_logs(booking.id(), task_logs);
   }
 
 }