AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-15 22:20:12 +00:00
Code Issues Releases Wiki Activity

Merge pull request #2243 from crewmatt/master

Browse Source 
Enhances gRPC Plugin
This commit is contained in:
erwincoumans 2019-05-07 12:33:39 -07:00 committed by GitHub
commit a5fe998f13
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 97 additions and 44 deletions
Show Stats 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);

21
examples/SharedMemory/grpc/ConvertGRPCBullet.cpp
View File

@ -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;

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