Add missing remove-from-waitset part (ros2#76)

Signed-off-by: Simon Hoinkis <[email protected]>
ApexAI · Jan 5, 2023 · 1911d21 · 1911d21
1 parent 1d4b469
commit 1911d21
Show file tree

Hide file tree

Showing 3 changed files with 37 additions and 6 deletions.
diff --git a/rmw_iceoryx_cpp/src/rmw_request.cpp b/rmw_iceoryx_cpp/src/rmw_request.cpp
@@ -77,7 +77,7 @@ rmw_send_request(
             rmw_iceoryx_cpp::serializeRequest(ros_request, &iceoryx_client_abstraction->type_supports_, payload_vector);
             memcpy(requestPayload, payload_vector.data(), payload_vector.size());
           }
-
+          std::cout << "Client sent request!" << std::endl;
           iceoryx_client->send(requestPayload).or_else(
               [&](auto&) {
                 RMW_SET_ERROR_MSG("rmw_send_request error!");
@@ -165,6 +165,7 @@ rmw_take_request(
   iceoryx_server->releaseRequest(user_payload);
   *taken = true;
   ret = RMW_RET_OK;
+  std::cout << "Server took request!" << std::endl;
 
   return ret;
 }

diff --git a/rmw_iceoryx_cpp/src/rmw_response.cpp b/rmw_iceoryx_cpp/src/rmw_response.cpp
@@ -74,7 +74,7 @@ rmw_take_response(
       }
       else
       {
-          std::cout << "Got Response with outdated sequence number!" << std::endl;
+        std::cout << "Got Response with outdated sequence number!" << std::endl;
         ret = RMW_RET_ERROR;
       }
     })
@@ -102,6 +102,7 @@ rmw_take_response(
   iceoryx_client->releaseResponse(user_payload);
   *taken = true;
   ret = RMW_RET_OK;
+  std::cout << "Client took response!" << std::endl;
 
   *taken = false;
   return ret;
@@ -154,6 +155,7 @@ rmw_send_response(
             rmw_iceoryx_cpp::serializeRequest(ros_response, &iceoryx_server_abstraction->type_supports_, payload_vector);
             memcpy(responsePayload, payload_vector.data(), payload_vector.size());
           }
+          std::cout << "Server sent response!" << std::endl;
           iceoryx_server->send(responsePayload).or_else(
               [&](auto&) {
                 RMW_SET_ERROR_MSG("rmw_send_response send error!");

diff --git a/rmw_iceoryx_cpp/src/rmw_wait.cpp b/rmw_iceoryx_cpp/src/rmw_wait.cpp
@@ -75,25 +75,25 @@ rmw_wait(
   // attach all iceoryx servers to WaitSet
   for (size_t i = 0; i < services->service_count; ++i) {
     auto iceoryx_server_abstraction =
-      static_cast<IceoryxServer *>(subscriptions->subscribers[i]);
+      static_cast<IceoryxServer *>(services->services[i]);
     auto iceoryx_server = iceoryx_server_abstraction->iceoryx_server_;
 
     waitset->attachState(*iceoryx_server, iox::popo::ServerState::HAS_REQUEST).or_else(
       [&](auto &) {
-        RMW_SET_ERROR_MSG("failed to attach subscriber");
+        RMW_SET_ERROR_MSG("failed to attach service");
         skip_wait = true;
       });
   }
 
   // attach all iceoryx client to WaitSet
   for (size_t i = 0; i < clients->client_count; ++i) {
     auto iceoryx_client_abstraction =
-      static_cast<IceoryxClient *>(subscriptions->subscribers[i]);
+      static_cast<IceoryxClient *>(clients->clients[i]);
     auto iceoryx_client = iceoryx_client_abstraction->iceoryx_client_;
 
     waitset->attachState(*iceoryx_client, iox::popo::ClientState::HAS_RESPONSE).or_else(
       [&](auto &) {
-        RMW_SET_ERROR_MSG("failed to attach subscriber");
+        RMW_SET_ERROR_MSG("failed to attach client");
         skip_wait = true;
       });
   }
@@ -141,6 +141,34 @@ rmw_wait(
     }
   }
 
+  // reset all the servers that don't have new data
+  for (size_t i = 0; i < services->service_count; ++i) {
+    auto iceoryx_server_abstraction =
+      static_cast<IceoryxServer *>(services->services[i]);
+    iox::popo::UntypedServer * iceoryx_server = iceoryx_server_abstraction->iceoryx_server_;
+
+    // remove waitset from all receivers because next call a new waitset could be provided
+    waitset->detachState(*iceoryx_server, iox::popo::ServerState::HAS_REQUEST);
+
+    if (!iceoryx_server->hasRequests()) {
+      services->services[i] = nullptr;
+    }
+  }
+
+  // reset all the clients that don't have new data
+  for (size_t i = 0; i < clients->client_count; ++i) {
+    auto iceoryx_client_abstraction =
+      static_cast<IceoryxClient *>(clients->clients[i]);
+    iox::popo::UntypedClient * iceoryx_client = iceoryx_client_abstraction->iceoryx_client_;
+
+    // remove waitset from all receivers because next call a new waitset could be provided
+    waitset->detachState(*iceoryx_client, iox::popo::ClientState::HAS_RESPONSE);
+
+    if (!iceoryx_client->hasResponses()) {
+      clients->clients[i] = nullptr;
+    }
+  }
+
   // reset all the guard_conditions that have not triggered
   for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) {
     auto iceoryx_guard_condition =