Feature/task dispatcher

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"]

rmf_fleet_adapter/src/full_control/main.cpp

@@ -614,7 +614,7 @@ std::shared_ptr<Connections> 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;

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(

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp

@@ -146,16 +146,17 @@ 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();
 
   // 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(

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"

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

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

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

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
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>
+    ${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
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_LIST_DIR}/>
+  )
+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()

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 <rclcpp/node.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rmf_utils/impl_ptr.hpp>
+#include <rmf_utils/optional.hpp>
+
+#include <rmf_task_ros2/bidding/Auctioneer.hpp>
+#include <rmf_task_ros2/TaskStatus.hpp>
+
+
+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<Dispatcher>
+{
+public:
+  using DispatchTasks = std::unordered_map<TaskID, TaskStatusPtr>;
+  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<Dispatcher> 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<Dispatcher> 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<Dispatcher> make(
+    const std::shared_ptr<rclcpp::Node>& 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<TaskID> 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<TaskStatus::State> 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<void(const TaskStatusPtr status)>;
+
+  /// 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<bidding::Auctioneer::Evaluator> evaluator);
+
+  /// Get the rclcpp::Node that this dispatcher will be using for communication.
+  std::shared_ptr<rclcpp::Node> node();
+
+  /// spin dispatcher node
+  void spin();
+
+  class Implementation;
+
+private:
+  Dispatcher();
+  rmf_utils::unique_impl_ptr<Implementation> _pimpl;
+};
+
+} // namespace rmf_task_ros2
+
+#endif // RMF_TASK_ROS2__DISPATCHER_HPP