diff --git a/rmf_fleet_adapter/CHANGELOG.md b/rmf_fleet_adapter/CHANGELOG.md
index 60c4ed9e3..a425053b4 100644
--- a/rmf_fleet_adapter/CHANGELOG.md
+++ b/rmf_fleet_adapter/CHANGELOG.md
@@ -2,6 +2,7 @@
 
 2.1.0 (2022-XX-YY)
 ------------------
+* Add APIs for cancelling and killing tasks from the `RobotUpdateHandle`: [#205](https://github.com/open-rmf/rmf_ros2/pull/205)
 * Add a WaitUntil event and use it for ResponsiveWait: [#199](https://github.com/open-rmf/rmf_ros2/pull/199)
 
 2.0.0 (2022-03-18)
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 d671c944c..79cadb5a3 100644
--- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp
+++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp
@@ -19,6 +19,7 @@
 #define RMF_FLEET_ADAPTER__AGV__ROBOTUPDATEHANDLE_HPP
 
 #include <rmf_traffic/Time.hpp>
+#include <rmf_traffic/agv/Planner.hpp>
 #include <rmf_utils/impl_ptr.hpp>
 #include <rmf_utils/optional.hpp>
 
@@ -92,6 +93,10 @@ class RobotUpdateHandle
     const double max_merge_lane_distance = 1.0,
     const double min_lane_length = 1e-8);
 
+  /// Update the current position of the robot by specifying a plan start set
+  /// for it.
+  void update_position(rmf_traffic::agv::Plan::StartSet position);
+
   /// Set the waypoint where the charger for this robot is located.
   /// If not specified, the nearest waypoint in the graph with the is_charger()
   /// property will be assumed as the charger for this robot.
@@ -218,6 +223,42 @@ class RobotUpdateHandle
     std::vector<std::string> labels,
     std::function<void()> robot_is_interrupted);
 
+  /// Cancel a task, if it has been assigned to this robot
+  ///
+  /// \param[in] task_id
+  ///   The ID of the task to be canceled
+  ///
+  /// \param[in] labels
+  ///   Labels that will be assigned to this cancellation. It is recommended to
+  ///   include information about why the cancellation is happening.
+  ///
+  /// \param[in] on_cancellation
+  ///   Callback that will be triggered after the cancellation is issued.
+  ///   task_was_found will be true if the task was successfully found and
+  ///   issued the cancellation, false otherwise.
+  void cancel_task(
+    std::string task_id,
+    std::vector<std::string> labels,
+    std::function<void(bool task_was_found)> on_cancellation);
+
+  /// Kill a task, if it has been assigned to this robot
+  ///
+  /// \param[in] task_id
+  ///   The ID of the task to be canceled
+  ///
+  /// \param[in] labels
+  ///   Labels that will be assigned to this cancellation. It is recommended to
+  ///   include information about why the cancellation is happening.
+  ///
+  /// \param[in] on_kill
+  ///   Callback that will be triggered after the cancellation is issued.
+  ///   task_was_found will be true if the task was successfully found and
+  ///   issued the kill, false otherwise.
+  void kill_task(
+    std::string task_id,
+    std::vector<std::string> labels,
+    std::function<void(bool task_was_found)> on_kill);
+
   enum class Tier
   {
     /// General status information, does not require special attention
diff --git a/rmf_fleet_adapter/schemas/event_description__go_to_place.json b/rmf_fleet_adapter/schemas/event_description__go_to_place.json
index c325b4739..9e0d5a1f8 100644
--- a/rmf_fleet_adapter/schemas/event_description__go_to_place.json
+++ b/rmf_fleet_adapter/schemas/event_description__go_to_place.json
@@ -3,5 +3,20 @@
   "$id": "https://raw.githubusercontent.com/open-rmf/rmf_ros2/main/rmf_fleet_adapter/schemas/event_description__go_to_place.json",
   "title": "Go To Place Event Description",
   "description": "Have a robot go to a place",
-  "$ref": "place.json"
+  "oneOf": [
+    {
+      "$ref": "place.json"
+    },
+    {
+      "type": "object",
+      "properties": {
+        "place": { "$ref": "place.json" },
+        "followed_by": {
+          "description": "A list of places that the robot might go after it reaches this one. It will not actually go to these places unless other activities bring it there, but the traffic system will be told to expect the robot to proceed through these places.",
+          "type": "array",
+          "items": { "$ref": "place.json" }
+        }
+      }
+    }
+  ]
 }
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
index 184fd276d..6f45d659e 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
@@ -1077,10 +1077,10 @@ nlohmann::json TaskManager::submit_direct_request(
 //==============================================================================
 void TaskManager::Interruption::resume(std::vector<std::string> labels)
 {
+  std::lock_guard<std::mutex> lock(mutex);
   if (resumed)
     return;
 
-  std::lock_guard<std::mutex> lock(mutex);
   resumed = true;
   if (const auto mgr = w_mgr.lock())
   {
@@ -1136,6 +1136,64 @@ void TaskManager::interrupt_robot(
   _process_robot_interrupts();
 }
 
+namespace {
+//==============================================================================
+std::vector<std::string> get_labels(const nlohmann::json& request)
+{
+  const auto labels_it = request.find("labels");
+  if (labels_it != request.end())
+    return labels_it->get<std::vector<std::string>>();
+
+  return {};
+}
+} // anonymous namespace
+
+//==============================================================================
+bool TaskManager::cancel_task(
+  const std::string& task_id,
+  std::vector<std::string> labels)
+{
+  if (_active_task && _active_task.id() == task_id)
+  {
+    _task_state_update_available = true;
+    _active_task.cancel(std::move(labels), _context->now());
+    return true;
+  }
+
+  // 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 (_cancel_task_from_dispatch_queue(task_id, labels))
+    return true;
+
+  if (_cancel_task_from_direct_queue(task_id, labels))
+    return true;
+
+  return false;
+}
+
+//==============================================================================
+bool TaskManager::kill_task(
+  const std::string& task_id,
+  std::vector<std::string> labels)
+{
+  if (_active_task && _active_task.id() == task_id)
+  {
+    _task_state_update_available = true;
+    _active_task.kill(std::move(labels), _context->now());
+    return true;
+  }
+
+  std::lock_guard<std::mutex> lock(_mutex);
+  if (_cancel_task_from_dispatch_queue(task_id, labels))
+    return true;
+
+  if (_cancel_task_from_direct_queue(task_id, labels))
+    return true;
+
+  return false;
+}
+
 //==============================================================================
 void TaskManager::_begin_next_task()
 {
@@ -1678,49 +1736,130 @@ void TaskManager::_publish_task_state()
   _active_task.publish_task_state(*this);
 }
 
+//==============================================================================
+rmf_task::State TaskManager::_publish_pending_task(
+  const Assignment& pending,
+  rmf_task::State expected_state,
+  const rmf_task::Parameters& parameters)
+{
+  const auto info = pending.request()->description()->generate_info(
+    std::move(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;
+
+  pending_json["unix_millis_start_time"] =
+    to_millis(pending.deployment_time().time_since_epoch()).count();
+
+  if (pending.finish_state().time())
+  {
+    pending_json["unix_millis_finish_time"] =
+      to_millis(pending.finish_state().time()->time_since_epoch()).count();
+
+    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"] = "queued";
+
+  auto task_state_update = _task_state_update_json;
+  task_state_update["data"] = pending_json;
+
+  static const auto validator =
+    _make_validator(rmf_api_msgs::schemas::task_state_update);
+
+  _validate_and_publish_websocket(task_state_update, validator);
+
+  return pending.finish_state();
+}
+
 //==============================================================================
 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 : _direct_queue)
+  {
+    expected_state = _publish_pending_task(
+      pending.assignment, std::move(expected_state), parameters);
+  }
 
   for (const auto& pending : _queue)
   {
-    const auto info = pending.request()->description()->generate_info(
-      expected_state, parameters);
+    expected_state = _publish_pending_task(
+      pending, std::move(expected_state), parameters);
+  }
+}
 
-    nlohmann::json pending_json;
-    const auto& booking = *pending.request()->booking();
-    copy_booking_data(pending_json["booking"], booking);
+//==============================================================================
+void TaskManager::_publish_canceled_pending_task(
+  const Assignment& pending,
+  std::vector<std::string> labels)
+{
+  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;
+  pending_json["unix_millis_start_time"] =
+    to_millis(pending.deployment_time().time_since_epoch()).count();
 
-    pending_json["unix_millis_start_time"] =
-      to_millis(pending.deployment_time().time_since_epoch()).count();
+  copy_assignment(pending_json["assigned_to"], *_context);
+  pending_json["status"] = "canceled";
 
-    if (pending.finish_state().time())
-    {
-      pending_json["unix_millis_finish_time"] =
-        to_millis(pending.finish_state().time()->time_since_epoch()).count();
+  nlohmann::json cancellation;
+  cancellation["unix_millis_request_time"] =
+    to_millis(_context->now().time_since_epoch()).count();
+  cancellation["labels"] = std::move(labels);
+  pending_json["cancellation"] = std::move(cancellation);
 
-      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"] = "queued";
+  auto task_state_update = _task_state_update_json;
+  task_state_update["data"] = pending_json;
 
-    auto task_state_update = _task_state_update_json;
-    task_state_update["data"] = pending_json;
+  static const auto validator =
+    _make_validator(rmf_api_msgs::schemas::task_state_update);
 
-    _validate_and_publish_websocket(task_state_update, validator);
+  _validate_and_publish_websocket(task_state_update, validator);
+}
+
+//==============================================================================
+bool TaskManager::_cancel_task_from_dispatch_queue(
+  const std::string& task_id,
+  const std::vector<std::string>& labels)
+{
+  for (auto it = _queue.begin(); it != _queue.end(); ++it)
+  {
+    if (it->request()->booking()->id() == task_id)
+    {
+      _publish_canceled_pending_task(*it, labels);
+      _queue.erase(it);
+      return true;
+    }
+  }
+  return false;
+}
 
-    expected_state = pending.finish_state();
+//==============================================================================
+bool TaskManager::_cancel_task_from_direct_queue(
+  const std::string& task_id,
+  const std::vector<std::string>& labels)
+{
+  for (auto it = _direct_queue.begin(); it != _direct_queue.end(); ++it)
+  {
+    if (it->assignment.request()->booking()->id() == task_id)
+    {
+      _publish_canceled_pending_task(it->assignment, labels);
+      _direct_queue.erase(it);
+      return true;
+    }
   }
+  return false;
 }
 
 //==============================================================================
@@ -1991,64 +2130,6 @@ void TaskManager::_handle_request(
   }
 }
 
-namespace {
-//==============================================================================
-std::vector<std::string> get_labels(const nlohmann::json& request)
-{
-  const auto labels_it = request.find("labels");
-  if (labels_it != request.end())
-    return labels_it->get<std::vector<std::string>>();
-
-  return {};
-}
-
-//==============================================================================
-bool 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);
-      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,
@@ -2088,18 +2169,8 @@ void TaskManager::_handle_cancel_request(
     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), _context->now());
-    _task_state_update_available = true;
-    return _send_simple_success_response(request_id);
-  }
-  // 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);
+  if (cancel_task(task_id, get_labels(request_json)))
+    _send_simple_success_response(request_id);
 }
 
 //==============================================================================
@@ -2114,19 +2185,8 @@ void TaskManager::_handle_kill_request(
     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), _context->now());
-    return _send_simple_success_response(request_id);
-  }
-
-  // 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);
+  if (kill_task(task_id, get_labels(request_json)))
+    _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 af4a58fc0..179592277 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
@@ -192,6 +192,18 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
     std::vector<std::string> labels,
     std::function<void()> robot_is_interrupted);
 
+  /// Cancel a task for this robot. Returns true if the task was being managed
+  /// by this task manager, or false if it was not.
+  bool cancel_task(
+    const std::string& task_id,
+    std::vector<std::string> labels);
+
+  /// Kill a task for this robot. Returns true if the task was being managed by
+  /// this task manager, or false if it was not.
+  bool kill_task(
+    const std::string& task_id,
+    std::vector<std::string> labels);
+
 private:
 
   TaskManager(
@@ -398,9 +410,31 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
   /// Publish the current task state
   void _publish_task_state();
 
+  /// Publish one of the pending tasks
+  rmf_task::State _publish_pending_task(
+    const Assignment& assignment,
+    rmf_task::State expected_state,
+    const rmf_task::Parameters& parameters);
+
   /// Publish the current pending task list
   void _publish_task_queue();
 
+  void _publish_canceled_pending_task(
+    const Assignment& assignment,
+    std::vector<std::string> labels);
+
+  /// Cancel a task that is in the dispatch queue. Returns false if the task
+  /// was not present.
+  bool _cancel_task_from_dispatch_queue(
+    const std::string& task_id,
+    const std::vector<std::string>& labels);
+
+  /// Cancel a task that is in the direct queue. Returns false if the task was
+  /// not present.
+  bool _cancel_task_from_direct_queue(
+    const std::string& task_id,
+    const std::vector<std::string>& labels);
+
   /// Schema loader for validating jsons
   void _schema_loader(
     const nlohmann::json_uri& id, nlohmann::json& value) const;
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 5c11098c7..c81b83c66 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyTrafficLight.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyTrafficLight.cpp
@@ -964,7 +964,8 @@ void EasyTrafficLight::Implementation::Shared::respond(
   auto approval_cb =
     [w = weak_from_this(), request_path_version = path_version](
     const rmf_traffic::PlanId plan_id,
-    const rmf_traffic::agv::Plan& plan)
+    const rmf_traffic::agv::Plan& plan,
+    const auto&)
     -> std::optional<rmf_traffic::schedule::ItineraryVersion>
     {
       if (const auto self = w.lock())
@@ -986,7 +987,7 @@ void EasyTrafficLight::Implementation::Shared::respond(
 
   auto negotiate = services::Negotiate::path(
     state.itinerary->assign_plan_id(), state.planner,
-    {*state.last_known_location}, std::move(goal),
+    {*state.last_known_location}, std::move(goal), {},
     viewer, responder, std::move(approval_cb), evaluator);
 
   auto negotiate_sub =
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 61c3e0033..58132d401 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp
@@ -741,13 +741,14 @@ get_nearest_charger(
 
 namespace {
 //==============================================================================
-rmf_fleet_msgs::msg::Location convert_location(const agv::RobotContext& context)
+std::optional<rmf_fleet_msgs::msg::Location> convert_location(
+  const agv::RobotContext& context)
 {
   if (context.location().empty())
   {
     // TODO(MXG): We should emit some kind of critical error if this ever
     // happens
-    return rmf_fleet_msgs::msg::Location();
+    return std::nullopt;
   }
 
   const auto& graph = context.planner()->get_configuration().graph();
@@ -769,9 +770,13 @@ rmf_fleet_msgs::msg::Location convert_location(const agv::RobotContext& context)
 }
 
 //==============================================================================
-rmf_fleet_msgs::msg::RobotState convert_state(const TaskManager& mgr)
+std::optional<rmf_fleet_msgs::msg::RobotState> convert_state(
+  const TaskManager& mgr)
 {
   const RobotContext& context = *mgr.context();
+  const auto location = convert_location(context);
+  if (!location.has_value())
+    return std::nullopt;
 
   const auto mode = mgr.robot_mode();
 
@@ -786,7 +791,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(convert_location(context))
+    .location(*location)
     // 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
@@ -800,7 +805,13 @@ 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));
+  {
+    auto state = convert_state(*mgr);
+    if (!state.has_value())
+      continue;
+
+    robot_states.emplace_back(std::move(*state));
+  }
 
   auto fleet_state = rmf_fleet_msgs::build<rmf_fleet_msgs::msg::FleetState>()
     .name(name)
@@ -841,10 +852,13 @@ void FleetUpdateHandle::Implementation::update_fleet_state() const
 
       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;
+      if (!location_msg.has_value())
+        continue;
+
+      location["map"] = location_msg->level_name;
+      location["x"] = location_msg->x;
+      location["y"] = location_msg->y;
+      location["yaw"] = location_msg->yaw;
 
       std::lock_guard<std::mutex> lock(context->reporting().mutex());
       const auto& issues = context->reporting().open_issues();
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 e87c9a9c5..47f69cb21 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp
@@ -173,6 +173,20 @@ void RobotUpdateHandle::update_position(
   }
 }
 
+//==============================================================================
+void RobotUpdateHandle::update_position(
+  rmf_traffic::agv::Plan::StartSet position)
+{
+  if (const auto context = _pimpl->get_context())
+  {
+    context->worker().schedule(
+      [context, starts = std::move(position)](const auto&)
+      {
+        context->_location = starts;
+      });
+  }
+}
+
 //==============================================================================
 RobotUpdateHandle& RobotUpdateHandle::set_charger_waypoint(
   const std::size_t charger_wp)
@@ -434,6 +448,68 @@ auto RobotUpdateHandle::interrupt(
   return handle;
 }
 
+//==============================================================================
+void RobotUpdateHandle::cancel_task(
+  std::string task_id,
+  std::vector<std::string> labels,
+  std::function<void(bool)> on_cancellation)
+{
+  if (const auto context = _pimpl->get_context())
+  {
+    context->worker().schedule(
+      [
+        task_id = std::move(task_id),
+        labels = std::move(labels),
+        on_cancellation = std::move(on_cancellation),
+        c = context->weak_from_this()
+      ](const auto&)
+      {
+        const auto context = c.lock();
+        if (!context)
+          return;
+
+        const auto mgr = context->task_manager();
+        if (!mgr)
+          return;
+
+        const auto result = mgr->cancel_task(task_id, labels);
+        if (on_cancellation)
+          on_cancellation(result);
+      });
+  }
+}
+
+//==============================================================================
+void RobotUpdateHandle::kill_task(
+  std::string task_id,
+  std::vector<std::string> labels,
+  std::function<void(bool)> on_kill)
+{
+  if (const auto context = _pimpl->get_context())
+  {
+    context->worker().schedule(
+      [
+        task_id = std::move(task_id),
+        labels = std::move(labels),
+        on_kill = std::move(on_kill),
+        c = context->weak_from_this()
+      ](const auto&)
+      {
+        const auto context = c.lock();
+        if (!context)
+          return;
+
+        const auto mgr = context->task_manager();
+        if (!mgr)
+          return;
+
+        const auto result = mgr->kill_task(task_id, labels);
+        if (on_kill)
+          on_kill(result);
+      });
+  }
+}
+
 //==============================================================================
 class RobotUpdateHandle::IssueTicket::Implementation
 {
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 54e0d9796..acb7d3d0e 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
@@ -245,7 +245,7 @@ class FleetUpdateHandle::Implementation
 
   std::shared_ptr<std::mutex> update_callback_mutex =
     std::make_shared<std::mutex>();
-  std::function<void(const nlohmann::json&)> update_callback;
+  std::function<void(const nlohmann::json&)> update_callback = nullptr;
 
   TaskActivation activation = TaskActivation();
   TaskDeserialization deserialization = TaskDeserialization();
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp
index 20f2a1b63..71e3a0c13 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp
@@ -266,8 +266,12 @@ void EmergencyPullover::Active::_find_plan()
       self->_state->update_status(Status::Underway);
       self->_state->update_log().info("Found an emergency pullover");
 
+      auto full_itinerary = result->get_itinerary();
       self->_execute_plan(
-        self->_context->itinerary().assign_plan_id(), *std::move(result));
+        self->_context->itinerary().assign_plan_id(),
+        *std::move(result),
+        std::move(full_itinerary));
+
       self->_find_pullover_service = nullptr;
       self->_retry_timer = nullptr;
     });
@@ -315,14 +319,15 @@ void EmergencyPullover::Active::_schedule_retry()
 //==============================================================================
 void EmergencyPullover::Active::_execute_plan(
   const rmf_traffic::PlanId plan_id,
-  rmf_traffic::agv::Plan plan)
+  rmf_traffic::agv::Plan plan,
+  rmf_traffic::schedule::Itinerary full_itinerary)
 {
   if (_is_interrupted)
     return;
 
   _execution = ExecutePlan::make(
-    _context, plan_id, std::move(plan), _assign_id, _state,
-    _update, _finished, std::nullopt);
+    _context, plan_id, std::move(plan), std::move(full_itinerary), _assign_id,
+    _state, _update, _finished, std::nullopt);
 
   if (!_execution.has_value())
   {
@@ -341,12 +346,13 @@ Negotiator::NegotiatePtr EmergencyPullover::Active::_respond(
 {
   auto approval_cb = [w = weak_from_this()](
     const rmf_traffic::PlanId plan_id,
-    const rmf_traffic::agv::Plan& plan)
+    const rmf_traffic::agv::Plan& plan,
+    rmf_traffic::schedule::Itinerary full_itinerary)
     -> std::optional<rmf_traffic::schedule::ItineraryVersion>
     {
       if (auto self = w.lock())
       {
-        self->_execute_plan(plan_id, plan);
+        self->_execute_plan(plan_id, plan, std::move(full_itinerary));
         return self->_context->itinerary().version();
       }
 
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.hpp
index ce0efdc94..6a4b4ca9e 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.hpp
@@ -102,7 +102,8 @@ class EmergencyPullover : public rmf_task_sequence::Event
 
     void _execute_plan(
       rmf_traffic::PlanId plan_id,
-      rmf_traffic::agv::Plan plan);
+      rmf_traffic::agv::Plan plan,
+      rmf_traffic::schedule::Itinerary full_itinerary);
 
     Negotiator::NegotiatePtr _respond(
       const Negotiator::TableViewerPtr& table_view,
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp
index c82f4be45..2ff3bff41 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp
@@ -423,6 +423,7 @@ std::optional<ExecutePlan> ExecutePlan::make(
   agv::RobotContextPtr context,
   rmf_traffic::PlanId plan_id,
   rmf_traffic::agv::Plan plan,
+  rmf_traffic::schedule::Itinerary full_itinerary,
   const rmf_task::Event::AssignIDPtr& event_id,
   rmf_task::events::SimpleEventStatePtr state,
   std::function<void()> update,
@@ -618,7 +619,7 @@ std::optional<ExecutePlan> ExecutePlan::make(
     standbys, state, std::move(update))->begin([]() {}, std::move(finished));
 
   std::size_t attempts = 0;
-  while (!context->itinerary().set(plan_id, plan.get_itinerary()))
+  while (!context->itinerary().set(plan_id, std::move(full_itinerary)))
   {
     // Some mysterious behavior has been happening where plan_ids are invalid.
     // We will attempt to catch that here and try to learn more about what
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.hpp
index 6710f6cd1..1f2fd5945 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.hpp
@@ -33,6 +33,7 @@ struct ExecutePlan
     agv::RobotContextPtr context,
     rmf_traffic::PlanId plan_id,
     rmf_traffic::agv::Plan plan,
+    rmf_traffic::schedule::Itinerary full_itinerary,
     const rmf_task_sequence::Event::AssignIDPtr& event_id,
     rmf_task::events::SimpleEventStatePtr state,
     std::function<void()> update,
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 acfd44d9f..eef55baf8 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp
@@ -16,6 +16,7 @@
 */
 
 #include "GoToPlace.hpp"
+#include "../project_itinerary.hpp"
 
 #include <rmf_traffic/schedule/StubbornNegotiator.hpp>
 
@@ -67,6 +68,7 @@ auto GoToPlace::Standby::make(
   const auto header = description.generate_header(state, *parameters);
 
   auto standby = std::make_shared<Standby>(Standby{description.destination()});
+  standby->_followed_by = description.expected_next_destinations();
   standby->_assign_id = id;
   standby->_context = context;
   standby->_time_estimate = header.original_duration_estimate();
@@ -113,6 +115,7 @@ auto GoToPlace::Standby::begin(
       _assign_id,
       _context,
       _goal,
+      _followed_by,
       _tail_period,
       _state,
       _update,
@@ -127,12 +130,14 @@ auto GoToPlace::Active::make(
   const AssignIDPtr& id,
   agv::RobotContextPtr context,
   rmf_traffic::agv::Plan::Goal goal,
+  std::vector<rmf_traffic::agv::Plan::Goal> followed_by,
   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(std::move(goal)));
+  active->_followed_by = std::move(followed_by);
   active->_assign_id = id;
   active->_context = std::move(context);
   active->_tail_period = tail_period;
@@ -336,8 +341,14 @@ void GoToPlace::Active::_find_plan()
         "Found a plan to move from ["
         + start_name + "] to [" + goal_name + "]");
 
+      auto full_itinerary = project_itinerary(
+        *result, self->_followed_by, *self->_context->planner());
+
       self->_execute_plan(
-        self->_context->itinerary().assign_plan_id(), *std::move(result));
+        self->_context->itinerary().assign_plan_id(),
+        *std::move(result),
+        std::move(full_itinerary));
+
       self->_find_path_service = nullptr;
       self->_retry_timer = nullptr;
     });
@@ -392,12 +403,13 @@ void GoToPlace::Active::_schedule_retry()
 //==============================================================================
 void GoToPlace::Active::_execute_plan(
   const rmf_traffic::PlanId plan_id,
-  rmf_traffic::agv::Plan plan)
+  rmf_traffic::agv::Plan plan,
+  rmf_traffic::schedule::Itinerary full_itinerary)
 {
   if (_is_interrupted)
     return;
 
-  if (plan.get_itinerary().empty())
+  if (plan.get_itinerary().empty() || plan.get_waypoints().empty())
   {
     _state->update_status(Status::Completed);
     _state->update_log().info(
@@ -407,8 +419,8 @@ void GoToPlace::Active::_execute_plan(
   }
 
   _execution = ExecutePlan::make(
-    _context, plan_id, std::move(plan), _assign_id, _state,
-    _update, _finished, _tail_period);
+    _context, plan_id, std::move(plan), std::move(full_itinerary),
+    _assign_id, _state, _update, _finished, _tail_period);
 
   if (!_execution.has_value())
   {
@@ -437,12 +449,13 @@ Negotiator::NegotiatePtr GoToPlace::Active::_respond(
 {
   auto approval_cb = [w = weak_from_this()](
     const rmf_traffic::PlanId plan_id,
-    const rmf_traffic::agv::Plan& plan)
+    const rmf_traffic::agv::Plan& plan,
+    rmf_traffic::schedule::Itinerary itinerary)
     -> std::optional<rmf_traffic::schedule::ItineraryVersion>
     {
       if (auto self = w.lock())
       {
-        self->_execute_plan(plan_id, plan);
+        self->_execute_plan(plan_id, plan, std::move(itinerary));
         return self->_context->itinerary().version();
       }
 
@@ -452,7 +465,7 @@ Negotiator::NegotiatePtr GoToPlace::Active::_respond(
   const auto evaluator = Negotiator::make_evaluator(table_view);
   return services::Negotiate::path(
     _context->itinerary().assign_plan_id(), _context->planner(),
-    _context->location(), _goal, table_view,
+    _context->location(), _goal, _followed_by, table_view,
     responder, std::move(approval_cb), std::move(evaluator));
 }
 
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 4a67faa9c..199db1e9d 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp
@@ -66,6 +66,7 @@ class GoToPlace : public rmf_task_sequence::Event
     Standby(rmf_traffic::agv::Plan::Goal goal);
 
     rmf_traffic::agv::Plan::Goal _goal;
+    std::vector<rmf_traffic::agv::Plan::Goal> _followed_by;
     AssignIDPtr _assign_id;
     agv::RobotContextPtr _context;
     rmf_traffic::Duration _time_estimate;
@@ -85,6 +86,7 @@ class GoToPlace : public rmf_task_sequence::Event
       const AssignIDPtr& id,
       agv::RobotContextPtr context,
       rmf_traffic::agv::Plan::Goal goal,
+      std::vector<rmf_traffic::agv::Plan::Goal> followed_by,
       std::optional<rmf_traffic::Duration> tail_period,
       rmf_task::events::SimpleEventStatePtr state,
       std::function<void()> update,
@@ -112,7 +114,8 @@ class GoToPlace : public rmf_task_sequence::Event
 
     void _execute_plan(
       rmf_traffic::PlanId plan_id,
-      rmf_traffic::agv::Plan plan);
+      rmf_traffic::agv::Plan plan,
+      rmf_traffic::schedule::Itinerary full_itinerary);
 
     void _stop_and_clear();
 
@@ -121,6 +124,7 @@ class GoToPlace : public rmf_task_sequence::Event
       const Negotiator::ResponderPtr& responder);
 
     rmf_traffic::agv::Plan::Goal _goal;
+    std::vector<rmf_traffic::agv::Plan::Goal> _followed_by;
     AssignIDPtr _assign_id;
     agv::RobotContextPtr _context;
     std::optional<rmf_traffic::Duration> _tail_period;
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ResponsiveWait.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ResponsiveWait.cpp
index 86b56413e..85d180baf 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ResponsiveWait.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ResponsiveWait.cpp
@@ -301,6 +301,7 @@ void ResponsiveWait::Active::_begin_movement()
     _assign_id,
     _context,
     std::move(goal),
+    {},
     _description.period,
     _state,
     _update,
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/project_itinerary.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/project_itinerary.cpp
new file mode 100644
index 000000000..55dd588b9
--- /dev/null
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/project_itinerary.cpp
@@ -0,0 +1,67 @@
+/*
+ * 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 "project_itinerary.hpp"
+
+namespace rmf_fleet_adapter {
+
+//==============================================================================
+rmf_traffic::schedule::Itinerary project_itinerary(
+  const rmf_traffic::agv::Plan& starting_from,
+  const std::vector<rmf_traffic::agv::Plan::Goal>& through_destinations,
+  const rmf_traffic::agv::Planner& with_planner)
+{
+  auto itinerary = starting_from.get_itinerary();
+  auto last_plan = starting_from;
+  for (const auto& destination : through_destinations)
+  {
+    if (last_plan.get_waypoints().empty())
+      break;
+
+    const auto& wp = last_plan.get_waypoints().back();
+    if (!wp.graph_index().has_value())
+      break;
+
+    rmf_traffic::agv::Plan::Start start(
+      wp.time(), wp.graph_index().value(), wp.position()[2]);
+
+    auto options = with_planner.get_default_options();
+    options.validator(nullptr);
+    auto result = with_planner.plan(start, destination, options);
+    if (!result)
+      break;
+
+    last_plan = *std::move(result);
+    const auto& new_itinerary = last_plan.get_itinerary();
+    if (new_itinerary.front().map() == itinerary.back().map())
+    {
+      // We only look at the first route because we're not going to include
+      // any map switches for now.
+      for (const auto& wp : new_itinerary.front().trajectory())
+        itinerary.back().trajectory().insert(wp);
+    }
+    else
+    {
+      // If the map has switched, let's break out of this loop.
+      break;
+    }
+  }
+
+  return itinerary;
+}
+
+} // namespace rmf_fleet_adapter
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/project_itinerary.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/project_itinerary.hpp
new file mode 100644
index 000000000..c8fe77776
--- /dev/null
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/project_itinerary.hpp
@@ -0,0 +1,34 @@
+/*
+ * 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__PROJECT_ITINERARY_HPP
+#define SRC__RMF_FLEET_ADAPTER__PROJECT_ITINERARY_HPP
+
+#include <rmf_traffic/schedule/Itinerary.hpp>
+#include <rmf_traffic/agv/Planner.hpp>
+
+namespace rmf_fleet_adapter {
+
+//==============================================================================
+rmf_traffic::schedule::Itinerary project_itinerary(
+  const rmf_traffic::agv::Plan& starting_from,
+  const std::vector<rmf_traffic::agv::Plan::Goal>& through_destinations,
+  const rmf_traffic::agv::Planner& with_planner);
+
+} // namespace rmf_fleet_adapter
+
+#endif // SRC__RMF_FLEET_ADAPTER__PROJECT_ITINERARY_HPP
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/services/Negotiate.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/services/Negotiate.cpp
index 43eb72b2c..83f731091 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/services/Negotiate.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/services/Negotiate.cpp
@@ -26,6 +26,7 @@ Negotiate::Negotiate(
   std::shared_ptr<const rmf_traffic::agv::Planner> planner,
   rmf_traffic::agv::Plan::StartSet starts,
   std::vector<rmf_traffic::agv::Plan::Goal> goals,
+  std::vector<rmf_traffic::agv::Plan::Goal> followed_by,
   rmf_traffic::schedule::Negotiator::TableViewerPtr viewer,
   rmf_traffic::schedule::Negotiator::ResponderPtr responder,
   ApprovalCallback approval,
@@ -37,6 +38,7 @@ Negotiate::Negotiate(
   _planner(std::move(planner)),
   _starts(std::move(starts)),
   _goals(std::move(goals)),
+  _followed_by(std::move(followed_by)),
   _viewer(std::move(viewer)),
   _responder(std::move(responder)),
   _approval(std::move(approval)),
@@ -52,6 +54,7 @@ std::shared_ptr<Negotiate> Negotiate::path(
   std::shared_ptr<const rmf_traffic::agv::Planner> planner,
   rmf_traffic::agv::Plan::StartSet starts,
   rmf_traffic::agv::Plan::Goal goal,
+  std::vector<rmf_traffic::agv::Plan::Goal> followed_by,
   rmf_traffic::schedule::Negotiator::TableViewerPtr viewer,
   rmf_traffic::schedule::Negotiator::ResponderPtr responder,
   ApprovalCallback approval,
@@ -63,6 +66,7 @@ std::shared_ptr<Negotiate> Negotiate::path(
     std::move(planner),
     std::move(starts),
     std::vector<rmf_traffic::agv::Plan::Goal>({std::move(goal)}),
+    std::move(followed_by),
     std::move(viewer),
     std::move(responder),
     std::move(approval),
@@ -97,6 +101,7 @@ std::shared_ptr<Negotiate> Negotiate::emergency_pullover(
     std::move(planner),
     std::move(starts),
     std::move(goals),
+    std::vector<rmf_traffic::agv::Plan::Goal>(),
     std::move(viewer),
     std::move(responder),
     std::move(approval),
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/services/Negotiate.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/services/Negotiate.hpp
index 60635adf2..1aeb022a6 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/services/Negotiate.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/services/Negotiate.hpp
@@ -36,7 +36,8 @@ class Negotiate : public std::enable_shared_from_this<Negotiate>
   using ApprovalCallback =
     std::function<UpdateVersion(
         rmf_traffic::PlanId,
-        const rmf_traffic::agv::Plan&)
+        const rmf_traffic::agv::Plan&,
+        rmf_traffic::schedule::Itinerary)
     >;
 
   Negotiate(
@@ -44,6 +45,7 @@ class Negotiate : public std::enable_shared_from_this<Negotiate>
     std::shared_ptr<const rmf_traffic::agv::Planner> planner,
     rmf_traffic::agv::Plan::StartSet starts,
     std::vector<rmf_traffic::agv::Plan::Goal> goals,
+    std::vector<rmf_traffic::agv::Plan::Goal> followed_by,
     rmf_traffic::schedule::Negotiator::TableViewerPtr viewer,
     rmf_traffic::schedule::Negotiator::ResponderPtr responder,
     ApprovalCallback approval,
@@ -55,6 +57,7 @@ class Negotiate : public std::enable_shared_from_this<Negotiate>
     std::shared_ptr<const rmf_traffic::agv::Planner> planner,
     rmf_traffic::agv::Plan::StartSet starts,
     rmf_traffic::agv::Plan::Goal goal,
+    std::vector<rmf_traffic::agv::Plan::Goal> followed_by,
     rmf_traffic::schedule::Negotiator::TableViewerPtr viewer,
     rmf_traffic::schedule::Negotiator::ResponderPtr responder,
     ApprovalCallback approval,
@@ -97,6 +100,7 @@ class Negotiate : public std::enable_shared_from_this<Negotiate>
   std::shared_ptr<const rmf_traffic::agv::Planner> _planner;
   rmf_traffic::agv::Plan::StartSet _starts;
   std::vector<rmf_traffic::agv::Plan::Goal> _goals;
+  std::vector<rmf_traffic::agv::Plan::Goal> _followed_by;
   rmf_traffic::schedule::Negotiator::TableViewerPtr _viewer;
   rmf_traffic::schedule::Negotiator::ResponderPtr _responder;
   ApprovalCallback _approval;
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/services/detail/impl_Negotiate.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/services/detail/impl_Negotiate.hpp
index e4ca61bd4..8819c6387 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/services/detail/impl_Negotiate.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/services/detail/impl_Negotiate.hpp
@@ -19,6 +19,7 @@
 #define SRC__RMF_FLEET_ADAPTER__SERVICES__DETAIL__IMPL_NEGOTIATE_HPP
 
 #include "../Negotiate.hpp"
+#include "../../project_itinerary.hpp"
 
 namespace rmf_fleet_adapter {
 namespace services {
@@ -89,8 +90,11 @@ void Negotiate::operator()(const Subscriber& s)
               self->shared_from_this(),
               [r = *self->_evaluator.best_result.progress,
               initial_itinerary = std::move(self->_initial_itinerary),
+              followed_by = self->_followed_by,
+              planner = self->_planner,
               approval = std::move(self->_approval),
               responder = self->_responder,
+              viewer = self->_viewer,
               plan_id = self->_plan_id]()
               {
                 std::vector<rmf_traffic::Route> final_itinerary;
@@ -106,14 +110,43 @@ void Negotiate::operator()(const Subscriber& s)
                   }
                 }
 
+                final_itinerary = project_itinerary(*r, followed_by, *planner);
+                for (const auto& parent : viewer->base_proposals())
+                {
+                  // Make sure all parent dependencies are accounted for
+                  // TODO(MXG): This is kind of a gross hack that we add to
+                  // force the lookahead to work for patrols. This approach
+                  // should be reworked in a future redesign of the traffic
+                  // system.
+                  for (auto& r : final_itinerary)
+                  {
+                    for (std::size_t i = 0; i < parent.itinerary.size(); ++i)
+                    {
+                      r.add_dependency(
+                        r.trajectory().size(),
+                        rmf_traffic::Dependency{
+                          parent.participant,
+                          parent.plan,
+                          i,
+                          parent.itinerary[i].trajectory().size()
+                        });
+                    }
+                  }
+                }
+
                 responder->submit(
                   plan_id,
-                  std::move(final_itinerary),
-                  [plan_id, plan = *r, approval = std::move(approval)]()
+                  final_itinerary,
+                  [
+                    plan_id,
+                    plan = *r,
+                    approval = std::move(approval),
+                    final_itinerary
+                  ]()
                   -> UpdateVersion
                   {
                     if (approval)
-                      return approval(plan_id, plan);
+                      return approval(plan_id, plan, final_itinerary);
 
                     return rmf_utils::nullopt;
                   });
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Patrol.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Patrol.cpp
index a54e7cb3c..765a8730c 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Patrol.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Patrol.cpp
@@ -49,15 +49,40 @@ void add_patrol(
     [place_deser = deserialization.place](const nlohmann::json& msg)
     -> agv::DeserializedEvent
     {
-      auto place = place_deser(msg);
+      nlohmann::json place_msg;
+      const auto place_it = msg.find("place");
+      if (place_it == msg.end())
+        place_msg = msg;
+      else
+        place_msg = place_it.value();
+
+      auto place = place_deser(place_msg);
       if (!place.description.has_value())
         return {nullptr, std::move(place.errors)};
 
+      const auto desc =
+        GoToPlace::Description::make(std::move(*place.description));
+
+      const auto followed_by_it = msg.find("followed_by");
+      if (followed_by_it != msg.end())
+      {
+        std::vector<rmf_traffic::agv::Plan::Goal> followed_by;
+        for (const auto& f_msg : followed_by_it.value())
+        {
+          auto f = place_deser(f_msg);
+          place.errors.insert(
+            place.errors.end(), f.errors.begin(), f.errors.end());
+
+          if (!f.description.has_value())
+            return {nullptr, place.errors};
+
+          followed_by.push_back(*f.description);
+        }
+        desc->expected_next_destinations(std::move(followed_by));
+      }
+
       /* *INDENT-OFF* */
-      return {
-        GoToPlace::Description::make(std::move(*place.description)),
-        std::move(place.errors)
-      };
+      return {desc, std::move(place.errors)};
       /* *INDENT-ON* */
     };
 
@@ -119,10 +144,14 @@ void add_patrol(
       rmf_task_sequence::Task::Builder builder;
       for (std::size_t i = 0; i < rounds; ++i)
       {
-        for (const auto& place : places)
+        for (std::size_t j = 0; j < places.size(); ++j)
         {
-          builder.add_phase(
-            Phase::Description::make(GoToPlace::Description::make(place)), {});
+          const auto& place = places[j];
+          auto go_to_place = GoToPlace::Description::make(place);
+          auto next = places;
+          next.erase(next.begin(), next.begin() + j + 1);
+          go_to_place->expected_next_destinations(std::move(next));
+          builder.add_phase(Phase::Description::make(go_to_place), {});
         }
       }
 
diff --git a/rmf_fleet_adapter/test/services/test_Negotiate.cpp b/rmf_fleet_adapter/test/services/test_Negotiate.cpp
index 9d4d2bc9d..2386abb23 100644
--- a/rmf_fleet_adapter/test/services/test_Negotiate.cpp
+++ b/rmf_fleet_adapter/test/services/test_Negotiate.cpp
@@ -364,7 +364,7 @@ class TestPathNegotiator
     }
 
     auto negotiate = rmf_fleet_adapter::services::Negotiate::path(
-      0, _planner, _starts, _goal, table_viewer, responder, nullptr,
+      0, _planner, _starts, _goal, {}, table_viewer, responder, nullptr,
       evaluator);
 
     auto sub = rmf_rxcpp::make_job<
diff --git a/rmf_fleet_adapter_python/src/adapter.cpp b/rmf_fleet_adapter_python/src/adapter.cpp
index ad0c7b21f..2a06ec3cb 100644
--- a/rmf_fleet_adapter_python/src/adapter.cpp
+++ b/rmf_fleet_adapter_python/src/adapter.cpp
@@ -1,6 +1,5 @@
 #include <pybind11/pybind11.h>
 #include <pybind11/functional.h>
-#include <pybind11/iostream.h>
 #include <pybind11/chrono.h>
 #include <pybind11/eigen.h>
 #include <pybind11/stl.h>
@@ -69,9 +68,7 @@ PYBIND11_MODULE(rmf_adapter, m) {
     std::shared_ptr<agv::RobotCommandHandle>>(
     m, "RobotCommandHandle", py::dynamic_attr())
   .def(py::init<>())
-  .def("follow_new_path", &agv::RobotCommandHandle::follow_new_path,
-    py::call_guard<py::scoped_ostream_redirect,
-    py::scoped_estream_redirect>())
+  .def("follow_new_path", &agv::RobotCommandHandle::follow_new_path)
   .def("stop", &agv::RobotCommandHandle::stop)
   .def("dock", &agv::RobotCommandHandle::dock);
 
@@ -86,25 +83,19 @@ PYBIND11_MODULE(rmf_adapter, m) {
     py::overload_cast<std::size_t, double>(
       &agv::RobotUpdateHandle::update_position),
     py::arg("waypoint"),
-    py::arg("orientation"),
-    py::call_guard<py::scoped_ostream_redirect,
-    py::scoped_estream_redirect>())
+    py::arg("orientation"))
   .def("update_current_lanes",
     py::overload_cast<const Eigen::Vector3d&,
     const std::vector<std::size_t>&>(
       &agv::RobotUpdateHandle::update_position),
     py::arg("position"),
-    py::arg("lanes"),
-    py::call_guard<py::scoped_ostream_redirect,
-    py::scoped_estream_redirect>())
+    py::arg("lanes"))
   .def("update_off_grid_position",
     py::overload_cast<const Eigen::Vector3d&,
     std::size_t>(
       &agv::RobotUpdateHandle::update_position),
     py::arg("position"),
-    py::arg("target_waypoint"),
-    py::call_guard<py::scoped_ostream_redirect,
-    py::scoped_estream_redirect>())
+    py::arg("target_waypoint"))
   .def("update_lost_position",
     py::overload_cast<const std::string&,
     const Eigen::Vector3d&,
@@ -116,17 +107,15 @@ PYBIND11_MODULE(rmf_adapter, m) {
     py::arg("position"),
     py::arg("max_merge_waypoint_distance") = 0.1,
     py::arg("max_merge_lane_distance") = 1.0,
-    py::arg("min_lane_length") = 1e-8,
-    py::call_guard<py::scoped_ostream_redirect,
-    py::scoped_estream_redirect>())
+    py::arg("min_lane_length") = 1e-8)
+  .def("update_position",
+    py::overload_cast<rmf_traffic::agv::Plan::StartSet>(
+      &agv::RobotUpdateHandle::update_position),
+    py::arg("start_set"))
   .def("set_charger_waypoint", &agv::RobotUpdateHandle::set_charger_waypoint,
-    py::arg("charger_wp"),
-    py::call_guard<py::scoped_ostream_redirect,
-    py::scoped_estream_redirect>())
+    py::arg("charger_wp"))
   .def("update_battery_soc", &agv::RobotUpdateHandle::update_battery_soc,
-    py::arg("battery_soc"),
-    py::call_guard<py::scoped_ostream_redirect,
-    py::scoped_estream_redirect>())
+    py::arg("battery_soc"))
   .def("override_status", &agv::RobotUpdateHandle::override_status,
     py::arg("battery_soc"))
   .def_property("maximum_delay",
@@ -200,6 +189,16 @@ PYBIND11_MODULE(rmf_adapter, m) {
     &agv::RobotUpdateHandle::interrupt,
     py::arg("labels"),
     py::arg("robot_is_interrupted"))
+  .def("cancel_task",
+    &agv::RobotUpdateHandle::cancel_task,
+    py::arg("task_id"),
+    py::arg("labels"),
+    py::arg("on_cancellation"))
+  .def("kill_task",
+    &agv::RobotUpdateHandle::kill_task,
+    py::arg("task_id"),
+    py::arg("labels"),
+    py::arg("on_kill"))
   .def("create_issue",
     &agv::RobotUpdateHandle::create_issue,
     py::arg("tier"),
@@ -499,9 +498,7 @@ PYBIND11_MODULE(rmf_adapter, m) {
   .def("follow_new_path",
     py::overload_cast<const std::vector<agv::Waypoint>&>(
       &agv::EasyTrafficLight::follow_new_path),
-    py::arg("waypoint"),
-    py::call_guard<py::scoped_ostream_redirect,
-    py::scoped_estream_redirect>())
+    py::arg("waypoint"))
   .def("moving_from",
     py::overload_cast<std::size_t, Eigen::Vector3d>(
       &agv::EasyTrafficLight::moving_from),
@@ -579,9 +576,7 @@ PYBIND11_MODULE(rmf_adapter, m) {
     py::arg("node_name"),
     py::arg("node_options") = rclcpp::NodeOptions(),
     py::arg("wait_time") = rmf_utils::optional<rmf_traffic::Duration>(
-      rmf_utils::nullopt),
-    py::call_guard<py::scoped_ostream_redirect,
-    py::scoped_estream_redirect>())
+      rmf_utils::nullopt))
   .def("add_fleet", &agv::Adapter::add_fleet,
     py::arg("fleet_name"),
     py::arg("traits"),
@@ -610,9 +605,7 @@ PYBIND11_MODULE(rmf_adapter, m) {
   .def(py::init<const std::string&,
     const rclcpp::NodeOptions&>(),
     py::arg("node_name"),
-    py::arg("node_options") = rclcpp::NodeOptions(),
-    py::call_guard<py::scoped_ostream_redirect,
-    py::scoped_estream_redirect>())
+    py::arg("node_options") = rclcpp::NodeOptions())
   .def("add_fleet", &agv::test::MockAdapter::add_fleet,
     py::arg("fleet_name"),
     py::arg("traits"),
diff --git a/rmf_fleet_adapter_python/src/battery/battery.cpp b/rmf_fleet_adapter_python/src/battery/battery.cpp
index 3103ed6c0..391345f1d 100644
--- a/rmf_fleet_adapter_python/src/battery/battery.cpp
+++ b/rmf_fleet_adapter_python/src/battery/battery.cpp
@@ -1,6 +1,5 @@
 #include <pybind11/pybind11.h>
 #include <pybind11/operators.h>
-#include <pybind11/iostream.h>
 #include <pybind11/eigen.h>
 #include <pybind11/chrono.h>
 #include <pybind11/stl.h>
diff --git a/rmf_fleet_adapter_python/src/graph/graph.cpp b/rmf_fleet_adapter_python/src/graph/graph.cpp
index 45f192a91..a540c2408 100644
--- a/rmf_fleet_adapter_python/src/graph/graph.cpp
+++ b/rmf_fleet_adapter_python/src/graph/graph.cpp
@@ -1,5 +1,4 @@
 #include <pybind11/pybind11.h>
-#include <pybind11/iostream.h>
 #include <pybind11/chrono.h>
 #include <pybind11/eigen.h>
 #include <pybind11/stl.h>
@@ -137,9 +136,7 @@ void bind_graph(py::module& m)
     &Graph::add_lane,
     py::arg("entry"),
     py::arg("exit"),
-    py::arg("properties") = Lane::Properties(),
-    py::call_guard<py::scoped_ostream_redirect,
-    py::scoped_estream_redirect>())
+    py::arg("properties") = Lane::Properties())
   .def("add_dock_lane",
     [&](Graph& self,
     const std::size_t w0,
@@ -162,9 +159,7 @@ void bind_graph(py::module& m)
     py::arg("w1"),
     py::arg("dock_name"),
     py::arg("dock_seconds") = 10,
-    py::arg("properties") = Lane::Properties(),
-    py::call_guard<py::scoped_ostream_redirect,
-    py::scoped_estream_redirect>())
+    py::arg("properties") = Lane::Properties())
   .def("add_bidir_lane",
     [&](Graph& self,
     const std::size_t w0,
@@ -172,9 +167,7 @@ void bind_graph(py::module& m)
     {
       self.add_lane(w0, w1);
       self.add_lane(w1, w0);
-    },
-    py::call_guard<py::scoped_ostream_redirect,
-    py::scoped_estream_redirect>())
+    })
   .def("get_lane", py::overload_cast<std::size_t>(&Graph::get_lane))
   .def("get_lane", py::overload_cast<std::size_t>(
       &Graph::get_lane, py::const_))
diff --git a/rmf_fleet_adapter_python/src/nodes/nodes.cpp b/rmf_fleet_adapter_python/src/nodes/nodes.cpp
index 376adde7e..9459dac9c 100644
--- a/rmf_fleet_adapter_python/src/nodes/nodes.cpp
+++ b/rmf_fleet_adapter_python/src/nodes/nodes.cpp
@@ -17,7 +17,6 @@
 
 #include <pybind11/pybind11.h>
 #include <pybind11/operators.h>
-#include <pybind11/iostream.h>
 #include <pybind11/eigen.h>
 #include <pybind11/chrono.h>
 #include <pybind11/stl.h>
diff --git a/rmf_fleet_adapter_python/src/planner/planner.cpp b/rmf_fleet_adapter_python/src/planner/planner.cpp
index d0e1e4934..2703e7f36 100644
--- a/rmf_fleet_adapter_python/src/planner/planner.cpp
+++ b/rmf_fleet_adapter_python/src/planner/planner.cpp
@@ -1,5 +1,4 @@
 #include <pybind11/pybind11.h>
-#include <pybind11/iostream.h>
 #include <pybind11/chrono.h>
 #include <pybind11/eigen.h>
 #include <pybind11/stl.h>
@@ -116,9 +115,7 @@ void bind_plan(py::module& m)
     py::arg("start_time"),
     py::arg("max_merge_waypoint_distance") = 0.1,
     py::arg("max_merge_lane_distance") = 1.0,
-    py::arg("min_lane_length") = 1e-8,
-    py::call_guard<py::scoped_ostream_redirect,
-    py::scoped_estream_redirect>());
+    py::arg("min_lane_length") = 1e-8);
 
   // PLAN ======================================================================
   py::class_<Plan>(m_plan, "Plan")
diff --git a/rmf_fleet_adapter_python/src/tests.cpp b/rmf_fleet_adapter_python/src/tests.cpp
index d56f0cef6..8cbad6f09 100644
--- a/rmf_fleet_adapter_python/src/tests.cpp
+++ b/rmf_fleet_adapter_python/src/tests.cpp
@@ -1,5 +1,4 @@
 #include <pybind11/pybind11.h>
-#include <pybind11/iostream.h>
 #include <pybind11/functional.h>
 #include <pybind11/eigen.h>
 #include <memory>
@@ -29,9 +28,7 @@ void bind_tests(py::module& m)
     py::arg("print_str") = "DUMMY_DOCK_NAME",
     py::arg("docking_finished_callback") = (std::function<void()>)[] (){},
     "Test a shared_ptr<agv::RobotCommandHandle> "
-    "by testing its dock() method",
-    py::call_guard<py::scoped_ostream_redirect,
-    py::scoped_estream_redirect>()
+    "by testing its dock() method"
   );
 
   // Test clone_ptr passing
@@ -47,8 +44,6 @@ void bind_tests(py::module& m)
     },
     "Test a clone_ptr<rmf_traffic::agv::Graph::OrientationConstraint> "
     "by testing its get() method",
-    py::call_guard<py::scoped_ostream_redirect,
-    py::scoped_estream_redirect>(),
     py::return_value_policy::reference_internal
   );
 }
diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Node.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Node.cpp
index 164094ba3..19b6587ba 100644
--- a/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Node.cpp
+++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Node.cpp
@@ -92,17 +92,17 @@ std::vector<ScheduleNode::ConflictSet> get_conflicts(
 
         const auto* dep_v =
           vc->route->check_dependencies(participant, plan_id, r);
-        if (dep_v && !dep_v->empty())
+        if (dep_v)
           continue;
 
         const auto* dep_u =
           route->check_dependencies(vc->participant, vc->plan_id, vc->route_id);
-        if (dep_u && !dep_u->empty())
+        if (dep_u)
           continue;
 
         const auto found_conflict = rmf_traffic::DetectConflict::between(
-          vc->description.profile(), vc->route->trajectory(), dep_v,
-          description->profile(), route->trajectory(), dep_u);
+          vc->description.profile(), vc->route->trajectory(), nullptr,
+          description->profile(), route->trajectory(), nullptr);
         if (found_conflict.has_value())
         {
           conflicts.push_back({participant, vc->participant});