Loop and Delivery integration with dispatcher framework

rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp

@@ -20,11 +20,14 @@
 #include <rmf_task/requests/ChargeBattery.hpp>
 #include <rmf_task/requests/Clean.hpp>
 #include <rmf_task/requests/Delivery.hpp>
+#include <rmf_task/requests/Loop.hpp>
 
 #include <rmf_traffic_ros2/Time.hpp>
 
 #include "tasks/Clean.hpp"
 #include "tasks/ChargeBattery.hpp"
+#include "tasks/Delivery.hpp"
+#include "tasks/Loop.hpp"
 
 #include <iostream>
 
@@ -195,13 +198,50 @@ void TaskManager::set_queue(
       msg.task_profile.task_id = _queue.back()->id();
       msg.state = msg.STATE_QUEUED;
       msg.robot_name = _context->name();
-      this->_context->node()->task_summary()->publish(msg);    }
+      this->_context->node()->task_summary()->publish(msg);
+
+    }
 
     else if (const auto request =
       std::dynamic_pointer_cast<const rmf_task::requests::Delivery>(
         a.request()))
     {
+      const auto task = tasks::make_delivery(
+        request,
+        _context,
+        start,
+        a.deployment_time(),
+        a.state());
+
+      _queue.push_back(task);
 
+      rmf_task_msgs::msg::TaskSummary msg;
+      msg.task_id = _queue.back()->id();
+      msg.task_profile.task_id = _queue.back()->id();
+      msg.state = msg.STATE_QUEUED;
+      msg.robot_name = _context->name();
+      this->_context->node()->task_summary()->publish(msg);
+
+    }
+
+    else if (const auto request =
+      std::dynamic_pointer_cast<const rmf_task::requests::Loop>(a.request()))
+    {
+      const auto task = tasks::make_loop(
+        request,
+        _context,
+        start,
+        a.deployment_time(),
+        a.state());
+
+      _queue.push_back(task);
+
+      rmf_task_msgs::msg::TaskSummary msg;
+      msg.task_id = _queue.back()->id();
+      msg.task_profile.task_id = _queue.back()->id();
+      msg.state = msg.STATE_QUEUED;
+      msg.robot_name = _context->name();
+      this->_context->node()->task_summary()->publish(msg);
     }
 
     else

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp

@@ -22,6 +22,14 @@
 #include "../tasks/Delivery.hpp"
 #include "../tasks/Loop.hpp"
 
+#include <rmf_task/requests/Clean.hpp>
+#include <rmf_task/requests/Delivery.hpp>
+#include <rmf_task/requests/Loop.hpp>
+
+#include <rmf_task_msgs/msg/clean.hpp>
+#include <rmf_task_msgs/msg/delivery.hpp> 
+#include <rmf_task_msgs/msg/loop.hpp>
+
 #include <iostream>
 #include <unordered_map>
 
@@ -236,11 +244,180 @@ void FleetUpdateHandle::Implementation::bid_notice_cb(
 
   else if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_DELIVERY)
   {
-    // TODO(YV)
+    const auto& delivery = task_profile.delivery;
+    if (delivery.pickup_place_name.empty())
+    {
+      RCLCPP_INFO(
+        node->get_logger(),
+        "Required param [delivery.pickup_place_name] missing in TaskProfile."
+        "Rejecting BidNotice with task_id:[%s]" , id.c_str());
+
+      return;
+    }
+
+    if (delivery.pickup_dispenser.empty())
+    {
+      RCLCPP_INFO(
+        node->get_logger(),
+        "Required param [delivery.pickup_dispenser] missing in TaskProfile."
+        "Rejecting BidNotice with task_id:[%s]" , id.c_str());
+
+      return;
+    }
+
+    if (delivery.dropoff_place_name.empty())
+    {
+      RCLCPP_INFO(
+        node->get_logger(),
+        "Required param [delivery.dropoff_place_name] missing in TaskProfile."
+        "Rejecting BidNotice with task_id:[%s]" , id.c_str());
+
+      return;
+    }
+
+    if (delivery.dropoff_place_name.empty())
+    {
+      RCLCPP_INFO(
+        node->get_logger(),
+        "Required param [delivery.dropoff_place_name] missing in TaskProfile."
+        "Rejecting BidNotice with task_id:[%s]" , id.c_str());
+
+      return;
+    }
+
+    if (delivery.dropoff_ingestor.empty())
+    {
+      RCLCPP_INFO(
+        node->get_logger(),
+        "Required param [delivery.dropoff_ingestor] missing in TaskProfile."
+        "Rejecting BidNotice with task_id:[%s]" , id.c_str());
+
+      return;
+    }
+
+    const auto pickup_wp = graph.find_waypoint(delivery.pickup_place_name);
+    if (!pickup_wp)
+    {
+      RCLCPP_INFO(
+        node->get_logger(),
+        "Fleet [%s] does not have a named waypoint [%s] configured in its "
+        "nav graph. Rejecting BidNotice with task_id:[%s]",
+        name.c_str(), delivery.pickup_place_name.c_str(), id.c_str());
+
+        return;
+    }
+
+    const auto dropoff_wp = graph.find_waypoint(delivery.dropoff_place_name);
+    if (!dropoff_wp)
+    {
+      RCLCPP_INFO(
+        node->get_logger(),
+        "Fleet [%s] does not have a named waypoint [%s] configured in its "
+        "nav graph. Rejecting BidNotice with task_id:[%s]",
+        name.c_str(), delivery.dropoff_place_name.c_str(), id.c_str());
+
+        return;
+    }
+
+    // TODO(YV) get rid of id field in RequestPtr
+    std::stringstream id_stream(id);
+    std::size_t request_id;
+    id_stream >> request_id;
+
+    new_request = rmf_task::requests::Delivery::make(
+      request_id,
+      pickup_wp->index(),
+      delivery.pickup_dispenser,
+      dropoff_wp->index(),
+      delivery.dropoff_ingestor,
+      delivery.items,
+      motion_sink,
+      ambient_sink,
+      planner,
+      start_time,
+      drain_battery);
+
+    RCLCPP_INFO(
+      node->get_logger(),
+      "Generated Delivery request");
+
   }
   else if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_LOOP)
   {
-    // TODO(YV)
+    const auto& loop = task_profile.loop;
+    if (loop.start_name.empty())
+    {
+      RCLCPP_INFO(
+        node->get_logger(),
+        "Required param [loop.start_name] missing in TaskProfile."
+        "Rejecting BidNotice with task_id:[%s]" , id.c_str());
+
+      return;
+    }
+
+    if (loop.finish_name.empty())
+    {
+      RCLCPP_INFO(
+        node->get_logger(),
+        "Required param [loop.finish_name] missing in TaskProfile."
+        "Rejecting BidNotice with task_id:[%s]" , id.c_str());
+
+      return;
+    }
+
+    if (loop.num_loops < 1)
+    {
+      RCLCPP_INFO(
+        node->get_logger(),
+        "Required param [loop.num_loops] in TaskProfile is invalid."
+        "Rejecting BidNotice with task_id:[%s]" , id.c_str());
+
+      return;
+    }
+
+    const auto start_wp = graph.find_waypoint(loop.start_name);
+    if (!start_wp)
+    {
+      RCLCPP_INFO(
+        node->get_logger(),
+        "Fleet [%s] does not have a named waypoint [%s] configured in its "
+        "nav graph. Rejecting BidNotice with task_id:[%s]",
+        name.c_str(), loop.start_name.c_str(), id.c_str());
+
+        return;
+    }
+
+    const auto finish_wp = graph.find_waypoint(loop.finish_name);
+    if (!finish_wp)
+    {
+      RCLCPP_INFO(
+        node->get_logger(),
+        "Fleet [%s] does not have a named waypoint [%s] configured in its "
+        "nav graph. Rejecting BidNotice with task_id:[%s]",
+        name.c_str(), loop.finish_name.c_str(), id.c_str());
+
+        return;
+    }
+
+    // TODO(YV) get rid of id field in RequestPtr
+    std::stringstream id_stream(id);
+    std::size_t request_id;
+    id_stream >> request_id;
+
+    new_request = rmf_task::requests::Loop::make(
+      request_id,
+      start_wp->index(),
+      finish_wp->index(),
+      loop.num_loops,
+      motion_sink,
+      ambient_sink,
+      planner,
+      start_time,
+      drain_battery);
+
+    RCLCPP_INFO(
+      node->get_logger(),
+      "Generated Loop request");
   }
   else
   {
@@ -354,7 +531,6 @@ void FleetUpdateHandle::Implementation::bid_notice_cb(
 
 }
 
-
 void FleetUpdateHandle::Implementation::dispatch_request_cb(
   const DispatchRequest::SharedPtr msg)
 {