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

fixes in shared memory:

Browse Source 
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.
This commit is contained in:
=
2015-08-06 11:59:31 -07:00
parent dcab0e2b1f
commit 46fae61c69
14 changed files with 215 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
examples/ExampleBrowser/main.cpp
View File

@@ -13,6 +13,9 @@
#include "ExampleEntries.h"
#include "Bullet3Common/b3Logging.h"
int main(int argc, char* argv[])
{
b3CommandLineArgs args(argc,argv);

4
examples/RollingFrictionDemo/RollingFrictionDemo.cpp
View File

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

37
examples/SharedMemory/PhysicsClient.cpp
View File

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

4
examples/SharedMemory/PhysicsClient.h
View File

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

5
examples/SharedMemory/PhysicsClientC_API.cpp
View File

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

3
examples/SharedMemory/PhysicsClientC_API.h
View File

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

110
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -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;i<MAX_DEGREE_OF_FREEDOM;i++)
{
command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[i] = 1;
command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 100;
}
break;
}
case CONTROL_MODE_TORQUE:
{
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
{
command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 100;
}
break;
}
default:
{
b3Printf("Unknown control mode in client CMD_SEND_DESIRED_STATE");
btAssert(0);
}
}
command.m_sendDesiredStateCommandArgument.m_controlMode = controlMode;
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
{
command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[i] = 0;
command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 1000;
}
for (int i=0;i<cl->m_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;i<m_physicsClient.getNumJoints();i++)
{
b3JointInfo info;
m_physicsClient.getJointInfo(i,info);
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
if (info.m_flags & JOINT_HAS_MOTORIZED_POWER)
{
if (m_numMotors<MAX_NUM_MOTORS)
{
char motorName[1024];
sprintf(motorName,"%s q'", info.m_jointName);
MyMotorInfo2* motorInfo = &m_motorTargetVelocities[m_numMotors];
motorInfo->m_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())

47
examples/SharedMemory/PhysicsServer.cpp
View File

@@ -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],

2
examples/SharedMemory/PhysicsServer.h
View File

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

18
examples/SharedMemory/PhysicsServerExample.cpp
View File

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

4
examples/SharedMemory/PosixSharedMemory.cpp
View File

@@ -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)
{

15
examples/SharedMemory/RobotControlExample.cpp
View File

@@ -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())
{

1
examples/SharedMemory/SharedMemoryCommon.h
View File

@@ -12,6 +12,7 @@ public:
}
virtual bool wantsTermination()=0;
virtual bool isConnected()=0;
};
#endif//

54
examples/SharedMemory/main.cpp
View File

@@ -22,27 +22,63 @@ subject to the following restrictions:
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
#include "SharedMemoryCommon.h"
#include <signal.h>
#include <err.h>
#include <stdlib.h>
static SharedMemoryCommon* example = NULL;
static bool interrupted = false;
#ifndef _WIN32
#include <unistd.h>
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);
}