diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 000000000..cc9032378 --- /dev/null +++ b/Dockerfile @@ -0,0 +1,23 @@ +ARG TAG="foxy-ros-base-focal" +FROM ros:$TAG + +ENV HOME /home/ws_rmf/ + +RUN apt-get update && apt-get install -y g++-8 \ + ros-foxy-rmw-cyclonedds-cpp +RUN mkdir -p /home/ws_rmf + +WORKDIR /home/ws_rmf/ +COPY . src +RUN rosdep update +RUN rosdep install --from-paths src --ignore-src --rosdistro foxy -yr + +RUN /ros_entrypoint.sh \ + colcon build --cmake-args -DCMAKE_BUILD_TYPE=RELEASE && \ + sed -i '$isource "/home/ws_rmf/install/setup.bash"' /ros_entrypoint.sh && \ + rm -rf build devel src + +# todo: should have a multistage build + +ENTRYPOINT ["/ros_entrypoint.sh"] +CMD ["bash"] diff --git a/rmf_fleet_adapter/src/full_control/main.cpp b/rmf_fleet_adapter/src/full_control/main.cpp index 55e91b316..71264415b 100644 --- a/rmf_fleet_adapter/src/full_control/main.cpp +++ b/rmf_fleet_adapter/src/full_control/main.cpp @@ -614,7 +614,7 @@ std::shared_ptr make_fleet( connections->fleet->accept_task_requests( [task_types](const rmf_task_msgs::msg::TaskProfile& msg) { - if (task_types.find(msg.task_type.type) != task_types.end()) + if (task_types.find(msg.description.task_type.type) != task_types.end()) return true; return false; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index e1928c6cd..69e466148 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -244,7 +244,7 @@ void TaskManager::set_queue( msg.state = msg.STATE_QUEUED; msg.robot_name = _context->name(); msg.fleet_name = _context->description().owner(); - msg.task_profile.task_type = task_type_msg; + 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( 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 f95a6cef9..8670e71d4 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -146,8 +146,9 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( // Determine task type and convert to request pointer rmf_task::ConstRequestPtr new_request = nullptr; const auto& task_profile = msg->task_profile; - const auto& task_type = task_profile.task_type; - const rmf_traffic::Time start_time = rmf_traffic_ros2::convert(task_profile.start_time); + const auto& task_type = task_profile.description.task_type; + 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 auto& graph = planner->get_configuration().graph(); @@ -155,7 +156,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( // Process Cleaning task if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_CLEAN) { - if (task_profile.clean.start_waypoint.empty()) + if (task_profile.description.clean.start_waypoint.empty()) { RCLCPP_ERROR( node->get_logger(), @@ -166,7 +167,8 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( } // Check for valid start waypoint - const std::string start_wp_name = task_profile.clean.start_waypoint; + const std::string start_wp_name = + task_profile.description.clean.start_waypoint; const auto start_wp = graph.find_waypoint(start_wp_name); if (!start_wp) { @@ -245,7 +247,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( else if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_DELIVERY) { - const auto& delivery = task_profile.delivery; + const auto& delivery = task_profile.description.delivery; if (delivery.pickup_place_name.empty()) { RCLCPP_ERROR( @@ -340,7 +342,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( } else if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_LOOP) { - const auto& loop = task_profile.loop; + const auto& loop = task_profile.description.loop; if (loop.start_name.empty()) { RCLCPP_ERROR( diff --git a/rmf_task_msgs/CMakeLists.txt b/rmf_task_msgs/CMakeLists.txt index ed5a750cf..90a1d44ae 100644 --- a/rmf_task_msgs/CMakeLists.txt +++ b/rmf_task_msgs/CMakeLists.txt @@ -22,6 +22,7 @@ set(msg_files "msg/Behavior.msg" "msg/BehaviorParameter.msg" "msg/Station.msg" + "msg/TaskDescription.msg" "msg/TaskSummary.msg" "msg/Tasks.msg" "msg/Loop.msg" diff --git a/rmf_task_msgs/msg/TaskDescription.msg b/rmf_task_msgs/msg/TaskDescription.msg new file mode 100644 index 000000000..4f6cb8bf7 --- /dev/null +++ b/rmf_task_msgs/msg/TaskDescription.msg @@ -0,0 +1,13 @@ +# Desired start time of a task +builtin_interfaces/Time start_time + +# Task type +TaskType task_type + +# Task definitions +Station station +Loop loop +Delivery delivery +# Charge charge +Clean clean +# Patrol patrol diff --git a/rmf_task_msgs/msg/TaskProfile.msg b/rmf_task_msgs/msg/TaskProfile.msg index 571474f2d..7c75dd5a8 100644 --- a/rmf_task_msgs/msg/TaskProfile.msg +++ b/rmf_task_msgs/msg/TaskProfile.msg @@ -4,17 +4,4 @@ string task_id # Task submission time builtin_interfaces/Time submission_time -# Desired start time of a task -builtin_interfaces/Time start_time - -# Task type -TaskType task_type - -# Task definitions -Station station -Loop loop -Delivery delivery -# Charge charge -Clean clean -# Patrol patrol - +TaskDescription description diff --git a/rmf_task_msgs/srv/SubmitTask.srv b/rmf_task_msgs/srv/SubmitTask.srv index 7115aff51..08d048e87 100644 --- a/rmf_task_msgs/srv/SubmitTask.srv +++ b/rmf_task_msgs/srv/SubmitTask.srv @@ -6,16 +6,8 @@ string requester # Desired start time of a task builtin_interfaces/Time start_time -# Task type -TaskType task_type - -# Task definitions -Station station -Loop loop -Delivery delivery -# Charge charge -Clean clean -# Patrol patrol +# desciption of task +TaskDescription description # fleet selection evaluator uint8 evaluator diff --git a/rmf_task_ros2/CMakeLists.txt b/rmf_task_ros2/CMakeLists.txt new file mode 100644 index 000000000..208358372 --- /dev/null +++ b/rmf_task_ros2/CMakeLists.txt @@ -0,0 +1,95 @@ +cmake_minimum_required(VERSION 3.5) + +project(rmf_task_ros2) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + # we dont use add_compile_options with pedantic in message packages + # because the Python C extensions dont comply with it + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +endif() + +include(GNUInstallDirs) + +find_package(ament_cmake REQUIRED) +find_package(rmf_traffic REQUIRED) +find_package(rmf_traffic_ros2 REQUIRED) +find_package(rmf_task_msgs REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(rclcpp REQUIRED) + +file(GLOB_RECURSE core_lib_srcs "src/rmf_task_ros2/*.cpp") +add_library(rmf_task_ros2 SHARED ${core_lib_srcs}) + +target_link_libraries(rmf_task_ros2 + PUBLIC + rmf_traffic::rmf_traffic + rmf_traffic_ros2::rmf_traffic_ros2 + ${rmf_task_msgs_LIBRARIES} + ${rclcpp_LIBRARIES} +) + +target_include_directories(rmf_task_ros2 + PUBLIC + $ + $ + ${rmf_traffic_ros2_INCLUDE_DIRS} + ${rmf_task_msgs_INCLUDE_DIRS} + ${rclcpp_INCLUDE_DIRS} +) + +ament_export_targets(rmf_task_ros2 HAS_LIBRARY_TARGET) +ament_export_dependencies(rmf_traffic rmf_task_msgs rclcpp) + +#=============================================================================== + +find_package(ament_cmake_catch2 QUIET) +if(BUILD_TESTING AND ament_cmake_catch2_FOUND) + file(GLOB_RECURSE unit_test_srcs "test/*.cpp") + + ament_add_catch2( + test_rmf_task_ros2 test/main.cpp ${unit_test_srcs} + TIMEOUT 300) + target_link_libraries(test_rmf_task_ros2 + rmf_task_ros2 + rmf_traffic::rmf_traffic + rmf_traffic_ros2::rmf_traffic_ros2 + ) + + target_include_directories(test_rmf_task_ros2 + PRIVATE + $ + ) +endif() + +#=============================================================================== + +add_executable(rmf_bidder_node + src/mock_bidder/main.cpp +) +target_link_libraries(rmf_bidder_node PUBLIC rmf_task_ros2) + +#=============================================================================== + +add_executable(rmf_task_dispatcher + src/dispatcher_node/main.cpp +) +target_link_libraries(rmf_task_dispatcher PUBLIC rmf_task_ros2) + +#=============================================================================== +install( + DIRECTORY include/ + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) + +install( + TARGETS rmf_task_ros2 rmf_task_dispatcher rmf_bidder_node + EXPORT rmf_task_ros2 + RUNTIME DESTINATION lib/rmf_task_ros2 + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib +) + +ament_package() diff --git a/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp b/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp new file mode 100644 index 000000000..f01ebf656 --- /dev/null +++ b/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp @@ -0,0 +1,141 @@ +/* + * 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__DISPATCHER_HPP +#define RMF_TASK_ROS2__DISPATCHER_HPP + +#include +#include +#include +#include + +#include +#include + + +namespace rmf_task_ros2 { + +//============================================================================== +/// This dispatcher class holds an instance which handles the dispatching of +/// tasks to all downstream RMF fleet adapters. +class Dispatcher : public std::enable_shared_from_this +{ +public: + 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 + /// 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 + /// + /// \sa init_and_make_node() + static std::shared_ptr init_and_make_node( + const std::string dispatcher_node_name); + + /// 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); + + /// Create a dispatcher by providing the ros2 node + /// + /// \param[in] node + /// ROS 2 node instance + /// + /// \sa make() + static std::shared_ptr make( + const std::shared_ptr& node); + + /// Submit task to dispatcher node. Calling this function will immediately + /// trigger the bidding process, then the task "action". Once submmitted, + /// Task State will be in 'Pending' State, till the task is awarded to a fleet + /// then the state will turn to 'Queued' + /// + /// \param [in] task_description + /// Submit a task to dispatch + /// + /// \return task_id + /// self-generated task_id, nullopt is submit task failed + std::optional submit_task( + const TaskDescription& 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 + /// the respective fleet adapter. It is the responsibility of the fleet adapter + /// to make sure it cancels the task internally. + /// + /// \param [in] task_id + /// Task to cancel + /// + /// \return true if success + bool cancel_task(const TaskID& task_id); + + /// Check the state of a submited task. It can be either active or terminated + /// + /// \param [in] task_id + /// 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 TaskID& task_id) const; + + /// Get a mutable ref of active tasks map list handled by dispatcher + const DispatchTasks& active_tasks() const; + + /// Get a mutable ref of terminated tasks map list + const DispatchTasks& terminated_tasks() const; + + using StatusCallback = std::function; + + /// Trigger this callback when a task status is changed. This will return the + /// Changed task status. + /// + /// \param [in] callback function + void on_change(StatusCallback on_change_fn); + + /// Change the default evaluator to a custom evaluator, which is used by + /// bidding auctioneer. Default evaluator is: `LeastFleetDiffCostEvaluator` + /// + /// \param [in] evaluator + /// evaluator used to select the best bid from fleets + void evaluator(std::shared_ptr evaluator); + + /// Get the rclcpp::Node that this dispatcher will be using for communication. + std::shared_ptr node(); + + /// spin dispatcher node + void spin(); + + class Implementation; + +private: + Dispatcher(); + rmf_utils::unique_impl_ptr _pimpl; +}; + +} // namespace rmf_task_ros2 + +#endif // RMF_TASK_ROS2__DISPATCHER_HPP diff --git a/rmf_task_ros2/include/rmf_task_ros2/StandardNames.hpp b/rmf_task_ros2/include/rmf_task_ros2/StandardNames.hpp new file mode 100644 index 000000000..f45a95044 --- /dev/null +++ b/rmf_task_ros2/include/rmf_task_ros2/StandardNames.hpp @@ -0,0 +1,38 @@ +/* + * 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__STANDARDNAMES_HPP +#define RMF_TASK_ROS2__STANDARDNAMES_HPP + +#include + +namespace rmf_task_ros2 { + +const std::string Prefix = "rmf_task/"; +const std::string BidNoticeTopicName = Prefix + "bid_notice"; +const std::string BidProposalTopicName = Prefix + "bid_proposal"; + +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 TaskStatusTopicName = "task_summaries"; + +} // namespace rmf_task_ros2 + +#endif // RMF_TASK_ROS2__STANDARDNAMES_HPP diff --git a/rmf_task_ros2/include/rmf_task_ros2/TaskStatus.hpp b/rmf_task_ros2/include/rmf_task_ros2/TaskStatus.hpp new file mode 100644 index 000000000..a8f96fcd6 --- /dev/null +++ b/rmf_task_ros2/include/rmf_task_ros2/TaskStatus.hpp @@ -0,0 +1,68 @@ +/* + * 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__TASK_STATUS_HPP +#define RMF_TASK_ROS2__TASK_STATUS_HPP + +#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; + +//============================================================================== +/// \note TaskStatus struct is based on TaskSummary.msg +struct 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 + }; + + 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 + + bool is_terminated() const; +}; + +using TaskStatusPtr = std::shared_ptr; + +// ============================================================================== +TaskStatus convert_status(const StatusMsg& from); + +// ============================================================================== +StatusMsg convert_status(const TaskStatus& from); + +} // namespace rmf_task_ros2 + +#endif // RMF_TASK_ROS2__TASK_STATUS_HPP diff --git a/rmf_task_ros2/include/rmf_task_ros2/bidding/Auctioneer.hpp b/rmf_task_ros2/include/rmf_task_ros2/bidding/Auctioneer.hpp new file mode 100644 index 000000000..4b9b5a87d --- /dev/null +++ b/rmf_task_ros2/include/rmf_task_ros2/bidding/Auctioneer.hpp @@ -0,0 +1,119 @@ +/* + * 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__AUCTIONEER_HPP +#define RMF_TASK_ROS2__BIDDING__AUCTIONEER_HPP + +#include +#include +#include + +#include + +namespace rmf_task_ros2 { +namespace bidding { + +//============================================================================== +/// The Auctioneer class is responsible for initiating and mediating the bidding +/// process during task dispatching. Hence, this class instance is solely used +/// within the Dispatcher class +class Auctioneer : public std::enable_shared_from_this +{ +public: + /// Callback which will provide the winner when a bid is concluded + /// + /// \param[in] task_id + /// bidding task id + /// + /// \param[in] winner + /// single winner from all submissions. nullopt if non + using BiddingResultCallback = + std::function winner)>; + + /// Create an instance of the Auctioneer. This instance will handle all + /// the task dispatching bidding mechanism. A default evaluator is used. + /// + /// \param[in] node + /// ros2 node which will manage the bidding + /// + /// \param[in] result_callback + /// This callback fn will be called when a bidding result is concluded + /// + /// \sa make() + static std::shared_ptr make( + const std::shared_ptr& node, + BiddingResultCallback result_callback); + + /// Start a bidding process by provide a bidding task. Note the each + /// bidding process is conducted sequentially + /// + /// \param[in] bid_notice + /// 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); + + class Implementation; + +private: + Auctioneer(); + 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 + +#endif // RMF_TASK_ROS2__BIDDING__AUCTIONEER_HPP diff --git a/rmf_task_ros2/include/rmf_task_ros2/bidding/MinimalBidder.hpp b/rmf_task_ros2/include/rmf_task_ros2/bidding/MinimalBidder.hpp new file mode 100644 index 000000000..ef53db6fc --- /dev/null +++ b/rmf_task_ros2/include/rmf_task_ros2/bidding/MinimalBidder.hpp @@ -0,0 +1,87 @@ +/* + * 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__MINIMALBIDDER_HPP +#define RMF_TASK_ROS2__BIDDING__MINIMALBIDDER_HPP + +#include + +#include +#include + +#include + +namespace rmf_task_ros2 { +namespace bidding { + +//============================================================================== +class MinimalBidder +{ +public: + /// Type of Task in RMF + using TaskTypeMsg = rmf_task_msgs::msg::TaskType; + enum class TaskType + { + Station = TaskTypeMsg::TYPE_STATION, + Loop = TaskTypeMsg::TYPE_LOOP, + Delivery = TaskTypeMsg::TYPE_DELIVERY, + ChargeBattery = TaskTypeMsg::TYPE_CHARGE_BATTERY, + Clean = TaskTypeMsg::TYPE_CLEAN, + Patrol = TaskTypeMsg::TYPE_PATROL + }; + + /// Callback function when a bid notice is received from the autioneer + /// + /// \param[in] notice + /// bid notice msg + /// + /// \return submission + /// Estimates of a task. This submission is used by dispatcher for eval + using ParseSubmissionCallback = + std::function; + + + /// Create a bidder to bid for incoming task requests from Task Dispatcher + /// + /// \param[in] node + /// ROS 2 node instance + /// + /// \param[in] fleet_name + /// Name of the bidder + /// + /// \param[in] valid_task_types + /// A list of valid tasks types which are supported by the bidder + /// + /// \param[in] submission_cb + /// fn which is used to provide a bid submission during a call for bid + static std::shared_ptr make( + const std::shared_ptr& node, + const std::string& fleet_name, + const std::unordered_set& valid_task_types, + ParseSubmissionCallback submission_cb); + + class Implementation; + +private: + MinimalBidder(); + rmf_utils::unique_impl_ptr _pimpl; +}; + +} // namespace bidding +} // namespace rmf_task_ros2 + +#endif // RMF_TASK_ROS2__BIDDING__MINIMALBIDDER_HPP diff --git a/rmf_task_ros2/include/rmf_task_ros2/bidding/Submission.hpp b/rmf_task_ros2/include/rmf_task_ros2/bidding/Submission.hpp new file mode 100644 index 000000000..b9b8cc813 --- /dev/null +++ b/rmf_task_ros2/include/rmf_task_ros2/bidding/Submission.hpp @@ -0,0 +1,44 @@ +/* + * 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/package.xml b/rmf_task_ros2/package.xml new file mode 100644 index 000000000..268800c53 --- /dev/null +++ b/rmf_task_ros2/package.xml @@ -0,0 +1,26 @@ + + + + rmf_task_ros2 + 1.1.0 + A package managing the dispatching of tasks in RMF system. + youliang + Apache License 2.0 + + ament_cmake + + rmf_utils + rmf_traffic + rmf_traffic_ros2 + rmf_task_msgs + rclcpp + + eigen + + ament_cmake_catch2 + rmf_cmake_uncrustify + + + ament_cmake + + diff --git a/rmf_task_ros2/src/dispatcher_node/main.cpp b/rmf_task_ros2/src/dispatcher_node/main.cpp new file mode 100644 index 000000000..a96d066fd --- /dev/null +++ b/rmf_task_ros2/src/dispatcher_node/main.cpp @@ -0,0 +1,34 @@ +/* + * 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 + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + std::cout << "~Initializing Dispatcher Node~" << std::endl; + + auto dispatcher = rmf_task_ros2::Dispatcher::make_node( + "rmf_dispatcher_node"); + + 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"); + rclcpp::shutdown(); +} diff --git a/rmf_task_ros2/src/mock_bidder/main.cpp b/rmf_task_ros2/src/mock_bidder/main.cpp new file mode 100644 index 000000000..7340c6b30 --- /dev/null +++ b/rmf_task_ros2/src/mock_bidder/main.cpp @@ -0,0 +1,119 @@ +/* + * 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. + * +*/ + +/// Note: This is a testing bidder node script + +#include +#include "../rmf_task_ros2/action/Server.hpp" + +#include +#include + +using namespace rmf_task_ros2; +using TaskType = bidding::MinimalBidder::TaskType; + +int main(int argc, char* argv[]) +{ + std::string fleet_name = "dummy_fleet"; + + rclcpp::init(argc, argv); + std::shared_ptr node = rclcpp::Node::make_shared(fleet_name); + + RCLCPP_INFO( + node->get_logger(), + "Beginning example task bidder node"); + + //============================================================================ + // Create Bidder instance + std::shared_ptr bidder = bidding::MinimalBidder::make( + node, + fleet_name, + { TaskType::Clean, TaskType::Delivery }, + [](const bidding::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; + best_robot_estimate.robot_name = "dumbot"; + best_robot_estimate.prev_cost = 10.2; + best_robot_estimate.new_cost = 13.5; + best_robot_estimate.finish_time = + rmf_traffic::time::apply_offset(req_start_time, 7); + return best_robot_estimate; + } + ); + + //============================================================================ + // Create sample RMF task action server + auto action_server = action::Server::make(node, fleet_name); + + action_server->register_callbacks( + [&action_server, &node](const TaskProfile& task_profile) + { + std::cout << "[MockBidder] ~Start Queue Task: " + << task_profile.task_id<now()); + status.end_time = + rmf_traffic::time::apply_offset(status.start_time, 7); + + const auto id = profile.task_id; + std::cout << " [MockBidder] Queued, TaskID: " << id << std::endl; + 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); + + 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); + }, task_profile + ); + t.detach(); + + return true; //successs (send State::Queued) + }, + [&action_server](const TaskProfile& task_profile) + { + std::cout << "[MockBidder] ~Cancel Executing Task: " + << task_profile.task_id<get_logger(), + "Closing down task bidder node"); + + rclcpp::shutdown(); +} diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp new file mode 100644 index 000000000..1133177f6 --- /dev/null +++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp @@ -0,0 +1,440 @@ +/* + * 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 + +#include "action/Client.hpp" + +#include +#include +#include + +#include + +namespace rmf_task_ros2 { + +//============================================================================== +class Dispatcher::Implementation +{ +public: + std::shared_ptr node; + std::shared_ptr auctioneer; + std::shared_ptr action_client; + + using SubmitTaskSrv = rmf_task_msgs::srv::SubmitTask; + using CancelTaskSrv = rmf_task_msgs::srv::CancelTask; + using GetTaskListSrv = rmf_task_msgs::srv::GetTaskList; + + rclcpp::Service::SharedPtr submit_task_srv; + rclcpp::Service::SharedPtr cancel_task_srv; + rclcpp::Service::SharedPtr get_task_list_srv; + + StatusCallback on_change_fn; + + 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; + + std::unordered_map task_type_name = + { + {0, "Station"}, + {1, "Loop"}, + {2, "Delivery"}, + {3, "ChargeBattery"}, + {4, "Clean"}, + {5, "Patrol"} + }; + + 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; + } + ); + + cancel_task_srv = node->create_service( + rmf_task_ros2::CancelTaskSrvName, + [this]( + const std::shared_ptr request, + std::shared_ptr response) + { + auto id = request->task_id; + response->success = this->cancel_task(id); + } + ); + + get_task_list_srv = node->create_service( + rmf_task_ros2::GetTaskListSrvName, + [this]( + const std::shared_ptr request, + std::shared_ptr response) + { + for (auto task : (this->active_dispatch_tasks)) + { + response->active_tasks.push_back( + rmf_task_ros2::convert_status(*(task.second))); + } + + // Terminated Tasks + for (auto task : (this->terminal_dispatch_tasks)) + { + response->terminated_tasks.push_back( + rmf_task_ros2::convert_status(*(task.second))); + } + response->success = true; + } + ); + } + + void start() + { + using namespace std::placeholders; + auctioneer = bidding::Auctioneer::make(node, + std::bind(&Implementation::receive_bidding_winner_cb, this, _1, _2)); + action_client->on_terminate( + std::bind(&Implementation::terminate_task, this, _1)); + action_client->on_change( + std::bind(&Implementation::task_status_cb, this, _1)); + } + + std::optional submit_task(const TaskDescription& description) + { + TaskProfile submitted_task; + submitted_task.submission_time = node->now(); + submitted_task.description = description; + + const auto task_type = static_cast(description.task_type.type); + + if (!task_type_name.count(task_type)) + { + RCLCPP_ERROR(node->get_logger(), "TaskType: %d is invalid", task_type); + return std::nullopt; + } + + // auto generate a task_id for a given submitted task + submitted_task.task_id = + task_type_name[task_type] + std::to_string(task_counter++); + + // 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; + + if (on_change_fn) + on_change_fn(new_task_status); + + bidding::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)); + auctioneer->start_bidding(bid_notice); + + return submitted_task.task_id; + } + + bool cancel_task(const TaskID& task_id) + { + // check if key exists + const auto it = active_dispatch_tasks.find(task_id); + if (it == active_dispatch_tasks.end()) + return false; + + // Cancel bidding. This will remove the bidding process + const auto& cancel_task_status = it->second; + 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); + + return true; + } + + // 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); + } + + const std::optional get_task_state( + const TaskID& task_id) const + { + // check if taskid exists in active tasks + auto it = active_dispatch_tasks.find(task_id); + if (it != active_dispatch_tasks.end()) + return it->second->state; + + // check if taskid exists in terminated tasks + it = terminal_dispatch_tasks.find(task_id); + if (it != terminal_dispatch_tasks.end()) + return it->second->state; + + return std::nullopt; + } + + void receive_bidding_winner_cb( + const TaskID& task_id, + const rmf_utils::optional winner) + { + const auto it = active_dispatch_tasks.find(task_id); + if (it == active_dispatch_tasks.end()) + return; + const auto& pending_task_status = it->second; + + if (!winner) + { + RCLCPP_WARN(node->get_logger(), "[Dispatch::Bidding Result] task " + "%s has no submissions during bidding :(", task_id.c_str()); + pending_task_status->state = TaskStatus::State::Failed; + terminate_task(pending_task_status); + + if (on_change_fn) + on_change_fn(pending_task_status); + + return; + } + + // now we know which fleet will execute the task + pending_task_status->fleet_name = winner->fleet_name; + + RCLCPP_INFO(node->get_logger(), "[Dispatch::Bidding Result] task " + "%s is accepted by Fleet adapter %s", + task_id.c_str(), winner->fleet_name.c_str()); + + // Remove previous self-generated charging task from "active_dispatch_tasks" + // this is to prevent duplicated charging task (as certain queued charging + // tasks are not terminated when task is reassigned). + // TODO: a better way to impl this + 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); + + if (is_charging_task && is_fleet_name) + it = active_dispatch_tasks.erase(it); + else + ++it; + } + + // add task to action server + action_client->add_task( + winner->fleet_name, + pending_task_status->task_profile, + pending_task_status); + + // Note: this might be untrue since task might be ignored by server + pending_task_status->state = TaskStatus::State::Queued; + } + + void terminate_task(const TaskStatusPtr terminate_status) + { + assert(terminate_status->is_terminated()); + + // prevent terminal_dispatch_tasks from piling up meaning + if (terminal_dispatch_tasks.size() >= terminated_tasks_max_size) + { + RCLCPP_WARN(node->get_logger(), + "Terminated tasks reached max size, remove earliest submited task"); + + 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)) + rm_task = it; + } + terminal_dispatch_tasks.erase(terminal_dispatch_tasks.begin() ); + } + + const auto id = terminate_status->task_profile.task_id; + RCLCPP_WARN(node->get_logger(), "Terminate Task ID: %s", id.c_str()); + + // destroy prev status ptr and recreate one + auto status = std::make_shared(*terminate_status); + (terminal_dispatch_tasks)[id] = status; + active_dispatch_tasks.erase(id); + } + + void task_status_cb(const TaskStatusPtr status) + { + // 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 auto it = active_dispatch_tasks.find(id); + if (it == active_dispatch_tasks.end()) + { + active_dispatch_tasks[id] = status; + RCLCPP_WARN(node->get_logger(), + "Add previously unheard task: %s", id.c_str()); + } + + if (on_change_fn) + on_change_fn(status); + } +}; + +//============================================================================== +std::shared_ptr Dispatcher::init_and_make_node( + const std::string dispatcher_node_name) +{ + rclcpp::init(0, nullptr); + return make_node(dispatcher_node_name); +} + +//============================================================================== +std::shared_ptr Dispatcher::make_node( + const std::string dispatcher_node_name) +{ + return make(rclcpp::Node::make_shared(dispatcher_node_name)); +} + +//============================================================================== +std::shared_ptr Dispatcher::make( + const std::shared_ptr& node) +{ + auto pimpl = rmf_utils::make_impl(node); + pimpl->action_client = action::Client::make(node); + + auto dispatcher = std::shared_ptr(new Dispatcher()); + dispatcher->_pimpl = std::move(pimpl); + dispatcher->_pimpl->start(); + return dispatcher; +} + +//============================================================================== +std::optional Dispatcher::submit_task( + const TaskDescription& task_description) +{ + return _pimpl->submit_task(task_description); +} + +//============================================================================== +bool Dispatcher::cancel_task(const TaskID& task_id) +{ + return _pimpl->cancel_task(task_id); +} + +//============================================================================== +const std::optional Dispatcher::get_task_state( + const TaskID& task_id) const +{ + return _pimpl->get_task_state(task_id); +} + +//============================================================================== +const Dispatcher::DispatchTasks& Dispatcher::active_tasks() const +{ + return _pimpl->active_dispatch_tasks; +} + +//============================================================================== +const Dispatcher::DispatchTasks& Dispatcher::terminated_tasks() const +{ + return _pimpl->terminal_dispatch_tasks; +} + +//============================================================================== +void Dispatcher::on_change(StatusCallback on_change_fn) +{ + _pimpl->on_change_fn = on_change_fn; +} + +//============================================================================== +void Dispatcher::evaluator( + std::shared_ptr evaluator) +{ + _pimpl->auctioneer->select_evaluator(evaluator); +} + +//============================================================================== +std::shared_ptr Dispatcher::node() +{ + return _pimpl->node; +} + +//============================================================================== +void Dispatcher::spin() +{ + rclcpp::spin(_pimpl->node); +} + +//============================================================================== +Dispatcher::Dispatcher() +{ + // Do Nothing +} + +} // namespace rmf_task_ros2 diff --git a/rmf_task_ros2/src/rmf_task_ros2/TaskStatus.cpp b/rmf_task_ros2/src/rmf_task_ros2/TaskStatus.cpp new file mode 100644 index 000000000..cd06e3c94 --- /dev/null +++ b/rmf_task_ros2/src/rmf_task_ros2/TaskStatus.cpp @@ -0,0 +1,60 @@ +/* + * 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_ros2 { + +// ============================================================================== +bool TaskStatus::is_terminated() const +{ + return (state == State::Failed) || + (state == State::Completed) || + (state == State::Canceled); +} + +// ============================================================================== +TaskStatus convert_status(const StatusMsg& from) +{ + 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; +} + +// ============================================================================== +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.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; +} + +} // 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 new file mode 100644 index 000000000..faca48311 --- /dev/null +++ b/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp @@ -0,0 +1,179 @@ +/* + * 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 "Client.hpp" + +namespace rmf_task_ros2 { +namespace action { + +std::shared_ptr +Client::make(std::shared_ptr node) +{ + return std::shared_ptr(new Client(node)); +} + +//============================================================================== +Client::Client(std::shared_ptr node) +: _node(node) +{ + const auto dispatch_qos = rclcpp::ServicesQoS().reliable(); + + _request_msg_pub = _node->create_publisher( + TaskRequestTopicName, dispatch_qos); + + _status_msg_sub = _node->create_subscription( + TaskStatusTopicName, dispatch_qos, + [&](const std::unique_ptr msg) + { + auto task_id = msg->task_profile.task_id; + // status update mode + if (_active_task_status.count(task_id)) + { + auto weak_status = _active_task_status[task_id].lock(); + + if (!weak_status) + { + RCLCPP_INFO(_node->get_logger(), "status has expired"); + _active_task_status.erase(task_id); + 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; + + if (_on_change_callback) + _on_change_callback(weak_status); + + // if active task terminated + if (weak_status->is_terminated()) + { + RCLCPP_INFO(_node->get_logger(), + "[action] Done Terminated Task: %s", task_id.c_str()); + _active_task_status.erase(task_id); + + if (_on_terminate_callback) + _on_terminate_callback(weak_status); + } + } + else + { + // will still provide onchange even if the task_id is unknown. + RCLCPP_WARN(_node->get_logger(), + "[action] Unknown task: %s", task_id.c_str()); + auto task_status = std::make_shared(convert_status(*msg)); + + if (_on_change_callback) + _on_change_callback(task_status); + + if (!task_status->is_terminated()) + _active_task_status[task_id] = task_status; + } + }); +} + +//============================================================================== +void Client::add_task( + const std::string& fleet_name, + const TaskProfile& task_profile, + TaskStatusPtr status_ptr) +{ + // send request and wait for acknowledgement + RequestMsg request_msg; + 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; + RCLCPP_INFO(_node->get_logger(), "Add Action Task: %s", + task_profile.task_id.c_str()); + return; +} + +//============================================================================== +bool Client::cancel_task( + const TaskProfile& task_profile) +{ + const auto task_id = task_profile.task_id; + RCLCPP_INFO(_node->get_logger(), "Cancel Active Task: %s", task_id.c_str()); + + // check if task is previously added + if (!_active_task_status.count(task_id)) + { + RCLCPP_WARN(_node->get_logger(), + "[action] Not found Task: %s", task_id.c_str()); + return false; + } + + auto weak_status = _active_task_status[task_id].lock(); + + if (!weak_status) + { + std::cerr << "weak status has expired, cancel failed \n"; + _active_task_status.erase(task_id); + return false; + } + + // send cancel + RequestMsg request_msg; + 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; +} + +//============================================================================== +int Client::size() +{ + for (auto it = _active_task_status.begin(); it != _active_task_status.end(); ) + { + if (auto weak_status = it->second.lock() ) + { + if (weak_status->is_terminated()) + it = _active_task_status.erase(it); + else + ++it; + } + else + it = _active_task_status.erase(it); + } + return _active_task_status.size(); +} + +//============================================================================== +void Client::on_change( + StatusCallback status_cb_fn) +{ + _on_change_callback = std::move(status_cb_fn); +} + +//============================================================================== +void Client::on_terminate( + StatusCallback status_cb_fn) +{ + _on_terminate_callback = std::move(status_cb_fn); +} + +} // namespace action +} // 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 new file mode 100644 index 000000000..f1fa804eb --- /dev/null +++ b/rmf_task_ros2/src/rmf_task_ros2/action/Client.hpp @@ -0,0 +1,108 @@ +/* + * 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__ACTION__CLIENT_HPP +#define SRC__RMF_TASK_ROS2__ACTION__CLIENT_HPP + +#include + +#include +#include +#include + +namespace rmf_task_ros2 { +namespace action { + +//============================================================================== +// Task Action Client -- responsible for initiating a rmf task to a target +// 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: + /// make an action client + /// + /// \param[in] node + /// ros2 node instance + static std::shared_ptr make( + std::shared_ptr node); + + /// Add 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( + const std::string& fleet_name, + const TaskProfile& task_profile, + TaskStatusPtr status_ptr); + + /// Cancel an added task + /// + /// \param[in] task_profile + /// Task which to cancel + /// + /// \return bool which indicate if cancel task is success + bool cancel_task(const TaskProfile& task_profile); + + /// Get the number of active task being track by client + /// + /// \return number of active task + int size(); + + /// Callback Function which will trigger during an event + /// + /// \param[in] status + /// Status of a task which the event is triggered + using StatusCallback = std::function; + + /// Callback when a task status has changed + /// + /// \param[in] status_cb_fn + /// Status callback function + void on_change(StatusCallback status_cb_fn); + + /// Callback when a task is terminated + /// + /// \param[in] status_cb_fn + /// Status callback function + void on_terminate(StatusCallback status_cb_fn); + +private: + Client(std::shared_ptr node); + + using RequestMsg = rmf_task_msgs::msg::DispatchRequest; + + std::shared_ptr _node; + StatusCallback _on_change_callback; + StatusCallback _on_terminate_callback; + std::unordered_map> _active_task_status; + rclcpp::Publisher::SharedPtr _request_msg_pub; + rclcpp::Subscription::SharedPtr _status_msg_sub; +}; + +} // namespace action +} // 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 new file mode 100644 index 000000000..f0d760835 --- /dev/null +++ b/rmf_task_ros2/src/rmf_task_ros2/action/Server.cpp @@ -0,0 +1,97 @@ +/* + * 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 "Server.hpp" + +namespace rmf_task_ros2 { +namespace action { + +std::shared_ptr Server::make( + std::shared_ptr node, + const std::string& fleet_name) +{ + return std::shared_ptr(new Server(node, fleet_name)); +} + +//============================================================================== +void Server::register_callbacks( + AddTaskCallback add_task_cb_fn, + CancelTaskCallback cancel_task_cb_fn) +{ + _add_task_cb_fn = std::move(add_task_cb_fn); + _cancel_task_cb_fn = std::move(cancel_task_cb_fn); +} + +//============================================================================== +void Server::update_status( + const TaskStatus& task_status) +{ + auto msg = convert_status(task_status); + msg.fleet_name = _fleet_name; + _status_msg_pub->publish(msg); +} + +//============================================================================== +Server::Server( + std::shared_ptr node, + const std::string& fleet_name) +: _node(node), _fleet_name(fleet_name) +{ + const auto dispatch_qos = rclcpp::ServicesQoS().reliable(); + + _request_msg_sub = _node->create_subscription( + TaskRequestTopicName, dispatch_qos, + [&](const std::unique_ptr msg) + { + if (msg->fleet_name != _fleet_name) + return;// not me + + RCLCPP_INFO(_node->get_logger(), "[action] Receive a task request!"); + StatusMsg status_msg; + status_msg.fleet_name = _fleet_name; + status_msg.state = StatusMsg::STATE_FAILED; + status_msg.task_profile = msg->task_profile; + + switch (msg->method) + { + // Add Request + case RequestMsg::ADD: + { + if (_add_task_cb_fn && _add_task_cb_fn(msg->task_profile)) + status_msg.state = StatusMsg::STATE_QUEUED; + break; + } + // Cancel Request + case RequestMsg::CANCEL: + { + if (_cancel_task_cb_fn && _cancel_task_cb_fn(msg->task_profile)) + status_msg.state = StatusMsg::STATE_CANCELED; + break; + } + default: + RCLCPP_ERROR(_node->get_logger(), "Request Method is not supported!"); + } + + _status_msg_pub->publish(status_msg); + }); + + _status_msg_pub = _node->create_publisher( + TaskStatusTopicName, dispatch_qos); +} + +} // namespace action +} // namespace rmf_task_ros2 diff --git a/rmf_task_ros2/src/rmf_task_ros2/action/Server.hpp b/rmf_task_ros2/src/rmf_task_ros2/action/Server.hpp new file mode 100644 index 000000000..e85514bca --- /dev/null +++ b/rmf_task_ros2/src/rmf_task_ros2/action/Server.hpp @@ -0,0 +1,91 @@ +/* + * 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__ACTION__SERVER_HPP +#define SRC__RMF_TASK_ROS2__ACTION__SERVER_HPP + +#include + +#include +#include +#include +#include + +using TaskProfile = rmf_task_msgs::msg::TaskProfile; + +namespace rmf_task_ros2 { +namespace action { + +//============================================================================== +/// Task Action Server - This is used within the fleet adapter with the role of +/// receiving incoming dispatch requests (from a action_client/Dispatcher), +/// then execute the task accordingly. +class Server +{ +public: + /// initialize action server + /// + /// \param[in] node + /// ros2 node instance + /// + /// \param[in] fleet_name + /// action server id (e.g. fleet adapter name) + static std::shared_ptr make( + std::shared_ptr node, + const std::string& fleet_name); + + using AddTaskCallback = std::function; + using CancelTaskCallback = std::function; + + /// Add event callback functions. These functions will be triggered when a + /// relevant task requests msg is received. + /// + /// \param[in] add_task_cb + /// When a new task request is called. + /// + /// \param[in] cancel_task_cb + /// when a cancel task request is called + void register_callbacks( + AddTaskCallback add_task_cb, + CancelTaskCallback cancel_task_cb); + + /// Use this to send a status update to action client + /// A On Change update is recommended to inform the task progress + /// + /// \param[in] task_status + /// latest status of the task + void update_status(const TaskStatus& task_status); + +private: + Server( + std::shared_ptr node, + const std::string& fleet_name); + + using RequestMsg = rmf_task_msgs::msg::DispatchRequest; + + std::shared_ptr _node; + std::string _fleet_name; + AddTaskCallback _add_task_cb_fn; + CancelTaskCallback _cancel_task_cb_fn; + rclcpp::Subscription::SharedPtr _request_msg_sub; + rclcpp::Publisher::SharedPtr _status_msg_pub; +}; + +} // namespace action +} // namespace rmf_task_ros2 + +#endif // SRC__RMF_TASK_ROS2__ACTION__SERVER_HPP diff --git a/rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.cpp b/rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.cpp new file mode 100644 index 000000000..519c43637 --- /dev/null +++ b/rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.cpp @@ -0,0 +1,253 @@ +/* + * 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 "internal_Auctioneer.hpp" + +namespace rmf_task_ros2 { +namespace bidding { + +//============================================================================== +Submission convert(const BidProposal& from) +{ + Submission submission; + submission.fleet_name = from.fleet_name; + submission.robot_name = from.robot_name; + submission.prev_cost = from.prev_cost; + submission.new_cost = from.new_cost; + submission.finish_time = rmf_traffic_ros2::convert(from.finish_time); + return submission; +} + +//============================================================================== +Auctioneer::Implementation::Implementation( + const std::shared_ptr& node_, + BiddingResultCallback result_callback) +: node{std::move(node_)}, + bidding_result_callback{std::move(result_callback)} +{ + // default evaluator + evaluator = std::make_shared(); + const auto dispatch_qos = rclcpp::ServicesQoS().reliable(); + + bid_notice_pub = node->create_publisher( + rmf_task_ros2::BidNoticeTopicName, dispatch_qos); + + bid_proposal_sub = node->create_subscription( + rmf_task_ros2::BidProposalTopicName, dispatch_qos, + [&](const BidProposal::UniquePtr msg) + { + this->receive_proposal(*msg); + }); + + timer = node->create_wall_timer(std::chrono::milliseconds(500), [&]() + { + this->check_bidding_process(); + }); +} + +//============================================================================== +void Auctioneer::Implementation::start_bidding( + const BidNotice& bid_notice) +{ + RCLCPP_INFO(node->get_logger(), "[Auctioneer] Add Bidding task %s to queue", + bid_notice.task_profile.task_id.c_str()); + + BiddingTask bidding_task; + bidding_task.bid_notice = bid_notice; + bidding_task.start_time = node->now(); + queue_bidding_tasks.push(bidding_task); +} + +//============================================================================== +void Auctioneer::Implementation::receive_proposal( + const BidProposal& msg) +{ + const auto id = msg.task_profile.task_id; + RCLCPP_DEBUG(node->get_logger(), + "[Auctioneer] Receive proposal from task_id: %s | from: %s", + id.c_str(), msg.fleet_name.c_str()); + + // check if bidding task is initiated by the auctioneer previously + // add submited proposal to the current bidding tasks list + if (queue_bidding_tasks.front().bid_notice.task_profile.task_id == id) + queue_bidding_tasks.front().submissions.push_back(convert(msg)); +} + +//============================================================================== +// determine the winner within a bidding task instance +void Auctioneer::Implementation::check_bidding_process() +{ + if (queue_bidding_tasks.size() == 0) + return; + + // Executing the task at the front queue + auto front_task = queue_bidding_tasks.front(); + + if (bidding_in_proccess) + { + if (determine_winner(front_task)) + { + queue_bidding_tasks.pop(); + bidding_in_proccess = false; + } + } + else + { + RCLCPP_DEBUG(node->get_logger(), " - Start new bidding task: %s", + front_task.bid_notice.task_profile.task_id.c_str()); + queue_bidding_tasks.front().start_time = node->now(); + bid_notice_pub->publish(front_task.bid_notice); + bidding_in_proccess = true; + } +} + +//============================================================================== +bool Auctioneer::Implementation::determine_winner( + const BiddingTask& bidding_task) +{ + const auto duration = node->now() - bidding_task.start_time; + + if (duration > bidding_task.bid_notice.time_window) + { + auto id = bidding_task.bid_notice.task_profile.task_id; + RCLCPP_INFO(node->get_logger(), "Bidding Deadline reached: %s", + id.c_str()); + std::optional winner = std::nullopt; + + if (bidding_task.submissions.size() == 0) + { + RCLCPP_WARN(node->get_logger(), + "Bidding task has not received any bids"); + } + else + { + winner = evaluate(bidding_task.submissions); + RCLCPP_INFO(node->get_logger(), + "Found winning Fleet Adapter: %s, from %d submissions", + winner->fleet_name.c_str(), bidding_task.submissions.size()); + } + + // Call the user defined callback function + if (bidding_result_callback) + bidding_result_callback(id, winner); + + return true; + } + return false; +} + +//============================================================================== +std::optional Auctioneer::Implementation::evaluate( + const Submissions& submissions) +{ + if (submissions.size() == 0) + return std::nullopt; + + if (!evaluator) + { + RCLCPP_WARN(node->get_logger(), "Evaluator is not set"); + return std::nullopt; + } + + const std::size_t choice = evaluator->choose(submissions); + + if (choice >= submissions.size()) + return std::nullopt; + + return submissions[choice]; +} + +//============================================================================== +std::shared_ptr Auctioneer::make( + const std::shared_ptr& node, + BiddingResultCallback result_callback) +{ + auto pimpl = rmf_utils::make_unique_impl( + node, result_callback); + auto auctioneer = std::shared_ptr(new Auctioneer()); + auctioneer->_pimpl = std::move(pimpl); + return auctioneer; +} + +//============================================================================== +void Auctioneer::start_bidding(const BidNotice& bid_notice) +{ + _pimpl->start_bidding(bid_notice); +} + +//============================================================================== +void Auctioneer::select_evaluator( + std::shared_ptr evaluator) +{ + _pimpl->evaluator = std::move(evaluator); +} + +//============================================================================== +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 new file mode 100644 index 000000000..bd1230acb --- /dev/null +++ b/rmf_task_ros2/src/rmf_task_ros2/bidding/MinimalBidder.cpp @@ -0,0 +1,132 @@ +/* + * 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 + +#include +#include +#include + +namespace rmf_task_ros2 { +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 +{ +public: + + std::shared_ptr node; + std::string fleet_name; + std::unordered_set valid_task_types; + ParseSubmissionCallback get_submission_fn; + + using BidNoticeSub = rclcpp::Subscription; + BidNoticeSub::SharedPtr dispatch_notice_sub; + + using BidProposalPub = rclcpp::Publisher; + BidProposalPub::SharedPtr dispatch_proposal_pub; + + Implementation( + std::shared_ptr node_, + const std::string& fleet_name_, + const std::unordered_set& valid_task_types_, + ParseSubmissionCallback submission_cb) + : node{std::move(node_)}, + fleet_name{std::move(fleet_name_)}, + valid_task_types{std::move(valid_task_types_)}, + get_submission_fn{std::move(submission_cb)} + { + const auto dispatch_qos = rclcpp::ServicesQoS().reliable(); + + dispatch_notice_sub = node->create_subscription( + rmf_task_ros2::BidNoticeTopicName, dispatch_qos, + [&](const BidNotice::UniquePtr msg) + { + this->receive_notice(*msg); + }); + + dispatch_proposal_pub = node->create_publisher( + rmf_task_ros2::BidProposalTopicName, dispatch_qos); + } + + // Callback fn when a dispatch notice is received + void receive_notice(const BidNotice& msg) + { + RCLCPP_INFO(node->get_logger(), + "[Bidder] Received Bidding notice for task_id: %s", + msg.task_profile.task_id.c_str()); + + const auto task_type = (msg.task_profile.description.task_type.type); + + // check if task type is valid + if (!valid_task_types.count(static_cast(task_type))) + { + RCLCPP_WARN(node->get_logger(), "%s: task type %d", + fleet_name.c_str(), task_type); + return; + } + + // check if get submission function is declared + 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); + } +}; + +//============================================================================== +std::shared_ptr MinimalBidder::make( + const std::shared_ptr& node, + const std::string& fleet_name, + const std::unordered_set& valid_task_types, + ParseSubmissionCallback submission_cb) +{ + auto pimpl = rmf_utils::make_unique_impl( + node, fleet_name, valid_task_types, submission_cb); + auto bidder = std::shared_ptr(new MinimalBidder()); + bidder->_pimpl = std::move(pimpl); + return bidder; +} + +//============================================================================== +MinimalBidder::MinimalBidder() +{ + // do nothing +} + +} // namespace bidding +} // namespace rmf_task_ros2 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 new file mode 100644 index 000000000..57cf1a0eb --- /dev/null +++ b/rmf_task_ros2/src/rmf_task_ros2/bidding/internal_Auctioneer.hpp @@ -0,0 +1,94 @@ +/* + * 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; + +//============================================================================== +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; + std::vector 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/action/test_ActionInterface.cpp b/rmf_task_ros2/test/action/test_ActionInterface.cpp new file mode 100644 index 000000000..11241a6e0 --- /dev/null +++ b/rmf_task_ros2/test/action/test_ActionInterface.cpp @@ -0,0 +1,187 @@ +/* + * 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/action/Client.hpp" +#include "../../src/rmf_task_ros2/action/Server.hpp" +#include + +#include +#include +#include + +namespace rmf_task_ros2 { +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; + + TaskProfile task_profile2; + task_profile2.task_id = "task2"; + task_profile2.description.task_type.type = + rmf_task_msgs::msg::TaskType::TYPE_STATION; + + //============================================================================ + + // received task to test + std::optional test_add_task; + std::optional test_cancel_task; + + // Creating 1 auctioneer and 1 bidder + rclcpp::init(0, nullptr); + 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); + + // received test request msg from client + action_server->register_callbacks( + // add task + [&test_add_task](const TaskProfile& task_profile) + { + test_add_task = task_profile; + return true; + }, + // cancel task + [&test_cancel_task](const TaskProfile& task_profile) + { + test_cancel_task = task_profile; + return true; + } + ); + + // ROS Spin: forever incompleted future + std::promise ready_promise; + std::shared_future ready_future(ready_promise.get_future()); + + WHEN("Add Task") + { + // Add invalid Task! + TaskStatusPtr status_ptr(new TaskStatus); + + action_client->add_task("wrong_server", task_profile1, status_ptr); + + 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); + + action_client->add_task("test_server", task_profile1, status_ptr); + executor.spin_until_future_complete(ready_future, + rmf_traffic::time::from_seconds(0.5)); + + // check status + REQUIRE(status_ptr->state == TaskStatus::State::Queued); + + // status ptr is destroyed, should not have anymore tracking + status_ptr.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); + + 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); + executor.spin_until_future_complete(ready_future, + rmf_traffic::time::from_seconds(0.5)); + REQUIRE(!test_cancel_task); + REQUIRE(cancel_success == false); // cancel failed + REQUIRE(action_client->size() == 1); + + // Valid Cancel task + action_client->cancel_task(task_profile2); + 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(action_client->size() == 0); + } + + //============================================================================ + + std::optional test_task_onchange; + std::optional test_task_onterminate; + + // received status update from server + action_client->on_change( + [&test_task_onchange](const TaskStatusPtr status) + { + test_task_onchange = *status; + } + ); + action_client->on_terminate( + [&test_task_onterminate](const TaskStatusPtr 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); + + executor.spin_until_future_complete(ready_future, + rmf_traffic::time::from_seconds(0.5)); + + 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; + + // Update it as executing + action_server->update_status(server_task); + executor.spin_until_future_complete(ready_future, + rmf_traffic::time::from_seconds(1.5)); + + REQUIRE(test_task_onchange->state == TaskStatus::State::Executing); + REQUIRE(!test_task_onterminate); // havnt terminated yet + + // completion + server_task.state = TaskStatus::State::Completed; + // Update it as executing + action_server->update_status(server_task); + executor.spin_until_future_complete(ready_future, + rmf_traffic::time::from_seconds(0.5)); + + REQUIRE(test_task_onterminate); + REQUIRE(test_task_onterminate->state == TaskStatus::State::Completed); + } + rclcpp::shutdown(); +} + +} // namespace action +} // namespace rmf_task_ros2 diff --git a/rmf_task_ros2/test/bidding/test_Evaluator.cpp b/rmf_task_ros2/test/bidding/test_Evaluator.cpp new file mode 100644 index 000000000..cc9159ad9 --- /dev/null +++ b/rmf_task_ros2/test/bidding/test_Evaluator.cpp @@ -0,0 +1,116 @@ +/* + * 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 new file mode 100644 index 000000000..4656e70e4 --- /dev/null +++ b/rmf_task_ros2/test/bidding/test_SelfBid.cpp @@ -0,0 +1,156 @@ +/* + * 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 +#include +#include + +#include +#include +#include + +namespace rmf_task_ros2 { +namespace bidding { + +using TaskProfile = rmf_task_msgs::msg::TaskProfile; +using TaskType = bidding::MinimalBidder::TaskType; + +//============================================================================== +BidNotice bidding_task1; +BidNotice bidding_task2; + +// set time window to 2s +auto timeout = rmf_traffic_ros2::convert(rmf_traffic::time::from_seconds(2.0)); + +//============================================================================== +SCENARIO("Auction with 2 Bids", "[TwoBids]") +{ + // Initializing bidding task + bidding_task1.task_profile.task_id = "bid1"; + bidding_task1.time_window = timeout; + bidding_task1.task_profile.description.task_type.type = + rmf_task_msgs::msg::TaskType::TYPE_STATION; + + bidding_task2.task_profile.task_id = "bid2"; + bidding_task2.time_window = timeout; + bidding_task2.task_profile.description.task_type.type = + rmf_task_msgs::msg::TaskType::TYPE_DELIVERY; + + //============================================================================ + // test received msg + std::optional test_notice_bidder1; + std::optional test_notice_bidder2; + std::string r_result_id = ""; + std::string r_result_winner = ""; + + // Creating 1 auctioneer and 1 bidder + rclcpp::init(0, nullptr); + auto node = rclcpp::Node::make_shared("test_selfbidding"); + + auto auctioneer = Auctioneer::make( + node, + /// Bidding Result Callback Function + [&r_result_id, &r_result_winner]( + const std::string& task_id, const std::optional winner) + { + if (!winner) + return; + r_result_id = task_id; + r_result_winner = winner->fleet_name; + return; + } + ); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + + auto bidder1 = MinimalBidder::make( + node, "bidder1", { TaskType::Station, TaskType::Delivery }, + [&test_notice_bidder1](const BidNotice& notice) + { + Submission best_robot_estimate; + test_notice_bidder1 = notice.task_profile; + return best_robot_estimate; + } + ); + + auto bidder2 = MinimalBidder::make( + node, "bidder2", { TaskType::Delivery, TaskType::Clean }, + [&test_notice_bidder2](const BidNotice& notice) + { + // TaskType should not be supported + 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; + } + ); + + // ROS Spin: forever incompleted future + std::promise ready_promise; + std::shared_future ready_future(ready_promise.get_future()); + + WHEN("First 'Station' Task Bid") + { + // start bidding + bidding_task1.task_profile.submission_time = node->now(); + auctioneer->start_bidding(bidding_task1); + + executor.spin_until_future_complete(ready_future, + rmf_traffic::time::from_seconds(1.0)); + + // Check if bidder 1 & 2 receive BidNotice1 + REQUIRE(test_notice_bidder1); + REQUIRE(test_notice_bidder1->task_id == bidding_task1.task_profile.task_id); + REQUIRE(!test_notice_bidder2); // bidder2 doesnt support tasktype + + executor.spin_until_future_complete(ready_future, + rmf_traffic::time::from_seconds(2.5)); + + // Check if Auctioneer received Bid from bidder1 + REQUIRE(r_result_winner == "bidder1"); + REQUIRE(r_result_id == "bid1"); + } + + WHEN("Second 'Delivery' Task bid") + { + // start bidding + bidding_task2.task_profile.submission_time = node->now(); + auctioneer->start_bidding(bidding_task2); + + executor.spin_until_future_complete(ready_future, + rmf_traffic::time::from_seconds(1.0)); + + // Check if bidder 1 & 2 receive BidNotice2 + auto task2_profile = bidding_task2.task_profile; + REQUIRE(test_notice_bidder1->task_id == task2_profile.task_id); + REQUIRE(test_notice_bidder2->task_id == task2_profile.task_id); + + executor.spin_until_future_complete(ready_future, + rmf_traffic::time::from_seconds(2.5)); + + // Check if Auctioneer received Bid from bidder1 + REQUIRE(r_result_winner == "bidder2"); + REQUIRE(r_result_id == "bid2"); + } + + rclcpp::shutdown(); +} + +} // namespace bidding +} // namespace rmf_task_ros2 diff --git a/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp b/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp new file mode 100644 index 000000000..266bdd2e9 --- /dev/null +++ b/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp @@ -0,0 +1,242 @@ +/* + * 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 + +// mock Fleet Adapter to test dispatcher +#include +#include "../../src/rmf_task_ros2/action/Server.hpp" + +#include +#include +#include + +#include +#include +#include + +namespace rmf_task_ros2 { + +//============================================================================== +SCENARIO("Dispatcehr API Test", "[Dispatcher]") +{ + Dispatcher::TaskDescription task_desc1; + Dispatcher::TaskDescription task_desc2; + task_desc1.task_type.type = rmf_task_msgs::msg::TaskType::TYPE_STATION; + task_desc2.task_type.type = rmf_task_msgs::msg::TaskType::TYPE_CLEAN; + + //============================================================================ + auto dispatcher = Dispatcher::init_and_make_node("test_dispatcher_node"); + + auto spin_thread = std::thread( + [&dispatcher]() + { + dispatcher->spin(); + }); + spin_thread.detach(); + + + WHEN("Check service interfaces") + { + using SubmitTaskSrv = rmf_task_msgs::srv::SubmitTask; + using CancelTaskSrv = rmf_task_msgs::srv::CancelTask; + using GetTaskListSrv = rmf_task_msgs::srv::GetTaskList; + + auto submit_client = dispatcher->node()->create_client( + rmf_task_ros2::SubmitTaskSrvName); + REQUIRE(submit_client->wait_for_service(std::chrono::milliseconds(0))); + auto cancel_client = dispatcher->node()->create_client( + rmf_task_ros2::CancelTaskSrvName); + REQUIRE(cancel_client->wait_for_service(std::chrono::milliseconds(0))); + auto get_tasks_client = dispatcher->node()->create_client( + rmf_task_ros2::GetTaskListSrvName); + REQUIRE(get_tasks_client->wait_for_service(std::chrono::milliseconds(0))); + } + + WHEN("Add 1 and cancel task") + { + // add task + const auto id = dispatcher->submit_task(task_desc1); + REQUIRE(dispatcher->active_tasks().size() == 1); + REQUIRE(dispatcher->terminated_tasks().size() == 0); + REQUIRE(dispatcher->get_task_state(*id) == TaskStatus::State::Pending); + + // cancel task + REQUIRE(dispatcher->cancel_task(*id)); + REQUIRE(dispatcher->active_tasks().size() == 0); + REQUIRE(dispatcher->terminated_tasks().size() == 1); + + // check random id + REQUIRE(!(dispatcher->get_task_state("non_existence_id"))); + + // add an invalid task + task_desc2.task_type.type = 10; // this is invalid + REQUIRE(dispatcher->submit_task(task_desc2) == std::nullopt); + } + + //============================================================================ + // test on change fn callback + int change_times = 0; + TaskProfile test_taskprofile; + dispatcher->on_change( + [&change_times, &test_taskprofile](const TaskStatusPtr status) + { + test_taskprofile = status->task_profile; + change_times++; + } + ); + + WHEN("Track Task till Bidding Failed") + { + // Submit first task and wait for bidding + auto id = dispatcher->submit_task(task_desc1); + REQUIRE(dispatcher->active_tasks().size() == 1); + REQUIRE(dispatcher->get_task_state(*id) == TaskStatus::State::Pending); + + // Default 2s timeout, wait 3s for timetout, should fail here + 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); + CHECK(change_times == 2); // add and failed + + // Submit another task + id = dispatcher->submit_task(task_desc2); + std::this_thread::sleep_for(std::chrono::milliseconds(3000)); + REQUIRE(dispatcher->terminated_tasks().size() == 2); + REQUIRE(test_taskprofile.task_id == *id); + CHECK(change_times == 4); // add and failed x2 + } + + //============================================================================ + // Setup Mock Fleetadapter: mock bidder to test + using TaskType = bidding::MinimalBidder::TaskType; + auto node = dispatcher->node(); + auto bidder = bidding::MinimalBidder::make( + node, + "dummy_fleet", + { TaskType::Station, TaskType::Clean }, + [](const bidding::BidNotice&) + { + // Provide a best estimate + bidding::Submission best_robot_estimate; + best_robot_estimate.new_cost = 13.5; + return best_robot_estimate; + } + ); + + //============================================================================ + // Setup Mock Fleetadapter: action server to test + auto action_server = action::Server::make(node, "dummy_fleet"); + + bool task_canceled_flag = false; + + action_server->register_callbacks( + // Add Task callback + [&action_server, &task_canceled_flag](const TaskProfile& task_profile) + { + // Start action task + auto t = std::thread( + [&action_server, &task_canceled_flag](auto profile) + { + TaskStatus status; + status.task_profile = profile; + status.robot_name = "dumbot"; + std::this_thread::sleep_for(std::chrono::seconds(2)); + + if (task_canceled_flag) + { + // std::cout << "[task impl] Cancelled!" << std::endl; + return; + } + + // Executing + 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); + }, task_profile + ); + t.detach(); + return true; //successs (send State::Queued) + }, + // Cancel Task callback + [&action_server, &task_canceled_flag](const TaskProfile&) + { + task_canceled_flag = true; + return true; //success ,send State::Canceled when dispatcher->cancel_task + } + ); + + //============================================================================ + WHEN("Full Dispatch cycle") + { + const auto id = dispatcher->submit_task(task_desc1); + CHECK(dispatcher->get_task_state(*id) == TaskStatus::State::Pending); + std::this_thread::sleep_for(std::chrono::milliseconds(3500)); + + // now should queue the task + CHECK(dispatcher->get_task_state(*id) == TaskStatus::State::Queued); + REQUIRE(dispatcher->terminated_tasks().size() == 0); + CHECK(change_times == 2); // Pending and Queued + + std::this_thread::sleep_for(std::chrono::seconds(3)); + CHECK(dispatcher->get_task_state(*id) == TaskStatus::State::Completed); + REQUIRE(dispatcher->active_tasks().size() == 0); + REQUIRE(dispatcher->terminated_tasks().size() == 1); + 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); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + CHECK(change_times == 5); // new stray charge task + REQUIRE(dispatcher->active_tasks().size() == 1); + } + + WHEN("Half way cancel Dispatch cycle") + { + const auto id = dispatcher->submit_task(task_desc2); + CHECK(dispatcher->get_task_state(*id) == TaskStatus::State::Pending); + REQUIRE(dispatcher->active_tasks().size() == 1); + std::this_thread::sleep_for(std::chrono::milliseconds(3000)); + + // cancel the task after QUEUED State + REQUIRE(dispatcher->cancel_task(*id)); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + 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; + CHECK(status->state == TaskStatus::State::Canceled); + CHECK(change_times == 3); // Pending -> Queued -> Canceled + } + + rclcpp::shutdown(); +} + +} // namespace rmf_task_ros2 diff --git a/rmf_task_ros2/test/main.cpp b/rmf_task_ros2/test/main.cpp new file mode 100644 index 000000000..23e837acb --- /dev/null +++ b/rmf_task_ros2/test/main.cpp @@ -0,0 +1,21 @@ +/* + * 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. + * +*/ + +#define CATCH_CONFIG_MAIN +#include + +// This will create the main(int argc, char* argv[]) entry point for testing