small improvement to the shared memory physics server/client

examples/SharedMemory/PhysicsClient.cpp

@@ -49,7 +49,13 @@ void	PhysicsClient::initPhysics()
     m_testBlock1 = (SharedMemoryExampleData*)m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
     if (m_testBlock1)
     {
-        btAssert(m_testBlock1->m_magicId == SHARED_MEMORY_MAGIC_NUMBER);
+     //   btAssert(m_testBlock1->m_magicId == SHARED_MEMORY_MAGIC_NUMBER);
+        if (m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
+        {
+            b3Error("Error: please start server before client");
+            m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
+            m_testBlock1 = 0;
+        }
     }
     
 }
@@ -66,7 +72,8 @@ void	PhysicsClient::stepSimulation(float deltaTime)
             once=false;
         
             b3Printf("Client created CMD_LOAD_URDF");
-            m_testBlock1->m_clientCommands[0] =CMD_LOAD_URDF;
+            m_testBlock1->m_clientCommands[0].m_type =CMD_LOAD_URDF;
+            sprintf(m_testBlock1->m_clientCommands[0].m_urdfArguments.m_urdfFileName,"r2d2.urdf");
             m_testBlock1->m_numClientCommands++;
         }
     }

examples/SharedMemory/PhysicsServer.cpp

@@ -3,6 +3,10 @@
 
 #include "../CommonInterfaces/CommonMultiBodyBase.h"
 #include "PosixSharedMemory.h"
+#include "../Importers/ImportURDFDemo/MyURDFImporter.h"
+#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
+#include "../Importers/ImportURDFDemo/URDF2Bullet.h"
+
 
 
 
@@ -21,6 +25,9 @@ public:
     
 	virtual void	stepSimulation(float deltaTime);
     
+    bool loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn,
+                  bool useMultiBody, bool useFixedBase);
+    
 	virtual void resetCamera()
 	{
 		float dist = 1;
@@ -53,6 +60,8 @@ PhysicsServer::~PhysicsServer()
 
 void	PhysicsServer::initPhysics()
 {
+    createEmptyDynamicsWorld();
+    
     m_testBlock1 = (SharedMemoryExampleData*) m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
     
 //    btAssert(m_testBlock1);
@@ -77,6 +86,31 @@ void	PhysicsServer::initPhysics()
     }
 }
 
+bool PhysicsServer::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn,
+                             bool useMultiBody, bool useFixedBase)
+{
+ 
+    MyURDFImporter u2b(m_guiHelper);
+    bool loadOk =  u2b.loadURDF(fileName);
+    if (loadOk)
+    {
+        b3Printf("loaded %s OK!", fileName);
+        
+        btTransform tr;
+        tr.setIdentity();
+        tr.setOrigin(pos);
+        tr.setRotation(orn);
+        int rootLinkIndex = u2b.getRootLinkIndex();
+        //                      printf("urdf root link index = %d\n",rootLinkIndex);
+        MyMultiBodyCreator creation(m_guiHelper);
+        bool m_useMultiBody = true;
+        ConvertURDF2Bullet(u2b,creation, tr,m_dynamicsWorld,useMultiBody,u2b.getPathPrefix());
+        btMultiBody* mb = creation.getBulletMultiBody();
+
+        return false;
+    }
+}
+
 void	PhysicsServer::stepSimulation(float deltaTime)
 {
 
@@ -89,20 +123,31 @@ void	PhysicsServer::stepSimulation(float deltaTime)
             //until we implement a proper ring buffer, we assume always maximum of 1 outstanding commands
             btAssert(m_testBlock1->m_numClientCommands==m_testBlock1->m_numProcessedClientCommands+1);
             
+            const SharedMemoryCommand& clientCmd =m_testBlock1->m_clientCommands[0];
+            
             //consume the command
-            switch (m_testBlock1->m_clientCommands[0])
+            switch (clientCmd.m_type)
             {
                 case CMD_LOAD_URDF:
                 {
-                    b3Printf("Processed CMD_LOAD_URDF");
+                    b3Printf("Processed CMD_LOAD_URDF:%s",clientCmd.m_urdfArguments.m_urdfFileName);
                     
                     //load the actual URDF and send a report: completed or failed
 
-                    m_testBlock1->m_serverCommands[0] =CMD_URDF_LOADING_COMPLETED;
-                    m_testBlock1->m_numServerCommands++;
                     
-                    //CMD_URDF_LOADING_COMPLETED,
+                    bool completedOk = loadUrdf(clientCmd.m_urdfArguments.m_urdfFileName,
-                    //CMD_URDF_LOADING_FAILED,
+                                               btVector3(0,0,0), btQuaternion(0,0,0,1),true,true );
+                    SharedMemoryCommand& serverCmd =m_testBlock1->m_serverCommands[0];
+ 
+                    if (completedOk)
+                    {
+                        serverCmd.m_type =CMD_URDF_LOADING_COMPLETED;
+                    } else
+                    {
+                        serverCmd.m_type =CMD_URDF_LOADING_FAILED;
+                    
+                    }
+                    m_testBlock1->m_numServerCommands++;
                 }
                 default:
                 {

examples/SharedMemory/PosixSharedMemory.cpp

@@ -1,8 +1,10 @@
 #include "PosixSharedMemory.h"
 #include "Bullet3Common/b3Logging.h"
 #include "LinearMath/btScalar.h" //for btAssert
+
 #ifdef __APPLE__
-//#define TEST_SHARED_MEMORY
+#define TEST_SHARED_MEMORY
+
 #endif
 
 #include <stddef.h>
@@ -71,13 +73,13 @@ void PosixSharedMemory::releaseSharedMemory(int key, int size)
     int id = shmget((key_t) key, (size_t) size,flags);
     if (id < 0)
     {
-        b3Error("shmget error");
+        b3Error("PosixSharedMemory::releaseSharedMemory: shmget error");
     } else
     {
         int result = shmctl(id,IPC_RMID,0);
         if (result == -1)
         {
-            b3Error("shmat returned -1");
+            b3Error("PosixSharedMemory::releaseSharedMemory: shmat returned -1");
         }
     }
 #endif

examples/SharedMemory/SharedMemoryInterface.h

@@ -1,7 +1,7 @@
 #ifndef SHARED_MEMORY_INTERFACE_H
 #define SHARED_MEMORY_INTERFACE_H
 
-#define SHARED_MEMORY_KEY 12345
+#define SHARED_MEMORY_KEY 12347
 #define SHARED_MEMORY_MAGIC_NUMBER 64738
 #define SHARED_MEMORY_MAX_COMMANDS 64
 
@@ -26,17 +26,33 @@ enum SharedMemoryClientCommand{
 #define MAX_NUM_SENSORS 1024
 #define MAX_URDF_FILENAME_LENGTH 1024
 
-struct CommandArguments
+
+struct UrdfCommandArgument
+{
+    char m_urdfFileName[MAX_URDF_FILENAME_LENGTH];
+};
+
+struct StepSimulationCommandArgument
 {
     double m_deltaTimeInSeconds;
-    char m_urdfFileName[MAX_URDF_FILENAME_LENGTH];
+};
+
+struct SharedMemoryCommand
+{
+    int m_type;
+    
+    union
+    {
+        UrdfCommandArgument m_urdfArguments;
+        StepSimulationCommandArgument m_stepSimulationArguments;
+    };
 };
 
 struct SharedMemoryExampleData
 {
     int m_magicId;
-    int m_clientCommands[SHARED_MEMORY_MAX_COMMANDS];
+    SharedMemoryCommand m_clientCommands[SHARED_MEMORY_MAX_COMMANDS];
-    int m_serverCommands[SHARED_MEMORY_MAX_COMMANDS];
+    SharedMemoryCommand m_serverCommands[SHARED_MEMORY_MAX_COMMANDS];
     
     int m_numClientCommands;
     int m_numProcessedClientCommands;
@@ -45,12 +61,16 @@ struct SharedMemoryExampleData
     int m_numProcessedServerCommands;
     
    
-    double m_stateQ[MAX_DEGREE_OF_FREEDOM];
+    //desired state is only written by the client, read-only access by server is expected
-    double m_stateQdot[MAX_DEGREE_OF_FREEDOM];
+    double m_desiredStateQ[MAX_DEGREE_OF_FREEDOM];
-    double m_stateSensors[MAX_NUM_SENSORS];//these are force sensors and IMU information
+    double m_desiredStateQdot[MAX_DEGREE_OF_FREEDOM];
+    double m_desiredStateSensors[MAX_NUM_SENSORS];//these are force sensors and IMU information
+    
+    //actual state is only written by the server, read-only access by client is expected
+    double m_actualStateQ[MAX_DEGREE_OF_FREEDOM];
+    double m_actualStateQdot[MAX_DEGREE_OF_FREEDOM];
+    double m_actualStateSensors[MAX_NUM_SENSORS];//these are force sensors and IMU information
     
-    CommandArguments m_clientCommandArguments[SHARED_MEMORY_MAX_COMMANDS];
-    CommandArguments m_serverCommandArguments[SHARED_MEMORY_MAX_COMMANDS];
     
 };