From 5e4badc099ca8c861cbc3e6c4f0450b92427c398 Mon Sep 17 00:00:00 2001 From: = <=> Date: Thu, 24 Sep 2015 22:50:34 -0700 Subject: [PATCH] make PhysicsClient an interface class, and move shared memory implementation of physics client into PhysicsClientSharedMemory class/file. This will allow implementation of UDP and other transport protocols to a physics server. --- examples/ExampleBrowser/CMakeLists.txt | 1 + examples/ExampleBrowser/premake4.lua | 1 + examples/SharedMemory/PhysicsClient.cpp | 611 +----------------- examples/SharedMemory/PhysicsClient.h | 55 +- examples/SharedMemory/PhysicsClientC_API.cpp | 34 +- examples/SharedMemory/PhysicsClientC_API.h | 2 +- .../PhysicsClientSharedMemory.cpp | 527 +++++++++++++++ .../SharedMemory/PhysicsClientSharedMemory.h | 48 ++ examples/SharedMemory/premake4.lua | 1 + test/SharedMemory/premake4.lua | 2 + 10 files changed, 623 insertions(+), 659 deletions(-) create mode 100644 examples/SharedMemory/PhysicsClientSharedMemory.cpp create mode 100644 examples/SharedMemory/PhysicsClientSharedMemory.h diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 317b89dbc..3d40b8c81 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -16,6 +16,7 @@ SET(App_ExampleBrowser_SRCS ExampleEntries.cpp ExampleEntries.h ../SharedMemory/PhysicsServer.cpp + ../SharedMemory/PhysicsClientSharedMemory.cpp ../SharedMemory/PhysicsClient.cpp ../SharedMemory/PhysicsClientC_API.cpp ../SharedMemory/PhysicsServerExample.cpp diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index fcc3330ab..439b64670 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -57,6 +57,7 @@ "../SharedMemory/PhysicsServerExample.cpp", "../SharedMemory/PhysicsClientExample.cpp", "../SharedMemory/PhysicsServer.cpp", + "../SharedMemory/PhysicsClientSharedMemory.cpp", "../SharedMemory/PhysicsClient.cpp", "../SharedMemory/PosixSharedMemory.cpp", "../SharedMemory/Win32SharedMemory.cpp", diff --git a/examples/SharedMemory/PhysicsClient.cpp b/examples/SharedMemory/PhysicsClient.cpp index 0a5e41d18..4eada50a8 100644 --- a/examples/SharedMemory/PhysicsClient.cpp +++ b/examples/SharedMemory/PhysicsClient.cpp @@ -1,612 +1,3 @@ #include "PhysicsClient.h" -#include "PosixSharedMemory.h" -#include "Win32SharedMemory.h" -#include "LinearMath/btAlignedObjectArray.h" -#include "LinearMath/btVector3.h" -#include "Bullet3Common/b3Logging.h" -#include "../Utils/b3ResourcePath.h" -#include "../../Extras/Serialize/BulletFileLoader/btBulletFile.h" -#include "../../Extras/Serialize/BulletFileLoader/autogenerated/bullet.h" -#include "SharedMemoryBlock.h" - - -//copied from btMultiBodyLink.h -enum JointType -{ - eRevoluteType = 0, - ePrismaticType = 1, -}; - -struct TmpFloat3 -{ - float m_x; - float m_y; - float m_z; - -}; - -TmpFloat3 CreateTmpFloat3(float x,float y, float z) -{ - TmpFloat3 tmp; tmp.m_x = x;tmp.m_y = y;tmp.m_z = z; return tmp; -} - -struct PhysicsClientSharedMemoryInternalData -{ - SharedMemoryInterface* m_sharedMemory; - SharedMemoryBlock* m_testBlock1; - - btAlignedObjectArray m_robotMultiBodyData; - btAlignedObjectArray m_jointInfo; - btAlignedObjectArray m_debugLinesFrom; - btAlignedObjectArray m_debugLinesTo; - btAlignedObjectArray m_debugLinesColor; - - SharedMemoryStatus m_lastServerStatus; - - int m_counter; - bool m_serverLoadUrdfOK; - bool m_isConnected; - bool m_waitingForServer; - bool m_hasLastServerStatus; - int m_sharedMemoryKey; - bool m_verboseOutput; - - PhysicsClientSharedMemoryInternalData() - :m_sharedMemory(0), - m_testBlock1(0), - m_counter(0), - m_serverLoadUrdfOK(false), - m_isConnected(false), - m_waitingForServer(false), - m_hasLastServerStatus(false), - m_sharedMemoryKey(SHARED_MEMORY_KEY), - m_verboseOutput(false) - { - } - - - - void processServerStatus(); - - bool canSubmitCommand() const; - - -}; - - -int PhysicsClientSharedMemory::getNumJoints() const -{ - return m_data->m_jointInfo.size(); -} - -void PhysicsClientSharedMemory::getJointInfo(int index, b3JointInfo& info) const -{ - info = m_data->m_jointInfo[index]; -} - - -PhysicsClientSharedMemory::PhysicsClientSharedMemory() - -{ - m_data = new PhysicsClientSharedMemoryInternalData; - -#ifdef _WIN32 - m_data->m_sharedMemory = new Win32SharedMemoryClient(); -#else - m_data->m_sharedMemory = new PosixSharedMemory(); -#endif - -} - -PhysicsClientSharedMemory::~PhysicsClientSharedMemory() -{ - if (m_data->m_isConnected) - { - disconnectSharedMemory(); - } - delete m_data->m_sharedMemory; - delete m_data; -} - -void PhysicsClientSharedMemory::setSharedMemoryKey(int key) -{ - m_data->m_sharedMemoryKey = key; -} - -void PhysicsClientSharedMemory::disconnectSharedMemory () -{ - if (m_data->m_isConnected) - { - m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE); - m_data->m_isConnected = false; - } -} - -bool PhysicsClientSharedMemory::isConnected() const -{ - return m_data->m_isConnected ; -} - -bool PhysicsClientSharedMemory::connect() -{ - ///server always has to create and initialize shared memory - bool allowCreation = false; - m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE, allowCreation); - - if (m_data->m_testBlock1) - { - if (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER) - { - b3Error("Error: please start server before client\n"); - m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE); - m_data->m_testBlock1 = 0; - return false; - } else - { - if (m_data->m_verboseOutput) - { - b3Printf("Connected to existing shared memory, status OK.\n"); - } - m_data->m_isConnected = true; - } - } else - { - b3Error("Cannot connect to shared memory"); - return false; - } - return true; -} - - - - -const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() -{ - SharedMemoryStatus* stat = 0; - - if (!m_data->m_testBlock1) - { - return 0; - } - - if (!m_data->m_waitingForServer) - { - return 0; - } - - if (m_data->m_testBlock1->m_numServerCommands> m_data->m_testBlock1->m_numProcessedServerCommands) - { - btAssert(m_data->m_testBlock1->m_numServerCommands==m_data->m_testBlock1->m_numProcessedServerCommands+1); - - const SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; - m_data->m_lastServerStatus = serverCmd; - - EnumSharedMemoryServerStatus s = (EnumSharedMemoryServerStatus)serverCmd.m_type; - //consume the command - switch (serverCmd.m_type) - { - case CMD_CLIENT_COMMAND_COMPLETED: - { - if (m_data->m_verboseOutput) - { - b3Printf("Server completed command"); - } - break; - } - case CMD_URDF_LOADING_COMPLETED: - { - m_data->m_serverLoadUrdfOK = true; - if (m_data->m_verboseOutput) - { - b3Printf("Server loading the URDF OK\n"); - } - - if (serverCmd.m_dataStreamArguments.m_streamChunkLength>0) - { - bParse::btBulletFile* bf = new bParse::btBulletFile(this->m_data->m_testBlock1->m_bulletStreamDataServerToClient,serverCmd.m_dataStreamArguments.m_streamChunkLength); - bf->setFileDNAisMemoryDNA(); - bf->parse(false); - m_data->m_robotMultiBodyData.push_back(bf); - - for (int i=0;im_multiBodies.size();i++) - { - int flag = bf->getFlags(); - int qOffset = 7; - int uOffset=6; - - if ((flag&bParse::FD_DOUBLE_PRECISION)!=0) - { - Bullet::btMultiBodyDoubleData* mb = (Bullet::btMultiBodyDoubleData*)bf->m_multiBodies[i]; - if (mb->m_baseName) - { - if (m_data->m_verboseOutput) - { - b3Printf("mb->m_baseName = %s\n",mb->m_baseName); - } - } - - for (int link=0;linkm_numLinks;link++) - { - { - b3JointInfo info; - info.m_flags = 0; - info.m_jointIndex = link; - info.m_qIndex = (0 < mb->m_links[link].m_posVarCount) ? qOffset : -1; - info.m_uIndex = (0 < mb->m_links[link].m_dofCount) ? uOffset : -1; - - if (mb->m_links[link].m_linkName) - { - if (m_data->m_verboseOutput) - { - b3Printf("mb->m_links[%d].m_linkName = %s\n",link,mb->m_links[link].m_linkName); - } - info.m_linkName = mb->m_links[link].m_linkName; - } - if (mb->m_links[link].m_jointName) - { - if (m_data->m_verboseOutput) - { - b3Printf("mb->m_links[%d].m_jointName = %s\n",link,mb->m_links[link].m_jointName); - } - info.m_jointName = mb->m_links[link].m_jointName; - info.m_jointType = mb->m_links[link].m_jointType; - } - if ((mb->m_links[link].m_jointType == eRevoluteType)|| - (mb->m_links[link].m_jointType == ePrismaticType)) - { - info.m_flags |= JOINT_HAS_MOTORIZED_POWER; - } - m_data->m_jointInfo.push_back(info); - } - qOffset+= mb->m_links[link].m_posVarCount; - uOffset+= mb->m_links[link].m_dofCount; - - } - - } else - { - Bullet::btMultiBodyFloatData* mb = (Bullet::btMultiBodyFloatData*) bf->m_multiBodies[i]; - if (mb->m_baseName) - { - if (m_data->m_verboseOutput) - { - b3Printf("mb->m_baseName = %s\n",mb->m_baseName); - } - } - for (int link=0;linkm_numLinks;link++) - { - { - b3JointInfo info; - info.m_flags = 0; - info.m_jointIndex = link; - info.m_qIndex = (0 < mb->m_links[link].m_posVarCount) ? qOffset : -1; - info.m_uIndex = (0 < mb->m_links[link].m_dofCount) ? uOffset : -1; - - if (mb->m_links[link].m_linkName) - { - if (m_data->m_verboseOutput) - { - b3Printf("mb->m_links[%d].m_linkName = %s\n",link,mb->m_links[link].m_linkName); - } - info.m_linkName = mb->m_links[link].m_linkName; - } - if (mb->m_links[link].m_jointName) - { - if (m_data->m_verboseOutput) - { - b3Printf("mb->m_links[%d].m_jointName = %s\n",link,mb->m_links[link].m_jointName); - } - info.m_jointName = mb->m_links[link].m_jointName; - info.m_jointType = mb->m_links[link].m_jointType; - } - if ((mb->m_links[link].m_jointType == eRevoluteType)|| - (mb->m_links[link].m_jointType == ePrismaticType)) - { - info.m_flags |= JOINT_HAS_MOTORIZED_POWER; - } - m_data->m_jointInfo.push_back(info); - } - qOffset+= mb->m_links[link].m_posVarCount; - uOffset+= mb->m_links[link].m_dofCount; - } - - } - } - if (bf->ok()) - { - if (m_data->m_verboseOutput) - { - b3Printf("Received robot description ok!\n"); - } - } else - { - b3Warning("Robot description not received"); - } - } - break; - } - case CMD_DESIRED_STATE_RECEIVED_COMPLETED: - { - if (m_data->m_verboseOutput) - { - b3Printf("Server received desired state"); - } - break; - } - case CMD_STEP_FORWARD_SIMULATION_COMPLETED: - { - if (m_data->m_verboseOutput) - { - b3Printf("Server completed step simulation"); - } - break; - } - case CMD_URDF_LOADING_FAILED: - { - if (m_data->m_verboseOutput) - { - b3Printf("Server failed loading the URDF...\n"); - } - m_data->m_serverLoadUrdfOK = false; - break; - } - - case CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED: - { - if (m_data->m_verboseOutput) - { - b3Printf("Server received bullet data stream OK\n"); - } - - - - - break; - } - case CMD_BULLET_DATA_STREAM_RECEIVED_FAILED: - { - if (m_data->m_verboseOutput) - { - b3Printf("Server failed receiving bullet data stream\n"); - } - - break; - } - - - case CMD_ACTUAL_STATE_UPDATE_COMPLETED: - { - if (m_data->m_verboseOutput) - { - b3Printf("Received actual state\n"); - } - SharedMemoryStatus& command = m_data->m_testBlock1->m_serverCommands[0]; - - int numQ = command.m_sendActualStateArgs.m_numDegreeOfFreedomQ; - int numU = command.m_sendActualStateArgs.m_numDegreeOfFreedomU; - if (m_data->m_verboseOutput) - { - b3Printf("size Q = %d, size U = %d\n", numQ,numU); - } - char msg[1024]; - - { - sprintf(msg,"Q=["); - - for (int i=0;im_verboseOutput) - { - b3Printf(msg); - } - - { - sprintf(msg,"U=["); - - for (int i=0;im_verboseOutput) - { - b3Printf(msg); - } - - - if (m_data->m_verboseOutput) - { - b3Printf("\n"); - } - break; - } - case CMD_RESET_SIMULATION_COMPLETED: - { - if (m_data->m_verboseOutput) - { - b3Printf("CMD_RESET_SIMULATION_COMPLETED clean data\n"); - } - for (int i=0;im_robotMultiBodyData.size();i++) - { - delete m_data->m_robotMultiBodyData[i]; - } - m_data->m_robotMultiBodyData.clear(); - - m_data->m_jointInfo.clear(); - break; - } - case CMD_DEBUG_LINES_COMPLETED: - { - if (m_data->m_verboseOutput) - { - b3Printf("Success receiving %d debug lines",serverCmd.m_sendDebugLinesArgs.m_numDebugLines); - } - - int numLines = serverCmd.m_sendDebugLinesArgs.m_numDebugLines; - float* linesFrom = (float*)&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]; - float* linesTo = (float*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]+numLines*3*sizeof(float)); - float* linesColor = (float*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]+2*numLines*3*sizeof(float)); - - m_data->m_debugLinesFrom.resize(serverCmd.m_sendDebugLinesArgs.m_startingLineIndex+numLines); - m_data->m_debugLinesTo.resize(serverCmd.m_sendDebugLinesArgs.m_startingLineIndex+numLines); - m_data->m_debugLinesColor.resize(serverCmd.m_sendDebugLinesArgs.m_startingLineIndex+numLines); - - for (int i=0;im_debugLinesFrom[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex+i] = from; - m_data->m_debugLinesTo[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex+i] = to; - m_data->m_debugLinesColor[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex+i] = color; - } - - break; - } - case CMD_DEBUG_LINES_OVERFLOW_FAILED: - { - b3Warning("Error receiving debug lines"); - m_data->m_debugLinesFrom.resize(0); - m_data->m_debugLinesTo.resize(0); - m_data->m_debugLinesColor.resize(0); - - break; - } - - default: - { - b3Error("Unknown server status\n"); - btAssert(0); - } - }; - - - m_data->m_testBlock1->m_numProcessedServerCommands++; - //we don't have more than 1 command outstanding (in total, either server or client) - btAssert(m_data->m_testBlock1->m_numProcessedServerCommands == m_data->m_testBlock1->m_numServerCommands); - - if (m_data->m_testBlock1->m_numServerCommands == m_data->m_testBlock1->m_numProcessedServerCommands) - { - m_data->m_waitingForServer = false; - } else - { - m_data->m_waitingForServer = true; - } - - if ((serverCmd.m_type == CMD_DEBUG_LINES_COMPLETED) && (serverCmd.m_sendDebugLinesArgs.m_numRemainingDebugLines>0)) - { - SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; - - //continue requesting debug lines for drawing - command.m_type =CMD_REQUEST_DEBUG_LINES; - command.m_requestDebugLinesArguments.m_startingLineIndex = serverCmd.m_sendDebugLinesArgs.m_numDebugLines+serverCmd.m_sendDebugLinesArgs.m_startingLineIndex; - submitClientCommand(command); - return 0; - } - - return &m_data->m_lastServerStatus; - - } else - { - if (m_data->m_verboseOutput) - { - b3Printf("m_numServerStatus = %d, processed = %d\n", m_data->m_testBlock1->m_numServerCommands, - m_data->m_testBlock1->m_numProcessedServerCommands); - } - } - return 0; -} - -bool PhysicsClientSharedMemory::canSubmitCommand() const -{ - return (m_data->m_isConnected && !m_data->m_waitingForServer); -} - -struct SharedMemoryCommand* PhysicsClientSharedMemory::getAvailableSharedMemoryCommand() -{ - return &m_data->m_testBlock1->m_clientCommands[0]; -} - -bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& command) -{ - ///at the moment we allow a maximum of 1 outstanding command, so we check for this - //once the server processed the command and returns a status, we clear the flag "m_data->m_waitingForServer" and allow submitting the next command - btAssert(!m_data->m_waitingForServer); - - if (!m_data->m_waitingForServer) - { - if (&m_data->m_testBlock1->m_clientCommands[0] != &command) - { - m_data->m_testBlock1->m_clientCommands[0] = command; - } - m_data->m_testBlock1->m_numClientCommands++; - m_data->m_waitingForServer = true; - return true; - } - return false; -} - -void PhysicsClientSharedMemory::uploadBulletFileToSharedMemory(const char* data, int len) -{ - btAssert(len=SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE) - { - b3Warning("uploadBulletFileToSharedMemory %d exceeds max size %d\n",len,SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); - } else - { - for (int i=0;im_testBlock1->m_bulletStreamDataClientToServer[i] = data[i]; - } - } -} - - -const float* PhysicsClientSharedMemory::getDebugLinesFrom() const -{ - if (m_data->m_debugLinesFrom.size()) - { - return &m_data->m_debugLinesFrom[0].m_x; - } - return 0; -} -const float* PhysicsClientSharedMemory::getDebugLinesTo() const -{ - if (m_data->m_debugLinesTo.size()) - { - return &m_data->m_debugLinesTo[0].m_x; - } - return 0; -} -const float* PhysicsClientSharedMemory::getDebugLinesColor() const -{ - if (m_data->m_debugLinesColor.size()) - { - return &m_data->m_debugLinesColor[0].m_x; - } - return 0; -} -int PhysicsClientSharedMemory::getNumDebugLines() const -{ - return m_data->m_debugLinesFrom.size(); -} +PhysicsClient::~PhysicsClient() {} diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h index 185a4d0e7..9090596b8 100644 --- a/examples/SharedMemory/PhysicsClient.h +++ b/examples/SharedMemory/PhysicsClient.h @@ -4,46 +4,39 @@ //#include "SharedMemoryCommands.h" #include "LinearMath/btVector3.h" -class PhysicsClientSharedMemory -{ - struct PhysicsClientSharedMemoryInternalData* m_data; -protected: - +class PhysicsClient { public: + virtual ~PhysicsClient(); - PhysicsClientSharedMemory(); - virtual ~PhysicsClientSharedMemory(); + // return true if connection succesfull, can also check 'isConnected' + virtual bool connect() = 0; - //return true if connection succesfull, can also check 'isConnected' - virtual bool connect(); - - virtual void disconnectSharedMemory (); + virtual void disconnectSharedMemory() = 0; - virtual bool isConnected() const; + virtual bool isConnected() const = 0; - // return non-null if there is a status, nullptr otherwise - virtual const struct SharedMemoryStatus* processServerStatus(); - - virtual struct SharedMemoryCommand* getAvailableSharedMemoryCommand(); - - virtual bool canSubmitCommand() const; - - virtual bool submitClientCommand(const struct SharedMemoryCommand& command); + // return non-null if there is a status, nullptr otherwise + virtual const struct SharedMemoryStatus* processServerStatus() = 0; - virtual int getNumJoints() const; - - virtual void getJointInfo(int index, struct b3JointInfo& info) const; - - virtual void setSharedMemoryKey(int key); + virtual struct SharedMemoryCommand* getAvailableSharedMemoryCommand() = 0; - virtual void uploadBulletFileToSharedMemory(const char* data, int len); + virtual bool canSubmitCommand() const = 0; - virtual int getNumDebugLines() const; + virtual bool submitClientCommand(const struct SharedMemoryCommand& command) = 0; - virtual const float* getDebugLinesFrom() const; - virtual const float* getDebugLinesTo() const; - virtual const float* getDebugLinesColor() const; + virtual int getNumJoints() const = 0; + virtual void getJointInfo(int index, struct b3JointInfo& info) const = 0; + + virtual void setSharedMemoryKey(int key) = 0; + + virtual void uploadBulletFileToSharedMemory(const char* data, int len) = 0; + + virtual int getNumDebugLines() const = 0; + + virtual const float* getDebugLinesFrom() const = 0; + virtual const float* getDebugLinesTo() const = 0; + virtual const float* getDebugLinesColor() const = 0; }; -#endif //BT_PHYSICS_CLIENT_API_H +#endif // BT_PHYSICS_CLIENT_API_H diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index d1fa6a5f0..62e75e042 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1,5 +1,5 @@ #include "PhysicsClientC_API.h" -#include "PhysicsClient.h" +#include "PhysicsClientSharedMemory.h" #include "Bullet3Common/b3Scalar.h" #include #include "SharedMemoryCommands.h" @@ -8,7 +8,7 @@ b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName) { - PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); b3Assert(cl->canSubmitCommand()); @@ -70,7 +70,7 @@ int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHand b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient) { - PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); b3Assert(cl->canSubmitCommand()); struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); @@ -101,7 +101,7 @@ int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient) { - PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); b3Assert(cl->canSubmitCommand()); struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); @@ -113,7 +113,7 @@ b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle ph b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHandle physClient) { - PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); b3Assert(cl->canSubmitCommand()); struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); @@ -128,7 +128,7 @@ b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHand b3SharedMemoryCommandHandle b3JointControlCommandInit( b3PhysicsClientHandle physClient, int controlMode) { - PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); b3Assert(cl->canSubmitCommand()); struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); @@ -191,7 +191,7 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient) { - PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); b3Assert(cl->canSubmitCommand()); struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); @@ -215,7 +215,7 @@ void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandl b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit(b3PhysicsClientHandle physClient) { - PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); b3Assert(cl->canSubmitCommand()); struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); @@ -270,7 +270,7 @@ int b3CreateBoxCommandSetHalfExtents(b3SharedMemoryCommandHandle commandHandle, b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient) { - PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); b3Assert(cl->canSubmitCommand()); struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); @@ -323,13 +323,13 @@ b3PhysicsClientHandle b3ConnectSharedMemory(int key) void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient) { - PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + PhysicsClient* cl = (PhysicsClient* ) physClient; delete cl; } b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClient) { - PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + PhysicsClient* cl = (PhysicsClient* ) physClient; const SharedMemoryStatus* stat = cl->processServerStatus(); return (b3SharedMemoryStatusHandle) stat; @@ -352,14 +352,14 @@ int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle) int b3CanSubmitCommand(b3PhysicsClientHandle physClient) { - PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + PhysicsClient* cl = (PhysicsClient* ) physClient; return (int)cl->canSubmitCommand(); } int b3SubmitClientCommand(b3PhysicsClientHandle physClient, const b3SharedMemoryCommandHandle commandHandle) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; - PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + PhysicsClient* cl = (PhysicsClient* ) physClient; return (int)cl->submitClientCommand(*command); } @@ -381,14 +381,14 @@ b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHan int b3GetNumJoints(b3PhysicsClientHandle physClient) { - PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + PhysicsClient* cl = (PhysicsClient* ) physClient; return cl->getNumJoints(); } void b3GetJointInfo(b3PhysicsClientHandle physClient, int linkIndex, struct b3JointInfo* info) { - PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + PhysicsClient* cl = (PhysicsClient* ) physClient; cl->getJointInfo(linkIndex,*info); } @@ -432,7 +432,7 @@ int b3RemovePickingConstraint(b3SharedMemoryCommandHandle commandHandle) b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode) { - PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); b3Assert(cl->canSubmitCommand()); struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); @@ -445,7 +445,7 @@ b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle } void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* lines) { - PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(lines); if (lines) diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 19ddc2832..7e6c3c771 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -89,7 +89,7 @@ int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle c int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, int linkIndex, int enable); b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient); -void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, b3JointSensorState *state); +void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, struct b3JointSensorState *state); int b3PickBody(struct SharedMemoryCommand *command, double rayFromWorldX, double rayFromWorldY, double rayFromWorldZ, diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp new file mode 100644 index 000000000..71bf35d4a --- /dev/null +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -0,0 +1,527 @@ +#include "PhysicsClientSharedMemory.h" +#include "PosixSharedMemory.h" +#include "Win32SharedMemory.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "LinearMath/btVector3.h" + +#include "Bullet3Common/b3Logging.h" +#include "../Utils/b3ResourcePath.h" +#include "../../Extras/Serialize/BulletFileLoader/btBulletFile.h" +#include "../../Extras/Serialize/BulletFileLoader/autogenerated/bullet.h" +#include "SharedMemoryBlock.h" + +// copied from btMultiBodyLink.h +enum JointType { + eRevoluteType = 0, + ePrismaticType = 1, +}; + +struct TmpFloat3 { + float m_x; + float m_y; + float m_z; +}; + +TmpFloat3 CreateTmpFloat3(float x, float y, float z) { + TmpFloat3 tmp; + tmp.m_x = x; + tmp.m_y = y; + tmp.m_z = z; + return tmp; +} + +struct PhysicsClientSharedMemoryInternalData { + SharedMemoryInterface* m_sharedMemory; + SharedMemoryBlock* m_testBlock1; + + btAlignedObjectArray m_robotMultiBodyData; + btAlignedObjectArray m_jointInfo; + btAlignedObjectArray m_debugLinesFrom; + btAlignedObjectArray m_debugLinesTo; + btAlignedObjectArray m_debugLinesColor; + + SharedMemoryStatus m_lastServerStatus; + + int m_counter; + bool m_serverLoadUrdfOK; + bool m_isConnected; + bool m_waitingForServer; + bool m_hasLastServerStatus; + int m_sharedMemoryKey; + bool m_verboseOutput; + + PhysicsClientSharedMemoryInternalData() + : m_sharedMemory(0), + m_testBlock1(0), + m_counter(0), + m_serverLoadUrdfOK(false), + m_isConnected(false), + m_waitingForServer(false), + m_hasLastServerStatus(false), + m_sharedMemoryKey(SHARED_MEMORY_KEY), + m_verboseOutput(false) {} + + void processServerStatus(); + + bool canSubmitCommand() const; +}; + +int PhysicsClientSharedMemory::getNumJoints() const { return m_data->m_jointInfo.size(); } + +void PhysicsClientSharedMemory::getJointInfo(int index, b3JointInfo& info) const { + info = m_data->m_jointInfo[index]; +} + +PhysicsClientSharedMemory::PhysicsClientSharedMemory() + +{ + m_data = new PhysicsClientSharedMemoryInternalData; + +#ifdef _WIN32 + m_data->m_sharedMemory = new Win32SharedMemoryClient(); +#else + m_data->m_sharedMemory = new PosixSharedMemory(); +#endif +} + +PhysicsClientSharedMemory::~PhysicsClientSharedMemory() { + if (m_data->m_isConnected) { + disconnectSharedMemory(); + } + delete m_data->m_sharedMemory; + delete m_data; +} + +void PhysicsClientSharedMemory::setSharedMemoryKey(int key) { m_data->m_sharedMemoryKey = key; } + +void PhysicsClientSharedMemory::disconnectSharedMemory() { + if (m_data->m_isConnected) { + m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE); + m_data->m_isConnected = false; + } +} + +bool PhysicsClientSharedMemory::isConnected() const { return m_data->m_isConnected; } + +bool PhysicsClientSharedMemory::connect() { + /// server always has to create and initialize shared memory + bool allowCreation = false; + m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory( + m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE, allowCreation); + + if (m_data->m_testBlock1) { + if (m_data->m_testBlock1->m_magicId != SHARED_MEMORY_MAGIC_NUMBER) { + b3Error("Error: please start server before client\n"); + m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, + SHARED_MEMORY_SIZE); + m_data->m_testBlock1 = 0; + return false; + } else { + if (m_data->m_verboseOutput) { + b3Printf("Connected to existing shared memory, status OK.\n"); + } + m_data->m_isConnected = true; + } + } else { + b3Error("Cannot connect to shared memory"); + return false; + } + return true; +} + +const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { + SharedMemoryStatus* stat = 0; + + if (!m_data->m_testBlock1) { + return 0; + } + + if (!m_data->m_waitingForServer) { + return 0; + } + + if (m_data->m_testBlock1->m_numServerCommands > + m_data->m_testBlock1->m_numProcessedServerCommands) { + btAssert(m_data->m_testBlock1->m_numServerCommands == + m_data->m_testBlock1->m_numProcessedServerCommands + 1); + + const SharedMemoryStatus& serverCmd = m_data->m_testBlock1->m_serverCommands[0]; + m_data->m_lastServerStatus = serverCmd; + + EnumSharedMemoryServerStatus s = (EnumSharedMemoryServerStatus)serverCmd.m_type; + // consume the command + switch (serverCmd.m_type) { + case CMD_CLIENT_COMMAND_COMPLETED: { + if (m_data->m_verboseOutput) { + b3Printf("Server completed command"); + } + break; + } + case CMD_URDF_LOADING_COMPLETED: { + m_data->m_serverLoadUrdfOK = true; + if (m_data->m_verboseOutput) { + b3Printf("Server loading the URDF OK\n"); + } + + if (serverCmd.m_dataStreamArguments.m_streamChunkLength > 0) { + bParse::btBulletFile* bf = new bParse::btBulletFile( + this->m_data->m_testBlock1->m_bulletStreamDataServerToClient, + serverCmd.m_dataStreamArguments.m_streamChunkLength); + bf->setFileDNAisMemoryDNA(); + bf->parse(false); + m_data->m_robotMultiBodyData.push_back(bf); + + for (int i = 0; i < bf->m_multiBodies.size(); i++) { + int flag = bf->getFlags(); + int qOffset = 7; + int uOffset = 6; + + if ((flag & bParse::FD_DOUBLE_PRECISION) != 0) { + Bullet::btMultiBodyDoubleData* mb = + (Bullet::btMultiBodyDoubleData*)bf->m_multiBodies[i]; + if (mb->m_baseName) { + if (m_data->m_verboseOutput) { + b3Printf("mb->m_baseName = %s\n", mb->m_baseName); + } + } + + for (int link = 0; link < mb->m_numLinks; link++) { + { + b3JointInfo info; + info.m_flags = 0; + info.m_jointIndex = link; + info.m_qIndex = + (0 < mb->m_links[link].m_posVarCount) ? qOffset : -1; + info.m_uIndex = + (0 < mb->m_links[link].m_dofCount) ? uOffset : -1; + + if (mb->m_links[link].m_linkName) { + if (m_data->m_verboseOutput) { + b3Printf("mb->m_links[%d].m_linkName = %s\n", link, + mb->m_links[link].m_linkName); + } + info.m_linkName = mb->m_links[link].m_linkName; + } + if (mb->m_links[link].m_jointName) { + if (m_data->m_verboseOutput) { + b3Printf("mb->m_links[%d].m_jointName = %s\n", link, + mb->m_links[link].m_jointName); + } + info.m_jointName = mb->m_links[link].m_jointName; + info.m_jointType = mb->m_links[link].m_jointType; + } + if ((mb->m_links[link].m_jointType == eRevoluteType) || + (mb->m_links[link].m_jointType == ePrismaticType)) { + info.m_flags |= JOINT_HAS_MOTORIZED_POWER; + } + m_data->m_jointInfo.push_back(info); + } + qOffset += mb->m_links[link].m_posVarCount; + uOffset += mb->m_links[link].m_dofCount; + } + + } else { + Bullet::btMultiBodyFloatData* mb = + (Bullet::btMultiBodyFloatData*)bf->m_multiBodies[i]; + if (mb->m_baseName) { + if (m_data->m_verboseOutput) { + b3Printf("mb->m_baseName = %s\n", mb->m_baseName); + } + } + for (int link = 0; link < mb->m_numLinks; link++) { + { + b3JointInfo info; + info.m_flags = 0; + info.m_jointIndex = link; + info.m_qIndex = + (0 < mb->m_links[link].m_posVarCount) ? qOffset : -1; + info.m_uIndex = + (0 < mb->m_links[link].m_dofCount) ? uOffset : -1; + + if (mb->m_links[link].m_linkName) { + if (m_data->m_verboseOutput) { + b3Printf("mb->m_links[%d].m_linkName = %s\n", link, + mb->m_links[link].m_linkName); + } + info.m_linkName = mb->m_links[link].m_linkName; + } + if (mb->m_links[link].m_jointName) { + if (m_data->m_verboseOutput) { + b3Printf("mb->m_links[%d].m_jointName = %s\n", link, + mb->m_links[link].m_jointName); + } + info.m_jointName = mb->m_links[link].m_jointName; + info.m_jointType = mb->m_links[link].m_jointType; + } + if ((mb->m_links[link].m_jointType == eRevoluteType) || + (mb->m_links[link].m_jointType == ePrismaticType)) { + info.m_flags |= JOINT_HAS_MOTORIZED_POWER; + } + m_data->m_jointInfo.push_back(info); + } + qOffset += mb->m_links[link].m_posVarCount; + uOffset += mb->m_links[link].m_dofCount; + } + } + } + if (bf->ok()) { + if (m_data->m_verboseOutput) { + b3Printf("Received robot description ok!\n"); + } + } else { + b3Warning("Robot description not received"); + } + } + break; + } + case CMD_DESIRED_STATE_RECEIVED_COMPLETED: { + if (m_data->m_verboseOutput) { + b3Printf("Server received desired state"); + } + break; + } + case CMD_STEP_FORWARD_SIMULATION_COMPLETED: { + if (m_data->m_verboseOutput) { + b3Printf("Server completed step simulation"); + } + break; + } + case CMD_URDF_LOADING_FAILED: { + if (m_data->m_verboseOutput) { + b3Printf("Server failed loading the URDF...\n"); + } + m_data->m_serverLoadUrdfOK = false; + break; + } + + case CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED: { + if (m_data->m_verboseOutput) { + b3Printf("Server received bullet data stream OK\n"); + } + + break; + } + case CMD_BULLET_DATA_STREAM_RECEIVED_FAILED: { + if (m_data->m_verboseOutput) { + b3Printf("Server failed receiving bullet data stream\n"); + } + + break; + } + + case CMD_ACTUAL_STATE_UPDATE_COMPLETED: { + if (m_data->m_verboseOutput) { + b3Printf("Received actual state\n"); + } + SharedMemoryStatus& command = m_data->m_testBlock1->m_serverCommands[0]; + + int numQ = command.m_sendActualStateArgs.m_numDegreeOfFreedomQ; + int numU = command.m_sendActualStateArgs.m_numDegreeOfFreedomU; + if (m_data->m_verboseOutput) { + b3Printf("size Q = %d, size U = %d\n", numQ, numU); + } + char msg[1024]; + + { + sprintf(msg, "Q=["); + + for (int i = 0; i < numQ; i++) { + if (i < numQ - 1) { + sprintf(msg, "%s%f,", msg, + command.m_sendActualStateArgs.m_actualStateQ[i]); + } else { + sprintf(msg, "%s%f", msg, + command.m_sendActualStateArgs.m_actualStateQ[i]); + } + } + sprintf(msg, "%s]", msg); + } + if (m_data->m_verboseOutput) { + b3Printf(msg); + } + + { + sprintf(msg, "U=["); + + for (int i = 0; i < numU; i++) { + if (i < numU - 1) { + sprintf(msg, "%s%f,", msg, + command.m_sendActualStateArgs.m_actualStateQdot[i]); + } else { + sprintf(msg, "%s%f", msg, + command.m_sendActualStateArgs.m_actualStateQdot[i]); + } + } + sprintf(msg, "%s]", msg); + } + if (m_data->m_verboseOutput) { + b3Printf(msg); + } + + if (m_data->m_verboseOutput) { + b3Printf("\n"); + } + break; + } + case CMD_RESET_SIMULATION_COMPLETED: { + if (m_data->m_verboseOutput) { + b3Printf("CMD_RESET_SIMULATION_COMPLETED clean data\n"); + } + for (int i = 0; i < m_data->m_robotMultiBodyData.size(); i++) { + delete m_data->m_robotMultiBodyData[i]; + } + m_data->m_robotMultiBodyData.clear(); + + m_data->m_jointInfo.clear(); + break; + } + case CMD_DEBUG_LINES_COMPLETED: { + if (m_data->m_verboseOutput) { + b3Printf("Success receiving %d debug lines", + serverCmd.m_sendDebugLinesArgs.m_numDebugLines); + } + + int numLines = serverCmd.m_sendDebugLinesArgs.m_numDebugLines; + float* linesFrom = + (float*)&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]; + float* linesTo = + (float*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0] + + numLines * 3 * sizeof(float)); + float* linesColor = + (float*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0] + + 2 * numLines * 3 * sizeof(float)); + + m_data->m_debugLinesFrom.resize(serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + + numLines); + m_data->m_debugLinesTo.resize(serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + + numLines); + m_data->m_debugLinesColor.resize( + serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + numLines); + + for (int i = 0; i < numLines; i++) { + TmpFloat3 from = CreateTmpFloat3(linesFrom[i * 3], linesFrom[i * 3 + 1], + linesFrom[i * 3 + 2]); + TmpFloat3 to = + CreateTmpFloat3(linesTo[i * 3], linesTo[i * 3 + 1], linesTo[i * 3 + 2]); + TmpFloat3 color = CreateTmpFloat3(linesColor[i * 3], linesColor[i * 3 + 1], + linesColor[i * 3 + 2]); + + m_data + ->m_debugLinesFrom[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + i] = + from; + m_data->m_debugLinesTo[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + i] = + to; + m_data->m_debugLinesColor[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + + i] = color; + } + + break; + } + case CMD_DEBUG_LINES_OVERFLOW_FAILED: { + b3Warning("Error receiving debug lines"); + m_data->m_debugLinesFrom.resize(0); + m_data->m_debugLinesTo.resize(0); + m_data->m_debugLinesColor.resize(0); + + break; + } + + default: { + b3Error("Unknown server status\n"); + btAssert(0); + } + }; + + m_data->m_testBlock1->m_numProcessedServerCommands++; + // we don't have more than 1 command outstanding (in total, either server or client) + btAssert(m_data->m_testBlock1->m_numProcessedServerCommands == + m_data->m_testBlock1->m_numServerCommands); + + if (m_data->m_testBlock1->m_numServerCommands == + m_data->m_testBlock1->m_numProcessedServerCommands) { + m_data->m_waitingForServer = false; + } else { + m_data->m_waitingForServer = true; + } + + if ((serverCmd.m_type == CMD_DEBUG_LINES_COMPLETED) && + (serverCmd.m_sendDebugLinesArgs.m_numRemainingDebugLines > 0)) { + SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; + + // continue requesting debug lines for drawing + command.m_type = CMD_REQUEST_DEBUG_LINES; + command.m_requestDebugLinesArguments.m_startingLineIndex = + serverCmd.m_sendDebugLinesArgs.m_numDebugLines + + serverCmd.m_sendDebugLinesArgs.m_startingLineIndex; + submitClientCommand(command); + return 0; + } + + return &m_data->m_lastServerStatus; + + } else { + if (m_data->m_verboseOutput) { + b3Printf("m_numServerStatus = %d, processed = %d\n", + m_data->m_testBlock1->m_numServerCommands, + m_data->m_testBlock1->m_numProcessedServerCommands); + } + } + return 0; +} + +bool PhysicsClientSharedMemory::canSubmitCommand() const { + return (m_data->m_isConnected && !m_data->m_waitingForServer); +} + +struct SharedMemoryCommand* PhysicsClientSharedMemory::getAvailableSharedMemoryCommand() { + return &m_data->m_testBlock1->m_clientCommands[0]; +} + +bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& command) { + /// at the moment we allow a maximum of 1 outstanding command, so we check for this + // once the server processed the command and returns a status, we clear the flag + // "m_data->m_waitingForServer" and allow submitting the next command + btAssert(!m_data->m_waitingForServer); + + if (!m_data->m_waitingForServer) { + if (&m_data->m_testBlock1->m_clientCommands[0] != &command) { + m_data->m_testBlock1->m_clientCommands[0] = command; + } + m_data->m_testBlock1->m_numClientCommands++; + m_data->m_waitingForServer = true; + return true; + } + return false; +} + +void PhysicsClientSharedMemory::uploadBulletFileToSharedMemory(const char* data, int len) { + btAssert(len < SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + if (len >= SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE) { + b3Warning("uploadBulletFileToSharedMemory %d exceeds max size %d\n", len, + SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + } else { + for (int i = 0; i < len; i++) { + m_data->m_testBlock1->m_bulletStreamDataClientToServer[i] = data[i]; + } + } +} + +const float* PhysicsClientSharedMemory::getDebugLinesFrom() const { + if (m_data->m_debugLinesFrom.size()) { + return &m_data->m_debugLinesFrom[0].m_x; + } + return 0; +} +const float* PhysicsClientSharedMemory::getDebugLinesTo() const { + if (m_data->m_debugLinesTo.size()) { + return &m_data->m_debugLinesTo[0].m_x; + } + return 0; +} +const float* PhysicsClientSharedMemory::getDebugLinesColor() const { + if (m_data->m_debugLinesColor.size()) { + return &m_data->m_debugLinesColor[0].m_x; + } + return 0; +} +int PhysicsClientSharedMemory::getNumDebugLines() const { return m_data->m_debugLinesFrom.size(); } diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.h b/examples/SharedMemory/PhysicsClientSharedMemory.h new file mode 100644 index 000000000..fb5e10181 --- /dev/null +++ b/examples/SharedMemory/PhysicsClientSharedMemory.h @@ -0,0 +1,48 @@ +#ifndef BT_PHYSICS_CLIENT_SHARED_MEMORY_API_H +#define BT_PHYSICS_CLIENT_SHARED_MEMORY_API_H + +#include "PhysicsClient.h" + +//#include "SharedMemoryCommands.h" +#include "LinearMath/btVector3.h" + +class PhysicsClientSharedMemory : public PhysicsClient { + struct PhysicsClientSharedMemoryInternalData* m_data; + +protected: +public: + PhysicsClientSharedMemory(); + virtual ~PhysicsClientSharedMemory(); + + // return true if connection succesfull, can also check 'isConnected' + virtual bool connect(); + + virtual void disconnectSharedMemory(); + + virtual bool isConnected() const; + + // return non-null if there is a status, nullptr otherwise + virtual const struct SharedMemoryStatus* processServerStatus(); + + virtual struct SharedMemoryCommand* getAvailableSharedMemoryCommand(); + + virtual bool canSubmitCommand() const; + + virtual bool submitClientCommand(const struct SharedMemoryCommand& command); + + virtual int getNumJoints() const; + + virtual void getJointInfo(int index, struct b3JointInfo& info) const; + + virtual void setSharedMemoryKey(int key); + + virtual void uploadBulletFileToSharedMemory(const char* data, int len); + + virtual int getNumDebugLines() const; + + virtual const float* getDebugLinesFrom() const; + virtual const float* getDebugLinesTo() const; + virtual const float* getDebugLinesColor() const; +}; + +#endif // BT_PHYSICS_CLIENT_API_H diff --git a/examples/SharedMemory/premake4.lua b/examples/SharedMemory/premake4.lua index 63a5d0e90..c93a4250a 100644 --- a/examples/SharedMemory/premake4.lua +++ b/examples/SharedMemory/premake4.lua @@ -17,6 +17,7 @@ language "C++" files { "PhysicsClient.cpp", + "PhysicsClientSharedMemory.cpp", "PhysicsClientExample.cpp", "PhysicsServerExample.cpp", "main.cpp", diff --git a/test/SharedMemory/premake4.lua b/test/SharedMemory/premake4.lua index de781f51e..aa71e7180 100644 --- a/test/SharedMemory/premake4.lua +++ b/test/SharedMemory/premake4.lua @@ -14,6 +14,8 @@ "test.c", "../../examples/SharedMemory/PhysicsClient.cpp", "../../examples/SharedMemory/PhysicsClient.h", + "../../examples/SharedMemory/PhysicsClientSharedMemory.cpp", + "../../examples/SharedMemory/PhysicsClientSharedMemory.h", "../../examples/SharedMemory/PhysicsClientC_API.cpp", "../../examples/SharedMemory/PhysicsClientC_API.h", "../../examples/SharedMemory/Win32SharedMemory.cpp",