Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Reduces wait in the gRPC plugin, fix state bug.

Browse Source 
Updates the gRPC plugin to gather messages in its own thread, and
corrects the state details not being correctly added to shared memory.
This commit is contained in:
mbennice
2019-05-07 11:11:01 -07:00
parent 111d06abd0
commit cf16c5c51f
2 changed files with 77 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

53
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -2843,7 +2843,7 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
bool PhysicsServerCommandProcessor::supportsJointMotor(btMultiBody* mb, int mbLinkIndex)
{
bool canHaveMotor = (mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::eRevolute
bool canHaveMotor = (mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::eRevolute
|| mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::ePrismatic);
return canHaveMotor;
}
@@ -3056,7 +3056,7 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
std::string* baseName = new std::string(u2b.getLinkName(u2b.getRootLinkIndex()));
m_data->m_strings.push_back(baseName);
mb->setBaseName(baseName->c_str());
#if 0
btAlignedObjectArray<char> urdf;
mb->dumpUrdf(urdf);
@@ -3067,7 +3067,7 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
fclose(f);
}
#endif
}
else
{
@@ -6315,7 +6315,7 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc
//we store the state details in the shared memory block, to reduce status size
SendActualStateSharedMemoryStorage* stateDetails = (SendActualStateSharedMemoryStorage*)bufferServerToClient;
//make sure the storage fits, otherwise
btAssert(sizeof(SendActualStateSharedMemoryStorage) < bufferSizeInBytes);
if (sizeof(SendActualStateSharedMemoryStorage) > bufferSizeInBytes)
@@ -6327,11 +6327,12 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc
{
btMultiBody* mb = body->m_multiBody;
SharedMemoryStatus& serverCmd = serverStatusOut;
serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED;
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;
@@ -6540,9 +6541,10 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc
btRigidBody* rb = body->m_rigidBody;
SharedMemoryStatus& serverCmd = serverStatusOut;
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;
int totalDegreeOfFreedomQ = 0;
@@ -6584,6 +6586,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] =
@@ -7197,7 +7200,7 @@ bool PhysicsServerCommandProcessor::processCreateMultiBodyCommandSingle(const st
m_data->m_sdfRecentLoadedBodies.clear();
if (bodyUniqueId >= 0)
{
serverStatusOut.m_type = CMD_CREATE_MULTI_BODY_COMPLETED;
if (bufferSizeInBytes>0 && serverStatusOut.m_numDataStreamBytes==0)
{
@@ -7205,7 +7208,7 @@ bool PhysicsServerCommandProcessor::processCreateMultiBodyCommandSingle(const st
BT_PROFILE("autogenerateGraphicsObjects");
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
}
BT_PROFILE("createBodyInfoStream");
int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;
@@ -7667,7 +7670,7 @@ bool PhysicsServerCommandProcessor::processForwardDynamicsCommand(const struct S
}
btScalar deltaTimeScaled = m_data->m_physicsDeltaTime * simTimeScalingFactor;
int numSteps = 0;
if (m_data->m_numSimulationSubSteps > 0)
{
@@ -7694,7 +7697,7 @@ bool PhysicsServerCommandProcessor::processForwardDynamicsCommand(const struct S
m_data->m_dynamicsWorld->getAnalyticsData(islandAnalyticsData);
serverCmd.m_forwardDynamicsAnalyticsArgs.m_numIslands = islandAnalyticsData.size();
int numIslands = btMin(islandAnalyticsData.size(), MAX_ISLANDS_ANALYTICS);
for (int i=0;i<numIslands;i++)
{
serverCmd.m_forwardDynamicsAnalyticsArgs.m_numSolverCalls = islandAnalyticsData[i].m_numSolverCalls;
@@ -7786,7 +7789,7 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
{
mb->setCanWakeup(false);
}
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING)
@@ -7892,7 +7895,7 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
{
mb->setMaxCoordinateVelocity(clientCmd.m_changeDynamicsInfoArgs.m_maxJointVelocity);
}
}
else
{
@@ -7980,7 +7983,7 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
btRigidBody* childRb = &body->m_rigidBodyJoints[linkIndex]->getRigidBodyB();
rb = childRb;
}
}
}
@@ -8071,13 +8074,13 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
{
rb->setAnisotropicFriction(anisotropicFriction);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_PROCESSING_THRESHOLD)
{
rb->setContactProcessingThreshold(clientCmd.m_changeDynamicsInfoArgs.m_contactProcessingThreshold);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CCD_SWEPT_SPHERE_RADIUS)
{
@@ -8498,7 +8501,7 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
{
m_data->m_dynamicsWorld->getSolverInfo().m_reportSolverAnalytics = clientCmd.m_physSimParamArgs.m_reportSolverAnalytics;
}
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
return hasStatus;
@@ -8592,7 +8595,7 @@ bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMe
{
int posVarCount = mb->getLink(i).m_posVarCount;
bool hasPosVar = posVarCount > 0;
for (int j = 0; j < posVarCount; j++)
{
if (clientCmd.m_initPoseArgs.m_hasInitialStateQ[posVarCountIndex+j] == 0)
@@ -8621,7 +8624,7 @@ bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMe
mb->setJointVelMultiDof(i, vel);
}
}
bool hasVel = true;
for (int j = 0; j < mb->getLink(i).m_dofCount; j++)
{
@@ -8644,7 +8647,7 @@ bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMe
mb->setJointVelMultiDof(i, &clientCmd.m_initPoseArgs.m_initialStateQdot[uDofIndex]);
}
}
posVarCountIndex += mb->getLink(i).m_posVarCount;
uDofIndex += mb->getLink(i).m_dofCount;
}
@@ -9015,7 +9018,7 @@ bool PhysicsServerCommandProcessor::processInverseDynamicsCommand(const struct S
{
#ifdef STATIC_LINK_SPD_PLUGIN
{
cRBDModel* rbdModel = m_data->findOrCreateRBDModel(bodyHandle->m_multiBody,
cRBDModel* rbdModel = m_data->findOrCreateRBDModel(bodyHandle->m_multiBody,
clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ,
clientCmd.m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot);
if (rbdModel)
@@ -9032,9 +9035,9 @@ bool PhysicsServerCommandProcessor::processInverseDynamicsCommand(const struct S
}
serverCmd.m_inverseDynamicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
serverCmd.m_inverseDynamicsResultArgs.m_dofCount = dof;
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED;
}
}
#endif
@@ -9506,7 +9509,7 @@ bool PhysicsServerCommandProcessor::processRemoveBodyCommand(const struct Shared
int numCollisionObjects = m_data->m_dynamicsWorld->getNumCollisionObjects();
m_data->m_dynamicsWorld->removeMultiBody(bodyHandle->m_multiBody);
numCollisionObjects = m_data->m_dynamicsWorld->getNumCollisionObjects();
delete bodyHandle->m_multiBody;
bodyHandle->m_multiBody = 0;
serverCmd.m_type = CMD_REMOVE_BODY_COMPLETED;
@@ -10933,7 +10936,7 @@ bool PhysicsServerCommandProcessor::processRemoveStateCommand(const struct Share
bool hasStatus = true;
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_REMOVE_STATE_FAILED;
if (clientCmd.m_loadStateArguments.m_stateId >= 0)
{
if (clientCmd.m_loadStateArguments.m_stateId < m_data->m_savedStates.size())
@@ -11273,7 +11276,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
hasStatus = processCreateVisualShapeCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_CREATE_MULTI_BODY:
{
hasStatus = processCreateMultiBodyCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);

67
examples/SharedMemory/plugins/grpcPlugin/grpcPlugin.cpp
View File

@@ -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 = m_requestQueue.front();
m_requestQueue.pop_front();
status = static_cast<CallData*>(tag)->Proceed();
}
{
void* tag; // uniquely identifies a request.
bool ok;
// 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