diff --git a/rmf_fleet_adapter/src/door_supervisor/Node.cpp b/rmf_fleet_adapter/src/door_supervisor/Node.cpp index 5b5f149a..83abfc92 100644 --- 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( FinalDoorRequestTopicName, default_qos); diff --git a/rmf_fleet_adapter/src/lift_supervisor/Node.cpp b/rmf_fleet_adapter/src/lift_supervisor/Node.cpp index e1bb36e6..de59217a 100644 --- 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 index 5365b922..a3a31f63 100644 --- 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;