diff --git a/build3/premake4.lua b/build3/premake4.lua index 345091d3e..9d4efd5ab 100644 --- a/build3/premake4.lua +++ b/build3/premake4.lua @@ -71,6 +71,12 @@ description = "Don't build Extras" } + newoption + { + trigger = "standalone-examples", + description = "Build standalone examples with reduced dependencies." + } + newoption { trigger = "no-clsocket", @@ -249,17 +255,9 @@ end include "../examples/ExampleBrowser" include "../examples/OpenGLWindow" include "../examples/ThirdPartyLibs/Gwen" - include "../examples/SimpleOpenGL3" - include "../examples/TinyRenderer" - include "../examples/HelloWorld" - include "../examples/BasicDemo" - include "../examples/InverseDynamics" - include "../examples/ExtendedTutorials" include "../examples/SharedMemory" include "../examples/ThirdPartyLibs/BussIK" - - include "../examples/MultiThreading" if _OPTIONS["lua"] then include "../examples/ThirdPartyLibs/lua-5.2.3" @@ -268,9 +266,17 @@ end include "../examples/pybullet" end + if _OPTIONS["standalone-examples"] then + include "../examples/SimpleOpenGL3" + include "../examples/TinyRenderer" + include "../examples/BasicDemo" + include "../examples/InverseDynamics" + include "../examples/ExtendedTutorials" + include "../examples/MultiThreading" + end + if not _OPTIONS["no-test"] then include "../test/SharedMemory" - end end diff --git a/build_and_run_cmake.sh b/build_and_run_cmake.sh deleted file mode 100755 index 13cb76c62..000000000 --- a/build_and_run_cmake.sh +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -rm CMakeCache.txt -mkdir build_cmake -cd build_cmake -cmake -DBUILD_PYBULLET=OFF -DCMAKE_BUILD_TYPE=Release .. -make -j12 -examples/ExampleBrowser/App_ExampleBrowser diff --git a/build_and_run_premake.sh b/build_and_run_premake.sh deleted file mode 100755 index 3d4b2cfc6..000000000 --- a/build_and_run_premake.sh +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -cd build3 -./premake4_linux64 gmake -./premake4_osx gmake -cd gmake -make -j12 --double --enable_pybullet -../../bin/App_BulletExampleBrowser_gmake_x64_release diff --git a/build_and_run_cmake_pybullet_double.sh b/build_cmake_pybullet_double.sh similarity index 78% rename from build_and_run_cmake_pybullet_double.sh rename to build_cmake_pybullet_double.sh index d1520da70..0bddae1a5 100755 --- a/build_and_run_cmake_pybullet_double.sh +++ b/build_cmake_pybullet_double.sh @@ -2,7 +2,7 @@ rm CMakeCache.txt mkdir build_cmake cd build_cmake -cmake -DBUILD_PYBULLET=ON -DBUILD_PYBULLET_NUMPY=OFF -DUSE_DOUBLE_PRECISION=ON -DCMAKE_BUILD_TYPE=Release -G Xcode .. +cmake -DBUILD_PYBULLET=ON -DBUILD_PYBULLET_NUMPY=OFF -DUSE_DOUBLE_PRECISION=ON -DCMAKE_BUILD_TYPE=Release .. make -j12 cd examples cd pybullet diff --git a/build_cmake_pybullet_win32.bat b/build_cmake_pybullet_win32.bat deleted file mode 100644 index 67dec97b8..000000000 --- a/build_cmake_pybullet_win32.bat +++ /dev/null @@ -1,4 +0,0 @@ -mkdir cm -cd cm -cmake -DBUILD_PYBULLET=ON -DCMAKE_BUILD_TYPE=Release -DUSE_DOUBLE_PRECISION=ON -DPYTHON_INCLUDE_DIR=c:\python-3.5.2\include -DPYTHON_LIBRARY=c:\python-3.5.2\libs\python35.lib -DPYTHON_DEBUG_LIBRARY=c:\python-3.5.2\libs\python35_d.lib .. -start . diff --git a/build_visual_studio.bat b/build_visual_studio.bat deleted file mode 100644 index d8a85bc44..000000000 --- a/build_visual_studio.bat +++ /dev/null @@ -1,5 +0,0 @@ - - -cd build3 -premake4 --targetdir="../bin" vs2010 -start vs2010 diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index ffcc2e14b..1a72658c0 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -133,6 +133,17 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(1,"Inverted Pendulum PD","Keep an inverted pendulum up using open loop PD control", InvertedPendulumPDControlCreateFunc), ExampleEntry(1,"MultiBody Soft Contact", "Using the error correction parameter (ERP) and constraint force mixing (CFM) values for contacts to simulate compliant contact.",MultiBodySoftContactCreateFunc,0), + ExampleEntry(0,"Physics Client-Server"), + ExampleEntry(1,"Physics Server", "Create a physics server that communicates with a physics client over shared memory. You can connect to the server using pybullet, a PhysicsClient or a UDP/TCP Bridge.", + PhysicsServerCreateFunc), + ExampleEntry(1, "Physics Client (Shared Mem)", "Create a physics client that can communicate with a physics server over shared memory.", PhysicsClientCreateFunc), + +// ExampleEntry(1,"Physics Server (Logging)", "Create a physics server that communicates with a physics client over shared memory. It will log all commands to a file.", +// PhysicsServerCreateFunc,PHYSICS_SERVER_ENABLE_COMMAND_LOGGING), +// ExampleEntry(1,"Physics Server (Replay Log)", "Create a physics server that replay a command log from disk.", +// PhysicsServerCreateFunc,PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG), +// +// ExampleEntry(1, "Physics Client (Direct)", "Create a physics client that can communicate with a physics server directly in-process.", PhysicsClientCreateFunc,eCLIENTEXAMPLE_DIRECT), ExampleEntry(0,"Inverse Dynamics"), ExampleEntry(1,"Inverse Dynamics URDF", "Create a btMultiBody from URDF. Create an inverse MultiBodyTree model from that. Use either decoupled PD control or computed torque control using the inverse model to track joint position targets", InverseDynamicsExampleCreateFunc,BT_ID_LOAD_URDF), @@ -251,19 +262,10 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(0,"Experiments"), + + ExampleEntry(1,"Robot Control", "Create a physics client and server to create and control robots.", PhysicsClientCreateFunc, eCLIENTEXAMPLE_SERVER), - ExampleEntry(1,"Physics Server", "Create a physics server that communicates with a physics client over shared memory", - PhysicsServerCreateFunc), - ExampleEntry(1,"Physics Server (RTC)", "Create a physics server that communicates with a physics client over shared memory. At each update, the Physics Server will continue calling 'stepSimulation' based on the real-time clock (RTC).", - PhysicsServerCreateFunc,PHYSICS_SERVER_USE_RTC_CLOCK), - - ExampleEntry(1,"Physics Server (Logging)", "Create a physics server that communicates with a physics client over shared memory. It will log all commands to a file.", - PhysicsServerCreateFunc,PHYSICS_SERVER_ENABLE_COMMAND_LOGGING), - ExampleEntry(1,"Physics Server (Replay Log)", "Create a physics server that replay a command log from disk.", - PhysicsServerCreateFunc,PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG), - ExampleEntry(1, "Physics Client (Shared Mem)", "Create a physics client that can communicate with a physics server over shared memory.", PhysicsClientCreateFunc), - ExampleEntry(1, "Physics Client (Direct)", "Create a physics client that can communicate with a physics server directly in-process.", PhysicsClientCreateFunc,eCLIENTEXAMPLE_DIRECT), ExampleEntry(1,"R2D2 Grasp","Load the R2D2 robot from URDF file and control it to grasp objects", R2D2GraspExampleCreateFunc, eROBOTIC_LEARN_GRASP), ExampleEntry(1,"Kuka IK","Control a Kuka IIWA robot to follow a target using IK. This IK is not setup properly yet.", KukaGraspExampleCreateFunc,0), diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index b89d618f5..58f3c7c97 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1033,9 +1033,15 @@ int b3SubmitClientCommand(b3PhysicsClientHandle physClient, const b3SharedMemory } +#include "../Utils/b3Clock.h" + + b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHandle physClient, const b3SharedMemoryCommandHandle commandHandle) { - int timeout = 1024 * 1024 * 1024; + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = 10; + b3SharedMemoryStatusHandle statusHandle = 0; b3Assert(commandHandle); b3Assert(physClient); @@ -1043,7 +1049,7 @@ b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHan { b3SubmitClientCommand(physClient, commandHandle); - while ((statusHandle == 0) && (timeout-- > 0)) + while ((statusHandle == 0) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) { statusHandle = b3ProcessServerStatus(physClient); } diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index b9b187b92..c1b82b740 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -268,11 +268,13 @@ bool PhysicsClientSharedMemory::connect() { command.m_type = CMD_REQUEST_BODY_INFO; command.m_sdfRequestInfoArgs.m_bodyUniqueId = 37; submitClientCommand(command); - int timeout = 1024 * 1024 * 1024; - + + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = 10; + const SharedMemoryStatus* status = 0; - while ((status == 0) && (timeout-- > 0)) + while ((status == 0) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) { status = processServerStatus(); diff --git a/examples/SharedMemory/PhysicsClientTCP.cpp b/examples/SharedMemory/PhysicsClientTCP.cpp index 9ad9fdab9..4961dad01 100644 --- a/examples/SharedMemory/PhysicsClientTCP.cpp +++ b/examples/SharedMemory/PhysicsClientTCP.cpp @@ -64,7 +64,14 @@ struct TcpNetworkedInternalData m_tcpSocket.Initialize(); m_isConnected = m_tcpSocket.Open(m_hostName.c_str(),m_port); - + if (m_isConnected) + { + m_tcpSocket.SetSendTimeout(5,0); + m_tcpSocket.SetReceiveTimeout(5,0); + } + int key = SHARED_MEMORY_MAGIC_NUMBER; + m_tcpSocket.Send((uint8*)&key,4); + return m_isConnected; } @@ -243,6 +250,8 @@ bool TcpNetworkedPhysicsProcessor::connect() void TcpNetworkedPhysicsProcessor::disconnect() { + const char msg[16]="disconnect"; + m_data->m_tcpSocket.Send((const uint8 *)msg,10); m_data->m_tcpSocket.Close(); m_data->m_isConnected = false; } diff --git a/examples/SharedMemory/PhysicsClientUDP.cpp b/examples/SharedMemory/PhysicsClientUDP.cpp index a578dd81f..4da08895a 100644 --- a/examples/SharedMemory/PhysicsClientUDP.cpp +++ b/examples/SharedMemory/PhysicsClientUDP.cpp @@ -464,26 +464,32 @@ bool UdpNetworkedPhysicsProcessor::processCommand(const struct SharedMemoryComma printf("PhysicsClientUDP::processCommand\n"); } // int sz = sizeof(SharedMemoryCommand); - int timeout = 1024 * 1024 * 1024; + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = 10; m_data->m_cs->lock(); m_data->m_clientCmd = clientCmd; m_data->m_hasCommand = true; m_data->m_cs->unlock(); - while (m_data->m_hasCommand && (timeout-- > 0)) + while ((m_data->m_hasCommand) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) { b3Clock::usleep(0); } #if 0 - timeout = 1024 * 1024 * 1024; bool hasStatus = false; + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = 10; + const SharedMemoryStatus* stat = 0; - while ((!hasStatus) && (timeout-- > 0)) + while ((!hasStatus) && (clock.getTimeInSeconds() - startTime < timeOutInSeconds)) { hasStatus = receiveStatus(serverStatusOut, bufferServerToClient, bufferSizeInBytes); b3Clock::usleep(100); diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index bd54f02e6..892f6682b 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -4,7 +4,7 @@ #include "../CommonInterfaces/CommonGUIHelperInterface.h" #include "SharedMemoryCommands.h" #include "PhysicsCommandProcessorInterface.h" - +#include "../Utils/b3Clock.h" #include "LinearMath/btHashMap.h" #include "LinearMath/btAlignedObjectArray.h" @@ -137,8 +137,10 @@ bool PhysicsDirect::connect() } else { - int timeout = 1024 * 1024 * 1024; - while ((!hasStatus) && (timeout-- > 0)) + b3Clock clock; + double timeSec = clock.getTimeInSeconds(); + + while ((!hasStatus) && (clock.getTimeInSeconds()-timeSec <10 )) { const SharedMemoryStatus* stat = processServerStatus(); if (stat) @@ -226,8 +228,11 @@ bool PhysicsDirect::processDebugLines(const struct SharedMemoryCommand& orgComma bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); - int timeout = 1024 * 1024 * 1024; - while ((!hasStatus) && (timeout-- > 0)) + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = 10; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) { const SharedMemoryStatus* stat = processServerStatus(); if (stat) @@ -308,8 +313,11 @@ bool PhysicsDirect::processVisualShapeData(const struct SharedMemoryCommand& org { bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); - int timeout = 1024 * 1024 * 1024; - while ((!hasStatus) && (timeout-- > 0)) + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = 10; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) { const SharedMemoryStatus* stat = processServerStatus(); if (stat) @@ -359,8 +367,11 @@ bool PhysicsDirect::processOverlappingObjects(const struct SharedMemoryCommand& { bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); - int timeout = 1024 * 1024 * 1024; - while ((!hasStatus) && (timeout-- > 0)) + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = 10; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) { const SharedMemoryStatus* stat = processServerStatus(); if (stat) @@ -414,8 +425,11 @@ bool PhysicsDirect::processContactPointData(const struct SharedMemoryCommand& or { bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); - int timeout = 1024 * 1024 * 1024; - while ((!hasStatus) && (timeout-- > 0)) + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = 10; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) { const SharedMemoryStatus* stat = processServerStatus(); if (stat) @@ -475,8 +489,11 @@ bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand) bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); - int timeout = 1024 * 1024 * 1024; - while ((!hasStatus) && (timeout-- > 0)) + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = 10; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) { const SharedMemoryStatus* stat = processServerStatus(); if (stat) @@ -735,8 +752,11 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd bool hasStatus = m_data->m_commandProcessor->processCommand(infoRequestCommand, infoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); - int timeout = 1024 * 1024 * 1024; - while ((!hasStatus) && (timeout-- > 0)) + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = 10; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) { hasStatus = m_data->m_commandProcessor->receiveStatus(infoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); } @@ -759,8 +779,11 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd bool hasStatus = m_data->m_commandProcessor->processCommand(infoRequestCommand, infoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); - int timeout = 1024 * 1024 * 1024; - while ((!hasStatus) && (timeout-- > 0)) + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = 10; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) { hasStatus = m_data->m_commandProcessor->receiveStatus(infoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); } diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index cb05ce921..f84cc8df1 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -1153,6 +1153,12 @@ public: gCreateDefaultRobotAssets = true; } + if (args.CheckCmdLineFlag("realtimesimulation")) + { + //gEnableRealTimeSimVR = true; + m_physicsServer.enableRealTimeSimulation(true); + } + if (args.CheckCmdLineFlag("norobotassets")) { gCreateDefaultRobotAssets = false; diff --git a/examples/SharedMemory/SharedMemoryBlock.h b/examples/SharedMemory/SharedMemoryBlock.h index f266db496..f00a907a1 100644 --- a/examples/SharedMemory/SharedMemoryBlock.h +++ b/examples/SharedMemory/SharedMemoryBlock.h @@ -1,9 +1,9 @@ #ifndef SHARED_MEMORY_BLOCK_H #define SHARED_MEMORY_BLOCK_H -#define SHARED_MEMORY_MAGIC_NUMBER 64738 #define SHARED_MEMORY_MAX_COMMANDS 4 + #include "SharedMemoryCommands.h" struct SharedMemoryBlock diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 7fa209856..131c4049a 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -2,6 +2,9 @@ #define SHARED_MEMORY_PUBLIC_H #define SHARED_MEMORY_KEY 12347 +///increase the SHARED_MEMORY_MAGIC_NUMBER whenever incompatible changes are made in the structures +///my convention is year/month/day/rev +#define SHARED_MEMORY_MAGIC_NUMBER 201702220 enum EnumSharedMemoryClientCommand { diff --git a/examples/SharedMemory/tcp/main.cpp b/examples/SharedMemory/tcp/main.cpp index 3eb0b3c02..6c2df155d 100644 --- a/examples/SharedMemory/tcp/main.cpp +++ b/examples/SharedMemory/tcp/main.cpp @@ -44,11 +44,8 @@ int main(int argc, char *argv[]) sm->setGuiHelper(&guiHelper); int port = 6667; - if (parseArgs.GetCmdLineArgument("port",port)) - { - printf("Using TCP port %d\n", port); - } - + parseArgs.GetCmdLineArgument("port",port); + gVerboseNetworkMessagesServer = parseArgs.CheckCmdLineFlag("verbose"); #ifndef NO_SHARED_MEMORY @@ -64,7 +61,9 @@ int main(int argc, char *argv[]) if (isPhysicsClientConnected) { - + + printf("Starting TCP server using port %d\n", port); + CPassiveSocket socket; CActiveSocket *pClient = NULL; @@ -74,7 +73,7 @@ int main(int argc, char *argv[]) socket.Initialize(); socket.Listen("localhost", port); - socket.SetBlocking(); + //socket.SetBlocking(); int curNumErr = 0; @@ -89,7 +88,20 @@ int main(int argc, char *argv[]) int clientPort = socket.GetClientPort(); printf("connected from %s:%d\n", socket.GetClientAddr(),clientPort); - + + if (pClient->Receive(4)) + { + int clientKey = *(int*)pClient->GetData(); + + if (clientKey==SHARED_MEMORY_MAGIC_NUMBER) + { + printf("Client version OK %d\n", clientKey); + } else + { + printf("Server version (%d) mismatches Client Version (%d)\n", SHARED_MEMORY_MAGIC_NUMBER,clientKey); + continue; + } + } //---------------------------------------------------------------------- // Receive request from the client. @@ -102,25 +114,27 @@ int main(int argc, char *argv[]) int maxLen = 4 + sizeof(SharedMemoryStatus)+SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE; - //heuristic to detect disconnected clients - CSimpleSocket::CSocketError err = pClient->GetSocketError(); - if (err != CSimpleSocket::SocketSuccess) - { - b3Clock::usleep(100); - - curNumErr++; - - if (curNumErr>100) - { - printf("TCP Connection error = %d, curNumErr = %d\n", (int)err, curNumErr); - - break; - } - } if (pClient->Receive(maxLen)) { + + //heuristic to detect disconnected clients + CSimpleSocket::CSocketError err = pClient->GetSocketError(); + if (err != CSimpleSocket::SocketSuccess || !pClient->IsSocketValid()) + { + b3Clock::usleep(100); + + curNumErr++; + + if (curNumErr>100) + { + printf("TCP Connection error = %d, curNumErr = %d\n", (int)err, curNumErr); + + break; + } + } + curNumErr = 0; char* msg2 = (char*) pClient->GetData(); int numBytesRec2 = pClient->GetBytesReceived(); @@ -132,9 +146,26 @@ int main(int argc, char *argv[]) bytesReceived[curSize+i] = msg2[i]; } - if (bytesReceived.size() == 4 || bytesReceived.size()==sizeof(SharedMemoryCommand)) + if (bytesReceived.size() >= 4) { int numBytesRec = bytesReceived.size(); + if (numBytesRec>=10) + { + if (strncmp(&bytesReceived[0],"disconnect",10)==0) + { + printf("Disconnect request received\n"); + bytesReceived.clear(); + break; + } + + if (strncmp(&bytesReceived[0],"terminateserver",10)==0) + { + printf("Terminate server request received\n"); + exitRequested = true; + bytesReceived.clear(); + break; + } + } if (gVerboseNetworkMessagesServer) { @@ -143,15 +174,6 @@ int main(int argc, char *argv[]) receivedData = true; - if (strncmp(&bytesReceived[0],"stop",4)==0) - { - printf("Stop request received\n"); - exitRequested = true; - bytesReceived.clear(); - break; - } - - SharedMemoryCommand cmd; SharedMemoryCommand* cmdPtr = 0; @@ -233,12 +255,15 @@ int main(int argc, char *argv[]) pClient->Send( &packetData[0], packetData.size() ); } } + + bytesReceived.clear(); + } else { - printf("received packet with unknown contents\n"); + //likely an incomplete packet, let's append more bytes + //printf("received packet with unknown contents\n"); } - bytesReceived.clear(); } } @@ -256,7 +281,10 @@ int main(int argc, char *argv[]) socket.Close(); socket.Shutdown(CSimpleSocket::Both); - } + } else + { + printf("Error: cannot connect to shared memory physics server."); + } delete sm; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 6cd50d129..206273f58 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -377,10 +377,11 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); #if 0 - if (statusType != CMD_BODY_INFO_COMPLETED) { - PyErr_SetString(SpamError, "b3InitSyncBodyInfoCommand failed."); - return NULL; - } + if (statusType != CMD_BODY_INFO_COMPLETED) + { + PyErr_SetString(SpamError, "b3InitSyncBodyInfoCommand failed."); + return NULL; + } #endif } diff --git a/test/SharedMemory/CMakeLists.txt b/test/SharedMemory/CMakeLists.txt index 25161a4e8..6ae0aea3e 100644 --- a/test/SharedMemory/CMakeLists.txt +++ b/test/SharedMemory/CMakeLists.txt @@ -56,17 +56,19 @@ ENDIF() ../../examples/SharedMemory/PosixSharedMemory.h ../../examples/Utils/b3ResourcePath.cpp ../../examples/Utils/b3ResourcePath.h - ../../examples/Utils/RobotLoggingUtil.cpp + ../../examples/Utils/b3Clock.cpp + ../../examples/Utils/b3Clock.h + ../../examples/Utils/RobotLoggingUtil.cpp ../../examples/Utils/RobotLoggingUtil.h - ../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp - ../../examples/SharedMemory/TinyRendererVisualShapeConverter.h - ../../examples/OpenGLWindow/SimpleCamera.cpp - ../../examples/OpenGLWindow/SimpleCamera.h - ../../examples/TinyRenderer/geometry.cpp - ../../examples/TinyRenderer/model.cpp - ../../examples/TinyRenderer/tgaimage.cpp - ../../examples/TinyRenderer/our_gl.cpp - ../../examples/TinyRenderer/TinyRenderer.cpp + ../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp + ../../examples/SharedMemory/TinyRendererVisualShapeConverter.h + ../../examples/OpenGLWindow/SimpleCamera.cpp + ../../examples/OpenGLWindow/SimpleCamera.h + ../../examples/TinyRenderer/geometry.cpp + ../../examples/TinyRenderer/model.cpp + ../../examples/TinyRenderer/tgaimage.cpp + ../../examples/TinyRenderer/our_gl.cpp + ../../examples/TinyRenderer/TinyRenderer.cpp ../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp ../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp ../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp diff --git a/test/SharedMemory/premake4.lua b/test/SharedMemory/premake4.lua index cfe86780e..a61f7291c 100644 --- a/test/SharedMemory/premake4.lua +++ b/test/SharedMemory/premake4.lua @@ -25,6 +25,8 @@ project ("Test_SharedMemoryPhysicsClient") "../../examples/SharedMemory/Win32SharedMemory.h", "../../examples/SharedMemory/PosixSharedMemory.cpp", "../../examples/SharedMemory/PosixSharedMemory.h", + "../../examples/Utils/b3Clock.cpp", + "../../examples/Utils/b3Clock.h", "../../examples/Utils/b3ResourcePath.cpp", "../../examples/Utils/b3ResourcePath.h", } @@ -49,6 +51,9 @@ project ("Test_PhysicsClientUDP") defines { "WIN32" } links {"Ws2_32","Winmm"} end + if os.is("Linux") then + links {"pthread"} + end defines {"PHYSICS_UDP"} @@ -137,9 +142,6 @@ project ("Test_PhysicsClientTCP") "../../examples/Utils/b3ResourcePath.h", "../../examples/SharedMemory/PhysicsDirect.cpp", "../../examples/Utils/b3Clock.cpp", - "../../examples/MultiThreading/b3PosixThreadSupport.cpp", - "../../examples/MultiThreading/b3Win32ThreadSupport.cpp", - "../../examples/MultiThreading/b3ThreadSupportInterface.cpp", } @@ -200,6 +202,8 @@ project ("Test_PhysicsServerLoopBack") "../../examples/Utils/b3ResourcePath.h", "../../examples/Utils/RobotLoggingUtil.cpp", "../../examples/Utils/RobotLoggingUtil.h", + "../../examples/Utils/b3Clock.cpp", + "../../examples/Utils/b3Clock.h", "../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp", "../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp", "../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp", @@ -275,7 +279,9 @@ project ("Test_PhysicsServerLoopBack") "../../examples/Utils/b3ResourcePath.cpp", "../../examples/Utils/b3ResourcePath.h", "../../examples/Utils/RobotLoggingUtil.cpp", - "../../examples/Utils/RobotLoggingUtil.h", + "../../examples/Utils/RobotLoggingUtil.h", + "../../examples/Utils/b3Clock.cpp", + "../../examples/Utils/b3Clock.h", "../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp", "../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp", "../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",