Event based lift / door logic (#320)

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai> Signed-off-by: Arjo Chakravarty <arjoc@google.com>
open-rmf · Jun 7, 2024 · dba5bbd · dba5bbd
1 parent eaec283
commit dba5bbd
Show file tree

Hide file tree

Showing 3 changed files with 15 additions and 4 deletions.
diff --git a/rmf_fleet_adapter/src/door_supervisor/Node.cpp b/rmf_fleet_adapter/src/door_supervisor/Node.cpp
@@ -28,7 +28,8 @@ const std::string DoorSupervisorRequesterID = "door_supervisor";
 Node::Node()
 : rclcpp::Node("door_supervisor")
 {
-  const auto default_qos = rclcpp::SystemDefaultsQoS().keep_last(10);
+  const auto default_qos =
+    rclcpp::SystemDefaultsQoS().durability_volatile().keep_last(100).reliable();
 
   _door_request_pub = create_publisher<DoorRequest>(
     FinalDoorRequestTopicName, default_qos);

diff --git a/rmf_fleet_adapter/src/lift_supervisor/Node.cpp b/rmf_fleet_adapter/src/lift_supervisor/Node.cpp
@@ -27,7 +27,8 @@ namespace lift_supervisor {
 Node::Node()
 : rclcpp::Node("rmf_lift_supervisor")
 {
-  const auto default_qos = rclcpp::SystemDefaultsQoS().keep_last(10);
+  const auto default_qos =
+    rclcpp::SystemDefaultsQoS().durability_volatile().keep_last(100).reliable();
   const auto transient_qos = rclcpp::SystemDefaultsQoS()
     .reliable().keep_last(100).transient_local();
 
@@ -68,12 +69,14 @@ void Node::_adapter_lift_request_update(LiftRequest::UniquePtr msg)
   {
     if (curr_request->session_id == msg->session_id)
     {
+      msg->request_time = this->now();
+      _lift_request_pub->publish(*msg);
       if (msg->request_type != LiftRequest::REQUEST_END_SESSION)
+      {
         curr_request = std::move(msg);
+      }
       else
       {
-        msg->request_time = this->now();
-        _lift_request_pub->publish(*msg);
         RCLCPP_INFO(
           this->get_logger(),
           "[%s] Published end lift session from lift supervisor",
@@ -85,6 +88,7 @@ void Node::_adapter_lift_request_update(LiftRequest::UniquePtr msg)
   }
   else
   {
+    _lift_request_pub->publish(*msg);
     if (msg->request_type != LiftRequest::REQUEST_END_SESSION)
     {
       curr_request = std::move(msg);

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp
@@ -1077,6 +1077,12 @@ void RobotContext::release_lift()
       "Releasing lift [%s] for [%s]",
       _lift_destination->lift_name.c_str(),
       requester_id().c_str());
+    rmf_lift_msgs::msg::LiftRequest msg;
+    msg.lift_name = _lift_destination->lift_name;
+    msg.request_type = rmf_lift_msgs::msg::LiftRequest::REQUEST_END_SESSION;
+    msg.session_id = requester_id();
+    msg.destination_floor = _lift_destination->destination_floor;
+    _node->lift_request()->publish(msg);
   }
   _lift_destination = nullptr;
   _initial_time_idle_outside_lift = std::nullopt;