From 46fae61c69736fbed8e2c1a006dcf356721ff0b7 Mon Sep 17 00:00:00 2001 From: = <=> Date: Thu, 6 Aug 2015 11:59:31 -0700 Subject: [PATCH] fixes in shared memory: only allow server to create and initialize shared memory, client will report failure intercept signals to cleanup shared memory in standalone app, thanks to Roland Philippsen. --- examples/ExampleBrowser/main.cpp | 3 + .../RollingFrictionDemo.cpp | 4 + examples/SharedMemory/PhysicsClient.cpp | 37 +++--- examples/SharedMemory/PhysicsClient.h | 4 +- examples/SharedMemory/PhysicsClientC_API.cpp | 5 +- examples/SharedMemory/PhysicsClientC_API.h | 3 +- .../SharedMemory/PhysicsClientExample.cpp | 110 +++++++++++++----- examples/SharedMemory/PhysicsServer.cpp | 47 ++++---- examples/SharedMemory/PhysicsServer.h | 2 +- .../SharedMemory/PhysicsServerExample.cpp | 18 ++- examples/SharedMemory/PosixSharedMemory.cpp | 4 +- examples/SharedMemory/RobotControlExample.cpp | 15 ++- examples/SharedMemory/SharedMemoryCommon.h | 1 + examples/SharedMemory/main.cpp | 54 +++++++-- 14 files changed, 215 insertions(+), 92 deletions(-) diff --git a/examples/ExampleBrowser/main.cpp b/examples/ExampleBrowser/main.cpp index f292bb751..c3442de73 100644 --- a/examples/ExampleBrowser/main.cpp +++ b/examples/ExampleBrowser/main.cpp @@ -13,6 +13,9 @@ #include "ExampleEntries.h" #include "Bullet3Common/b3Logging.h" + + + int main(int argc, char* argv[]) { b3CommandLineArgs args(argc,argv); diff --git a/examples/RollingFrictionDemo/RollingFrictionDemo.cpp b/examples/RollingFrictionDemo/RollingFrictionDemo.cpp index 55f079a78..1d3213124 100644 --- a/examples/RollingFrictionDemo/RollingFrictionDemo.cpp +++ b/examples/RollingFrictionDemo/RollingFrictionDemo.cpp @@ -229,6 +229,9 @@ void RollingFrictionDemo::initPhysics() } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); + + if (0) + { btSerializer* s = new btDefaultSerializer; m_dynamicsWorld->serialize(s); b3ResourcePath p; @@ -239,6 +242,7 @@ void RollingFrictionDemo::initPhysics() fwrite(s->getBufferPointer(),s->getCurrentBufferSize(),1,f); fclose(f); } + } } diff --git a/examples/SharedMemory/PhysicsClient.cpp b/examples/SharedMemory/PhysicsClient.cpp index e068b409a..bdfeba9e5 100644 --- a/examples/SharedMemory/PhysicsClient.cpp +++ b/examples/SharedMemory/PhysicsClient.cpp @@ -77,38 +77,43 @@ PhysicsClientSharedMemory::PhysicsClientSharedMemory() PhysicsClientSharedMemory::~PhysicsClientSharedMemory() { - m_data->m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); + if (m_data->m_isConnected) + { + disconnectSharedMemory(); + } delete m_data->m_sharedMemory; delete m_data; } +void PhysicsClientSharedMemory::disconnectSharedMemory () +{ + if (m_data->m_isConnected) + { + m_data->m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); + m_data->m_isConnected = false; + } +} + bool PhysicsClientSharedMemory::isConnected() const { return m_data->m_isConnected ; } -bool PhysicsClientSharedMemory::connect(bool allowSharedMemoryInitialization) +bool PhysicsClientSharedMemory::connect() { - bool allowCreation = true; + ///server always has to create and initialize shared memory + bool allowCreation = false; m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE, allowCreation); if (m_data->m_testBlock1) { if (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER) { - if (allowSharedMemoryInitialization) - { - InitSharedMemoryBlock(m_data->m_testBlock1); - b3Printf("Created and initialized shared memory block"); - m_data->m_isConnected = true; - } else - { - b3Error("Error: please start server before client\n"); - m_data->m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); - m_data->m_testBlock1 = 0; - return false; - } - } else + b3Error("Error: please start server before client\n"); + m_data->m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); + m_data->m_testBlock1 = 0; + return false; + } else { b3Printf("Connected to existing shared memory, status OK.\n"); m_data->m_isConnected = true; diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h index 29e00f19e..1f46363b6 100644 --- a/examples/SharedMemory/PhysicsClient.h +++ b/examples/SharedMemory/PhysicsClient.h @@ -15,7 +15,9 @@ public: virtual ~PhysicsClientSharedMemory(); //return true if connection succesfull, can also check 'isConnected' - virtual bool connect(bool allowSharedMemoryInitialization = true); + virtual bool connect(); + + virtual void disconnectSharedMemory (); virtual bool isConnected() const; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 14f58cd31..8d627322a 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -191,10 +191,11 @@ int b3CreateSensorEnable6DofJointForceTorqueSensor(struct SharedMemoryCommand* c } -b3PhysicsClientHandle b3ConnectSharedMemory( int allowSharedMemoryInitialization) +b3PhysicsClientHandle b3ConnectSharedMemory() { PhysicsClientSharedMemory* cl = new PhysicsClientSharedMemory(); - cl->connect(allowSharedMemoryInitialization); + ///client should never create shared memory, only the server does + cl->connect(); return (b3PhysicsClientHandle ) cl; } diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 35a1a5801..d726d2740 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -12,7 +12,8 @@ B3_DECLARE_HANDLE(b3PhysicsRobotHandle); extern "C" { #endif -b3PhysicsClientHandle b3ConnectSharedMemory(int allowSharedMemoryInitialization); +///make sure to start the server first! +b3PhysicsClientHandle b3ConnectSharedMemory(); void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient); diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 3f7e6a833..7eb507c05 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -9,6 +9,15 @@ #include "PhysicsClient.h" #include "SharedMemoryCommands.h" +struct MyMotorInfo2 +{ + btScalar m_velTarget; + btScalar m_maxForce; + int m_uIndex; +}; + +#define MAX_NUM_MOTORS 128 + class PhysicsClientExample : public SharedMemoryCommon { protected: @@ -22,6 +31,11 @@ protected: public: + //@todo, add accessor methods + MyMotorInfo2 m_motorTargetVelocities[MAX_NUM_MOTORS]; + int m_numMotors; + + PhysicsClientExample(GUIHelperInterface* helper); virtual ~PhysicsClientExample(); @@ -42,11 +56,20 @@ public: { return m_wantsTermination; } + + virtual bool isConnected() + { + return m_physicsClient.isConnected(); + } + void enqueueCommand(const SharedMemoryCommand& orgCommand); }; + + + void MyCallback(int buttonId, bool buttonState, void* userPtr) { PhysicsClientExample* cl = (PhysicsClientExample*) userPtr; @@ -58,8 +81,8 @@ void MyCallback(int buttonId, bool buttonState, void* userPtr) case CMD_LOAD_URDF: { command.m_type =CMD_LOAD_URDF; - sprintf(command.m_urdfArguments.m_urdfFileName,"hinge.urdf");//r2d2.urdf"); - command.m_urdfArguments.m_initialPosition[0] = 0.0; + sprintf(command.m_urdfArguments.m_urdfFileName,"kuka_lwr/kuka.urdf"); + command.m_urdfArguments.m_initialPosition[0] = 0.0; command.m_urdfArguments.m_initialPosition[1] = 0.0; command.m_urdfArguments.m_initialPosition[2] = 0.0; command.m_urdfArguments.m_initialOrientation[0] = 0.0; @@ -92,36 +115,29 @@ void MyCallback(int buttonId, bool buttonState, void* userPtr) case CMD_SEND_DESIRED_STATE: { - command.m_type =CMD_SEND_DESIRED_STATE; - int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE; + command.m_type =CMD_SEND_DESIRED_STATE; + int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE; - command.m_sendDesiredStateCommandArgument.m_controlMode = controlMode; - //todo: expose a drop box in the GUI for this - switch (controlMode) - { - case CONTROL_MODE_VELOCITY: - { - for (int i=0;im_numMotors;i++) + { + btScalar targetVel = cl->m_motorTargetVelocities[i].m_velTarget; + + int uIndex = cl->m_motorTargetVelocities[i].m_uIndex; + if (targetVel>1) + { + printf("testme"); + } + command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[uIndex] = targetVel; + + } + cl->enqueueCommand(command); break; } @@ -200,7 +216,7 @@ void PhysicsClientExample::initPhysics() if (!m_physicsClient.connect()) { - b3Warning("Cannot eonnect to physics client"); + b3Warning("Cannot connect to physics client"); } } @@ -222,6 +238,36 @@ void PhysicsClientExample::stepSimulation(float deltaTime) b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex); } + + if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED) + { + for (int i=0;im_velTarget = 0.f; + motorInfo->m_uIndex = info.m_uIndex; + + SliderParams slider(motorName,&motorInfo->m_velTarget); + slider.m_minVal=-4; + slider.m_maxVal=4; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + m_numMotors++; + } + } + + } + + } } if (m_physicsClient.canSubmitCommand()) diff --git a/examples/SharedMemory/PhysicsServer.cpp b/examples/SharedMemory/PhysicsServer.cpp index 726d85a30..faadd1f19 100644 --- a/examples/SharedMemory/PhysicsServer.cpp +++ b/examples/SharedMemory/PhysicsServer.cpp @@ -92,7 +92,7 @@ PhysicsServerSharedMemory::~PhysicsServerSharedMemory() -bool PhysicsServerSharedMemory::connectSharedMemory(bool allowSharedMemoryInitialization, class btMultiBodyDynamicsWorld* dynamicsWorld, struct GUIHelperInterface* guiHelper) +bool PhysicsServerSharedMemory::connectSharedMemory( class btMultiBodyDynamicsWorld* dynamicsWorld, struct GUIHelperInterface* guiHelper) { m_data->m_dynamicsWorld = dynamicsWorld; m_data->m_guiHelper = guiHelper; @@ -100,34 +100,37 @@ bool PhysicsServerSharedMemory::connectSharedMemory(bool allowSharedMemoryInitia bool allowCreation = true; bool allowConnectToExistingSharedMemory = false; + if (m_data->m_isConnected) + { + b3Warning("connectSharedMemory, while already connected"); + return m_data->m_isConnected; + } + + m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE,allowCreation); if (m_data->m_testBlock1) { - if (!allowConnectToExistingSharedMemory || (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)) + int magicId =m_data->m_testBlock1->m_magicId; + b3Printf("magicId = %d\n", magicId); + + if (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER) { - if (allowSharedMemoryInitialization) - { - InitSharedMemoryBlock(m_data->m_testBlock1); - b3Printf("Created and initialized shared memory block"); - m_data->m_isConnected = true; - } else - { - b3Error("Error: please start server before client\n"); - m_data->m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); - m_data->m_testBlock1 = 0; - return false; - } + InitSharedMemoryBlock(m_data->m_testBlock1); + b3Printf("Created and initialized shared memory block\n"); + m_data->m_isConnected = true; } else { - b3Printf("Connected to existing shared memory, status OK.\n"); - m_data->m_isConnected = true; + b3Error("Server cannot connect to existing shared memory, disconnecting shared memory.\n"); + m_data->m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); + m_data->m_testBlock1 = 0; + m_data->m_isConnected = false; } } else { b3Error("Cannot connect to shared memory"); - return false; + m_data->m_isConnected = false; } - return true; + return m_data->m_isConnected; } @@ -693,7 +696,7 @@ void PhysicsServerSharedMemory::processClientCommands() case CMD_CREATE_BOX_COLLISION_SHAPE: { btVector3 halfExtents(1,1,1); - if (clientCmd.m_updateFlags | BOX_SHAPE_HAS_HALF_EXTENTS) + if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_HALF_EXTENTS) { halfExtents = btVector3( clientCmd.m_createBoxShapeArguments.m_halfExtentsX, @@ -702,7 +705,7 @@ void PhysicsServerSharedMemory::processClientCommands() } btTransform startTrans; startTrans.setIdentity(); - if (clientCmd.m_updateFlags | BOX_SHAPE_HAS_INITIAL_POSITION) + if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_INITIAL_POSITION) { startTrans.setOrigin(btVector3( clientCmd.m_createBoxShapeArguments.m_initialPosition[0], @@ -710,8 +713,10 @@ void PhysicsServerSharedMemory::processClientCommands() clientCmd.m_createBoxShapeArguments.m_initialPosition[2])); } - if (clientCmd.m_updateFlags | BOX_SHAPE_HAS_INITIAL_ORIENTATION) + if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_INITIAL_ORIENTATION) { + + b3Printf("test\n"); startTrans.setRotation(btQuaternion( clientCmd.m_createBoxShapeArguments.m_initialOrientation[0], clientCmd.m_createBoxShapeArguments.m_initialOrientation[1], diff --git a/examples/SharedMemory/PhysicsServer.h b/examples/SharedMemory/PhysicsServer.h index 2b326cbb6..c069bb2fd 100644 --- a/examples/SharedMemory/PhysicsServer.h +++ b/examples/SharedMemory/PhysicsServer.h @@ -20,7 +20,7 @@ public: virtual ~PhysicsServerSharedMemory(); //todo: implement option to allocated shared memory from client - virtual bool connectSharedMemory(bool allowSharedMemoryInitialization, class btMultiBodyDynamicsWorld* dynamicsWorld, struct GUIHelperInterface* guiHelper); + virtual bool connectSharedMemory(class btMultiBodyDynamicsWorld* dynamicsWorld, struct GUIHelperInterface* guiHelper); virtual void disconnectSharedMemory (bool deInitializeSharedMemory); diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index e7f6c6d9c..3d0ad9ae7 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -24,9 +24,9 @@ class PhysicsServerExample : public SharedMemoryCommon { PhysicsServerSharedMemory m_physicsServer; - - bool m_wantsShutdown; + bool m_wantsShutdown; + bool m_isConnected; public: @@ -50,11 +50,14 @@ public: } virtual bool wantsTermination(); + virtual bool isConnected(); + }; PhysicsServerExample::PhysicsServerExample(GUIHelperInterface* helper) :SharedMemoryCommon(helper), -m_wantsShutdown(false) +m_wantsShutdown(false), +m_isConnected(false) { b3Printf("Started PhysicsServer\n"); } @@ -65,6 +68,12 @@ PhysicsServerExample::~PhysicsServerExample() { bool deInitializeSharedMemory = true; m_physicsServer.disconnectSharedMemory(deInitializeSharedMemory); + m_isConnected = false; +} + +bool PhysicsServerExample::isConnected() +{ + return m_isConnected; } void PhysicsServerExample::initPhysics() @@ -80,8 +89,7 @@ void PhysicsServerExample::initPhysics() grav[upAxis] = 0;//-9.8; this->m_dynamicsWorld->setGravity(grav); - bool allowSharedMemoryInitialization = true; - m_physicsServer.connectSharedMemory(allowSharedMemoryInitialization, m_dynamicsWorld,m_guiHelper); + m_isConnected = m_physicsServer.connectSharedMemory( m_dynamicsWorld,m_guiHelper); } diff --git a/examples/SharedMemory/PosixSharedMemory.cpp b/examples/SharedMemory/PosixSharedMemory.cpp index 3bf0165dc..00b3029b7 100644 --- a/examples/SharedMemory/PosixSharedMemory.cpp +++ b/examples/SharedMemory/PosixSharedMemory.cpp @@ -40,10 +40,10 @@ struct btPointerCaster }; }; -void* PosixSharedMemory::allocateSharedMemory(int key, int size, bool /*allowCreation*/) +void* PosixSharedMemory::allocateSharedMemory(int key, int size, bool allowCreation) { #ifdef TEST_SHARED_MEMORY - int flags = IPC_CREAT | 0666; + int flags = (allowCreation ? IPC_CREAT : 0) | 0666; int id = shmget((key_t) key, (size_t) size,flags); if (id < 0) { diff --git a/examples/SharedMemory/RobotControlExample.cpp b/examples/SharedMemory/RobotControlExample.cpp index d9ec47176..6249fc211 100644 --- a/examples/SharedMemory/RobotControlExample.cpp +++ b/examples/SharedMemory/RobotControlExample.cpp @@ -71,8 +71,13 @@ public: } virtual bool wantsTermination(); + virtual bool isConnected(); }; +bool RobotControlExample::isConnected() +{ + return m_physicsClient.isConnected(); +} void MyCallback2(int buttonId, bool buttonState, void* userPtr) { @@ -131,6 +136,12 @@ void MyCallback2(int buttonId, bool buttonState, void* userPtr) case CMD_CREATE_BOX_COLLISION_SHAPE: { command.m_type =CMD_CREATE_BOX_COLLISION_SHAPE; + command.m_updateFlags = BOX_SHAPE_HAS_INITIAL_POSITION; + command.m_createBoxShapeArguments.m_initialPosition[0] = 0; + command.m_createBoxShapeArguments.m_initialPosition[1] = 0; + command.m_createBoxShapeArguments.m_initialPosition[2] = -3; + + cl->enqueueCommand(command); break; } @@ -232,6 +243,7 @@ m_numMotors(0) RobotControlExample::~RobotControlExample() { bool deInitializeSharedMemory = true; + m_physicsClient.disconnectSharedMemory(); m_physicsServer.disconnectSharedMemory(deInitializeSharedMemory); } @@ -248,8 +260,7 @@ void RobotControlExample::initPhysics() grav[upAxis] = 0;//-9.8; this->m_dynamicsWorld->setGravity(grav); - bool allowSharedMemoryInitialization = true; - m_physicsServer.connectSharedMemory(allowSharedMemoryInitialization, m_dynamicsWorld,m_guiHelper); + m_physicsServer.connectSharedMemory( m_dynamicsWorld,m_guiHelper); if (m_guiHelper && m_guiHelper->getParameterInterface()) { diff --git a/examples/SharedMemory/SharedMemoryCommon.h b/examples/SharedMemory/SharedMemoryCommon.h index e479acd33..04192d974 100644 --- a/examples/SharedMemory/SharedMemoryCommon.h +++ b/examples/SharedMemory/SharedMemoryCommon.h @@ -12,6 +12,7 @@ public: } virtual bool wantsTermination()=0; + virtual bool isConnected()=0; }; #endif// diff --git a/examples/SharedMemory/main.cpp b/examples/SharedMemory/main.cpp index c78f4245f..88b6c1ee8 100644 --- a/examples/SharedMemory/main.cpp +++ b/examples/SharedMemory/main.cpp @@ -22,27 +22,63 @@ subject to the following restrictions: #include "../CommonInterfaces/CommonGUIHelperInterface.h" #include "SharedMemoryCommon.h" +#include +#include +#include + + +static SharedMemoryCommon* example = NULL; +static bool interrupted = false; + +#ifndef _WIN32 +#include +static void cleanup(int signo) +{ + if (interrupted) { // this is the second time, we're hanging somewhere + // if (example) { + // example->abort(); + // } + b3Printf("Aborting and deleting SharedMemoryCommon object"); + sleep(1); + delete example; + errx(EXIT_FAILURE, "aborted example on signal %d", signo); + } + interrupted = true; + warnx("caught signal %d", signo); +} +#endif//_WIN32 + int main(int argc, char* argv[]) { - +#ifndef _WIN32 + struct sigaction action; + memset(&action, 0x0, sizeof(action)); + action.sa_handler = cleanup; + static const int signos[] = { SIGHUP, SIGINT, SIGQUIT, SIGABRT, SIGSEGV, SIGPIPE, SIGTERM }; + for (int ii(0); ii < sizeof(signos) / sizeof(*signos); ++ii) { + if (0 != sigaction(signos[ii], &action, NULL)) { + err(EXIT_FAILURE, "signal %d", signos[ii]); + } + } +#endif + b3CommandLineArgs args(argc, argv); DummyGUIHelper noGfx; CommonExampleOptions options(&noGfx); - SharedMemoryCommon* example = 0; if (args.CheckCmdLineFlag("client")) - { - example = (SharedMemoryCommon*)PhysicsClientCreateFunc(options); - }else - { - example = (SharedMemoryCommon*)PhysicsServerCreateFunc(options); - } + { + example = (SharedMemoryCommon*)PhysicsClientCreateFunc(options); + }else + { + example = (SharedMemoryCommon*)PhysicsServerCreateFunc(options); + } example->initPhysics(); - while (!example->wantsTermination()) + while (example->isConnected() && !(example->wantsTermination() || interrupted)) { example->stepSimulation(1.f/60.f); }