diff --git a/nexus_integration_tests/CMakeLists.txt b/nexus_integration_tests/CMakeLists.txt index e8c1518..04f3dd8 100644 --- a/nexus_integration_tests/CMakeLists.txt +++ b/nexus_integration_tests/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 - $ - $ - ${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 index 59c8c78..a301813 100644 --- 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 new file mode 100644 index 0000000..1bb1482 --- /dev/null +++ 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. +int32 ACTION_UNDEFINED=0 +int32 ACTION_TRANSIT=1 +int32 ACTION_PICKUP=2 +int32 ACTION_DROPOFF=3 + +# The type of action to perform at the destination. +int32 action + +# Additional parameters. +string params \ No newline at end of file diff --git a/nexus_msgs/nexus_transporter_msgs/msg/TransportationRequest.msg b/nexus_msgs/nexus_transporter_msgs/msg/TransportationRequest.msg index ee1f3a1..46a6655 100644 --- 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 index af3dc25..435fecd 100644 --- 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 + namespace nexus::system_orchestrator { BT::PortsList BidTransporter::providedPorts() @@ -50,7 +52,14 @@ BT::NodeStatus BidTransporter::onStart() std::make_shared(); 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() + .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 index 818a1e3..63400b4 100644 --- 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 +#include 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() + .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 index 19f5240..edda70e 100644 --- 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 + $ + $ + ${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 index f27f989..ce77621 100644 --- 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 + +#include + #include #include +#include + //============================================================================== 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 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& destinations() const; - /// Set the destination - Itinerary& destination(std::string destination); + /// Set the destinations + Itinerary& destinations(std::vector 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 index 33daf9f..5205615 100644 --- 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 + #include +#include #include #include @@ -39,6 +42,7 @@ class Transporter using TransportFeedback = std::function; /// A callback to execute once the transportation is completed using TransportCompleted = std::function; + /// 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 get_itinerary( const std::string& job_id, - const std::string& destination) = 0; + const std::vector& 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 index 8895393..1a55bbd 100644 --- a/nexus_transporter/package.xml +++ b/nexus_transporter/package.xml @@ -20,6 +20,9 @@ rclcpp_lifecycle rmf_utils tf2_ros + yaml-cpp + yaml_cpp_vendor + ament_cmake_catch2 ament_cmake_uncrustify diff --git a/nexus_transporter/src/TransporterNode.cpp b/nexus_transporter/src/TransporterNode.cpp index 6cc3074..cef732f 100644 --- a/nexus_transporter/src/TransporterNode.cpp +++ b/nexus_transporter/src/TransporterNode.cpp @@ -18,13 +18,25 @@ #include "TransporterNode.hpp" -#include +#include #include //============================================================================== namespace nexus_transporter { +//============================================================================== +TransporterNode::Data::Data() +: transporter_loader("nexus_transporter", "nexus_transporter::Transporter"), + transporter(nullptr), + availability_srv(nullptr), + action_srv(nullptr), + tf_broadcaster(nullptr), + ongoing_register(std::nullopt) +{ + // Do nothing. +} + //============================================================================== auto TransporterNode::on_configure(const State& /*previous_state*/) -> CallbackReturn @@ -54,19 +66,16 @@ auto TransporterNode::on_configure(const State& /*previous_state*/) RCLCPP_INFO( node->get_logger(), - "Received IsTransporterAvailable request from %s with id %s. " - "Destination: %s. Payload: [%s]", + "Received IsTransporterAvailable request from %s with id %s. ", request->request.requester.c_str(), - request->request.id.c_str(), - request->request.destination.c_str(), - request->request.payload.c_str() + request->request.id.c_str() ); - if (request->request.destination.empty()) + if (request->request.destinations.empty()) { RCLCPP_WARN( node->get_logger(), - "Ignoring request as destination is empty." + "Ignoring request as destinations is empty." ); return; } @@ -92,14 +101,13 @@ auto TransporterNode::on_configure(const State& /*previous_state*/) auto itinerary = data->transporter->get_itinerary( request->request.id, - request->request.destination); + request->request.destinations); if (!itinerary.has_value()) { RCLCPP_WARN( node->get_logger(), - "The transporter is not configured to go to destination [%s]", - request->request.destination.c_str() + "The transporter is not configured to go to destinations." ); return; @@ -109,10 +117,7 @@ auto TransporterNode::on_configure(const State& /*previous_state*/) response->available = true; response->transporter = itinerary->transporter_name(); response->estimated_finish_time = itinerary->estimated_finish_time(); - }, - rclcpp::ServicesQoS(), - _data->cb_group - ); + }); // Load transporter plugin if (_data->transporter_plugin_name.empty()) @@ -158,14 +163,15 @@ auto TransporterNode::on_configure(const State& /*previous_state*/) return CallbackReturn::FAILURE; } + // Create the client for registration. + this->_data->register_client = this->create_client( + RegisterTransporterService::service_name()); + //Timer for registering with the system orchestrator - this->_register_timer = this->create_wall_timer(std::chrono::seconds{1}, + this->_data->register_timer = this->create_wall_timer(std::chrono::seconds{1}, [this]() { - if (this->registration_callback()) - { - this->_register_timer.reset(); - } + this->_data->_register(); }); // Setup Transport action server @@ -187,12 +193,9 @@ auto TransporterNode::on_configure(const State& /*previous_state*/) { RCLCPP_INFO( node->get_logger(), - "Received transport goal request from [%s] with id [%s] for destination " - "%s and payload [%s]", + "Received transport goal request from [%s] with id [%s].", goal->request.requester.c_str(), - goal->request.id.c_str(), - goal->request.destination.c_str(), - goal->request.payload.c_str() + goal->request.id.c_str() ); } @@ -212,16 +215,15 @@ auto TransporterNode::on_configure(const State& /*previous_state*/) auto itinerary = data->transporter->get_itinerary( goal->request.id, - goal->request.destination); + goal->request.destinations); if (!itinerary.has_value()) { if (node) { RCLCPP_ERROR( node->get_logger(), - "Unable to generate an itinerary for destination [%s]. " - "Rejecting goal...", - goal->request.destination.c_str() + "Unable to generate an itinerary for destinations. " + "Rejecting goal..." ); } return rclcpp_action::GoalResponse::REJECT; @@ -271,9 +273,8 @@ auto TransporterNode::on_configure(const State& /*previous_state*/) { RCLCPP_INFO( node->get_logger(), - "Successfully cancelled transport with id [%s] to destination [%s]", - it->second->id().c_str(), - it->second->destination().c_str() + "Successfully cancelled transport with id [%s].", + it->second->id().c_str() ); } return rclcpp_action::CancelResponse::ACCEPT; @@ -284,9 +285,8 @@ auto TransporterNode::on_configure(const State& /*previous_state*/) { RCLCPP_INFO( node->get_logger(), - "Unable to cancel transport with id [%s] to destination [%s]", - it->second->id().c_str(), - it->second->destination().c_str() + "Unable to cancel transport with id [%s]", + it->second->id().c_str() ); } return rclcpp_action::CancelResponse::REJECT; @@ -366,10 +366,7 @@ auto TransporterNode::on_configure(const State& /*previous_state*/) } }); - }, - rcl_action_server_get_default_options(), - _data->cb_group - ); + }); RCLCPP_INFO(this->get_logger(), "Successfully configured."); return CallbackReturn::SUCCESS; @@ -380,7 +377,7 @@ auto TransporterNode::on_cleanup(const State& /*previous_state*/) -> CallbackReturn { RCLCPP_INFO(this->get_logger(), "Cleaning up..."); - this->_register_timer.reset(); + _data->register_timer.reset(); _data->transporter.reset(); _data->availability_srv.reset(); _data->action_srv.reset(); @@ -437,9 +434,6 @@ TransporterNode::TransporterNode(const rclcpp::NodeOptions& options) _data = std::make_shared(); - _data->cb_group = this->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - _data->transporter_plugin_name = this->declare_parameter( "transporter_plugin", std::string("")); @@ -459,44 +453,72 @@ TransporterNode::TransporterNode(const rclcpp::NodeOptions& options) ); } -bool TransporterNode::registration_callback() +void TransporterNode::Data::_register() { - RCLCPP_INFO(this->get_logger(), "Registering with system orchestrator..."); - auto client = this->create_client( - RegisterTransporterService::service_name(), - rclcpp::ServicesQoS(), - _data->cb_group - ); - - if (!client->wait_for_service(_data->connection_timeout)) + auto node = w_node.lock(); + if (node == nullptr) { - RCLCPP_ERROR(this->get_logger(), "Could not find system orchestrator!"); - return false; + return; } - auto req = std::make_shared(); - req->description.workcell_id = this->get_name(); - auto fut = client->async_send_request(req); - - if (fut.wait_for(_data->connection_timeout) != std::future_status::ready) + if (this->ongoing_register) { RCLCPP_ERROR( - this->get_logger(), "Could not register with system orchestrator"); - return false; + node->get_logger(), + "Failed to register: No response from system orchestrator."); + if (!this->register_client->remove_pending_request(*this-> + ongoing_register)) + { + RCLCPP_WARN(node->get_logger(), + "Unable to remove pending request during transporter registration."); + } } - auto resp = fut.get(); - if (!resp->success) + + RCLCPP_INFO(node->get_logger(), "Registering with system orchestrator..."); + auto register_cb = + [this, node](rclcpp::Client::SharedFuture future) + { + this->ongoing_register = std::nullopt; + auto resp = future.get(); + if (!resp->success) + { + switch (resp->error_code) + { + case RegisterTransporter::Response::ERROR_NOT_READY: + RCLCPP_ERROR( + node->get_logger(), + "Error while registering with system orchestrator, retrying again... [%s]", + resp->message.c_str()); + break; + default: + RCLCPP_FATAL(node->get_logger(), + "Failed to register with system orchestrator! [%s]", + resp->message.c_str()); + throw std::runtime_error(resp->message); + } + return; + } + RCLCPP_INFO(node->get_logger(), + "Successfully registered with system orchestrator"); + this->register_timer->cancel(); + // TODO(luca) reintroduce once https://github.com/ros2/rclcpp/issues/2652 is fixed and released + // this->register_timer.reset(); + }; + + if (!this->register_client->wait_for_service(connection_timeout)) { - RCLCPP_ERROR( - this->get_logger(), "Failed to register with system orchestrator (%s)", - resp->message.c_str()); - return false; + RCLCPP_ERROR(node->get_logger(), "Could not find system orchestrator!"); + // timer is not cancelled so it will run again. + return; } - RCLCPP_INFO(this->get_logger(), - "Successfully registered with system orchestrator"); - return true; + + auto req = std::make_shared(); + req->description.workcell_id = node->get_name(); + this->ongoing_register = + this->register_client->async_send_request(req, register_cb); } } // namespace nexus_transporter +#include RCLCPP_COMPONENTS_REGISTER_NODE(nexus_transporter::TransporterNode) diff --git a/nexus_transporter/src/TransporterNode.hpp b/nexus_transporter/src/TransporterNode.hpp index aef56f5..76744ac 100644 --- a/nexus_transporter/src/TransporterNode.hpp +++ b/nexus_transporter/src/TransporterNode.hpp @@ -31,6 +31,7 @@ #include #include +#include //============================================================================== namespace nexus_transporter { @@ -53,9 +54,6 @@ class TransporterNode : public rclcpp_lifecycle::LifecycleNode /// Constructor TransporterNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); - /// Register the transporter to the system orchestrator. - bool registration_callback(); - /// rclcpp_lifecycle::LifecycleNode functions to override CallbackReturn on_configure(const State& previous_state) override; CallbackReturn on_cleanup(const State& previous_state) override; @@ -65,12 +63,16 @@ class TransporterNode : public rclcpp_lifecycle::LifecycleNode CallbackReturn on_error(const State& previous_state) override; private: - // Bundling all member variables into a data struct and capturing a shared_ptr // of this struct within lambdas of various callbacks will guarantee thread // safety over capturing "this" raw ptr by reference. + // TODO(Yadunund): Add an internal node and get rid of dependency on + // multithreaded executor. struct Data { + /// Register the transporter to the system orchestrator. + void _register(); + /// pluginlib clasloader to dynamically load the transporter plugin. pluginlib::ClassLoader transporter_loader; /// The loaded transporter plugin. @@ -88,10 +90,6 @@ class TransporterNode : public rclcpp_lifecycle::LifecycleNode rclcpp_action::Server::SharedPtr action_srv; - /// A separate callback group to prevent deadlock during state transitions - /// and other callbacks that need to execute. - rclcpp::CallbackGroup::SharedPtr cb_group; - // Map itinerary id to GoalData. std::unordered_map> itineraries; @@ -102,20 +100,22 @@ class TransporterNode : public rclcpp_lifecycle::LifecycleNode // Weakptr to lifecycle node. std::weak_ptr w_node; - Data() - : transporter_loader("nexus_transporter", "nexus_transporter::Transporter"), - transporter(nullptr), - availability_srv(nullptr), - action_srv(nullptr), - tf_broadcaster(nullptr) - { - // Do nothing. - } + // Client for registration. + rclcpp::Client::SharedPtr register_client; + + // Asynchronously monitor whether the transport has registered with the + // system orchestrator. + std::optional:: + SharedFutureAndRequestId> ongoing_register; + + rclcpp::TimerBase::SharedPtr register_timer; + + + Data(); }; std::shared_ptr _data; - rclcpp::TimerBase::SharedPtr _register_timer; }; } // namespace nexus_transporter diff --git a/nexus_transporter/src/nexus_transporter/Itinerary.cpp b/nexus_transporter/src/nexus_transporter/Itinerary.cpp index 1a2a56d..56e418b 100644 --- a/nexus_transporter/src/nexus_transporter/Itinerary.cpp +++ b/nexus_transporter/src/nexus_transporter/Itinerary.cpp @@ -17,16 +17,16 @@ #include -//============================================================================== -namespace nexus_transporter { +#include +namespace nexus_transporter { //============================================================================== class Itinerary::Implementation { public: // Documentation same as constructor input params std::string id; - std::string destination; + std::vector destinations; std::string transporter_name; rclcpp::Time finish_time; rclcpp::Time expiration_time; @@ -46,15 +46,15 @@ Itinerary& Itinerary::id(std::string id) } //============================================================================== -const std::string& Itinerary::destination() const +const std::vector& Itinerary::destinations() const { - return _pimpl->destination; + return _pimpl->destinations; } //============================================================================== -Itinerary& Itinerary::destination(std::string destination) +Itinerary& Itinerary::destinations(std::vector destinations_) { - _pimpl->destination = std::move(destination); + _pimpl->destinations = std::move(destinations_); return *this; } @@ -100,14 +100,14 @@ Itinerary& Itinerary::expiration_time(rclcpp::Time time) //============================================================================== Itinerary::Itinerary( std::string id, - std::string destination, + std::vector destinations, std::string transporter_name, rclcpp::Time estimated_finish_time, rclcpp::Time expiration_time) : _pimpl(rmf_utils::make_impl( Implementation{ std::move(id), - std::move(destination), + std::move(destinations), std::move(transporter_name), std::move(estimated_finish_time), std::move(expiration_time) diff --git a/nexus_integration_tests/src/mock_transporter.cpp b/nexus_transporter/src/transporters/mock_transporter.cpp similarity index 89% rename from nexus_integration_tests/src/mock_transporter.cpp rename to nexus_transporter/src/transporters/mock_transporter.cpp index 512e507..7972554 100644 --- a/nexus_integration_tests/src/mock_transporter.cpp +++ b/nexus_transporter/src/transporters/mock_transporter.cpp @@ -23,6 +23,7 @@ namespace nexus_transporter { +//============================================================================== struct Location { double pose; @@ -36,6 +37,7 @@ struct Location {} }; +//============================================================================== struct MockTransporter3000 { std::string name; @@ -52,6 +54,7 @@ struct MockTransporter3000 {} }; +//============================================================================== // MockTransporter that can only process once request at a time. class MockTransporter : public Transporter { @@ -135,9 +138,15 @@ class MockTransporter : public Transporter std::optional get_itinerary( const std::string& id, - const std::string& destination) + const std::vector& destinations) { - if (_destinations.find(destination) == _destinations.end()) + if (destinations.empty()) + { + return std::nullopt; + } + // This transporter can only go to one destination at a time. + const auto & destination = destinations[0]; + if (_destinations.find(destination.name) == _destinations.end()) return std::nullopt; auto n = _node.lock(); @@ -156,11 +165,11 @@ class MockTransporter : public Transporter } const auto& transporter_location = it.first->second->current_location; - const auto& dest_pose = _destinations.find(destination)->second; + const auto& dest_pose = _destinations.find(destination.name)->second; double travel_duration; if (transporter_location.name.has_value() && - destination == transporter_location.name) + destination.name == transporter_location.name) { travel_duration = 0.0; } @@ -171,7 +180,7 @@ class MockTransporter : public Transporter return Itinerary{ id, - destination, + destinations, transporter_name, now + rclcpp::Duration::from_seconds(travel_duration), now + rclcpp::Duration::from_seconds(60.0) @@ -203,8 +212,15 @@ class MockTransporter : public Transporter } } + const auto & destinations = itinerary.destinations(); + if (destinations.empty()) + { + completed_cb(true); + return; + } + const auto & destination = destinations[0]; if (current_location.name.has_value() && - itinerary.destination() == current_location.name) + destination.name == current_location.name) { completed_cb(true); return; @@ -215,7 +231,7 @@ class MockTransporter : public Transporter RCLCPP_INFO(n->get_logger(), "Received request for transporter [%s] to %s", - current_transporter.c_str(), itinerary.destination().c_str()); + current_transporter.c_str(), destination.name.c_str()); // TODO(YV): Capture a data_ptr to avoid reference captures _thread = std::thread( @@ -228,9 +244,9 @@ class MockTransporter : public Transporter _transporters.at(current_transporter)->itinerary = itinerary; } - + const auto & destination = itinerary.destinations()[0]; const auto& dest_pose = - _destinations.find(itinerary.destination())->second; + _destinations.find(destination.name)->second; const auto& current_pose = _transporters.at( current_transporter)->current_location.pose; const auto& dist = abs(dest_pose - current_pose); @@ -278,7 +294,7 @@ class MockTransporter : public Transporter std::lock_guard lock(_mutex); // Check if the transporter is at the unloading station - if (itinerary.destination() == _unloading_station) + if (destination.name == _unloading_station) { // Work order completed, ok to remove transporter from list _transporters.erase( diff --git a/nexus_integration_tests/mock_plugins.xml b/nexus_transporter/src/transporters/plugins.xml similarity index 100% rename from nexus_integration_tests/mock_plugins.xml rename to nexus_transporter/src/transporters/plugins.xml diff --git a/nexus_transporter/test/test_Itinerary.cpp b/nexus_transporter/test/test_Itinerary.cpp index 8ca2793..4ede31a 100644 --- a/nexus_transporter/test/test_Itinerary.cpp +++ b/nexus_transporter/test/test_Itinerary.cpp @@ -17,13 +17,22 @@ #include +#include + #include +using Destination = nexus_transporter::Destination; //============================================================================== SCENARIO("Test Itinerary") { const std::string id = "A123"; - const std::string destination = "workcell_1"; + std::vector destinations; + destinations.emplace_back( + nexus_transporter_msgs::build() + .name("workcell_1") + .action(Destination::ACTION_PICKUP) + .params("") + ); const std::string transporter_name = "pallet_1"; const auto& now = rclcpp::Clock().now(); const rclcpp::Time finish_time = @@ -32,14 +41,18 @@ SCENARIO("Test Itinerary") now + rclcpp::Duration::from_seconds(5.0); auto itinerary = nexus_transporter::Itinerary( id, - destination, + std::move(destinations), transporter_name, finish_time, expiry_time ); CHECK(itinerary.id() == id); - CHECK(itinerary.destination() == destination); + const auto & destinations_ = itinerary.destinations(); + CHECK(destinations_.size() == 1); + CHECK(destinations_[0].name == "pallet_1"); + CHECK(destinations_[0].action == Destination::ACTION_PICKUP); + CHECK(destinations_[0].params == ""); CHECK(itinerary.transporter_name() == transporter_name); CHECK(itinerary.estimated_finish_time() == finish_time); CHECK(itinerary.expiration_time() == expiry_time); @@ -52,9 +65,19 @@ SCENARIO("Test Itinerary") } WHEN("Setting new destination") { - const std::string new_destination = "workcell_2"; - itinerary.destination(new_destination); - CHECK(itinerary.destination() == new_destination); + std::vector new_destinations; + new_destinations.emplace_back( + nexus_transporter_msgs::build() + .name("workcell_2") + .action(Destination::ACTION_DROPOFF) + .params("") + ); + itinerary.destinations(std::move(new_destinations)); + const auto & new_destinations_ = itinerary.destinations(); + CHECK(new_destinations_.size() == 1); + CHECK(new_destinations_[0].name == "pallet_1"); + CHECK(new_destinations_[0].action == Destination::ACTION_PICKUP); + CHECK(new_destinations_[0].params == ""); } WHEN("Setting new transporter_name") {