Support multiple destinations with actions in nexus_transporter (#69)

* Update Transporter API to support multiple destinations with actions Signed-off-by: Yadunund <[email protected]> * Update mock transporter Signed-off-by: Yadunund <[email protected]> * Update so impl Signed-off-by: Yadunund <[email protected]> * Move mock_transporters into nexus_transporter Signed-off-by: Yadunund <[email protected]> * Swtich to single threaded executor for transporter node Signed-off-by: Yadunund <[email protected]> * Address feedback Signed-off-by: Yadunund <[email protected]> --------- Signed-off-by: Yadunund <[email protected]>
osrf · Feb 19, 2025 · 59a59e0 · 59a59e0
1 parent ea1bf90
commit 59a59e0
Show file tree

Hide file tree

Showing 16 changed files with 263 additions and 150 deletions.
diff --git a/nexus_demos/CMakeLists.txt b/nexus_demos/CMakeLists.txt
@@ -63,22 +63,6 @@ rclcpp_components_register_node(mock_emergency_alarm_component
 install(TARGETS mock_emergency_alarm_component RUNTIME DESTINATION lib/${PROJECT_NAME})
 
 ###############################################################################
-
-add_library(mock_transporter SHARED src/mock_transporter.cpp)
-target_include_directories(mock_transporter PUBLIC
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>
-  ${pluginlub_INCLUDE_DIRS}
-)
-target_link_libraries(mock_transporter
-  PUBLIC
-    nexus_transporter::nexus_transporter
-    pluginlib::pluginlib
-)
-
-pluginlib_export_plugin_description_file(nexus_transporter mock_plugins.xml)
-
-ament_export_libraries(mock_transporter)
 ament_export_targets(export_${PROJECT_NAME})
 
 ###############################################################################
@@ -122,7 +106,6 @@ install(DIRECTORY ${zenoh_cfg_output_dir} DESTINATION share/${PROJECT_NAME}/conf
 
 install(
   TARGETS
-    mock_transporter
     mock_printer
     EXPORT export_${PROJECT_NAME}
   ARCHIVE DESTINATION lib

diff --git a/nexus_msgs/nexus_transporter_msgs/CMakeLists.txt b/nexus_msgs/nexus_transporter_msgs/CMakeLists.txt
@@ -15,6 +15,7 @@ find_package(geometry_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
 
 set(msg_files
+  "msg/Destination.msg"
   "msg/TransportationRequest.msg"
   "msg/TransporterState.msg"
 )

diff --git a/nexus_msgs/nexus_transporter_msgs/msg/Destination.msg b/nexus_msgs/nexus_transporter_msgs/msg/Destination.msg
@@ -0,0 +1,14 @@
+# Name of the destination
+string name
+
+# Action constants for the action field.
+uint8 ACTION_UNDEFINED=0
+uint8 ACTION_TRANSIT=1
+uint8 ACTION_PICKUP=2
+uint8 ACTION_DROPOFF=3
+
+# The type of action to perform at the destination.
+uint8 action
+
+# Additional parameters.
+string params
diff --git a/nexus_msgs/nexus_transporter_msgs/msg/TransportationRequest.msg b/nexus_msgs/nexus_transporter_msgs/msg/TransportationRequest.msg
@@ -6,9 +6,5 @@ string requester
 # from the previous request needs to move to the new destination.
 string id
 
-# The name of the destination
-string destination
-
-# The items that need to be transported to the destination.
-# A json schema may be defined in the future for the contents of this field.
-string payload
+# The destinations for this itinerary.
+Destination[] destinations
diff --git a/nexus_system_orchestrator/src/bid_transporter.cpp b/nexus_system_orchestrator/src/bid_transporter.cpp
@@ -17,6 +17,8 @@
 
 #include "bid_transporter.hpp"
 
+#include <nexus_transporter_msgs/msg/destination.hpp>
+
 namespace nexus::system_orchestrator {
 
 BT::PortsList BidTransporter::providedPorts()
@@ -50,7 +52,14 @@ BT::NodeStatus BidTransporter::onStart()
     std::make_shared<endpoints::IsTransporterAvailableService::ServiceType::Request>();
   req->request.id = this->_ctx->job_id;
   req->request.requester = node->get_name();
-  req->request.destination = destination;
+  // TODO(Yadunund): Parse work order and assign action type and params.
+  // See https://github.com/osrf/nexus/issues/68.
+  req->request.destinations.emplace_back(
+    nexus_transporter_msgs::build<nexus_transporter_msgs::msg::Destination>()
+      .name(destination)
+      .action(nexus_transporter_msgs::msg::Destination::ACTION_PICKUP)
+      .params("")
+  );
   // send request to all transporters in parallel
   for (auto& [transporter_id, session] : this->_ctx->transporter_sessions)
   {

diff --git a/nexus_system_orchestrator/src/transporter_request.cpp b/nexus_system_orchestrator/src/transporter_request.cpp
@@ -18,6 +18,7 @@
 #include "transporter_request.hpp"
 
 #include <nexus_orchestrator_msgs/msg/workcell_task.hpp>
+#include <nexus_transporter_msgs/msg/destination.hpp>
 
 namespace nexus::system_orchestrator {
 
@@ -67,7 +68,14 @@ make_goal()
   endpoints::TransportAction::ActionType::Goal goal;
   goal.request.id = this->_ctx->job_id;
   goal.request.requester = this->_node->get_name();
-  goal.request.destination = this->_destination;
+  // TODO(Yadunund): Parse work order and assign action type and params.
+  // See https://github.com/osrf/nexus/issues/68.
+  goal.request.destinations.emplace_back(
+    nexus_transporter_msgs::build<nexus_transporter_msgs::msg::Destination>()
+      .name(this->_destination)
+      .action(nexus_transporter_msgs::msg::Destination::ACTION_PICKUP)
+      .params("")
+  );
   return goal;
 }
 

diff --git a/nexus_transporter/CMakeLists.txt b/nexus_transporter/CMakeLists.txt
@@ -20,6 +20,8 @@ set(dep_pkgs
   rclcpp_lifecycle
   rmf_utils
   tf2_ros
+  yaml_cpp_vendor
+  yaml-cpp
 )
 foreach(pkg ${dep_pkgs})
   find_package(${pkg} REQUIRED)
@@ -37,6 +39,7 @@ target_link_libraries(nexus_transporter
     ${rclcpp_action_TARGETS}
     ${rclcpp_lifecycle_TARGETS}
     rmf_utils::rmf_utils
+    yaml-cpp::yaml-cpp
 )
 
 target_include_directories(nexus_transporter
@@ -68,6 +71,7 @@ target_link_libraries(nexus_transporter_component
     ${rclcpp_components_TARGETS}
     ${rclcpp_lifecycle_TARGETS}
     ${tf2_ros_TARGETS}
+    yaml-cpp::yaml-cpp
 )
 
 target_include_directories(nexus_transporter_component
@@ -89,7 +93,26 @@ target_compile_features(nexus_transporter_component INTERFACE cxx_std_17)
 rclcpp_components_register_node(nexus_transporter_component
   PLUGIN "nexus_transporter::TransporterNode"
   EXECUTABLE nexus_transporter_node
-  EXECUTOR MultiThreadedExecutor)
+  EXECUTOR SingleThreadedExecutor)
+
+
+#===============================================================================
+# Mock transporter plugin.
+add_library(mock_transporter SHARED src/transporters/mock_transporter.cpp)
+target_include_directories(mock_transporter PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+  ${pluginlub_INCLUDE_DIRS}
+)
+target_link_libraries(mock_transporter
+  PUBLIC
+    nexus_transporter
+    pluginlib::pluginlib
+)
+
+pluginlib_export_plugin_description_file(nexus_transporter src/transporters/plugins.xml)
+
+ament_export_libraries(mock_transporter)
 
 #===============================================================================
 find_package(ament_cmake_uncrustify QUIET)
@@ -130,6 +153,7 @@ install(
 
 install(
   TARGETS
+    mock_transporter
     nexus_transporter
     nexus_transporter_component
   EXPORT nexus_transporter

diff --git a/nexus_transporter/include/nexus_transporter/Itinerary.hpp b/nexus_transporter/include/nexus_transporter/Itinerary.hpp
@@ -18,13 +18,21 @@
 #ifndef NEXUS_TRANSPORTER__ITINERARY_HPP
 #define NEXUS_TRANSPORTER__ITINERARY_HPP
 
+#include <vector>
+
+#include <nexus_transporter_msgs/msg/destination.hpp>
+
 #include <rclcpp/rclcpp.hpp>
 
 #include <rmf_utils/impl_ptr.hpp>
 
+#include <yaml-cpp/yaml.h>
+
 //==============================================================================
 namespace nexus_transporter {
 
+using Destination = nexus_transporter_msgs::msg::Destination;
+
 /// An itinerary that is generated by the Transporter.
 class Itinerary
 {
@@ -34,8 +42,8 @@ class Itinerary
   /// \param[in] id
   ///   A unique id for this itinerary
   ///
-  /// \param[in] destination
-  ///   The destination for this itinerary
+  /// \param[in] destinations
+  ///   The destinations for this itinerary
   ///
   /// \param[in] transporter_name
   ///   The name of the transporter that is assigned to this itinerary
@@ -47,7 +55,7 @@ class Itinerary
   ///   The time until when this itinerary is valid
   Itinerary(
     std::string id,
-    std::string destination,
+    std::vector<Destination> destinations,
     std::string transporter_name,
     rclcpp::Time estimated_finish_time,
     rclcpp::Time expiration_time
@@ -59,11 +67,11 @@ class Itinerary
   /// Set the id
   Itinerary& id(std::string id);
 
-  /// Get the destination
-  const std::string& destination() const;
+  /// Get the destinations
+  const std::vector<Destination>& destinations() const;
 
-  /// Set the destination
-  Itinerary& destination(std::string destination);
+  /// Set the destinations
+  Itinerary& destinations(std::vector<Destination> destination);
 
   /// Get the name of the transporter
   const std::string& transporter_name() const;

diff --git a/nexus_transporter/include/nexus_transporter/Transporter.hpp b/nexus_transporter/include/nexus_transporter/Transporter.hpp
@@ -18,8 +18,11 @@
 #ifndef NEXUS_TRANSPORTER__TRANSPORTER_HPP
 #define NEXUS_TRANSPORTER__TRANSPORTER_HPP
 
+#include <vector>
+
 #include <rclcpp_lifecycle/lifecycle_node.hpp>
 
+#include <nexus_transporter_msgs/msg/destination.hpp>
 #include <nexus_transporter_msgs/msg/transporter_state.hpp>
 
 #include <nexus_transporter/Itinerary.hpp>
@@ -39,6 +42,7 @@ class Transporter
   using TransportFeedback = std::function<void(const TransporterState& state)>;
   /// A callback to execute once the transportation is completed
   using TransportCompleted = std::function<void(bool success)>;
+  /// Destination.
 
   /// Configure the transporter including getting relevant information from
   /// ROS 2 parameters. (Eg. List of destinations)
@@ -59,13 +63,13 @@ class Transporter
   /// \param[in] job_id
   /// An id for this request.
   ///
-  /// \param[in] destination
-  /// The name of the destination
-  /// \return A nullopt is returned if the destination is not valid.
+  /// \param[in] destinations
+  /// A list of destinations.
+  /// \return A nullopt is returned if the destinations is not valid.
   // TODO(YV): Consider creating a separate class for destination
   virtual std::optional<Itinerary> get_itinerary(
     const std::string& job_id,
-    const std::string& destination) = 0;
+    const std::vector<Destination>& destinations) = 0;
 
   /// Request the transporter to go to a destination. This call should be
   /// non-blocking.

diff --git a/nexus_transporter/package.xml b/nexus_transporter/package.xml
@@ -20,6 +20,9 @@
   <depend>rclcpp_lifecycle</depend>
   <depend>rmf_utils</depend>
   <depend>tf2_ros</depend>
+  <depend>yaml-cpp</depend>
+  <depend>yaml_cpp_vendor</depend>
+
 
   <test_depend>ament_cmake_catch2</test_depend>
   <test_depend>ament_cmake_uncrustify</test_depend>