From 10d491217a4b87e5c8a249bad3d932ba4d7bd100 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Tue, 19 Jan 2021 18:10:55 +0800
Subject: [PATCH 1/6] Added scaffolding

---
 rmf_fleet_adapter/CMakeLists.txt              |   5 +
 .../rmf_fleet_adapter/StandardNames.hpp       |   3 +
 rmf_fleet_adapter/package.xml                 |   1 +
 .../src/rmf_fleet_adapter/agv/Node.cpp        |  16 +++
 .../src/rmf_fleet_adapter/agv/Node.hpp        |  13 ++
 .../phases/ReadyToCharge.cpp                  | 118 ++++++++++++++++++
 .../phases/ReadyToCharge.hpp                  |  90 +++++++++++++
 .../rmf_fleet_adapter/tasks/ChargeBattery.cpp |   2 +-
 8 files changed, 247 insertions(+), 1 deletion(-)
 create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.cpp
 create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.hpp

diff --git a/rmf_fleet_adapter/CMakeLists.txt b/rmf_fleet_adapter/CMakeLists.txt
index 59e9625ef..00dfaa95e 100644
--- a/rmf_fleet_adapter/CMakeLists.txt
+++ b/rmf_fleet_adapter/CMakeLists.txt
@@ -29,6 +29,7 @@ endif()
 set(dep_pkgs
   rclcpp
   rmf_utils
+  rmf_charger_msgs
   rmf_dispenser_msgs
   rmf_ingestor_msgs
   rmf_door_msgs
@@ -72,6 +73,7 @@ target_link_libraries(rmf_fleet_adapter
     ${rmf_lift_msgs_LIBRARIES}
     ${rmf_dispenser_msgs_LIBRARIES}
     ${rmf_ingestor_msgs_LIBRARIES}
+    ${rmf_charger_msgs_LIBRARIES}
 )
 
 target_include_directories(rmf_fleet_adapter
@@ -88,6 +90,7 @@ target_include_directories(rmf_fleet_adapter
     ${rmf_lift_msgs_INCLUDE_DIRS}
     ${rmf_dispenser_msgs_INCLUDE_DIRS}
     ${rmf_ingestor_msgs_INCLUDE_DIRS}
+    ${rmf_charger_msgs_INCLUDE_DIRS}
 )
 
 if (BUILD_TESTING)
@@ -122,6 +125,7 @@ if (BUILD_TESTING)
       ${rmf_dispenser_msgs_INCLUDE_DIRS}
       ${rmf_ingestor_msgs_INCLUDE_DIRS}
       ${std_msgs_INCLUDE_DIRS}
+      ${rmf_charger_msgs_INCLUDE_DIRS}
   )
   target_link_libraries(rmf_fleet_adapter_test
     PRIVATE
@@ -135,6 +139,7 @@ if (BUILD_TESTING)
       rmf_fleet_adapter
       rmf_utils::rmf_utils
       ${std_msgs_LIBRARIES}
+      ${rmf_charger_msgs_LIBRARIES}
   )
 
   target_compile_definitions(rmf_fleet_adapter_test
diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp
index e0576111c..6c9c4c15d 100644
--- a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp
+++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp
@@ -56,6 +56,9 @@ const std::string DispatchRequestTopicName = "rmf_task/dispatch_request";
 
 const std::string DockSummaryTopicName = "dock_summary";
 
+const std::string ChargerCancelTopicName = "rmf_charger/cancel";
+const std::string ChargerRequestTopicName = "rmf_charger/requests";
+const std::string ChargerStateTopicName = "rmf_charger/state";
 } // namespace rmf_fleet_adapter
 
 #endif // RMF_FLEET_ADAPTER__STANDARDNAMES_HPP
diff --git a/rmf_fleet_adapter/package.xml b/rmf_fleet_adapter/package.xml
index fad1cd873..cfca49555 100644
--- a/rmf_fleet_adapter/package.xml
+++ b/rmf_fleet_adapter/package.xml
@@ -23,6 +23,7 @@
   <depend>rmf_traffic_msgs</depend>
   <depend>rmf_battery</depend>
   <depend>rmf_task</depend>
+  <depend>rmf_charger_msgs</depend>
 
   <build_depend>eigen</build_depend>
   <build_depend>yaml-cpp</build_depend>
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 9e35fa9d0..e912c8ecc 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp
@@ -62,6 +62,10 @@ std::shared_ptr<Node> Node::make(
         IngestorStateTopicName, default_qos);
   node->_fleet_state_pub = node->create_publisher<FleetState>(
         FleetStateTopicName, default_qos);
+  node->_charger_state_obs = node->create_observable<ChargerState>(
+        ChargerStateTopicName, default_qos);
+  node->_charger_request_pub = node->create_publisher<ChargerRequest>(
+        ChargerRequestTopicName, default_qos);
 
   return node;
 }
@@ -159,5 +163,17 @@ auto Node::fleet_state() const -> const FleetStatePub&
   return _fleet_state_pub;
 }
 
+//==============================================================================
+auto Node::charger_state() const -> const ChargerStateObs&
+{
+  return _charger_state_obs;
+}
+
+//==============================================================================
+auto Node::charger_request() const -> const ChargerRequestPub&
+{
+  return _charger_request_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 39e1811cc..7d926aa78 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp
@@ -20,6 +20,9 @@
 
 #include <rmf_rxcpp/Transport.hpp>
 
+#include <rmf_charger_msgs/msg/charger_cancel.hpp>
+#include <rmf_charger_msgs/msg/charger_request.hpp>
+#include <rmf_charger_msgs/msg/charger_state.hpp>
 #include <rmf_dispenser_msgs/msg/dispenser_request.hpp>
 #include <rmf_dispenser_msgs/msg/dispenser_result.hpp>
 #include <rmf_dispenser_msgs/msg/dispenser_state.hpp>
@@ -101,6 +104,14 @@ class Node : public rmf_rxcpp::Transport
   using IngestorStateObs = rxcpp::observable<IngestorState::SharedPtr>;
   const IngestorStateObs& ingestor_state() const;
 
+  using ChargerState = rmf_charger_msgs::msg::ChargerState;
+  using ChargerStateObs =  rxcpp::observable<ChargerState::SharedPtr>;
+  const ChargerStateObs& charger_state() const;
+
+  using ChargerRequest = rmf_charger_msgs::msg::ChargerRequest;
+  using ChargerRequestPub = rclcpp::Publisher<ChargerRequest>::SharedPtr;
+  const ChargerRequestPub& charger_request() const;
+
   using FleetState = rmf_fleet_msgs::msg::FleetState;
   using FleetStatePub = rclcpp::Publisher<FleetState>::SharedPtr;
   const FleetStatePub& fleet_state() const;
@@ -126,6 +137,8 @@ class Node : public rmf_rxcpp::Transport
   IngestorResultObs _ingestor_result_obs;
   IngestorStateObs _ingestor_state_obs;
   FleetStatePub _fleet_state_pub;
+  ChargerStateObs _charger_state_obs;
+  ChargerRequestPub _charger_request_pub;
 
 };
 
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.cpp
new file mode 100644
index 000000000..508aad11b
--- /dev/null
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.cpp
@@ -0,0 +1,118 @@
+/*
+ * 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 "ReadyToCharge.hpp"
+
+namespace rmf_fleet_adapter {
+namespace phases {
+
+//==============================================================================
+auto ReadyToCharge::Active::observe() const -> const rxcpp::observable<StatusMsg>&
+{
+  return _status_obs;
+}
+
+//==============================================================================
+rmf_traffic::Duration ReadyToCharge::Active::estimate_remaining_time() const
+{
+  // TODO 
+  return rmf_traffic::time::from_seconds(1);
+}
+
+//==============================================================================
+void ReadyToCharge::Active::emergency_alarm(const bool value)
+{
+  // Assume charging station is a holding point
+}
+
+//==============================================================================
+void ReadyToCharge::Active::cancel()
+{
+  // TODO
+}
+
+//==============================================================================
+const std::string& ReadyToCharge::Active::description() const
+{
+  return _description;
+}
+
+//==============================================================================
+ReadyToCharge::Active::Active(
+  agv::RobotContextPtr context)
+: _context(std::move(context)),
+{
+  using rmf_charger_msgs::msg::ChargerState;
+  _description = "Charger Negotiation";
+
+  _status_obs = _context->node()->charger_state()
+    .lift<ChargerState::SharedPtr>(on_subscribe([]()
+    {
+
+    }));
+}
+
+//==============================================================================
+std::shared_ptr<Task::ActivePhase> ReadyToCharge::Pending::begin()
+{
+  auto active =
+      std::shared_ptr<Active>(new Active(_context));
+  return active;
+}
+
+//==============================================================================
+rmf_traffic::Duration ReadyToCharge::Pending::estimate_phase_duration() const
+{
+  return rmf_traffic::Duration{0};
+}
+
+//==============================================================================
+const std::string& ReadyToCharge::Pending::description() const
+{
+  return _description;
+}
+
+//==============================================================================
+ReadyToCharge::Pending::Pending(
+  agv::RobotContextPtr context)
+: _context(std::move(context)),
+  _battery_system(battery_system),
+  _charge_to_soc(charge_to_soc),
+  _time_estimate(time_estimate)
+{
+  _description =
+    "Charging robot to [" + std::to_string(100.0 * charge_to_soc) + "%]";
+}
+
+//==============================================================================
+auto WaitForCharge::make(
+    agv::RobotContextPtr context,
+    rmf_battery::agv::BatterySystem battery_system,
+    double charge_to_soc) -> std::unique_ptr<Pending>
+{
+
+  const double capacity = battery_system.capacity();
+  const double charging_current = battery_system.charging_current();
+  const double time_estimate =
+    3600.0 * capacity * (charge_to_soc - context->current_battery_soc()) / charging_current;
+  
+  return std::unique_ptr<Pending>(
+        new Pending(context, battery_system, charge_to_soc, time_estimate));
+}
+
+}
+};
\ No newline at end of file
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.hpp
new file mode 100644
index 000000000..62707bfb9
--- /dev/null
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.hpp
@@ -0,0 +1,90 @@
+/*
+ * 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__PHASES__READYTOCHARGE_HPP
+#define SRC__RMF_FLEET_ADAPTER__PHASES__READYTOCHARGE_HPP
+
+
+#include "../Task.hpp"
+#include "../agv/RobotContext.hpp"
+
+namespace rmf_fleet_adapter {
+namespace phases {
+
+//==============================================================================
+class ReadyToCharge
+{
+
+  using StatusMsg = Task::StatusMsg;
+  class Pending;
+
+  class Active
+    : public Task::ActivePhase,
+      public std::enable_shared_from_this<Active>
+  {
+  public:
+
+    // Documentation inherited from ActivePhase
+    const rxcpp::observable<StatusMsg>& observe() const final;
+
+    // Documentation inherited from ActivePhase
+    rmf_traffic::Duration estimate_remaining_time() const final;
+
+    // Documentation inherited from ActivePhase
+    void emergency_alarm(bool on) final;
+
+    // Documentation inherited from ActivePhase
+    void cancel() final;
+
+    // Documentation inherited from ActivePhase
+    const std::string & description() const final;
+  
+  private:
+    friend class Pending;
+    rxcpp::observable<StatusMsg> _status_obs;
+    rxcpp::subjects::subject<StatusMsg> _status_publisher;
+    agv::RobotContextPtr _context;
+  };
+
+  class Pending : public Task::PendingPhase
+  {
+  public:
+    // Documentation inherited
+    std::shared_ptr<Task::ActivePhase> begin() final;
+
+    // Documentation inherited
+    rmf_traffic::Duration estimate_phase_duration() const final;
+
+    // Documentation inherited
+    const std::string& description() const final;
+  
+  private:
+    friend class ReadyToCharge;
+    Pending(
+      agv::RobotContextPtr context,
+      double time_estimate);
+
+    agv::RobotContextPtr _context;
+    std::string _description;
+  };
+
+  static std::unique_ptr<Pending> make(
+    agv::RobotContextPtr context);
+};
+}
+}
+#endif
\ No newline at end of file
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 9ef580863..e51cf0839 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp
@@ -16,7 +16,7 @@
 */
 
 #include "../phases/GoToPlace.hpp"
-
+#include "../phases/ReadyToCharge.hpp"
 #include "../phases/WaitForCharge.hpp"
 
 #include "ChargeBattery.hpp"

From 247b9dd0087c284a94535398d69a46e4d3f39c82 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Wed, 20 Jan 2021 13:46:47 +0800
Subject: [PATCH 2/6] Ad preliminary logic

---
 .../phases/ReadyToCharge.cpp                  | 81 ++++++++++++++++++-
 .../phases/ReadyToCharge.hpp                  |  9 +++
 2 files changed, 87 insertions(+), 3 deletions(-)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.cpp
index 508aad11b..384585557 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.cpp
@@ -57,13 +57,88 @@ ReadyToCharge::Active::Active(
 : _context(std::move(context)),
 {
   using rmf_charger_msgs::msg::ChargerState;
+  using rmf_charger_msgs::msg::ChargerRequest;
   _description = "Charger Negotiation";
 
+  //Place holder value for testing
+  std::string desired_charger_name = "charger1";
+
+  //INIT
+  _current_state == State::AWAITING_STATUS;
+
+  //TODO: Add request logic
+  auto request_id = "1234";  
   _status_obs = _context->node()->charger_state()
-    .lift<ChargerState::SharedPtr>(on_subscribe([]()
+    .filter([desired_charger_name](const auto& status_msg)
     {
-
-    }));
+      return status_msgs.charger_name == desired_charger_name;
+    })
+    .map([weak = weak_from_this(), desired_charger_name, request_id](const auto& status_msg)
+    {
+      auto me = weak.lock();
+      if (!me)
+        return Task::StatusMsg();
+
+      if (me->_current_state == State::AWAITING_STATUS)
+      {
+        if(status_msgs.state != ChargerState::CHARGER_IDLE)
+        {
+          Task::StatusMsg status;
+          status.state = Task::StatusMsg::STATE_FAILED;
+          status.status = status_msg.error_message;
+          return status;
+        }
+
+        ChargerRequest request {
+          desired_charger_name,
+          _me->_context->profile()->owner(),
+          _me->_context->profile()->name(),
+          rclcpp::Duration {10,0},
+          request_id
+        }; 
+        
+        _me->_current_state = State::AWAITING_RESPONSE;
+        _me->_timer = _me->_context->node()->create_wall_timer(
+          std::chrono::milliseconds(1000),
+          [weak, request]()
+          {
+            auto me = weak.lock();
+            if (!me)
+              return;
+
+            _me->_context->node()->charger_request().publish(request);
+          }
+        );
+      }
+      else if (_me->_current_state == State::AWAITING_RESPONSE)
+      {
+        if((status_msg.state == ChargerState::CHARGER_ASSIGNED
+          || status_msg.state == ChargerState::CHARGER_CHARGING)
+          && status_msg.robot_name == _me->_context->profile()->name()
+          && status_msg.robot_fleet == _me->_context->profile()->owner()
+          && status_msg.request == request_id)
+        {
+          _me->_timer->stop();
+          Task::StatusMsg status;
+          status.state = Task::StatusMsg::STATE_COMPLETED;
+          status.status = "Negotiation Success";
+          return status;
+        }
+        else if(status_msg.state == ChargerState::CHARGER_ERROR
+        && status_msg.request == request_id)
+        {
+          _me->_timer->stop();
+          Task::StatusMsg status;
+          status.state = Task::StatusMsg::STATE_FAILED;
+          status.status = status_msg.error_message;
+          return status;
+        }
+      }
+      Task::StatusMsg status;
+      status.state = Task::StatusMsg::STATE_ACTIVE;
+      status.status = "Negotiation In progress";
+      return status;
+    });
 }
 
 //==============================================================================
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.hpp
index 62707bfb9..70129213a 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.hpp
@@ -54,10 +54,19 @@ class ReadyToCharge
     const std::string & description() const final;
   
   private:
+    enum State
+    {
+      AWAITING_STATUS = 0,
+      AWAITING_RESPONSE,
+      REQUEST_SUCCESS
+    };
+    
     friend class Pending;
     rxcpp::observable<StatusMsg> _status_obs;
     rxcpp::subjects::subject<StatusMsg> _status_publisher;
     agv::RobotContextPtr _context;
+    State _current_state;
+    rclcpp::TimerBase::SharedPtr _timer;
   };
 
   class Pending : public Task::PendingPhase

From 48ce3619bb60ec37a04d97f1de58c6a0e430de72 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Wed, 20 Jan 2021 15:26:31 +0800
Subject: [PATCH 3/6] fix compile issues

---
 .../phases/ReadyToCharge.cpp                  | 87 ++++++++-----------
 .../phases/ReadyToCharge.hpp                  | 11 ++-
 .../rmf_fleet_adapter/tasks/ChargeBattery.cpp |  2 +
 3 files changed, 48 insertions(+), 52 deletions(-)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.cpp
index 384585557..e77f860cc 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.cpp
@@ -53,8 +53,9 @@ const std::string& ReadyToCharge::Active::description() const
 
 //==============================================================================
 ReadyToCharge::Active::Active(
-  agv::RobotContextPtr context)
-: _context(std::move(context)),
+  agv::RobotContextPtr context,
+  std::string request_id)
+: _context(std::move(context)), _id(request_id)
 {
   using rmf_charger_msgs::msg::ChargerState;
   using rmf_charger_msgs::msg::ChargerRequest;
@@ -64,16 +65,14 @@ ReadyToCharge::Active::Active(
   std::string desired_charger_name = "charger1";
 
   //INIT
-  _current_state == State::AWAITING_STATUS;
+  _current_state = State::AWAITING_STATUS;
 
-  //TODO: Add request logic
-  auto request_id = "1234";  
   _status_obs = _context->node()->charger_state()
     .filter([desired_charger_name](const auto& status_msg)
     {
-      return status_msgs.charger_name == desired_charger_name;
+      return status_msg->charger_name == desired_charger_name;
     })
-    .map([weak = weak_from_this(), desired_charger_name, request_id](const auto& status_msg)
+    .map([weak = weak_from_this(), desired_charger_name](const auto& status_msg)
     {
       auto me = weak.lock();
       if (!me)
@@ -81,24 +80,23 @@ ReadyToCharge::Active::Active(
 
       if (me->_current_state == State::AWAITING_STATUS)
       {
-        if(status_msgs.state != ChargerState::CHARGER_IDLE)
+        if(status_msg->state != ChargerState::CHARGER_IDLE)
         {
           Task::StatusMsg status;
           status.state = Task::StatusMsg::STATE_FAILED;
-          status.status = status_msg.error_message;
+          status.status = status_msg->error_message;
           return status;
         }
 
-        ChargerRequest request {
-          desired_charger_name,
-          _me->_context->profile()->owner(),
-          _me->_context->profile()->name(),
-          rclcpp::Duration {10,0},
-          request_id
-        }; 
+        ChargerRequest request;
+        request.charger_name = desired_charger_name;
+        request.fleet_name = me->_context->description().owner();
+        request.robot_name = me->_context->description().name();
+        request.start_timeout = rclcpp::Duration {10,0};
+        request.request_id = me->_id; 
         
-        _me->_current_state = State::AWAITING_RESPONSE;
-        _me->_timer = _me->_context->node()->create_wall_timer(
+        me->_current_state = State::AWAITING_RESPONSE;
+        me->_timer = me->_context->node()->create_wall_timer(
           std::chrono::milliseconds(1000),
           [weak, request]()
           {
@@ -106,31 +104,31 @@ ReadyToCharge::Active::Active(
             if (!me)
               return;
 
-            _me->_context->node()->charger_request().publish(request);
+            me->_context->node()->charger_request()->publish(request);
           }
         );
       }
-      else if (_me->_current_state == State::AWAITING_RESPONSE)
+      else if (me->_current_state == State::AWAITING_RESPONSE)
       {
-        if((status_msg.state == ChargerState::CHARGER_ASSIGNED
-          || status_msg.state == ChargerState::CHARGER_CHARGING)
-          && status_msg.robot_name == _me->_context->profile()->name()
-          && status_msg.robot_fleet == _me->_context->profile()->owner()
-          && status_msg.request == request_id)
+        if((status_msg->state == ChargerState::CHARGER_ASSIGNED
+          || status_msg->state == ChargerState::CHARGER_CHARGING)
+          && status_msg->robot_name == me->_context->description().name()
+          && status_msg->robot_fleet == me->_context->description().owner()
+          && status_msg->request_id == me->_id)
         {
-          _me->_timer->stop();
+          me->_timer->cancel();
           Task::StatusMsg status;
           status.state = Task::StatusMsg::STATE_COMPLETED;
           status.status = "Negotiation Success";
           return status;
         }
-        else if(status_msg.state == ChargerState::CHARGER_ERROR
-        && status_msg.request == request_id)
+        else if(status_msg->state == ChargerState::CHARGER_ERROR
+        && status_msg->request_id == me->_id)
         {
-          _me->_timer->stop();
+          me->_timer->cancel();
           Task::StatusMsg status;
           status.state = Task::StatusMsg::STATE_FAILED;
-          status.status = status_msg.error_message;
+          status.status = status_msg->error_message;
           return status;
         }
       }
@@ -145,7 +143,7 @@ ReadyToCharge::Active::Active(
 std::shared_ptr<Task::ActivePhase> ReadyToCharge::Pending::begin()
 {
   auto active =
-      std::shared_ptr<Active>(new Active(_context));
+      std::shared_ptr<Active>(new Active(_context, _id));
   return active;
 }
 
@@ -163,31 +161,22 @@ const std::string& ReadyToCharge::Pending::description() const
 
 //==============================================================================
 ReadyToCharge::Pending::Pending(
-  agv::RobotContextPtr context)
-: _context(std::move(context)),
-  _battery_system(battery_system),
-  _charge_to_soc(charge_to_soc),
-  _time_estimate(time_estimate)
+  agv::RobotContextPtr context,
+  std::string id)
+: _context(std::move(context)), _id(id)
 {
   _description =
-    "Charging robot to [" + std::to_string(100.0 * charge_to_soc) + "%]";
+    "PErform9iong Charger Negotiation";
 }
 
 //==============================================================================
-auto WaitForCharge::make(
+auto ReadyToCharge::make(
     agv::RobotContextPtr context,
-    rmf_battery::agv::BatterySystem battery_system,
-    double charge_to_soc) -> std::unique_ptr<Pending>
-{
-
-  const double capacity = battery_system.capacity();
-  const double charging_current = battery_system.charging_current();
-  const double time_estimate =
-    3600.0 * capacity * (charge_to_soc - context->current_battery_soc()) / charging_current;
-  
+    std::string id) -> std::unique_ptr<Pending>
+{  
   return std::unique_ptr<Pending>(
-        new Pending(context, battery_system, charge_to_soc, time_estimate));
+        new Pending(context, id));
 }
 
 }
-};
\ No newline at end of file
+}
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.hpp
index 70129213a..0f019de1c 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.hpp
@@ -28,7 +28,7 @@ namespace phases {
 //==============================================================================
 class ReadyToCharge
 {
-
+public:
   using StatusMsg = Task::StatusMsg;
   class Pending;
 
@@ -37,6 +37,7 @@ class ReadyToCharge
       public std::enable_shared_from_this<Active>
   {
   public:
+    Active(agv::RobotContextPtr context, std::string request_id);
 
     // Documentation inherited from ActivePhase
     const rxcpp::observable<StatusMsg>& observe() const final;
@@ -67,6 +68,8 @@ class ReadyToCharge
     agv::RobotContextPtr _context;
     State _current_state;
     rclcpp::TimerBase::SharedPtr _timer;
+    std::string _id;
+    std::string _description;
   };
 
   class Pending : public Task::PendingPhase
@@ -85,14 +88,16 @@ class ReadyToCharge
     friend class ReadyToCharge;
     Pending(
       agv::RobotContextPtr context,
-      double time_estimate);
+      std::string id);
 
     agv::RobotContextPtr _context;
     std::string _description;
+    std::string _id;
   };
 
   static std::unique_ptr<Pending> make(
-    agv::RobotContextPtr context);
+    agv::RobotContextPtr context,
+    std::string id);
 };
 }
 }
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 e51cf0839..f98054180 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp
@@ -37,6 +37,8 @@ std::shared_ptr<Task> make_charge_battery(
   Task::PendingPhases phases;
   phases.push_back(
     phases::GoToPlace::make(context, std::move(start), goal));
+  phases.push_back(
+    phases::ReadyToCharge::make(context, request->id()));
   phases.push_back(
     phases::WaitForCharge::make(context, request->battery_system(), 0.99));
 

From 7bd2d9197deabb3463d4d31597877431b9a37995 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Thu, 21 Jan 2021 16:06:22 +0800
Subject: [PATCH 4/6] fix deadlock, add tests.

---
 rmf_fleet_adapter/CMakeLists.txt              |  1 +
 .../phases/ReadyToCharge.cpp                  | 60 +++++-------
 .../phases/ReadyToCharge.hpp                  |  3 +-
 .../test/phases/ReadyToChargeTest.cpp         | 97 +++++++++++++++++++
 4 files changed, 124 insertions(+), 37 deletions(-)
 create mode 100644 rmf_fleet_adapter/test/phases/ReadyToChargeTest.cpp

diff --git a/rmf_fleet_adapter/CMakeLists.txt b/rmf_fleet_adapter/CMakeLists.txt
index 00dfaa95e..1f88de5f7 100644
--- a/rmf_fleet_adapter/CMakeLists.txt
+++ b/rmf_fleet_adapter/CMakeLists.txt
@@ -105,6 +105,7 @@ if (BUILD_TESTING)
       test/phases/DoorOpenTest.cpp
       test/phases/DoorCloseTest.cpp
       test/phases/RequestLiftTest.cpp
+      test/phases/ReadyToChargeTest.cpp
       test/phases/DispenseItemTest.cpp
       test/phases/IngestItemTest.cpp
       test/phases/test_GoToPlace.cpp
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.cpp
index e77f860cc..3e849b3f6 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.cpp
@@ -65,50 +65,40 @@ ReadyToCharge::Active::Active(
   std::string desired_charger_name = "charger1";
 
   //INIT
-  _current_state = State::AWAITING_STATUS;
+  _current_state = State::AWAITING_RESPONSE;
+
+  _timer = _context->node()->create_wall_timer(
+    std::chrono::milliseconds(1000),
+    [me = this, desired_charger_name]()
+    {
+      /*auto me = weak.lock();
+      if (!me)
+      {
+        std::cout << "no lock" <<std::endl;
+        return;
+      }*/
+      ChargerRequest request;
+      request.charger_name = desired_charger_name;
+      request.fleet_name = me->_context->description().owner();
+      request.robot_name = me->_context->description().name();
+      request.start_timeout = rclcpp::Duration {10,0};
+      request.request_id = me->_id; 
+      me->_context->node()->charger_request()->publish(request);
+    }
+  );
 
   _status_obs = _context->node()->charger_state()
     .filter([desired_charger_name](const auto& status_msg)
     {
       return status_msg->charger_name == desired_charger_name;
     })
-    .map([weak = weak_from_this(), desired_charger_name](const auto& status_msg)
+    .map([me = weak_from_this()](const auto& status_msg)
     {
-      auto me = weak.lock();
+      /*auto me = weak.lock();
       if (!me)
-        return Task::StatusMsg();
+        return Task::StatusMsg();*/
 
-      if (me->_current_state == State::AWAITING_STATUS)
-      {
-        if(status_msg->state != ChargerState::CHARGER_IDLE)
-        {
-          Task::StatusMsg status;
-          status.state = Task::StatusMsg::STATE_FAILED;
-          status.status = status_msg->error_message;
-          return status;
-        }
-
-        ChargerRequest request;
-        request.charger_name = desired_charger_name;
-        request.fleet_name = me->_context->description().owner();
-        request.robot_name = me->_context->description().name();
-        request.start_timeout = rclcpp::Duration {10,0};
-        request.request_id = me->_id; 
-        
-        me->_current_state = State::AWAITING_RESPONSE;
-        me->_timer = me->_context->node()->create_wall_timer(
-          std::chrono::milliseconds(1000),
-          [weak, request]()
-          {
-            auto me = weak.lock();
-            if (!me)
-              return;
-
-            me->_context->node()->charger_request()->publish(request);
-          }
-        );
-      }
-      else if (me->_current_state == State::AWAITING_RESPONSE)
+      if (me->_current_state == State::AWAITING_RESPONSE)
       {
         if((status_msg->state == ChargerState::CHARGER_ASSIGNED
           || status_msg->state == ChargerState::CHARGER_CHARGING)
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.hpp
index 0f019de1c..db4314b5b 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.hpp
@@ -57,8 +57,7 @@ class ReadyToCharge
   private:
     enum State
     {
-      AWAITING_STATUS = 0,
-      AWAITING_RESPONSE,
+      AWAITING_RESPONSE = 0,
       REQUEST_SUCCESS
     };
     
diff --git a/rmf_fleet_adapter/test/phases/ReadyToChargeTest.cpp b/rmf_fleet_adapter/test/phases/ReadyToChargeTest.cpp
new file mode 100644
index 000000000..b98cdc3f8
--- /dev/null
+++ b/rmf_fleet_adapter/test/phases/ReadyToChargeTest.cpp
@@ -0,0 +1,97 @@
+
+/*
+ * 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 "MockAdapterFixture.hpp"
+
+#include <phases/ReadyToCharge.hpp>
+#include <rmf_fleet_adapter/StandardNames.hpp>
+#include <rmf_utils/catch.hpp>
+
+namespace rmf_fleet_adapter {
+namespace phases {
+namespace test {
+
+using rmf_charger_msgs::msg::ChargerState;
+using rmf_charger_msgs::msg::ChargerRequest;
+
+SCENARIO_METHOD(MockAdapterFixture, "Charger Request Phase", "[phases]")
+{
+  std::mutex m;
+  std::condition_variable received_requests_cv;
+  std::list<ChargerRequest> received_requests;
+
+
+  const auto info = add_robot();
+  const auto& context = info.context;
+
+  auto dispenser_request_pub = adapter->node()->create_publisher<ChargerRequest>(
+    IngestorRequestTopicName, 10);
+  
+  auto request_guid = "test/charger";
+  auto pending_phase = ReadyToCharge::make(
+    context,
+    request_guid
+  );
+  auto active_phase = pending_phase->begin();
+
+  WHEN("it is started")
+  {
+    auto rcl_subscription = adapter->node()->create_subscription<ChargerRequest>(
+    ChargerRequestTopicName,
+    10,
+    [&](ChargerRequest::UniquePtr ingestor_request)
+    {
+      std::unique_lock<std::mutex> lk(m);
+      received_requests.emplace_back(*ingestor_request);
+      received_requests_cv.notify_all();
+    });
+    
+    std::condition_variable status_updates_cv;
+    std::list<Task::StatusMsg> status_updates;
+    /*auto sub = active_phase->observe().subscribe(
+      [&](const auto& status)
+      {
+        std::unique_lock<std::mutex> lk(m);
+        status_updates.emplace_back(status);
+        status_updates_cv.notify_all();
+      });
+
+    THEN("it should send charger request")
+    {
+      std::unique_lock<std::mutex> lk(m);
+      if (received_requests.empty())
+        received_requests_cv.wait(lk, [&]() { return !received_requests.empty(); });
+      REQUIRE(received_requests.size() == 1);
+    }*/
+
+    THEN("it should continuously charger request")
+    {
+      std::unique_lock<std::mutex> lk(m);
+      received_requests_cv.wait(lk, [&]() { return received_requests.size() >= 3; });
+    }
+
+
+    //sub.unsubscribe();
+  }
+
+  WHEN
+}
+
+} // namespace test
+} // namespace phases
+} // namespace rmf_fleet_adapter

From 2c70ae5b85ee95d98f4cf0960499f15e33dcdbb1 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Thu, 21 Jan 2021 18:21:51 +0800
Subject: [PATCH 5/6] Add more tests.

---
 .../phases/ReadyToCharge.cpp                  |  4 +-
 .../test/phases/ReadyToChargeTest.cpp         | 56 ++++++++++++-------
 2 files changed, 39 insertions(+), 21 deletions(-)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.cpp
index 3e849b3f6..38a51038d 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.cpp
@@ -92,12 +92,12 @@ ReadyToCharge::Active::Active(
     {
       return status_msg->charger_name == desired_charger_name;
     })
-    .map([me = weak_from_this()](const auto& status_msg)
+    .map([me =this](const auto& status_msg)
     {
       /*auto me = weak.lock();
       if (!me)
         return Task::StatusMsg();*/
-
+      std::cout << "status received" <<std::endl;
       if (me->_current_state == State::AWAITING_RESPONSE)
       {
         if((status_msg->state == ChargerState::CHARGER_ASSIGNED
diff --git a/rmf_fleet_adapter/test/phases/ReadyToChargeTest.cpp b/rmf_fleet_adapter/test/phases/ReadyToChargeTest.cpp
index b98cdc3f8..58cbe07f2 100644
--- a/rmf_fleet_adapter/test/phases/ReadyToChargeTest.cpp
+++ b/rmf_fleet_adapter/test/phases/ReadyToChargeTest.cpp
@@ -39,16 +39,16 @@ SCENARIO_METHOD(MockAdapterFixture, "Charger Request Phase", "[phases]")
   const auto info = add_robot();
   const auto& context = info.context;
 
-  auto dispenser_request_pub = adapter->node()->create_publisher<ChargerRequest>(
-    IngestorRequestTopicName, 10);
+  auto charger_state_pub = adapter->node()->create_publisher<ChargerState>(
+    ChargerStateTopicName, 10);
   
   auto request_guid = "test/charger";
   auto pending_phase = ReadyToCharge::make(
     context,
     request_guid
   );
-  auto active_phase = pending_phase->begin();
-
+ 
+#if 0
   WHEN("it is started")
   {
     auto rcl_subscription = adapter->node()->create_subscription<ChargerRequest>(
@@ -61,35 +61,53 @@ SCENARIO_METHOD(MockAdapterFixture, "Charger Request Phase", "[phases]")
       received_requests_cv.notify_all();
     });
     
+    THEN("it should continuously request a response")
+    {
+      std::unique_lock<std::mutex> lk(m);
+      received_requests_cv.wait(lk, [&]() { return received_requests.size() >= 3; });
+    }
+
+
+    //sub.unsubscribe();
+  }
+#endif
+  WHEN("a response is sent")
+  {
+    auto rcl_subscription = adapter->node()->create_subscription<ChargerRequest>(
+    ChargerRequestTopicName,
+    10,
+    [&](ChargerRequest::UniquePtr request)
+    {
+      std::cout << "received request" <<std::endl;
+      ChargerState state;
+      state.state = ChargerState::CHARGER_ASSIGNED;
+      state.robot_name = request->robot_name;
+      state.robot_fleet = request->fleet_name; 
+      state.request_id = request->request_id;
+      state.charger_name = request->charger_name;
+      charger_state_pub->publish(state);
+    });
+    
+    auto active_phase = pending_phase->begin();
     std::condition_variable status_updates_cv;
     std::list<Task::StatusMsg> status_updates;
-    /*auto sub = active_phase->observe().subscribe(
+    auto sub = active_phase->observe().subscribe(
       [&](const auto& status)
       {
+        std::cout << "status update" <<std::endl;
         std::unique_lock<std::mutex> lk(m);
         status_updates.emplace_back(status);
         status_updates_cv.notify_all();
       });
 
-    THEN("it should send charger request")
+    THEN("it should send a OK ")
     {
       std::unique_lock<std::mutex> lk(m);
-      if (received_requests.empty())
-        received_requests_cv.wait(lk, [&]() { return !received_requests.empty(); });
-      REQUIRE(received_requests.size() == 1);
-    }*/
-
-    THEN("it should continuously charger request")
-    {
-      std::unique_lock<std::mutex> lk(m);
-      received_requests_cv.wait(lk, [&]() { return received_requests.size() >= 3; });
+      status_updates_cv.wait(lk, [&]() { return !status_updates.empty(); });
+      REQUIRE(status_updates.begin()->state == Task::StatusMsg::STATE_COMPLETED);
     }
-
-
     //sub.unsubscribe();
   }
-
-  WHEN
 }
 
 } // namespace test

From 2791c16bfd07f190f752994800943767cf84b372 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Fri, 22 Jan 2021 09:49:17 +0800
Subject: [PATCH 6/6] Add support for location

---
 .../phases/ReadyToCharge.cpp                  | 21 +++++++++++--------
 .../phases/ReadyToCharge.hpp                  | 13 ++++++++----
 .../rmf_fleet_adapter/tasks/ChargeBattery.cpp |  5 ++++-
 .../test/phases/ReadyToChargeTest.cpp         |  3 ++-
 4 files changed, 27 insertions(+), 15 deletions(-)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.cpp
index 38a51038d..967cd0d4f 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.cpp
@@ -30,7 +30,7 @@ auto ReadyToCharge::Active::observe() const -> const rxcpp::observable<StatusMsg
 rmf_traffic::Duration ReadyToCharge::Active::estimate_remaining_time() const
 {
   // TODO 
-  return rmf_traffic::time::from_seconds(1);
+  return rmf_traffic::time::from_seconds(5);
 }
 
 //==============================================================================
@@ -54,7 +54,8 @@ const std::string& ReadyToCharge::Active::description() const
 //==============================================================================
 ReadyToCharge::Active::Active(
   agv::RobotContextPtr context,
-  std::string request_id)
+  const std::string request_id,
+  const std::string location)
 : _context(std::move(context)), _id(request_id)
 {
   using rmf_charger_msgs::msg::ChargerState;
@@ -62,7 +63,7 @@ ReadyToCharge::Active::Active(
   _description = "Charger Negotiation";
 
   //Place holder value for testing
-  std::string desired_charger_name = "charger1";
+  std::string desired_charger_name = location;
 
   //INIT
   _current_state = State::AWAITING_RESPONSE;
@@ -133,7 +134,7 @@ ReadyToCharge::Active::Active(
 std::shared_ptr<Task::ActivePhase> ReadyToCharge::Pending::begin()
 {
   auto active =
-      std::shared_ptr<Active>(new Active(_context, _id));
+      std::shared_ptr<Active>(new Active(_context, _id, _location));
   return active;
 }
 
@@ -152,20 +153,22 @@ const std::string& ReadyToCharge::Pending::description() const
 //==============================================================================
 ReadyToCharge::Pending::Pending(
   agv::RobotContextPtr context,
-  std::string id)
-: _context(std::move(context)), _id(id)
+  const std::string id,
+  const std::string location)
+: _context(std::move(context)), _id(id), _location(location)
 {
   _description =
-    "PErform9iong Charger Negotiation";
+    "Performing Charger Negotiation";
 }
 
 //==============================================================================
 auto ReadyToCharge::make(
     agv::RobotContextPtr context,
-    std::string id) -> std::unique_ptr<Pending>
+    const std::string id,
+    const std::string location) -> std::unique_ptr<Pending>
 {  
   return std::unique_ptr<Pending>(
-        new Pending(context, id));
+        new Pending(context, id, location));
 }
 
 }
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.hpp
index db4314b5b..6a834ac6e 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.hpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.hpp
@@ -37,7 +37,9 @@ class ReadyToCharge
       public std::enable_shared_from_this<Active>
   {
   public:
-    Active(agv::RobotContextPtr context, std::string request_id);
+    Active(agv::RobotContextPtr context,
+      std::string request_id,
+      std::string location);
 
     // Documentation inherited from ActivePhase
     const rxcpp::observable<StatusMsg>& observe() const final;
@@ -87,17 +89,20 @@ class ReadyToCharge
     friend class ReadyToCharge;
     Pending(
       agv::RobotContextPtr context,
-      std::string id);
+      std::string id,
+      std::string location);
 
     agv::RobotContextPtr _context;
     std::string _description;
     std::string _id;
+    std::string _location;
   };
 
   static std::unique_ptr<Pending> make(
     agv::RobotContextPtr context,
-    std::string id);
-};
+    const std::string id,
+    const std::string location);
+  };
 }
 }
 #endif
\ No newline at end of file
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 f98054180..441de4861 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp
@@ -33,12 +33,15 @@ std::shared_ptr<Task> make_charge_battery(
     const rmf_task::agv::State finish_state)
 {
   rmf_traffic::agv::Planner::Goal goal{finish_state.charging_waypoint()};
+  const auto location_waypoint = 
+    context->navigation_graph().get_waypoint(finish_state.charging_waypoint());
+  auto location = location_waypoint.name();
 
   Task::PendingPhases phases;
   phases.push_back(
     phases::GoToPlace::make(context, std::move(start), goal));
   phases.push_back(
-    phases::ReadyToCharge::make(context, request->id()));
+    phases::ReadyToCharge::make(context, request->id(), *location));
   phases.push_back(
     phases::WaitForCharge::make(context, request->battery_system(), 0.99));
 
diff --git a/rmf_fleet_adapter/test/phases/ReadyToChargeTest.cpp b/rmf_fleet_adapter/test/phases/ReadyToChargeTest.cpp
index 58cbe07f2..facfff70f 100644
--- a/rmf_fleet_adapter/test/phases/ReadyToChargeTest.cpp
+++ b/rmf_fleet_adapter/test/phases/ReadyToChargeTest.cpp
@@ -45,7 +45,8 @@ SCENARIO_METHOD(MockAdapterFixture, "Charger Request Phase", "[phases]")
   auto request_guid = "test/charger";
   auto pending_phase = ReadyToCharge::make(
     context,
-    request_guid
+    request_guid,
+    "charger1"
   );
  
 #if 0