Merge branch 'main' into feat/updates-over-ros2

open-rmf · Oct 4, 2024 · 489142f · 489142f
2 parents f61f8bd + 6ab83a2
commit 489142f
Show file tree

Hide file tree

Showing 10 changed files with 324 additions and 56 deletions.
diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp
@@ -316,6 +316,15 @@ class EasyFullControl::RobotConfiguration
   /// Set the minimum lane length.
   void set_min_lane_length(std::optional<double> distance);
 
+  /// Get the idle behavior.
+  ///
+  /// If std::nullopt is used, then the fleet-wide default finishing request
+  /// will be used.
+  std::optional<rmf_task::ConstRequestFactoryPtr> finishing_request() const;
+
+  /// Set the finishing request.
+  void set_finishing_request(std::optional<rmf_task::ConstRequestFactoryPtr> request);
+
   class Implementation;
 private:
   rmf_utils::impl_ptr<Implementation> _pimpl;

diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp
@@ -25,6 +25,8 @@
 
 #include <rmf_traffic/schedule/Participant.hpp>
 
+#include <rmf_task/RequestFactory.hpp>
+
 #include <Eigen/Geometry>
 #include <nlohmann/json.hpp>
 
@@ -102,6 +104,13 @@ class RobotUpdateHandle
   /// property will be assumed as the charger for this robot.
   RobotUpdateHandle& set_charger_waypoint(const std::size_t charger_wp);
 
+  /// Set a finishing request for this robot.
+  RobotUpdateHandle& set_finishing_request(rmf_task::ConstRequestFactoryPtr finishing_request);
+
+  /// Set a finishing request for this robot to use the fleet-wide finishing
+  /// request.
+  RobotUpdateHandle& use_default_finishing_request();
+
   /// Update the current battery level of the robot by specifying its state of
   /// charge as a fraction of its total charge capacity, i.e. a value from 0.0
   /// to 1.0.

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
@@ -950,6 +950,24 @@ void TaskManager::set_idle_task(rmf_task::ConstRequestFactoryPtr task)
   }
 }
 
+//==============================================================================
+void TaskManager::use_default_idle_task()
+{
+  auto fleet_handle = _fleet_handle.lock();
+  if (!fleet_handle)
+  {
+    RCLCPP_ERROR(
+      _context->node()->get_logger(),
+      "Attempting to use default idle task for [%s] but its fleet is shutting down",
+      _context->requester_id().c_str());
+    return;
+  }
+  const auto& impl =
+    agv::FleetUpdateHandle::Implementation::get(*fleet_handle);
+
+  set_idle_task(impl.idle_task);
+}
+
 //==============================================================================
 void TaskManager::set_queue(
   const std::vector<TaskManager::Assignment>& assignments)
@@ -1700,7 +1718,13 @@ void TaskManager::_begin_waiting()
   }
 
   if (!_responsive_wait_enabled)
+  {
+    if (_waiting)
+    {
+      _waiting.cancel({"Idle behavior updated"}, _context->now());
+    }
     return;
+  }
 
   if (_context->location().empty())
   {

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp
@@ -127,6 +127,8 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
 
   void set_idle_task(rmf_task::ConstRequestFactoryPtr task);
 
+  void use_default_idle_task();
+
   /// Set the queue for this task manager with assignments generated from the
   /// task planner
   void set_queue(const std::vector<Assignment>& assignments);