Add lookahead behavior for GoToPlace and fix various sources of seg faults

rmf_fleet_adapter/CHANGELOG.md

@@ -2,6 +2,7 @@
 
 2.1.0 (2022-XX-YY)
 ------------------
+* Add APIs for cancelling and killing tasks from the `RobotUpdateHandle`: [#205](https://github.com/open-rmf/rmf_ros2/pull/205)
 * Add a WaitUntil event and use it for ResponsiveWait: [#199](https://github.com/open-rmf/rmf_ros2/pull/199)
 
 2.0.0 (2022-03-18)

rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp

@@ -19,6 +19,7 @@
 #define RMF_FLEET_ADAPTER__AGV__ROBOTUPDATEHANDLE_HPP
 
 #include <rmf_traffic/Time.hpp>
+#include <rmf_traffic/agv/Planner.hpp>
 #include <rmf_utils/impl_ptr.hpp>
 #include <rmf_utils/optional.hpp>
 
@@ -92,6 +93,10 @@ class RobotUpdateHandle
     const double max_merge_lane_distance = 1.0,
     const double min_lane_length = 1e-8);
 
+  /// Update the current position of the robot by specifying a plan start set
+  /// for it.
+  void update_position(rmf_traffic::agv::Plan::StartSet position);
+
   /// Set the waypoint where the charger for this robot is located.
   /// If not specified, the nearest waypoint in the graph with the is_charger()
   /// property will be assumed as the charger for this robot.
@@ -218,6 +223,42 @@ class RobotUpdateHandle
     std::vector<std::string> labels,
     std::function<void()> robot_is_interrupted);
 
+  /// Cancel a task, if it has been assigned to this robot
+  ///
+  /// \param[in] task_id
+  ///   The ID of the task to be canceled
+  ///
+  /// \param[in] labels
+  ///   Labels that will be assigned to this cancellation. It is recommended to
+  ///   include information about why the cancellation is happening.
+  ///
+  /// \param[in] on_cancellation
+  ///   Callback that will be triggered after the cancellation is issued.
+  ///   task_was_found will be true if the task was successfully found and
+  ///   issued the cancellation, false otherwise.
+  void cancel_task(
+    std::string task_id,
+    std::vector<std::string> labels,
+    std::function<void(bool task_was_found)> on_cancellation);
+
+  /// Kill a task, if it has been assigned to this robot
+  ///
+  /// \param[in] task_id
+  ///   The ID of the task to be canceled
+  ///
+  /// \param[in] labels
+  ///   Labels that will be assigned to this cancellation. It is recommended to
+  ///   include information about why the cancellation is happening.
+  ///
+  /// \param[in] on_kill
+  ///   Callback that will be triggered after the cancellation is issued.
+  ///   task_was_found will be true if the task was successfully found and
+  ///   issued the kill, false otherwise.
+  void kill_task(
+    std::string task_id,
+    std::vector<std::string> labels,
+    std::function<void(bool task_was_found)> on_kill);
+
   enum class Tier
   {
     /// General status information, does not require special attention

rmf_fleet_adapter/schemas/event_description__go_to_place.json

@@ -3,5 +3,20 @@
   "$id": "https://raw.githubusercontent.com/open-rmf/rmf_ros2/main/rmf_fleet_adapter/schemas/event_description__go_to_place.json",
   "title": "Go To Place Event Description",
   "description": "Have a robot go to a place",
-  "$ref": "place.json"
+  "oneOf": [
+    {
+      "$ref": "place.json"
+    },
+    {
+      "type": "object",
+      "properties": {
+        "place": { "$ref": "place.json" },
+        "followed_by": {
+          "description": "A list of places that the robot might go after it reaches this one. It will not actually go to these places unless other activities bring it there, but the traffic system will be told to expect the robot to proceed through these places.",
+          "type": "array",
+          "items": { "$ref": "place.json" }
+        }
+      }
+    }
+  ]
 }