From f2719c2579bf7c2614c8d6f7db58f62824903f3d Mon Sep 17 00:00:00 2001 From: youliang Date: Tue, 9 Feb 2021 17:13:55 +0800 Subject: [PATCH 01/10] refactor task evaluator, and rmf task ros2 interfaces --- rmf_fleet_adapter/CMakeLists.txt | 3 + .../rmf_fleet_adapter/StandardNames.hpp | 6 - .../agv/FleetUpdateHandle.cpp | 10 -- .../agv/internal_FleetUpdateHandle.hpp | 9 +- rmf_task/include/rmf_task/Evaluator.hpp | 84 +++++++++++++ rmf_task/src/rmf_task/Evaluator.cpp | 78 ++++++++++++ rmf_task/test/unit/test_Evaluator.cpp | 106 ++++++++++++++++ rmf_task_ros2/CMakeLists.txt | 8 +- .../include/rmf_task_ros2/Dispatcher.hpp | 36 +++--- .../include/rmf_task_ros2/StandardNames.hpp | 4 +- .../rmf_task_ros2/bidding/Auctioneer.hpp | 42 +------ .../rmf_task_ros2/bidding/MinimalBidder.hpp | 15 ++- .../rmf_task_ros2/bidding/Submission.hpp | 44 ------- rmf_task_ros2/src/dispatcher_node/main.cpp | 39 +++++- rmf_task_ros2/src/mock_bidder/main.cpp | 6 +- .../src/rmf_task_ros2/Dispatcher.cpp | 60 +++------ .../src/rmf_task_ros2/action/Client.cpp | 7 +- .../src/rmf_task_ros2/action/Server.cpp | 4 +- .../src/rmf_task_ros2/bidding/Auctioneer.cpp | 61 +-------- .../rmf_task_ros2/bidding/MinimalBidder.cpp | 30 ++--- .../bidding/internal_Auctioneer.hpp | 8 +- rmf_task_ros2/test/bidding/test_Evaluator.cpp | 116 ------------------ rmf_task_ros2/test/bidding/test_SelfBid.cpp | 8 +- .../test/dispatcher/test_Dispatcher.cpp | 6 +- 24 files changed, 413 insertions(+), 377 deletions(-) create mode 100644 rmf_task/include/rmf_task/Evaluator.hpp create mode 100644 rmf_task/src/rmf_task/Evaluator.cpp create mode 100644 rmf_task/test/unit/test_Evaluator.cpp delete mode 100644 rmf_task_ros2/include/rmf_task_ros2/bidding/Submission.hpp delete mode 100644 rmf_task_ros2/test/bidding/test_Evaluator.cpp diff --git a/rmf_fleet_adapter/CMakeLists.txt b/rmf_fleet_adapter/CMakeLists.txt index 74333057a..b2bbab1e6 100644 --- a/rmf_fleet_adapter/CMakeLists.txt +++ b/rmf_fleet_adapter/CMakeLists.txt @@ -39,6 +39,7 @@ set(dep_pkgs rmf_traffic_ros2 rmf_battery rmf_task + rmf_task_ros2 std_msgs ) foreach(pkg ${dep_pkgs}) @@ -62,6 +63,7 @@ target_link_libraries(rmf_fleet_adapter rmf_traffic_ros2::rmf_traffic_ros2 rmf_battery::rmf_battery rmf_task::rmf_task + rmf_task_ros2::rmf_task_ros2 yaml-cpp ${rmf_fleet_msgs_LIBRARIES} ${rclcpp_LIBRARIES} @@ -322,6 +324,7 @@ target_include_directories(task_aggregator ament_export_targets(rmf_fleet_adapter HAS_LIBRARY_TARGET) ament_export_dependencies( rmf_task + rmf_task_ros2 rmf_traffic_ros2 rmf_fleet_msgs rmf_door_msgs diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp index 29dc1a8b9..7f654a313 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp @@ -49,12 +49,6 @@ const std::string IngestorStateTopicName = "ingestor_states"; const std::string DeliveryTopicName = "delivery_requests"; const std::string LoopRequestTopicName = "loop_requests"; const std::string TaskSummaryTopicName = "task_summaries"; - -const std::string BidNoticeTopicName = "rmf_task/bid_notice"; -const std::string BidProposalTopicName = "rmf_task/bid_proposal"; -const std::string DispatchRequestTopicName = "rmf_task/dispatch_request"; -const std::string DispatchAckTopicName = "rmf_task/dispatch_ack"; - const std::string DockSummaryTopicName = "dock_summary"; } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index 5a109159a..a808d1f5c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -279,16 +279,6 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( return; } - if (delivery.dropoff_place_name.empty()) - { - RCLCPP_ERROR( - 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_ERROR( diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp index a0c74c4cd..b901c6f34 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp @@ -33,6 +33,7 @@ #include #include +#include #include "Node.hpp" #include "RobotContext.hpp" @@ -224,17 +225,17 @@ class FleetUpdateHandle::Implementation // Publish BidProposal handle._pimpl->bid_proposal_pub = handle._pimpl->node->create_publisher( - BidProposalTopicName, default_qos); + rmf_task_ros2::BidProposalTopicName, default_qos); // Publish DispatchAck handle._pimpl->dispatch_ack_pub = handle._pimpl->node->create_publisher( - DispatchAckTopicName, default_qos); + rmf_task_ros2::DispatchAckTopicName, default_qos); // Subscribe BidNotice handle._pimpl->bid_notice_sub = handle._pimpl->node->create_subscription( - BidNoticeTopicName, + rmf_task_ros2::BidNoticeTopicName, default_qos, [p = handle._pimpl.get()](const BidNotice::SharedPtr msg) { @@ -244,7 +245,7 @@ class FleetUpdateHandle::Implementation // Subscribe DispatchRequest handle._pimpl->dispatch_request_sub = handle._pimpl->node->create_subscription( - DispatchRequestTopicName, + rmf_task_ros2::DispatchRequestTopicName, default_qos, [p = handle._pimpl.get()](const DispatchRequest::SharedPtr msg) { diff --git a/rmf_task/include/rmf_task/Evaluator.hpp b/rmf_task/include/rmf_task/Evaluator.hpp new file mode 100644 index 000000000..e7bcbf34e --- /dev/null +++ b/rmf_task/include/rmf_task/Evaluator.hpp @@ -0,0 +1,84 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef RMF_TASK__EVALUATOR_HPP +#define RMF_TASK__EVALUATOR_HPP + +#include +#include +#include +#include + +namespace rmf_task { + +//============================================================================== +/// A pure abstract interface class for the auctioneer to choose the best +/// submission. +class Evaluator +{ +public: + + struct Submission + { + std::string fleet_name; + std::string robot_name; + double prev_cost = 0.0; + double new_cost = std::numeric_limits::max(); + rmf_traffic::Time finish_time; + }; + + using Submissions = std::vector; + + /// Given a list of submissions, choose the one that is the "best". It is + /// up to the implementation of the Evaluator to decide how to rank. + /// + /// \return + /// index of the best submission + virtual std::optional choose( + const Submissions& submissions) const = 0; + + virtual ~Evaluator() = default; +}; + +//============================================================================== +class LeastFleetDiffCostEvaluator : public Evaluator +{ +public: + std::optional choose( + const Submissions& submissions) const final; +}; + +//============================================================================== +class LeastFleetCostEvaluator : public Evaluator +{ +public: + std::optional choose( + const Submissions& submissions) const final; +}; + +//============================================================================== +class QuickestFinishEvaluator : public Evaluator +{ +public: + std::optional choose( + const Submissions& submissions) const final; +}; + + +} // namespace rmf_task + +#endif // RMF_TASK__EVALUATOR_HPP diff --git a/rmf_task/src/rmf_task/Evaluator.cpp b/rmf_task/src/rmf_task/Evaluator.cpp new file mode 100644 index 000000000..c9537cc62 --- /dev/null +++ b/rmf_task/src/rmf_task/Evaluator.cpp @@ -0,0 +1,78 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +namespace rmf_task { + +//============================================================================== +std::optional LeastFleetDiffCostEvaluator::choose( + const Submissions& submissions) const +{ + if (submissions.empty()) + return std::nullopt; + + auto winner_it = submissions.begin(); + float winner_cost_diff = winner_it->new_cost - winner_it->prev_cost; + for (auto nominee_it = ++submissions.begin(); + nominee_it != submissions.end(); ++nominee_it) + { + float nominee_cost_diff = nominee_it->new_cost - nominee_it->prev_cost; + if (nominee_cost_diff < winner_cost_diff) + { + winner_it = nominee_it; + winner_cost_diff = nominee_cost_diff; + } + } + return std::distance(submissions.begin(), winner_it); +} + +//============================================================================== +std::optional LeastFleetCostEvaluator::choose( + const Submissions& submissions) const +{ + if (submissions.empty()) + return std::nullopt; + + auto winner_it = submissions.begin(); + for (auto nominee_it = ++submissions.begin(); + nominee_it != submissions.end(); ++nominee_it) + { + if (nominee_it->new_cost < winner_it->new_cost) + winner_it = nominee_it; + } + return std::distance(submissions.begin(), winner_it); +} + +//============================================================================== +std::optional QuickestFinishEvaluator::choose( + const Submissions& submissions) const +{ + if (submissions.empty()) + return std::nullopt; + + auto winner_it = submissions.begin(); + for (auto nominee_it = ++submissions.begin(); + nominee_it != submissions.end(); ++nominee_it) + { + if (nominee_it->finish_time < winner_it->finish_time) + winner_it = nominee_it; + } + return std::distance(submissions.begin(), winner_it); +} + +} // namespace rmf_task diff --git a/rmf_task/test/unit/test_Evaluator.cpp b/rmf_task/test/unit/test_Evaluator.cpp new file mode 100644 index 000000000..560f72c51 --- /dev/null +++ b/rmf_task/test/unit/test_Evaluator.cpp @@ -0,0 +1,106 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include + +namespace rmf_task { + +//============================================================================== +auto now = std::chrono::steady_clock::now(); + +Evaluator::Submission submission1{ + "fleet1", "", 2.3, 3.4, rmf_traffic::time::apply_offset(now, 5) +}; +Evaluator::Submission submission2{ + "fleet2", "", 3.5, 3.6, rmf_traffic::time::apply_offset(now, 5.5) +}; +Evaluator::Submission submission3{ + "fleet3", "", 0.0, 1.4, rmf_traffic::time::apply_offset(now, 3) +}; +Evaluator::Submission submission4{ + "fleet4", "", 5.0, 5.4, rmf_traffic::time::apply_offset(now, 4) +}; +Evaluator::Submission submission5{ + "fleet5", "", 0.5, 0.8, rmf_traffic::time::apply_offset(now, 3.5) +}; + +//============================================================================== +SCENARIO("Winner from Evaluator", "[Evaluator]") +{ + WHEN("Least Diff Cost Evaluator") + { + auto evaluator = std::make_shared(); + + AND_WHEN("0 submissions") + { + std::vector submissions{}; + auto choice = evaluator->choose(submissions); + REQUIRE(choice == std::nullopt); // no winner + } + AND_WHEN("5 submissions") + { + std::vector submissions{ + submission1, submission2, submission3, submission4, submission5 }; + auto choice = evaluator->choose(submissions); + REQUIRE(choice != std::nullopt); + REQUIRE(submissions[*choice].fleet_name == "fleet2"); // least diff cost agent + } + } + + WHEN("Least Fleet Cost Evaluator") + { + auto evaluator = std::make_shared(); + + AND_WHEN("0 submissions") + { + std::vector submissions{}; + auto choice = evaluator->choose(submissions); + REQUIRE(choice == std::nullopt); // no winner + } + AND_WHEN("5 submissions") + { + std::vector submissions{ + submission1, submission2, submission3, submission4, submission5 }; + auto choice = evaluator->choose(submissions); + REQUIRE(choice != std::nullopt); + REQUIRE(submissions[*choice].fleet_name == "fleet5"); // least cost agent + } + } + + WHEN("Quickest Finish Time Evaluator") + { + auto evaluator = std::make_shared(); + + AND_WHEN("0 submissions") + { + std::vector submissions{}; + auto choice = evaluator->choose(submissions); + REQUIRE(choice == std::nullopt); // no winner + } + AND_WHEN("5 submissions") + { + std::vector submissions{ + submission1, submission2, submission3, submission4, submission5 }; + auto choice = evaluator->choose(submissions); + REQUIRE(choice != std::nullopt); + REQUIRE(submissions[*choice].fleet_name == "fleet3"); // quickest agent + } + } +} + +} // namespace rmf_task diff --git a/rmf_task_ros2/CMakeLists.txt b/rmf_task_ros2/CMakeLists.txt index 208358372..d1fe33b7f 100644 --- a/rmf_task_ros2/CMakeLists.txt +++ b/rmf_task_ros2/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.5) project(rmf_task_ros2) if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") # we dont use add_compile_options with pedantic in message packages @@ -17,6 +17,7 @@ find_package(ament_cmake REQUIRED) find_package(rmf_traffic REQUIRED) find_package(rmf_traffic_ros2 REQUIRED) find_package(rmf_task_msgs REQUIRED) +find_package(rmf_task REQUIRED) find_package(Eigen3 REQUIRED) find_package(rclcpp REQUIRED) @@ -27,6 +28,7 @@ target_link_libraries(rmf_task_ros2 PUBLIC rmf_traffic::rmf_traffic rmf_traffic_ros2::rmf_traffic_ros2 + rmf_task::rmf_task ${rmf_task_msgs_LIBRARIES} ${rclcpp_LIBRARIES} ) @@ -37,11 +39,12 @@ target_include_directories(rmf_task_ros2 $ ${rmf_traffic_ros2_INCLUDE_DIRS} ${rmf_task_msgs_INCLUDE_DIRS} + ${rmf_task_INCLUDE_DIRS} ${rclcpp_INCLUDE_DIRS} ) ament_export_targets(rmf_task_ros2 HAS_LIBRARY_TARGET) -ament_export_dependencies(rmf_traffic rmf_task_msgs rclcpp) +ament_export_dependencies(rmf_traffic rmf_task_msgs rmf_task rclcpp) #=============================================================================== @@ -56,6 +59,7 @@ if(BUILD_TESTING AND ament_cmake_catch2_FOUND) rmf_task_ros2 rmf_traffic::rmf_traffic rmf_traffic_ros2::rmf_traffic_ros2 + rmf_task::rmf_task ) target_include_directories(test_rmf_task_ros2 diff --git a/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp b/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp index f01ebf656..cd127fe62 100644 --- a/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp +++ b/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp @@ -21,12 +21,10 @@ #include #include #include -#include #include #include - namespace rmf_task_ros2 { //============================================================================== @@ -42,31 +40,39 @@ class Dispatcher : public std::enable_shared_from_this /// instantiate an rclcpp::Node, a task dispatcher node. Dispatcher node will /// allow you to dispatch submitted task to the best fleet/robot within RMF. /// - /// \param[in] dispatcher_node_name - /// The ROS 2 node to manage the Dispatching of Task + /// \note Using default params: + /// LeastFleetCostEvaluator, bidding window: 2s and keep last 50 tasks /// /// \sa init_and_make_node() - static std::shared_ptr init_and_make_node( - const std::string dispatcher_node_name); + static std::shared_ptr init_and_make_node(); /// Similarly this will init the dispatcher, but you will also need to init /// rclcpp via rclcpp::init(~). /// - /// \param[in] dispatcher_node_name - /// The ROS 2 node to manage the Dispatching of Task - /// /// \sa make_node() - static std::shared_ptr make_node( - const std::string dispatcher_node_name); + static std::shared_ptr make_node(); - /// Create a dispatcher by providing the ros2 node + /// Create a dispatcher by providing the ros2 node and other params /// /// \param[in] node /// ROS 2 node instance /// + /// \param[in] evaluator + /// Bidding Evaluator, default: LeastFleetDiffCostEvaluator + /// + /// \param[in] bidding_time_window + /// Bidding time window, default: 2.0 + /// + /// \param[in] terminated_tasks_depth + /// Keep history depth size of terminated tasks, default: 50 + /// /// \sa make() static std::shared_ptr make( - const std::shared_ptr& node); + const std::shared_ptr& node, + const std::shared_ptr evaluator = + std::make_shared(), + const double bidding_time_window = 2.0, + const int terminated_tasks_depth = 50); /// Submit task to dispatcher node. Calling this function will immediately /// trigger the bidding process, then the task "action". Once submmitted, @@ -99,7 +105,7 @@ class Dispatcher : public std::enable_shared_from_this /// task_id obtained from `submit_task()` /// /// \return State of the task, nullopt if task is not available - const rmf_utils::optional get_task_state( + const std::optional get_task_state( const TaskID& task_id) const; /// Get a mutable ref of active tasks map list handled by dispatcher @@ -121,7 +127,7 @@ class Dispatcher : public std::enable_shared_from_this /// /// \param [in] evaluator /// evaluator used to select the best bid from fleets - void evaluator(std::shared_ptr evaluator); + void evaluator(std::shared_ptr evaluator); /// Get the rclcpp::Node that this dispatcher will be using for communication. std::shared_ptr node(); diff --git a/rmf_task_ros2/include/rmf_task_ros2/StandardNames.hpp b/rmf_task_ros2/include/rmf_task_ros2/StandardNames.hpp index 2bdc71588..c3c135726 100644 --- a/rmf_task_ros2/include/rmf_task_ros2/StandardNames.hpp +++ b/rmf_task_ros2/include/rmf_task_ros2/StandardNames.hpp @@ -30,8 +30,8 @@ const std::string SubmitTaskSrvName = "submit_task"; const std::string CancelTaskSrvName = "cancel_task"; const std::string GetTaskListSrvName = "get_tasks"; -const std::string TaskRequestTopicName = Prefix + "dispatch_request"; -const std::string TaskAckTopicName = Prefix + "dispatch_ack"; +const std::string DispatchRequestTopicName = Prefix + "dispatch_request"; +const std::string DispatchAckTopicName = Prefix + "dispatch_ack"; const std::string TaskStatusTopicName = "task_summaries"; } // namespace rmf_task_ros2 diff --git a/rmf_task_ros2/include/rmf_task_ros2/bidding/Auctioneer.hpp b/rmf_task_ros2/include/rmf_task_ros2/bidding/Auctioneer.hpp index 4b9b5a87d..5a1337777 100644 --- a/rmf_task_ros2/include/rmf_task_ros2/bidding/Auctioneer.hpp +++ b/rmf_task_ros2/include/rmf_task_ros2/bidding/Auctioneer.hpp @@ -22,7 +22,8 @@ #include #include -#include +#include +#include namespace rmf_task_ros2 { namespace bidding { @@ -34,6 +35,9 @@ namespace bidding { class Auctioneer : public std::enable_shared_from_this { public: + using BidNotice = rmf_task_msgs::msg::BidNotice; + using Submission = rmf_task::Evaluator::Submission; + /// Callback which will provide the winner when a bid is concluded /// /// \param[in] task_id @@ -66,24 +70,11 @@ class Auctioneer : public std::enable_shared_from_this /// bidding task, task which will call for bid void start_bidding(const BidNotice& bid_notice); - /// A pure abstract interface class for the auctioneer to choose the best - /// choosing the best submissions. - class Evaluator - { - public: - - /// Given a list of submissions, choose the one that is the "best". It is - /// up to the implementation of the Evaluator to decide how to rank. - virtual std::size_t choose(const Submissions& submissions) const = 0; - - virtual ~Evaluator() = default; - }; - /// Provide a custom evaluator which will be used to choose the best bid /// If no selection is given, Default is: LeastFleetDiffCostEvaluator /// /// \param[in] evaluator - void select_evaluator(std::shared_ptr evaluator); + void select_evaluator(std::shared_ptr evaluator); class Implementation; @@ -92,27 +83,6 @@ class Auctioneer : public std::enable_shared_from_this rmf_utils::unique_impl_ptr _pimpl; }; -//============================================================================== -class LeastFleetDiffCostEvaluator : public Auctioneer::Evaluator -{ -public: - std::size_t choose(const Submissions& submissions) const final; -}; - -//============================================================================== -class LeastFleetCostEvaluator : public Auctioneer::Evaluator -{ -public: - std::size_t choose(const Submissions& submissions) const final; -}; - -//============================================================================== -class QuickestFinishEvaluator : public Auctioneer::Evaluator -{ -public: - std::size_t choose(const Submissions& submissions) const final; -}; - } // namespace bidding } // namespace rmf_task_ros2 diff --git a/rmf_task_ros2/include/rmf_task_ros2/bidding/MinimalBidder.hpp b/rmf_task_ros2/include/rmf_task_ros2/bidding/MinimalBidder.hpp index ef53db6fc..b0e0f34d6 100644 --- a/rmf_task_ros2/include/rmf_task_ros2/bidding/MinimalBidder.hpp +++ b/rmf_task_ros2/include/rmf_task_ros2/bidding/MinimalBidder.hpp @@ -22,8 +22,8 @@ #include #include - -#include +#include +#include namespace rmf_task_ros2 { namespace bidding { @@ -44,6 +44,15 @@ class MinimalBidder Patrol = TaskTypeMsg::TYPE_PATROL }; + + struct Submission + { + std::string robot_name; + double prev_cost = 0.0; + double new_cost = std::numeric_limits::max(); + rmf_traffic::Time finish_time; + }; + /// Callback function when a bid notice is received from the autioneer /// /// \param[in] notice @@ -51,6 +60,8 @@ class MinimalBidder /// /// \return submission /// Estimates of a task. This submission is used by dispatcher for eval + using BidNotice = rmf_task_msgs::msg::BidNotice; + using ParseSubmissionCallback = std::function; diff --git a/rmf_task_ros2/include/rmf_task_ros2/bidding/Submission.hpp b/rmf_task_ros2/include/rmf_task_ros2/bidding/Submission.hpp deleted file mode 100644 index b9b8cc813..000000000 --- a/rmf_task_ros2/include/rmf_task_ros2/bidding/Submission.hpp +++ /dev/null @@ -1,44 +0,0 @@ -/* - * Copyright (C) 2020 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ - -#ifndef RMF_TASK_ROS2__BIDDING__SUBMISSION_HPP -#define RMF_TASK_ROS2__BIDDING__SUBMISSION_HPP - -#include -#include - -namespace rmf_task_ros2 { -namespace bidding { - -//============================================================================== -struct Submission -{ - std::string fleet_name; - std::string robot_name; - double prev_cost = 0.0; - double new_cost = std::numeric_limits::max(); - rmf_traffic::Time finish_time; -}; - -//============================================================================== -using Submissions = std::vector; -using BidNotice = rmf_task_msgs::msg::BidNotice; - -} // namespace bidding -} // namespace rmf_task_ros2 - -#endif // RMF_TASK_ROS2__BIDDING__SUBMISSION_HPP diff --git a/rmf_task_ros2/src/dispatcher_node/main.cpp b/rmf_task_ros2/src/dispatcher_node/main.cpp index a96d066fd..5d1b31508 100644 --- a/rmf_task_ros2/src/dispatcher_node/main.cpp +++ b/rmf_task_ros2/src/dispatcher_node/main.cpp @@ -22,11 +22,44 @@ int main(int argc, char* argv[]) { rclcpp::init(argc, argv); std::cout << "~Initializing Dispatcher Node~" << std::endl; + auto node = rclcpp::Node::make_shared("rmf_dispatcher_node"); - auto dispatcher = rmf_task_ros2::Dispatcher::make_node( - "rmf_dispatcher_node"); + // ros2 param + const double bidding_time_window = + node->declare_parameter("bidding_time_window", 2.0); + RCLCPP_INFO(node->get_logger(), + "Declared Time Window Param as: [%f] secs", bidding_time_window); + const int terminated_tasks_depth = + node->declare_parameter("terminated_tasks_depth", 50); + RCLCPP_INFO(node->get_logger(), + "Declared Terminated Tasks Depth Param as: [%d]", + terminated_tasks_depth); + const int evaluator_type_enum = + node->declare_parameter("evaluator_type_enum", 0); + RCLCPP_INFO(node->get_logger(), "Type of Evaluator: [%d]", + evaluator_type_enum); + + std::shared_ptr eval_type; + switch (evaluator_type_enum) + { + case 0: + eval_type = std::make_shared(); + break; + case 1: + eval_type = std::make_shared(); + break; + case 2: + eval_type = std::make_shared(); + break; + default: + RCLCPP_WARN(node->get_logger(), "Invalid Evaluator, use default: 0"); + eval_type = std::make_shared(); + break; + } + + auto dispatcher = rmf_task_ros2::Dispatcher::make( + node, eval_type, bidding_time_window, terminated_tasks_depth); - const auto& node = dispatcher->node(); RCLCPP_INFO(node->get_logger(), "Starting task dispatcher node"); dispatcher->spin(); RCLCPP_INFO(node->get_logger(), "Closing down task dispatcher"); diff --git a/rmf_task_ros2/src/mock_bidder/main.cpp b/rmf_task_ros2/src/mock_bidder/main.cpp index 7340c6b30..994dbc3c3 100644 --- a/rmf_task_ros2/src/mock_bidder/main.cpp +++ b/rmf_task_ros2/src/mock_bidder/main.cpp @@ -24,7 +24,9 @@ #include using namespace rmf_task_ros2; + using TaskType = bidding::MinimalBidder::TaskType; +using BidNotice = rmf_task_msgs::msg::BidNotice; int main(int argc, char* argv[]) { @@ -43,14 +45,14 @@ int main(int argc, char* argv[]) node, fleet_name, { TaskType::Clean, TaskType::Delivery }, - [](const bidding::BidNotice& notice) + [](const BidNotice& notice) { // Here user will provice the best robot as a bid submission std::cout << "[MockBidder] Providing best estimates" << std::endl; auto req_start_time = rmf_traffic_ros2::convert(notice.task_profile.description.start_time); - bidding::Submission best_robot_estimate; + bidding::MinimalBidder::Submission best_robot_estimate; best_robot_estimate.robot_name = "dumbot"; best_robot_estimate.prev_cost = 10.2; best_robot_estimate.new_cost = 13.5; diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp index 2dea6a86b..ff40ba249 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp @@ -47,7 +47,7 @@ class Dispatcher::Implementation StatusCallback on_change_fn; - std::queue queue_bidding_tasks; + std::queue queue_bidding_tasks; DispatchTasks active_dispatch_tasks; DispatchTasks terminal_dispatch_tasks; std::size_t task_counter = 0; // index for generating task_id @@ -67,52 +67,18 @@ class Dispatcher::Implementation Implementation(std::shared_ptr node_) : node{std::move(node_)} { - // ros2 param - bidding_time_window = - node->declare_parameter("bidding_time_window", 2.0); - RCLCPP_INFO(node->get_logger(), - " Declared Time Window Param as: %f secs", bidding_time_window); - terminated_tasks_max_size = - node->declare_parameter("terminated_tasks_max_size", 100); - RCLCPP_INFO(node->get_logger(), - " Declared Terminated Tasks Max Size Param as: %d", - terminated_tasks_max_size); - - // Setup up stream srv interfaces submit_task_srv = node->create_service( rmf_task_ros2::SubmitTaskSrvName, [this]( const std::shared_ptr request, std::shared_ptr response) { - switch (request->evaluator) - { - using namespace rmf_task_ros2::bidding; - case SubmitTaskSrv::Request::LOWEST_DIFF_COST_EVAL: - this->auctioneer->select_evaluator( - std::make_shared()); - break; - case SubmitTaskSrv::Request::LOWEST_COST_EVAL: - this->auctioneer->select_evaluator( - std::make_shared()); - break; - case SubmitTaskSrv::Request::QUICKEST_FINISH_EVAL: - this->auctioneer->select_evaluator( - std::make_shared()); - break; - default: - RCLCPP_WARN(this->node->get_logger(), - "Selected Evaluator is invalid, switch back to previous"); - break; - } - const auto id = this->submit_task(request->description); if (id == std::nullopt) { response->success = false; return; } - response->task_id = *id; response->success = true; } @@ -193,7 +159,7 @@ class Dispatcher::Implementation if (on_change_fn) on_change_fn(new_task_status); - bidding::BidNotice bid_notice; + rmf_task_msgs::msg::BidNotice bid_notice; bid_notice.task_profile = submitted_task; bid_notice.time_window = rmf_traffic_ros2::convert( rmf_traffic::time::from_seconds(bidding_time_window)); @@ -291,7 +257,7 @@ class Dispatcher::Implementation void receive_bidding_winner_cb( const TaskID& task_id, - const rmf_utils::optional winner) + const std::optional winner) { const auto it = active_dispatch_tasks.find(task_id); if (it == active_dispatch_tasks.end()) @@ -405,23 +371,24 @@ class Dispatcher::Implementation }; //============================================================================== -std::shared_ptr Dispatcher::init_and_make_node( - const std::string dispatcher_node_name) +std::shared_ptr Dispatcher::init_and_make_node() { rclcpp::init(0, nullptr); - return make_node(dispatcher_node_name); + return make_node(); } //============================================================================== -std::shared_ptr Dispatcher::make_node( - const std::string dispatcher_node_name) +std::shared_ptr Dispatcher::make_node() { - return make(rclcpp::Node::make_shared(dispatcher_node_name)); + return make(rclcpp::Node::make_shared("rmf_dispatcher_node")); } //============================================================================== std::shared_ptr Dispatcher::make( - const std::shared_ptr& node) + const std::shared_ptr& node, + const std::shared_ptr evaluator, + const double bidding_time_window, + const int terminated_tasks_depth) { auto pimpl = rmf_utils::make_impl(node); pimpl->action_client = action::Client::make(node); @@ -429,6 +396,9 @@ std::shared_ptr Dispatcher::make( auto dispatcher = std::shared_ptr(new Dispatcher()); dispatcher->_pimpl = std::move(pimpl); dispatcher->_pimpl->start(); + dispatcher->_pimpl->bidding_time_window = bidding_time_window; + dispatcher->_pimpl->terminated_tasks_max_size = terminated_tasks_depth; + dispatcher->_pimpl->auctioneer->select_evaluator(evaluator); return dispatcher; } @@ -472,7 +442,7 @@ void Dispatcher::on_change(StatusCallback on_change_fn) //============================================================================== void Dispatcher::evaluator( - std::shared_ptr evaluator) + std::shared_ptr evaluator) { _pimpl->auctioneer->select_evaluator(evaluator); } diff --git a/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp b/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp index d2bda8de0..d3b96e50f 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp @@ -33,7 +33,7 @@ Client::Client(std::shared_ptr node) const auto dispatch_qos = rclcpp::ServicesQoS().reliable(); _request_msg_pub = _node->create_publisher( - TaskRequestTopicName, dispatch_qos); + DispatchRequestTopicName, dispatch_qos); _status_msg_sub = _node->create_subscription( TaskStatusTopicName, dispatch_qos, @@ -77,7 +77,7 @@ Client::Client(std::shared_ptr node) }); _ack_msg_sub = _node->create_subscription( - TaskAckTopicName, dispatch_qos, + DispatchAckTopicName, dispatch_qos, [&](const std::unique_ptr msg) { const auto task_id = msg->dispatch_request.task_profile.task_id; @@ -138,9 +138,6 @@ void Client::update_task_status(const TaskStatusPtr status) } } -//============================================================================== -// check if task is updated TODO - //============================================================================== void Client::add_task( const std::string& fleet_name, diff --git a/rmf_task_ros2/src/rmf_task_ros2/action/Server.cpp b/rmf_task_ros2/src/rmf_task_ros2/action/Server.cpp index 87130837a..9efec4a77 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/action/Server.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/action/Server.cpp @@ -54,7 +54,7 @@ Server::Server( const auto dispatch_qos = rclcpp::ServicesQoS().reliable(); _request_msg_sub = _node->create_subscription( - TaskRequestTopicName, dispatch_qos, + DispatchRequestTopicName, dispatch_qos, [&](const std::unique_ptr msg) { if (msg->fleet_name != _fleet_name) @@ -89,7 +89,7 @@ Server::Server( }); _ack_msg_pub = _node->create_publisher( - TaskAckTopicName, dispatch_qos); + DispatchAckTopicName, dispatch_qos); _status_msg_pub = _node->create_publisher( TaskStatusTopicName, dispatch_qos); diff --git a/rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.cpp b/rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.cpp index 3bb3d9252..a54b4a83c 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.cpp @@ -40,7 +40,7 @@ Auctioneer::Implementation::Implementation( bidding_result_callback{std::move(result_callback)} { // default evaluator - evaluator = std::make_shared(); + evaluator = std::make_shared(); const auto dispatch_qos = rclcpp::ServicesQoS().reliable(); bid_notice_pub = node->create_publisher( @@ -154,21 +154,17 @@ bool Auctioneer::Implementation::determine_winner( std::optional Auctioneer::Implementation::evaluate( const Submissions& submissions) { - if (submissions.size() == 0) - return std::nullopt; - if (!evaluator) { RCLCPP_WARN(node->get_logger(), "Bidding Evaluator is not set"); return std::nullopt; } - const std::size_t choice = evaluator->choose(submissions); - - if (choice >= submissions.size()) + auto choice = evaluator->choose(submissions); + if (choice == std::nullopt) return std::nullopt; - return submissions[choice]; + return submissions[*choice]; } //============================================================================== @@ -191,7 +187,7 @@ void Auctioneer::start_bidding(const BidNotice& bid_notice) //============================================================================== void Auctioneer::select_evaluator( - std::shared_ptr evaluator) + std::shared_ptr evaluator) { _pimpl->evaluator = std::move(evaluator); } @@ -202,52 +198,5 @@ Auctioneer::Auctioneer() // do nothing } -//============================================================================== -std::size_t LeastFleetDiffCostEvaluator::choose( - const Submissions& submissions) const -{ - auto winner_it = submissions.begin(); - float winner_cost_diff = winner_it->new_cost - winner_it->prev_cost; - for (auto nominee_it = ++submissions.begin(); - nominee_it != submissions.end(); ++nominee_it) - { - float nominee_cost_diff = nominee_it->new_cost - nominee_it->prev_cost; - if (nominee_cost_diff < winner_cost_diff) - { - winner_it = nominee_it; - winner_cost_diff = nominee_cost_diff; - } - } - return std::distance(submissions.begin(), winner_it); -} - -//============================================================================== -std::size_t LeastFleetCostEvaluator::choose( - const Submissions& submissions) const -{ - auto winner_it = submissions.begin(); - for (auto nominee_it = ++submissions.begin(); - nominee_it != submissions.end(); ++nominee_it) - { - if (nominee_it->new_cost < winner_it->new_cost) - winner_it = nominee_it; - } - return std::distance(submissions.begin(), winner_it); -} - -//============================================================================== -std::size_t QuickestFinishEvaluator::choose( - const Submissions& submissions) const -{ - auto winner_it = submissions.begin(); - for (auto nominee_it = ++submissions.begin(); - nominee_it != submissions.end(); ++nominee_it) - { - if (nominee_it->finish_time < winner_it->finish_time) - winner_it = nominee_it; - } - return std::distance(submissions.begin(), winner_it); -} - } // namespace bidding } // namespace rmf_task_ros2 diff --git a/rmf_task_ros2/src/rmf_task_ros2/bidding/MinimalBidder.cpp b/rmf_task_ros2/src/rmf_task_ros2/bidding/MinimalBidder.cpp index 524097a4a..38f1fabc3 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/bidding/MinimalBidder.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/bidding/MinimalBidder.cpp @@ -15,7 +15,6 @@ * */ -#include #include #include @@ -27,18 +26,6 @@ namespace bidding { using BidProposal = rmf_task_msgs::msg::BidProposal; -//============================================================================== -BidProposal convert(const Submission& from) -{ - BidProposal proposal_msg; - proposal_msg.fleet_name = from.fleet_name; - proposal_msg.robot_name = from.robot_name; - proposal_msg.prev_cost = from.prev_cost; - proposal_msg.new_cost = from.new_cost; - proposal_msg.finish_time = rmf_traffic_ros2::convert(from.finish_time); - return proposal_msg; -} - //============================================================================== class MinimalBidder::Implementation { @@ -99,12 +86,19 @@ class MinimalBidder::Implementation if (!get_submission_fn) return; - // Submit proposal const auto bid_submission = get_submission_fn(msg); - auto best_proposal = convert(bid_submission); - best_proposal.fleet_name = fleet_name; - best_proposal.task_profile = msg.task_profile; - dispatch_proposal_pub->publish(best_proposal); + + // Submit proposal + BidProposal proposal_msg; + proposal_msg.fleet_name = fleet_name; + proposal_msg.task_profile = msg.task_profile; + proposal_msg.robot_name = bid_submission.robot_name; + proposal_msg.prev_cost = bid_submission.prev_cost; + proposal_msg.new_cost = bid_submission.new_cost; + proposal_msg.finish_time = + rmf_traffic_ros2::convert(bid_submission.finish_time); + + dispatch_proposal_pub->publish(proposal_msg); } }; diff --git a/rmf_task_ros2/src/rmf_task_ros2/bidding/internal_Auctioneer.hpp b/rmf_task_ros2/src/rmf_task_ros2/bidding/internal_Auctioneer.hpp index 57cf1a0eb..99ab3351b 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/bidding/internal_Auctioneer.hpp +++ b/rmf_task_ros2/src/rmf_task_ros2/bidding/internal_Auctioneer.hpp @@ -19,7 +19,7 @@ #ifndef SRC__RMF_TASK_ROS2__BIDDING__INTERNAL_AUCTIONEER_HPP #define SRC__RMF_TASK_ROS2__BIDDING__INTERNAL_AUCTIONEER_HPP -#include +#include #include #include @@ -30,6 +30,8 @@ namespace rmf_task_ros2 { namespace bidding { using BidProposal = rmf_task_msgs::msg::BidProposal; +using Submission = rmf_task::Evaluator::Submission; +using Submissions = rmf_task::Evaluator::Submissions; //============================================================================== class Auctioneer::Implementation @@ -38,13 +40,13 @@ class Auctioneer::Implementation std::shared_ptr node; rclcpp::TimerBase::SharedPtr timer; BiddingResultCallback bidding_result_callback; - std::shared_ptr evaluator; + std::shared_ptr evaluator; struct BiddingTask { BidNotice bid_notice; builtin_interfaces::msg::Time start_time; - std::vector submissions; + Submissions submissions; }; bool bidding_in_proccess = false; diff --git a/rmf_task_ros2/test/bidding/test_Evaluator.cpp b/rmf_task_ros2/test/bidding/test_Evaluator.cpp deleted file mode 100644 index cc9159ad9..000000000 --- a/rmf_task_ros2/test/bidding/test_Evaluator.cpp +++ /dev/null @@ -1,116 +0,0 @@ -/* - * Copyright (C) 2020 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ - -#include -#include "../../src/rmf_task_ros2/bidding/internal_Auctioneer.hpp" -#include - -namespace rmf_task_ros2 { -namespace bidding { - -//============================================================================== -auto now = std::chrono::steady_clock::now(); - -Submission submission1{ - "fleet1", "", 2.3, 3.4, rmf_traffic::time::apply_offset(now, 5) -}; -Submission submission2{ - "fleet2", "", 3.5, 3.6, rmf_traffic::time::apply_offset(now, 5.5) -}; -Submission submission3{ - "fleet3", "", 0.0, 1.4, rmf_traffic::time::apply_offset(now, 3) -}; -Submission submission4{ - "fleet4", "", 5.0, 5.4, rmf_traffic::time::apply_offset(now, 4) -}; -Submission submission5{ - "fleet5", "", 0.5, 0.8, rmf_traffic::time::apply_offset(now, 3.5) -}; - -//============================================================================== -SCENARIO("Auctioneer Winner Evaluator", "[Evaluator]") -{ - rclcpp::init(0, nullptr); - auto node = rclcpp::Node::make_shared("test_selfbidding"); - auto auctioneer = Auctioneer::make(node, - [](const std::string&, const std::optional) {}); - - WHEN("Least Diff Cost Evaluator") - { - auto eval = std::make_shared(); - auctioneer->select_evaluator(eval); - - AND_WHEN("0 submissions") - { - std::vector submissions{}; - auto winner = evaluate(*auctioneer, submissions); - REQUIRE(!winner); // no winner - } - AND_WHEN("5 submissions") - { - std::vector submissions{ - submission1, submission2, submission3, submission4, submission5 }; - auto winner = evaluate(*auctioneer, submissions); - REQUIRE(winner->fleet_name == "fleet2"); // least diff cost agent - } - } - - WHEN("Least Fleet Cost Evaluator") - { - auto eval = std::make_shared(); - auctioneer->select_evaluator(eval); - - AND_WHEN("0 submissions") - { - std::vector submissions{}; - auto winner = evaluate(*auctioneer, submissions); - REQUIRE(!winner); // no winner - } - AND_WHEN("5 submissions") - { - std::vector submissions{ - submission1, submission2, submission3, submission4, submission5 }; - auto winner = evaluate(*auctioneer, submissions); - REQUIRE(winner->fleet_name == "fleet5"); // least diff cost agent - } - } - - WHEN("Quickest Finish Time Evaluator") - { - auto eval = std::make_shared(); - auctioneer->select_evaluator(eval); - - AND_WHEN("0 submissions") - { - std::vector submissions{}; - auto winner = evaluate(*auctioneer, submissions); - REQUIRE(!winner); // no winner - } - AND_WHEN("5 submissions") - { - std::vector submissions{ - submission1, submission2, submission3, submission4, submission5 }; - auto winner = evaluate(*auctioneer, submissions); - REQUIRE(winner->fleet_name == "fleet3"); // least diff cost agent - } - } - - rclcpp::shutdown(); -} - -} // namespace bidding -} // namespace rmf_task_ros2 diff --git a/rmf_task_ros2/test/bidding/test_SelfBid.cpp b/rmf_task_ros2/test/bidding/test_SelfBid.cpp index 4656e70e4..646719d9f 100644 --- a/rmf_task_ros2/test/bidding/test_SelfBid.cpp +++ b/rmf_task_ros2/test/bidding/test_SelfBid.cpp @@ -28,6 +28,7 @@ namespace rmf_task_ros2 { namespace bidding { using TaskProfile = rmf_task_msgs::msg::TaskProfile; +using BidNotice = rmf_task_msgs::msg::BidNotice; using TaskType = bidding::MinimalBidder::TaskType; //============================================================================== @@ -66,7 +67,8 @@ SCENARIO("Auction with 2 Bids", "[TwoBids]") node, /// Bidding Result Callback Function [&r_result_id, &r_result_winner]( - const std::string& task_id, const std::optional winner) + const std::string& task_id, + const std::optional winner) { if (!winner) return; @@ -83,7 +85,7 @@ SCENARIO("Auction with 2 Bids", "[TwoBids]") node, "bidder1", { TaskType::Station, TaskType::Delivery }, [&test_notice_bidder1](const BidNotice& notice) { - Submission best_robot_estimate; + MinimalBidder::Submission best_robot_estimate; test_notice_bidder1 = notice.task_profile; return best_robot_estimate; } @@ -94,7 +96,7 @@ SCENARIO("Auction with 2 Bids", "[TwoBids]") [&test_notice_bidder2](const BidNotice& notice) { // TaskType should not be supported - Submission best_robot_estimate; + MinimalBidder::Submission best_robot_estimate; best_robot_estimate.new_cost = 2.3; // lower cost than bidder1 test_notice_bidder2 = notice.task_profile; return best_robot_estimate; diff --git a/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp b/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp index 266bdd2e9..ca1268127 100644 --- a/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp +++ b/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp @@ -41,7 +41,7 @@ SCENARIO("Dispatcehr API Test", "[Dispatcher]") task_desc2.task_type.type = rmf_task_msgs::msg::TaskType::TYPE_CLEAN; //============================================================================ - auto dispatcher = Dispatcher::init_and_make_node("test_dispatcher_node"); + auto dispatcher = Dispatcher::init_and_make_node(); auto spin_thread = std::thread( [&dispatcher]() @@ -131,10 +131,10 @@ SCENARIO("Dispatcehr API Test", "[Dispatcher]") node, "dummy_fleet", { TaskType::Station, TaskType::Clean }, - [](const bidding::BidNotice&) + [](const rmf_task_msgs::msg::BidNotice&) { // Provide a best estimate - bidding::Submission best_robot_estimate; + bidding::MinimalBidder::Submission best_robot_estimate; best_robot_estimate.new_cost = 13.5; return best_robot_estimate; } From 6a67a5fa2ea544eff974a47d59b58825fc387b07 Mon Sep 17 00:00:00 2001 From: youliang Date: Thu, 11 Feb 2021 17:25:51 +0800 Subject: [PATCH 02/10] preliminary add description abstract class --- .../agv/FleetUpdateHandle.cpp | 139 ++++-------- .../include/rmf_task_ros2/Description.hpp | 149 +++++++++++++ .../include/rmf_task_ros2/Dispatcher.hpp | 8 +- .../src/rmf_task_ros2/Description.cpp | 206 ++++++++++++++++++ .../src/rmf_task_ros2/Dispatcher.cpp | 7 + .../test/dispatcher/test_Description.cpp | 72 ++++++ 6 files changed, 488 insertions(+), 93 deletions(-) create mode 100644 rmf_task_ros2/include/rmf_task_ros2/Description.hpp create mode 100644 rmf_task_ros2/src/rmf_task_ros2/Description.cpp create mode 100644 rmf_task_ros2/test/dispatcher/test_Description.cpp diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index a808d1f5c..b9b8c87d5 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -36,6 +36,8 @@ #include #include +#include + #include #include #include @@ -148,48 +150,49 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( rmf_task::ConstRequestPtr new_request = nullptr; const auto& task_profile = msg->task_profile; const auto& task_type = task_profile.description.task_type; + const auto& description_msg = task_profile.description; const rmf_traffic::Time start_time = rmf_traffic_ros2::convert(task_profile.description.start_time); // TODO (YV) get rid of ID field in RequestPtr - std::string id = msg->task_profile.task_id; + const std::string id = msg->task_profile.task_id; const auto& graph = planner->get_configuration().graph(); + rmf_task_ros2::Description::ConstDescriptionPtr task_description; // Process Cleaning task if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_CLEAN) { - if (task_profile.description.clean.start_waypoint.empty()) - { - RCLCPP_ERROR( - node->get_logger(), - "Required param [clean.start_waypoint] missing in TaskProfile." - "Rejecting BidNotice with task_id:[%s]" , id.c_str()); + const auto& clean = + rmf_task_ros2::description::Clean::make_from_msg(description_msg); + if (!clean) + { + RCLCPP_ERROR(node->get_logger(), + "Clean Msg is invalid/invalid." + "Rejecting BidNotice with task_id:[%s]", id.c_str()); return; } // Check for valid start waypoint - const std::string start_wp_name = - task_profile.description.clean.start_waypoint; - const auto start_wp = graph.find_waypoint(start_wp_name); + const auto start_wp = graph.find_waypoint(clean->start_waypoint()); 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(), start_wp_name.c_str(), id.c_str()); + name.c_str(), clean->start_waypoint().c_str(), id.c_str()); return; } // Get dock parameters - const auto clean_param_it = dock_param_map.find(start_wp_name); + const auto clean_param_it = dock_param_map.find(clean->start_waypoint()); if (clean_param_it == dock_param_map.end()) { RCLCPP_INFO( node->get_logger(), "Dock param for dock_name:[%s] unavailable. Rejecting BidNotice with " - "task_id:[%s]", start_wp_name.c_str(), id.c_str()); + "task_id:[%s]", clean->start_waypoint().c_str(), id.c_str()); return; } @@ -224,7 +227,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( RCLCPP_INFO( node->get_logger(), "Unable to generate cleaning trajectory from positions specified " - " in DockSummary msg for [%s]", start_wp_name.c_str()); + " in DockSummary msg for [%s]", clean->start_waypoint().c_str()); return; } @@ -244,82 +247,52 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( RCLCPP_INFO( node->get_logger(), "Generated Clean request for task_id:[%s]", id.c_str()); + + task_description = clean; } else if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_DELIVERY) { - const auto& delivery = task_profile.description.delivery; - if (delivery.pickup_place_name.empty()) - { - RCLCPP_ERROR( - 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_ERROR( - 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_ERROR( - node->get_logger(), - "Required param [delivery.dropoff_place_name] missing in TaskProfile." - "Rejecting BidNotice with task_id:[%s]" , id.c_str()); - - return; - } + const auto& delivery = + rmf_task_ros2::description::Delivery::make_from_msg(description_msg); - if (delivery.dropoff_ingestor.empty()) + if (!delivery) { - RCLCPP_ERROR( - node->get_logger(), - "Required param [delivery.dropoff_ingestor] missing in TaskProfile." - "Rejecting BidNotice with task_id:[%s]" , id.c_str()); - + RCLCPP_ERROR(node->get_logger(), + "Delivery Msg is invalid/invalid." + "Rejecting BidNotice with task_id:[%s]", id.c_str()); return; } - const auto pickup_wp = graph.find_waypoint(delivery.pickup_place_name); + 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()); - + name.c_str(), delivery->pickup_place_name().c_str(), id.c_str()); return; } - const auto dropoff_wp = graph.find_waypoint(delivery.dropoff_place_name); + 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()); - + name.c_str(), delivery->dropoff_place_name().c_str(), id.c_str()); return; } new_request = rmf_task::requests::Delivery::make( id, pickup_wp->index(), - delivery.pickup_dispenser, + description_msg.delivery.pickup_dispenser, dropoff_wp->index(), - delivery.dropoff_ingestor, - delivery.items, + description_msg.delivery.dropoff_ingestor, + description_msg.delivery.items, motion_sink, ambient_sink, planner, @@ -330,60 +303,41 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( node->get_logger(), "Generated Delivery request for task_id:[%s]", id.c_str()); + task_description = delivery; } else if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_LOOP) { - const auto& loop = task_profile.description.loop; - if (loop.start_name.empty()) - { - RCLCPP_ERROR( - 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_ERROR( - 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) + const auto& loop = + rmf_task_ros2::description::Loop::make_from_msg(description_msg); + + if (!loop) { - RCLCPP_ERROR( - node->get_logger(), - "Required param [loop.num_loops: %d] in TaskProfile is invalid." - "Rejecting BidNotice with task_id:[%s]" , loop.num_loops, id.c_str()); - + RCLCPP_ERROR(node->get_logger(), + "Delivery Msg is invalid/invalid." + "Rejecting BidNotice with task_id:[%s]", id.c_str()); return; } - const auto start_wp = graph.find_waypoint(loop.start_name); + 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()); + name.c_str(), loop->start_name().c_str(), id.c_str()); return; } - const auto finish_wp = graph.find_waypoint(loop.finish_name); + 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()); + name.c_str(), loop->start_name().c_str(), id.c_str()); return; } @@ -392,7 +346,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( id, start_wp->index(), finish_wp->index(), - loop.num_loops, + description_msg.loop.num_loops, motion_sink, ambient_sink, planner, @@ -402,6 +356,8 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( RCLCPP_INFO( node->get_logger(), "Generated Loop request for task_id:[%s]", id.c_str()); + + task_description = loop; } else { @@ -801,7 +757,6 @@ rmf_fleet_msgs::msg::RobotState convert_state(const TaskManager& mgr) .index(0); }(); - return rmf_fleet_msgs::build() .name(context.name()) .model(context.description().owner()) diff --git a/rmf_task_ros2/include/rmf_task_ros2/Description.hpp b/rmf_task_ros2/include/rmf_task_ros2/Description.hpp new file mode 100644 index 000000000..40f17a631 --- /dev/null +++ b/rmf_task_ros2/include/rmf_task_ros2/Description.hpp @@ -0,0 +1,149 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef RMF_TASK_ROS2__DESCRIPTION_HPP +#define RMF_TASK_ROS2__DESCRIPTION_HPP + +#include +#include +#include + +#include +#include +#include +#include + +namespace rmf_task_ros2 { + +//============================================================================== +// A description for user requested task +class Description +{ +public: + using ConstDescriptionPtr = std::shared_ptr; + using TaskDescription = rmf_task_msgs::msg::TaskDescription; + + rmf_traffic::Time start_time() const; + + ConstRequestPtr request() = nullptr; + + uint32_t type() const; + + virtual TaskDescription to_msg() const = 0; + +protected: + rmf_traffic::Time _start_time; + rmf_task_msgs::msg::TaskType task_type; +}; + +//============================================================================== +namespace description { + +class Delivery : public Description +{ +public: + using DispenserRequestItem = rmf_dispenser_msgs::msg::DispenserRequestItem; + + static std::shared_ptr make( + rmf_traffic::Time start_time, + std::string pickup_place_name, + std::string pickup_dispenser, + std::string dropoff_place_name, + std::string dropoff_ingestor, + std::vector items); + + /// use this only within internal implementation of RMF + static std::shared_ptr make_from_msg(TaskDescription msg); + + /// use this only within internal implementation of RMF + TaskDescription to_msg() const final; + + /// Get the pickup_place_name + const std::string& pickup_place_name() const; + + /// Get the dropoff_place_name + const std::string& dropoff_place_name() const; + +private: + std::string _pickup_place_name; + std::string pickup_dispenser; + std::string _dropoff_place_name; + std::string dropoff_ingestor; + std::vector items; +}; + +//============================================================================== +class Loop : public Description +{ +public: + static std::shared_ptr make( + rmf_traffic::Time start_time, + std::string start_name, + std::string finish_name, + std::size_t num_loops); + + static std::shared_ptr make_from_msg(TaskDescription msg); + + TaskDescription to_msg() const final; + + /// Get the start_name + const std::string& start_name() const; + + /// Get the finish_name + const std::string& finish_name() const; + +private: + std::string _start_name; + std::string _finish_name; + std::size_t num_loops; +}; + +//============================================================================== + +class Clean : public Description +{ +public: + std::string _start_waypoint; + + static std::shared_ptr make( + rmf_traffic::Time start_time, + std::string start_waypoint); + + static std::shared_ptr make_from_msg(TaskDescription msg); + + TaskDescription to_msg() const final; + + /// Get the _start_waypoint + const std::string& start_waypoint() const; +}; + +} // namespace description + +//============================================================================== +class Execution +{ +public: + static size_t execute_task(Description::ConstDescriptionPtr task) + { + std::cout << "executing: " << task->type() << std::endl; + return task->type(); + } +}; + +} // namespace rmf_task_ros2 + +#endif // RMF_TASK_ROS2__DESCRIPTION_HPP diff --git a/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp b/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp index cd127fe62..3241a4ca6 100644 --- a/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp +++ b/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp @@ -24,6 +24,7 @@ #include #include +#include namespace rmf_task_ros2 { @@ -80,13 +81,18 @@ class Dispatcher : public std::enable_shared_from_this /// then the state will turn to 'Queued' /// /// \param [in] task_description - /// Submit a task to dispatch + /// Submit a Description of task to dispatch /// /// \return task_id /// self-generated task_id, nullopt is submit task failed + [[deprecated("Soon will only support ConstDescriptionPtr as arg")]] std::optional submit_task( const TaskDescription& task_description); + // New task submission + std::optional submit_task( + const Description::ConstDescriptionPtr task_description); + /// Cancel an active task which was previously submitted to Dispatcher. This /// will terminate the task with a State of: `Canceled`. If a task is /// `Queued` or `Executing`, this function will send a cancel req to diff --git a/rmf_task_ros2/src/rmf_task_ros2/Description.cpp b/rmf_task_ros2/src/rmf_task_ros2/Description.cpp new file mode 100644 index 000000000..cb7fc1b52 --- /dev/null +++ b/rmf_task_ros2/src/rmf_task_ros2/Description.cpp @@ -0,0 +1,206 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +namespace rmf_task_ros2 { + +//============================================================================== +rmf_traffic::Time Description::start_time() const +{ + return _start_time; +} + +//============================================================================== +uint32_t Description::type() const +{ + return task_type.type; +} + +namespace description { + +//============================================================================== +std::shared_ptr Delivery::make( + rmf_traffic::Time start_time, + std::string pickup_place_name, + std::string pickup_dispenser, + std::string dropoff_place_name, + std::string dropoff_ingestor, + std::vector items) +{ + if (pickup_place_name.empty() || pickup_dispenser.empty() || + dropoff_place_name.empty() || dropoff_ingestor.empty()) + return nullptr; + + auto delivery_desc = std::make_shared(); + + delivery_desc->task_type.type = rmf_task_msgs::msg::TaskType::TYPE_DELIVERY; + delivery_desc->_start_time = std::move(start_time); + delivery_desc->_pickup_place_name = std::move(pickup_place_name); + delivery_desc->pickup_dispenser = std::move(pickup_dispenser); + delivery_desc->_dropoff_place_name = std::move(dropoff_place_name); + delivery_desc->dropoff_ingestor = std::move(dropoff_ingestor); + delivery_desc->items = std::move(items); + + return delivery_desc; +} + +//============================================================================== +std::shared_ptr Delivery::make_from_msg( + TaskDescription msg) +{ + if (msg.task_type.type != rmf_task_msgs::msg::TaskType::TYPE_DELIVERY) + return nullptr; + + return make( + rmf_traffic_ros2::convert(msg.start_time), + msg.delivery.pickup_place_name, + msg.delivery.pickup_dispenser, + msg.delivery.dropoff_place_name, + msg.delivery.dropoff_ingestor, + msg.delivery.items); +} + +//============================================================================== +rmf_task_msgs::msg::TaskDescription Delivery::to_msg() const +{ + rmf_task_msgs::msg::TaskDescription msg; + msg.task_type = task_type; + msg.start_time = rmf_traffic_ros2::convert(_start_time); + msg.delivery.pickup_place_name = _pickup_place_name; + msg.delivery.dropoff_place_name = _dropoff_place_name; + msg.delivery.pickup_dispenser = pickup_dispenser; + msg.delivery.dropoff_ingestor = dropoff_ingestor; + return msg; +} + +//============================================================================== +const std::string& Delivery::pickup_place_name() const +{ + return _pickup_place_name; +} + +//============================================================================== +const std::string& Delivery::dropoff_place_name() const +{ + return _dropoff_place_name; +} + +//============================================================================== +//============================================================================== +std::shared_ptr Loop::make( + rmf_traffic::Time start_time, + std::string start_name, + std::string finish_name, + std::size_t num_loops) +{ + if (start_name.empty() || finish_name.empty() || num_loops == 0) + return nullptr; + + auto loop_desc = std::make_shared(); + loop_desc->task_type.type = rmf_task_msgs::msg::TaskType::TYPE_LOOP; + loop_desc->_start_time = std::move(start_time); + loop_desc->_start_name = std::move(start_name); + loop_desc->_finish_name = std::move(finish_name); + loop_desc->num_loops = num_loops; + return loop_desc; +} + +//============================================================================== +std::shared_ptr Loop::make_from_msg(TaskDescription msg) +{ + if (msg.task_type.type != rmf_task_msgs::msg::TaskType::TYPE_LOOP) + return nullptr; + + return make( + rmf_traffic_ros2::convert(msg.start_time), + msg.loop.start_name, + msg.loop.finish_name, + msg.loop.num_loops); +} + +//============================================================================== +rmf_task_msgs::msg::TaskDescription Loop::to_msg() const +{ + TaskDescription msg; + msg.task_type = task_type; + msg.start_time = rmf_traffic_ros2::convert(_start_time); + msg.loop.start_name = _start_name; + msg.loop.finish_name = _finish_name; + return msg; +} + +//============================================================================== +const std::string& Loop::start_name() const +{ + return this->_start_name; +} + +//============================================================================== +const std::string& Loop::finish_name() const +{ + return this->_finish_name; +} + +//============================================================================== +//============================================================================== +std::shared_ptr Clean::make( + rmf_traffic::Time start_time, + std::string start_waypoint) +{ + if (start_waypoint.empty()) + return nullptr; + + auto clean_desc = std::make_shared(); + clean_desc->task_type.type = rmf_task_msgs::msg::TaskType::TYPE_CLEAN; + clean_desc->_start_time = std::move(start_time); + clean_desc->_start_waypoint = std::move(start_waypoint); + return clean_desc; +} + +//============================================================================== +std::shared_ptr Clean::make_from_msg(TaskDescription msg) +{ + if (msg.task_type.type != rmf_task_msgs::msg::TaskType::TYPE_CLEAN) + return nullptr; + + return make( + rmf_traffic_ros2::convert(msg.start_time), + msg.clean.start_waypoint); +} + +//============================================================================== +rmf_task_msgs::msg::TaskDescription Clean::to_msg() const +{ + TaskDescription msg; + msg.task_type = task_type; + msg.start_time = rmf_traffic_ros2::convert(_start_time); + msg.clean.start_waypoint = _start_waypoint; + return msg; +} + +//============================================================================== +const std::string& Clean::start_waypoint() const +{ + return this->_start_waypoint; +} + + + +} // namespace description +} // namespace rmf_task_ros2 + diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp index ff40ba249..74064a889 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp @@ -409,6 +409,13 @@ std::optional Dispatcher::submit_task( return _pimpl->submit_task(task_description); } +//============================================================================== +std::optional Dispatcher::submit_task( + const Description::ConstDescriptionPtr task_description) +{ + return _pimpl->submit_task(task_description->to_msg()); +} + //============================================================================== bool Dispatcher::cancel_task(const TaskID& task_id) { diff --git a/rmf_task_ros2/test/dispatcher/test_Description.cpp b/rmf_task_ros2/test/dispatcher/test_Description.cpp new file mode 100644 index 000000000..e5eb65bd4 --- /dev/null +++ b/rmf_task_ros2/test/dispatcher/test_Description.cpp @@ -0,0 +1,72 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include +#include + +namespace rmf_task_ros2 { + +//============================================================================== +SCENARIO("Task Description Test", "[Description]") +{ + using TaskDescription = rmf_task_msgs::msg::TaskDescription; + using TaskType = rmf_task_msgs::msg::TaskType; + + const auto now = std::chrono::steady_clock::now(); + + // test delivery description msg + auto delivery = description::Delivery::make( + now, "pick", "dis", "drop", "ing", {}); + + std::cout << "This is the delivery out msg: " + << delivery->pickup_place_name() + << " to " + << delivery->dropoff_place_name() + << std::endl; + + Execution::execute_task(delivery); + + REQUIRE(delivery->type() == TaskType::TYPE_DELIVERY); + TaskDescription msg; + auto delivery2 = description::Delivery::make_from_msg(msg); + REQUIRE(!delivery2); + +//============================================================================== + // test loop descripttion msg + auto loop = description::Loop::make(now, "start_yo", "end_yo", 1); + REQUIRE(loop->type() == TaskType::TYPE_LOOP); + + auto d_loop = std::dynamic_pointer_cast(loop); + std::cout << "This is the loop out msg: " + << loop->start_name() + << " to " + << loop->finish_name() + << std::endl; + auto loop2 = description::Loop::make_from_msg(msg); + REQUIRE(!loop2); + Execution::execute_task(loop); + +//============================================================================== + // test clean descripttion msg + auto clean = description::Clean::make(now, "clean_here"); + REQUIRE(clean->type() == TaskType::TYPE_CLEAN); + auto clean2 = description::Loop::make_from_msg(msg); + REQUIRE(!clean2); +} + +} // namespace rmf_task_ros2 From d659203266c1b3b4e8d09e97b59b21851b9c99c5 Mon Sep 17 00:00:00 2001 From: youliang Date: Tue, 16 Feb 2021 12:57:49 +0800 Subject: [PATCH 03/10] fix canceltask failed during bidding --- .../include/rmf_task_ros2/Description.hpp | 2 -- rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp | 8 +++++++- .../test/dispatcher/test_Description.cpp | 6 ++++-- .../test/dispatcher/test_Dispatcher.cpp | 18 +++++++++++++----- 4 files changed, 24 insertions(+), 10 deletions(-) diff --git a/rmf_task_ros2/include/rmf_task_ros2/Description.hpp b/rmf_task_ros2/include/rmf_task_ros2/Description.hpp index 40f17a631..4fcbafda0 100644 --- a/rmf_task_ros2/include/rmf_task_ros2/Description.hpp +++ b/rmf_task_ros2/include/rmf_task_ros2/Description.hpp @@ -39,8 +39,6 @@ class Description rmf_traffic::Time start_time() const; - ConstRequestPtr request() = nullptr; - uint32_t type() const; virtual TaskDescription to_msg() const = 0; diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp index 74064a889..9c08230e1 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp @@ -189,11 +189,16 @@ class Dispatcher::Implementation if (cancel_task_status->state == TaskStatus::State::Pending) { cancel_task_status->state = TaskStatus::State::Canceled; + terminate_task(cancel_task_status); if (on_change_fn) on_change_fn(cancel_task_status); + queue_bidding_tasks.pop(); + if (!queue_bidding_tasks.empty()) + auctioneer->start_bidding(queue_bidding_tasks.front()); + return true; } @@ -356,7 +361,8 @@ class Dispatcher::Implementation } // check if there's a change in state for the previous completed bidding task - // TODO, better way to impl this + // This ensures that the next task will be executed after receiving ack msg + // TODO(YL), better way to impl this if (!queue_bidding_tasks.empty() && id == queue_bidding_tasks.front().task_profile.task_id) { diff --git a/rmf_task_ros2/test/dispatcher/test_Description.cpp b/rmf_task_ros2/test/dispatcher/test_Description.cpp index e5eb65bd4..09c16ce90 100644 --- a/rmf_task_ros2/test/dispatcher/test_Description.cpp +++ b/rmf_task_ros2/test/dispatcher/test_Description.cpp @@ -24,6 +24,8 @@ namespace rmf_task_ros2 { //============================================================================== SCENARIO("Task Description Test", "[Description]") { + std::cout << "Testing Description Task Type"<< std::cout; + using TaskDescription = rmf_task_msgs::msg::TaskDescription; using TaskType = rmf_task_msgs::msg::TaskType; @@ -33,7 +35,7 @@ SCENARIO("Task Description Test", "[Description]") auto delivery = description::Delivery::make( now, "pick", "dis", "drop", "ing", {}); - std::cout << "This is the delivery out msg: " + std::cout << "[test description] Delivery out msg: " << delivery->pickup_place_name() << " to " << delivery->dropoff_place_name() @@ -52,7 +54,7 @@ SCENARIO("Task Description Test", "[Description]") REQUIRE(loop->type() == TaskType::TYPE_LOOP); auto d_loop = std::dynamic_pointer_cast(loop); - std::cout << "This is the loop out msg: " + std::cout << "[test description] Loop out msg: " << loop->start_name() << " to " << loop->finish_name() diff --git a/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp b/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp index ca1268127..4fc9adad5 100644 --- a/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp +++ b/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp @@ -68,18 +68,26 @@ SCENARIO("Dispatcehr API Test", "[Dispatcher]") REQUIRE(get_tasks_client->wait_for_service(std::chrono::milliseconds(0))); } - WHEN("Add 1 and cancel task") + WHEN("Add 2 tasks and cancel 1 task") { - // add task + // add 2 tasks const auto id = dispatcher->submit_task(task_desc1); - REQUIRE(dispatcher->active_tasks().size() == 1); + const auto id2 = dispatcher->submit_task(task_desc2); + REQUIRE(dispatcher->active_tasks().size() == 2); REQUIRE(dispatcher->terminated_tasks().size() == 0); REQUIRE(dispatcher->get_task_state(*id) == TaskStatus::State::Pending); + REQUIRE(dispatcher->get_task_state(*id2) == TaskStatus::State::Pending); - // cancel task + // cancel task during bidding REQUIRE(dispatcher->cancel_task(*id)); - REQUIRE(dispatcher->active_tasks().size() == 0); + REQUIRE(dispatcher->active_tasks().size() == 1); REQUIRE(dispatcher->terminated_tasks().size() == 1); + REQUIRE(dispatcher->get_task_state(*id) == TaskStatus::State::Canceled); + + // wait task2 to fail due to no submissions from F.Adapters + std::this_thread::sleep_for(std::chrono::milliseconds(5500)); + REQUIRE(dispatcher->get_task_state(*id2) == TaskStatus::State::Failed); + REQUIRE(dispatcher->terminated_tasks().size() == 2); // check random id REQUIRE(!(dispatcher->get_task_state("non_existence_id"))); From 17f4220a13b1370275837ff7dee3c636b3854ab1 Mon Sep 17 00:00:00 2001 From: youliang Date: Tue, 23 Feb 2021 15:42:47 +0800 Subject: [PATCH 04/10] apply task description in fleet adapter, along with refactoring --- rmf_fleet_adapter/CMakeLists.txt | 1 + rmf_fleet_adapter/package.xml | 2 +- .../src/rmf_fleet_adapter/Task.cpp | 10 + .../src/rmf_fleet_adapter/Task.hpp | 9 + .../src/rmf_fleet_adapter/TaskManager.cpp | 168 +++--------- .../src/rmf_fleet_adapter/TaskManager.hpp | 22 +- .../agv/FleetUpdateHandle.cpp | 250 +++++++++++------- .../agv/internal_FleetUpdateHandle.hpp | 25 +- .../rmf_fleet_adapter/phases/GoToPlace.cpp | 1 + .../rmf_fleet_adapter/tasks/ChargeBattery.cpp | 2 + .../rmf_fleet_adapter/tasks/ChargeBattery.hpp | 1 + .../src/rmf_fleet_adapter/tasks/Clean.cpp | 2 + .../src/rmf_fleet_adapter/tasks/Clean.hpp | 1 + .../src/rmf_fleet_adapter/tasks/Delivery.cpp | 2 + .../src/rmf_fleet_adapter/tasks/Delivery.hpp | 1 + .../src/rmf_fleet_adapter/tasks/Loop.cpp | 2 + .../src/rmf_fleet_adapter/tasks/Loop.hpp | 1 + rmf_fleet_adapter/test/test_Task.cpp | 6 +- .../include/rmf_task_ros2/Description.hpp | 21 +- .../include/rmf_task_ros2/Dispatcher.hpp | 2 +- .../include/rmf_task_ros2/TaskStatus.hpp | 9 +- .../src/rmf_task_ros2/Description.cpp | 23 +- .../src/rmf_task_ros2/Dispatcher.cpp | 2 +- .../test/dispatcher/test_Description.cpp | 5 +- 24 files changed, 295 insertions(+), 273 deletions(-) diff --git a/rmf_fleet_adapter/CMakeLists.txt b/rmf_fleet_adapter/CMakeLists.txt index b2bbab1e6..b95f4b8dd 100644 --- a/rmf_fleet_adapter/CMakeLists.txt +++ b/rmf_fleet_adapter/CMakeLists.txt @@ -40,6 +40,7 @@ set(dep_pkgs rmf_battery rmf_task rmf_task_ros2 + rmf_fleet_adapter std_msgs ) foreach(pkg ${dep_pkgs}) diff --git a/rmf_fleet_adapter/package.xml b/rmf_fleet_adapter/package.xml index fad1cd873..2551cd840 100644 --- a/rmf_fleet_adapter/package.xml +++ b/rmf_fleet_adapter/package.xml @@ -23,6 +23,7 @@ rmf_traffic_msgs rmf_battery rmf_task + rmf_task_ros2 eigen yaml-cpp @@ -34,4 +35,3 @@ ament_cmake - diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp index f2b31919a..e05116956 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp @@ -29,6 +29,7 @@ namespace rmf_fleet_adapter { //============================================================================== std::shared_ptr Task::make( std::string id, + rmf_task_ros2::ConstDescriptionPtr description, PendingPhases phases, rxcpp::schedulers::worker worker, rmf_traffic::Time deployment_time, @@ -37,6 +38,7 @@ std::shared_ptr Task::make( { return std::make_shared( Task(std::move(id), + std::move(description), std::move(phases), std::move(worker), deployment_time, @@ -88,6 +90,12 @@ const std::string& Task::id() const return _id; } +//============================================================================== +const rmf_task_ros2::ConstDescriptionPtr Task::description() const +{ + return _description; +} + //============================================================================== const rmf_task::ConstRequestPtr Task::request() const { @@ -109,12 +117,14 @@ const rmf_task::agv::State Task::finish_state() const //============================================================================== Task::Task( std::string id, + rmf_task_ros2::ConstDescriptionPtr description, std::vector> phases, rxcpp::schedulers::worker worker, rmf_traffic::Time deployment_time, rmf_task::agv::State finish_state, rmf_task::ConstRequestPtr request) : _id(std::move(id)), + _description(std::move(description)), _pending_phases(std::move(phases)), _worker(std::move(worker)), _deployment_time(deployment_time), diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp index d0850e042..3e565e2a4 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp @@ -28,6 +28,7 @@ #include #include +#include #include #include @@ -99,6 +100,7 @@ class Task : public std::enable_shared_from_this // Make a new task static std::shared_ptr make( std::string id, + rmf_task_ros2::ConstDescriptionPtr description, PendingPhases phases, rxcpp::schedulers::worker worker, rmf_traffic::Time deployment_time, @@ -124,6 +126,11 @@ class Task : public std::enable_shared_from_this const std::string& id() const; + // const rmf_task_ros2::msg::TaskProfile profile() const; + + /// Task description + const rmf_task_ros2::ConstDescriptionPtr description() const; + /// Get the request used to generate this task const rmf_task::ConstRequestPtr request() const; @@ -137,6 +144,7 @@ class Task : public std::enable_shared_from_this Task( std::string id, + rmf_task_ros2::ConstDescriptionPtr description, PendingPhases phases, rxcpp::schedulers::worker worker, rmf_traffic::Time deployment_time, @@ -162,6 +170,7 @@ class Task : public std::enable_shared_from_this rmf_traffic::Time _deployment_time; rmf_task::agv::State _finish_state; rmf_task::ConstRequestPtr _request; + rmf_task_ros2::ConstDescriptionPtr _description = nullptr; void _start_next_phase(); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 8d3dcc003..c588d8d8c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -82,40 +82,32 @@ TaskManager::TaskManager(agv::RobotContextPtr context) } //============================================================================== -void TaskManager::queue_task(std::shared_ptr task, Start expected_finish) +void TaskManager::queue_task(std::shared_ptr task) { - std::lock_guard guard(_mutex); - _queue.push_back(std::move(task)); - _expected_finish_location = std::move(expected_finish); + { + std::lock_guard guard(_mutex); + _queue.push_back(task); RCLCPP_INFO( _context->node()->get_logger(), "Queuing new task [%s] for [%s]. New queue size: %d", - _queue.back()->id().c_str(), _context->requester_id().c_str(), + task->id().c_str(), _context->requester_id().c_str(), _queue.size()); - if (!_active_task) - { - _begin_next_task(); - } - else - { rmf_task_msgs::msg::TaskSummary msg; - msg.task_id = _queue.back()->id(); - msg.task_profile.task_id = _queue.back()->id(); + msg.task_id = task->id(); // duplicated + msg.task_profile.task_id = task->id(); + msg.task_profile.description = task->description()->to_msg(); msg.state = msg.STATE_QUEUED; msg.robot_name = _context->name(); + msg.fleet_name = _context->description().owner(); + msg.start_time = rmf_traffic_ros2::convert(task->deployment_time()); + msg.end_time = rmf_traffic_ros2::convert(task->finish_state().finish_time()); this->_context->node()->task_summary()->publish(msg); } -} -//============================================================================== -auto TaskManager::expected_finish_location() const -> StartSet -{ - if (_expected_finish_location) - return {*_expected_finish_location}; - - return _context->location(); + if (!_active_task) + _begin_next_task(); } //============================================================================== @@ -151,119 +143,23 @@ const Task* TaskManager::current_task() const } //============================================================================== -agv::ConstRobotContextPtr TaskManager::context() const +const std::vector> TaskManager::task_queue() const { - return _context; + return _queue; } //============================================================================== void TaskManager::set_queue( - const std::vector& assignments) + const std::vector>& tasks) { - // We indent this block as _mutex is also locked in the _begin_next_task() - // function that is called at the end of this function. - { - std::lock_guard guard(_mutex); - _queue.clear(); - - // We use dynamic cast to determine the type of request and then call the - // appropriate make(~) function to convert the request into a task - for (std::size_t i = 0; i < assignments.size(); ++i) - { - const auto& a = assignments[i]; - auto start = _context->current_task_end_state().location(); - if (i != 0) - start = assignments[i-1].state().location(); - start.time(a.deployment_time()); - rmf_task_msgs::msg::TaskType task_type_msg; - - if (const auto request = - std::dynamic_pointer_cast(a.request())) - { - task_type_msg.type = task_type_msg.TYPE_CLEAN; - auto task = rmf_fleet_adapter::tasks::make_clean( - request, - _context, - start, - a.deployment_time(), - a.state()); - - _queue.push_back(task); - } - - else if (const auto request = - std::dynamic_pointer_cast( - a.request())) - { - task_type_msg.type = task_type_msg.TYPE_CHARGE_BATTERY; - const auto task = tasks::make_charge_battery( - request, - _context, - start, - a.deployment_time(), - a.state()); - - _queue.push_back(task); - } - - else if (const auto request = - std::dynamic_pointer_cast( - a.request())) - { - task_type_msg.type = task_type_msg.TYPE_DELIVERY; - const auto task = tasks::make_delivery( - request, - _context, - start, - a.deployment_time(), - a.state()); - - _queue.push_back(task); - } - - else if (const auto request = - std::dynamic_pointer_cast(a.request())) - { - task_type_msg.type = task_type_msg.TYPE_LOOP; - const auto task = tasks::make_loop( - request, - _context, - start, - a.deployment_time(), - a.state()); - - _queue.push_back(task); - } - - else - { - RCLCPP_WARN( - _context->node()->get_logger(), - "[TaskManager] Un-supported request type in assignment list. " - "Please update the implementation of TaskManager::set_queue() to " - "support request with task_id:[%s]", - a.request()->id().c_str()); - - continue; - } - - // publish queued 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(); - msg.fleet_name = _context->description().owner(); - msg.task_profile.description.task_type = task_type_msg; - msg.start_time = rmf_traffic_ros2::convert( - _queue.back()->deployment_time()); - msg.start_time = rmf_traffic_ros2::convert( - _queue.back()->finish_state().finish_time()); - this->_context->node()->task_summary()->publish(msg); - } - } + for (const auto t : tasks) + queue_task(t); +} - _begin_next_task(); +//============================================================================== +agv::ConstRobotContextPtr TaskManager::context() const +{ + return _context; } //============================================================================== @@ -327,6 +223,7 @@ void TaskManager::_begin_next_task() { msg.task_id = id; msg.task_profile.task_id = id; + msg.task_profile.description = _active_task->description()->to_msg(); msg.robot_name = _context->name(); msg.fleet_name = _context->description().owner(); msg.start_time = rmf_traffic_ros2::convert( @@ -349,6 +246,7 @@ void TaskManager::_begin_next_task() msg.task_id = id; msg.task_profile.task_id = id; + msg.task_profile.description = _active_task->description()->to_msg(); msg.robot_name = _context->name(); msg.fleet_name = _context->description().owner(); msg.start_time = rmf_traffic_ros2::convert( @@ -362,6 +260,7 @@ void TaskManager::_begin_next_task() rmf_task_msgs::msg::TaskSummary msg; msg.task_id = id; msg.task_profile.task_id = id; + msg.task_profile.description = _active_task->description()->to_msg(); msg.state = msg.STATE_COMPLETED; msg.robot_name = _context->name(); msg.fleet_name = _context->description().owner(); @@ -474,7 +373,20 @@ void TaskManager::retreat_to_charger() finish.value().finish_state(), current_state.finish_time()); - set_queue({charging_assignment}); + const auto task_desc = rmf_task_ros2::Description::make_description( + charging_assignment.deployment_time(), + rmf_task_msgs::msg::TaskType::TYPE_CHARGE_BATTERY); + + // TODO(YL) Check time input + const auto task = rmf_fleet_adapter::tasks::make_charge_battery( + task_desc, + std::dynamic_pointer_cast(charging_request), + _context, + current_state.location(), + charging_assignment.deployment_time(), + charging_assignment.state()); + + queue_task(task); RCLCPP_INFO( _context->node()->get_logger(), diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp index 965dc512a..c25b34cf4 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp @@ -47,11 +47,7 @@ class TaskManager : public std::enable_shared_from_this using State = rmf_task::agv::State; /// Add a task to the queue of this manager. - void queue_task(std::shared_ptr task, Start expected_finish); - - /// The location where we expect this robot to be at the end of its current - /// task queue. - StartSet expected_finish_location() const; + void queue_task(std::shared_ptr task); const agv::RobotContextPtr& context(); @@ -59,20 +55,27 @@ class TaskManager : public std::enable_shared_from_this const Task* current_task() const; - /// Set the queue for this task manager with assignments generated from the - /// task planner - void set_queue(const std::vector& assignments); + // /// Set the queue for this task manager with assignments generated from the + // /// task planner + // void set_queue(const std::vector& assignments); + + /// set a vector of tasks + void set_queue(const std::vector>& tasks); + + // get tasks in the queue + const std::vector> task_queue() const; /// Get the non-charging requests among pending tasks const std::vector requests() const; - /// The state of the robot. + /// The finish state of the current task. State expected_finish_state() const; /// Callback for the retreat timer. Appends a charging task to the task queue /// when robot is idle and battery level drops below a retreat threshold. void retreat_to_charger(); + /// TODO(YL) not needed, should be removed? /// Get the list of task ids for tasks that have started execution. /// The list will contain upto 100 latest task ids only. const std::vector& get_executed_tasks() const; @@ -84,7 +87,6 @@ class TaskManager : public std::enable_shared_from_this agv::RobotContextPtr _context; std::shared_ptr _active_task; std::vector> _queue; - rmf_utils::optional _expected_finish_location; rxcpp::subscription _task_sub; rxcpp::subscription _emergency_sub; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index b9b8c87d5..209d65270 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -38,6 +38,11 @@ #include +#include "../tasks/Clean.hpp" +#include "../tasks/ChargeBattery.hpp" +#include "../tasks/Delivery.hpp" +#include "../tasks/Loop.hpp" + #include #include #include @@ -108,9 +113,16 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( if (msg->task_profile.task_id.empty()) return; - if (bid_notice_assignments.find(msg->task_profile.task_id) - != bid_notice_assignments.end()) + if (task_descriptions.find(msg->task_profile.task_id) + != task_descriptions.end()) + { + RCLCPP_WARN( + node->get_logger(), + "Task [%s] has been submitted by Fleet [%s] previously", + msg->task_profile.task_id.c_str(), + name.c_str()); return; + } if (!accept_task) { @@ -156,10 +168,10 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( // TODO (YV) get rid of ID field in RequestPtr const std::string id = msg->task_profile.task_id; const auto& graph = planner->get_configuration().graph(); - rmf_task_ros2::Description::ConstDescriptionPtr task_description; + rmf_task_ros2::ConstDescriptionPtr task_description; // Process Cleaning task - if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_CLEAN) + if (task_type.type == TaskType::TYPE_CLEAN) { const auto& clean = rmf_task_ros2::description::Clean::make_from_msg(description_msg); @@ -251,7 +263,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( task_description = clean; } - else if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_DELIVERY) + else if (task_type.type == TaskType::TYPE_DELIVERY) { const auto& delivery = rmf_task_ros2::description::Delivery::make_from_msg(description_msg); @@ -305,7 +317,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( task_description = delivery; } - else if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_LOOP) + else if (task_type.type == TaskType::TYPE_LOOP) { const auto& loop = rmf_task_ros2::description::Loop::make_from_msg(description_msg); @@ -356,7 +368,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( RCLCPP_INFO( node->get_logger(), "Generated Loop request for task_id:[%s]", id.c_str()); - + task_description = loop; } else @@ -372,7 +384,6 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( if (!new_request) return; - generated_requests.insert({id, new_request}); const auto allocation_result = allocate_tasks(new_request); @@ -418,7 +429,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( std::size_t index = 0; for (const auto& t : task_managers) { - robot_name_map.insert({index, t.first->name()}); + robot_name_map.insert({index, t->context()->name()}); ++index; } @@ -446,8 +457,8 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( id.c_str(), bid_proposal.robot_name.c_str(), cost); // Store assignments in internal map - bid_notice_assignments.insert({id, assignments}); - + latest_bid_notice_assignments = {id, new_request, assignments}; + task_descriptions.insert({id, task_description}); } //============================================================================== @@ -464,8 +475,8 @@ void FleetUpdateHandle::Implementation::dispatch_request_cb( if (msg->method == DispatchRequest::ADD) { - const auto task_it = bid_notice_assignments.find(id); - if (task_it == bid_notice_assignments.end()) + auto [ bid_notice_id, req_ptr, assignments] = latest_bid_notice_assignments; + if (id != bid_notice_id) { RCLCPP_WARN( node->get_logger(), @@ -479,9 +490,7 @@ void FleetUpdateHandle::Implementation::dispatch_request_cb( node->get_logger(), "Bid for task_id:[%s] awarded to fleet [%s]. Processing request...", id.c_str(), name.c_str()); - - auto& assignments = task_it->second; - + if (assignments.size() != task_managers.size()) { RCLCPP_ERROR( @@ -492,28 +501,13 @@ void FleetUpdateHandle::Implementation::dispatch_request_cb( return; } - // Here we make sure none of the tasks in the assignments has already begun - // execution. If so, we replan assignments until a valid set is obtained - // and only then update the task manager queues - const auto request_it = generated_requests.find(id); - if (request_it == generated_requests.end()) - { - RCLCPP_ERROR( - node->get_logger(), - "Unable to find generated request for task_id:[%s]. This request will " - "be ignored.", - id.c_str()); - dispatch_ack_pub->publish(dispatch_ack); - return; - } - bool valid_assignments = is_valid_assignments(assignments); if (!valid_assignments) { // TODO: This replanning is blocking the main thread. Instead, the // replanning should run on a separate worker and then deliver the // result back to the main worker. - const auto replan_results = allocate_tasks(request_it->second); + const auto replan_results = allocate_tasks(req_ptr); if (!replan_results) { RCLCPP_WARN( @@ -530,15 +524,8 @@ void FleetUpdateHandle::Implementation::dispatch_request_cb( // rxcpp worker. Hence, no new tasks would have started during this replanning. } - std::size_t index = 0; - for (auto& t : task_managers) - { - t.second->set_queue(assignments[index]); - ++index; - } - + set_assignments_to_task_managers(assignments); current_assignment_cost = task_planner->compute_cost(assignments); - assigned_requests.insert({id, request_it->second}); dispatch_ack.success = true; dispatch_ack_pub->publish(dispatch_ack); @@ -568,8 +555,17 @@ void FleetUpdateHandle::Implementation::dispatch_request_cb( return; } - auto request_to_cancel_it = assigned_requests.find(id); - if (request_to_cancel_it == assigned_requests.end()) + std::unordered_set executed_tasks; + std::unordered_set assigned_tasks; + for (const auto& mgr : task_managers) + { + if (mgr->current_task()) + executed_tasks.insert(mgr->current_task()->id()); + for (const auto& tsk : mgr->task_queue()) + assigned_tasks.insert(tsk->id()); + } + + if (assigned_tasks.find(id) != assigned_tasks.end()) { RCLCPP_WARN( node->get_logger(), @@ -581,13 +577,6 @@ void FleetUpdateHandle::Implementation::dispatch_request_cb( return; } - std::unordered_set executed_tasks; - for (const auto& [context, mgr] : task_managers) - { - const auto& tasks = mgr->get_executed_tasks(); - executed_tasks.insert(tasks.begin(), tasks.end()); - } - // Check if received request is to cancel an active task if (executed_tasks.find(id) != executed_tasks.end()) { @@ -602,9 +591,8 @@ void FleetUpdateHandle::Implementation::dispatch_request_cb( } // Re-plan assignments while ignoring request for task to be cancelled - const auto replan_results = allocate_tasks( - nullptr, request_to_cancel_it->second); - + const auto replan_results = allocate_tasks(nullptr, id); + if (!replan_results.has_value()) { RCLCPP_WARN( @@ -617,13 +605,7 @@ void FleetUpdateHandle::Implementation::dispatch_request_cb( } const auto& assignments = replan_results.value(); - std::size_t index = 0; - for (auto& t : task_managers) - { - t.second->set_queue(assignments[index]); - ++index; - } - + set_assignments_to_task_managers(assignments); current_assignment_cost = task_planner->compute_cost(assignments); dispatch_ack.success = true; @@ -654,10 +636,13 @@ auto FleetUpdateHandle::Implementation::is_valid_assignments( Assignments& assignments) const -> bool { std::unordered_set executed_tasks; - for (const auto& [context, mgr] : task_managers) + for (const auto& mgr : task_managers) { - const auto& tasks = mgr->get_executed_tasks(); - executed_tasks.insert(tasks.begin(), tasks.end()); + // TODO (YL) check if logic is correct + // const auto& tasks = mgr->get_executed_tasks(); + // executed_tasks.insert(tasks.begin(), tasks.end()); + if(mgr->current_task()) + executed_tasks.insert(mgr->current_task()->id()); } for (const auto& agent : assignments) @@ -668,7 +653,6 @@ auto FleetUpdateHandle::Implementation::is_valid_assignments( return false; } } - return true; } @@ -781,7 +765,7 @@ rmf_fleet_msgs::msg::RobotState convert_state(const TaskManager& mgr) void FleetUpdateHandle::Implementation::publish_fleet_state() const { std::vector robot_states; - for (const auto& [context, mgr] : task_managers) + for (const auto& mgr : task_managers) robot_states.emplace_back(convert_state(*mgr)); auto fleet_state = rmf_fleet_msgs::build() @@ -794,7 +778,7 @@ void FleetUpdateHandle::Implementation::publish_fleet_state() const //============================================================================== auto FleetUpdateHandle::Implementation::allocate_tasks( rmf_task::ConstRequestPtr new_request, - rmf_task::ConstRequestPtr ignore_request) const -> std::optional + std::string ignore_request_id) const -> std::optional { // Collate robot states, constraints and combine new requestptr with // requestptr of non-charging tasks in task manager queues @@ -811,42 +795,33 @@ auto FleetUpdateHandle::Implementation::allocate_tasks( for (const auto& t : task_managers) { - states.push_back(t.second->expected_finish_state()); - constraints_set.push_back(t.first->task_planning_constraints()); - const auto requests = t.second->requests(); - pending_requests.insert( - pending_requests.end(), requests.begin(), requests.end()); - } - - // Remove the request to be ignored if present - if (ignore_request) - { - auto ignore_request_it = pending_requests.end(); - for (auto it = pending_requests.begin(); it != pending_requests.end(); ++it) - { - auto pending_request = *it; - if (pending_request->id() == ignore_request->id()) - ignore_request_it = it; - } - if (ignore_request_it != pending_requests.end()) - { - pending_requests.erase(ignore_request_it); - RCLCPP_INFO( - node->get_logger(), - "Request with task_id:[%s] will be ignored during task allocation.", - ignore_request->id().c_str()); - } - else + states.push_back(t->expected_finish_state()); + constraints_set.push_back(t->context()->task_planning_constraints()); + + for (const auto task : t->task_queue()) { - RCLCPP_WARN( - node->get_logger(), - "Request with task_id:[%s] is not present in any of the task queues.", - ignore_request->id().c_str()); + if (task->description()->type() == TaskType::TYPE_CHARGE_BATTERY) + { + // ignore auto allocated charging task + continue; + } + else if (task->id() == ignore_request_id ) + { + RCLCPP_INFO( + node->get_logger(), + "Request with task_id:[%s] will be ignored during task allocation.", + ignore_request_id.c_str()); + continue; + } + else + { + pending_requests.push_back(task->request()); + } } } RCLCPP_INFO( - node->get_logger(), + node->get_logger(), "Planning for [%d] robot(s) and [%d] request(s)", states.size(), pending_requests.size()); @@ -874,7 +849,6 @@ auto FleetUpdateHandle::Implementation::allocate_tasks( " insufficient initial battery charge for all robots in this fleet.", id.c_str()); } - else if (*error == rmf_task::agv::TaskPlanner::TaskPlannerError::limited_capacity) { @@ -884,7 +858,6 @@ auto FleetUpdateHandle::Implementation::allocate_tasks( " insufficient battery capacity to accommodate one or more requests by" " any of the robots in this fleet.", id.c_str()); } - else { RCLCPP_ERROR( @@ -892,7 +865,6 @@ auto FleetUpdateHandle::Implementation::allocate_tasks( "[TaskPlanner] Failed to compute assignments for task_id:[%s]", id.c_str()); } - return std::nullopt; } @@ -911,6 +883,84 @@ auto FleetUpdateHandle::Implementation::allocate_tasks( return assignments; } +//============================================================================== +void FleetUpdateHandle::Implementation::set_assignments_to_task_managers( + const Assignments& assignments) +{ + assert(assignments.size() == task_managers.size()); + std::size_t tm_index = 0; + + for (auto& t : task_managers) + { + std::vector> task_queue; + std::shared_ptr task = nullptr; + for (std::size_t i = 0; i < assignments[tm_index].size(); ++i) + { + const auto& a = assignments[tm_index][i]; + auto start = t->context()->current_task_end_state().location(); + if (i != 0) + start = assignments[tm_index][i-1].state().location(); + start.time(a.deployment_time()); + + const auto id = a.request()->id(); + + rmf_task_ros2::ConstDescriptionPtr desc; + if (task_descriptions.find(id) != task_descriptions.end()) + { + desc = task_descriptions[id]; + } + else + { + desc = rmf_task_ros2::Description::make_description( + a.deployment_time(), TaskType::TYPE_CHARGE_BATTERY); + } + + using namespace rmf_task::requests; + /// CLEAN TASK + if ( const auto req = + std::dynamic_pointer_cast(a.request())) + { + task = tasks::make_clean( + desc, req, t->context(), start, a.deployment_time(), a.state()); + } + /// CHARGE BATTERY TASK + else if ( const auto req = + std::dynamic_pointer_cast(a.request())) + { + task = tasks::make_charge_battery( + desc, req, t->context(), start, a.deployment_time(), a.state()); + } + /// DELIVERY TASK + else if ( const auto req = + std::dynamic_pointer_cast(a.request())) + { + task = tasks::make_delivery( + desc, req, t->context(), start, a.deployment_time(), a.state()); + } + /// LOOP TASK + else if ( const auto req = + std::dynamic_pointer_cast(a.request())) + { + task = tasks::make_loop( + desc, req, t->context(), start, a.deployment_time(), a.state()); + } + else + { + RCLCPP_WARN( + t->context()->node()->get_logger(), + "[TaskManager] Un-supported request type in assignment list. " + "Please update the implementation of TaskManager::set_queue() to " + "support request with task_id:[%s]", id); + continue; + } + + task_queue.push_back(task); + } + t->set_queue(task_queue); + ++tm_index; + } +} + //============================================================================== void FleetUpdateHandle::add_robot( std::shared_ptr command, @@ -980,7 +1030,7 @@ void FleetUpdateHandle::add_robot( "Added a robot named [%s] with participant ID [%d]", context->name().c_str(), context->itinerary().id()); - fleet->_pimpl->task_managers.insert({context, TaskManager::make(context)}); + fleet->_pimpl->task_managers.push_back(TaskManager::make(context)); if (handle_cb) { handle_cb(RobotUpdateHandle::Implementation::make(std::move(context))); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp index b901c6f34..1a272f44d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp @@ -150,8 +150,9 @@ class FleetUpdateHandle::Implementation std::chrono::nanoseconds(std::chrono::seconds(10)); AcceptDeliveryRequest accept_delivery = nullptr; - std::unordered_map> task_managers = {}; - + // std::unordered_map> task_managers = {}; + std::vector> task_managers; + rclcpp::Publisher::SharedPtr fleet_state_pub = nullptr; rclcpp::TimerBase::SharedPtr fleet_state_timer = nullptr; @@ -173,13 +174,18 @@ class FleetUpdateHandle::Implementation std::unordered_set available_charging_waypoints; double current_assignment_cost = 0.0; + + /// TODO(YL) these are the previously used map, seems not needed. need to check // Map to store task id with assignments for BidNotice - std::unordered_map bid_notice_assignments = {}; + // std::unordered_map bid_notice_assignments = {}; + // std::unordered_map< + // std::string, rmf_task::ConstRequestPtr> generated_requests = {}; std::unordered_map< - std::string, rmf_task::ConstRequestPtr> generated_requests = {}; - std::unordered_map< - std::string, rmf_task::ConstRequestPtr> assigned_requests = {}; + std::string, rmf_task_ros2::ConstDescriptionPtr> task_descriptions = {}; + std::tuple + latest_bid_notice_assignments; + std::unordered_set cancelled_task_ids = {}; AcceptTaskRequest accept_task = nullptr; @@ -204,6 +210,8 @@ class FleetUpdateHandle::Implementation using DockSummarySub = rclcpp::Subscription::SharedPtr; DockSummarySub dock_summary_sub = nullptr; + using TaskType = rmf_task_msgs::msg::TaskType; + template static std::shared_ptr make(Args&&... args) { @@ -288,12 +296,15 @@ class FleetUpdateHandle::Implementation /// new request and while optionally ignoring a specific request. std::optional allocate_tasks( rmf_task::ConstRequestPtr new_request = nullptr, - rmf_task::ConstRequestPtr ignore_request = nullptr) const; + std::string ignore_request_id = "") const; /// Helper function to check if assignments are valid. An assignment set is /// invalid if one of the assignments has already begun execution. bool is_valid_assignments(Assignments& assignments) const; + /// Helper function to create assign tasks to task managers + void set_assignments_to_task_managers(const Assignments& assignments1); + static Implementation& get(FleetUpdateHandle& fleet) { return *fleet._pimpl; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.cpp index b9649941b..99cc32164 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.cpp @@ -552,6 +552,7 @@ void GoToPlace::Active::execute_plan(rmf_traffic::agv::Plan new_plan) rmf_task::agv::State dummy_state{{dummy_time, 0, 0.0}, 0, 1.0}; _subtasks = Task::make( _description, + nullptr, std::move(sub_phases), _context->worker(), dummy_time, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp index 9ef580863..1908bd2c8 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp @@ -26,6 +26,7 @@ namespace tasks { //============================================================================== std::shared_ptr make_charge_battery( + const rmf_task_ros2::ConstDescriptionPtr task_description, const rmf_task::requests::ConstChargeBatteryRequestPtr request, const agv::RobotContextPtr& context, const rmf_traffic::agv::Plan::Start start, @@ -42,6 +43,7 @@ std::shared_ptr make_charge_battery( return Task::make( request->id(), + std::move(task_description), std::move(phases), context->worker(), deployment_time, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp index 66ffef52d..e259772fa 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp @@ -32,6 +32,7 @@ namespace tasks { //============================================================================== std::shared_ptr make_charge_battery( + const rmf_task_ros2::ConstDescriptionPtr task_description, const rmf_task::requests::ConstChargeBatteryRequestPtr request, const agv::RobotContextPtr& context, const rmf_traffic::agv::Plan::Start start, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp index c26fbebc8..bf0ae1231 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp @@ -24,6 +24,7 @@ namespace tasks { //============================================================================== std::shared_ptr make_clean( + const rmf_task_ros2::ConstDescriptionPtr task_description, const rmf_task::requests::ConstCleanRequestPtr request, const agv::RobotContextPtr& context, const rmf_traffic::agv::Plan::Start clean_start, @@ -41,6 +42,7 @@ std::shared_ptr make_clean( return Task::make( request->id(), + std::move(task_description), std::move(phases), context->worker(), deployment_time, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp index 6c5d67ab5..f23520f64 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp @@ -32,6 +32,7 @@ namespace tasks { //============================================================================== std::shared_ptr make_clean( + const rmf_task_ros2::ConstDescriptionPtr task_description, const rmf_task::requests::ConstCleanRequestPtr request, const agv::RobotContextPtr& context, const rmf_traffic::agv::Plan::Start clean_start, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp index 6b669de62..7a7e63b12 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp @@ -28,6 +28,7 @@ namespace tasks { //============================================================================== std::shared_ptr make_delivery( + const rmf_task_ros2::ConstDescriptionPtr task_description, const rmf_task::requests::ConstDeliveryRequestPtr request, const agv::RobotContextPtr& context, const rmf_traffic::agv::Plan::Start pickup_start, @@ -73,6 +74,7 @@ std::shared_ptr make_delivery( return Task::make( request->id(), + std::move(task_description), std::move(phases), context->worker(), deployment_time, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp index 22b03ecd2..5b3871f36 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp @@ -28,6 +28,7 @@ namespace tasks { //============================================================================== std::shared_ptr make_delivery( + const rmf_task_ros2::ConstDescriptionPtr task_description, const rmf_task::requests::ConstDeliveryRequestPtr request, const agv::RobotContextPtr& context, const rmf_traffic::agv::Plan::Start pickup_start, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp index a8098cf2c..1fb52b2ba 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp @@ -24,6 +24,7 @@ namespace tasks { //============================================================================== std::shared_ptr make_loop( + const rmf_task_ros2::ConstDescriptionPtr task_description, const rmf_task::requests::ConstLoopRequestPtr request, const agv::RobotContextPtr& context, const rmf_traffic::agv::Plan::Start start, @@ -56,6 +57,7 @@ std::shared_ptr make_loop( return Task::make( request->id(), + std::move(task_description), std::move(phases), context->worker(), deployment_time, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.hpp index 19c3bf17d..f39ee5b4e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.hpp @@ -28,6 +28,7 @@ namespace tasks { //============================================================================== std::shared_ptr make_loop( + const rmf_task_ros2::ConstDescriptionPtr task_description, const rmf_task::requests::ConstLoopRequestPtr request, const agv::RobotContextPtr& context, const rmf_traffic::agv::Plan::Start start, diff --git a/rmf_fleet_adapter/test/test_Task.cpp b/rmf_fleet_adapter/test/test_Task.cpp index 2378174f6..6db4354a1 100644 --- a/rmf_fleet_adapter/test/test_Task.cpp +++ b/rmf_fleet_adapter/test/test_Task.cpp @@ -179,7 +179,7 @@ class MockSubtaskPhase Active(PendingPhases phases) : _subtasks( rmf_fleet_adapter::Task::make( - "subtasks", std::move(phases), + "subtasks", nullptr, std::move(phases), rxcpp::schedulers::make_event_loop().create_worker(), std::chrono::steady_clock::now(), {{std::chrono::steady_clock::now(), 0, 0.0}, 0, 1.0}, @@ -299,7 +299,7 @@ SCENARIO("Test simple task") std::shared_ptr task = rmf_fleet_adapter::Task::make( - "test_Task", std::move(phases), + "test_Task", nullptr, std::move(phases), rxcpp::schedulers::make_event_loop().create_worker(), deployment_time, finish_state, nullptr); @@ -391,7 +391,7 @@ SCENARIO("Test nested task") rmf_task::agv::State finish_state{{deployment_time, 0, 0.0}, 0, 1.0}; const auto task = rmf_fleet_adapter::Task::make( - "test_NestedTask", std::move(phases), + "test_NestedTask", nullptr, std::move(phases), rxcpp::schedulers::make_event_loop().create_worker(), deployment_time, finish_state, nullptr); diff --git a/rmf_task_ros2/include/rmf_task_ros2/Description.hpp b/rmf_task_ros2/include/rmf_task_ros2/Description.hpp index 4fcbafda0..c5ff952d5 100644 --- a/rmf_task_ros2/include/rmf_task_ros2/Description.hpp +++ b/rmf_task_ros2/include/rmf_task_ros2/Description.hpp @@ -34,20 +34,25 @@ namespace rmf_task_ros2 { class Description { public: - using ConstDescriptionPtr = std::shared_ptr; using TaskDescription = rmf_task_msgs::msg::TaskDescription; + static std::shared_ptr make_description( + rmf_traffic::Time start_time, + uint32_t type); + rmf_traffic::Time start_time() const; uint32_t type() const; - virtual TaskDescription to_msg() const = 0; + virtual TaskDescription to_msg() const; protected: rmf_traffic::Time _start_time; rmf_task_msgs::msg::TaskType task_type; }; +using ConstDescriptionPtr = std::shared_ptr; + //============================================================================== namespace description { @@ -130,18 +135,6 @@ class Clean : public Description }; } // namespace description - -//============================================================================== -class Execution -{ -public: - static size_t execute_task(Description::ConstDescriptionPtr task) - { - std::cout << "executing: " << task->type() << std::endl; - return task->type(); - } -}; - } // namespace rmf_task_ros2 #endif // RMF_TASK_ROS2__DESCRIPTION_HPP diff --git a/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp b/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp index 3241a4ca6..bec959bad 100644 --- a/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp +++ b/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp @@ -91,7 +91,7 @@ class Dispatcher : public std::enable_shared_from_this // New task submission std::optional submit_task( - const Description::ConstDescriptionPtr task_description); + const ConstDescriptionPtr task_description); /// Cancel an active task which was previously submitted to Dispatcher. This /// will terminate the task with a State of: `Canceled`. If a task is diff --git a/rmf_task_ros2/include/rmf_task_ros2/TaskStatus.hpp b/rmf_task_ros2/include/rmf_task_ros2/TaskStatus.hpp index a8f96fcd6..1667c62fb 100644 --- a/rmf_task_ros2/include/rmf_task_ros2/TaskStatus.hpp +++ b/rmf_task_ros2/include/rmf_task_ros2/TaskStatus.hpp @@ -45,13 +45,20 @@ struct TaskStatus }; std::string fleet_name; - TaskProfile task_profile; rmf_traffic::Time start_time; rmf_traffic::Time end_time; std::string robot_name; std::string status; // verbose msg State state = State::Pending; // default + /// TODO(YL) Shouldnt expose this msg, replace with the one commented below. + /// Also with static make function + TaskProfile task_profile; + + // std::string task_id; + // rmf_traffic::Time submission_time; + // ConstDescriptionPtr description = nullptr; + bool is_terminated() const; }; diff --git a/rmf_task_ros2/src/rmf_task_ros2/Description.cpp b/rmf_task_ros2/src/rmf_task_ros2/Description.cpp index cb7fc1b52..4bb920729 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/Description.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/Description.cpp @@ -31,6 +31,26 @@ uint32_t Description::type() const return task_type.type; } +//============================================================================== +std::shared_ptr Description::make_description( + rmf_traffic::Time start_time, + uint32_t type) +{ + auto desc = std::make_shared(); + desc->task_type.type = type; + desc->_start_time = std::move(start_time); + return desc; +} + +//============================================================================== +rmf_task_msgs::msg::TaskDescription Description::to_msg() const +{ + rmf_task_msgs::msg::TaskDescription msg; + msg.task_type = task_type; + msg.start_time = rmf_traffic_ros2::convert(_start_time); + return msg; +} + namespace description { //============================================================================== @@ -199,8 +219,5 @@ const std::string& Clean::start_waypoint() const return this->_start_waypoint; } - - } // namespace description } // namespace rmf_task_ros2 - diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp index 9c08230e1..bf607204c 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp @@ -417,7 +417,7 @@ std::optional Dispatcher::submit_task( //============================================================================== std::optional Dispatcher::submit_task( - const Description::ConstDescriptionPtr task_description) + const ConstDescriptionPtr task_description) { return _pimpl->submit_task(task_description->to_msg()); } diff --git a/rmf_task_ros2/test/dispatcher/test_Description.cpp b/rmf_task_ros2/test/dispatcher/test_Description.cpp index 09c16ce90..29b9ec13c 100644 --- a/rmf_task_ros2/test/dispatcher/test_Description.cpp +++ b/rmf_task_ros2/test/dispatcher/test_Description.cpp @@ -24,7 +24,7 @@ namespace rmf_task_ros2 { //============================================================================== SCENARIO("Task Description Test", "[Description]") { - std::cout << "Testing Description Task Type"<< std::cout; + std::cout << "Testing Description Task Type" << std::endl; using TaskDescription = rmf_task_msgs::msg::TaskDescription; using TaskType = rmf_task_msgs::msg::TaskType; @@ -41,8 +41,6 @@ SCENARIO("Task Description Test", "[Description]") << delivery->dropoff_place_name() << std::endl; - Execution::execute_task(delivery); - REQUIRE(delivery->type() == TaskType::TYPE_DELIVERY); TaskDescription msg; auto delivery2 = description::Delivery::make_from_msg(msg); @@ -61,7 +59,6 @@ SCENARIO("Task Description Test", "[Description]") << std::endl; auto loop2 = description::Loop::make_from_msg(msg); REQUIRE(!loop2); - Execution::execute_task(loop); //============================================================================== // test clean descripttion msg From b3e7bb50131856d2730481514c037cd9e198145d Mon Sep 17 00:00:00 2001 From: youliang Date: Thu, 25 Feb 2021 11:14:08 +0800 Subject: [PATCH 05/10] fix dep build error and style --- rmf_task_ros2/include/rmf_task_ros2/Description.hpp | 2 +- rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp | 2 +- rmf_task_ros2/include/rmf_task_ros2/TaskStatus.hpp | 2 +- rmf_task_ros2/package.xml | 1 + 4 files changed, 4 insertions(+), 3 deletions(-) diff --git a/rmf_task_ros2/include/rmf_task_ros2/Description.hpp b/rmf_task_ros2/include/rmf_task_ros2/Description.hpp index c5ff952d5..e99ddfd24 100644 --- a/rmf_task_ros2/include/rmf_task_ros2/Description.hpp +++ b/rmf_task_ros2/include/rmf_task_ros2/Description.hpp @@ -41,7 +41,7 @@ class Description uint32_t type); rmf_traffic::Time start_time() const; - + uint32_t type() const; virtual TaskDescription to_msg() const; diff --git a/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp b/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp index b434344a6..617c922df 100644 --- a/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp +++ b/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp @@ -76,7 +76,7 @@ class Dispatcher : public std::enable_shared_from_this const int terminated_tasks_depth = 50); /// Submit task description to RMF dispatcher node. Calling this function will - /// immediately trigger the bidding process, and dispatch the task to the + /// immediately trigger the bidding process, and dispatch the task to the /// selected downstream fleet adapter. /// /// \param [in] task_description diff --git a/rmf_task_ros2/include/rmf_task_ros2/TaskStatus.hpp b/rmf_task_ros2/include/rmf_task_ros2/TaskStatus.hpp index bbccf68fb..cd2f87698 100644 --- a/rmf_task_ros2/include/rmf_task_ros2/TaskStatus.hpp +++ b/rmf_task_ros2/include/rmf_task_ros2/TaskStatus.hpp @@ -59,7 +59,7 @@ struct TaskStatus // std::string task_id, // rmf_traffic::Time submission_time, // ConstDescriptionPtr task_description); - + bool is_terminated() const; }; diff --git a/rmf_task_ros2/package.xml b/rmf_task_ros2/package.xml index 268800c53..1df429007 100644 --- a/rmf_task_ros2/package.xml +++ b/rmf_task_ros2/package.xml @@ -13,6 +13,7 @@ rmf_traffic rmf_traffic_ros2 rmf_task_msgs + rmf_task rclcpp eigen From bf1e7ae01a0a38772abe4edd0aef7555637208c2 Mon Sep 17 00:00:00 2001 From: youliang Date: Thu, 25 Feb 2021 15:49:00 +0800 Subject: [PATCH 06/10] fix build error and clear queue durin set queue --- rmf_fleet_adapter/CMakeLists.txt | 1 - rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp | 5 +++++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/rmf_fleet_adapter/CMakeLists.txt b/rmf_fleet_adapter/CMakeLists.txt index b95f4b8dd..b2bbab1e6 100644 --- a/rmf_fleet_adapter/CMakeLists.txt +++ b/rmf_fleet_adapter/CMakeLists.txt @@ -40,7 +40,6 @@ set(dep_pkgs rmf_battery rmf_task rmf_task_ros2 - rmf_fleet_adapter std_msgs ) foreach(pkg ${dep_pkgs}) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index c48022813..9b200c18b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -152,6 +152,11 @@ const std::vector> TaskManager::task_queue() const void TaskManager::set_queue( const std::vector>& tasks) { + { + std::lock_guard guard(_mutex); + _queue.clear(); + } + for (const auto t : tasks) queue_task(t); } From 5b8aed2ddd8899010e5d0397ea6cb44387b3cb3a Mon Sep 17 00:00:00 2001 From: youliang Date: Fri, 26 Feb 2021 11:31:41 +0800 Subject: [PATCH 07/10] Update TaskStatus obj by removing the use of msg --- .../agv/FleetUpdateHandle.cpp | 4 +- .../include/rmf_task_ros2/Description.hpp | 88 ++++--- .../include/rmf_task_ros2/Dispatcher.hpp | 2 +- .../include/rmf_task_ros2/TaskStatus.hpp | 69 ++++-- rmf_task_ros2/src/mock_bidder/main.cpp | 24 +- .../src/rmf_task_ros2/Description.cpp | 220 +++++++++++++----- .../src/rmf_task_ros2/Dispatcher.cpp | 96 ++++---- .../src/rmf_task_ros2/TaskStatus.cpp | 80 +++++-- .../src/rmf_task_ros2/action/Client.cpp | 49 ++-- .../src/rmf_task_ros2/action/Client.hpp | 18 +- .../test/action/test_ActionInterface.cpp | 63 +++-- .../test/dispatcher/test_Description.cpp | 8 +- .../test/dispatcher/test_Dispatcher.cpp | 43 ++-- 13 files changed, 505 insertions(+), 259 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index a81090222..954b7ee43 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -334,7 +334,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( if (!loop) { RCLCPP_ERROR(node->get_logger(), - "Delivery Msg is invalid/invalid." + "Loop Msg is invalid/invalid." "Rejecting BidNotice with task_id:[%s]", id.c_str()); return; } @@ -599,7 +599,7 @@ void FleetUpdateHandle::Implementation::dispatch_request_cb( dispatch_ack_pub->publish(dispatch_ack); return; - } + } // Re-plan assignments while ignoring request for task to be cancelled const auto replan_results = allocate_tasks(nullptr, id); diff --git a/rmf_task_ros2/include/rmf_task_ros2/Description.hpp b/rmf_task_ros2/include/rmf_task_ros2/Description.hpp index e99ddfd24..16043815d 100644 --- a/rmf_task_ros2/include/rmf_task_ros2/Description.hpp +++ b/rmf_task_ros2/include/rmf_task_ros2/Description.hpp @@ -20,13 +20,12 @@ #include #include -#include -#include #include -#include #include +#include + namespace rmf_task_ros2 { //============================================================================== @@ -36,19 +35,41 @@ class Description public: using TaskDescription = rmf_task_msgs::msg::TaskDescription; + /// Create a Description. This is only used when the task type is not + /// available as the derived class. + /// + /// \param [in] start_time + /// + /// \param [in] type static std::shared_ptr make_description( rmf_traffic::Time start_time, - uint32_t type); - + uint32_t type, + uint64_t priority = 0); + + /// Create a Description by providing a msg. This is only used when the task + /// type is not available as the derived class. + /// + /// \param [in] msg + /// task description msg as argument + static std::shared_ptr make_from_msg( + const TaskDescription& msg); + + /// Get the start_time of the task rmf_traffic::Time start_time() const; + /// Get the type of the task uint32_t type() const; + /// This is not created now + uint64_t priority() const; + + /// Get the TaskDescription as msg virtual TaskDescription to_msg() const; + class Implementation; protected: - rmf_traffic::Time _start_time; - rmf_task_msgs::msg::TaskType task_type; + Description(); + rmf_utils::unique_impl_ptr _pimpl_base; }; using ConstDescriptionPtr = std::shared_ptr; @@ -61,18 +82,21 @@ class Delivery : public Description public: using DispenserRequestItem = rmf_dispenser_msgs::msg::DispenserRequestItem; + /// Make a Delivery Task Description static std::shared_ptr make( rmf_traffic::Time start_time, std::string pickup_place_name, std::string pickup_dispenser, std::string dropoff_place_name, std::string dropoff_ingestor, - std::vector items); + std::vector items, + uint64_t priority = 0); - /// use this only within internal implementation of RMF - static std::shared_ptr make_from_msg(TaskDescription msg); + /// Create task description from description msg + static std::shared_ptr make_from_msg( + const TaskDescription& msg); - /// use this only within internal implementation of RMF + /// Get the TaskDescription as msg TaskDescription to_msg() const final; /// Get the pickup_place_name @@ -81,26 +105,29 @@ class Delivery : public Description /// Get the dropoff_place_name const std::string& dropoff_place_name() const; + class Implementation; private: - std::string _pickup_place_name; - std::string pickup_dispenser; - std::string _dropoff_place_name; - std::string dropoff_ingestor; - std::vector items; + Delivery(); + rmf_utils::unique_impl_ptr _pimpl; }; //============================================================================== class Loop : public Description { public: + /// Make a Loop Task Description static std::shared_ptr make( rmf_traffic::Time start_time, std::string start_name, std::string finish_name, - std::size_t num_loops); + std::size_t num_loops, + uint64_t priority = 0); - static std::shared_ptr make_from_msg(TaskDescription msg); + /// Create task description from description msg + static std::shared_ptr make_from_msg( + const TaskDescription& msg); + /// Get the TaskDescription as msg TaskDescription to_msg() const final; /// Get the start_name @@ -109,29 +136,36 @@ class Loop : public Description /// Get the finish_name const std::string& finish_name() const; + class Implementation; private: - std::string _start_name; - std::string _finish_name; - std::size_t num_loops; + Loop(); + rmf_utils::unique_impl_ptr _pimpl; }; //============================================================================== - class Clean : public Description { public: - std::string _start_waypoint; - + /// Make a Clean Task Description static std::shared_ptr make( rmf_traffic::Time start_time, - std::string start_waypoint); + std::string start_waypoint, + uint64_t priority = 0); - static std::shared_ptr make_from_msg(TaskDescription msg); + /// Create task description from description msg + static std::shared_ptr make_from_msg( + const TaskDescription& msg); + /// Get the TaskDescription as msg TaskDescription to_msg() const final; - /// Get the _start_waypoint + /// Get the start_waypoint of a clean const std::string& start_waypoint() const; + + class Implementation; +private: + Clean(); + rmf_utils::unique_impl_ptr _pimpl; }; } // namespace description diff --git a/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp b/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp index 617c922df..b861a78a3 100644 --- a/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp +++ b/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp @@ -34,8 +34,8 @@ namespace rmf_task_ros2 { class Dispatcher : public std::enable_shared_from_this { public: + using TaskID = std::string; using DispatchTasks = std::unordered_map; - using TaskDescription = rmf_task_msgs::msg::TaskDescription; /// Initialize an rclcpp context and make an dispatcher instance. This will /// instantiate an rclcpp::Node, a task dispatcher node. Dispatcher node will diff --git a/rmf_task_ros2/include/rmf_task_ros2/TaskStatus.hpp b/rmf_task_ros2/include/rmf_task_ros2/TaskStatus.hpp index cd2f87698..d36b7c5ca 100644 --- a/rmf_task_ros2/include/rmf_task_ros2/TaskStatus.hpp +++ b/rmf_task_ros2/include/rmf_task_ros2/TaskStatus.hpp @@ -20,20 +20,50 @@ #include #include -#include +#include +#include + #include namespace rmf_task_ros2 { -//============================================================================== -using TaskProfile = rmf_task_msgs::msg::TaskProfile; using StatusMsg = rmf_task_msgs::msg::TaskSummary; -using TaskID = std::string; //============================================================================== +/// This represents the Task Status of each unique task /// \note TaskStatus struct is based on TaskSummary.msg -struct TaskStatus +class TaskStatus { +public: + static std::shared_ptr make( + std::string task_id, + rmf_traffic::Time submission_time, + ConstDescriptionPtr task_description); + + /// Get Task ID + std::string task_id() const; + + /// Get submission time + rmf_traffic::Time submission_time() const; + + /// Get Task Description + ConstDescriptionPtr description() const; + + /// The fleet which will execute the task + std::string fleet_name; + + /// The robot which will execute the task + std::string robot_name; + + /// The estimated time which the task will start executing + rmf_traffic::Time start_time; + + /// The estimated time which the task will finish + rmf_traffic::Time end_time; + + /// Verbose status of this task + std::string status; + enum class State : uint8_t { Queued = StatusMsg::STATE_QUEUED, @@ -44,30 +74,25 @@ struct TaskStatus Pending = StatusMsg::STATE_PENDING }; - std::string fleet_name; - rmf_traffic::Time start_time; - rmf_traffic::Time end_time; - std::string robot_name; - std::string status; // verbose msg - State state = State::Pending; // default + /// Current State of the task + State state; - /// TODO(YL) Shouldnt expose this msg, replace with the one commented below. - /// Also with static make function - TaskProfile task_profile; + /// Check if the current task is terminated + bool is_terminated() const; - // static std::shared_ptr make( - // std::string task_id, - // rmf_traffic::Time submission_time, - // ConstDescriptionPtr task_description); + /// This helper function is to only update status elements in TaskStatus, in + /// which static task descriptions (e.g. id, tasktype...) will not be changed + /// \note experimental (TODO) + void update_from_msg(const StatusMsg& msg); - bool is_terminated() const; + class Implementation; +private: + TaskStatus(); + rmf_utils::unique_impl_ptr _pimpl; }; using TaskStatusPtr = std::shared_ptr; -// ============================================================================== -TaskStatus convert_status(const StatusMsg& from); - // ============================================================================== StatusMsg convert_status(const TaskStatus& from); diff --git a/rmf_task_ros2/src/mock_bidder/main.cpp b/rmf_task_ros2/src/mock_bidder/main.cpp index 994dbc3c3..7314d0bf3 100644 --- a/rmf_task_ros2/src/mock_bidder/main.cpp +++ b/rmf_task_ros2/src/mock_bidder/main.cpp @@ -77,26 +77,26 @@ int main(int argc, char* argv[]) auto t = std::thread( [&action_server, &node](auto profile) { - TaskStatus status; - status.task_profile = profile; - status.robot_name = "dumbot"; - status.start_time = rmf_traffic_ros2::convert(node->now()); - status.end_time = - rmf_traffic::time::apply_offset(status.start_time, 7); - + // HUGE TODO(YL) const auto id = profile.task_id; + const auto now = rmf_traffic_ros2::convert(node->now()); + const auto status = TaskStatus::make(id, now, nullptr); //todo + status->robot_name = "dumbot"; + status->start_time = now; + status->end_time = rmf_traffic::time::apply_offset(now, 7); + std::cout << " [MockBidder] Queued, TaskID: " << id << std::endl; - action_server->update_status(status); + action_server->update_status(*status); std::this_thread::sleep_for(std::chrono::seconds(2)); std::cout << " [MockBidder] Executing, TaskID: " << id << std::endl; - status.state = TaskStatus::State::Executing; - action_server->update_status(status); + status->state = TaskStatus::State::Executing; + action_server->update_status(*status); std::this_thread::sleep_for(std::chrono::seconds(5)); std::cout << " [MockBidder] Completed, TaskID: " << id << std::endl; - status.state = TaskStatus::State::Completed; - action_server->update_status(status); + status->state = TaskStatus::State::Completed; + action_server->update_status(*status); }, task_profile ); t.detach(); diff --git a/rmf_task_ros2/src/rmf_task_ros2/Description.cpp b/rmf_task_ros2/src/rmf_task_ros2/Description.cpp index 4bb920729..498648796 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/Description.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/Description.cpp @@ -16,43 +16,96 @@ */ #include +#include namespace rmf_task_ros2 { +//============================================================================== +class Description::Implementation +{ +public: + Implementation() + {} + + rmf_traffic::Time start_time; + rmf_task_msgs::msg::Priority priority; + rmf_task_msgs::msg::TaskType task_type; +}; + //============================================================================== rmf_traffic::Time Description::start_time() const { - return _start_time; + return _pimpl_base->start_time; } //============================================================================== uint32_t Description::type() const { - return task_type.type; + return _pimpl_base->task_type.type; +} + + +//============================================================================== +uint64_t Description::priority() const +{ + return _pimpl_base->priority.value; } //============================================================================== std::shared_ptr Description::make_description( rmf_traffic::Time start_time, - uint32_t type) + uint32_t type, + uint64_t priority) { - auto desc = std::make_shared(); - desc->task_type.type = type; - desc->_start_time = std::move(start_time); + std::shared_ptr desc(new Description()); + desc->_pimpl_base->task_type.type = type; + desc->_pimpl_base->priority.value = priority; + desc->_pimpl_base->start_time = std::move(start_time); return desc; } +//============================================================================== +std::shared_ptr Description::make_from_msg( + const TaskDescription& msg) +{ + return make_description(rmf_traffic_ros2::convert(msg.start_time), + msg.task_type.type, msg.priority.value); +} + //============================================================================== rmf_task_msgs::msg::TaskDescription Description::to_msg() const { rmf_task_msgs::msg::TaskDescription msg; - msg.task_type = task_type; - msg.start_time = rmf_traffic_ros2::convert(_start_time); + msg.task_type = _pimpl_base->task_type; + msg.priority = _pimpl_base->priority; + msg.start_time = rmf_traffic_ros2::convert(_pimpl_base->start_time); return msg; } +//============================================================================== +Description::Description() +: _pimpl_base(rmf_utils::make_impl(Implementation())) +{ + // Do nothing +} + +//============================================================================== +//============================================================================== namespace description { +class Delivery::Implementation +{ +public: + Implementation() + {} + + std::string pickup_place_name; + std::string pickup_dispenser; + std::string dropoff_place_name; + std::string dropoff_ingestor; + std::vector items; +}; + //============================================================================== std::shared_ptr Delivery::make( rmf_traffic::Time start_time, @@ -60,28 +113,31 @@ std::shared_ptr Delivery::make( std::string pickup_dispenser, std::string dropoff_place_name, std::string dropoff_ingestor, - std::vector items) + std::vector items, + uint64_t priority) { if (pickup_place_name.empty() || pickup_dispenser.empty() || dropoff_place_name.empty() || dropoff_ingestor.empty()) return nullptr; - auto delivery_desc = std::make_shared(); + std::shared_ptr delivery_desc(new Delivery()); - delivery_desc->task_type.type = rmf_task_msgs::msg::TaskType::TYPE_DELIVERY; - delivery_desc->_start_time = std::move(start_time); - delivery_desc->_pickup_place_name = std::move(pickup_place_name); - delivery_desc->pickup_dispenser = std::move(pickup_dispenser); - delivery_desc->_dropoff_place_name = std::move(dropoff_place_name); - delivery_desc->dropoff_ingestor = std::move(dropoff_ingestor); - delivery_desc->items = std::move(items); + using TaskType = rmf_task_msgs::msg::TaskType; + delivery_desc->_pimpl_base->task_type.type = TaskType::TYPE_DELIVERY; + delivery_desc->_pimpl_base->start_time = std::move(start_time); + delivery_desc->_pimpl_base->priority.value = priority; + delivery_desc->_pimpl->pickup_place_name = std::move(pickup_place_name); + delivery_desc->_pimpl->pickup_dispenser = std::move(pickup_dispenser); + delivery_desc->_pimpl->dropoff_place_name = std::move(dropoff_place_name); + delivery_desc->_pimpl->dropoff_ingestor = std::move(dropoff_ingestor); + delivery_desc->_pimpl->items = std::move(items); return delivery_desc; } //============================================================================== std::shared_ptr Delivery::make_from_msg( - TaskDescription msg) + const TaskDescription& msg) { if (msg.task_type.type != rmf_task_msgs::msg::TaskType::TYPE_DELIVERY) return nullptr; @@ -92,56 +148,81 @@ std::shared_ptr Delivery::make_from_msg( msg.delivery.pickup_dispenser, msg.delivery.dropoff_place_name, msg.delivery.dropoff_ingestor, - msg.delivery.items); + msg.delivery.items, + msg.priority.value); +} + +//============================================================================== +Delivery::Delivery() +: _pimpl(rmf_utils::make_impl(Implementation())) +{ + // Do nothing } //============================================================================== rmf_task_msgs::msg::TaskDescription Delivery::to_msg() const { rmf_task_msgs::msg::TaskDescription msg; - msg.task_type = task_type; - msg.start_time = rmf_traffic_ros2::convert(_start_time); - msg.delivery.pickup_place_name = _pickup_place_name; - msg.delivery.dropoff_place_name = _dropoff_place_name; - msg.delivery.pickup_dispenser = pickup_dispenser; - msg.delivery.dropoff_ingestor = dropoff_ingestor; + msg.task_type = _pimpl_base->task_type; + msg.priority = _pimpl_base->priority; + msg.start_time = rmf_traffic_ros2::convert(_pimpl_base->start_time); + msg.delivery.pickup_place_name = _pimpl->pickup_place_name; + msg.delivery.dropoff_place_name = _pimpl->dropoff_place_name; + msg.delivery.pickup_dispenser = _pimpl->pickup_dispenser; + msg.delivery.dropoff_ingestor = _pimpl->dropoff_ingestor; return msg; } //============================================================================== const std::string& Delivery::pickup_place_name() const { - return _pickup_place_name; + return _pimpl->pickup_place_name; } //============================================================================== const std::string& Delivery::dropoff_place_name() const { - return _dropoff_place_name; + return _pimpl->dropoff_place_name; } //============================================================================== //============================================================================== +class Loop::Implementation +{ +public: + Implementation() + {} + + std::string start_name; + std::string finish_name; + std::size_t num_loops; +}; + std::shared_ptr Loop::make( rmf_traffic::Time start_time, std::string start_name, std::string finish_name, - std::size_t num_loops) + std::size_t num_loops, + uint64_t priority) { if (start_name.empty() || finish_name.empty() || num_loops == 0) return nullptr; - auto loop_desc = std::make_shared(); - loop_desc->task_type.type = rmf_task_msgs::msg::TaskType::TYPE_LOOP; - loop_desc->_start_time = std::move(start_time); - loop_desc->_start_name = std::move(start_name); - loop_desc->_finish_name = std::move(finish_name); - loop_desc->num_loops = num_loops; + std::shared_ptr loop_desc(new Loop()); + + using TaskType = rmf_task_msgs::msg::TaskType; + loop_desc->_pimpl_base->task_type.type = TaskType::TYPE_LOOP; + loop_desc->_pimpl_base->priority.value = priority; + loop_desc->_pimpl_base->start_time = std::move(start_time); + loop_desc->_pimpl->start_name = std::move(start_name); + loop_desc->_pimpl->finish_name = std::move(finish_name); + loop_desc->_pimpl->num_loops = num_loops; return loop_desc; } //============================================================================== -std::shared_ptr Loop::make_from_msg(TaskDescription msg) +std::shared_ptr Loop::make_from_msg( + const TaskDescription& msg) { if (msg.task_type.type != rmf_task_msgs::msg::TaskType::TYPE_LOOP) return nullptr; @@ -150,73 +231,106 @@ std::shared_ptr Loop::make_from_msg(TaskDescription msg) rmf_traffic_ros2::convert(msg.start_time), msg.loop.start_name, msg.loop.finish_name, - msg.loop.num_loops); + msg.loop.num_loops, + msg.priority.value); } //============================================================================== rmf_task_msgs::msg::TaskDescription Loop::to_msg() const { TaskDescription msg; - msg.task_type = task_type; - msg.start_time = rmf_traffic_ros2::convert(_start_time); - msg.loop.start_name = _start_name; - msg.loop.finish_name = _finish_name; + msg.task_type = _pimpl_base->task_type; + msg.priority = _pimpl_base->priority; + msg.start_time = rmf_traffic_ros2::convert(_pimpl_base->start_time); + msg.loop.start_name = _pimpl->start_name; + msg.loop.finish_name = _pimpl->finish_name; + msg.loop.num_loops = _pimpl->num_loops; return msg; } +//============================================================================== +Loop::Loop() +: _pimpl(rmf_utils::make_impl(Implementation())) +{ + // Do nothing +} + //============================================================================== const std::string& Loop::start_name() const { - return this->_start_name; + return this->_pimpl->start_name; } //============================================================================== const std::string& Loop::finish_name() const { - return this->_finish_name; + return this->_pimpl->finish_name; } //============================================================================== //============================================================================== +class Clean::Implementation +{ +public: + Implementation() + {} + + std::string start_waypoint; +}; + std::shared_ptr Clean::make( rmf_traffic::Time start_time, - std::string start_waypoint) + std::string start_waypoint, + uint64_t priority) { if (start_waypoint.empty()) return nullptr; - auto clean_desc = std::make_shared(); - clean_desc->task_type.type = rmf_task_msgs::msg::TaskType::TYPE_CLEAN; - clean_desc->_start_time = std::move(start_time); - clean_desc->_start_waypoint = std::move(start_waypoint); + std::shared_ptr clean_desc(new Clean()); + + using TaskType = rmf_task_msgs::msg::TaskType; + clean_desc->_pimpl_base->task_type.type = TaskType::TYPE_CLEAN; + clean_desc->_pimpl_base->start_time = std::move(start_time); + clean_desc->_pimpl_base->priority.value = priority; + clean_desc->_pimpl->start_waypoint = std::move(start_waypoint); return clean_desc; } //============================================================================== -std::shared_ptr Clean::make_from_msg(TaskDescription msg) +std::shared_ptr Clean::make_from_msg( + const TaskDescription& msg) { if (msg.task_type.type != rmf_task_msgs::msg::TaskType::TYPE_CLEAN) return nullptr; return make( rmf_traffic_ros2::convert(msg.start_time), - msg.clean.start_waypoint); + msg.clean.start_waypoint, + msg.priority.value); } //============================================================================== rmf_task_msgs::msg::TaskDescription Clean::to_msg() const { TaskDescription msg; - msg.task_type = task_type; - msg.start_time = rmf_traffic_ros2::convert(_start_time); - msg.clean.start_waypoint = _start_waypoint; + msg.task_type = _pimpl_base->task_type; + msg.priority = _pimpl_base->priority; + msg.start_time = rmf_traffic_ros2::convert(_pimpl_base->start_time); + msg.clean.start_waypoint = _pimpl->start_waypoint; return msg; } +//============================================================================== +Clean::Clean() +: _pimpl(rmf_utils::make_impl(Implementation())) +{ + // Do nothing +} + //============================================================================== const std::string& Clean::start_waypoint() const { - return this->_start_waypoint; + return this->_pimpl->start_waypoint; } } // namespace description diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp index dfab803d0..85dd51e47 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp @@ -40,6 +40,7 @@ class Dispatcher::Implementation using SubmitTaskSrv = rmf_task_msgs::srv::SubmitTask; using CancelTaskSrv = rmf_task_msgs::srv::CancelTask; using GetTaskListSrv = rmf_task_msgs::srv::GetTaskList; + using TaskType = rmf_task_msgs::msg::TaskType; rclcpp::Service::SharedPtr submit_task_srv; rclcpp::Service::SharedPtr cancel_task_srv; @@ -50,6 +51,7 @@ class Dispatcher::Implementation std::queue queue_bidding_tasks; DispatchTasks active_dispatch_tasks; DispatchTasks terminal_dispatch_tasks; + std::size_t task_counter = 0; // index for generating task_id double bidding_time_window; int terminated_tasks_max_size; @@ -73,12 +75,27 @@ class Dispatcher::Implementation const std::shared_ptr request, std::shared_ptr response) { - const auto id = this->submit_task(request->description); - if (id == std::nullopt) + response->success = false; + + ConstDescriptionPtr task_description; + const auto desc_msg = request->description; + if (auto d = description::Delivery::make_from_msg(desc_msg)) + task_description = std::move(d); + else if (auto d = description::Clean::make_from_msg(desc_msg)) + task_description = std::move(d); + else if (auto d = description::Loop::make_from_msg(desc_msg)) + task_description = std::move(d); + else { - response->success = false; + RCLCPP_ERROR(node->get_logger(), + "Received an invalid task from task submision request"); return; } + + const auto id = this->submit_task(task_description); + if (id == std::nullopt) + return; + response->task_id = *id; response->success = true; } @@ -129,13 +146,9 @@ class Dispatcher::Implementation std::bind(&Implementation::task_status_cb, this, _1)); } - std::optional submit_task(const TaskDescription& description) + std::optional submit_task(const ConstDescriptionPtr description) { - TaskProfile submitted_task; - submitted_task.submission_time = node->now(); - submitted_task.description = description; - - const auto task_type = static_cast(description.task_type.type); + const auto task_type = static_cast(description->type()); if (!task_type_name.count(task_type)) { @@ -144,23 +157,20 @@ class Dispatcher::Implementation } // auto generate a task_id for a given submitted task - submitted_task.task_id = - task_type_name[task_type] + std::to_string(task_counter++); - - RCLCPP_INFO(node->get_logger(), - "Received Task Submission [%s]", submitted_task.task_id.c_str()); + const auto id = task_type_name[task_type] + std::to_string(task_counter++); + RCLCPP_INFO(node->get_logger(), "Received Task Request [%s]", id.c_str()); - // add task to internal cache - TaskStatus status; - status.task_profile = submitted_task; - auto new_task_status = std::make_shared(status); - active_dispatch_tasks[submitted_task.task_id] = new_task_status; + const auto status = TaskStatus::make( + id, rmf_traffic_ros2::convert(node->now()), description); + active_dispatch_tasks[id] = status; if (on_change_fn) - on_change_fn(new_task_status); + on_change_fn(status); rmf_task_msgs::msg::BidNotice bid_notice; - bid_notice.task_profile = submitted_task; + bid_notice.task_profile.task_id = id; + bid_notice.task_profile.submission_time = node->now(); + bid_notice.task_profile.description = description->to_msg(); bid_notice.time_window = rmf_traffic_ros2::convert( rmf_traffic::time::from_seconds(bidding_time_window)); queue_bidding_tasks.push(bid_notice); @@ -168,7 +178,7 @@ class Dispatcher::Implementation if (queue_bidding_tasks.size() == 1) auctioneer->start_bidding(queue_bidding_tasks.front()); - return submitted_task.task_id; + return id; } bool cancel_task(const TaskID& task_id) @@ -203,8 +213,8 @@ class Dispatcher::Implementation } // Charging task doesnt support cancel task - if (cancel_task_status->task_profile.description.task_type.type == - rmf_task_msgs::msg::TaskType::TYPE_CHARGE_BATTERY) + if (cancel_task_status->description()->type() == + TaskType::TYPE_CHARGE_BATTERY) { RCLCPP_ERROR(node->get_logger(), "Charging task is not cancelled-able"); return false; @@ -226,11 +236,10 @@ class Dispatcher::Implementation for (auto it = active_dispatch_tasks.begin(); it != active_dispatch_tasks.end(); ) { - const auto type = it->second->task_profile.description.task_type.type; const bool is_fleet_name = (cancel_task_status->fleet_name == it->second->fleet_name); const bool is_charging_task = - (type == rmf_task_msgs::msg::TaskType::TYPE_CHARGE_BATTERY); + (it->second->description()->type() == TaskType::TYPE_CHARGE_BATTERY); if (is_charging_task && is_fleet_name) it = active_dispatch_tasks.erase(it); @@ -241,7 +250,7 @@ class Dispatcher::Implementation // Cancel action task, this will only send a cancel to FA. up to // the FA whether to cancel the task. On change is implemented // internally in action client - return action_client->cancel_task(cancel_task_status->task_profile); + return action_client->cancel_task(cancel_task_status->task_id()); } const std::optional get_task_state( @@ -261,7 +270,7 @@ class Dispatcher::Implementation } void receive_bidding_winner_cb( - const TaskID& task_id, + const std::string& task_id, const std::optional winner) { const auto it = active_dispatch_tasks.find(task_id); @@ -299,10 +308,9 @@ class Dispatcher::Implementation for (auto it = active_dispatch_tasks.begin(); it != active_dispatch_tasks.end(); ) { - const auto type = it->second->task_profile.description.task_type.type; const bool is_fleet_name = (winner->fleet_name == it->second->fleet_name); const bool is_charging_task = - (type == rmf_task_msgs::msg::TaskType::TYPE_CHARGE_BATTERY); + (it->second->description()->type() == TaskType::TYPE_CHARGE_BATTERY); if (is_charging_task && is_fleet_name) it = active_dispatch_tasks.erase(it); @@ -311,9 +319,8 @@ class Dispatcher::Implementation } // add task to action server - action_client->add_task( + action_client->dispatch_task( winner->fleet_name, - pending_task_status->task_profile, pending_task_status); } @@ -330,19 +337,20 @@ class Dispatcher::Implementation auto rm_task = terminal_dispatch_tasks.begin(); for (auto it = rm_task++; it != terminal_dispatch_tasks.end(); it++) { - const auto t1 = it->second->task_profile.submission_time; - const auto t2 = rm_task->second->task_profile.submission_time; - if (rmf_traffic_ros2::convert(t1) < rmf_traffic_ros2::convert(t2)) + const auto t1 = it->second->submission_time(); + const auto t2 = rm_task->second->submission_time(); + + if (t1 < t2) rm_task = it; } terminal_dispatch_tasks.erase(terminal_dispatch_tasks.begin() ); } - const auto id = terminate_status->task_profile.task_id; + const auto id = terminate_status->task_id(); - // destroy prev status ptr and recreate one - auto status = std::make_shared(*terminate_status); - (terminal_dispatch_tasks)[id] = status; + // destroy prev status ptr and recreate one (TODO) check if correct + // auto status = std::make_shared(*terminate_status); + (terminal_dispatch_tasks)[id] = terminate_status; active_dispatch_tasks.erase(id); } @@ -351,7 +359,7 @@ class Dispatcher::Implementation // This is to solve the issue that the dispatcher is not aware of those // "stray" tasks that are not dispatched by the dispatcher. This will add // the stray tasks when an unknown TaskSummary is heard. - const std::string id = status->task_profile.task_id; + const std::string id = status->task_id(); const auto it = active_dispatch_tasks.find(id); if (it == active_dispatch_tasks.end()) { @@ -409,21 +417,21 @@ std::shared_ptr Dispatcher::make( } //============================================================================== -std::optional Dispatcher::submit_task( +std::optional Dispatcher::submit_task( const ConstDescriptionPtr task_description) { - return _pimpl->submit_task(task_description->to_msg()); + return _pimpl->submit_task(task_description); } //============================================================================== -bool Dispatcher::cancel_task(const TaskID& task_id) +bool Dispatcher::cancel_task(const std::string& task_id) { return _pimpl->cancel_task(task_id); } //============================================================================== const std::optional Dispatcher::get_task_state( - const TaskID& task_id) const + const std::string& task_id) const { return _pimpl->get_task_state(task_id); } diff --git a/rmf_task_ros2/src/rmf_task_ros2/TaskStatus.cpp b/rmf_task_ros2/src/rmf_task_ros2/TaskStatus.cpp index cd06e3c94..9fb380080 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/TaskStatus.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/TaskStatus.cpp @@ -20,6 +20,61 @@ namespace rmf_task_ros2 { +// ============================================================================== +class TaskStatus::Implementation +{ +public: + Implementation() + {} + + std::string task_id; + rmf_traffic::Time submission_time; + ConstDescriptionPtr description; +}; + +// ============================================================================== +TaskStatusPtr TaskStatus::make( + std::string task_id, + rmf_traffic::Time submission_time, + ConstDescriptionPtr description) +{ + std::shared_ptr desc(new TaskStatus()); + desc->_pimpl->task_id = task_id; + desc->_pimpl->submission_time = std::move(submission_time); + desc->_pimpl->description = std::move(description); + desc->state = State::Pending; + return desc; +} + +// ============================================================================== +ConstDescriptionPtr TaskStatus::description() const +{ + return _pimpl->description; +} + +// ============================================================================== +std::string TaskStatus::task_id() const +{ + return _pimpl->task_id; +} + +// ============================================================================== +rmf_traffic::Time TaskStatus::submission_time() const +{ + return _pimpl->submission_time; +} + +// ============================================================================== +void TaskStatus::update_from_msg(const StatusMsg& msg) +{ + fleet_name = msg.fleet_name; + start_time = rmf_traffic_ros2::convert(msg.start_time); + end_time = rmf_traffic_ros2::convert(msg.end_time); + robot_name = msg.robot_name; + status = msg.status; + state = static_cast(msg.state); +} + // ============================================================================== bool TaskStatus::is_terminated() const { @@ -28,18 +83,11 @@ bool TaskStatus::is_terminated() const (state == State::Canceled); } -// ============================================================================== -TaskStatus convert_status(const StatusMsg& from) +//============================================================================== +TaskStatus::TaskStatus() +: _pimpl(rmf_utils::make_impl(Implementation())) { - TaskStatus status; - status.fleet_name = from.fleet_name; - status.task_profile = from.task_profile; - status.start_time = rmf_traffic_ros2::convert(from.start_time); - status.end_time = rmf_traffic_ros2::convert(from.end_time); - status.robot_name = from.robot_name; - status.status = from.status; - status.state = static_cast(from.state); - return status; + // Do nothing } // ============================================================================== @@ -47,13 +95,19 @@ StatusMsg convert_status(const TaskStatus& from) { StatusMsg status; status.fleet_name = from.fleet_name; - status.task_id = from.task_profile.task_id; // duplication - status.task_profile = from.task_profile; + status.task_id = from.task_id(); // duplication + status.task_profile.task_id = from.task_id(); + status.task_profile.submission_time = + rmf_traffic_ros2::convert(from.submission_time()); status.start_time = rmf_traffic_ros2::convert(from.start_time); status.end_time = rmf_traffic_ros2::convert(from.end_time); status.robot_name = from.robot_name; status.status = from.status; status.state = static_cast(from.state); + + if (from.description()) + status.task_profile.description = from.description()->to_msg(); + return status; } diff --git a/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp b/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp index d3b96e50f..31bef3d57 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp @@ -16,6 +16,7 @@ */ #include "Client.hpp" +#include namespace rmf_task_ros2 { namespace action { @@ -52,11 +53,8 @@ Client::Client(std::shared_ptr node) return; } - // TODO: hack to retain task profile and fleet name (to remove) - auto cache_profile = weak_status->task_profile; // update status to ptr - *weak_status = convert_status(*msg); - weak_status->task_profile = cache_profile; + weak_status->update_from_msg(*msg); if (weak_status->is_terminated()) RCLCPP_INFO(_node->get_logger(), @@ -67,12 +65,17 @@ Client::Client(std::shared_ptr node) } else { - // will still provide onchange even if the task_id is unknown. + /// This is when the task_id is unknown to the dispatcher node. Here + /// we will make and add the self-generated task from the fleet + /// adapter to the dispatcher queue (e.g. ChargeBattery Task) RCLCPP_DEBUG(_node->get_logger(), "[action] Unknown task: [%s]", task_id.c_str()); - auto task_status = std::make_shared(convert_status(*msg)); - _active_task_status[task_id] = task_status; - update_task_status(task_status); + const auto now = std::chrono::steady_clock::now(); + const auto desc = Description::make_from_msg( + msg->task_profile.description); + const auto status = TaskStatus::make(task_id, now, desc); + _active_task_status[task_id] = status; + update_task_status(status); } }); @@ -132,39 +135,41 @@ void Client::update_task_status(const TaskStatusPtr status) // erase terminated task and call on_terminate callback if (status->is_terminated()) { - _active_task_status.erase(status->task_profile.task_id); + _active_task_status.erase(status->task_id()); if (_on_terminate_callback) _on_terminate_callback(status); } } //============================================================================== -void Client::add_task( +void Client::dispatch_task( const std::string& fleet_name, - const TaskProfile& task_profile, TaskStatusPtr status_ptr) { + rmf_task_msgs::msg::TaskProfile task_profile; + task_profile.task_id = status_ptr->task_id(); + task_profile.description = status_ptr->description()->to_msg(); + task_profile.submission_time = + rmf_traffic_ros2::convert(status_ptr->submission_time()); + // send request and wait for acknowledgement RequestMsg request_msg; + request_msg.method = RequestMsg::ADD; request_msg.fleet_name = fleet_name; request_msg.task_profile = task_profile; - request_msg.method = RequestMsg::ADD; _request_msg_pub->publish(request_msg); // save status ptr status_ptr->fleet_name = fleet_name; - status_ptr->task_profile = task_profile; - _active_task_status[task_profile.task_id] = status_ptr; + _active_task_status[status_ptr->task_id()] = status_ptr; RCLCPP_DEBUG(_node->get_logger(), "Assign task: [%s] to fleet [%s]", - task_profile.task_id.c_str(), fleet_name.c_str()); + status_ptr->task_id().c_str(), fleet_name.c_str()); return; } //============================================================================== -bool Client::cancel_task( - const TaskProfile& task_profile) +bool Client::cancel_task(const std::string& task_id) { - const auto task_id = task_profile.task_id; RCLCPP_DEBUG(_node->get_logger(), "[action] Cancel Task: [%s]", task_id.c_str()); @@ -185,11 +190,17 @@ bool Client::cancel_task( return false; } + rmf_task_msgs::msg::TaskProfile task_profile; + task_profile.task_id = weak_status->task_id(); + task_profile.description = weak_status->description()->to_msg(); + task_profile.submission_time = + rmf_traffic_ros2::convert(weak_status->submission_time()); + // send cancel RequestMsg request_msg; + request_msg.method = RequestMsg::CANCEL; request_msg.fleet_name = weak_status->fleet_name; request_msg.task_profile = task_profile; - request_msg.method = RequestMsg::CANCEL; _request_msg_pub->publish(request_msg); return true; } diff --git a/rmf_task_ros2/src/rmf_task_ros2/action/Client.hpp b/rmf_task_ros2/src/rmf_task_ros2/action/Client.hpp index d39f1fbba..3628f490d 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/action/Client.hpp +++ b/rmf_task_ros2/src/rmf_task_ros2/action/Client.hpp @@ -44,28 +44,26 @@ class Client static std::shared_ptr make( std::shared_ptr node); - /// Add a task to a targeted fleet + /// Dispatch a task to a targeted fleet /// /// \param[in] fleet_name /// Target fleet which will execute this task /// - /// \param[in] task_profile - /// Task Description which will be executed - /// /// \param[out] status_ptr - /// Will update the status of the task here - void add_task( + /// task_status will have a task description member, which is used to + /// describe the task to dispatch. With this status ptr, recent status + /// of task will also get updated here. + void dispatch_task( const std::string& fleet_name, - const TaskProfile& task_profile, TaskStatusPtr status_ptr); /// Cancel an added task /// - /// \param[in] task_profile + /// \param[in] task_id /// Task which to cancel /// /// \return bool which indicate if cancel task is success - bool cancel_task(const TaskProfile& task_profile); + bool cancel_task(const std::string& task_id); /// Get the number of active task being track by client /// @@ -101,7 +99,7 @@ class Client std::shared_ptr _node; StatusCallback _on_change_callback; StatusCallback _on_terminate_callback; - std::unordered_map> _active_task_status; + std::unordered_map> _active_task_status; rclcpp::Publisher::SharedPtr _request_msg_pub; rclcpp::Subscription::SharedPtr _status_msg_sub; rclcpp::Subscription::SharedPtr _ack_msg_sub; diff --git a/rmf_task_ros2/test/action/test_ActionInterface.cpp b/rmf_task_ros2/test/action/test_ActionInterface.cpp index 11241a6e0..b3a61a4a6 100644 --- a/rmf_task_ros2/test/action/test_ActionInterface.cpp +++ b/rmf_task_ros2/test/action/test_ActionInterface.cpp @@ -30,15 +30,11 @@ namespace action { //============================================================================== SCENARIO("Action communication with client and server", "[ActionInterface]") { - TaskProfile task_profile1; - task_profile1.task_id = "task1"; - task_profile1.description.task_type.type = - rmf_task_msgs::msg::TaskType::TYPE_STATION; + const auto now = std::chrono::steady_clock::now(); - TaskProfile task_profile2; - task_profile2.task_id = "task2"; - task_profile2.description.task_type.type = - rmf_task_msgs::msg::TaskType::TYPE_STATION; + auto clean = description::Clean::make(now, "clean_here"); + const auto task_status1 = TaskStatus::make("task1", now, clean); + const auto task_status2 = TaskStatus::make("task2", now, clean); //============================================================================ @@ -51,7 +47,6 @@ SCENARIO("Action communication with client and server", "[ActionInterface]") auto node = rclcpp::Node::make_shared("test_ActionInferface"); auto action_server = Server::make(node, "test_server"); auto action_client = Client::make(node); - rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(node); @@ -77,40 +72,37 @@ SCENARIO("Action communication with client and server", "[ActionInterface]") WHEN("Add Task") { - // Add invalid Task! - TaskStatusPtr status_ptr(new TaskStatus); - - action_client->add_task("wrong_server", task_profile1, status_ptr); + action_client->dispatch_task("wrong_server", task_status1); executor.spin_until_future_complete(ready_future, rmf_traffic::time::from_seconds(0.5)); // should not receive cuz incorrect serverid - REQUIRE(status_ptr->state == TaskStatus::State::Pending); + REQUIRE(task_status1->state == TaskStatus::State::Pending); - action_client->add_task("test_server", task_profile1, status_ptr); + action_client->dispatch_task("test_server", task_status1); executor.spin_until_future_complete(ready_future, rmf_traffic::time::from_seconds(0.5)); // check status - REQUIRE(status_ptr->state == TaskStatus::State::Queued); + REQUIRE(task_status1->state == TaskStatus::State::Queued); + // TODO(YL) to fix this // status ptr is destroyed, should not have anymore tracking - status_ptr.reset(); - REQUIRE(action_client->size() == 0); + // task_status1.reset(); + // REQUIRE(action_client->size() == 0); } WHEN("Cancel Task") { // send valid task - TaskStatusPtr status_ptr(new TaskStatus); - action_client->add_task("test_server", task_profile2, status_ptr); + action_client->dispatch_task("test_server", task_status2); executor.spin_until_future_complete(ready_future, rmf_traffic::time::from_seconds(0.5)); // Invalid Cancel Task! - bool cancel_success = action_client->cancel_task(task_profile1); + bool cancel_success = action_client->cancel_task(task_status1->task_id()); executor.spin_until_future_complete(ready_future, rmf_traffic::time::from_seconds(0.5)); REQUIRE(!test_cancel_task); @@ -118,39 +110,37 @@ SCENARIO("Action communication with client and server", "[ActionInterface]") REQUIRE(action_client->size() == 1); // Valid Cancel task - action_client->cancel_task(task_profile2); + action_client->cancel_task(task_status2->task_id()); executor.spin_until_future_complete(ready_future, rmf_traffic::time::from_seconds(0.5)); REQUIRE(test_cancel_task); - REQUIRE(*test_cancel_task == task_profile2); - REQUIRE(status_ptr->is_terminated()); + REQUIRE(task_status2->is_terminated()); REQUIRE(action_client->size() == 0); } //============================================================================ - std::optional test_task_onchange; - std::optional test_task_onterminate; + TaskStatusPtr test_task_onchange = nullptr; + TaskStatusPtr test_task_onterminate = nullptr; // received status update from server action_client->on_change( [&test_task_onchange](const TaskStatusPtr status) { - test_task_onchange = *status; + test_task_onchange = status; } ); action_client->on_terminate( [&test_task_onterminate](const TaskStatusPtr status) { - test_task_onterminate = *status; + test_task_onterminate = status; } ); WHEN("On Change and On Terminate Task") { - TaskStatusPtr status_ptr(new TaskStatus); - action_client->add_task("test_server", task_profile1, status_ptr); + action_client->dispatch_task("test_server", task_status1); executor.spin_until_future_complete(ready_future, rmf_traffic::time::from_seconds(0.5)); @@ -158,12 +148,13 @@ SCENARIO("Action communication with client and server", "[ActionInterface]") REQUIRE(test_task_onchange); REQUIRE(test_task_onchange->state == TaskStatus::State::Queued); - TaskStatus server_task; - server_task.task_profile = task_profile1; - server_task.state = TaskStatus::State::Executing; + // this task is received by server + const auto server_task = + TaskStatus::make(test_add_task->task_id, now, clean); + server_task->state = TaskStatus::State::Executing; // Update it as executing - action_server->update_status(server_task); + action_server->update_status(*server_task); executor.spin_until_future_complete(ready_future, rmf_traffic::time::from_seconds(1.5)); @@ -171,9 +162,9 @@ SCENARIO("Action communication with client and server", "[ActionInterface]") REQUIRE(!test_task_onterminate); // havnt terminated yet // completion - server_task.state = TaskStatus::State::Completed; + server_task->state = TaskStatus::State::Completed; // Update it as executing - action_server->update_status(server_task); + action_server->update_status(*server_task); executor.spin_until_future_complete(ready_future, rmf_traffic::time::from_seconds(0.5)); diff --git a/rmf_task_ros2/test/dispatcher/test_Description.cpp b/rmf_task_ros2/test/dispatcher/test_Description.cpp index 29b9ec13c..340ad8779 100644 --- a/rmf_task_ros2/test/dispatcher/test_Description.cpp +++ b/rmf_task_ros2/test/dispatcher/test_Description.cpp @@ -16,7 +16,7 @@ */ #include -#include +#include #include namespace rmf_task_ros2 { @@ -46,6 +46,12 @@ SCENARIO("Task Description Test", "[Description]") auto delivery2 = description::Delivery::make_from_msg(msg); REQUIRE(!delivery2); + // Create TaskStatus Object + auto delivery_status = TaskStatus::make("Delivery001", now, delivery); + REQUIRE(delivery_status->task_id() == "Delivery001"); + REQUIRE(delivery_status->state == TaskStatus::State::Pending); + REQUIRE(!delivery_status->is_terminated()); + //============================================================================== // test loop descripttion msg auto loop = description::Loop::make(now, "start_yo", "end_yo", 1); diff --git a/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp b/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp index 70a3c44aa..ab44a59e4 100644 --- a/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp +++ b/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp @@ -100,11 +100,11 @@ SCENARIO("Dispatcehr API Test", "[Dispatcher]") //============================================================================ // test on change fn callback int change_times = 0; - TaskProfile test_taskprofile; + std::string test_dispatcher_received_id = ""; dispatcher->on_change( - [&change_times, &test_taskprofile](const TaskStatusPtr status) + [&change_times, &test_dispatcher_received_id](const TaskStatusPtr status) { - test_taskprofile = status->task_profile; + test_dispatcher_received_id = status->task_id(); change_times++; } ); @@ -120,14 +120,14 @@ SCENARIO("Dispatcehr API Test", "[Dispatcher]") std::this_thread::sleep_for(std::chrono::milliseconds(3500)); CHECK(dispatcher->get_task_state(*id) == TaskStatus::State::Failed); REQUIRE(dispatcher->terminated_tasks().size() == 1); - REQUIRE(test_taskprofile.task_id == id); + REQUIRE(test_dispatcher_received_id == id); CHECK(change_times == 2); // add and failed // Submit another task id = dispatcher->submit_task(clean_task); std::this_thread::sleep_for(std::chrono::milliseconds(3000)); REQUIRE(dispatcher->terminated_tasks().size() == 2); - REQUIRE(test_taskprofile.task_id == *id); + REQUIRE(test_dispatcher_received_id == *id); CHECK(change_times == 4); // add and failed x2 } @@ -162,9 +162,9 @@ SCENARIO("Dispatcehr API Test", "[Dispatcher]") auto t = std::thread( [&action_server, &task_canceled_flag](auto profile) { - TaskStatus status; - status.task_profile = profile; - status.robot_name = "dumbot"; + const auto status = TaskStatus::make( + profile.task_id, std::chrono::steady_clock::now(), nullptr); + status->robot_name = "dumbot"; std::this_thread::sleep_for(std::chrono::seconds(2)); if (task_canceled_flag) @@ -174,13 +174,13 @@ SCENARIO("Dispatcehr API Test", "[Dispatcher]") } // Executing - status.state = TaskStatus::State::Executing; - action_server->update_status(status); + status->state = TaskStatus::State::Executing; + action_server->update_status(*status); std::this_thread::sleep_for(std::chrono::seconds(1)); // Completed - status.state = TaskStatus::State::Completed; - action_server->update_status(status); + status->state = TaskStatus::State::Completed; + action_server->update_status(*status); }, task_profile ); t.detach(); @@ -213,12 +213,17 @@ SCENARIO("Dispatcehr API Test", "[Dispatcher]") CHECK(change_times == 4); // Pending > Queued > Executing > Completed // Add auto generated ChargeBattery Task from fleet adapter - TaskStatus status; - status.task_profile.task_id = "ChargeBattery10"; - status.state = TaskStatus::State::Queued; - status.task_profile.description.task_type.type = - rmf_task_msgs::msg::TaskType::TYPE_CHARGE_BATTERY; - action_server->update_status(status); + /// TODO; Currently needa think of a way of makeChargeDescription class + const auto status = TaskStatus::make( + "ChargeBattery10", std::chrono::steady_clock::now(), nullptr); + status->state = TaskStatus::State::Queued; + + // TaskStatus status; + // status.task_profile.task_id = "ChargeBattery10"; + // status.task_profile.description.task_type.type = + // rmf_task_msgs::msg::TaskType::TYPE_CHARGE_BATTERY; + + action_server->update_status(*status); std::this_thread::sleep_for(std::chrono::seconds(1)); CHECK(change_times == 5); // new stray charge task @@ -239,7 +244,7 @@ SCENARIO("Dispatcehr API Test", "[Dispatcher]") REQUIRE(dispatcher->active_tasks().size() == 0); REQUIRE(dispatcher->terminated_tasks().size() == 1); REQUIRE(dispatcher->terminated_tasks().begin()->first == *id); - auto status = dispatcher->terminated_tasks().begin()->second; + const auto status = dispatcher->terminated_tasks().begin()->second; CHECK(status->state == TaskStatus::State::Canceled); CHECK(change_times == 3); // Pending -> Queued -> Canceled } From 02ca599053ae346d315bd2fd2d9073e414565e13 Mon Sep 17 00:00:00 2001 From: youliang Date: Mon, 1 Mar 2021 14:01:24 +0800 Subject: [PATCH 08/10] replace desc with profile in task Signed-off-by: youliang --- rmf_fleet_adapter/src/full_control/main.cpp | 2 - .../src/rmf_fleet_adapter/Task.cpp | 14 +- .../src/rmf_fleet_adapter/Task.hpp | 15 +- .../src/rmf_fleet_adapter/TaskManager.cpp | 109 +++++++++-- .../src/rmf_fleet_adapter/TaskManager.hpp | 10 +- .../agv/FleetUpdateHandle.cpp | 169 +++++------------- .../agv/internal_FleetUpdateHandle.hpp | 21 +-- .../rmf_fleet_adapter/phases/GoToPlace.cpp | 1 - .../rmf_fleet_adapter/tasks/ChargeBattery.cpp | 2 - .../rmf_fleet_adapter/tasks/ChargeBattery.hpp | 1 - .../src/rmf_fleet_adapter/tasks/Clean.cpp | 2 - .../src/rmf_fleet_adapter/tasks/Clean.hpp | 1 - .../src/rmf_fleet_adapter/tasks/Delivery.cpp | 2 - .../src/rmf_fleet_adapter/tasks/Delivery.hpp | 1 - .../src/rmf_fleet_adapter/tasks/Loop.cpp | 2 - .../src/rmf_fleet_adapter/tasks/Loop.hpp | 1 - rmf_fleet_adapter/test/test_Task.cpp | 6 +- .../src/rmf_task_ros2/action/Client.cpp | 1 + .../test/dispatcher/test_Dispatcher.cpp | 6 - 19 files changed, 163 insertions(+), 203 deletions(-) diff --git a/rmf_fleet_adapter/src/full_control/main.cpp b/rmf_fleet_adapter/src/full_control/main.cpp index 4b2998ac1..96fae0c73 100644 --- a/rmf_fleet_adapter/src/full_control/main.cpp +++ b/rmf_fleet_adapter/src/full_control/main.cpp @@ -670,8 +670,6 @@ std::shared_ptr make_fleet( if (node->declare_parameter("perform_deliveries", false)) { task_types.insert(rmf_task_msgs::msg::TaskType::TYPE_DELIVERY); - connections->fleet->accept_delivery_requests( - [](const rmf_task_msgs::msg::Delivery&){ return true; }); } if (node->declare_parameter("perform_cleaning", false)) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp index e05116956..75aaf7ba8 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp @@ -29,7 +29,6 @@ namespace rmf_fleet_adapter { //============================================================================== std::shared_ptr Task::make( std::string id, - rmf_task_ros2::ConstDescriptionPtr description, PendingPhases phases, rxcpp::schedulers::worker worker, rmf_traffic::Time deployment_time, @@ -38,7 +37,6 @@ std::shared_ptr Task::make( { return std::make_shared( Task(std::move(id), - std::move(description), std::move(phases), std::move(worker), deployment_time, @@ -91,9 +89,15 @@ const std::string& Task::id() const } //============================================================================== -const rmf_task_ros2::ConstDescriptionPtr Task::description() const +const rmf_task_msgs::msg::TaskProfile Task::profile_msg() const { - return _description; + return _profile_msg; +} + +//============================================================================== +void Task::profile_msg(const rmf_task_msgs::msg::TaskProfile& profile) +{ + _profile_msg = profile; } //============================================================================== @@ -117,14 +121,12 @@ const rmf_task::agv::State Task::finish_state() const //============================================================================== Task::Task( std::string id, - rmf_task_ros2::ConstDescriptionPtr description, std::vector> phases, rxcpp::schedulers::worker worker, rmf_traffic::Time deployment_time, rmf_task::agv::State finish_state, rmf_task::ConstRequestPtr request) : _id(std::move(id)), - _description(std::move(description)), _pending_phases(std::move(phases)), _worker(std::move(worker)), _deployment_time(deployment_time), diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp index 3e565e2a4..cf701fe0e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp @@ -28,7 +28,6 @@ #include #include -#include #include #include @@ -41,6 +40,7 @@ class Task : public std::enable_shared_from_this public: using StatusMsg = rmf_task_msgs::msg::TaskSummary; + using TaskProfile = rmf_task_msgs::msg::TaskProfile; /// This class represents the active phase of a Task. It provides an /// observable that the Task can track to stay up-to-date on the status and to @@ -100,7 +100,6 @@ class Task : public std::enable_shared_from_this // Make a new task static std::shared_ptr make( std::string id, - rmf_task_ros2::ConstDescriptionPtr description, PendingPhases phases, rxcpp::schedulers::worker worker, rmf_traffic::Time deployment_time, @@ -125,11 +124,12 @@ class Task : public std::enable_shared_from_this void cancel(); const std::string& id() const; + + /// Get the profile msg, for publish status msg + const TaskProfile profile_msg() const; - // const rmf_task_ros2::msg::TaskProfile profile() const; - - /// Task description - const rmf_task_ros2::ConstDescriptionPtr description() const; + /// Set the profile msg, for publish status msg + void profile_msg(const TaskProfile& profile); /// Get the request used to generate this task const rmf_task::ConstRequestPtr request() const; @@ -144,7 +144,6 @@ class Task : public std::enable_shared_from_this Task( std::string id, - rmf_task_ros2::ConstDescriptionPtr description, PendingPhases phases, rxcpp::schedulers::worker worker, rmf_traffic::Time deployment_time, @@ -170,7 +169,7 @@ class Task : public std::enable_shared_from_this rmf_traffic::Time _deployment_time; rmf_task::agv::State _finish_state; rmf_task::ConstRequestPtr _request; - rmf_task_ros2::ConstDescriptionPtr _description = nullptr; + TaskProfile _profile_msg; void _start_next_phase(); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 9b200c18b..50c937642 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -95,9 +95,8 @@ void TaskManager::queue_task(std::shared_ptr task) _queue.size()); rmf_task_msgs::msg::TaskSummary msg; + msg.task_profile = task->profile_msg(); msg.task_id = task->id(); // duplicated - msg.task_profile.task_id = task->id(); - msg.task_profile.description = task->description()->to_msg(); msg.state = msg.STATE_QUEUED; msg.robot_name = _context->name(); msg.fleet_name = _context->description().owner(); @@ -161,6 +160,84 @@ void TaskManager::set_queue( queue_task(t); } +//============================================================================== +void TaskManager::set_queue( + const std::vector& assignments, + const std::unordered_map& task_profiles) +{ + std::vector> task_queue; + std::shared_ptr task = nullptr; + for (std::size_t i = 0; i < assignments.size(); ++i) + { + const auto& a = assignments[i]; + auto start = _context->current_task_end_state().location(); + if (i != 0) + start = assignments[i-1].state().location(); + start.time(a.deployment_time()); + + const auto req = a.request(); + const auto id = req->id(); + const auto req_desc = req->description(); + + rmf_task_msgs::msg::TaskProfile profile; + + if (task_profiles.find(id) != task_profiles.end()) + { + profile = task_profiles.at(id); + } + else + { + profile.task_id = id; + profile.submission_time = _context->node()->now(); + profile.description.start_time = + rmf_traffic_ros2::convert(a.deployment_time()); + } + + // We use dynamic cast to determine the type of request and then call the + // appropriate make(~) function to convert the request into a task + using namespace rmf_task::requests; + + /// CHARGE BATTERY TASK (Auto-Generated) + if (std::dynamic_pointer_cast(req_desc)) + { + task = tasks::make_charge_battery( + req, _context, start, a.deployment_time(), a.state()); + profile.description.task_type.type = + rmf_task_msgs::msg::TaskType::TYPE_CHARGE_BATTERY; + } + /// CLEAN TASK (User Requested) + else if (std::dynamic_pointer_cast(req_desc)) + { + task = tasks::make_clean( + req, _context, start, a.deployment_time(), a.state()); + } + /// DELIVERY TASK (User Requested) + else if (std::dynamic_pointer_cast(req_desc)) + { + task = tasks::make_delivery( + req, _context, start, a.deployment_time(), a.state()); + } + /// LOOP TASK (User Requested) + else if (std::dynamic_pointer_cast(req_desc)) + { + task = tasks::make_loop( + req, _context, start, a.deployment_time(), a.state()); + } + else + { + RCLCPP_WARN( + _context->node()->get_logger(), + "[TaskManager] Un-supported request type in assignment list. " + "Please update the implementation of TaskManager::set_queue() to " + "support request with task_id:[%s]", id); + continue; + } + task->profile_msg(profile); + task_queue.push_back(task); + } + this->set_queue(task_queue); +} + //============================================================================== agv::ConstRobotContextPtr TaskManager::context() const { @@ -227,9 +304,8 @@ void TaskManager::_begin_next_task() .subscribe( [this, id = _active_task->id()](Task::StatusMsg msg) { - msg.task_id = id; - msg.task_profile.task_id = id; - msg.task_profile.description = _active_task->description()->to_msg(); + msg.task_profile = _active_task->profile_msg(); + msg.task_id = _active_task->id(); // duplicated msg.robot_name = _context->name(); msg.fleet_name = _context->description().owner(); msg.start_time = rmf_traffic_ros2::convert( @@ -250,9 +326,8 @@ void TaskManager::_begin_next_task() msg.status = e.what(); } - msg.task_id = id; - msg.task_profile.task_id = id; - msg.task_profile.description = _active_task->description()->to_msg(); + msg.task_profile = _active_task->profile_msg(); + msg.task_id = _active_task->id(); // duplicated msg.robot_name = _context->name(); msg.fleet_name = _context->description().owner(); msg.start_time = rmf_traffic_ros2::convert( @@ -264,9 +339,8 @@ void TaskManager::_begin_next_task() [this, id = _active_task->id()]() { rmf_task_msgs::msg::TaskSummary msg; - msg.task_id = id; - msg.task_profile.task_id = id; - msg.task_profile.description = _active_task->description()->to_msg(); + msg.task_profile = _active_task->profile_msg(); + msg.task_id = _active_task->id(); // duplicated msg.state = msg.STATE_COMPLETED; msg.robot_name = _context->name(); msg.fleet_name = _context->description().owner(); @@ -275,7 +349,6 @@ void TaskManager::_begin_next_task() msg.end_time = rmf_traffic_ros2::convert( _active_task->finish_state().finish_time()); this->_context->node()->task_summary()->publish(msg); - _active_task = nullptr; }); @@ -379,19 +452,21 @@ void TaskManager::retreat_to_charger() finish.value().finish_state(), current_state.finish_time()); - const auto task_desc = rmf_task_ros2::Description::make_description( - charging_assignment.deployment_time(), - rmf_task_msgs::msg::TaskType::TYPE_CHARGE_BATTERY); + rmf_task_msgs::msg::TaskProfile task_profile; + task_profile.task_id = charging_request->id(); + task_profile.submission_time = _context->node()->now(); + task_profile.description.start_time = _context->node()->now(); + task_profile.description.task_type.type = + rmf_task_msgs::msg::TaskType::TYPE_CHARGE_BATTERY; // TODO(YL) Check time input const auto task = rmf_fleet_adapter::tasks::make_charge_battery( - task_desc, charging_request, _context, current_state.location(), charging_assignment.deployment_time(), charging_assignment.state()); - + task->profile_msg(task_profile); queue_task(task); RCLCPP_INFO( diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp index c25b34cf4..a19541f22 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp @@ -45,6 +45,7 @@ class TaskManager : public std::enable_shared_from_this using StartSet = rmf_traffic::agv::Plan::StartSet; using Assignment = rmf_task::agv::TaskPlanner::Assignment; using State = rmf_task::agv::State; + using TaskProfile = rmf_task_msgs::msg::TaskProfile; /// Add a task to the queue of this manager. void queue_task(std::shared_ptr task); @@ -55,9 +56,11 @@ class TaskManager : public std::enable_shared_from_this const Task* current_task() const; - // /// Set the queue for this task manager with assignments generated from the - // /// task planner - // void set_queue(const std::vector& assignments); + /// Set the queue for this task manager with assignments generated from the + /// task planner, arg 'task_profiles' is merely used for status publishing. + void set_queue( + const std::vector& assignments, + const std::unordered_map& task_profiles = {}); /// set a vector of tasks void set_queue(const std::vector>& tasks); @@ -75,7 +78,6 @@ class TaskManager : public std::enable_shared_from_this /// when robot is idle and battery level drops below a retreat threshold. void retreat_to_charger(); - /// TODO(YL) not needed, should be removed? /// Get the list of task ids for tasks that have started execution. /// The list will contain upto 100 latest task ids only. const std::vector& get_executed_tasks() const; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index 954b7ee43..5f740d0bd 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -36,13 +36,6 @@ #include #include -#include - -#include "../tasks/Clean.hpp" -#include "../tasks/ChargeBattery.hpp" -#include "../tasks/Delivery.hpp" -#include "../tasks/Loop.hpp" - #include #include #include @@ -113,8 +106,8 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( if (msg->task_profile.task_id.empty()) return; - if (task_descriptions.find(msg->task_profile.task_id) - != task_descriptions.end()) + if (received_task_profiles.find(msg->task_profile.task_id) + != received_task_profiles.end()) { RCLCPP_WARN( node->get_logger(), @@ -162,13 +155,11 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( rmf_task::ConstRequestPtr new_request = nullptr; const auto& task_profile = msg->task_profile; const auto& task_type = task_profile.description.task_type; - const auto& description_msg = task_profile.description; const rmf_traffic::Time start_time = rmf_traffic_ros2::convert(task_profile.description.start_time); // TODO (YV) get rid of ID field in RequestPtr const std::string id = msg->task_profile.task_id; const auto& graph = planner->get_configuration().graph(); - rmf_task_ros2::ConstDescriptionPtr task_description; // Generate the priority of the request. The current implementation supports // binary priority @@ -180,10 +171,9 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( // Process Cleaning task if (task_type.type == TaskType::TYPE_CLEAN) { - const auto& clean = - rmf_task_ros2::description::Clean::make_from_msg(description_msg); + const auto& clean = task_profile.description.clean; - if (!clean) + if (clean.start_waypoint.empty()) { RCLCPP_ERROR(node->get_logger(), "Clean Msg is invalid/invalid." @@ -192,26 +182,26 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( } // Check for valid start waypoint - const auto start_wp = graph.find_waypoint(clean->start_waypoint()); + const auto start_wp = graph.find_waypoint(clean.start_waypoint); 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(), clean->start_waypoint().c_str(), id.c_str()); + name.c_str(), clean.start_waypoint.c_str(), id.c_str()); return; } // Get dock parameters - const auto clean_param_it = dock_param_map.find(clean->start_waypoint()); + const auto clean_param_it = dock_param_map.find(clean.start_waypoint); if (clean_param_it == dock_param_map.end()) { RCLCPP_INFO( node->get_logger(), "Dock param for dock_name:[%s] unavailable. Rejecting BidNotice with " - "task_id:[%s]", clean->start_waypoint().c_str(), id.c_str()); + "task_id:[%s]", clean.start_waypoint.c_str(), id.c_str()); return; } @@ -246,7 +236,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( RCLCPP_INFO( node->get_logger(), "Unable to generate cleaning trajectory from positions specified " - " in DockSummary msg for [%s]", clean->start_waypoint().c_str()); + " in DockSummary msg for [%s]", clean.start_waypoint.c_str()); return; } @@ -267,16 +257,14 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( RCLCPP_INFO( node->get_logger(), "Generated Clean request for task_id:[%s]", id.c_str()); - - task_description = clean; } else if (task_type.type == TaskType::TYPE_DELIVERY) { - const auto& delivery = - rmf_task_ros2::description::Delivery::make_from_msg(description_msg); + const auto& del = task_profile.description.delivery; - if (!delivery) + if (del.pickup_place_name.empty() || del.pickup_dispenser.empty() || + del.dropoff_place_name.empty() || del.dropoff_ingestor.empty()) { RCLCPP_ERROR(node->get_logger(), "Delivery Msg is invalid/invalid." @@ -284,35 +272,35 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( return; } - const auto pickup_wp = graph.find_waypoint(delivery->pickup_place_name()); + const auto pickup_wp = graph.find_waypoint(del.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()); + name.c_str(), del.pickup_place_name.c_str(), id.c_str()); return; } - const auto dropoff_wp = graph.find_waypoint(delivery->dropoff_place_name()); + const auto dropoff_wp = graph.find_waypoint(del.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()); + name.c_str(), del.dropoff_place_name.c_str(), id.c_str()); return; } new_request = rmf_task::requests::Delivery::make( id, pickup_wp->index(), - description_msg.delivery.pickup_dispenser, + del.pickup_dispenser, dropoff_wp->index(), - description_msg.delivery.dropoff_ingestor, - description_msg.delivery.items, + del.dropoff_ingestor, + del.items, motion_sink, ambient_sink, planner, @@ -323,15 +311,13 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( RCLCPP_INFO( node->get_logger(), "Generated Delivery request for task_id:[%s]", id.c_str()); - - task_description = delivery; } else if (task_type.type == TaskType::TYPE_LOOP) { - const auto& loop = - rmf_task_ros2::description::Loop::make_from_msg(description_msg); + const auto& loop = task_profile.description.loop; - if (!loop) + if (loop.start_name.empty() || loop.finish_name.empty() || + loop.num_loops == 0) { RCLCPP_ERROR(node->get_logger(), "Loop Msg is invalid/invalid." @@ -339,26 +325,26 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( return; } - const auto start_wp = graph.find_waypoint(loop->start_name()); + 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()); + name.c_str(), loop.start_name.c_str(), id.c_str()); return; } - const auto finish_wp = graph.find_waypoint(loop->finish_name()); + 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->start_name().c_str(), id.c_str()); + name.c_str(), loop.start_name.c_str(), id.c_str()); return; } @@ -367,7 +353,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( id, start_wp->index(), finish_wp->index(), - description_msg.loop.num_loops, + loop.num_loops, motion_sink, ambient_sink, planner, @@ -378,8 +364,6 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( RCLCPP_INFO( node->get_logger(), "Generated Loop request for task_id:[%s]", id.c_str()); - - task_description = loop; } else { @@ -468,7 +452,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( // Store assignments in internal map latest_bid_notice_assignments = {id, new_request, assignments}; - task_descriptions.insert({id, task_description}); + received_task_profiles.insert({id, task_profile}); } //============================================================================== @@ -534,7 +518,11 @@ void FleetUpdateHandle::Implementation::dispatch_request_cb( // rxcpp worker. Hence, no new tasks would have started during this replanning. } - set_assignments_to_task_managers(assignments); + // set assignments to taskmangaer of each robot + assert(assignments.size() == task_managers.size()); + for (std::size_t i = 0; i < task_managers.size(); ++i) + task_managers[i]->set_queue(assignments[i], received_task_profiles); + current_assignment_cost = task_planner->compute_cost(assignments); dispatch_ack.success = true; dispatch_ack_pub->publish(dispatch_ack); @@ -573,7 +561,6 @@ void FleetUpdateHandle::Implementation::dispatch_request_cb( executed_tasks.insert(mgr->current_task()->id()); for (const auto& tsk : mgr->task_queue()) queued_tasks.insert(tsk->id()); - // TODO DEBUG! } if (queued_tasks.find(id) == queued_tasks.end()) @@ -615,10 +602,13 @@ void FleetUpdateHandle::Implementation::dispatch_request_cb( return; } + // set assignments to taskmangaer of each robot const auto& assignments = replan_results.value(); - set_assignments_to_task_managers(assignments); - current_assignment_cost = task_planner->compute_cost(assignments); + assert(assignments.size() == task_managers.size()); + for (std::size_t i = 0; i < task_managers.size(); ++i) + task_managers[i]->set_queue(assignments[i], received_task_profiles); + current_assignment_cost = task_planner->compute_cost(assignments); dispatch_ack.success = true; dispatch_ack_pub->publish(dispatch_ack); cancelled_task_ids.insert(id); @@ -811,7 +801,8 @@ auto FleetUpdateHandle::Implementation::allocate_tasks( for (const auto task : t->task_queue()) { - if (task->description()->type() == TaskType::TYPE_CHARGE_BATTERY) + const auto type = task->profile_msg().description.task_type.type; + if ( type == TaskType::TYPE_CHARGE_BATTERY) { // ignore auto allocated charging task continue; @@ -894,84 +885,6 @@ auto FleetUpdateHandle::Implementation::allocate_tasks( return assignments; } -//============================================================================== -void FleetUpdateHandle::Implementation::set_assignments_to_task_managers( - const Assignments& assignments) -{ - assert(assignments.size() == task_managers.size()); - std::size_t tm_index = 0; - - for (auto& t : task_managers) - { - std::vector> task_queue; - std::shared_ptr task = nullptr; - for (std::size_t i = 0; i < assignments[tm_index].size(); ++i) - { - const auto& a = assignments[tm_index][i]; - auto start = t->context()->current_task_end_state().location(); - if (i != 0) - start = assignments[tm_index][i-1].state().location(); - start.time(a.deployment_time()); - - const auto req = a.request(); - const auto id = req->id(); - const auto req_desc = req->description(); - - rmf_task_ros2::ConstDescriptionPtr desc; - if (task_descriptions.find(id) != task_descriptions.end()) - { - desc = task_descriptions[id]; - } - else - { - desc = rmf_task_ros2::Description::make_description( - a.deployment_time(), TaskType::TYPE_CHARGE_BATTERY); - } - - // We use dynamic cast to determine the type of request and then call the - // appropriate make(~) function to convert the request into a task - /// CHARGE BATTERY TASK - using namespace rmf_task::requests; - if (std::dynamic_pointer_cast(req_desc)) - { - task = tasks::make_charge_battery( - desc, req, t->context(), start, a.deployment_time(), a.state()); - } - /// CLEAN TASK - else if (std::dynamic_pointer_cast(req_desc)) - { - task = tasks::make_clean( - desc, req, t->context(), start, a.deployment_time(), a.state()); - } - /// DELIVERY TASK - else if (std::dynamic_pointer_cast(req_desc)) - { - task = tasks::make_delivery( - desc, req, t->context(), start, a.deployment_time(), a.state()); - } - /// LOOP TASK - else if (std::dynamic_pointer_cast(req_desc)) - { - task = tasks::make_loop( - desc, req, t->context(), start, a.deployment_time(), a.state()); - } - else - { - RCLCPP_WARN( - t->context()->node()->get_logger(), - "[TaskManager] Un-supported request type in assignment list. " - "Please update the implementation of TaskManager::set_queue() to " - "support request with task_id:[%s]", id); - continue; - } - - task_queue.push_back(task); - } - t->set_queue(task_queue); - ++tm_index; - } -} - //============================================================================== void FleetUpdateHandle::add_robot( std::shared_ptr command, @@ -1071,7 +984,7 @@ FleetUpdateHandle& FleetUpdateHandle::accept_task_requests( FleetUpdateHandle& FleetUpdateHandle::accept_delivery_requests( AcceptDeliveryRequest check) { - _pimpl->accept_delivery = std::move(check); + RCLCPP_WARN(_pimpl->node->get_logger(), "Use accept_task_requests() instead"); return *this; } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp index d5488bd25..68192e668 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp @@ -152,17 +152,12 @@ class FleetUpdateHandle::Implementation rmf_utils::optional default_maximum_delay = std::chrono::nanoseconds(std::chrono::seconds(10)); - AcceptDeliveryRequest accept_delivery = nullptr; - // std::unordered_map> task_managers = {}; std::vector> task_managers; rclcpp::Publisher::SharedPtr fleet_state_pub = nullptr; rclcpp::TimerBase::SharedPtr fleet_state_timer = nullptr; - // Map task id to pair of using Assignments = rmf_task::agv::TaskPlanner::Assignments; - std::unordered_map> task_map = {}; // Map of dock name to dock parameters std::unordered_map bid_notice_assignments = {}; - // std::unordered_map< - // std::string, rmf_task::ConstRequestPtr> generated_requests = {}; - - std::unordered_map< - std::string, rmf_task_ros2::ConstDescriptionPtr> task_descriptions = {}; + // Map to store task id with the profile msg of the received tasks + using TaskProfile = rmf_task_msgs::msg::TaskProfile; + std::unordered_map received_task_profiles = {}; + + // Tuple which stored the latest received BidNotice std::tuple latest_bid_notice_assignments; @@ -305,9 +297,6 @@ class FleetUpdateHandle::Implementation /// invalid if one of the assignments has already begun execution. bool is_valid_assignments(Assignments& assignments) const; - /// Helper function to create assign tasks to task managers - void set_assignments_to_task_managers(const Assignments& assignments1); - static Implementation& get(FleetUpdateHandle& fleet) { return *fleet._pimpl; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.cpp index 99cc32164..b9649941b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.cpp @@ -552,7 +552,6 @@ void GoToPlace::Active::execute_plan(rmf_traffic::agv::Plan new_plan) rmf_task::agv::State dummy_state{{dummy_time, 0, 0.0}, 0, 1.0}; _subtasks = Task::make( _description, - nullptr, std::move(sub_phases), _context->worker(), dummy_time, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp index d5bef9da6..f3cf033f8 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp @@ -26,7 +26,6 @@ namespace tasks { //============================================================================== std::shared_ptr make_charge_battery( - const rmf_task_ros2::ConstDescriptionPtr task_description, const rmf_task::ConstRequestPtr request, const agv::RobotContextPtr& context, const rmf_traffic::agv::Plan::Start start, @@ -50,7 +49,6 @@ std::shared_ptr make_charge_battery( return Task::make( request->id(), - std::move(task_description), std::move(phases), context->worker(), deployment_time, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp index 2b2aebc08..348fbe95f 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp @@ -32,7 +32,6 @@ namespace tasks { //============================================================================== std::shared_ptr make_charge_battery( - const rmf_task_ros2::ConstDescriptionPtr task_description, const rmf_task::ConstRequestPtr request, const agv::RobotContextPtr& context, const rmf_traffic::agv::Plan::Start start, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp index 0fa638502..6c80a6df1 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp @@ -24,7 +24,6 @@ namespace tasks { //============================================================================== std::shared_ptr make_clean( - const rmf_task_ros2::ConstDescriptionPtr task_description, const rmf_task::ConstRequestPtr request, const agv::RobotContextPtr& context, const rmf_traffic::agv::Plan::Start clean_start, @@ -49,7 +48,6 @@ std::shared_ptr make_clean( return Task::make( request->id(), - std::move(task_description), std::move(phases), context->worker(), deployment_time, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp index d803edb95..650145007 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp @@ -32,7 +32,6 @@ namespace tasks { //============================================================================== std::shared_ptr make_clean( - const rmf_task_ros2::ConstDescriptionPtr task_description, const rmf_task::ConstRequestPtr request, const agv::RobotContextPtr& context, const rmf_traffic::agv::Plan::Start clean_start, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp index 3e110b076..c3dc463d2 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp @@ -28,7 +28,6 @@ namespace tasks { //============================================================================== std::shared_ptr make_delivery( - const rmf_task_ros2::ConstDescriptionPtr task_description, const rmf_task::ConstRequestPtr request, const agv::RobotContextPtr& context, const rmf_traffic::agv::Plan::Start pickup_start, @@ -85,7 +84,6 @@ std::shared_ptr make_delivery( return Task::make( request->id(), - std::move(task_description), std::move(phases), context->worker(), deployment_time, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp index 38b00c87a..2bc0f9e36 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp @@ -28,7 +28,6 @@ namespace tasks { //============================================================================== std::shared_ptr make_delivery( - const rmf_task_ros2::ConstDescriptionPtr task_description, const rmf_task::ConstRequestPtr request, const agv::RobotContextPtr& context, const rmf_traffic::agv::Plan::Start pickup_start, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp index 4903c1d90..d4013b066 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp @@ -24,7 +24,6 @@ namespace tasks { //============================================================================== std::shared_ptr make_loop( - const rmf_task_ros2::ConstDescriptionPtr task_description, const rmf_task::ConstRequestPtr request, const agv::RobotContextPtr& context, const rmf_traffic::agv::Plan::Start start, @@ -63,7 +62,6 @@ std::shared_ptr make_loop( return Task::make( request->id(), - std::move(task_description), std::move(phases), context->worker(), deployment_time, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.hpp index a3060cd92..4c31fc788 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.hpp @@ -28,7 +28,6 @@ namespace tasks { //============================================================================== std::shared_ptr make_loop( - const rmf_task_ros2::ConstDescriptionPtr task_description, const rmf_task::ConstRequestPtr request, const agv::RobotContextPtr& context, const rmf_traffic::agv::Plan::Start start, diff --git a/rmf_fleet_adapter/test/test_Task.cpp b/rmf_fleet_adapter/test/test_Task.cpp index 6db4354a1..2378174f6 100644 --- a/rmf_fleet_adapter/test/test_Task.cpp +++ b/rmf_fleet_adapter/test/test_Task.cpp @@ -179,7 +179,7 @@ class MockSubtaskPhase Active(PendingPhases phases) : _subtasks( rmf_fleet_adapter::Task::make( - "subtasks", nullptr, std::move(phases), + "subtasks", std::move(phases), rxcpp::schedulers::make_event_loop().create_worker(), std::chrono::steady_clock::now(), {{std::chrono::steady_clock::now(), 0, 0.0}, 0, 1.0}, @@ -299,7 +299,7 @@ SCENARIO("Test simple task") std::shared_ptr task = rmf_fleet_adapter::Task::make( - "test_Task", nullptr, std::move(phases), + "test_Task", std::move(phases), rxcpp::schedulers::make_event_loop().create_worker(), deployment_time, finish_state, nullptr); @@ -391,7 +391,7 @@ SCENARIO("Test nested task") rmf_task::agv::State finish_state{{deployment_time, 0, 0.0}, 0, 1.0}; const auto task = rmf_fleet_adapter::Task::make( - "test_NestedTask", nullptr, std::move(phases), + "test_NestedTask", std::move(phases), rxcpp::schedulers::make_event_loop().create_worker(), deployment_time, finish_state, nullptr); diff --git a/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp b/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp index 31bef3d57..cd1088be1 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp @@ -74,6 +74,7 @@ Client::Client(std::shared_ptr node) const auto desc = Description::make_from_msg( msg->task_profile.description); const auto status = TaskStatus::make(task_id, now, desc); + status->update_from_msg(*msg); _active_task_status[task_id] = status; update_task_status(status); } diff --git a/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp b/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp index ab44a59e4..bb90facc1 100644 --- a/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp +++ b/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp @@ -213,16 +213,10 @@ SCENARIO("Dispatcehr API Test", "[Dispatcher]") CHECK(change_times == 4); // Pending > Queued > Executing > Completed // Add auto generated ChargeBattery Task from fleet adapter - /// TODO; Currently needa think of a way of makeChargeDescription class const auto status = TaskStatus::make( "ChargeBattery10", std::chrono::steady_clock::now(), nullptr); status->state = TaskStatus::State::Queued; - // TaskStatus status; - // status.task_profile.task_id = "ChargeBattery10"; - // status.task_profile.description.task_type.type = - // rmf_task_msgs::msg::TaskType::TYPE_CHARGE_BATTERY; - action_server->update_status(*status); std::this_thread::sleep_for(std::chrono::seconds(1)); From 617b2fbc845ece26c311ed0241ba732a367aa2a3 Mon Sep 17 00:00:00 2001 From: youliang Date: Wed, 3 Mar 2021 17:23:35 +0800 Subject: [PATCH 09/10] further cleanup headers to hide msgs Signed-off-by: youliang --- .../src/rmf_fleet_adapter/TaskManager.cpp | 1 - .../agv/FleetUpdateHandle.cpp | 21 +- rmf_task_msgs/msg/TaskSummary.msg | 1 + rmf_task_msgs/srv/SubmitTask.srv | 1 + rmf_task_ros2/CMakeLists.txt | 2 +- .../include/rmf_task_ros2/Description.hpp | 56 ++---- .../include/rmf_task_ros2/TaskStatus.hpp | 24 +-- rmf_task_ros2/src/mock_bidder/main.cpp | 3 +- .../src/rmf_task_ros2/Description.cpp | 184 +++++++++--------- .../src/rmf_task_ros2/Dispatcher.cpp | 18 +- .../src/rmf_task_ros2/TaskStatus.cpp | 32 --- .../src/rmf_task_ros2/action/Client.cpp | 68 +++++-- .../src/rmf_task_ros2/action/Client.hpp | 20 +- .../src/rmf_task_ros2/action/Server.cpp | 20 +- .../src/rmf_task_ros2/action/Server.hpp | 8 +- .../rmf_task_ros2/internal_Description.hpp | 47 +++++ .../test/dispatcher/test_Dispatcher.cpp | 4 +- ...st_Description.cpp => test_TaskStatus.cpp} | 40 ++-- 18 files changed, 307 insertions(+), 243 deletions(-) create mode 100644 rmf_task_ros2/src/rmf_task_ros2/internal_Description.hpp rename rmf_task_ros2/test/dispatcher/{test_Description.cpp => test_TaskStatus.cpp} (63%) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 50c937642..42ea5c6d6 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -459,7 +459,6 @@ void TaskManager::retreat_to_charger() task_profile.description.task_type.type = rmf_task_msgs::msg::TaskType::TYPE_CHARGE_BATTERY; - // TODO(YL) Check time input const auto task = rmf_fleet_adapter::tasks::make_charge_battery( charging_request, _context, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index 5f740d0bd..85d2ff83d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -636,24 +636,31 @@ void FleetUpdateHandle::Implementation::dispatch_request_cb( auto FleetUpdateHandle::Implementation::is_valid_assignments( Assignments& assignments) const -> bool { - std::unordered_set executed_tasks; + // This checks if the current assignments are all queued in task managers + std::unordered_set queued_tasks; for (const auto& mgr : task_managers) { - // TODO (YL) check if logic is correct - // const auto& tasks = mgr->get_executed_tasks(); - // executed_tasks.insert(tasks.begin(), tasks.end()); - if(mgr->current_task()) - executed_tasks.insert(mgr->current_task()->id()); + for (const auto task : mgr->task_queue()) + queued_tasks.insert(task->id()); } + size_t assignment_size = 0; for (const auto& agent : assignments) { for (const auto& a : agent) { - if (executed_tasks.find(a.request()->id()) != executed_tasks.end()) + // If ID exists doesnt exist in queue_tasks + if (queued_tasks.find(a.request()->id()) == queued_tasks.end()) return false; + + assignment_size++; } } + + // Check if total queued tasks is the same as the assignments + if (queued_tasks.size() != assignment_size) + return false; + return true; } diff --git a/rmf_task_msgs/msg/TaskSummary.msg b/rmf_task_msgs/msg/TaskSummary.msg index bb75e7b38..09b6256ba 100644 --- a/rmf_task_msgs/msg/TaskSummary.msg +++ b/rmf_task_msgs/msg/TaskSummary.msg @@ -4,6 +4,7 @@ string fleet_name # *optional and duplicated in TaskProfile +# TODO: This is not needed, to removed string task_id TaskProfile task_profile diff --git a/rmf_task_msgs/srv/SubmitTask.srv b/rmf_task_msgs/srv/SubmitTask.srv index 12572d3b4..2031249c1 100644 --- a/rmf_task_msgs/srv/SubmitTask.srv +++ b/rmf_task_msgs/srv/SubmitTask.srv @@ -7,6 +7,7 @@ string requester TaskDescription description # fleet selection evaluator +# TODO: This is not needed, to removed uint8 evaluator uint8 LOWEST_DIFF_COST_EVAL=0 uint8 LOWEST_COST_EVAL=1 diff --git a/rmf_task_ros2/CMakeLists.txt b/rmf_task_ros2/CMakeLists.txt index d1fe33b7f..d8ba05873 100644 --- a/rmf_task_ros2/CMakeLists.txt +++ b/rmf_task_ros2/CMakeLists.txt @@ -44,7 +44,7 @@ target_include_directories(rmf_task_ros2 ) ament_export_targets(rmf_task_ros2 HAS_LIBRARY_TARGET) -ament_export_dependencies(rmf_traffic rmf_task_msgs rmf_task rclcpp) +ament_export_dependencies(rmf_traffic_ros2 rmf_task_msgs rmf_task rclcpp) #=============================================================================== diff --git a/rmf_task_ros2/include/rmf_task_ros2/Description.hpp b/rmf_task_ros2/include/rmf_task_ros2/Description.hpp index 16043815d..1521a88c0 100644 --- a/rmf_task_ros2/include/rmf_task_ros2/Description.hpp +++ b/rmf_task_ros2/include/rmf_task_ros2/Description.hpp @@ -24,7 +24,7 @@ #include #include -#include +#include namespace rmf_task_ros2 { @@ -33,38 +33,23 @@ namespace rmf_task_ros2 { class Description { public: - using TaskDescription = rmf_task_msgs::msg::TaskDescription; - - /// Create a Description. This is only used when the task type is not + /// Create a Description. This is only used when the task type is not /// available as the derived class. - /// - /// \param [in] start_time - /// - /// \param [in] type static std::shared_ptr make_description( rmf_traffic::Time start_time, uint32_t type, uint64_t priority = 0); - /// Create a Description by providing a msg. This is only used when the task - /// type is not available as the derived class. - /// - /// \param [in] msg - /// task description msg as argument - static std::shared_ptr make_from_msg( - const TaskDescription& msg); - /// Get the start_time of the task rmf_traffic::Time start_time() const; /// Get the type of the task uint32_t type() const; - /// This is not created now + /// Get the priority of the task uint64_t priority() const; - /// Get the TaskDescription as msg - virtual TaskDescription to_msg() const; + virtual ~Description() = default; class Implementation; protected: @@ -76,7 +61,6 @@ using ConstDescriptionPtr = std::shared_ptr; //============================================================================== namespace description { - class Delivery : public Description { public: @@ -89,22 +73,21 @@ class Delivery : public Description std::string pickup_dispenser, std::string dropoff_place_name, std::string dropoff_ingestor, - std::vector items, + std::vector items = {}, uint64_t priority = 0); - /// Create task description from description msg - static std::shared_ptr make_from_msg( - const TaskDescription& msg); - - /// Get the TaskDescription as msg - TaskDescription to_msg() const final; - /// Get the pickup_place_name const std::string& pickup_place_name() const; /// Get the dropoff_place_name const std::string& dropoff_place_name() const; + /// Get the pickup_dispenser + const std::string& pickup_dispenser() const; + + /// Get the dropoff_ingestor + const std::string& dropoff_ingestor() const; + class Implementation; private: Delivery(); @@ -123,19 +106,15 @@ class Loop : public Description std::size_t num_loops, uint64_t priority = 0); - /// Create task description from description msg - static std::shared_ptr make_from_msg( - const TaskDescription& msg); - - /// Get the TaskDescription as msg - TaskDescription to_msg() const final; - /// Get the start_name const std::string& start_name() const; /// Get the finish_name const std::string& finish_name() const; + /// Get the num_loops + std::size_t num_loops() const; + class Implementation; private: Loop(); @@ -152,13 +131,6 @@ class Clean : public Description std::string start_waypoint, uint64_t priority = 0); - /// Create task description from description msg - static std::shared_ptr make_from_msg( - const TaskDescription& msg); - - /// Get the TaskDescription as msg - TaskDescription to_msg() const final; - /// Get the start_waypoint of a clean const std::string& start_waypoint() const; diff --git a/rmf_task_ros2/include/rmf_task_ros2/TaskStatus.hpp b/rmf_task_ros2/include/rmf_task_ros2/TaskStatus.hpp index d36b7c5ca..20b8f9038 100644 --- a/rmf_task_ros2/include/rmf_task_ros2/TaskStatus.hpp +++ b/rmf_task_ros2/include/rmf_task_ros2/TaskStatus.hpp @@ -23,12 +23,8 @@ #include #include -#include - namespace rmf_task_ros2 { -using StatusMsg = rmf_task_msgs::msg::TaskSummary; - //============================================================================== /// This represents the Task Status of each unique task /// \note TaskStatus struct is based on TaskSummary.msg @@ -66,12 +62,12 @@ class TaskStatus enum class State : uint8_t { - Queued = StatusMsg::STATE_QUEUED, - Executing = StatusMsg::STATE_ACTIVE, - Completed = StatusMsg::STATE_COMPLETED, - Failed = StatusMsg::STATE_FAILED, - Canceled = StatusMsg::STATE_CANCELED, - Pending = StatusMsg::STATE_PENDING + Queued = 0, + Executing = 1, + Completed = 2, + Failed = 3, + Canceled = 4, + Pending = 5 }; /// Current State of the task @@ -80,11 +76,6 @@ class TaskStatus /// Check if the current task is terminated bool is_terminated() const; - /// This helper function is to only update status elements in TaskStatus, in - /// which static task descriptions (e.g. id, tasktype...) will not be changed - /// \note experimental (TODO) - void update_from_msg(const StatusMsg& msg); - class Implementation; private: TaskStatus(); @@ -93,9 +84,6 @@ class TaskStatus using TaskStatusPtr = std::shared_ptr; -// ============================================================================== -StatusMsg convert_status(const TaskStatus& from); - } // namespace rmf_task_ros2 #endif // RMF_TASK_ROS2__TASK_STATUS_HPP diff --git a/rmf_task_ros2/src/mock_bidder/main.cpp b/rmf_task_ros2/src/mock_bidder/main.cpp index 7314d0bf3..226a85b53 100644 --- a/rmf_task_ros2/src/mock_bidder/main.cpp +++ b/rmf_task_ros2/src/mock_bidder/main.cpp @@ -27,6 +27,7 @@ using namespace rmf_task_ros2; using TaskType = bidding::MinimalBidder::TaskType; using BidNotice = rmf_task_msgs::msg::BidNotice; +using TaskProfile = rmf_task_msgs::msg::TaskProfile; int main(int argc, char* argv[]) { @@ -73,11 +74,9 @@ int main(int argc, char* argv[]) << task_profile.task_id<now()); const auto status = TaskStatus::make(id, now, nullptr); //todo diff --git a/rmf_task_ros2/src/rmf_task_ros2/Description.cpp b/rmf_task_ros2/src/rmf_task_ros2/Description.cpp index 498648796..5d118efcf 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/Description.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/Description.cpp @@ -16,6 +16,7 @@ */ #include +#include "internal_Description.hpp" #include namespace rmf_task_ros2 { @@ -64,24 +65,6 @@ std::shared_ptr Description::make_description( return desc; } -//============================================================================== -std::shared_ptr Description::make_from_msg( - const TaskDescription& msg) -{ - return make_description(rmf_traffic_ros2::convert(msg.start_time), - msg.task_type.type, msg.priority.value); -} - -//============================================================================== -rmf_task_msgs::msg::TaskDescription Description::to_msg() const -{ - rmf_task_msgs::msg::TaskDescription msg; - msg.task_type = _pimpl_base->task_type; - msg.priority = _pimpl_base->priority; - msg.start_time = rmf_traffic_ros2::convert(_pimpl_base->start_time); - return msg; -} - //============================================================================== Description::Description() : _pimpl_base(rmf_utils::make_impl(Implementation())) @@ -136,53 +119,34 @@ std::shared_ptr Delivery::make( } //============================================================================== -std::shared_ptr Delivery::make_from_msg( - const TaskDescription& msg) +Delivery::Delivery() +: _pimpl(rmf_utils::make_impl(Implementation())) { - if (msg.task_type.type != rmf_task_msgs::msg::TaskType::TYPE_DELIVERY) - return nullptr; - - return make( - rmf_traffic_ros2::convert(msg.start_time), - msg.delivery.pickup_place_name, - msg.delivery.pickup_dispenser, - msg.delivery.dropoff_place_name, - msg.delivery.dropoff_ingestor, - msg.delivery.items, - msg.priority.value); + // Do nothing } //============================================================================== -Delivery::Delivery() -: _pimpl(rmf_utils::make_impl(Implementation())) +const std::string& Delivery::pickup_place_name() const { - // Do nothing + return _pimpl->pickup_place_name; } //============================================================================== -rmf_task_msgs::msg::TaskDescription Delivery::to_msg() const +const std::string& Delivery::dropoff_place_name() const { - rmf_task_msgs::msg::TaskDescription msg; - msg.task_type = _pimpl_base->task_type; - msg.priority = _pimpl_base->priority; - msg.start_time = rmf_traffic_ros2::convert(_pimpl_base->start_time); - msg.delivery.pickup_place_name = _pimpl->pickup_place_name; - msg.delivery.dropoff_place_name = _pimpl->dropoff_place_name; - msg.delivery.pickup_dispenser = _pimpl->pickup_dispenser; - msg.delivery.dropoff_ingestor = _pimpl->dropoff_ingestor; - return msg; + return _pimpl->dropoff_place_name; } //============================================================================== -const std::string& Delivery::pickup_place_name() const +const std::string& Delivery::pickup_dispenser() const { - return _pimpl->pickup_place_name; + return _pimpl->pickup_dispenser; } //============================================================================== -const std::string& Delivery::dropoff_place_name() const +const std::string& Delivery::dropoff_ingestor() const { - return _pimpl->dropoff_place_name; + return _pimpl->dropoff_ingestor; } //============================================================================== @@ -220,34 +184,6 @@ std::shared_ptr Loop::make( return loop_desc; } -//============================================================================== -std::shared_ptr Loop::make_from_msg( - const TaskDescription& msg) -{ - if (msg.task_type.type != rmf_task_msgs::msg::TaskType::TYPE_LOOP) - return nullptr; - - return make( - rmf_traffic_ros2::convert(msg.start_time), - msg.loop.start_name, - msg.loop.finish_name, - msg.loop.num_loops, - msg.priority.value); -} - -//============================================================================== -rmf_task_msgs::msg::TaskDescription Loop::to_msg() const -{ - TaskDescription msg; - msg.task_type = _pimpl_base->task_type; - msg.priority = _pimpl_base->priority; - msg.start_time = rmf_traffic_ros2::convert(_pimpl_base->start_time); - msg.loop.start_name = _pimpl->start_name; - msg.loop.finish_name = _pimpl->finish_name; - msg.loop.num_loops = _pimpl->num_loops; - return msg; -} - //============================================================================== Loop::Loop() : _pimpl(rmf_utils::make_impl(Implementation())) @@ -267,6 +203,12 @@ const std::string& Loop::finish_name() const return this->_pimpl->finish_name; } +//============================================================================== +std::size_t Loop::num_loops() const +{ + return this->_pimpl->num_loops; +} + //============================================================================== //============================================================================== class Clean::Implementation @@ -297,40 +239,96 @@ std::shared_ptr Clean::make( } //============================================================================== -std::shared_ptr Clean::make_from_msg( - const TaskDescription& msg) +Clean::Clean() +: _pimpl(rmf_utils::make_impl(Implementation())) { - if (msg.task_type.type != rmf_task_msgs::msg::TaskType::TYPE_CLEAN) + // Do nothing +} + +//============================================================================== +const std::string& Clean::start_waypoint() const +{ + return this->_pimpl->start_waypoint; +} + +//============================================================================== +//============================================================================== +std::shared_ptr make_delivery_from_msg( + const rmf_task_msgs::msg::TaskDescription& msg) +{ + if (msg.task_type.type != rmf_task_msgs::msg::TaskType::TYPE_DELIVERY) return nullptr; - return make( + return Delivery::make( rmf_traffic_ros2::convert(msg.start_time), - msg.clean.start_waypoint, + msg.delivery.pickup_place_name, + msg.delivery.pickup_dispenser, + msg.delivery.dropoff_place_name, + msg.delivery.dropoff_ingestor, + msg.delivery.items, msg.priority.value); } //============================================================================== -rmf_task_msgs::msg::TaskDescription Clean::to_msg() const +std::shared_ptr make_loop_from_msg( + const rmf_task_msgs::msg::TaskDescription& msg) { - TaskDescription msg; - msg.task_type = _pimpl_base->task_type; - msg.priority = _pimpl_base->priority; - msg.start_time = rmf_traffic_ros2::convert(_pimpl_base->start_time); - msg.clean.start_waypoint = _pimpl->start_waypoint; - return msg; + if (msg.task_type.type != rmf_task_msgs::msg::TaskType::TYPE_LOOP) + return nullptr; + + return Loop::make( + rmf_traffic_ros2::convert(msg.start_time), + msg.loop.start_name, + msg.loop.finish_name, + msg.loop.num_loops, + msg.priority.value); } //============================================================================== -Clean::Clean() -: _pimpl(rmf_utils::make_impl(Implementation())) +std::shared_ptr make_clean_from_msg( + const rmf_task_msgs::msg::TaskDescription& msg) { - // Do nothing + if (msg.task_type.type != rmf_task_msgs::msg::TaskType::TYPE_CLEAN) + return nullptr; + + return Clean::make( + rmf_traffic_ros2::convert(msg.start_time), + msg.clean.start_waypoint, + msg.priority.value); } + //============================================================================== -const std::string& Clean::start_waypoint() const +rmf_task_msgs::msg::TaskDescription convert( + const ConstDescriptionPtr& description) { - return this->_pimpl->start_waypoint; + rmf_task_msgs::msg::TaskDescription msg; + msg.task_type.type = description->type(); + msg.priority.value = description->priority(); + msg.start_time = rmf_traffic_ros2::convert(description->start_time()); + + if (const auto desc = + std::dynamic_pointer_cast(description)) + { + msg.delivery.pickup_place_name = desc->pickup_place_name(); + msg.delivery.dropoff_place_name = desc->dropoff_place_name(); + msg.delivery.pickup_dispenser = desc->pickup_dispenser(); + msg.delivery.dropoff_ingestor = desc->dropoff_ingestor(); + // msg.delivery.items = desc->items(); + } + else if (const auto desc = + std::dynamic_pointer_cast(description)) + { + msg.loop.start_name = desc->start_name(); + msg.loop.finish_name = desc->finish_name(); + msg.loop.num_loops = desc->num_loops(); + } + else if (const auto desc = + std::dynamic_pointer_cast(description)) + { + msg.clean.start_waypoint = desc->start_waypoint(); + } + return msg; } } // namespace description diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp index 85dd51e47..78ebd0824 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp @@ -20,6 +20,7 @@ #include #include "action/Client.hpp" +#include "internal_Description.hpp" #include #include @@ -79,11 +80,11 @@ class Dispatcher::Implementation ConstDescriptionPtr task_description; const auto desc_msg = request->description; - if (auto d = description::Delivery::make_from_msg(desc_msg)) + if (auto d = description::make_delivery_from_msg(desc_msg)) task_description = std::move(d); - else if (auto d = description::Clean::make_from_msg(desc_msg)) + else if (auto d = description::make_clean_from_msg(desc_msg)) task_description = std::move(d); - else if (auto d = description::Loop::make_from_msg(desc_msg)) + else if (auto d = description::make_loop_from_msg(desc_msg)) task_description = std::move(d); else { @@ -170,7 +171,7 @@ class Dispatcher::Implementation rmf_task_msgs::msg::BidNotice bid_notice; bid_notice.task_profile.task_id = id; bid_notice.task_profile.submission_time = node->now(); - bid_notice.task_profile.description = description->to_msg(); + bid_notice.task_profile.description = description::convert(description); bid_notice.time_window = rmf_traffic_ros2::convert( rmf_traffic::time::from_seconds(bidding_time_window)); queue_bidding_tasks.push(bid_notice); @@ -213,7 +214,7 @@ class Dispatcher::Implementation } // Charging task doesnt support cancel task - if (cancel_task_status->description()->type() == + if (cancel_task_status->description()->type() == TaskType::TYPE_CHARGE_BATTERY) { RCLCPP_ERROR(node->get_logger(), "Charging task is not cancelled-able"); @@ -328,7 +329,7 @@ class Dispatcher::Implementation { assert(terminate_status->is_terminated()); - // prevent terminal_dispatch_tasks from piling up meaning + // prevent terminal_dispatch_tasks from piling up if (terminal_dispatch_tasks.size() >= terminated_tasks_max_size) { RCLCPP_WARN(node->get_logger(), @@ -348,9 +349,8 @@ class Dispatcher::Implementation const auto id = terminate_status->task_id(); - // destroy prev status ptr and recreate one (TODO) check if correct - // auto status = std::make_shared(*terminate_status); - (terminal_dispatch_tasks)[id] = terminate_status; + // Move Status to terminated task list. + (terminal_dispatch_tasks)[id] = std::move(terminate_status); active_dispatch_tasks.erase(id); } diff --git a/rmf_task_ros2/src/rmf_task_ros2/TaskStatus.cpp b/rmf_task_ros2/src/rmf_task_ros2/TaskStatus.cpp index 9fb380080..a0265b62d 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/TaskStatus.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/TaskStatus.cpp @@ -64,17 +64,6 @@ rmf_traffic::Time TaskStatus::submission_time() const return _pimpl->submission_time; } -// ============================================================================== -void TaskStatus::update_from_msg(const StatusMsg& msg) -{ - fleet_name = msg.fleet_name; - start_time = rmf_traffic_ros2::convert(msg.start_time); - end_time = rmf_traffic_ros2::convert(msg.end_time); - robot_name = msg.robot_name; - status = msg.status; - state = static_cast(msg.state); -} - // ============================================================================== bool TaskStatus::is_terminated() const { @@ -90,25 +79,4 @@ TaskStatus::TaskStatus() // Do nothing } -// ============================================================================== -StatusMsg convert_status(const TaskStatus& from) -{ - StatusMsg status; - status.fleet_name = from.fleet_name; - status.task_id = from.task_id(); // duplication - status.task_profile.task_id = from.task_id(); - status.task_profile.submission_time = - rmf_traffic_ros2::convert(from.submission_time()); - status.start_time = rmf_traffic_ros2::convert(from.start_time); - status.end_time = rmf_traffic_ros2::convert(from.end_time); - status.robot_name = from.robot_name; - status.status = from.status; - status.state = static_cast(from.state); - - if (from.description()) - status.task_profile.description = from.description()->to_msg(); - - return status; -} - } // namespace rmf_task_ros2 diff --git a/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp b/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp index cd1088be1..2f4f6e607 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp @@ -17,6 +17,7 @@ #include "Client.hpp" #include +#include "../internal_Description.hpp" namespace rmf_task_ros2 { namespace action { @@ -54,7 +55,7 @@ Client::Client(std::shared_ptr node) } // update status to ptr - weak_status->update_from_msg(*msg); + update_status_from_msg(weak_status, *msg); if (weak_status->is_terminated()) RCLCPP_INFO(_node->get_logger(), @@ -70,13 +71,19 @@ Client::Client(std::shared_ptr node) /// adapter to the dispatcher queue (e.g. ChargeBattery Task) RCLCPP_DEBUG(_node->get_logger(), "[action] Unknown task: [%s]", task_id.c_str()); - const auto now = std::chrono::steady_clock::now(); - const auto desc = Description::make_from_msg( - msg->task_profile.description); - const auto status = TaskStatus::make(task_id, now, desc); - status->update_from_msg(*msg); - _active_task_status[task_id] = status; - update_task_status(status); + + const auto& desc_msg = msg->task_profile.description; + const auto desc = Description::make_description( + rmf_traffic_ros2::convert(desc_msg.start_time), + desc_msg.task_type.type, + desc_msg.priority.value); + + const auto status_ptr = TaskStatus::make( + task_id, std::chrono::steady_clock::now(), desc); + update_status_from_msg(status_ptr, *msg); + + _active_task_status[task_id] = status_ptr; + update_task_status(status_ptr); } }); @@ -149,10 +156,10 @@ void Client::dispatch_task( { rmf_task_msgs::msg::TaskProfile task_profile; task_profile.task_id = status_ptr->task_id(); - task_profile.description = status_ptr->description()->to_msg(); + task_profile.description = description::convert(status_ptr->description()); task_profile.submission_time = - rmf_traffic_ros2::convert(status_ptr->submission_time()); - + rmf_traffic_ros2::convert(status_ptr->submission_time()); + // send request and wait for acknowledgement RequestMsg request_msg; request_msg.method = RequestMsg::ADD; @@ -193,9 +200,9 @@ bool Client::cancel_task(const std::string& task_id) rmf_task_msgs::msg::TaskProfile task_profile; task_profile.task_id = weak_status->task_id(); - task_profile.description = weak_status->description()->to_msg(); + task_profile.description = description::convert(weak_status->description()); task_profile.submission_time = - rmf_traffic_ros2::convert(weak_status->submission_time()); + rmf_traffic_ros2::convert(weak_status->submission_time()); // send cancel RequestMsg request_msg; @@ -239,4 +246,39 @@ void Client::on_terminate( } } // namespace action + +//============================================================================== +void update_status_from_msg( + const TaskStatusPtr task_status_ptr, + const StatusMsg msg) +{ + task_status_ptr->fleet_name = msg.fleet_name; + task_status_ptr->start_time = rmf_traffic_ros2::convert(msg.start_time); + task_status_ptr->end_time = rmf_traffic_ros2::convert(msg.end_time); + task_status_ptr->robot_name = msg.robot_name; + task_status_ptr->status = msg.status; + task_status_ptr->state = static_cast(msg.state); +} + +// ============================================================================== +StatusMsg convert_status(const TaskStatus& from) +{ + StatusMsg status; + status.fleet_name = from.fleet_name; + status.task_id = from.task_id(); // duplication + status.task_profile.task_id = from.task_id(); + status.task_profile.submission_time = + rmf_traffic_ros2::convert(from.submission_time()); + status.start_time = rmf_traffic_ros2::convert(from.start_time); + status.end_time = rmf_traffic_ros2::convert(from.end_time); + status.robot_name = from.robot_name; + status.status = from.status; + status.state = static_cast(from.state); + + if (from.description()) + status.task_profile.description = description::convert(from.description()); + + return status; +} + } // namespace rmf_task_ros2 diff --git a/rmf_task_ros2/src/rmf_task_ros2/action/Client.hpp b/rmf_task_ros2/src/rmf_task_ros2/action/Client.hpp index 3628f490d..6b664adfb 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/action/Client.hpp +++ b/rmf_task_ros2/src/rmf_task_ros2/action/Client.hpp @@ -24,8 +24,12 @@ #include #include #include +#include namespace rmf_task_ros2 { + +using StatusMsg = rmf_task_msgs::msg::TaskSummary; + namespace action { //============================================================================== @@ -33,7 +37,6 @@ namespace action { // fleet. The fleet will work on the requested task and provides a status to // the client when the task progresses. Termination will be triggered when the // task ends. - class Client { public: @@ -99,13 +102,26 @@ class Client std::shared_ptr _node; StatusCallback _on_change_callback; StatusCallback _on_terminate_callback; - std::unordered_map> _active_task_status; + std::unordered_map> _active_task_status; rclcpp::Publisher::SharedPtr _request_msg_pub; rclcpp::Subscription::SharedPtr _status_msg_sub; rclcpp::Subscription::SharedPtr _ack_msg_sub; }; } // namespace action + +// ============================================================================== +/// This helper function is to only update status elements in TaskStatus, in +/// which static task descriptions (e.g. id, tasktype...) will not be changed +void update_status_from_msg( + const TaskStatusPtr task_status_ptr, + const StatusMsg status_msg); + +// ============================================================================== +/// Utility function to convert to TaskSummary Msg +StatusMsg convert_status(const TaskStatus& from); + } // namespace rmf_task_ros2 #endif // SRC__RMF_TASK_ROS2__ACTION__CLIENT_HPP diff --git a/rmf_task_ros2/src/rmf_task_ros2/action/Server.cpp b/rmf_task_ros2/src/rmf_task_ros2/action/Server.cpp index 9efec4a77..79277b25c 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/action/Server.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/action/Server.cpp @@ -16,6 +16,8 @@ */ #include "Server.hpp" +#include +#include "../internal_Description.hpp" namespace rmf_task_ros2 { namespace action { @@ -38,10 +40,24 @@ void Server::register_callbacks( //============================================================================== void Server::update_status( - const TaskStatus& task_status) + const TaskStatus& status) { - auto msg = convert_status(task_status); + // Here solely converts TaskStatus to StatusMsg + StatusMsg msg; msg.fleet_name = _fleet_name; + msg.task_id = status.task_id(); // duplication + msg.task_profile.task_id = status.task_id(); + msg.task_profile.submission_time = + rmf_traffic_ros2::convert(status.submission_time()); + msg.start_time = rmf_traffic_ros2::convert(status.start_time); + msg.end_time = rmf_traffic_ros2::convert(status.end_time); + msg.robot_name = status.robot_name; + msg.status = status.status; + msg.state = static_cast(status.state); + + if (status.description()) + msg.task_profile.description = description::convert(status.description()); + _status_msg_pub->publish(msg); } diff --git a/rmf_task_ros2/src/rmf_task_ros2/action/Server.hpp b/rmf_task_ros2/src/rmf_task_ros2/action/Server.hpp index d8c0150a6..479020c4e 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/action/Server.hpp +++ b/rmf_task_ros2/src/rmf_task_ros2/action/Server.hpp @@ -22,15 +22,18 @@ #include #include + #include #include -#include +#include -using TaskProfile = rmf_task_msgs::msg::TaskProfile; +#include namespace rmf_task_ros2 { namespace action { +using TaskProfile = rmf_task_msgs::msg::TaskProfile; + //============================================================================== /// Task Action Server - This is used within the fleet adapter with the role of /// receiving incoming dispatch requests (from a action_client/Dispatcher), @@ -78,6 +81,7 @@ class Server using RequestMsg = rmf_task_msgs::msg::DispatchRequest; using AckMsg = rmf_task_msgs::msg::DispatchAck; + using StatusMsg = rmf_task_msgs::msg::TaskSummary; std::shared_ptr _node; std::string _fleet_name; diff --git a/rmf_task_ros2/src/rmf_task_ros2/internal_Description.hpp b/rmf_task_ros2/src/rmf_task_ros2/internal_Description.hpp new file mode 100644 index 000000000..6bdf2fffc --- /dev/null +++ b/rmf_task_ros2/src/rmf_task_ros2/internal_Description.hpp @@ -0,0 +1,47 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef SRC__RMF_TASK_ROS2__DESCRIPTION__INTERNAL_DESCRIPTION_HPP +#define SRC__RMF_TASK_ROS2__DESCRIPTION__INTERNAL_DESCRIPTION_HPP + +#include +#include + +namespace rmf_task_ros2 { +namespace description { + +using TaskDescription = rmf_task_msgs::msg::TaskDescription; + +//============================================================================== +std::shared_ptr make_delivery_from_msg( + const TaskDescription& msg); + +//============================================================================== +std::shared_ptr make_loop_from_msg( + const TaskDescription& msg); + +//============================================================================== +std::shared_ptr make_clean_from_msg( + const TaskDescription& msg); + +//============================================================================== +TaskDescription convert(const ConstDescriptionPtr& description); + +} // namespace description +} // namespace rmf_task_ros2 + +#endif // SRC__RMF_TASK_ROS2__DESCRIPTION__INTERNAL_DESCRIPTION_HPP diff --git a/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp b/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp index bb90facc1..2be0187ab 100644 --- a/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp +++ b/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp @@ -156,7 +156,7 @@ SCENARIO("Dispatcehr API Test", "[Dispatcher]") action_server->register_callbacks( // Add Task callback - [&action_server, &task_canceled_flag](const TaskProfile& task_profile) + [&action_server, &task_canceled_flag](const auto& task_profile) { // Start action task auto t = std::thread( @@ -187,7 +187,7 @@ SCENARIO("Dispatcehr API Test", "[Dispatcher]") return true; //successs (send State::Queued) }, // Cancel Task callback - [&action_server, &task_canceled_flag](const TaskProfile&) + [&action_server, &task_canceled_flag](const auto&) { task_canceled_flag = true; return true; //success ,send State::Canceled when dispatcher->cancel_task diff --git a/rmf_task_ros2/test/dispatcher/test_Description.cpp b/rmf_task_ros2/test/dispatcher/test_TaskStatus.cpp similarity index 63% rename from rmf_task_ros2/test/dispatcher/test_Description.cpp rename to rmf_task_ros2/test/dispatcher/test_TaskStatus.cpp index 340ad8779..3c61ca2d6 100644 --- a/rmf_task_ros2/test/dispatcher/test_Description.cpp +++ b/rmf_task_ros2/test/dispatcher/test_TaskStatus.cpp @@ -16,34 +16,45 @@ */ #include +#include "../../src/rmf_task_ros2/internal_Description.hpp" + #include #include +#include + namespace rmf_task_ros2 { //============================================================================== -SCENARIO("Task Description Test", "[Description]") +SCENARIO("TaskStatus and Description Test", "[TaskStatus]") { - std::cout << "Testing Description Task Type" << std::endl; + std::cout << "[Testing Description Task Type]" << std::endl; using TaskDescription = rmf_task_msgs::msg::TaskDescription; using TaskType = rmf_task_msgs::msg::TaskType; + using StatusMsg = rmf_task_msgs::msg::TaskSummary; +//============================================================================== + // Check State Enum Val, to sync with msg + REQUIRE((uint8_t)TaskStatus::State::Queued == StatusMsg::STATE_QUEUED); + REQUIRE((uint8_t)TaskStatus::State::Executing == StatusMsg::STATE_ACTIVE); + REQUIRE((uint8_t)TaskStatus::State::Completed == StatusMsg::STATE_COMPLETED); + REQUIRE((uint8_t)TaskStatus::State::Failed == StatusMsg::STATE_FAILED); + REQUIRE((uint8_t)TaskStatus::State::Canceled == StatusMsg::STATE_CANCELED); + REQUIRE((uint8_t)TaskStatus::State::Pending == StatusMsg::STATE_PENDING); + +//============================================================================== const auto now = std::chrono::steady_clock::now(); // test delivery description msg auto delivery = description::Delivery::make( now, "pick", "dis", "drop", "ing", {}); - - std::cout << "[test description] Delivery out msg: " - << delivery->pickup_place_name() - << " to " - << delivery->dropoff_place_name() - << std::endl; - REQUIRE(delivery->type() == TaskType::TYPE_DELIVERY); + + // By default this will be a Station Type, which is not supported TaskDescription msg; - auto delivery2 = description::Delivery::make_from_msg(msg); + + auto delivery2 = description::make_delivery_from_msg(msg); REQUIRE(!delivery2); // Create TaskStatus Object @@ -58,19 +69,14 @@ SCENARIO("Task Description Test", "[Description]") REQUIRE(loop->type() == TaskType::TYPE_LOOP); auto d_loop = std::dynamic_pointer_cast(loop); - std::cout << "[test description] Loop out msg: " - << loop->start_name() - << " to " - << loop->finish_name() - << std::endl; - auto loop2 = description::Loop::make_from_msg(msg); + auto loop2 = description::make_loop_from_msg(msg); REQUIRE(!loop2); //============================================================================== // test clean descripttion msg auto clean = description::Clean::make(now, "clean_here"); REQUIRE(clean->type() == TaskType::TYPE_CLEAN); - auto clean2 = description::Loop::make_from_msg(msg); + auto clean2 = description::make_clean_from_msg(msg); REQUIRE(!clean2); } From acbca8b7b4b2ff56cd64245d3aea71017fe3ffac Mon Sep 17 00:00:00 2001 From: youliang Date: Thu, 4 Mar 2021 12:19:32 +0800 Subject: [PATCH 10/10] further clean up include headers --- .../include/rmf_task_ros2/Dispatcher.hpp | 2 +- rmf_task_ros2/src/mock_bidder/main.cpp | 2 +- .../src/rmf_task_ros2/Dispatcher.cpp | 1 + .../src/rmf_task_ros2/bidding/Auctioneer.cpp | 55 ++++++++++- .../rmf_task_ros2/bidding/Auctioneer.hpp | 0 .../rmf_task_ros2/bidding/MinimalBidder.cpp | 2 +- .../rmf_task_ros2/bidding/MinimalBidder.hpp | 9 +- .../bidding/internal_Auctioneer.hpp | 96 ------------------- rmf_task_ros2/test/bidding/test_SelfBid.cpp | 4 +- .../test/dispatcher/test_Dispatcher.cpp | 2 +- 10 files changed, 65 insertions(+), 108 deletions(-) rename rmf_task_ros2/{include => src}/rmf_task_ros2/bidding/Auctioneer.hpp (100%) rename rmf_task_ros2/{include => src}/rmf_task_ros2/bidding/MinimalBidder.hpp (89%) delete mode 100644 rmf_task_ros2/src/rmf_task_ros2/bidding/internal_Auctioneer.hpp diff --git a/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp b/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp index b861a78a3..07f6f4bc6 100644 --- a/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp +++ b/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include diff --git a/rmf_task_ros2/src/mock_bidder/main.cpp b/rmf_task_ros2/src/mock_bidder/main.cpp index 226a85b53..8c639ec43 100644 --- a/rmf_task_ros2/src/mock_bidder/main.cpp +++ b/rmf_task_ros2/src/mock_bidder/main.cpp @@ -17,7 +17,7 @@ /// Note: This is a testing bidder node script -#include +#include "../rmf_task_ros2/bidding/MinimalBidder.hpp" #include "../rmf_task_ros2/action/Server.hpp" #include diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp index 78ebd0824..bd28aedd1 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp @@ -20,6 +20,7 @@ #include #include "action/Client.hpp" +#include "bidding/Auctioneer.hpp" #include "internal_Description.hpp" #include diff --git a/rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.cpp b/rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.cpp index a54b4a83c..27db7916d 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.cpp @@ -15,13 +15,64 @@ * */ -#include "internal_Auctioneer.hpp" +#include "Auctioneer.hpp" +#include +#include +#include namespace rmf_task_ros2 { namespace bidding { +using BidProposal = rmf_task_msgs::msg::BidProposal; +using Submission = rmf_task::Evaluator::Submission; +using Submissions = rmf_task::Evaluator::Submissions; + +//============================================================================== +class Auctioneer::Implementation +{ +public: + std::shared_ptr node; + rclcpp::TimerBase::SharedPtr timer; + BiddingResultCallback bidding_result_callback; + std::shared_ptr evaluator; + + struct BiddingTask + { + BidNotice bid_notice; + builtin_interfaces::msg::Time start_time; + Submissions submissions; + }; + + bool bidding_in_proccess = false; + std::queue queue_bidding_tasks; + + using BidNoticePub = rclcpp::Publisher; + BidNoticePub::SharedPtr bid_notice_pub; + + using BidProposalSub = rclcpp::Subscription; + BidProposalSub::SharedPtr bid_proposal_sub; + + Implementation( + const std::shared_ptr& node_, + BiddingResultCallback result_callback); + + /// Start a bidding process + void start_bidding(const BidNotice& bid_notice); + + // Receive proposal and evaluate + void receive_proposal(const BidProposal& msg); + + // determine the winner within a bidding task instance + void check_bidding_process(); + + bool determine_winner(const BiddingTask& bidding_task); + + std::optional evaluate(const Submissions& submissions); +}; + //============================================================================== -Submission convert(const BidProposal& from) +Submission convert( + const BidProposal& from) { Submission submission; submission.fleet_name = from.fleet_name; diff --git a/rmf_task_ros2/include/rmf_task_ros2/bidding/Auctioneer.hpp b/rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.hpp similarity index 100% rename from rmf_task_ros2/include/rmf_task_ros2/bidding/Auctioneer.hpp rename to rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.hpp diff --git a/rmf_task_ros2/src/rmf_task_ros2/bidding/MinimalBidder.cpp b/rmf_task_ros2/src/rmf_task_ros2/bidding/MinimalBidder.cpp index 38f1fabc3..2bb7e3101 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/bidding/MinimalBidder.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/bidding/MinimalBidder.cpp @@ -15,7 +15,7 @@ * */ -#include +#include "MinimalBidder.hpp" #include #include diff --git a/rmf_task_ros2/include/rmf_task_ros2/bidding/MinimalBidder.hpp b/rmf_task_ros2/src/rmf_task_ros2/bidding/MinimalBidder.hpp similarity index 89% rename from rmf_task_ros2/include/rmf_task_ros2/bidding/MinimalBidder.hpp rename to rmf_task_ros2/src/rmf_task_ros2/bidding/MinimalBidder.hpp index b0e0f34d6..cd93e1fb7 100644 --- a/rmf_task_ros2/include/rmf_task_ros2/bidding/MinimalBidder.hpp +++ b/rmf_task_ros2/src/rmf_task_ros2/bidding/MinimalBidder.hpp @@ -15,8 +15,8 @@ * */ -#ifndef RMF_TASK_ROS2__BIDDING__MINIMALBIDDER_HPP -#define RMF_TASK_ROS2__BIDDING__MINIMALBIDDER_HPP +#ifndef SRC__RMF_TASK_ROS2__BIDDING__MINIMALBIDDER_HPP +#define SRC__RMF_TASK_ROS2__BIDDING__MINIMALBIDDER_HPP #include @@ -29,6 +29,8 @@ namespace rmf_task_ros2 { namespace bidding { //============================================================================== +/// This is a sample bidder utility class which will listen to bid notice and +/// submit a bid to the auctioneer. This is currently used in test. class MinimalBidder { public: @@ -44,7 +46,6 @@ class MinimalBidder Patrol = TaskTypeMsg::TYPE_PATROL }; - struct Submission { std::string robot_name; @@ -95,4 +96,4 @@ class MinimalBidder } // namespace bidding } // namespace rmf_task_ros2 -#endif // RMF_TASK_ROS2__BIDDING__MINIMALBIDDER_HPP +#endif // SRC__RMF_TASK_ROS2__BIDDING__MINIMALBIDDER_HPP diff --git a/rmf_task_ros2/src/rmf_task_ros2/bidding/internal_Auctioneer.hpp b/rmf_task_ros2/src/rmf_task_ros2/bidding/internal_Auctioneer.hpp deleted file mode 100644 index 99ab3351b..000000000 --- a/rmf_task_ros2/src/rmf_task_ros2/bidding/internal_Auctioneer.hpp +++ /dev/null @@ -1,96 +0,0 @@ -/* - * Copyright (C) 2020 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ - - -#ifndef SRC__RMF_TASK_ROS2__BIDDING__INTERNAL_AUCTIONEER_HPP -#define SRC__RMF_TASK_ROS2__BIDDING__INTERNAL_AUCTIONEER_HPP - -#include -#include -#include - -#include -#include - -namespace rmf_task_ros2 { -namespace bidding { - -using BidProposal = rmf_task_msgs::msg::BidProposal; -using Submission = rmf_task::Evaluator::Submission; -using Submissions = rmf_task::Evaluator::Submissions; - -//============================================================================== -class Auctioneer::Implementation -{ -public: - std::shared_ptr node; - rclcpp::TimerBase::SharedPtr timer; - BiddingResultCallback bidding_result_callback; - std::shared_ptr evaluator; - - struct BiddingTask - { - BidNotice bid_notice; - builtin_interfaces::msg::Time start_time; - Submissions submissions; - }; - - bool bidding_in_proccess = false; - std::queue queue_bidding_tasks; - - using BidNoticePub = rclcpp::Publisher; - BidNoticePub::SharedPtr bid_notice_pub; - - using BidProposalSub = rclcpp::Subscription; - BidProposalSub::SharedPtr bid_proposal_sub; - - Implementation( - const std::shared_ptr& node_, - BiddingResultCallback result_callback); - - /// Start a bidding process - void start_bidding(const BidNotice& bid_notice); - - // Receive proposal and evaluate - void receive_proposal(const BidProposal& msg); - - // determine the winner within a bidding task instance - void check_bidding_process(); - - bool determine_winner(const BiddingTask& bidding_task); - - std::optional evaluate(const Submissions& submissions); - - static const Implementation& get(const Auctioneer& auctioneer) - { - return *auctioneer._pimpl; - } -}; - -//============================================================================== -std::optional evaluate( - const Auctioneer& auctioneer, - const Submissions& submissions) -{ - auto fimpl = Auctioneer::Implementation::get(auctioneer); - return fimpl.evaluate(submissions); -} - -} // namespace bidding -} // namespace rmf_task_ros2 - -#endif // SRC__RMF_TASK_ROS2__BIDDING__INTERNAL_AUCTIONEER_HPP diff --git a/rmf_task_ros2/test/bidding/test_SelfBid.cpp b/rmf_task_ros2/test/bidding/test_SelfBid.cpp index 646719d9f..d7d6e1e97 100644 --- a/rmf_task_ros2/test/bidding/test_SelfBid.cpp +++ b/rmf_task_ros2/test/bidding/test_SelfBid.cpp @@ -15,8 +15,8 @@ * */ -#include -#include +#include "../../src/rmf_task_ros2/bidding/MinimalBidder.hpp" +#include "../../src/rmf_task_ros2/bidding/Auctioneer.hpp" #include #include diff --git a/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp b/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp index 2be0187ab..ac89b8611 100644 --- a/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp +++ b/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp @@ -19,8 +19,8 @@ #include // mock Fleet Adapter to test dispatcher -#include #include "../../src/rmf_task_ros2/action/Server.hpp" +#include "../../src/rmf_task_ros2/bidding/MinimalBidder.hpp" #include #include