Merge pull request #2 from open-rmf/main

Adding initiator and request time to booking (open-rmf#267)

rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp

@@ -35,6 +35,8 @@
 #include <rmf_task_sequence/Phase.hpp>
 #include <rmf_task_sequence/Event.hpp>
 
+#include <rclcpp/node.hpp>
+
 namespace rmf_fleet_adapter {
 namespace agv {
 
@@ -367,6 +369,13 @@ class FleetUpdateHandle : public std::enable_shared_from_this<FleetUpdateHandle>
   FleetUpdateHandle& set_update_listener(
     std::function<void(const nlohmann::json&)> listener);
 
+  /// Get the rclcpp::Node that this fleet update handle will be using for
+  /// communication.
+  std::shared_ptr<rclcpp::Node> node();
+
+  /// const-qualified node()
+  std::shared_ptr<const rclcpp::Node> node() const;
+
   // Do not allow moving
   FleetUpdateHandle(FleetUpdateHandle&&) = delete;
   FleetUpdateHandle& operator=(FleetUpdateHandle&&) = delete;

rmf_fleet_adapter/src/full_control/main.cpp

@@ -1234,18 +1234,35 @@ std::shared_ptr<Connections> make_fleet(
   const std::string finishing_request_string =
     node->declare_parameter("finishing_request", "nothing");
   rmf_task::ConstRequestFactoryPtr finishing_request = nullptr;
+  std::function<rmf_traffic::Time()> get_time =
+    [n = std::weak_ptr<rclcpp::Node>(node)]()
+    {
+      const auto node = n.lock();
+      if (!node)
+      {
+        const auto time_since_epoch =
+          std::chrono::system_clock::now().time_since_epoch();
+        return rmf_traffic::Time(time_since_epoch);
+      }
+      return rmf_traffic_ros2::convert(node->now());
+    };
+
   if (finishing_request_string == "charge")
   {
     finishing_request =
-      std::make_shared<rmf_task::requests::ChargeBatteryFactory>();
+      std::make_shared<rmf_task::requests::ChargeBatteryFactory>(
+      std::string(node->get_name()),
+      std::move(get_time));
     RCLCPP_INFO(
       node->get_logger(),
       "Fleet is configured to perform ChargeBattery as finishing request");
   }
   else if (finishing_request_string == "park")
   {
     finishing_request =
-      std::make_shared<rmf_task::requests::ParkRobotFactory>();
+      std::make_shared<rmf_task::requests::ParkRobotFactory>(
+      std::string(node->get_name()),
+      std::move(get_time));
     RCLCPP_INFO(
       node->get_logger(),
       "Fleet is configured to perform ParkRobot as finishing request");

rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp

@@ -421,6 +421,17 @@ void copy_booking_data(
   booking_json["id"] = booking.id();
   booking_json["unix_millis_earliest_start_time"] =
     to_millis(booking.earliest_start_time().time_since_epoch()).count();
+  const auto requester = booking.requester();
+  if (requester.has_value())
+  {
+    booking_json["requester"] = requester.value();
+  }
+  const auto request_time = booking.request_time();
+  if (request_time.has_value())
+  {
+    booking_json["unix_millis_request_time"] =
+      to_millis(request_time.value().time_since_epoch()).count();
+  }
   // TODO(MXG): Add priority and labels
 }
 
@@ -1576,7 +1587,11 @@ void TaskManager::retreat_to_charger()
   {
     // Add a new charging task to the task queue
     const auto charging_request = rmf_task::requests::ChargeBattery::make(
-      current_state.time().value());
+      current_state.time().value(),
+      _context->requester_id(),
+      rmf_traffic_ros2::convert(_context->node()->now()),
+      nullptr,
+      true);
     const auto model = charging_request->description()->make_model(
       current_state.time().value(),
       parameters);

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp

@@ -238,6 +238,15 @@ std::shared_ptr<rmf_task::Request> FleetUpdateHandle::Implementation::convert(
       rmf_traffic::Time(std::chrono::milliseconds(t_it->get<int64_t>()));
   }
 
+  rmf_traffic::Time request_time = rmf_traffic_ros2::convert(
+    node->get_clock()->now());
+  const auto r_it = request_msg.find("unix_millis_request_time");
+  if (r_it != request_msg.end())
+  {
+    request_time =
+      rmf_traffic::Time(std::chrono::milliseconds(r_it->get<int64_t>()));
+  }
+
   // Note: make_low_priority() actually returns a nullptr.
   rmf_task::ConstPriorityPtr priority =
     rmf_task::BinaryPriorityScheme::make_low_priority();
@@ -279,11 +288,28 @@ std::shared_ptr<rmf_task::Request> FleetUpdateHandle::Implementation::convert(
     }
   }
 
-  const auto new_request =
-    std::make_shared<rmf_task::Request>(
+  std::optional<std::string> requester = std::nullopt;
+  const auto i_it = request_msg.find("requester");
+  if (i_it != request_msg.end())
+  {
+    requester = i_it->get<std::string>();
+  }
+
+  rmf_task::Task::ConstBookingPtr booking = requester.has_value() ?
+    std::make_shared<const rmf_task::Task::Booking>(
+    task_id,
+    earliest_start_time,
+    priority,
+    requester.value(),
+    request_time,
+    false) :
+    std::make_shared<const rmf_task::Task::Booking>(
     task_id,
     earliest_start_time,
     priority,
+    false);
+  const auto new_request = std::make_shared<rmf_task::Request>(
+    std::move(booking),
     deserialized_task.description);
 
   return new_request;
@@ -1995,6 +2021,18 @@ FleetUpdateHandle& FleetUpdateHandle::set_update_listener(
   return *this;
 }
 
+//==============================================================================
+std::shared_ptr<rclcpp::Node> FleetUpdateHandle::node()
+{
+  return _pimpl->node;
+}
+
+//==============================================================================
+std::shared_ptr<const rclcpp::Node> FleetUpdateHandle::node() const
+{
+  return _pimpl->node;
+}
+
 //==============================================================================
 bool FleetUpdateHandle::set_task_planner_params(
   std::shared_ptr<rmf_battery::agv::BatterySystem> battery_system,
@@ -2044,7 +2082,7 @@ bool FleetUpdateHandle::set_task_planner_params(
         // automatic retreat. Hence, we also update them whenever the
         // task planner here is updated.
         self->_pimpl->task_planner = std::make_shared<rmf_task::TaskPlanner>(
-          std::move(task_config), std::move(options));
+          self->_pimpl->name, std::move(task_config), std::move(options));
 
         for (const auto& t : self->_pimpl->task_managers)
           t.first->task_planner(self->_pimpl->task_planner);

rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp

@@ -53,7 +53,17 @@ rmf_task::Task::ActivePtr EmergencyPullover::start(
       std::make_shared<EmergencyPulloverDescription>()), {});
 
   const auto desc = builder.build("Emergency Pullover", "");
-  const rmf_task::Request request(task_id, context->now(), nullptr, desc, true);
+
+  const auto time_now = context->now();
+  rmf_task::Task::ConstBookingPtr booking =
+    std::make_shared<const rmf_task::Task::Booking>(
+    task_id,
+    time_now,
+    nullptr,
+    context->requester_id(),
+    time_now,
+    true);
+  const rmf_task::Request request(std::move(booking), desc);
 
   return activator.activate(
     context->make_get_state(),

rmf_fleet_adapter/src/rmf_fleet_adapter/events/ResponsiveWait.cpp

@@ -70,7 +70,17 @@ rmf_task::Task::ActivePtr ResponsiveWait::start(
       ResponsiveWait::Description::make_indefinite(waiting_point)), {});
 
   const auto desc = builder.build("Responsive Wait", "");
-  const rmf_task::Request request(task_id, context->now(), nullptr, desc, true);
+
+  const auto time_now = context->now();
+  rmf_task::Task::ConstBookingPtr booking =
+    std::make_shared<const rmf_task::Task::Booking>(
+    task_id,
+    time_now,
+    nullptr,
+    context->requester_id(),
+    time_now,
+    true);
+  const rmf_task::Request request(std::move(booking), desc);
 
   return context->task_activator()->activate(
     context->make_get_state(),

rmf_fleet_adapter_python/src/adapter.cpp

@@ -4,6 +4,7 @@
 #include <pybind11/eigen.h>
 #include <pybind11/stl.h>
 #include "pybind11_json/pybind11_json.hpp"
+#include <functional>
 #include <memory>
 
 #include "rmf_traffic_ros2/Time.hpp"
@@ -333,21 +334,38 @@ PYBIND11_MODULE(rmf_adapter, m) {
     bool account_for_battery_drain,
     const std::string& finishing_request_string = "nothing")
     {
+      std::function<rmf_traffic::Time()> time_now = nullptr;
+      std::optional<std::string> planner_id = std::nullopt;
+      const auto node = self.node();
+      if (node)
+      {
+        time_now = [n = node->weak_from_this()]()
+        {
+          const auto node = n.lock();
+          if (!node)
+          {
+            const auto time_since_epoch =
+              std::chrono::system_clock::now().time_since_epoch();
+            return rmf_traffic::Time(time_since_epoch);
+          }
+          return rmf_traffic_ros2::convert(node->now());
+        };
+        planner_id = node->get_name();
+      }
+
       // Supported finishing_request_string: [charge, park, nothing]
-      rmf_task::ConstRequestFactoryPtr finishing_request;
+      rmf_task::ConstRequestFactoryPtr finishing_request = nullptr;
       if (finishing_request_string == "charge")
       {
-        finishing_request =
-        std::make_shared<rmf_task::requests::ChargeBatteryFactory>();
+        finishing_request = planner_id.has_value() && time_now ?
+          std::make_shared<rmf_task::requests::ChargeBatteryFactory>(planner_id.value(), std::move(time_now)) :
+          std::make_shared<rmf_task::requests::ChargeBatteryFactory>();
       }
       else if (finishing_request_string == "park")
       {
-        finishing_request =
-        std::make_shared<rmf_task::requests::ParkRobotFactory>();
-      }
-      else
-      {
-        finishing_request = nullptr;
+        finishing_request = planner_id.has_value() && time_now ?
+          std::make_shared<rmf_task::requests::ParkRobotFactory>(planner_id.value(), std::move(time_now)) :
+          std::make_shared<rmf_task::requests::ParkRobotFactory>();
       }
 
       return self.set_task_planner_params(

rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp

@@ -800,8 +800,10 @@ class Dispatcher::Implementation
 
     static const std::vector<std::string> copy_fields = {
       "unix_millis_earliest_start_time",
+      "unix_millis_request_time",
       "priority",
-      "labels"
+      "labels",
+      "requester",
     };
 
     for (const auto& field : copy_fields)