Implement Ingestors

rmf_fleet_adapter/CMakeLists.txt

@@ -30,6 +30,7 @@ set(dep_pkgs
   rclcpp
   rmf_utils
   rmf_dispenser_msgs
+  rmf_ingestor_msgs
   rmf_door_msgs
   rmf_fleet_msgs
   rmf_lift_msgs
@@ -65,6 +66,8 @@ target_link_libraries(rmf_fleet_adapter
     ${rmf_task_msgs_LIBRARIES}
     ${rmf_door_msgs_LIBRARIES}
     ${rmf_lift_msgs_LIBRARIES}
+    ${rmf_dispenser_msgs_LIBRARIES}
+    ${rmf_ingestor_msgs_LIBRARIES}
 )
 
 target_include_directories(rmf_fleet_adapter
@@ -78,6 +81,8 @@ target_include_directories(rmf_fleet_adapter
     ${rmf_task_msgs_INCLUDE_DIRS}
     ${rmf_door_msgs_INCLUDE_DIRS}
     ${rmf_lift_msgs_INCLUDE_DIRS}
+    ${rmf_dispenser_msgs_INCLUDE_DIRS}
+    ${rmf_ingestor_msgs_INCLUDE_DIRS}
 )
 
 if (BUILD_TESTING)
@@ -93,6 +98,7 @@ if (BUILD_TESTING)
       test/phases/DoorCloseTest.cpp
       test/phases/RequestLiftTest.cpp
       test/phases/DispenseItemTest.cpp
+      test/phases/IngestItemTest.cpp
       test/phases/test_GoToPlace.cpp
       test/services/test_FindEmergencyPullover.cpp
       test/services/test_FindPath.cpp
@@ -108,7 +114,8 @@ if (BUILD_TESTING)
       ${rmf_task_msgs_INCLUDE_DIRS}
       ${rmf_door_msgs_INCLUDE_DIRS}
       ${rmf_lift_msgs_INCLUDE_DIRS}
-
+      ${rmf_dispenser_msgs_INCLUDE_DIRS}
+      ${rmf_ingestor_msgs_INCLUDE_DIRS}
       ${std_msgs_INCLUDE_DIRS}
   )
   target_link_libraries(rmf_fleet_adapter_test
@@ -118,7 +125,8 @@ if (BUILD_TESTING)
       ${rmf_task_msgs_LIBRARIES}
       ${rmf_door_msgs_LIBRARIES}
       ${rmf_lift_msgs_LIBRARIES}
-
+      ${rmf_dispenser_msgs_LIBRARIES}
+      ${rmf_ingestor_msgs_LIBRARIES}
       rmf_fleet_adapter
       rmf_utils::rmf_utils
       ${std_msgs_LIBRARIES}
@@ -154,6 +162,7 @@ target_link_libraries(full_control
     rmf_fleet_adapter
     ${rmf_task_msgs_LIBRARIES}
     ${rmf_dispenser_msgs_LIBRARIES}
+    ${rmf_ingestor_msgs_LIBRARIES}
     ${rmf_door_msgs_LIBRARIES}
     ${rmf_lift_msgs_LIBRARIES}
     ${std_msgs_LIBRARIES}
@@ -163,6 +172,7 @@ target_include_directories(full_control
   PRIVATE
     ${rmf_task_msgs_INCLUDE_DIRS}
     ${rmf_dispenser_msgs_INCLUDE_DIRS}
+    ${rmf_ingestor_msgs_INCLUDE_DIRS}
     ${rmf_door_msgs_INCLUDE_DIRS}
     ${rmf_lift_msgs_INCLUDE_DIRS}
     ${std_msgs_INCLUDE_DIRS}

rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp

@@ -41,6 +41,10 @@ const std::string DispenserRequestTopicName = "dispenser_requests";
 const std::string DispenserResultTopicName = "dispenser_results";
 const std::string DispenserStateTopicName = "dispenser_states";
 
+const std::string IngestorRequestTopicName = "ingestor_requests";
+const std::string IngestorResultTopicName = "ingestor_results";
+const std::string IngestorStateTopicName = "ingestor_states";
+
 const std::string DeliveryTopicName = "delivery_requests";
 const std::string LoopRequestTopicName = "loop_requests";
 const std::string TaskSummaryTopicName = "task_summaries";

rmf_fleet_adapter/package.xml

@@ -13,6 +13,7 @@
   <depend>rclcpp</depend>
   <depend>rmf_utils</depend>
   <depend>rmf_door_msgs</depend>
+  <depend>rmf_ingestor_msgs</depend>
   <depend>rmf_dispenser_msgs</depend>
   <depend>rmf_fleet_msgs</depend>
   <depend>rmf_lift_msgs</depend>

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp

@@ -53,6 +53,12 @@ std::shared_ptr<Node> Node::make(
         DispenserStateTopicName, default_qos);
   node->_emergency_notice_obs = node->create_observable<EmergencyNotice>(
         rmf_traffic_ros2::EmergencyTopicName, default_qos);
+  node->_ingestor_request_pub = node->create_publisher<IngestorRequest>(
+        IngestorRequestTopicName, default_qos);
+  node->_ingestor_result_obs = node->create_observable<IngestorResult>(
+        IngestorResultTopicName, default_qos);
+  node->_ingestor_state_obs = node->create_observable<IngestorState>(
+        IngestorStateTopicName, default_qos);
 
   return node;
 }
@@ -127,5 +133,23 @@ auto Node::emergency_notice() const -> const EmergencyNoticeObs&
   return _emergency_notice_obs;
 }
 
+auto Node::ingestor_request() const -> const IngestorRequestPub&
+{
+  return _ingestor_request_pub;
+}
+
+//==============================================================================
+auto Node::ingestor_result() const -> const IngestorResultObs&
+{
+  return _ingestor_result_obs;
+}
+
+//==============================================================================
+auto Node::ingestor_state() const -> const IngestorStateObs&
+{
+  return _ingestor_state_obs;
+}
+
+
 } // namespace agv
 } // namespace rmf_fleet_adapter

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp

@@ -23,6 +23,9 @@
 #include <rmf_dispenser_msgs/msg/dispenser_request.hpp>
 #include <rmf_dispenser_msgs/msg/dispenser_result.hpp>
 #include <rmf_dispenser_msgs/msg/dispenser_state.hpp>
+#include <rmf_ingestor_msgs/msg/ingestor_state.hpp>
+#include <rmf_ingestor_msgs/msg/ingestor_result.hpp>
+#include <rmf_ingestor_msgs/msg/ingestor_request.hpp>
 #include <rmf_door_msgs/msg/door_state.hpp>
 #include <rmf_door_msgs/msg/door_request.hpp>
 #include <rmf_door_msgs/msg/supervisor_heartbeat.hpp>
@@ -84,6 +87,18 @@ class Node : public rmf_rxcpp::Transport
   using EmergencyNoticeObs = rxcpp::observable<EmergencyNotice::SharedPtr>;
   const EmergencyNoticeObs& emergency_notice() const;
 
+  using IngestorRequest = rmf_ingestor_msgs::msg::IngestorRequest;
+  using IngestorRequestPub = rclcpp::Publisher<IngestorRequest>::SharedPtr;
+  const IngestorRequestPub& ingestor_request() const;
+
+  using IngestorResult = rmf_ingestor_msgs::msg::IngestorResult;
+  using IngestorResultObs = rxcpp::observable<IngestorResult::SharedPtr>;
+  const IngestorResultObs& ingestor_result() const;
+
+  using IngestorState = rmf_ingestor_msgs::msg::IngestorState;
+  using IngestorStateObs = rxcpp::observable<IngestorState::SharedPtr>;
+  const IngestorStateObs& ingestor_state() const;
+
 private:
 
   Node(
@@ -101,6 +116,9 @@ class Node : public rmf_rxcpp::Transport
   DispenserResultObs _dispenser_result_obs;
   DispenserStateObs _dispenser_state_obs;
   EmergencyNoticeObs _emergency_notice_obs;
+  IngestorRequestPub _ingestor_request_pub;
+  IngestorResultObs _ingestor_result_obs;
+  IngestorStateObs _ingestor_state_obs;
 };
 
 } // namespace agv