Merge branch 'main' into arjo/feat/integrated_ressys

rmf_fleet_adapter/schemas/priority_description__binary.json

@@ -0,0 +1,12 @@
+{
+  "$schema": "https://json-schema.org/draft/2020-12/schema",
+  "$id": "https://raw.githubusercontent.com/open-rmf/rmf_ros2/main/rmf_fleet_adapter/schemas/priority_description__binary.json",
+  "title": "Binary Priority Description",
+  "description": "Describes the priority of a task as a binary",
+  "type": "object",
+  "properties": {
+      "type": { "type": "string" },
+      "value": { "type": "integer", "minimum": 0, "maximum": 1 }
+  },
+  "required": ["type", "value"]
+}

rmf_fleet_adapter/src/lift_supervisor/Node.cpp

@@ -47,9 +47,6 @@ Node::Node()
     {
       _lift_state_update(std::move(msg));
     });
-
-  _emergency_notice_pub = create_publisher<EmergencyNotice>(
-    rmf_traffic_ros2::EmergencyTopicName, default_qos);
 }
 
 //==============================================================================
@@ -124,19 +121,6 @@ void Node::_lift_state_update(LiftState::UniquePtr msg)
     request.request_type = LiftRequest::REQUEST_END_SESSION;
     _lift_request_pub->publish(request);
   }
-
-  // For now, we do not need to publish this.
-
-//  std_msgs::msg::Bool emergency_msg;
-//  emergency_msg.data = false;
-
-//  if (LiftState::MODE_FIRE == msg->current_mode
-//      || LiftState::MODE_EMERGENCY == msg->current_mode)
-//  {
-//    emergency_msg.data = true;
-//  }
-
-//  _emergency_notice_pub->publish(emergency_msg);
 }
 
 } // namespace lift_supervisor

rmf_fleet_adapter/src/lift_supervisor/Node.hpp

@@ -53,10 +53,6 @@ class Node : public rclcpp::Node
   LiftStateSub::SharedPtr _lift_state_sub;
   void _lift_state_update(LiftState::UniquePtr msg);
 
-  using EmergencyNotice = std_msgs::msg::Bool;
-  using EmergencyNoticePub = rclcpp::Publisher<EmergencyNotice>;
-  EmergencyNoticePub::SharedPtr _emergency_notice_pub;
-
   std::unordered_map<std::string, LiftRequest::UniquePtr> _active_sessions;
 };
 

rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp

@@ -421,7 +421,11 @@ void copy_booking_data(
   {
     booking_json["labels"] = booking.labels();
   }
-  // TODO(MXG): Add priority
+  const auto priority = booking.priority();
+  if (priority)
+  {
+    booking_json["priority"] = booking.priority()->serialize();
+  }
 }
 
 //==============================================================================

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp

@@ -51,6 +51,7 @@
 #include <stdexcept>
 
 #include <rmf_fleet_adapter/schemas/place.hpp>
+#include <rmf_fleet_adapter/schemas/priority_description__binary.hpp>
 #include <rmf_api_msgs/schemas/task_request.hpp>
 
 namespace rmf_fleet_adapter {
@@ -241,38 +242,30 @@ std::shared_ptr<rmf_task::Request> FleetUpdateHandle::Implementation::convert(
   const auto p_it = request_msg.find("priority");
   if (p_it != request_msg.end())
   {
-    // Assume the schema is not valid until we have successfully parsed it.
-    bool valid_schema = false;
-    // TODO(YV): Validate with priority_description_Binary.json
-    if (p_it->contains("type") && p_it->contains("value"))
+    try
     {
-      const auto& p_type = (*p_it)["type"];
-      if (p_type.is_string() && p_type.get<std::string>() == "binary")
-      {
-        const auto& p_value = (*p_it)["value"];
-        if (p_value.is_number_integer())
-        {
-          // The message matches the expected schema, so now we can mark it as
-          // valid.
-          valid_schema = true;
+      static const auto validator =
+        make_validator(rmf_fleet_adapter::schemas::priority_description__binary);
+      validator.validate(*p_it);
 
-          // If we have an integer greater than 0, we assign a high priority.
-          // Else the priority will default to low.
-          if (p_value.get<uint64_t>() > 0)
-          {
-            priority = rmf_task::BinaryPriorityScheme::make_high_priority();
-          }
-        }
+      const auto& p_value = (*p_it)["value"];
+      if (p_value.get<uint64_t>() > 0)
+      {
+        priority = rmf_task::BinaryPriorityScheme::make_high_priority();
       }
     }
-
-    if (!valid_schema)
+    catch (const std::exception& e)
     {
-      errors.push_back(
-        make_error_str(
-          4, "Unsupported type",
-          "Fleet [" + name + "] does not support priority request: "
-          + p_it->dump() + "\nDefaulting to low binary priority."));
+      const std::string error_str = make_error_str(
+        4, "Unsupported type",
+        "Fleet [" + name + "] does not support priority request: "
+        + p_it->dump() + "\nDefaulting to low binary priority.");
+      RCLCPP_ERROR(
+        node->get_logger(),
+        "Malformed incoming priority description: %s\n%s",
+        e.what(),
+        error_str.c_str());
+      errors.push_back(error_str);
     }
   }
 

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp

@@ -76,7 +76,7 @@ std::shared_ptr<Node> Node::make(
 
   node->_emergency_notice_obs =
     node->create_observable<EmergencyNotice>(
-    rmf_traffic_ros2::EmergencyTopicName, default_qos);
+    rmf_traffic_ros2::EmergencyTopicName, transient_qos);
 
   node->_ingestor_request_pub =
     node->create_publisher<IngestorRequest>(

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp

@@ -1525,10 +1525,17 @@ void RobotContext::_check_door_supervisor(
 {
   const auto now = std::chrono::steady_clock::now();
   const auto dt = std::chrono::seconds(10);
-  if (_last_active_task_time + dt < now)
+  if (_current_task_id.has_value())
   {
-    // Do not hold a door if a robot is idle for more than 10 seconds
-    _holding_door = std::nullopt;
+    _last_active_task_time = now;
+  }
+  else
+  {
+    if (_last_active_task_time + dt < now)
+    {
+      // Do not hold a door if a robot is idle for more than 10 seconds
+      _holding_door = std::nullopt;
+    }
   }
 
   for (const auto& door : state.all_sessions)