loop delivery tests for dispatcher

rmf_fleet_adapter/include/rmf_fleet_adapter/agv/test/MockAdapter.hpp

@@ -22,8 +22,7 @@
 
 #include <rclcpp/node.hpp>
 
-#include <rmf_task_msgs/msg/delivery.hpp>
-#include <rmf_task_msgs/msg/loop.hpp>
+#include <rmf_task_msgs/msg/task_profile.hpp>
 
 #include <rmf_traffic/schedule/Snapshot.hpp>
 
@@ -41,21 +40,21 @@ class MockAdapter : public std::enable_shared_from_this<MockAdapter>
 
   /// Create a mock adapter
   MockAdapter(
-      const std::string& node_name,
-      const rclcpp::NodeOptions& node_options = rclcpp::NodeOptions());
+    const std::string& node_name,
+    const rclcpp::NodeOptions& node_options = rclcpp::NodeOptions());
 
   /// Add a fleet to test
   std::shared_ptr<FleetUpdateHandle> add_fleet(
-      const std::string& fleet_name,
-      rmf_traffic::agv::VehicleTraits traits,
-      rmf_traffic::agv::Graph navigation_graph);
+    const std::string& fleet_name,
+    rmf_traffic::agv::VehicleTraits traits,
+    rmf_traffic::agv::Graph navigation_graph);
 
   TrafficLight::UpdateHandlePtr add_traffic_light(
-      std::shared_ptr<TrafficLight::CommandHandle> command,
-      const std::string& fleet_name,
-      const std::string& robot_name,
-      rmf_traffic::agv::VehicleTraits traits,
-      rmf_traffic::Profile profile);
+    std::shared_ptr<TrafficLight::CommandHandle> command,
+    const std::string& fleet_name,
+    const std::string& robot_name,
+    rmf_traffic::agv::VehicleTraits traits,
+    rmf_traffic::Profile profile);
 
   /// Get the rclcpp Node for this adapter
   std::shared_ptr<rclcpp::Node> node();
@@ -69,11 +68,8 @@ class MockAdapter : public std::enable_shared_from_this<MockAdapter>
   /// Stop this adapter from spinning
   void stop();
 
-  /// Submit a delivery request
-  void request_delivery(const rmf_task_msgs::msg::Delivery& request);
-
-  /// Submit a loop request
-  void request_loop(const rmf_task_msgs::msg::Loop& request);
+  /// Submit a task request
+  void dispatch_task(const rmf_task_msgs::msg::TaskProfile& profile);
 
   class Implementation;
 private:

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp

@@ -95,7 +95,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb(
 {
   if (task_managers.empty())
     return;
-  
+
   if (msg->task_profile.task_id.empty())
     return;
 
@@ -802,5 +802,32 @@ FleetUpdateHandle::FleetUpdateHandle()
   // Do nothing
 }
 
+//==============================================================================
+void dispatch_task(
+    const rmf_task_msgs::msg::TaskProfile profile,
+    const std::vector<std::shared_ptr<FleetUpdateHandle>>& fleets)
+{
+  for (auto& fleet : fleets)
+  {
+    auto& fimpl = FleetUpdateHandle::Implementation::get(*fleet);
+    if (!fimpl.accept_task)
+      continue;
+
+    // NOTE: althought the current adapter supports multiple fleets. The test
+    // here assumses using a single fleet for each adapter
+    rmf_task_msgs::msg::BidNotice bid;
+    bid.task_profile = profile;
+    fimpl.bid_notice_cb(
+      std::make_shared<rmf_task_msgs::msg::BidNotice>(bid));
+
+    rmf_task_msgs::msg::DispatchRequest req;
+    req.task_profile = profile;
+    req.fleet_name = fimpl.name;
+    req.method = req.ADD;
+    fimpl.dispatch_request_cb(
+      std::make_shared<rmf_task_msgs::msg::DispatchRequest>(req));
+  }
+}
+
 } // namespace agv
 } // namespace rmf_fleet_adapter

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp

@@ -267,6 +267,11 @@ class FleetUpdateHandle::Implementation
 
 };
 
+//==============================================================================
+void dispatch_task(
+    const rmf_task_msgs::msg::TaskProfile profile,
+    const std::vector<std::shared_ptr<FleetUpdateHandle>>& fleets);
+
 } // namespace agv
 } // namespace rmf_fleet_adapter
 

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp

@@ -139,16 +139,11 @@ void MockAdapter::stop()
 }
 
 //==============================================================================
-void MockAdapter::request_delivery(const rmf_task_msgs::msg::Delivery& request)
+void MockAdapter::dispatch_task(const rmf_task_msgs::msg::TaskProfile& profile)
 {
-  // rmf_fleet_adapter::agv::request_delivery(request, _pimpl->fleets);
+  rmf_fleet_adapter::agv::dispatch_task(profile, _pimpl->fleets);
 }
 
-//==============================================================================
-void MockAdapter::request_loop(const rmf_task_msgs::msg::Loop& request)
-{
-  // rmf_fleet_adapter::agv::request_loop(request, _pimpl->fleets);
-}
 
 } // namespace test
 } // namespace agv