Merge branch 'master' of https://github.com/erwincoumans/bullet3

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -6337,6 +6337,7 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc
 
 		serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId;
 		serverCmd.m_sendActualStateArgs.m_numLinks = body->m_multiBody->getNumLinks();
+		serverCmd.m_numDataStreamBytes = sizeof(SendActualStateSharedMemoryStorage);
 		serverCmd.m_sendActualStateArgs.m_stateDetails = 0;
 		int totalDegreeOfFreedomQ = 0;
 		int totalDegreeOfFreedomU = 0;
@@ -6548,6 +6549,7 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc
 
           serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId;
           serverCmd.m_sendActualStateArgs.m_numLinks = 0;
+					serverCmd.m_numDataStreamBytes = sizeof(SendActualStateSharedMemoryStorage);
           serverCmd.m_sendActualStateArgs.m_stateDetails = 0;
 
           int totalDegreeOfFreedomQ = 0;
@@ -6590,6 +6592,7 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc
           serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED;
           serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId;
           serverCmd.m_sendActualStateArgs.m_numLinks = 0;
+					serverCmd.m_numDataStreamBytes = sizeof(SendActualStateSharedMemoryStorage);
           serverCmd.m_sendActualStateArgs.m_stateDetails = 0;
 
           serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] =

examples/SharedMemory/grpc/ConvertGRPCBullet.cpp

@@ -1413,6 +1413,22 @@ bool convertGRPCToStatus(const PyBulletStatus& grpcReply, SharedMemoryStatus& se
 
 			break;
 		}
+		case CMD_SDF_LOADING_COMPLETED:
+		{
+			converted = true;
+      const ::pybullet_grpc::SdfLoadedStatus* stat = &grpcReply.sdfstatus();
+      int numBodies = stat->bodyuniqueids_size();
+      if (numBodies > MAX_SDF_BODIES)
+			{
+				printf("SDF exceeds body capacity: %d > %d", numBodies, MAX_SDF_BODIES);
+			}
+      serverStatus.m_sdfLoadedArgs.m_numBodies = numBodies;
+      for (int i = 0; i < numBodies; i++)
+			{
+        serverStatus.m_sdfLoadedArgs.m_bodyUniqueIds[i] = stat->bodyuniqueids(i);
+			}
+			break;
+		}
 		case CMD_DESIRED_STATE_RECEIVED_COMPLETED:
 		{
 			converted = true;
@@ -1681,7 +1697,10 @@ bool convertStatusToGRPC(const SharedMemoryStatus& serverStatus, char* bufferSer
 		case CMD_ACTUAL_STATE_UPDATE_COMPLETED:
 		{
 			converted = true;
-			b3SharedMemoryStatusHandle statusHandle = (b3SharedMemoryStatusHandle)&serverStatus;
+			SharedMemoryStatus* status = (SharedMemoryStatus*)&serverStatus;
+      status->m_sendActualStateArgs.m_stateDetails = (SendActualStateSharedMemoryStorage*)bufferServerToClient;
+			b3SharedMemoryStatusHandle statusHandle = (b3SharedMemoryStatusHandle)status;
+
 			int bodyUniqueId;
 			int numLinks;
 

examples/SharedMemory/plugins/grpcPlugin/grpcPlugin.cpp

@@ -11,6 +11,9 @@
 
 #include <stdio.h>
 
+#include <mutex>
+#include <thread>
+
 #include <grpc++/grpc++.h>
 #include <grpc/support/log.h>
 #include "../../../Utils/b3Clock.h"
@@ -44,6 +47,9 @@ public:
 		if (server_)
 		{
 			server_->Shutdown();
+			m_requestThreadCancelled = true;
+			m_requestThread->join();
+			delete m_requestThread;
 			// Always shutdown the completion queue after the server.
 			cq_->Shutdown();
 			server_ = 0;
@@ -64,6 +70,10 @@ public:
 		server_ = m_builder.BuildAndStart();
 		std::cout << "grpcPlugin Bullet Physics GRPC server listening on " << hostNamePort << std::endl;
 
+		//Start the thread to gather the requests.
+		m_requestThreadCancelled = false;
+		m_requestThread = new std::thread(&ServerImpl::GatherRequests, this);
+
 		// Proceed to the server's main loop.
 		InitRpcs(comProc);
 	}
@@ -72,25 +82,13 @@ public:
 	bool HandleSingleRpc()
 	{
 		CallData::CallStatus status = CallData::CallStatus::CREATE;
-
+		std::lock_guard<std::mutex> guard(m_queueMutex);
-		{
+    if (!m_requestQueue.empty()) {
-			void* tag;  // uniquely identifies a request.
+      void* tag = m_requestQueue.front();
-			bool ok;
+      m_requestQueue.pop_front();
-
-			// Block waiting to read the next event from the completion queue. The
-			// event is uniquely identified by its tag, which in this case is the
-			// memory address of a CallData instance.
-			// The return value of Next should always be checked. This return value
-			// tells us whether there is any kind of event or cq_ is shutting down.
-
-			grpc::CompletionQueue::NextStatus nextStatus = cq_->AsyncNext(&tag, &ok, gpr_now(GPR_CLOCK_MONOTONIC));
-			if (nextStatus == grpc::CompletionQueue::NextStatus::GOT_EVENT)
-			{
-				//GPR_ASSERT(cq_->Next(&tag, &ok));
-				GPR_ASSERT(ok);
       status = static_cast<CallData*>(tag)->Proceed();
     }
-		}
+
 		return status == CallData::CallStatus::TERMINATE;
 	}
 
@@ -252,6 +250,39 @@ private:
 	std::unique_ptr<ServerCompletionQueue> cq_;
 	PyBulletAPI::AsyncService service_;
 	std::unique_ptr<Server> server_;
+
+	// Mutex to protect access to the request queue variables (m_requestQueue,
+	// m_requestThread, m_requestThreadCancelled).
+	std::mutex m_queueMutex;
+
+	// List of outstanding request tags.
+	std::list<void*> m_requestQueue;
+
+	// Whether or not the gathering thread is cancelled.
+	bool m_requestThreadCancelled;
+
+	// Thread to gather requests from the completion queue.
+	std::thread* m_requestThread;
+
+	void GatherRequests() {
+		void* tag;  // uniquely identifies a request.
+		bool ok;
+
+		while(!m_requestThreadCancelled) {
+			// Block waiting to read the next event from the completion queue. The
+			// event is uniquely identified by its tag, which in this case is the
+			// memory address of a CallData instance.
+			// The return value of Next should always be checked. This return value
+			// tells us whether there is any kind of event or cq_ is shutting down.
+			grpc::CompletionQueue::NextStatus nextStatus = cq_->AsyncNext(&tag, &ok, gpr_now(GPR_CLOCK_MONOTONIC));
+			if (nextStatus == grpc::CompletionQueue::NextStatus::GOT_EVENT)
+			{
+				GPR_ASSERT(ok);
+				std::lock_guard<std::mutex> guard(m_queueMutex);
+				m_requestQueue.push_back(tag);
+			}
+		}
+	}
 };
 
 struct grpcMyClass