[WIP] Feature/charger integration

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)
@@ -102,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
@@ -122,6 +126,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 +140,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

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

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>

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

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;
 
 };
 

rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.cpp

@@ -0,0 +1,175 @@
+/*
+ * 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(5);
+}
+
+//==============================================================================
+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,
+  const std::string request_id,
+  const std::string location)
+: _context(std::move(context)), _id(request_id)
+{
+  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 = location;
+
+  //INIT
+  _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([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
+          || 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->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_id == me->_id)
+        {
+          me->_timer->cancel();
+          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;
+    });
+}
+
+//==============================================================================
+std::shared_ptr<Task::ActivePhase> ReadyToCharge::Pending::begin()
+{
+  auto active =
+      std::shared_ptr<Active>(new Active(_context, _id, _location));
+  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,
+  const std::string id,
+  const std::string location)
+: _context(std::move(context)), _id(id), _location(location)
+{
+  _description =
+    "Performing Charger Negotiation";
+}
+
+//==============================================================================
+auto ReadyToCharge::make(
+    agv::RobotContextPtr context,
+    const std::string id,
+    const std::string location) -> std::unique_ptr<Pending>
+{  
+  return std::unique_ptr<Pending>(
+        new Pending(context, id, location));
+}
+
+}
+}